/**
 * @file arm_main.cpp
 * @brief 机械臂主任务 - C++版本
 */

#include "cmsis_os2.h"
#include "task/user_task.h"

#include "module/arm_oop.hpp"
#include "module/motor_base.hpp"
#include "module/joint.hpp"
#include "bsp/can.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"
using namespace arm;

// ============================================================================
// 电机和关节配置
// ============================================================================

// LZ电机参数（关节1-3）
static MOTOR_LZ_Param_t lz_params[3] = {
    { .can = BSP_CAN_1, .motor_id = 127, .host_id = 0xff, .module = MOTOR_LZ_RSO3, .reverse = true,   .mode=MOTOR_LZ_MODE_MOTION},
    { .can = BSP_CAN_1, .motor_id = 126, .host_id = 0xff, .module = MOTOR_LZ_RSO3, .reverse = true,   .mode=MOTOR_LZ_MODE_MOTION},
    { .can = BSP_CAN_1, .motor_id = 125, .host_id = 0xff, .module = MOTOR_LZ_RSO3, .reverse = false,  .mode=MOTOR_LZ_MODE_MOTION},
};

// DM电机参数（关节4-6）
static MOTOR_DM_Param_t dm_params[3] = {
    {.can = BSP_CAN_1, .master_id = 0x14,  .can_id = 0x04, .module = MOTOR_DM_J4310, .reverse = false,},
    {.can = BSP_CAN_1, .master_id = 0x15,  .can_id = 0x05, .module = MOTOR_DM_J4310, .reverse = false,},
    {.can = BSP_CAN_1, .master_id = 0x16,  .can_id = 0x06, .module = MOTOR_DM_J4310, .reverse = false,},
};

static Arm6dof_DHParams_t dh_params[6] = {
  // J1: origin(0, 0, 0.024) rpy(0,0,0)
  {.theta=0.0f, .d=0.024f,     .a=0.000f,    .alpha=0.0f,     .theta_offset=0.0f,      .qmin=-15.7f,   .qmax=15.7f,  .m=2.2629f,  .rc={-0.00718190f,  0.00031034f,  0.06159800f}},
  // J2: origin(-0.001395, 0, 0.1015) rpy(1.5708, 0, -1.5708)
  {.theta=0.0f, .d=0.1015f,    .a=-0.00139f, .alpha=M_PI_2,   .theta_offset=-M_PI_2,   .qmin=-1.57f,   .qmax=1.57f,  .m=0.97482f, .rc={ 0.00316320f, -0.00227070f, -0.22947000f}},
  // J3: origin(0.3265, 0, -0.0071975) rpy(0, 0, 0)
  {.theta=0.0f, .d=-0.00719f,  .a=0.3265f,   .alpha=0.0f,     .theta_offset=0.0f,      .qmin=-1.0f,    .qmax=3.0f,   .m=0.72964f, .rc={ 0.08696300f,  0.00167340f, -0.01780600f}},
  // J4: origin(0.0905, 0.052775, 0.0058025) rpy(1.5708, 0, 3.1416)
  {.theta=0.0f, .d=0.05278f,   .a=0.0905f,   .alpha=M_PI_2,   .theta_offset=M_PI,      .qmin=0.0f,     .qmax=6.3f,   .m=0.54148f, .rc={-0.00005530f,  0.10972717f, -0.00230270f}},
  // J5: origin(0.001627, 0, 0.18467) rpy(1.5708, 0, 0)
  {.theta=0.0f, .d=0.18467f,   .a=0.0016f,   .alpha=M_PI_2,   .theta_offset=0.0f,      .qmin=-1.9f,    .qmax=1.9f,   .m=0.21817f, .rc={ 0.05654200f,  0.00102680f, -0.00131060f}},
  // J6: origin(0.10487, 0.0013347, 0) rpy(1.5708, 0, 1.5708)
  {.theta=0.0f, .d=0.00133f,   .a=0.10487f,  .alpha=M_PI_2,   .theta_offset=M_PI_2,    .qmin=0.0f,     .qmax=6.3f,   .m=0.56255f, .rc={ 0.00001277f,  0.10159000f, -0.00000780f}},
};

static JointControlParams joint_params[6] = {
    {.qmin=-15.7f, .qmax=15.7f, .kp=10.0f, .kd=0.0f},
    {.qmin=-1.57f, .qmax=1.57f, .kp=10.0f, .kd=0.0f},
    {.qmin=-1.0f,  .qmax=3.0f,  .kp=20.0f, .kd=0.0f},
    {.qmin=0.0f,   .qmax=6.3f,  .kp=3.0f,  .kd=0.0f},
    {.qmin=-1.9f,  .qmax=1.9f,  .kp=5.0f,  .kd=0.0f},
    {.qmin=0.0f,   .qmax=6.3f,  .kp=5.0f,  .kd=0.0f},
};

static float q_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
// 全局对象指针
IMotor* motors[6] = {nullptr};       // 多态接口
MotorLZ* motors_lz[3] = {nullptr};   // 用于调试查看
MotorDM* motors_dm[3] = {nullptr};   // 用于调试查看
Joint* joints[6] = {nullptr};
RoboticArm* robot_arm = nullptr;

Arm_CMD_t arm_cmd;  // 当前机械臂控制命令（含 target_pose / set_target_as_current）

// ============================================================================
// 测试用调试变量（可在调试器Watch窗口直接观察和修改）
// ============================================================================
// 阶段控制：在调试器中修改此值切换测试阶段
//   0 = 仅计算重力补偿力矩，全部电机松弛，观察 gravity_torques_dbg 是否合理
//   1 = 仅关节6（Roll轴）输出纯力矩，其余松弛（Roll轴重力补偿接近0，最安全）
//   2 = 全部六轴输出（完整重力补偿，GRAVITY_COMP模式）
//   3 = 笛卡尔空间控制（旧版满秩库代数雅可比算法）
//   4 = 2.5D 解析降维控制（专为比赛设计的无奇异点稳定版，推荐！）
uint8_t test_stage = 1;
Arm6dof_JointAngles_t current_angles;  // 调试观察：当前各关节角度 (rad)
// 重力补偿力矩观察数组（对应关节1~6，单位 N·m）
float gravity_torques_dbg[6] = {0.0f};
// 末端位姿观察（mm换算，便于观察，不参与计算）
struct { float x, y, z, roll, pitch, yaw; } end_pose_mm_dbg = {0};
// FK→IK验证：IK解算得到的关节角度
Arm6dof_JointAngles_t ik_from_fk_result;  // IK解算结果

int setzero=0;
// 重力补偿缩放系数（可在调试器中实时修改，用于测试补偿强度）
float gravity_comp_scale = 1.0f;
// 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成
float traj_progress_dbg = 0.0f;
// 轨迹速度设置：可在调试器中修改这两个值，重启case4生效
float traj_lin_vel = 0.15f;   // 末端线速度 (m/s)，默认 150mm/s
float traj_ang_vel = 1.0f;    // 末端角速度 (rad/s)，默认 ~57°/s

uint8_t control_frame = 1;  // 选择在何种坐标系下控制 1= 世界系，2=工具系，3=航向系
// ============================================================================
// IK测试调试变量
// ============================================================================
// ik_test_enable：设置为1时每帧调用一次IK测试；设置为0停止
//   - 输入：target_pose（目标位姿）+ current_angles（当前角度作为初始猜测）
//   - 结果：ik_test_result（IK解算出的关节角度），ik_test_ret（返回码：0=成功 -1=失败）
int ik_test_enable = 0;
Arm6dof_JointAngles_t ik_test_result = {0};  // IK解算结果（rad），调试器Watch观察
int ik_test_ret = 0;                          // IK返回码：0=成功，-1=无解或未初始化

// cmd task 中的机械臂命令指针（cmd.cpp 中定义），通过 extern 访问以便写回初始位姿
extern Arm_CMD_t *cmd_for_arm;

/**
 * @brief 将当前世界系末端位姿转换到指定控制坐标系下的 target_pose，
 *        使 MoveCartesian*(result) 的目标恰好等于当前 cur_world（无初始跳变）
 * @param frame  1=世界系  2=工具系  3=航向系
 * @param cur    当前末端位姿（世界系，由 FK 计算得到）
 */
static Arm6dof_Pose_t SyncTargetToFrame(uint8_t frame, const Arm6dof_Pose_t& cur) {
    Arm6dof_Pose_t target;
    switch (frame) {
        case 2: {
            // 工具系：p_tool = R_cur^T * p_world，使 R_cur*p_tool = p_world
            // 姿态置零：R_tool = I，使 R_world = R_cur * I = R_cur（保持当前姿态）
            float rpy[3] = {cur.yaw, cur.pitch, cur.roll};
            Matrixf<3,3> RT = robotics::rpy2r(Matrixf<3,1>(rpy)).trans();
            float p[3] = {cur.x, cur.y, cur.z};
            Matrixf<3,1> pt = RT * Matrixf<3,1>(p);
            target.x = pt[0][0]; target.y = pt[1][0]; target.z = pt[2][0];
            target.yaw = 0.0f; target.pitch = 0.0f; target.roll = 0.0f;
            break;
        }
        case 3: {
            // 航向系：p_heading = Rz(-yaw)*[x,y]，z 直接赋值
            // 姿态：R_heading = Rz(-yaw)*R_cur，使 Rz(yaw)*R_heading = R_cur
            float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
            target.x =  cy * cur.x + sy * cur.y;
            target.y = -sy * cur.x + cy * cur.y;
            target.z = cur.z;
            float rpy_neg[3] = {-cur.yaw, 0.0f, 0.0f};
            float rpy_c[3]   = { cur.yaw, cur.pitch, cur.roll};
            Matrixf<3,3> Rn  = robotics::rpy2r(Matrixf<3,1>(rpy_neg))
                             * robotics::rpy2r(Matrixf<3,1>(rpy_c));
            Matrixf<3,1> rn  = robotics::r2rpy(Rn);
            target.yaw = rn[0][0]; target.pitch = rn[1][0]; target.roll = rn[2][0];
            break;
        }
        default:
            // 世界系：直接复制
            target = cur;
            break;
    }
    return target;
}

static void SyncCommandTargetFromCurrent() {
  Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
  arm_cmd.target_pose = sync;
  if (cmd_for_arm) {
    cmd_for_arm->target_pose = sync;
  }
}

static void HandleSetZeroRequest() {
  if (!setzero) {
    return;
  }

  if (setzero <= 3) {
    MOTOR_LZ_SetZero(&lz_params[setzero - 1]);
  } else if (setzero > 3 && setzero < 7) {
    MOTOR_DM_SetZero(&dm_params[setzero - 4]);
  } else if (setzero == 7) {
    for (int i = 0; i < 3; ++i) {
      MOTOR_LZ_SetZero(&lz_params[i]);
    }
    for (int i = 0; i < 3; ++i) {
      MOTOR_DM_SetZero(&dm_params[i]);
    }
  }
  setzero = 0;
}

static void RelaxAllMotors() {
  for (int i = 0; i < 6; ++i) {
    motors[i]->Relax();
  }
}

static void MoveBySelectedFrame(const Arm6dof_Pose_t& target) {
  switch (control_frame) {
    case 2:
      robot_arm->MoveCartesianTool(target);
      break;
    case 3:
      robot_arm->MoveCartesianHeading(target);
      break;
    default:
      robot_arm->MoveCartesian(target);
      break;
  }
}

static void HandleCartesianModeError() {
  if (robot_arm->GetState() == MotionState::ERROR) {
    robot_arm->ResetError();
    SyncCommandTargetFromCurrent();
  }
}

static void RunCartesianControl(ControlMode mode) {
  robot_arm->SetLinVelLimit(traj_lin_vel);
  robot_arm->SetAngVelLimit(traj_ang_vel);
  robot_arm->SetMode(mode);
  MoveBySelectedFrame(arm_cmd.target_pose);
  robot_arm->Control();
  HandleCartesianModeError();
  ik_from_fk_result = robot_arm->GetIkAngles();
  traj_progress_dbg = robot_arm->GetTrajProgress();
}

extern "C" {

void Task_arm_main(void* argument) {

  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / ARM_MAIN_FREQ;
  osDelay(ARM_MAIN_INIT_DELAY); /* 延时一段时间再开启任务 */
  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */

  BSP_CAN_Init();
  MOTOR_LZ_Init();  // 注册LZ电机ID解析器
  
  // 初始化运动学模型
  Arm6dof_Init(dh_params);
  
  // 创建电机对象（同时保存到基类和派生类指针）
  motors_lz[0] = new MotorLZ(lz_params[0]);
  motors_lz[1] = new MotorLZ(lz_params[1]);
  motors_lz[2] = new MotorLZ(lz_params[2]);
  motors_dm[0] = new MotorDM(dm_params[0]);
  motors_dm[1] = new MotorDM(dm_params[1]);
  motors_dm[2] = new MotorDM(dm_params[2]);
  
  // 填充基类指针数组（用于多态管理）
  motors[0] = motors_lz[0];
  motors[1] = motors_lz[1];
  motors[2] = motors_lz[2];
  motors[3] = motors_dm[0];
  motors[4] = motors_dm[1];
  motors[5] = motors_dm[2];
  for (int i = 0; i < 6; ++i) {
      joints[i] = new Joint(i, motors[i], joint_params[i], q_offset[i], ARM_MAIN_FREQ);
  }
  // 关节3与关节2存在机械耦合：J3电机读数包含J2的运动，需减去J2角度才是真实J3角度
  joints[2]->SetCoupledJoint(joints[1]);

  robot_arm = new RoboticArm();
  for (int i = 0; i < 6; ++i) {
      robot_arm->AddJoint(i, joints[i]);
  }
  robot_arm->Init();
  
  // 使能重力补偿
  robot_arm->EnableGravityCompensation(true);
  robot_arm->SetGravityCompScale(gravity_comp_scale);  // 补偿系数，可微调
  
  // 设置控制模式（可选以下模式之一）:
  // GRAVITY_COMP: 位置保持 + 重力补偿前馈（位控+补偿）
  // TEACH:        纯重力补偿力矩输出，零刚度（示教拖动）
  // JOINT_POSITION: 关节位置控制 + 重力补偿前馈
  robot_arm->SetMode(ControlMode::GRAVITY_COMP);

  // 读取当前末端位姿，转换到 control_frame 后同步到 cmd，防止上电时大范围跳变
  osDelay(100);
  robot_arm->Update();
  {
    SyncCommandTargetFromCurrent();
  }

  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
/*---------------------------零点校准---------------------------*/
    HandleSetZeroRequest();
/*--------------------------------------------------------------*/

    // 更新机械臂状态（读取关节角度、FK、重力补偿计算）
    robot_arm->Update();

    osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);  

    // 使能状态变化沿处理：仅在变化时下发使能，减少冗余调用
    static bool last_enable = false;
    if (arm_cmd.enable != last_enable) {
      robot_arm->Enable(arm_cmd.enable);
    }

    // 使能上升沿自动同步：避免使能时 target_pose 与当前实际位姿不一致导致初始跳变
    if (arm_cmd.enable && !last_enable) {
      SyncCommandTargetFromCurrent();
    }
    last_enable = arm_cmd.enable;

    // set_target_as_current：将 target_pose 同步到当前末端位姿（转换到当前 control_frame）
    if (arm_cmd.set_target_as_current) {
      SyncCommandTargetFromCurrent();
    }

    
    // 每帧同步调试观察变量
    for (int i = 0; i < 6; ++i) {
      current_angles.q[i] = joints[i]->GetCurrentAngle();
      gravity_torques_dbg[i] = robot_arm->GetGravityTorque(i);
    }
    
    // 末端位姿换算为mm，便于调试器观察（不参与任何计算）
    const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
    end_pose_mm_dbg = { ep.x * 1000.0f, ep.y * 1000.0f, ep.z * 1000.0f,
                        ep.roll, ep.pitch, ep.yaw };

      switch (test_stage) {
        case 0:
          //检测电机正电流旋转方向与urdf规定是否一致
          static float rotationDirectionCheck[6] = {0.0f};
          joints[0]->TorqueControl(rotationDirectionCheck[0]);  
          joints[1]->TorqueControl(rotationDirectionCheck[1]);  
          joints[2]->TorqueControl(rotationDirectionCheck[2]);  
          joints[3]->TorqueControl(rotationDirectionCheck[3]); 
          joints[4]->TorqueControl(rotationDirectionCheck[4]); 
          joints[5]->TorqueControl(rotationDirectionCheck[5]); 
          break;

        case 1:
          // 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理
          RelaxAllMotors();
          break;

        case 2:
          // 阶段1：测试单个关节力矩输出（用于验证力矩计算）
          // 修改下面的索引来测试不同关节：j2=1, j3=2, j5=4
          motors[0]->Relax();
          motors[1]->Relax();
          motors[2]->Relax();
          motors[3]->Relax();
          motors[4]->Relax();
          motors[5]->Relax();
          // joints[1]->TorqueControl(gravity_torques_dbg[1]);  // j2大臂
          // joints[2]->TorqueControl(gravity_torques_dbg[2]);  // j3小臂
          // joints[4]->TorqueControl(gravity_torques_dbg[4]);  // j5腕部

          // joints[0]->TorqueControl(gravity_torques_dbg[0]); 
          // joints[3]->TorqueControl(gravity_torques_dbg[3]); 
          // joints[5]->TorqueControl(gravity_torques_dbg[5]); 
          break;
        case 3:
        default:
          // 阶段4：全部六轴 GRAVITY_COMP 模式（位置保持 + 重力补偿前馈）
          robot_arm->SetMode(ControlMode::GRAVITY_COMP);
          robot_arm->Control();
          break;

        case 4:
          // 笛卡尔空间轨迹规划控制：基于数值逆解(雅可比)
          RunCartesianControl(ControlMode::CARTESIAN_POSITION);
          break;

        case 5: {
          // =================================================================================
          // ★ 2.5D 解析降维控制算法 ★ 
          // 已经集成到底层 arm_oop 的 CARTESIAN_ANALYTICAL 模式中，完全与原轨迹规划流程接轨
          // =================================================================================
          RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL); // 启用降维解析数学模式
          break;
        }
      }

    // IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
    // 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
    if (ik_test_enable) {
      ik_test_ret = Arm6dof_InverseKinematics(&arm_cmd.target_pose, &current_angles,
                                              &ik_test_result, 0.001f, 50);
      ik_test_ret++;                                       
    }

    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
} 

} // extern "C"