/*
  CAN总线上的设备1到7
  将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
*/

/* Includes ----------------------------------------------------------------- */
#include "can_use.h"
#include "user_math.h"
#include "device.h"
#include "error_detect.h"
#define CAN_MOTOR_ENC_RES (8191)  /* 电机编码器分辨率 */
#define CAN_MOTOR_CUR_RES (16384) 


#define CAN_GM6020_CTRL_ID_BASE (0x1ff)
#define CAN_PITCH6020_CTRL_ID (0x2ff)

#define CAN_G3508_CTRL_ALL_ID (0x200)
#define CAN_VESC_CTRL_ID_BASE (0x300)


#define CAN_MOTOR_TX_BUF_SIZE (8)
#define CAN_MOTOR_RX_BUF_SIZE (8)


/*电机最大电流绝对值*/
#define CAN_GM6020_MAX_ABS_CUR (20)
#define CAN_M3508_MAX_ABS_CUR (20)
#define CAN_M2006_MAX_ABS_CUR (10)

static CAN_RawRx_t raw_rx1;//原始的can数据
static CAN_RawRx_t raw_rx2;
CAN_RawTx_t raw_tx;


/*用于can原始数据，传入消息队列*/
static CAN_t *gcan;

/* Private function  -------------------------------------------------------- */

static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
                             const uint8_t *raw) {
  if (feedback == NULL || raw == NULL) return;
  uint16_t raw_ecd = (uint16_t)((raw[0] << 8) | raw[1]);
  int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);
	feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
  feedback->temp = raw[6]; 															 
														 
  feedback->rotor_ecd = raw_ecd ;
  feedback->torque_current =
  raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;														 
}
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
	                            const uint8_t *raw) {
	if (feedback == NULL || raw == NULL) return;			
	feedback->raw_dis[0] = (uint16_t)((raw[0] << 8) | raw[1]);														
	feedback->raw_dis[1] = (uint16_t)((raw[2] << 8) | raw[3]);																
	feedback->raw_dis[2] = (uint16_t)((raw[4] << 8) | raw[5]);
  feedback->raw_dis[3]= (uint16_t)((raw[6] << 8) | raw[7]);	
   																
}
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
                             const uint8_t *raw)
{
  if (feedback == NULL || raw == NULL) return;												 
	 union 
	{
		int x;
		uint8_t data[4];
	}speed;
	
	 speed.data[0]= raw[3];
	 speed.data[1]= raw[2];
	 speed.data[2]= raw[1];
	 speed.data[3]= raw[0];
   feedback->rotor_speed = speed.x;
   union
	 {
		 int16_t y;
		 uint8_t dat[2];
	 }current;
	 current.dat[0]= raw[5];
	 current.dat[1]= raw[4];
	 feedback->torque_current =(fp32)current.y/10;
}

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);
	raw_rx1.hcan =BSP_CAN_GetHandle(BSP_CAN_1);
  osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0);
}

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);
	
  raw_rx2.hcan =BSP_CAN_GetHandle(BSP_CAN_2);
  osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
}

/* Exported functions ------------------------------------------------------- */
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
  if (can == 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;
  return DEVICE_OK;
}

int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
                         CAN_t *can) {
  if (output == NULL) return DEVICE_ERR_NULL;

  int16_t motor1, motor2, motor3, motor4, motor5 ;

  switch (group) {
		
    case CAN_MOTOR_CHASSIS6020:
      motor1 =
          (int16_t)(output->chassis6020.named.m1);
      motor2 =
          (int16_t)(output->chassis6020.named.m2);
      motor3 =
          (int16_t)(output->chassis6020.named.m3);
      motor4 =
          (int16_t)(output->chassis6020.named.m4);

      raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
      raw_tx.tx_header.IDE = CAN_ID_STD;
      raw_tx.tx_header.RTR = CAN_RTR_DATA;
      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;

      raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
      raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
      raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
      raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
      raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
      raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
      raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
      raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);

      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
                           &raw_tx.tx_header, raw_tx.tx_data,
                           &(can->mailbox.up_6020 ));
      break;
		case CAN_MOTOR_3508:
      motor1 = 
          (int16_t)(output->motor3508.named.m1);
      motor2 = 
          (int16_t)(output->motor3508.named.m2);
      motor3 = 
          (int16_t)(output->motor3508.named.m3);
      motor4 = 
          (int16_t)(output->motor3508.named.m4);

      raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_ID;
      raw_tx.tx_header.IDE = CAN_ID_STD;
      raw_tx.tx_header.RTR = CAN_RTR_DATA;
      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;

      raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
      raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
      raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
      raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
      raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
      raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
      raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
      raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);

      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor3508),
                           &raw_tx.tx_header, raw_tx.tx_data,
                           &(can->mailbox.up_3508 ));
			break;
		case CAN_MOTOR_PITCH6020:
			 motor5 = 
          (int16_t)(output->pitch6020.named.m1);
		  raw_tx.tx_header.StdId = CAN_PITCH6020_CTRL_ID;
      raw_tx.tx_header.IDE = CAN_ID_STD;
      raw_tx.tx_header.RTR = CAN_RTR_DATA;
      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
		  
		  raw_tx.tx_data[0] = (uint8_t)((motor5 >> 8) & 0xFF);
      raw_tx.tx_data[1] = (uint8_t)(motor5 & 0xFF);
      raw_tx.tx_data[2] = 0;
      raw_tx.tx_data[3] = 0;
      raw_tx.tx_data[4] = 0;
      raw_tx.tx_data[5] = 0;
      raw_tx.tx_data[6] = 0;
      raw_tx.tx_data[7] = 0;
		
     HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->pitch6020),
                           &raw_tx.tx_header, raw_tx.tx_data,
                           &(can->mailbox.up_6020 ));
		  break;
    
    default:
      break;
  }
  return DEVICE_OK;
}
												 
//用来问答接收来自sick的数据
void CAN_Sick_Control(CAN_t *can)
{
	raw_tx.tx_header.StdId = 0x301;
	raw_tx.tx_header.IDE = CAN_ID_STD;
	raw_tx.tx_header.DLC = 0x08;
	raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡
	raw_tx.tx_data[0] = 0x00;
	raw_tx.tx_data[1] = 0x00;
	raw_tx.tx_data[2] = 0x00;
	raw_tx.tx_data[3] = 0x00;
	raw_tx.tx_data[4] = 0x00;
	raw_tx.tx_data[5] = 0x00;
	raw_tx.tx_data[6] = 0x00;
	raw_tx.tx_data[7] = 0x00;
	
	HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->sick),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.up_other ));
}	

int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){
    if (output == NULL) return DEVICE_ERR_NULL;
    int Byte[4];
   Vesc_ByteGet raw[4];
    switch (group) {
     case CAN_MOTOR_CHASSIS5065:
			 //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
			raw[0].as_array = output->chassis5065.named.m1;
		  raw[1].as_array = output->chassis5065.named.m2;
		  raw[2].as_array = output->chassis5065.named.m3;
		  raw[3].as_array = output->chassis5065.named.m4;
		 
     for(int i=0 ; i < 4 ; i ++)
     {			 
			 if(i==2)  //此处可能同时发送四个vesc导致只有三个轮子接收到数据 故加上delay
			 {
				osDelay(1);
			 }
			 //将单个电机的期望输出值通过联合体拆分
      Byte[0] = raw[i].byte.byte1;
      Byte[1] = raw[i].byte.byte2;
			Byte[2] = raw[i].byte.byte3;
			Byte[3] = raw[i].byte.byte4;
			 
      raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE;
      raw_tx.tx_header.IDE = CAN_ID_EXT;
      raw_tx.tx_header.RTR = CAN_RTR_DATA;
      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;

      raw_tx.tx_data[0] = Byte[3];
      raw_tx.tx_data[1] = Byte[2];
      raw_tx.tx_data[2] = Byte[1];
      raw_tx.tx_data[3] = Byte[0];
      raw_tx.tx_data[4] = 0;
      raw_tx.tx_data[5] = 0;
      raw_tx.tx_data[6] = 0;
      raw_tx.tx_data[7] = 0;

      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis5065),
                           &raw_tx.tx_header, raw_tx.tx_data,
                           &(can->mailbox.up_5065 ));
     }
    }
  return DEVICE_OK;
}
/**
 * @brief 将接收到的CAN消息存储在CAN_t结构体中，对容易有id冲突的进行can1和can2分类
 * @param can 要存储消息的CAN_t结构体
 * @param can_rx 接收到的CAN消息
 * @return 如果消息存储成功，DEVICE_ERR_NULL 如果can或can_rx为NULL
 */
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;

  // 处理CAN1消息
  if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_1)) {
    switch (can_rx->rx_header.StdId) {
      case CAN_G6020_AgvM1:
      case CAN_G6020_AgvM2:
      case CAN_G6020_AgvM3:
      case CAN_G6020_AgvM4:
        // 存储消息到对应的电机结构体中
        index = can_rx->rx_header.StdId - CAN_G6020_AgvM1;
        can->recive_flag |= 1 << (index);
        CAN_DJIMotor_Decode(&(can->motor.chassis6020.as_array[index]), can_rx->rx_data);
        break;

      case CAN_M3508_M1_ID:
      case CAN_M3508_M2_ID:
      case CAN_M3508_M3_ID:
      case CAN_M3508_M4_ID:
        // 存储消息到对应的电机结构体中
        index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
        can->recive_flag |= 1 << (index);
        CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data);
        break;

      default:
        break;
    }
  }

  // 处理CAN2消息
  if (can_rx->hcan == BSP_CAN_GetHandle(BSP_CAN_2)) {
    switch (can_rx->rx_header.StdId) {
      case CAN_G6020_Pitch:
        // 存储消息到对应的电机结构体中
        index = can_rx->rx_header.StdId - CAN_G6020_Pitch;
        can->recive_flag |= 1 << (index);
        CAN_DJIMotor_Decode(&(can->motor.pit6020.as_array[index]), can_rx->rx_data);
        break;
        case CAN_SICK_ID:
         // 存储消息到sickfed结构体中
         CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
        break;

      default:
        break;
    }
  }

  // 处理扩展ID消息
  switch (can_rx->rx_header.ExtId) {
		
    case CAN_VESC5065_M1_MSG1:
      // 存储消息到对应的电机结构体中
      CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[0]), can_rx->rx_data);
      break;

    case CAN_VESC5065_M2_MSG1:
      // 存储消息到对应的电机结构体中
      CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[1]), can_rx->rx_data);
      break;

//    case CAN_VESC5065_M3_MSG1:
//      // 存储消息到对应的电机结构体中
//      CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[2]), can_rx->rx_data);
//      break;

//    case CAN_VSEC5065_M4_MSG1:
//      // 存储消息到对应的电机结构体中
//      CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[3]), can_rx->rx_data);
      break;



    default:
      break;
  }

  return DEVICE_OK;
}	
bool_t CAN_CheckFlag(CAN_t *can, uint32_t flag) {
  if (can == NULL) return false;
  return (can->recive_flag & flag) == flag;
}

int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag) {
  if (can == NULL) return DEVICE_ERR_NULL;
  can->recive_flag &= ~flag;
  return DEVICE_OK;
}