/*
 * 底盘模组
 */

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

#include <stdlib.h>

#include "cmsis_os2.h"
#include "device/go.h"
#include "component/limiter.h"
#include "component/kinematics.h"
#include "component/path.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */

#define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */

#define CHASSIS_MAX_SPEED (0.03f) /*调试用速度限幅*/
/* Private macro ------------------------------------------------------------ */


/* Private variables -------------------------------------------------------- */
static Kinematics_JointCMD_t zero_torque = {
    .T = 0.0f, /* 零力矩 */
    .W = 0.0f, /* 零速度 */
    .Pos = 0.0f, /* 零位置 */
    .K_P = 0.0f, /* 零刚度系数 */
    .K_W = 0.0f, /* 零速度系数 */
};

static Kinematics_JointCMD_t damping_torque = {
    .T = 0.0f, /* 零力矩 */
    .W = 0.0f, /* 阻尼速度 */
    .Pos = 0.0f, /* 零位置 */
    .K_P = 0.0f, /* 零刚度系数 */
    .K_W = 0.1f, /* 阻尼速度系数 */
};

//位控模式
static Kinematics_JointCMD_t position_mode = {
    .T = 0.0f, /* 零力矩 */
    .W = 0.0f, /* 零速度 */
    .Pos = 0.0f, /* 零位置 */
    .K_P = 3.0f, /* 刚度系数 */
    .K_W = 0.2f, /* 速度系数 */
};

// static uint8_t chassis_mode_states = 0;
// static uint8_t chassis_action_states = 0;
static uint8_t stage = 0;
/* Private function  -------------------------------------------------------- */
/**
 * \brief 设置底盘运行模式
 *
 * \param c 包含底盘数据的结构体
 * \param mode 要设置的模式
 *
 * \return 函数运行结果
 */
static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisCmd_t *c_cmd) {
  if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
  if (c_cmd == NULL) return CHASSIS_ERR_NULL; /* 控制指令不能为空 */

  if (c->mode != c_cmd->mode) { /* 如果当前模式和要设置的模式不同 */
    c->mode = c_cmd->mode; /* 更新底盘模式 */
    stage = 0; /* 重置阶段 */
    c->time = 0.0f; /* 重置时间 */
    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
  }
  if (c->action != c_cmd->action) { /* 如果当前动作和要设置的动作不同 */
    c->action = c_cmd->action; /* 更新底盘动作 */
    stage = 0; /* 重置阶段 */
    c->time = 0.0f; /* 重置时间 */
    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
  }
  return CHASSIS_OK;
}

/* Exported functions ------------------------------------------------------- */

/**
 * \brief 初始化底盘
 *
 * \param c 包含底盘数据的结构体
 * \param param 包含底盘参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
                    float target_freq) {
  if (c == NULL) return CHASSIS_ERR_NULL;

  c->param = param;             /* 初始化参数 */
  c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
  c->height = CHASSIS_DEFAULT_HEIGHT; /* 设置底盘默认高度为0.2米 */

  c->pid.motor_id = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->pid.motor_id));
  if (c->pid.motor_id == NULL) goto error;

  c->filter.in = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->filter.in));
  if (c->filter.in == NULL) goto error;

  c->filter.out = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->filter.out));
  if (c->filter.out == NULL) goto error;

return CHASSIS_OK; /* 返回成功 */

error:
  /* 动态内存分配错误时，释放已经分配的内存，返回错误值 */
  BSP_Free(c->pid.motor_id);
  BSP_Free(c->filter.in);
  BSP_Free(c->filter.out);
  return CHASSIS_ERR_NULL;
}

/**
 * \brief 更新底盘的反馈信息
 *
 * \param c 包含底盘数据的结构体
 * \param can CAN设备结构体
 *
 * \return 函数运行结果
 */
int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go){
    for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
        Kinematics_RealFeedback(&c->feedback.id[i],&go->id[i],
                                c->param->mech_param.ratio.id[i],
                                c->param->mech_param.zero_point.id[i]);
    }
}

int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now){
  /* 底盘数据和控制指令结构体不能为空 */
  if (c == NULL) return CHASSIS_ERR_NULL;
  if (c_cmd == NULL) return CHASSIS_ERR_NULL;

  c->dt = (float)(now - c->lask_wakeup) / 1000.0f;
  c->lask_wakeup = now;
  c->time += c->dt; /* 更新用于生成动作的时间 */
  // while (c->time > 1.0f) { c ->time -= 1.0f; } /* 保持时间在0-1秒之间 */

  Chassis_SetMode(c, c_cmd); /* 设置底盘模式和动作 */

  switch (c->mode) {
    case CHASSIS_MODE_RELAX: /* 放松模式，电机不输出 */
      c->output = (GO_ChassisCMD_t){0}; /* 清空输出 */
      c->mode = CHASSIS_MODE_RELAX; /* 更新状态 */
      c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
      c->time = 0.0f; /* 重置时间 */
      break;
    case CHASSIS_MODE_DAMP: /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
      for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
        c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
      }
      c->mode = CHASSIS_MODE_DAMP; /* 更新状态 */
      c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
      c->time = 0.0f; /* 重置时间 */
      break;
    case CHASSIS_MODE_POSITION:{
      for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
              c->output.id[i] = position_mode; /* 设置位置模式 */}

      switch (c->action) {
        case CHASSIS_ACTION_NONE:{
          for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
            c->output.id[i].Pos = c->feedback.id[i].Pos;}
          c->mode = CHASSIS_MODE_POSITION;
          c->action = CHASSIS_ACTION_NONE;
          c->time = 0.0f; /* 重置时间 */
          break;
        }

        case CHASSIS_ACTION_STAND:{
          c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
          for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
            if (i % 2 == 0) { /* 左前腿和右后腿 */
              float target_pose[3] = {0.0, -0.0861, -c->height};
              float angle_pose[3];
              Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
              c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
              c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
              c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
            } else {
              float target_pose[3] = {0.0, 0.0861, -c->height}; /* 右前腿和左后腿 */
              float angle_pose[3];
              Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
              c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
              c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
              c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
            }
          }
          break;
        } /* 站立动作 */

        case CHASSIS_ACTION_WALK:{
            c->height = c->height + c_cmd->delta_hight * c->dt;
            float T = 2.0f; // 步态周期(s)
            float swing_height = 0.15f; // 摆动腿抬高高度
            float stride_x = c_cmd->ctrl_vec.vx * T; // x方向步幅
            float stride_y = c_cmd->ctrl_vec.vy * T; // y方向步幅
            float wz = c_cmd->ctrl_vec.wz; // 旋转速度

            uint8_t swing_leg = (uint8_t)(c->time / (T / 4)) % 4; // 当前摆动腿编号
            float t_phase = fmodf(c->time, T / 4) / (T / 4); // 当前腿相内归一化时间

            for (uint8_t leg = 0; leg < 4; leg++) {
                float target_pose[3];
                float base_y = (leg % 2 == 0) ? -0.0861f : 0.0861f;
                float base_x = 0.0f;

                if (leg == swing_leg) {
                    // 摆动腿：贝塞尔轨迹，抬高并前移
                    float start[3] = {base_x - stride_x/2, base_y - stride_y/2, -c->height};
                    float mid1[3] = {base_x, base_y, -c->height + swing_height};
                    float mid2[3] = {base_x, base_y, -c->height + swing_height};
                    float end[3] = {base_x + stride_x/2, base_y + stride_y/2, -c->height};
                    Path_Bezier3d(start, mid1, mid2, end, t_phase, target_pose);
                } else {
                    // 支撑腿：3/4周期内走完整步幅的1/3
                    // 计算支撑腿在支撑相内的归一化时间
                    uint8_t support_leg_index = (leg + 4 - swing_leg) % 4;
                    float support_phase = fmodf(c->time + (T / 4) * support_leg_index, T) / (T * 3.0f / 4.0f);
                    if (support_phase > 1.0f) support_phase = 1.0f;

                    // 支撑腿分三段，每段1/3步幅
                    float seg = support_phase * 3.0f; // [0,3)
                    int seg_idx = (int)seg; // 0,1,2
                    float seg_phase = seg - seg_idx; // [0,1)

                    // 每段起止点
                    float seg_start_x = base_x + stride_x/2 - stride_x * seg_idx / 3.0f;
                    float seg_start_y = base_y + stride_y/2 - stride_y * seg_idx / 3.0f;
                    float seg_end_x = base_x + stride_x/2 - stride_x * (seg_idx+1) / 3.0f;
                    float seg_end_y = base_y + stride_y/2 - stride_y * (seg_idx+1) / 3.0f;

                    Path_straight3d(
                        (float[3]){seg_start_x, seg_start_y, -c->height},
                        (float[3]){seg_end_x, seg_end_y, -c->height},
                        seg_phase, target_pose
                    );
                }
                float angle_pose[3];
                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[leg]);
                c->output.id[leg * 3 + 0].Pos = angle_pose[0];
                c->output.id[leg * 3 + 1].Pos = angle_pose[1];
                c->output.id[leg * 3 + 2].Pos = angle_pose[2];
            }
            break;
          }
          

        case CHASSIS_ACTION_TROT:{
          float T = 0.6f; // 步态周期(s)
          if (c->time > T) {
            c->time -= T; // 保持时间在0-T之间
          }
          float stride_x = c_cmd->ctrl_vec.vx / 10.0f;
          float stride_y = c_cmd->ctrl_vec.vy / 10.0f;
          float swing_height = 0.15f; // 摆动腿抬高高度

          // 对角腿分组：0-3一组，1-2一组
          for (uint8_t leg = 0; leg < 4; leg++) {
            float t_leg = fmodf(c->time + ((leg == 0 || leg == 3) ? 0.0f : T/2), T) / T; // 对角腿相差半周期
            float base_y = (leg % 2 == 0) ? -0.0861f : 0.0861f;
            float target_pose[3];
            if (t_leg < 0.5f) {
              // 摆动相，贝塞尔插值
              float t_bezier = t_leg / 0.5f;
              float start[3] = {-stride_x, base_y - stride_y, -c->height};
              float mid1[3] = {0, base_y, -c->height + swing_height};
              float mid2[3] = {0, base_y, -c->height + swing_height};
              float end[3] = {stride_x, base_y + stride_y, -c->height};
              Path_Bezier3d(start, mid1, mid2, end, t_bezier, target_pose);
            } else {
              // 支撑相，首尾相连，起点为上一个摆动相终点，终点为下一个摆动相起点
              float t_line = (t_leg - 0.5f) / 0.5f;
              float start[3] = {stride_x, base_y + stride_y, -c->height};    // 摆动相终点
              float end[3]   = {-stride_x, base_y - stride_y, -c->height};   // 下一个摆动相起点
              Path_straight3d(start, end, t_line, target_pose);
            }
            float angle_pose[3];
            Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[leg]);
            c->output.id[leg * 3 + 0].Pos = angle_pose[0];
            c->output.id[leg * 3 + 1].Pos = angle_pose[1];
            c->output.id[leg * 3 + 2].Pos = angle_pose[2];
          }
          break;
        }


      }
    }   
    
  }

  for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
    /* 限制输出 */
    Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,
                        c->param->mech_param.ratio.id[i] * CHASSIS_MAX_SPEED,
                        c->param->mech_param.limit.max.id[i],
                        c->param->mech_param.limit.min.id[i]);
  }

}

void Chassis_DumpOutput(const Chassis_t *c, GO_ChassisCMD_t *out){
  if (c == NULL || out == NULL) return; /* 主结构体和输出结构体不能为空 */

  for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
    Kinematics_RealOutput(&c->output.id[i], &out->id[i],
                          c->param->mech_param.ratio.id[i],
                          c->param->mech_param.zero_point.id[i]);
  }
}