/*
  CAN总线上的设
  将所有CAN总线上挂载的设抽象成一设进行配和控制
*/

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

#include <stdbool.h>
#include <string.h>

#include "bsp\can.h"
#include "bsp\buzzer.h"
#include "component\user_math.h"

/* Private define ----------------------------------------------------------- */
#define CAN_Mini_Croe (0x110)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static CAN_RawRx_t raw_rx1;
static CAN_RawRx_t raw_rx2;
static CAN_RawTx_t raw_tx;

static osThreadId_t thread_alert;
static CAN_t *gcan;
static bool inited = false;

// /* Private function  -------------------------------------------------------- */
static void CAN_CAN1RxFifoMsgPendingCallback(void) {
  HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_RX_FIFO0,
                       &raw_rx1.rx_header, raw_rx1.rx_data);
  osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0);
}

/**
 * @brief  CAN2接收FIFO1消息挂起回调函数
 * @param  None
 */
static void CAN_CAN2RxFifoMsgPendingCallback(void) {
  HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_RX_FIFO1,
                       &raw_rx2.rx_header, raw_rx2.rx_data);
  osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
}


static void CAN_Sick_Decode(CAN_Sick_Feedback_t *feedback, uint16_t *sick, const uint8_t *raw) {
  feedback->distance = (uint16_t)((raw[0] << 8) | raw[1]); // 转换为米
  feedback->save0 = (uint16_t)((raw[2] << 8) | raw[3]); // 转换为米
  feedback->save1 = (uint16_t)((raw[4] << 8) | raw[5]); // 转换为米
  feedback->save2 = (uint16_t)((raw[6] << 8) | raw[7]); // 转换为米
  *sick = feedback->distance; // 直接赋值给sick
}


// /* Exported functions ------------------------------------------------------- */
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
  if (can == NULL) return DEVICE_ERR_NULL;
  if (inited) return DEVICE_ERR_INITED;
  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

  can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL);

  can->param = param;

  CAN_FilterTypeDef can_filter = {0};

  can_filter.FilterBank = 0;
  can_filter.FilterIdHigh = 0;
  can_filter.FilterIdLow = 0;
  can_filter.FilterMode = CAN_FILTERMODE_IDMASK;
  can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter.FilterMaskIdHigh = 0;
  can_filter.FilterMaskIdLow = 0;
  can_filter.FilterActivation = ENABLE;
  can_filter.SlaveStartFilterBank = 14;
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO0;

  HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_1), &can_filter);
  HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_1));
  BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
                           CAN_CAN1RxFifoMsgPendingCallback);
  HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_1),
                               CAN_IT_RX_FIFO0_MSG_PENDING);

  can_filter.FilterBank = 14;
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO1;

  HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_2), &can_filter);
  HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_2));
  BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
                           CAN_CAN2RxFifoMsgPendingCallback);
  HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_2),
                               CAN_IT_RX_FIFO1_MSG_PENDING);

  gcan = can;
  inited = true;
  return DEVICE_OK;
}

int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
  if (can == NULL) return DEVICE_ERR_NULL;
  if (can_rx == NULL) return DEVICE_ERR_NULL;
  uint32_t index;
  switch (can_rx->rx_header.StdId) {
    case CAN_SICK1_ID:
    case CAN_SICK2_ID:
    case CAN_SICK3_ID:
    case CAN_SICK4_ID:
      index = can_rx->rx_header.StdId - CAN_SICK1_ID;
      CAN_Sick_Decode(&(can->sick.as_array[index]), &(can->sick.named.sick[index]), can_rx->rx_data);
      can->recive_flag |= 1 << index;
      break;

    default:
      break;
  }
  return DEVICE_OK;
}

int8_t CAN_Sick_Send(CAN_t *can) {

  uint16_t sick1, sick2, sick3, sick4;
  sick1 = can->sick.named.sick[0];
  sick2 = can->sick.named.sick[1];
  sick3 = can->sick.named.sick[2];
  sick4 = can->sick.named.sick[3];

  raw_tx.tx_header.StdId = CAN_Mini_Croe;
  raw_tx.tx_header.IDE = CAN_ID_STD;
  raw_tx.tx_header.RTR = CAN_RTR_DATA;
  raw_tx.tx_header.DLC = CAN_TX_SICK_BUF_SIZE;

  raw_tx.tx_data[0] = (uint8_t)(sick1 >> 8);
  raw_tx.tx_data[1] = (uint8_t)(sick1 & 0xFF);
  raw_tx.tx_data[2] = (uint8_t)(sick2 >> 8);
  raw_tx.tx_data[3] = (uint8_t)(sick2 & 0xFF);
  raw_tx.tx_data[4] = (uint8_t)(sick3 >> 8);
  raw_tx.tx_data[5] = (uint8_t)(sick3 & 0xFF);
  raw_tx.tx_data[6] = (uint8_t)(sick4 >> 8);
  raw_tx.tx_data[7] = (uint8_t)(sick4 & 0xFF);

  HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->c_board), &raw_tx.tx_header,
                        raw_tx.tx_data, &can->mailbox.sick);

  return DEVICE_OK;
}