// BaseClass.h - 完整修复版
#ifndef BASE_CLASS_H
#define BASE_CLASS_H

#include <string>
#include <vector>
#include <map>
#include <memory>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <iostream>
#include <mutex>
#include <optional>
#include <set>
#include <functional>
#include <sstream>
#include <iomanip>
#include <nlohmann/json.hpp>
#include "QuadrupedRobotSimulation/OPERATION.h"
#include "SharedGeometry/SharedGeometry.h"
#include "QuadrupedRobotSimulation/RobotConfig.hpp"

using json = nlohmann::json;

// 1. 首先完整定义 CheckResultItem 结构
struct CheckResultItem
{
    std::string Condition; // 添加缺失的成员变量
    bool Passed = false;
    std::optional<double> Actual;
    std::optional<double> Expected;
    std::optional<double> Tolerance;
    std::map<std::string, double> AdditionalData;

    CheckResultItem() : Condition(""), Passed(false)
    {
        AdditionalData = std::map<std::string, double>();
    }

    CheckResultItem(std::string cond, bool passed = false)
        : Condition(std::move(cond)), Passed(passed)
    {
        AdditionalData = std::map<std::string, double>();
    }
};

// 3. 现在安全地声明 CheckResultItem 的序列化函数
void to_json(json &j, const CheckResultItem &item);
void from_json(const json &j, CheckResultItem &item);
// 3. 前向声明 ModelID
struct ModelID;

// 4. 在namespace nlohmann中前向声明特化
namespace nlohmann
{
    // 为 std::optional 添加序列化支持
    template <typename T>
    struct adl_serializer<std::optional<T>>;

    // 为 std::vector<std::map<std::string, std::string>> 的序列化声明
    template <>
    struct adl_serializer<std::vector<std::map<std::string, std::string>>>;

    // 为 Vector2D 的序列化声明
    template <>
    struct adl_serializer<Vector2D>;

    // 为 ModelID 的序列化声明
    template <>
    struct adl_serializer<ModelID>;
}

struct RobotGaitRequest
{
    RequestParameters req_param;
};

struct LinkModelIDInfo
{
    std::string LinkName;
    std::string ModelID;
    std::string StartPoint;
    std::string EndPoint;
    std::string Description;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(LinkModelIDInfo, LinkName, ModelID, StartPoint, EndPoint, Description)
};

struct LegLinkDisplay
{
    std::string LegName;
    std::string LinkName;
    std::string ModelID;
    std::string StartPoint;
    std::string EndPoint;
    std::string Description;
    int Index = 0;
    std::string UniqueID;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegLinkDisplay, LegName, LinkName, ModelID, StartPoint, EndPoint, Description, Index, UniqueID)
};

struct RobotData
{
    GaitInfo GaitInfo;
    SystemParameters SystemParameters;
    std::vector<LinkModelIDInfo> LF;
    std::vector<LinkModelIDInfo> LH;
    std::vector<LinkModelIDInfo> RF;
    std::vector<LinkModelIDInfo> RH;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(RobotData, GaitInfo, SystemParameters, LF, LH, RF, RH)
};

struct Point2D
{
    double X = 0.0;
    double Y = 0.0;

    Point2D() = default;
    Point2D(double x, double y);

    friend void to_json(json &j, const Point2D &p);
    friend void from_json(const json &j, Point2D &p);
};

struct FrameData
{
    int FrameNumber = 0;
    double Time = 0.0;
    int ShiftedFrameNumber = 0;

    Point2D A_Point;
    Point2D B_Point;
    Point2D C_Point;
    Point2D L_Point;
    Point2D D1_Point;
    Point2D D2_Point;
    Point2D C4_Point;
    Point2D B1_Point;
    Point2D B2_Point;
    Point2D C1_Point;
    Point2D C2_Point;
    Point2D C3_Point;

    double AB_Horizontal_Angle = 0.0;
    double AB_BC_Angle = 0.0;
    double BC_CL_Angle = 0.0;
    double AB1_AB_Angle = 0.0;

    double C3_C4_Distance = 0.0;
    double C2_C3_Distance = 0.0;
    double C2_C4_Distance = 0.0;
    double C2_C3_C4_Angle = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(FrameData, FrameNumber, Time, ShiftedFrameNumber,
                                   A_Point, B_Point, C_Point, L_Point, D1_Point, D2_Point,
                                   C4_Point, B1_Point, B2_Point, C1_Point, C2_Point, C3_Point,
                                   AB_Horizontal_Angle, AB_BC_Angle, BC_CL_Angle, AB1_AB_Angle,
                                   C3_C4_Distance, C2_C3_Distance, C2_C4_Distance, C2_C3_C4_Angle)
};

struct LegTrajectory
{
    std::string LegCode;
    int PhaseShift = 0;
    bool IsRightLeg = false;
    std::vector<FrameData> Frames;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegTrajectory, LegCode, PhaseShift, IsRightLeg, Frames)
};

struct TrajectoryData
{
    int TotalFrames = 0;
    double StepTime = 0.0;
    std::map<std::string, LegTrajectory> Legs;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(TrajectoryData, TotalFrames, StepTime, Legs)
};

struct MotorInfo
{
    double Angle = 0.0;
    double AngleIncrement = 0.0;
    double Speed = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorInfo, Angle, AngleIncrement, Speed)
};

struct MotorFrameData
{
    int FrameNumber = 0;
    double StartTime = 0.0;
    double EndTime = 0.0;

    MotorInfo Thigh;
    MotorInfo Shank;
    MotorInfo Ankle;

    double C3C4_Distance = 0.0;
    double C3C4_DistanceIncrement = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorFrameData, FrameNumber, StartTime, EndTime,
                                   Thigh, Shank, Ankle, C3C4_Distance, C3C4_DistanceIncrement)
};

struct LegMotorData
{
    std::string LegCode;
    std::vector<MotorFrameData> Frames;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegMotorData, LegCode, Frames)
};

struct MotorDataExport
{
    double StepTime = 0.0;
    std::map<std::string, LegMotorData> Legs;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorDataExport, StepTime, Legs)
};

struct MinMax
{
    double Min = 0.0;
    double Max = 0.0;
    double Range = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MinMax, Min, Max, Range)
};

struct AngleConstraint
{
    double MinAllowed = 0.0;
    double MaxAllowed = 0.0;
    double MinActual = 0.0;
    double MaxActual = 0.0;
    bool IsValid = false;
    std::string CheckType;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(AngleConstraint, MinAllowed, MaxAllowed, MinActual, MaxActual, IsValid, CheckType)
};

struct DistanceConstraint
{
    double MinAllowed = 0.0;
    double MaxAllowed = 0.0;
    double MinActual = 0.0;
    double MaxActual = 0.0;
    bool IsValid = false;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(DistanceConstraint, MinAllowed, MaxAllowed, MinActual, MaxActual, IsValid)
};

struct PerpendicularityConstraint
{
    double MaxDotProduct = 0.0;
    double MinDotProduct = 0.0;
    bool IsValid = false;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(PerpendicularityConstraint, MaxDotProduct, MinDotProduct, IsValid)
};

struct ConstraintViolation
{
    std::string ConstraintType;
    int Frame = 0;
    double Time = 0.0;
    double Value = 0.0;
    std::string AllowedRange;
    std::string Severity;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(ConstraintViolation, ConstraintType, Frame, Time, Value, AllowedRange, Severity)
};

struct ConstraintData
{
    AngleConstraint AB1_AB_Constraint;
    DistanceConstraint C3C4_Constraint;
    DistanceConstraint CL_Distance_Constraint;
    PerpendicularityConstraint Perpendicularity_Constraint;
    std::vector<ConstraintViolation> ConstraintViolations;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(ConstraintData, AB1_AB_Constraint, C3C4_Constraint,
                                   CL_Distance_Constraint, Perpendicularity_Constraint, ConstraintViolations)
};

struct AngleStatistics
{
    MinMax AB_Horizontal;
    MinMax AB_BC;
    MinMax BC_CL;
    MinMax AB1_AB;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(AngleStatistics, AB_Horizontal, AB_BC, BC_CL, AB1_AB)
};

struct DistanceStatistics
{
    MinMax C3_C4;
    MinMax C2_C3;
    MinMax C2_C4;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(DistanceStatistics, C3_C4, C2_C3, C2_C4)
};

struct MotorSpeedStatistics
{
    MinMax Thigh;
    MinMax Shank;
    MinMax Ankle;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorSpeedStatistics, Thigh, Shank, Ankle)
};

struct TrajectoryStatistics
{
    int TotalFrames = 0;
    double TotalTime = 0.0;
    int SwingFrames = 0;
    int SupportFrames = 0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(TrajectoryStatistics, TotalFrames, TotalTime, SwingFrames, SupportFrames)
};

struct StatisticsData
{
    AngleStatistics AngleStatistics;
    DistanceStatistics DistanceStatistics;
    MotorSpeedStatistics MotorSpeedStatistics;
    TrajectoryStatistics TrajectoryStatistics;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(StatisticsData, AngleStatistics, DistanceStatistics,
                                   MotorSpeedStatistics, TrajectoryStatistics)
};

struct RawInitialAngles
{
    double Thigh = 0.0;
    double Shank = 0.0;
    double Ankle = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(RawInitialAngles, Thigh, Shank, Ankle)
};

struct CalibratedAngles
{
    double Thigh = 0.0;
    double Shank = 0.0;
    double Ankle = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(CalibratedAngles, Thigh, Shank, Ankle)
};

struct Adjustments
{
    double ThighAdjustment = 0.0;
    double ShankAdjustment = 0.0;
    double AnkleAdjustment = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(Adjustments, ThighAdjustment, ShankAdjustment, AnkleAdjustment)
};

struct LegInitialAngles
{
    std::string LegCode;
    RawInitialAngles RawInitialAngles;
    CalibratedAngles CalibratedAngles;
    Adjustments Adjustments;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegInitialAngles, LegCode, RawInitialAngles, CalibratedAngles, Adjustments)
};

struct InitialAnglesData
{
    std::map<std::string, LegInitialAngles> Legs;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(InitialAnglesData, Legs)
};

struct PhaseInfo
{
    std::string GaitType;
    double TimeLF = 0.0;
    double TimeLH = 0.0;
    double TimeRF = 0.0;
    double TimeRH = 0.0;
    int PhaseLF = 0;
    int PhaseLH = 0;
    int PhaseRF = 0;
    int PhaseRH = 0;
    double SupportRatio = 0.0;
    double SwingRatio = 0.0;
    double SupportTime = 0.0;
    double SwingTime = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(PhaseInfo, GaitType, TimeLF, TimeLH, TimeRF, TimeRH,
                                   PhaseLF, PhaseLH, PhaseRF, PhaseRH, SupportRatio,
                                   SwingRatio, SupportTime, SwingTime)
};

struct MotionRange
{
    MinMax X;
    MinMax Y;
    double TotalRange = 0.0;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotionRange, X, Y, TotalRange)
};

struct LegMotionRange
{
    std::string LegCode;
    int PhaseShift = 0;
    MotionRange L_Point;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegMotionRange, LegCode, PhaseShift, L_Point)
};

struct MotionRangeData
{
    std::map<std::string, LegMotionRange> Legs;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotionRangeData, Legs)
};

struct SplitModelID
{
    std::vector<std::map<std::string, std::vector<std::string>>> LF;
    std::vector<std::map<std::string, std::vector<std::string>>> LH;
    std::vector<std::map<std::string, std::vector<std::string>>> RF;
    std::vector<std::map<std::string, std::vector<std::string>>> RH;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(SplitModelID, LF, LH, RF, RH)
};

// 支撑点检查结果类
struct SupportCheckResult
{
    bool IsSupport = false;
    std::map<std::string, CheckResultItem> Checks;
    std::vector<std::string> Reasons;
    std::vector<std::string> FailedChecks;

    SupportCheckResult();

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(SupportCheckResult, IsSupport, Checks, Reasons, FailedChecks)
};

class MotorData
{
public:
    std::vector<double> frame_start_times;
    std::vector<double> frame_end_times;
    std::vector<double> AB_angles;
    std::vector<double> AB_angle_increments;
    std::vector<double> thigh_motor_speeds;
    std::vector<double> AB1_AB_angles;
    std::vector<double> AB1_AB_angle_increments;
    std::vector<double> shank_motor_speeds;
    std::vector<double> C3C4_distances;
    std::vector<double> C3C4_distance_increments;
    std::vector<double> ankle_motor_angles;
    std::vector<double> ankle_motor_angle_increments;
    std::vector<double> ankle_motor_speeds;

    MotorData(int n_frames = 0);
    void Resize(int n_frames);
    MotorData Clone() const;

    friend void to_json(json &j, const MotorData &md);
    friend void from_json(const json &j, MotorData &md);
};

struct ReverseCalculationResult
{
    int Frame = 0;
    double Time = 0.0;
    std::map<std::string, std::vector<double>> Points;
    bool Valid = false;
    std::vector<std::string> Errors;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(ReverseCalculationResult, Frame, Time, Points, Valid, Errors)
};

struct CompleteExportData
{
    GaitInfo GaitInfo;
    SystemParameters SystemParameters;
    TrajectoryData TrajectoryData;
    MotorDataExport MotorData;
    ConstraintData ConstraintData;
    StatisticsData Statistics;
    InitialAnglesData InitialAngles;
    PhaseInfo PhaseInfo;
    MotionRangeData MotionRange;

    NLOHMANN_DEFINE_TYPE_INTRUSIVE(CompleteExportData, GaitInfo, SystemParameters,
                                   TrajectoryData, MotorData, ConstraintData, Statistics,
                                   InitialAngles, PhaseInfo, MotionRange)
};

class ModelIDSplitter
{
public:
    // 修改参数类型为 const ModelID&
    static SplitModelID SplitModelIDToArrays(const ModelID &modelID,
                                             const std::map<std::string, std::vector<double>> &pointsDict);

private:
    static std::vector<std::map<std::string, std::vector<std::string>>>
    SplitLegDictionaryDataToArrays(const std::vector<std::map<std::string, std::string>> &legData,
                                   const std::set<std::string> &validKeys);

    static std::map<std::string, std::vector<std::string>>
    SplitDictionaryToArrays(const std::map<std::string, std::string> &dict,
                            const std::set<std::string> &validKeys);

    static std::vector<std::string> SplitKey(const std::string &compositeKey,
                                             const std::set<std::string> &validKeys);
};

class QuadrupedRobotConfiguration
{
public:
    // 基本几何参数
    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 s_l;
    double s_h;
    double X;

    // 脚部构型参数
    double D1_L_offset;
    double D2_L_offset;
    double C4_D2_offset;

    // 步态控制参数
    double T;
    double f;
    std::string gait_type;
    double support_ratio;
    double swing_ratio;

    // 电机参数
    double thigh_motor_reduction;
    double shank_motor_reduction;
    double ankle_motor_reduction;

    // 逆向运动学特有参数
    double adjusted_shank_angles_factory;

    // 初始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_reference_distance;
    double deltX;
    double step;
    double t_s;
    double t_w;
    double timeLF;
    double timeLH;
    double timeRF;
    double timeRH;

    QuadrupedRobotConfiguration();
    QuadrupedRobotConfiguration(const GaitInfo &gaitInfo, const SystemParameters &sysParams);
    void FromGaitInfoAndSystemParameters(const GaitInfo &gaitInfo, const SystemParameters &sysParams);
    void CalculateDerivedParameters();

    struct ValidationResultFrame
    {
        int Frame = 0;
        double ThighAngle = 0.0;
        double ShankAngle = 0.0;
        double AnkleAngle = 0.0;
        bool IsValid = false;

        NLOHMANN_DEFINE_TYPE_INTRUSIVE(ValidationResultFrame, Frame, ThighAngle, ShankAngle, AnkleAngle, IsValid)
    };

    // 在QuadrupedRobotConfiguration类中添加公共方法（BaseClass.h中）
    void debugPrint() const;

    struct ValidationResult
    {
        bool IsValid = false;
        std::vector<std::string> Errors;
        std::vector<std::string> Warnings;

        ValidationResult();

        NLOHMANN_DEFINE_TYPE_INTRUSIVE(ValidationResult, IsValid, Errors, Warnings)
    };

    ValidationResult Validate() const;

private:
    void FromSystemParameters(const SystemParameters &sysParams);
    void FromGaitInfo(const GaitInfo &gaitInfo);

    friend void to_json(json &j, const QuadrupedRobotConfiguration &config);
    friend void from_json(const json &j, QuadrupedRobotConfiguration &config);
};

class RobotGaitDataManager
{
private:
    RobotGaitRequest _data;
    static std::mutex _lock;

public:
    RobotGaitDataManager();
    const RobotGaitRequest &GetData() const;
    void SetData(const RobotGaitRequest &data);
    void LoadFromJson(const nlohmann::json &j, bool useDefaultForMissing = true);
    void LoadDefaultData();

    ModelID GetModelIDObject() const;

    GaitInfo GetGaitInfo() const;
    SystemParameters GetSystemParameters() const;
    std::map<std::string, std::vector<std::map<std::string, std::string>>> GetModelID() const;
    LegParameters GetParam() const;
    RobotBody GetRobotBody() const;
    std::map<std::string, LegParam> GetParamDictionary() const;

    // 在 RobotGaitDataManager 类声明中修改
    std::map<std::string, std::vector<double>> Get_old_initial_points_dictLF() const;
    std::map<std::string, std::vector<double>> Get_old_initial_points_dictLH() const;
    void Set_old_initial_points_dictLF(const std::map<std::string, std::vector<double>> &L_point);
    void Set_old_initial_points_dictLH(const std::map<std::string, std::vector<double>> &L_point);

    void SetRobotBody_x(const double x);

private:
    RobotGaitRequest CreateDefaultRobotGaitRequest() const;
    RobotGaitRequest FillMissingDataWithDefaults(const RobotGaitRequest &data) const;
    static bool IsDefaultValue(double value);
};

class SimulationStatistics
{
public:
    int TotalFrames = 0;
    double TotalTime = 0.0;
    double MinABAngle = 0.0;
    double MaxABAngle = 0.0;
    double MinBCAngle = 0.0;
    double MaxBCAngle = 0.0;
    double MinAB1ABAngle = 0.0;
    double MaxAB1ABAngle = 0.0;
    double MinC3C4Distance = 0.0;
    double MaxC3C4Distance = 0.0;
    double MaxThighSpeed = 0.0;
    double MaxShankSpeed = 0.0;
    double MaxAnkleSpeed = 0.0;

    SimulationStatistics() = default;
    explicit SimulationStatistics(const QuadrupedRobotConfiguration &config);

    std::string ToString() const;

    friend void to_json(json &j, const SimulationStatistics &stats);
    friend void from_json(const json &j, SimulationStatistics &stats);
};

class SplitModelIDExtensions
{
public:
    static std::map<std::string, std::map<std::string, std::vector<std::string>>>
    ToSimpleDictionary(const SplitModelID &splitModelID);

private:
    static std::map<std::string, std::vector<std::string>>
    MergeLegData(const std::vector<std::map<std::string, std::vector<std::string>>> &legData);
};

// 实用函数
Point2D ArrayToPoint(const std::vector<double> &array);

#endif // BASE_CLASS_H