/*
 底盘任务

*/
/* Includes ----------------------------------------------------------------- */
#include "chassis.h"
#include "device/motor_rm.h"
#include "bsp/time.h"
#include "bsp/can.h"
#include "math.h"
#include "component/pid.h"
#include "component/filter.h"

/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
查看6020反馈值，将6020反馈值放入motor_offset中*/

/*chassis_t结构体中的内容现在有 move_vec (最终输出速度)
hopemotorout(期望的底盘输出值)舵轮解算出的各个电机的期望输出值 包括四个6020 和四个3508
final_out;(经PID计算后的实际发送给电机的实时输出值)  四个6020 四个3508
motorfeedback(电机反馈数据) 四个3508 四个6020
pid 各个电机的pid参数
ChassisImu_t pos088;  088的实时姿态
 MotorOffset_t motoroffset;	      //5065校准数据
const	Chassis_Param_t *param;     //一些固定的参数
fp32 vofa_send[8];                //vofa输出数据
  LowPassFilter2p_t filled[9];      //低通滤波器
  float keep_angle[4];              // 保持的 6020 角度
*/

/*底盘初始化*/

int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
{
  if (c == NULL || param == NULL || target_freq <= 0.0f)
  {
    return CHASSIS_ERR; // 参数错误
  }
  c->param = param;
  c->mode = STOP; // 默认模式为停止锁死底盘
  /*初始化can*/
  BSP_CAN_Init();
  /*注册3508电机*/
  for (int i = 0; i < 4; i++)
  {
    MOTOR_RM_Register(&(c->param->motor_3508_param[i]));
  }
  /*注册6020电机*/
  for (int i = 0; i < 4; i++)
  {
    MOTOR_RM_Register(&(c->param->motor_6020_param[i]));
  }

  // 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
  MotorOffset_t motor_offset = {{3.14005876 / M_PI * 180.0f, 5.25618553 / M_PI * 180.0f, 2.10462165 / M_PI * 180.0f, 5.26108162 / M_PI * 180.0f}}; // 右右右右
  // MotorOffset_t motor_offset = {{0.0 / M_PI * 180.0f, 2.07317519 / M_PI * 180.0f, 5.23394215 / M_PI * 180.0f, 2.12379646 / M_PI * 180.0f}};//左左左左
  // MotorOffset_t motor_offset = {{6.28088474 / M_PI * 180.0f, 2.08161187 / M_PI * 180.0f, 2.09618473 / M_PI * 180.0f, 5.25465155 / M_PI * 180.0f}};//左左右右
  c->motoroffset = motor_offset;
  /*对3508的速度环和6020的角速度以及位置环pid进行初始化*/
  for (int i = 0; i < 4; i++)
  {
    PID_Init(&c->pid.chassis_3508VPID[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508v_param);
  }
  for (int i = 0; i < 4; i++)
  {
    PID_Init(&c->pid.chassis_6020OmegaPid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Omega_param);
    PID_Init(&c->pid.chassis_6020anglePid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Angle_param);
  }
  /*对遥控器的xyw进行低通滤波*/
  LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
  LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
  LowPassFilter2p_Init(&c->filled[2], target_freq, 20.0f); // vw

  LowPassFilter2p_Init(&c->filled[3], target_freq, 20.0f); // 3508-1
  LowPassFilter2p_Init(&c->filled[4], target_freq, 20.0f); // 3508-2
  LowPassFilter2p_Init(&c->filled[5], target_freq, 20.0f); // 3508-3  
  LowPassFilter2p_Init(&c->filled[6], target_freq, 20.0f); // 3508-4

  LowPassFilter2p_Init(&c->filled[7], target_freq, 20.0f); // 6020-1
  LowPassFilter2p_Init(&c->filled[8], target_freq, 20.0f); // 6020-2
  LowPassFilter2p_Init(&c->filled[9], target_freq, 20.0f); // 6020-3
  LowPassFilter2p_Init(&c->filled[10], target_freq, 20.0f); // 6020-4 

  // c->set_point.yaw = 0.0f;
  return CHASSIS_OK;
}

static int8_t Chassis_SetMode(Chassis_t *c, Chassis_CMD_t *c_cmd)
{
  if (c == NULL)
    return CHASSIS_ERR_NULL; /* 主结构体不能为空 */

  for (int i = 0; i < 4; i++)
  {
    PID_Reset(&c->pid.chassis_3508VPID[i]);
    PID_Reset(&c->pid.chassis_6020OmegaPid[i]);
    PID_Reset(&c->pid.chassis_6020anglePid[i]);
  }

  c->mode = c_cmd->mode;
  return CHASSIS_OK;
}
float angle_error[4]; // 角度误差
// 底盘解算
void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
{

  // RC模式下松开遥控器防止6020回到默认位置导致侧翻
  if (c->mode == RC && fabs(c->move_vec.Vx) < 0.1 && fabs(c->move_vec.Vy) < 0.1 && fabs(c->move_vec.Vw) < 0.1)
  {
    // 如果之前不处于保持模式，则记录当前角度
    if (!c->keeping_angle_flag)
    {
      c->keeping_angle_flag = 1; // 进入保持模式
    }
    // 使用保持的角度，而不是实时反馈值,速度为0
    for (uint8_t i = 0; i < 4; i++)
    {
      c->hopemotorout.rotor6020_jiesuan_1[i] = c->keep_angle[i];
      c->hopemotorout.rotor3508_jiesuan_1[i] = 0;
    }
      // c->hopemotorout.rotor6020_jiesuan_1[0] = 315;
      // c->hopemotorout.rotor6020_jiesuan_1[1] = 45;
      // c->hopemotorout.rotor6020_jiesuan_1[2] = 315;
      // c->hopemotorout.rotor6020_jiesuan_1[3] = 45;
  }
  else
  {
    // 如果有速度输入，则退出保持模式
    c->keeping_angle_flag = 0;
    // 让保持角度实时等于进入保持阈值前的最后一次角度值
    for (uint8_t i = 0; i < 4; i++)
    {
      c->keep_angle[i] = c->hopemotorout.rotor6020_jiesuan_1[i];
    }
    switch (c->mode)
    {
    case RC:

      // const double radians = atan(1.0f * 330 / 330);

      c->hopemotorout.rotor3508_jiesuan_1[0] = sqrt(
          (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));

      c->hopemotorout.rotor3508_jiesuan_1[1] = sqrt(
          (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));

      c->hopemotorout.rotor3508_jiesuan_1[2] = sqrt(
          (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));

      c->hopemotorout.rotor3508_jiesuan_1[3] = sqrt(
          (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));

      c->hopemotorout.rotor6020_jiesuan_1[0] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
                                                     (c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
                                               (180.0f / M_PI);
      c->hopemotorout.rotor6020_jiesuan_1[1] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
                                                     (c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
                                               (180.0f / M_PI);
      c->hopemotorout.rotor6020_jiesuan_1[2] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
                                                     (c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
                                               (180.0f / M_PI);
      c->hopemotorout.rotor6020_jiesuan_1[3] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
                                                     (c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
                                               (180.0f / M_PI);
      break;

    case STOP:

      for (int i = 0; i < 4; i++)
      {
        c->hopemotorout.rotor3508_jiesuan_1[i] = 0.0f;
      }
      //      c->hopemotorout.rotor6020_jiesuan_1[0] = 6.26554489/M_PI*180.0f;
      //      c->hopemotorout.rotor6020_jiesuan_1[1] = 2.1099906/M_PI*180.0f;
      //      c->hopemotorout.rotor6020_jiesuan_1[2] = 2.08391285/M_PI*180.0f;
      //      c->hopemotorout.rotor6020_jiesuan_1[3] = 5.26845694/M_PI*180.0f;
      c->hopemotorout.rotor6020_jiesuan_1[0] = 315;
      c->hopemotorout.rotor6020_jiesuan_1[1] = 45;
      c->hopemotorout.rotor6020_jiesuan_1[2] = 315;
      c->hopemotorout.rotor6020_jiesuan_1[3] = 45;

      break;

    case LOCK:

      for (int i = 0; i < 4; i++)
      {
        c->hopemotorout.rotor3508_jiesuan_1[i] = 0.0f;
      }
      c->hopemotorout.rotor6020_jiesuan_1[0] = 0;
      c->hopemotorout.rotor6020_jiesuan_1[1] = 0;
      c->hopemotorout.rotor6020_jiesuan_1[2] = 0;
      c->hopemotorout.rotor6020_jiesuan_1[3] = 0;
      break;
    }
  }
  // 角度归化到0°——360°

  for (uint8_t i = 0; i < 4; i++)
  {
    if (c->hopemotorout.rotor6020_jiesuan_1[i] < 0)
    {
      c->hopemotorout.rotor6020_jiesuan_1[i] += 360;
    }
  }

  for (uint8_t i = 0; i < 4; i++)
  {
    angle_error[i] = c->hopemotorout.rotor6020_jiesuan_1[i] - c->motorfeedback.rotor_angle6020[i];
    // 误差角度归化到-180°——+180°
    while (angle_error[i] > 180)
      angle_error[i] -= 360;
    while (angle_error[i] < -180)
      angle_error[i] += 360;
    /*这里发现如果下面的c->motorfeedback.rotor_angle6020[i]+angle_error[i]变为
        c->hopemotorout.rotor6020_jiesuan_1[i]会导致6020出现故障*/
    if (angle_error[i] > 90 && angle_error[i] <= 180)
    {
      c->hopemotorout.rotor3508_jiesuan_2[i] = c->hopemotorout.rotor3508_jiesuan_1[i];
      c->hopemotorout.rotor6020_jiesuan_2[i] = c->motorfeedback.rotor_angle6020[i] + angle_error[i] - 180;
    }
    else if (angle_error[i] < -90 && angle_error[i] >= -180)
    {
      c->hopemotorout.rotor3508_jiesuan_2[i] = c->hopemotorout.rotor3508_jiesuan_1[i];
      c->hopemotorout.rotor6020_jiesuan_2[i] = c->motorfeedback.rotor_angle6020[i] + angle_error[i] + 180;
    }
    else
    {
      c->hopemotorout.rotor3508_jiesuan_2[i] = -c->hopemotorout.rotor3508_jiesuan_1[i];
      c->hopemotorout.rotor6020_jiesuan_2[i] = c->motorfeedback.rotor_angle6020[i] + angle_error[i];
    }
  }
}

int8_t Chassis_update(Chassis_t *c)
{
  if (c == NULL)
  {
    return CHASSIS_ERR_NULL; // 参数错误
  }
  /*更新所有电机数据*/
  MOTOR_RM_UpdateAll();

  /*更新电机反馈*/
  for (int i = 0; i < 4; i++)
  {
    // c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) );
    c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / M_PI * 180.0f);
    c->motorfeedback.rotor_rpm6020[i] = (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / 320);
    c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) /10000;
    //      c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));

    c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
    if (c->motorfeedback.rotor_angle6020[i] < 0)
    {
      c->motorfeedback.rotor_angle6020[i] += 360;
    }
  }

  return CHASSIS_OK;
}

// 防止侧翻
/*具体开始上车实验之后在进行编写修改，先暂时不使用*/
void ChassisrolPrevent(Chassis_t *c)
{
}
float chassis6020_detangle[4]; // 6020解算出的角度
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
{

  if (c == NULL || c_cmd == NULL)
  {
    return CHASSIS_ERR_NULL; // 参数错误
  }

  c->dt = (BSP_TIME_Get_us() - c->last_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
  c->last_wakeup = BSP_TIME_Get_us();

  /*设置底盘模式*/
  if (Chassis_SetMode(c, c_cmd) != CHASSIS_OK)
  {
    return CHASSIS_ERR_MODE; /* 设置模式失败 */
  }

  
  /*根据底盘模式进行不同的控制*/
  switch (c->mode)
  {
  case STOP:
    // 停止模式
    c->move_vec.Vx = 0.0f;
    c->move_vec.Vy = 0.0f;
    c->move_vec.Vw = 0.0f;
    break;

  case RC:
    // 遥控模式
    // c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle * 1000.0f;
    // c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle * 1000.0f;
    c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle;
    c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle;
    c->move_vec.Vw = c_cmd->Vw * c_cmd->throttle;

    
    break;

  default:
    return CHASSIS_ERR_MODE; /* 未知模式 */
  }

  /*给输出的Vx，Vy，Vw进行滤波*/
  c->move_vec.Vx = LowPassFilter2p_Apply(&c->filled[0], c->move_vec.Vx);
  c->move_vec.Vy = LowPassFilter2p_Apply(&c->filled[1], c->move_vec.Vy);
  c->move_vec.Vw = LowPassFilter2p_Apply(&c->filled[2], c->move_vec.Vw);

  Chassis_speed_calculate(c, c_cmd);

  for (int i = 0; i < 4; i++)
  {
    c->hopemotorout.motor6020_target[i] = c->hopemotorout.rotor6020_jiesuan_2[i];
    chassis6020_detangle[i] = PID_Calc(&(c->pid.chassis_6020anglePid[i]), c->hopemotorout.motor6020_target[i],
                                       c->motorfeedback.rotor_angle6020[i], 0.0f, c->dt);
    // c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
    c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i],
                                             c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt);

    c->out.rotor6020_out[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_6020out[i]);

    c->hopemotorout.motor3508_target[i] = c->hopemotorout.rotor3508_jiesuan_2[i];
    c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508VPID[i]), c->hopemotorout.motor3508_target[i],
                                             c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
    c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
  }
  return CHASSIS_OK;
}

/*电机输出设定和发送*/
void Chassis_Setoutput(Chassis_t *c)
{
  for (int i = 0; i < 4; i++)
  {
    MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
    MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
  }
  // MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f);
  MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
  MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));

  /*最后这里的数据处理发送和PID计算还未处理写，后面更改，现在注释部分是王志城的代码*/

  // #ifdef calibration
  // #else
  //   for (uint8_t i = 0 ; i <4 ; i++){

  // 	  c->hopemotorout.motor6020_target[i]=c->hopemotorout.rotor6020_jiesuan_2[i];
  // 	  chassis6020_detangle[i] = PID_calc(&(c->pid.chassis_6020anglePid[i]),c->motorfeedback.rotor_angle6020[i],
  // 		                                                                    c->hopemotorout.motor6020_target[i]);
  // 	  c->final_out.final_6020out[i] = PID_calc(&(c->pid.chassis_6020OmegaPid[i]),c->motorfeedback.rotor_rpm6020[i],
  // 		                                                                                    chassis6020_detangle[i]);
  // 	  out->chassis6020.as_array[i] = c->final_out.final_6020out[i];

  // 	  c->hopemotorout.motor5065_target[i]=c->hopemotorout.rotor5065_jiesuan_2[i];
  // 		c->final_out.final_5065out[i]=c->hopemotorout.motor5065_target[i];
  // 		out->chassis5065.erpm[i] = c->final_out.final_5065out[i];
  // 	}
  // #endif
}