//
// Created by xtkuang on 2025/5/8.
//

#ifndef CMVR_ES_ABSTRACT_ROBOT_H
#define CMVR_ES_ABSTRACT_ROBOT_H
#pragma once

#include "jsoncpp/json/json.h"
#include "abstract_device.h"
#include "utils/controller/cartesian_controller.h"
#include "cmvr/msgs/geometry.pb.h"
#include "cmvr/msgs/motor.pb.h"

namespace cmvr::device{

     struct JointPoint{
        std::string joint_name;
        double rad;             // rad
        double vel;             // rad / s

        JointPoint(const std::string& name, double r, double v)
        : joint_name(name), rad(r), vel(v) {}
         JointPoint(const std::string& name, double r)
        : joint_name(name), rad(r), vel(0) {}
         JointPoint()
       : joint_name(""), rad(0), vel(0) {}
    } ;

    typedef struct {
        std::string joint_name;
        double vel;             // rad / s
    } JointVelocityCommand;

    typedef struct {
        std::string joint_name;
        double current;         //
    } JointCurrentCommand;

    class AbstractRobot: public AbstractDevice {
    public:
        explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
        ~AbstractRobot() override=default;

        virtual int getDOF() { throw std::runtime_error("Not implemented"); }

        virtual std::vector<std::string> getJointNames() { throw std::runtime_error("Not implemented"); }

        virtual std::unordered_map<std::string,double> getJointQ() const { throw std::runtime_error("Not implemented"); }

        /**
         *
         * @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值
         */
        virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const {throw std::runtime_error("Not implemented");}
        virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); }

        virtual void getJointsState(std::vector<JointState>& states)  = 0;
        virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); }

        virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}

        virtual void torqueOn() { throw std::runtime_error("Not implemented"); }
        virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }

        virtual void torqueOff() { throw std::runtime_error("Not implemented"); }
        virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }

        virtual void eStop() { throw std::runtime_error("Not implemented"); }

        virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }

        virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
        virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
        virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }

        virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }

        virtual void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1)  { throw std::runtime_error("Not implemented"); }

        virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }

        virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }

        virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
        virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }

        virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }

        virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }

        virtual void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
        virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
        virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
        virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }


        virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }

        virtual void calibrateZeroQ(const std::string &joint_name) = 0;

        void setToolFrame(const std::string& toolFrame)
        {
            toolFrame_ = toolFrame;
        }

        virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
        virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
    protected:
        int dof_{};
        RobotState state_{};
        std::string toolFrame_;
    };
}

#endif //CMVR_ES_ABSTRACT_ROBOT_H