/*
 底盘任务
*/

/* Includes ----------------------------------------------------------------- */
#include "chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include <math.h>
#include "module/cmd.h"
#include "stdlib.h"

/* Private typedef ---------------------------------------------------------- */

static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode, uint32_t now)
{
  if (!c)
    return CHASSIS_ERR_NULL;
  if (mode == c->mode)
    return CHASSIS_OK;

  // 重置 PID 和滤波器
  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    PID_Reset(&c->pid.motor[i]);
    LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
    LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
  }

  c->mode = mode;
  return CHASSIS_OK;
}

/**
 * @brief 底盘初始化
 * @param c 底盘结构体指针
 * @param param 底盘参数结构体指针
 * @param target_freq 控制频率(Hz)
 * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空指针 CHASSIS_ERR_TYPE:不支持的底盘类型
 */
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param,
                    float target_freq)
{
  if (!c)
    return CHASSIS_ERR_NULL;

  // 初始化 CAN 通信
  BSP_CAN_Init();
  c->param = param;
  c->mode = STOP;
  // 根据底盘类型配置轮组数量与混控器模式
  Mixer_Mode_t mixer_mode;
  switch (param->type)
  {
  case CHASSIS_TYPE_MECANUM: // 麦克纳姆轮
    c->num_wheel = 4;
    mixer_mode = MIXER_MECANUM;
    break;
  case CHASSIS_TYPE_PARLFIX4:
    c->num_wheel = 4;
    mixer_mode = MIXER_PARLFIX4;
    break;
  case CHASSIS_TYPE_PARLFIX2:
    c->num_wheel = 2;
    mixer_mode = MIXER_PARLFIX2;
    break;
  case CHASSIS_TYPE_OMNI_CROSS:
    c->num_wheel = 4;
    mixer_mode = MIXER_OMNICROSS;
    break;
  case CHASSIS_TYPE_OMNI_PLUS: // 全向轮（十字布局）
    c->num_wheel = 4;
    mixer_mode = MIXER_OMNIPLUS;
    break;
  case CHASSIS_TYPE_SINGLE:
    c->num_wheel = 1;
    mixer_mode = MIXER_SINGLE;
    break;
  default:
    return CHASSIS_ERR_TYPE;
  }
  // 初始化时间戳
  c->last_wakeup = 0;
  c->dt = 0.0f;
  // 初始化 PID 和滤波器
  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
    LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
    LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
    // 清零电机反馈
    c->feedback.motor[i].rotor_speed = 0;
    c->feedback.motor[i].torque_current = 0;
    c->feedback.motor[i].rotor_abs_angle = 0;
    c->feedback.motor[i].temp = 0;
  }
  // 初始化跟随 PID 和混控器
  PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->pid.follow_pid_param);
  Mixer_Init(&c->mixer, mixer_mode);
  // 清零运动向量与输出
  c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    c->out.motor[i] = 0.0f;
  }
  // 注册电机
  for (int i = 0; i < c->num_wheel; i++)
  {
    MOTOR_RM_Register(&(c->param->motor_param[i]));
  }
  return CHASSIS_OK;
}

/**
 * @brief 更新底盘反馈（电机状态）
 * @param c 底盘结构体指针
 * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空指针
 */
int8_t Chassis_UpdateFeedback(Chassis_t *c)
{

  // 更新所有电机反馈
  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    MOTOR_RM_Update(&(c->param->motor_param[i]));
    MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
    c->motors[i] = rm_motor;
    // MOTOR_RM_t *rm = c->motors[i];
    if (rm_motor != NULL)
    {
      c->feedback.motor[i] = rm_motor->feedback;
    }
    else
    {
      return CHASSIS_ERR_NULL;
    }
  }
  return CHASSIS_OK;
}

/**
 * @brief 底盘电机控制
 * @param c 底盘结构体指针
 * @param c_cmd 控制命令
 * @param now 当前时间戳(ms)
 * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空指针
 */
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now)
{
  if (!c || !c_cmd)
    return CHASSIS_ERR_NULL;
  // 计算控制周期
  c->dt = (float)(now - c->last_wakeup) / 1000.0f;
  c->last_wakeup = now;
  if (!isfinite(c->dt) || c->dt <= 0.0f)
  {
    c->dt = 0.001f;
  }
  if (c->dt < 0.0005f)
    c->dt = 0.0005f;
  if (c->dt > 0.050f)
    c->dt = 0.050f;
  // 设置模式
  Chassis_SetMode(c, c_cmd->mode, now);
  // 根据模式计算运动向量
  switch (c->mode)
  {
  case STOP:
    c->move_vec.vx = c->move_vec.vy = 0.0f;
    c->move_vec.wz = 0.0f;
    break;
  case RC:
    c->move_vec.vx = c_cmd->Vx;
    c->move_vec.vy = c_cmd->Vy;
    c->move_vec.wz = c_cmd->Vw;

    break;
  default:
    break;
  }

  Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);

  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    float rf = c->setpoint.motor_rpm[i]; /// 目标转速
    float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
    float out_current;
    switch (c->mode)
    {
    case RC:
      out_current = c->setpoint.motor_rpm[i] / 1000.0f;
      break;
    case STOP:
      out_current = 0.0f;
      break;
    default:
      out_current = 0.0f;
      break;
    }

    (void)rf;
    (void)fb;

    // 低通滤波和限幅
    c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
    // Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
  }

  return CHASSIS_OK;
}


/**
 * @brief 电机输出
 * @param c 底盘结构体指针
 */
void Chassis_Output(Chassis_t *c)
{
  if (!c)
    return; // 空指针检查

  // 1. 设置每个电机的输出值
  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    MOTOR_RM_t *rm = c->motors[i];
    if (!rm)
      continue;
    MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]); // 将计算好的电流值写入电机结构体
  }

  // 2. 触发CAN发送（只调用一次）
  MOTOR_RM_t *rm = c->motors[0];
  if (rm)
  {
    MOTOR_RM_Ctrl(&rm->param); // 通过CAN总线发送控制帧
  }
}

/**
 * @brief 重置底盘输出
 * @param c 底盘结构体指针
 */
void Chassis_ResetOutput(Chassis_t *c)
{
  if (!c)
    return;
  for (uint8_t i = 0; i < c->num_wheel; i++)
  {
    MOTOR_RM_t *m = c->motors[i];
    if (m)
    {
      MOTOR_RM_Relax(&(m->param));
    }
  }
}