// KinematicsSimulation.h
#ifndef KINEMATICS_SIMULATION_H
#define KINEMATICS_SIMULATION_H

#include "QuadrupedRobotSimulation/BaseClass.h"
#include <vector>
#include <string>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <iostream>
#include <functional>

class QuadrupedRobotSimulation
{
private:
    // 常量
    static constexpr double PI = 3.14159265358979323846;

public:
    // 使用统一配置
    QuadrupedRobotConfiguration _config;

    // ============== 从配置继承的字段 ==============
    // 主几何参数
    double A_x;                  // 固定点A的X坐标
    double A_y;                  // 固定点A的Y坐标
    std::vector<double> A_point; // A点坐标数组
    double L10;                  // AB连杆长度（大腿长度）
    double L20;                  // BC连杆长度（小腿上段长度）
    double L30;                  // C点到L点（脚端点）的垂直距离
    double s_l;                  // 步长
    double s_h;                  // 步高
    double X;                    // 下铰链点与大腿铰链点投影水平距离

    // 小腿连杆机构参数

    double L22;   // B1到B2的连杆长度
    double L23;   // B到B2的连杆长度
    double beta1; // B-B2与BC的夹角（度）
    double CC1;   // C到C1点的距离

    // 脚踝连杆机构参数
    double L31;         // C1到C2的连杆长度
    double C2C3_length; // C2到C3的连杆长度（固定长度）
    double lead1;       // 丝杠导程（mm）

    // 脚部构型参数
    double D1_L_offset;  // D1点与L点的水平偏移（左侧）
    double D2_L_offset;  // D2点与L点的水平偏移（右侧）
    double C4_D2_offset; // C4点与D2点的垂直距离

    // 电机参数
    double thigh_motor_reduction; // 大腿电机减速比
    double shank_motor_reduction; // 小腿电机减速比
    double ankle_motor_reduction; // 脚踝电机减速比

    // 逆向运动学参数
    double adjusted_shank_angles_factory; // 小腿角度调整参数

    // ============== 计算得到的中间变量 ==============
    double BB2_BC_angle; // B-B2与BC的夹角（弧度）
    double C1_B_length;  // C1到B点的距离

    // ============== 步态相位时间 ==============
    double timeLF = 0; // 左前腿相位时间
    double timeLH = 0; // 左后腿相位时间
    double timeRF = 0; // 右前腿相位时间
    double timeRH = 0; // 右后腿相位时间

    // ============== 轨迹数据列表 ==============

    std::vector<std::vector<double>> Trajectory_C;  // C点轨迹
    std::vector<std::vector<double>> Trajectory_B;  // B点轨迹
    std::vector<std::vector<double>> Trajectory_D1; // D1点轨迹
    std::vector<std::vector<double>> Trajectory_D2; // D2点轨迹
    std::vector<std::vector<double>> Trajectory_C4; // C4点轨迹
    std::vector<std::vector<double>> Trajectory_B1; // B1点轨迹
    std::vector<std::vector<double>> Trajectory_B2; // B2点轨迹
    std::vector<std::vector<double>> Trajectory_C1; // C1点轨迹
    std::vector<std::vector<double>> Trajectory_C2; // C2点轨迹
    std::vector<std::vector<double>> Trajectory_C3; // C3点轨迹

    // ============== 角度和距离数据列表 ==============
    std::vector<double> AB_horizontal_angles; // AB连杆与水平夹角（度）
    std::vector<double> AB_BC_angles;         // AB与BC夹角（度）
    std::vector<double> BC_CL_angles;         // BC与CL夹角（度）
    std::vector<double> AB1_AB_angles;        // AB1与AB夹角θ1（度）
    std::vector<double> BCL_angles;           // B-C-L夹角（度）
    std::vector<double> C3_C4_distances;      // C3-C4距离（mm）
    std::vector<double> C2_C3_distances;      // C2-C3距离（mm）
    std::vector<double> C2_C4_distances;      // C2-C4距离（mm）
    std::vector<double> C2_C3_C4_angles;      // ∠C2C3C4角度（度）
    std::vector<double> CL_distances;         // C-L距离（mm）
    std::vector<double> dot_products;         // 点积验证数据

    // ============== 电机数据对象 ==============
    MotorData motor_data_LF; // 左前腿电机数据
    MotorData motor_data_LH; // 左后腿电机数据
    MotorData motor_data_RF; // 右前腿电机数据
    MotorData motor_data_RH; // 右后腿电机数据

    // 原始电机数据（调整前）
    MotorData motor_data_LF_raw;
    MotorData motor_data_LH_raw;
    MotorData motor_data_RF_raw;
    MotorData motor_data_RH_raw;

    std::string gait_type;      // 步态类型："walk"、"trot"、"standup"
                                // 步态控制参数
    double T;                   // 步态周期时间（秒）
    double f;                   // 控制频率（Hz）
    double step;                // 时间步长（秒）
    double t_s;                 // 支撑阶段时间
    double t_w;                 // 摆动阶段时间
    double support_ratio = 0.5; // 支撑阶段比例
    double swing_ratio = 0.5;   // 摆动阶段比例

    double deltX; // x方向偏移距离
    double L21;   // A到B1的连杆长度

    std::vector<std::vector<double>> Trajectory_L; // L点（脚端点）轨迹

    // 构造函数
    QuadrupedRobotSimulation(const QuadrupedRobotConfiguration &config = QuadrupedRobotConfiguration());
    QuadrupedRobotSimulation();

    // 获取配置对象
    const QuadrupedRobotConfiguration &GetConfig() const { return _config; }

    // 主计算函数
    std::string CalculateAllTrajectories_JsonStr();
    void CalculateAllTrajectories();
    std::string CalculateAllTrajectoriesJsonString();

    // 设置步态参数
    void SetGaitParameters();

private:
    // 初始化方法
    void LoadParametersFromConfig();
    void InitializeParameters();

    // 轨迹计算方法
    void CalculateTrajectoryL_V2();
    void CalculateTrajectoryC();
    void CalculateTrajectoryB();
    void CalculateAllPoints();
    void CalculateAllAngles();
    void CheckConstraints();
    void CalculateMotorData();

    // 辅助计算方法
    std::vector<std::vector<double>> CalculateSupportPoints(const std::vector<double> &L_point);
    std::vector<double> Calculate_B_Position(const std::vector<double> &A, const std::vector<double> &C,
                                             double AB_length, double BC_length);
    double Calculate_BCL_Angle(const std::vector<double> &B_point, const std::vector<double> &C_point,
                               const std::vector<double> &L_point);
    std::vector<double> CalculateSwingPointWithConstraints(const std::vector<double> &L_point,
                                                           double target_BCL_angle,
                                                           const std::vector<double> &prev_C,
                                                           double &prev_angle);
    void RecalculateAllPoints();

    // 辅助点计算方法
    std::vector<double> Calculate_B2_Position(const std::vector<double> &B, const std::vector<double> &C,
                                              double B_B2_length, double BB2_BC_angle);
    std::vector<double> Calculate_B1_Position(const std::vector<double> &A, const std::vector<double> &B2,
                                              double A_B1_length, double B1B2_length);
    std::vector<double> Calculate_C1_Position(const std::vector<double> &B, const std::vector<double> &C,
                                              double C1_B_length);
    std::vector<double> Calculate_C2_Position(const std::vector<double> &C1, const std::vector<double> &B,
                                              const std::vector<double> &C, double C1C2_length);
    std::vector<double> Calculate_C3_Position(const std::vector<double> &C2, const std::vector<double> &C4,
                                              double C2C3_length);
    double Calculate_C2C3C4_Angle(const std::vector<double> &C2, const std::vector<double> &C3,
                                  const std::vector<double> &C4);
    double Distance(const std::vector<double> &p1, const std::vector<double> &p2);

    // 电机数据计算方法
    MotorData CalculateIncrementsAndVelocities();
    MotorData CalculateOtherLegData(const MotorData &baseData, int shift, bool isRightLeg);
    MotorData AdjustMotorAngles(const MotorData &motorData, const std::vector<double> &frame_start_times, double step);
    MotorData ReverseShankData(const MotorData &motorData, double step, double shank_motor_reduction);
    MotorData AdjustAnkleAngles(const MotorData &data);

    // 数学辅助函数
    double NonlinearAngleFunction(double t, double start_angle_deg, double end_angle_deg,
                                  const std::string &function_type, double curvature);
    std::vector<double> Linspace(double start, double end, int num);
    std::vector<std::vector<double>> Zeros(int rows, int cols);
    std::vector<std::vector<double>> Flip(const std::vector<std::vector<double>> &array);
    std::vector<std::vector<double>> VStack(const std::vector<std::vector<double>> &array1,
                                            const std::vector<std::vector<double>> &array2);
    int PythonRoundExact(double value);

    // 数组操作函数
    std::vector<double> ShiftArray(const std::vector<double> &array, int shiftFrames);
};

#endif // KINEMATICS_SIMULATION_H
