/*
  DR16接收机
*/

/* Includes ----------------------------------------------------------------- */
#include "dr16.h"

#include <string.h>

#include "bsp/uart.h"

/* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u)
#define DR16_CH_VALUE_MAX (1684u)
#define DR16_DATA_SIZE (18u)

/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */

static osThreadId_t thread_alert;
static bool inited = false;
static uint8_t rx_buff[DR16_DATA_SIZE] __attribute__ ((section(".AXI_SRAM")));

/* Private function  -------------------------------------------------------- */
static void DR16_RxCpltCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
}

static void DR16_ErrorCallback(void) {
  DR16_Restart();
}

static void DecodeDR16Data(DR16_t *dr16, const uint8_t *rx_buff) {
    if (dr16 == NULL || rx_buff == NULL) return;

    DR16_Data_t *data = &dr16->data;

    // 摇杆数据
    data->ch_r_x = ((rx_buff[0] | (rx_buff[1] << 8)) & 0x07FF);
    data->ch_r_y = (((rx_buff[1] >> 3) | (rx_buff[2] << 5)) & 0x07FF);
    data->ch_l_x = (((rx_buff[2] >> 6) | (rx_buff[3] << 2) | (rx_buff[4] << 10)) & 0x07FF);
    data->ch_l_y = (((rx_buff[4] >> 1) | (rx_buff[5] << 7)) & 0x07FF);

    // 俩拨杆
    data->sw_r = ((rx_buff[5] >> 4) & 0x03);
    data->sw_l = ((rx_buff[5] >> 6) & 0x03);

    // 鼠标数据
    data->x = (int16_t)(rx_buff[6] | (rx_buff[7] << 8));
    data->y = (int16_t)(rx_buff[8] | (rx_buff[9] << 8));
    data->z = (int16_t)(rx_buff[10] | (rx_buff[11] << 8));

    // 鼠标按键
    data->press_l = rx_buff[12];
    data->press_r = rx_buff[13];

    // 键盘按键
    data->key = (uint16_t)(rx_buff[14] | (rx_buff[15] << 8));

    // 左上角拨扭
    data->res = (uint16_t)(rx_buff[16] | (rx_buff[17] << 8));
}

static bool DR16_DataCorrupted(const DR16_t *dr16) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;

  if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) ||
      (dr16->data.ch_r_x > DR16_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) ||
      (dr16->data.ch_r_y > DR16_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) ||
      (dr16->data.ch_l_x > DR16_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_l_y < DR16_CH_VALUE_MIN) ||
      (dr16->data.ch_l_y > DR16_CH_VALUE_MAX))
    return true;

  if (dr16->data.sw_l == 0) return true;

  if (dr16->data.sw_r == 0) return true;

  return false;
}

/* Exported functions ------------------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;
  if (inited) return DEVICE_ERR_INITED;
  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

  BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB,
                            DR16_RxCpltCallback);
  BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_ERROR_CB,
                            DR16_ErrorCallback);

  inited = true;
  return DEVICE_OK;
}

int8_t DR16_Restart(void) {
  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
  return DEVICE_OK;
}

int8_t DR16_StartDmaRecv(DR16_t *dr16) {
  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), rx_buff, DR16_DATA_SIZE) != HAL_OK)
    return DEVICE_ERR;
  return DEVICE_OK;
}

bool DR16_WaitDmaCplt(uint32_t timeout) {
  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
          SIGNAL_DR16_RAW_REDY);
}

int8_t DR16_ParseData(DR16_t *dr16) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;

  DecodeDR16Data(dr16, rx_buff);

  if (DR16_DataCorrupted(dr16)) {
    return DEVICE_ERR;
  }

  return DEVICE_OK;
}

int8_t DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc){
  if (dr16 == NULL || rc == NULL) return DEVICE_ERR_NULL;

  float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);

  rc->ch_r_x = 2 * ((float)dr16->data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
  rc->ch_r_y = 2 * ((float)dr16->data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
  rc->ch_l_x = 2 * ((float)dr16->data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
  rc->ch_l_y = 2 * ((float)dr16->data.ch_l_y - DR16_CH_VALUE_MID) / full_range;

  rc->sw_r = dr16->data.sw_r;
  rc->sw_l = dr16->data.sw_l;

  rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;

  return DEVICE_OK;
}

int8_t DR16_HandleOffline(DR16_t *dr16) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;
  memset(&dr16->data, 0, sizeof(dr16->data));
  return DEVICE_OK;
}