// KinematicsReverse.h
#ifndef KINEMATICS_REVERSE_H
#define KINEMATICS_REVERSE_H

#include <vector>
#include <string>
#include <map>
#include <cmath>
#include <memory>
#include <algorithm>
#include <functional>
#include <optional>

#include "utils.h"
#include "QuadrupedRobotSimulation/BaseClass.h"
#include "QuadrupedRobotSimulation/OPERATION.h"
#include "SharedGeometry/SharedGeometry.h"

class ReverseKinematicsCalculator
{
private:
    // 统一配置
    QuadrupedRobotConfiguration _config;
    std::shared_ptr<RobotGaitDataManager> _manager;

    // 基本输入参数
    double adjusted_shank_angles_factory;
    std::vector<double> A_point;
    double L10;
    double L20;
    double L30;
    double L21;
    double L22;
    double L23;
    double beta1;
    double CC1;
    double L31;
    double C2C3_length;
    double lead1;
    double D1_L_offset;
    double D2_L_offset;
    double C4_D2_offset;

    // 初始C3C4距离
    double initial_C3C4_LF;
    double initial_C3C4_LH;
    double initial_C3C4_RF;
    double initial_C3C4_RH;

    // 计算得到的中间变量
    double _BB2_BC_angle_rad;
    double _C1_B_length;
    double _CC4_distance;

    // 数学常量
    static constexpr double PI = 3.14159265358979323846;
    static constexpr double DegToRad = PI / 180.0;
    static constexpr double RadToDeg = 180.0 / PI;

    // 静态变量（模拟C#的静态字段）
    std::map<std::string, std::vector<double>> old_initial_points_dictLF;
    std::map<std::string, std::vector<double>> old_initial_points_dictLH;
    static bool IsSupporLF;
    static bool IsSupporLH;
    static double BodyPosition_x;
    static double BodyPosition_y;
    static double BodyPosition_z;
    static double BodyPosition_qx;
    static double BodyPosition_qy;
    static double BodyPosition_qz;
    static double BodyPosition_qw;
    static const std::string BodyCode;

public:
    // 构造函数
    ReverseKinematicsCalculator(std::shared_ptr<RobotGaitDataManager> manager = nullptr);

    void debugPrintParamDict(const std::map<std::string, LegParam> &paramDict) const;
    void debugPrintLegCalculation(const std::string &legName, const LegParam &legParam,
                                  const std::map<std::string, std::vector<double>> &points_dict,
                                  const ModelID &modelID) const;
    void debugPrintFinalJson(const std::vector<C_ObjStates> &objStates) const;
    // 主计算函数
    std::string CalculateAllPointsFromMotorAnglesJsonStr();
    std::string CalculateAllPointsOnlyOneLegFromMotorAnglesJsonStr(
        double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
        const std::string &leg_type = "LF", bool IsReset = false);

    // 逆向计算主函数
    std::map<std::string, std::vector<double>> CalculateAllPointsFromMotorAnglesReverse(
        double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
        const std::string &leg_type = "LF", bool IsReset = false);

    // 判断是否为支撑点
    bool IsSupportPoint(const std::map<std::string, std::vector<double>> &pointsDict,
                        double groundHeight = 0.0, double tolerance = 1.0);

    // 验证计算结果
    std::pair<bool, std::vector<std::string>> ValidateReverseCalculation(
        const std::map<std::string, std::vector<double>> &points_dict,
        double thigh_angle, double shank_angle, double ankle_angle);

    // 批量计算
    std::vector<ReverseCalculationResult> BatchReverseCalculation(
        const std::map<std::string, std::vector<double>> &motor_data,
        const std::string &leg_type = "LF", std::optional<int> max_frames = std::nullopt);

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

    // 更新配置
    void UpdateConfiguration(const QuadrupedRobotConfiguration &newConfig);

private:
    // 从配置加载参数
    void LoadParametersFromConfig();

    // 计算派生参数
    void CalculateDerivedParameters();

    // 验证配置
    void ValidateConfiguration();

    // 计算点C（四杆机构解析解）
    std::optional<Vector2D> CalculatePointC(const Vector2D &A, const Vector2D &D,
                                            const Vector2D &B, double l2, double l3);

    // 选择C点（确保C和B在AD同侧）
    Vector2D SelectCPointByADSide(const Vector2D &A, const Vector2D &D,
                                  const Vector2D &B, const Vector2D &C1, const Vector2D &C2);

    // 根据大腿角度计算B点
    std::vector<double> CalculateBFromThighAngleReverse(double thigh_angle_deg);

    // 根据小腿角度计算B1点
    std::vector<double> CalculateB1FromShankAngleReverse(const std::vector<double> &B_p,
                                                         double shank_angle_deg);

    // 根据四杆机构计算B2点
    std::pair<std::vector<double>, std::string> CalculateB2FromFourbarReverse(
        const std::vector<double> &A_p, const std::vector<double> &B_p,
        const std::vector<double> &B1_p, double L22, double L23);

    // 根据B和B2计算C点
    std::vector<double> CalculateCFromBAndB2Reverse(const std::vector<double> &B1,
                                                    const std::vector<double> &B2,
                                                    double L, double theta_deg);

    // 反向计算C1点
    std::vector<double> CalculateC1PointReverse(const std::vector<double> &B_p,
                                                const std::vector<double> &C_p);

    // 根据C1点计算C2点
    std::vector<double> CalculateC2PointFromC1Reverse(const std::vector<double> &C1_p,
                                                      const std::vector<double> &B_p,
                                                      const std::vector<double> &C_point);

    // 根据脚踝角度计算C3C4距离
    double CalculateC3C4DistanceFromAnkleAngleReverse(double ankle_angle_deg,
                                                      const std::string &leg_type = "LF");

    // 已知三角形三边和两个顶点，求第三个顶点
    std::vector<double> FindThirdVertex(const std::vector<double> &C,
                                        const std::vector<double> &C2,
                                        double a, double b, double c);

    // 根据C和C2点计算C4点
    std::vector<double> CalculateC4PointFromCAndC2Reverse(const std::vector<double> &C_p,
                                                          const std::vector<double> &C2_p,
                                                          double ankle_angle_deg,
                                                          const std::string &leg_type = "LF");

    // 根据C和C4点计算L点
    std::vector<double> CalculateLPointsFromCAndC4Reverse(const std::vector<double> &C,
                                                          const std::vector<double> &C4);

    // 求直角三角形直角顶点
    std::vector<double> FindRightAngleThirdVertex(const std::vector<double> &C,
                                                  const std::vector<double> &L,
                                                  double a, double b);

    // 根据C和L点计算D1点
    std::vector<double> CalculateD1PointsFromCAndLReverse(const std::vector<double> &C_p,
                                                          const std::vector<double> &L_p);

    // 求直角三角形直角顶点D2
    std::vector<double> FindRightAngleVertexD2(const std::vector<double> &L,
                                               const std::vector<double> &C4,
                                               double a, double b);

    // 根据L和C4点计算D2点
    std::vector<double> CalculateD2PointsFromCAndLReverse(const std::vector<double> &L,
                                                          const std::vector<double> &C4);

    // 求直角三角形直角顶点C3
    std::vector<double> FindRightAngleVertex(const std::vector<double> &C2,
                                             const std::vector<double> &C4, double L);

    // 根据C2和C4点计算C3点
    std::vector<double> CalculateC3PointFromC2AndC4Reverse(const std::vector<double> &C2_point,
                                                           const std::vector<double> &C4_point);

    // 从字典安全获取点坐标
    std::vector<double> GetPointSafe(const std::map<std::string, std::vector<double>> &pointsDict,
                                     const std::string &key);

    // 获取模型ID
    // 新版本：
    // std::string GetValueModelID(
    //     const ModelID &modelID, // 改为 ModelID 类型
    //     const std::string &legName, const std::string &key);

    const LegModelIDs &GetLegModelIDs(const ModelID &modelID, const std::string &legName);

    // 辅助函数：旋转向量
    std::vector<double> RotateVector(double angle, const std::vector<double> &vector);
};

#endif // KINEMATICS_REVERSE_H