/*
 * VMC虚拟模型控制器实现
 * 
 * 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
 * 主要功能包括:
 * 1. 五连杆机构的正逆运动学解算
 * 2. 虚拟力到关节力矩的映射
 * 3. 地面接触检测
 * 4. 等效摆动杆模型转换
 * 
 * 参考文献:
 * - 韭菜的菜 知乎: 平衡步兵控制系统设计
 * - 上交轮腿电控开源方案 (2023)
 */

#include "vmc.h"
#include <string.h>

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

#define VMC_EPSILON         (1e-6f)     // 数值计算精度
#define VMC_MAX_ITER        (10)        // 最大迭代次数

/* Private macro ------------------------------------------------------------ */

/**
 * @brief 限制数值范围
 */
#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))

/**
 * @brief 安全开方
 */
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)

/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */

static int8_t VMC_ValidateParams(const VMC_Param_t *param);
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4);
static int8_t VMC_SolveClosedLoop(VMC_t *vmc);
static float VMC_ComputeNumericDerivative(float current, float previous, float dt);

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

/**
 * @brief 初始化VMC控制器
 */
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
    if (vmc == NULL || param == NULL || sample_freq <= 0) {
        return -1;
    }
    
    // 复制参数
    memcpy(&vmc->param, param, sizeof(VMC_Param_t));
    
    // 设置控制周期
    vmc->dt = 1.0f / sample_freq;
    
    // 重置状态
    VMC_Reset(vmc);
    
    vmc->initialized = true;
    
    return 0;
}

/**
 * @brief VMC五连杆正解算
 * 
 * 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数
 * 
 * 坐标系定义:
 * - x轴: 机体前进方向为正
 * - y轴: 竖直向下为正  
 * - 角度: 顺时针为正
 */
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
    if (vmc == NULL || !vmc->initialized) {
        return -1;
    }
    body_pitch = -body_pitch; // 机体俯仰角取反
    
    VMC_Leg_t *leg = &vmc->leg;
    
    // 保存历史值
    leg->last_phi0 = leg->phi0;
    leg->last_L0 = leg->L0;
    leg->last_d_L0 = leg->d_L0;
    leg->last_d_theta = leg->d_theta;
    
    // 更新关节角度
    leg->phi1 = phi1;
    leg->phi4 = phi4;
    
    // 更新运动学状态
    VMC_UpdateKinematics(vmc, phi1, phi4);
    
    // 求解闭环运动学
    if (VMC_SolveClosedLoop(vmc) != 0) {
        return -1;
    }
    
    // 计算足端坐标
    leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f;
    leg->foot_y = leg->YC;
    
    // 计算等效摆动杆参数
    leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y);
    leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
    
    // 计算等效摆动杆角度(相对于机体坐标系)
    leg->alpha = VMC_PI_2 - leg->phi0;
    leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
    
    // 角度归一化
    VMC_ANGLE_NORMALIZE(leg->theta);
    VMC_ANGLE_NORMALIZE(leg->phi0);
    
    // 计算角速度和长度变化率
    leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
    leg->d_alpha = -leg->d_phi0;
    leg->d_theta = body_pitch_rate + leg->d_phi0;
    leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
    
    // 计算角加速度
    leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
    
    return 0;
}

/**
 * @brief VMC五连杆逆解算(力矩分配)
 * 
 * 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
 */
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
    if (vmc == NULL || !vmc->initialized) {
        return -1;
    }
    
    // 保存虚拟力和力矩
    vmc->leg.F_virtual = F_virtual;
    vmc->leg.T_virtual = -T_virtual;
    
    // 计算雅可比矩阵
    if (VMC_ComputeJacobian(vmc) != 0) {
        return -1;
    }
    
    VMC_Leg_t *leg = &vmc->leg;
    
    // 通过雅可比转置计算关节力矩
    // tau = J^T * F_virtual
    leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual;
    leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual;
    
    return 0;
}

/**
 * @brief 地面接触检测
 * 
 * 基于虚拟力和腿部状态估计地面法向力
 */
float VMC_GroundContactDetection(VMC_t *vmc) {
    if (vmc == NULL || !vmc->initialized) {
        return 0.0f;
    }
    
    VMC_Leg_t *leg = &vmc->leg;
    
    // 计算地面法向力 
    // Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
    leg->Fn = leg->F_virtual * cosf(leg->theta) + 
              leg->T_virtual * sinf(leg->theta) / leg->L0 + 
              vmc->param.wheel_mass * 9.8f;  // 添加轮子重力
    
    // 地面接触判断
    leg->is_ground_contact = (leg->Fn > 10.0f);  // 10N阈值
    
    return leg->Fn;
}

/**
 * @brief 获取足端位置
 */
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
    if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
        return -1;
    }
    
    *x = vmc->leg.foot_x;
    *y = vmc->leg.foot_y;
    
    return 0;
}

/**
 * @brief 获取等效摆动杆参数
 */
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
    if (vmc == NULL || !vmc->initialized) {
        return -1;
    }
    
    if (length) *length = vmc->leg.L0;
    if (angle) *angle = vmc->leg.theta;
    if (d_length) *d_length = vmc->leg.d_L0;
    if (d_angle) *d_angle = vmc->leg.d_theta;
    
    return 0;
}

/**
 * @brief 获取关节输出力矩
 */
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
    if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
        return -1;
    }
    
    *tau_front = vmc->leg.tau_hip_front;
    *tau_rear = vmc->leg.tau_hip_rear;
    
    return 0;
}

/**
 * @brief 重置VMC控制器状态
 */
void VMC_Reset(VMC_t *vmc) {
    if (vmc == NULL) {
        return;
    }
    
    // 清零腿部状态
    memset(&vmc->leg, 0, sizeof(VMC_Leg_t));
    
    // 设置初始值
    vmc->leg.L0 = 0.15f;  // 默认腿长15cm
    vmc->leg.theta = 0.0f;
}

/**
 * @brief 设置虚拟力和力矩
 */
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
    if (vmc == NULL || !vmc->initialized) {
        return;
    }
    
    vmc->leg.F_virtual = F_virtual;
    vmc->leg.T_virtual = T_virtual;
}

/**
 * @brief 计算雅可比矩阵
 * 
 * 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
 */
int8_t VMC_ComputeJacobian(VMC_t *vmc) {
    if (vmc == NULL || !vmc->initialized) {
        return -1;
    }
    
    VMC_Leg_t *leg = &vmc->leg;
    
    // 检查分母不为零
    float sin_diff = sinf(leg->phi3 - leg->phi2);
    if (fabsf(sin_diff) < VMC_EPSILON) {
        return -1;  // 奇异配置
    }
    
    // 计算雅可比矩阵元素
    // J11: 后髋关节到支撑力的雅可比
    leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) * 
                sinf(leg->phi1 - leg->phi2)) / sin_diff;
    
    // J12: 后髋关节到摆动力矩的雅可比  
    leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) * 
                sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff);
    
    // J21: 前髋关节到支撑力的雅可比
    leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) * 
                sinf(leg->phi3 - leg->phi4)) / sin_diff;
    
    // J22: 前髋关节到摆动力矩的雅可比
    leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) * 
                sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff);
    
    return 0;
}

/* Private functions -------------------------------------------------------- */

/**
 * @brief 验证VMC参数有效性
 */
static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
    if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 || 
        param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
        return -1;
    }
    
    // 检查腿部几何约束
    if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) {
        return -1;  // 不满足闭环几何约束
    }
    
    return 0;
}

/**
 * @brief 更新基本运动学参数
 */
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) {
    VMC_Leg_t *leg = &vmc->leg;
    
    // 计算关键点坐标
    // 点B (后髋关节末端)
    leg->XB = vmc->param.leg_1 * cosf(phi1);
    leg->YB = vmc->param.leg_1 * sinf(phi1);
    
    // 点D (前髋关节末端)  
    leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4);
    leg->YD = vmc->param.leg_4 * sinf(phi4);
    
    // 计算BD连杆长度
    float dx = leg->XD - leg->XB;
    float dy = leg->YD - leg->YB;
    leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy);
}

/**
 * @brief 求解闭环运动学方程
 * 
 * 根据两个髋关节角度，求解中间关节角度
 */
static int8_t VMC_SolveClosedLoop(VMC_t *vmc) {
    VMC_Leg_t *leg = &vmc->leg;
    
    // 使用余弦定理求解phi2
    leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB);
    leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB);
    leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 + 
              leg->lBD * leg->lBD - 
              vmc->param.leg_3 * vmc->param.leg_3;
    
    // 检查判别式
    float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0;
    if (discriminant < 0) {
        return -1;  // 无解
    }
    
    float sqrt_discriminant = VMC_SAFE_SQRT(discriminant);
    
    // 计算phi2 (选择合适的解)
    leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0);
    
    // 计算phi3
    leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2),
                       leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2));
    
    // 计算足端坐标点C
    leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2);
    leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2);
    
    return 0;
}

/**
 * @brief 计算数值微分
 */
static float VMC_ComputeNumericDerivative(float current, float previous, float dt) {
    if (dt <= 0) {
        return 0.0f;
    }
    
    return (current - previous) / dt;
}