/**
  ****************************(C) COPYRIGHT ZhouCc****************************
  * @file       djiMotor.c/h
  * @brief      大疆电机接收发送程序
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023.12.11			ZhouCc					完成
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT ZhouCc****************************
  */

#include "djiMotor.h"
#include "can_it.h"
#include "can_init.h"
#if FREERTOS_DJI == 1
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"
#endif

//motor data read
#define get_motor_measure(ptr, data)                     					\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
				if ((ptr)->ecd - (ptr)->last_ecd > 4096)                         \
          (ptr)->round_cnt--;                                            \
        else if ((ptr)->ecd - (ptr)->last_ecd < -4096)                   \
          (ptr)->round_cnt++;																\
				if((ptr)->type == M3508)													\
					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508;      \
				else if((ptr)->type == M2006)																\
					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006;      \
				else																											\
				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;      \
				(ptr)->real_round = (ptr)->total_angle / 360;													\
				(ptr)->real_angle = (ptr)->total_angle % 360;													\
    }
 //单圈绝对值式解析   
 #define get_6020_motor_measure(ptr, data)                     								\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
        (ptr)->real_round=(ptr)->ecd/ (float)CAN_MOTOR_ENC_RES * 360.0f; \
				(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt);																	\
				(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt);																\
				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;														\
    } 

/*(ptr)->real_angle = (ptr)->real_angle % 360;	*/
#define get_motor_offset(ptr, data)																			\
		{																																		\
				(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
				(ptr)->offset_ecd = (ptr)->ecd;																	\
		}
		
//  static void VescMotor_Decode(CAN_MotorFeedback_t *feedback, CAN_RxHeaderTypeDef *rx_header, const uint8_t *rx_data)
//  {
//           // 检查接收到的ID是否匹配我们期望的VESC状态消息ID
//           // 提取数据字段
//           feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
//           feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
//           feedback->torque_current /= 1000;  // 将单位从 0.1A 转换为实际电流值
//           feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
//           feedback->duty_cycle /= 1000.0f;  // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)

//   }

//小米
#define ID_xiaomi 1		
#define Pmax 1		
#define speed_full_scale 200.0f
#define current_full_scale 4.0f


#if DEBUG == 1

//电机回传数据结构体
motor_measure_t motor_chassis[10];
CAN_MotorFeedback_t motor_5065[3];
#else
static motor_measure_t motor_chassis[5];
#endif
static CAN_TxHeaderTypeDef  tx_meessage_1ff;
static uint8_t              can_send_data_1ff[8];
static CAN_TxHeaderTypeDef  tx_meessage_2ff;
static uint8_t              can_send_data_2ff[8];
static CAN_TxHeaderTypeDef  tx_message_200;
static uint8_t              can_send_data_200[8];

//can1 dji
CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8];

CAN_RawTx_t raw_tx;

//can2 vesc xiaomi
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];

static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t  vesc_send_data[4];
//小米
uint8_t can_send_data[8];
// 定义 CAN 传输头部信息
static CAN_TxHeaderTypeDef tx_message;

MotorFeedback_t MotorFeedback;

/**
 * @brief           数据处理函数
 * @param[in]       none
 * @retval          none
*/
void djiMotorEncode()
{
    switch (dji_rx_header.StdId)
    {
    case M3508_1:
    case M3508_2:
    case M3508_3:
    case M3508_4:
    case M2006_1:
    {
        static uint8_t i = 0;
        //get motor id
        i = dji_rx_header.StdId - 0x201;
        if(motor_chassis[i].msg_cnt<=50)
        {
            motor_chassis[i].msg_cnt++;
            get_motor_offset(&motor_chassis[i], dji_rx_data);
        }else{
            get_motor_measure(&motor_chassis[i], dji_rx_data);
        }
        break;
    }
    case GM6020_1:
    {
      if(motor_chassis[6].msg_cnt<=50)
      {
        motor_chassis[6].msg_cnt++;
        get_motor_offset(&motor_chassis[6], dji_rx_data);
      }else{
        get_6020_motor_measure(&motor_chassis[6], dji_rx_data);
      }
        break;
    }
    case GM6020_2:
    {
      if(motor_chassis[7].msg_cnt<=50)
      {
        motor_chassis[7].msg_cnt++;
        get_motor_offset(&motor_chassis[7], dji_rx_data);
      }else{
        get_6020_motor_measure(&motor_chassis[7], dji_rx_data);
      }
        break;
    }

    default:
    {
        break;
    }
    }
}

void can2MotorEncode()
{

    switch (rx_data[0]) 
  {
				 case ID_xiaomi:
				 Parse_CAN_Response (rx_data,&MotorFeedback);
				 break ;
  
   default:
    break;
  }

}

#if FREERTOS_DJI == 0
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
    
    djiMotorEncode();
    
}
#else
//static osEventFlagsId_t eventReceive;
static osThreadId_t thread_alert;
/**
  * @brief          自定义大疆电机回调函数
  * @param[in]      none
  * @retval         none
  */
void Dji_Motor_CB()
{
  HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
  osThreadFlagsSet(thread_alert, EVENT_CAN1);

}
void can2_Motor_CB(void)
{
  HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
  osThreadFlagsSet(thread_alert, EVENT_CAN2);
}

/**
 * @brief     大疆电机初始化
 * @param     none
 * @retval    none
*/
void djiInit(void)
{
  thread_alert = osThreadGetId();
  BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
                          Dji_Motor_CB);
  BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
                         can2_Motor_CB);                        
                         
 can_filter_init();
}

/**
 * @brief 等待新数据
*/
uint32_t waitNewDji() 
{
  // 等待CAN1或CAN2任意一个事件
  return osThreadFlagsWait(
    EVENT_CAN1 | EVENT_CAN2, osFlagsWaitAny, osWaitForever);
}
#endif

/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x205) 电机控制电流
  * @param[in]      motor2: (0x206) 电机控制电流
  * @param[in]      motor3: (0x207) 电机控制电流
  * @param[in]      motor4: (0x208) 电机控制电流
  * @retval         none
  */
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_meessage_1ff.StdId = 0x1FF;
    tx_meessage_1ff.IDE = CAN_ID_STD;
    tx_meessage_1ff.RTR = CAN_RTR_DATA;
    tx_meessage_1ff.DLC = 0x08;
    can_send_data_1ff[0] = (motor1 >> 8);
    can_send_data_1ff[1] = motor1;
    can_send_data_1ff[2] = (motor2 >> 8);
    can_send_data_1ff[3] = motor2;
    can_send_data_1ff[4] = (motor3 >> 8);
    can_send_data_1ff[5] = motor3;
    can_send_data_1ff[6] = (motor4 >> 8);
    can_send_data_1ff[7] = motor4;
    HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
}

/**
  * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
  * @param[in]      motor1: (0x201) 电机控制电流
  * @param[in]      motor2: (0x202) 电机控制电流
  * @param[in]      motor3: (0x203) 电机控制电流
  * @param[in]      motor4: (0x204) 电机控制电流
  * @retval         none
  */
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_message_200.StdId = 0x200;
    tx_message_200.IDE = CAN_ID_STD;
    tx_message_200.RTR = CAN_RTR_DATA;
    tx_message_200.DLC = 0x08;
    can_send_data_200[0] = motor1 >> 8;
    can_send_data_200[1] = motor1;
    can_send_data_200[2] = motor2 >> 8;
    can_send_data_200[3] = motor2;
    can_send_data_200[4] = motor3 >> 8;
    can_send_data_200[5] = motor3;
    can_send_data_200[6] = motor4 >> 8;
    can_send_data_200[7] = motor4;

    HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
}

/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x209) 电机控制电流
  * @param[in]      motor2: (0x20A) 电机控制电流
  * @param[in]      motor3: (0x20B) 电机控制电流
  * @retval         none
  */
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_meessage_2ff.StdId = 0x2FF;
    tx_meessage_2ff.IDE = CAN_ID_STD;
    tx_meessage_2ff.RTR = CAN_RTR_DATA;
    tx_meessage_2ff.DLC = 0x08;
    can_send_data_2ff[0] = (motor1 >> 8);
    can_send_data_2ff[1] = motor1;
    can_send_data_2ff[2] = (motor2 >> 8);
    can_send_data_2ff[3] = motor2;
    can_send_data_2ff[4] = (motor3 >> 8);
    can_send_data_2ff[5] = motor3;
    can_send_data_2ff[6] = (0 >> 8);
    can_send_data_2ff[7] = 0;
    HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
}

//控制四个vesc的发送 
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan)
{
 
  uint32_t send_mail_box;
  int Byte[4];
  Vesc_ByteGet raw[4];

    //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
   raw[0].as_array = speed;
   raw[1].as_array = speed;

  for(int i=0 ; i < 2 ; i ++)
  {			 

    //将单个电机的期望输出值通过联合体拆分
   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(hcan, &raw_tx.tx_header, raw_tx.tx_data, &send_mail_box);
  }

return DEVICE_OK;

}

/**
   * @brief          限制vesc电机转速
   * @param[in/out]  rpm:  vesce电机转速
   * @retval         none
   *
   * 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
   * 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
   */
  void assert_param_rpm(float *rpm){
    if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
      *rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
  }
  
  /**
    * @brief          使vesc电机进入转速模式
    * @param[in]      controller_id: vesc电机控制器id
    * @param[in]      RPM: 电机转速
    * @retval         RPM（1000-50000之间的数）
    */
  void  CAN_VESC_RPM(uint8_t controller_id, float RPM)
  {
        uint32_t id;
        int32_t data;
        uint32_t send_mail_box;
    
        id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
        //assert_param_rpm(&RPM);
        data = (int32_t)(RPM);
    
        vesc_tx_message.ExtId = id;
        vesc_tx_message.IDE = CAN_ID_EXT;
        vesc_tx_message.RTR = CAN_RTR_DATA;
        vesc_tx_message.DLC = 0x04;
        vesc_send_data[0] = data >> 24 ;
        vesc_send_data[1] = data >> 16 ;
        vesc_send_data[2] = data >> 8 ;
        vesc_send_data[3] = data ;
    
        HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
  }
  
  
  /**
    * @brief          使vesc电机进入制动模式
    * @param[in]      controller_id: vesc电机控制器id
    * @retval         none
    */
  void  CAN_VESC_HEAD(uint8_t controller_id)
  {
        uint32_t id;
        uint32_t send_mail_box;
    
        id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
    
        vesc_tx_message.ExtId = id;
        vesc_tx_message.IDE = CAN_ID_EXT;
        vesc_tx_message.RTR = CAN_RTR_DATA;
        vesc_tx_message.DLC = 0x04;
    
        HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
}

/**
  * @brief          返回电机数据指针
  * @param[in]      i: 电机编号
  * @retval         电机数据指针
  */
 motor_measure_t *get_motor_point(uint8_t i)
 {
     return &motor_chassis[i];
 }

 CAN_MotorFeedback_t *get_vesc_point(uint8_t i)
 {
    return &motor_5065[i];
 }


 //小米
 void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) {
  // 1. 解析位置
  int16_t pos_code = (rx_data[1] << 8) | rx_data[2];
  feedback->position_deg = ((int16_t)(pos_code - 0x8000) / 32768.0f) * 360.0f * Pmax;

  // 2. 解析速度
  int16_t speed_code = ((rx_data[3] << 4) | (rx_data[4] >> 4)) - 0x800;
  feedback->speed_rads = (speed_code / 2048.0f) * speed_full_scale;

  // 3. 解析电流
  int16_t current_code = (((rx_data[4] & 0x0F) << 8) | rx_data[5]) - 0x800;
  feedback->current_A = (current_code / 2048.0f) * current_full_scale;

  // 记录电机ID
  feedback->can_id = rx_data[0];
}

void XiaomiWait_init(int id,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;

    tx_message.StdId = id;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_DATA;
    tx_message.DLC = 0x08;
	
		can_send_data[0] = 0xFF;
		can_send_data[1] = 0xFF;
		can_send_data[2] = 0xFF;
		can_send_data[3] = 0xFF;
		can_send_data[4] = 0xFF;
		can_send_data[5] = 0xFF;
		can_send_data[6] = 0xFF;
		can_send_data[7] = 0xFC;	

  	HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
}


void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
	  static int is_open=0;  //是否开启电机，根据手册，第一次使用需要ff开启
    tx_message.StdId = id;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_DATA;
    tx_message.DLC = 0x08;
	if(is_open){

		uint16_t pos_code = (uint16_t)((JZ_xiaomi->position  / (Pmax * 360.0f)) * 32768.0f + 32768.0f);
		uint16_t speed_code = (uint16_t)((JZ_xiaomi->speed  / 200.0f) * 2048.0f + 2048.0f);
		uint16_t current_code = (uint16_t)((JZ_xiaomi->K_C  / 4.0f) * 2048.0f + 2048.0f);

		can_send_data[0] = (pos_code >> 8) & 0xFF;    // 位置高8位
		can_send_data[1] = pos_code & 0xFF;            // 位置低8位

		can_send_data[2] = (speed_code >> 4) & 0xFF;  // 速度高8位（取12位中的高8）
		can_send_data[3] = ((speed_code & 0x0F) << 4) | ((JZ_xiaomi->K_P >> 8) & 0x0F); // 速度低4位 + Kp高4位

		can_send_data[4] = JZ_xiaomi->K_P  & 0xFF;                 // Kp低8位  位置增益

		can_send_data[5] = (JZ_xiaomi->K_D  >> 4) & 0xFF;           // Kd高8位（取12位中的高8） 位置阻尼
		can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位

		can_send_data[7] = current_code & 0xFF;       // 电流低8位	 力矩
			
	}
	else
	{
		can_send_data[0] = 0xFF;
		can_send_data[1] = 0xFF;
		can_send_data[2] = 0xFF;
		can_send_data[3] = 0xFF;
		can_send_data[4] = 0xFF;
		can_send_data[5] = 0xFF;
		can_send_data[6] = 0xFF;
		can_send_data[7] = 0xFC;	
		is_open=1;
		}

	
  	HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
}

MotorFeedback_t * get_CyberGear_point()
{
    return &MotorFeedback;
}
