#ifndef ROBOT_H
#define ROBOT_H
#include <unordered_map>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_lma.hpp> // 锟斤拷锟斤拷LMA锟斤拷锟斤拷锟酵凤拷募锟�
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <string>
// 锟斤拷 Robot.h 锟斤拷锟斤拷锟斤拷
#include <vector>
#include "utils.h"
class Robot
{
private:
    KDL::Chain kinematicChain;
    KDL::ChainFkSolverPos_recursive *fkSolver;
    KDL::ChainIkSolverVel_pinv *ikVelSolver;
    KDL::ChainIkSolverPos_NR *ikSolverNR;
    KDL::ChainIkSolverPos_LMA *ikSolverLMA; // 锟斤拷锟斤拷LMA锟斤拷锟斤拷锟�
    bool m_initialized;

    std::unordered_map<std::string, std::string> jointChildLinkUuidMap; // joint锟斤拷锟狡碉拷child link UUID锟斤拷映锟斤拷

    // 私锟叫革拷锟斤拷锟斤拷锟斤拷
    double quaternionAngleDifference(const double q1[4], const double q2[4]);
    int calculateAutoSteps(const double startPose[7], const double endPose[7],
                           int minSteps, int maxSteps,
                           double positionResolution, double orientationResolution);

    // 锟斤拷锟斤拷私锟叫凤拷锟斤拷
    bool parseJointChildLinkUuidsFromUrdf(const std::string &urdfString);

public:
    /**
     * @brief 默锟较癸拷锟届函锟斤拷
     */
    Robot();
    /**
     * @brief 锟斤拷URDF锟街凤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
     * @param urdfString URDF锟斤拷式锟斤拷锟街凤拷锟斤拷
     */
    explicit Robot(const std::string &urdfString);
    /**
     * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷源
     */
    ~Robot();
    int numberOfJoints;
    /**
     * @brief 锟斤拷URDF锟街凤拷锟斤拷锟斤拷始锟斤拷锟斤拷锟斤拷锟斤拷
     * @param urdfString URDF锟斤拷式锟斤拷锟街凤拷锟斤拷
     * @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
     */
    bool initRobot(const std::string &urdfString);

    /**
     * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷呕锟饺�joint锟斤拷应锟斤拷child link UUID
     * @param jointIndex joint锟斤拷锟� (1, 2, 3, ...)
     * @return 锟斤拷应锟斤拷UUID锟街凤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷蚍祷乜锟斤拷址锟斤拷锟�
     */
    std::string getJointUuidByIndex(int jointIndex) const;

    /**
     * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷呕锟饺�joint锟斤拷锟斤拷
     * @param jointIndex joint锟斤拷锟� (1, 2, 3, ...)
     * @return joint锟斤拷锟斤拷锟街凤拷锟斤拷锟斤拷锟斤拷 "joint_1", "joint_2" 锟斤拷
     */
    static std::string getJointNameByIndex(int jointIndex);

    /**
     * @brief 锟斤拷取锟斤拷锟叫匡拷锟斤拷joint锟斤拷锟斤拷斜锟�
     * @return 锟斤拷锟斤拷锟斤拷锟叫匡拷锟斤拷joint锟斤拷诺锟斤拷锟斤拷锟�
     */
    std::vector<int> getAvailableJointIndices() const;

    // 锟斤拷Robot.h锟斤拷锟斤拷锟斤拷
    std::string findJointUuidByAnyFormat(int jointIndex) const;

    json inversePoseStr(const std::string &pose_str, const std::string &q_init_str);
    // 锟斤拷锟斤拷锟斤拷锟斤拷筒锟街碉拷锟斤拷锟斤拷锟斤拷锟斤拷锟截诧拷趾锟斤拷锟斤拷锟�
    json inversePoseStr2PSteps(const std::string &pose_str, const std::string &q_init_str, const std::int32_t steps);
    // 锟斤拷锟斤拷锟斤拷牡锟街�锟戒不锟斤拷锟叫诧拷趾锟街憋拷臃锟斤拷囟锟接︼拷锟斤拷锟斤拷锟�
    json inversePoseStrNoDifference(const std::string &pose_str, const std::string &q_init_str);
    std::vector<std::vector<double>> parsePoseString(const std::string &pose_str);
    std::vector<std::vector<double>> parseJointListString(const std::string &pose_str);

    std::unordered_map<std::string, std::string> getJointChildLinkUuidMap() const { return jointChildLinkUuidMap; }
    std::string getJointChildLinkUuid(const std::string &jointName) const;
    std::vector<double> parseJointString(const std::string &joint_str);

    json kinematicsForwardAllJointsList(std::vector<double> joints);
    json handleKinematicsForwardAllJoints(const std::string &joints_str);
    json handleKinematicsForwardAllJoints_objStates(const std::string &joints_str);
    /**
     * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟角凤拷锟窖筹拷始锟斤拷
     * @return 锟窖筹拷始锟斤拷锟斤拷锟斤拷true
     */
    bool isInitialized() const { return m_initialized; }

    /**
     * @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� (NR锟斤拷锟斤拷)
     * @param pose 目锟斤拷位锟剿ｏ拷锟斤拷锟斤拷锟斤拷式[x, y, z, qx, qy, qz, qw]
     * @param iniJ 锟斤拷始锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
     * @param resultJoints 锟斤拷锟斤拷锟斤拷锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
     * @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
     */
    bool calculateIK_NR(const double pose[7], const double iniJ[6], double resultJoints[6]);

    /**
     * @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� (LMA锟斤拷锟斤拷)
     * @param pose 目锟斤拷位锟剿ｏ拷锟斤拷锟斤拷锟斤拷式[x, y, z, qx, qy, qz, qw]
     * @param iniJ 锟斤拷始锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
     * @param resultJoints 锟斤拷锟斤拷锟斤拷锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
     * @param eps 锟斤拷锟斤拷锟斤拷锟斤拷 (默锟斤拷: 1e-8)
     * @param maxiter 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷 (默锟斤拷: 3000)
     * @param eps_joints 锟截斤拷锟捷诧拷 (默锟斤拷: 1e-12)
     * @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
     */
    bool calculateIK_LMA(const double pose[7], const double iniJ[6], double resultJoints[6],
                         double eps = 1e-8, int maxiter = 3000, double eps_joints = 1e-12);

    /**
     * @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� - TCP位锟斤拷
     * @param joints 锟斤拷锟斤拷亟诮嵌龋锟斤拷锟斤拷锟斤拷锟绞絒6锟斤拷锟截节角讹拷]
     * @param tcpPose 锟斤拷锟絋CP位锟剿ｏ拷锟斤拷锟斤拷锟斤拷式[x, y, z, qx, qy, qz, qw]
     * @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
     */
    bool calculateFK_TCP(const double joints[6], double tcpPose[7]);

    /**
     * @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� - 锟斤拷锟叫关斤拷位锟斤拷
     * @param joints 锟斤拷锟斤拷亟诮嵌龋锟斤拷锟斤拷锟斤拷锟绞絒6锟斤拷锟截节角讹拷]
     * @param jointPoses 锟斤拷锟斤拷锟斤拷泄亟锟轿伙拷耍锟斤拷锟斤拷锟斤拷锟绞絒6*7锟斤拷元锟斤拷]
     * @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
     */
    bool calculateFK_AllJointsforwardKinematics(const double joints[6], double jointPoses[42]);
    bool forwardKinematics(const double joints[6], double jointPoses[42]);
    /**
     * @brief 锟斤拷取锟斤拷锟斤拷锟斤拷锟剿讹拷锟斤拷
     * @return KDL锟剿讹拷锟斤拷锟斤拷锟斤拷
     */
    const KDL::Chain &getKinematicChain() const { return kinematicChain; }

    /**
     * @brief 锟斤拷取锟截斤拷锟斤拷锟斤拷
     * @return 锟截斤拷锟斤拷锟斤拷
     */
    int getNumberOfJoints() const { return kinematicChain.getNrOfJoints(); }

    // 锟届迹锟芥划锟斤拷锟斤拷
    /**
     * @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊�锟斤拷锟斤拷锟斤拷姹撅拷锟�
     * @param pose1 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param pose2 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟斤拷锟斤拷锟�
     * @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐）
     * @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1
     */
    int trajectoryPlanning(const double pose1[7], const double pose2[7],
                           double outputPoses[][7], int npoint = 0);
    /**
     * @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊�锟斤拷锟斤拷锟斤拷姹撅拷锟斤拷锟斤拷锟斤拷vector锟斤拷
     * @param pose1 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param pose2 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟絭ector锟斤拷锟斤拷锟斤拷预锟饺凤拷锟斤拷锟节存）
     * @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐）
     * @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1
     */
    int trajectoryPlanning(const double pose1[7], const double pose2[7],
                           std::vector<std::vector<double>> &outputPoses,
                           int npoint = 0);

    /**
     * @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊�锟斤拷vector锟芥本锟斤拷
     * @param pose1 锟斤拷始位锟斤拷
     * @param pose2 锟斤拷止位锟斤拷
     * @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟斤拷锟斤拷锟�
     * @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐）
     * @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1
     */
    int trajectoryPlanning(const std::vector<double> &pose1, const std::vector<double> &pose2,
                           std::vector<std::vector<double>> &outputPoses, int npoint = 0);

    // 锟斤拷锟竭猴拷锟斤拷
    /**
     * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷元锟斤拷之锟斤拷慕嵌炔锟�
     * @param q1 锟斤拷元锟斤拷1 [qx, qy, qz, qw]
     * @param q2 锟斤拷元锟斤拷2 [qx, qy, qz, qw]
     * @return 锟角度差（锟斤拷锟饺ｏ拷
     */
    // static double quaternionAngleDifference(const double q1[4], const double q2[4]);

    /**
     * @brief 锟皆讹拷锟斤拷锟斤拷旒ｏ拷婊�锟斤拷锟斤拷锟斤拷锟�
     * @param startPose 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param endPose 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param minSteps 锟斤拷小锟斤拷锟斤拷
     * @param maxSteps 锟斤拷锟斤拷锟�
     * @param positionResolution 位锟矫分憋拷锟绞ｏ拷锟阶ｏ拷
     * @param orientationResolution 锟斤拷态锟街憋拷锟绞ｏ拷锟斤拷锟饺ｏ拷
     * @return 锟狡硷拷锟侥轨迹锟斤拷锟斤拷锟斤拷
     */
    // static int calculateAutoSteps(const double startPose[7], const double endPose[7],
    //     int minSteps = 10, int maxSteps = 100,
    //     double positionResolution = 0.01,
    //     double orientationResolution = 0.1);

    // 锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷息
    // const KDL::Chain& getKinematicChain() const { return kinematicChain; }
    // int getNumberOfJoints() const { return kinematicChain.getNrOfJoints(); }

    int calculateAutoSteps(const std::vector<double> &startPose, const std::vector<double> &endPose,
                           int minSteps, int maxSteps,
                           double positionResolution, double orientationResolution);
    double quaternionAngleDifference(const std::vector<double> &q1, const std::vector<double> &q2);

    /**
     * @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� (LMA锟斤拷锟斤拷) - 锟斤拷锟斤拷vector锟芥本
     * @param pose 目锟斤拷位锟斤拷 [x, y, z, qx, qy, qz, qw]
     * @param iniJ 锟斤拷始锟截节角讹拷 [6锟斤拷锟截节角讹拷]
     * @param eps 锟斤拷锟斤拷锟斤拷锟斤拷
     * @param maxiter 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
     * @param eps_joints 锟截斤拷锟捷诧拷
     * @return 锟截节角讹拷vector
     */
    std::vector<double> inverse(const double pose[7], const double iniJ[6],
                                double eps = 1e-8, int maxiter = 3000,
                                double eps_joints = 1e-12);

    std::vector<double> inverse(const std::vector<double> &pose,
                                const std::vector<double> &iniJ,
                                double eps = 1e-8, int maxiter = 3000,
                                double eps_joints = 1e-12);

    std::vector<double> inverse(const double pose[7],
                                double eps = 1e-8, int maxiter = 3000,
                                double eps_joints = 1e-12);

    std::vector<double> inverse(const std::vector<double> &pose,
                                double eps = 1e-8, int maxiter = 3000,
                                double eps_joints = 1e-12);
};

#endif // ROBOT_H