#ifndef RM_DRIVER_KINEMATICS_HPP
#define RM_DRIVER_KINEMATICS_HPP

#include "arm_driver.hpp"
#include "arm_define.h"
#include "rm_service.h"

enum {
    RM_DRIVER_KINEMATICS_UNINIT = 0,
    RM_DRIVER_KINEMATICS_INIT_OK,
    RM_DRIVER_KINEMATICS_INIT_ERR
};

enum RmKinematicsStatus {
    RM_KINEMATICS_STATUS_INIT = 0,
    RM_KINEMATICS_STATUS_RUN,
    RM_KINEMATICS_STATUS_END,
};

struct NodataTrajectory {
    float start[ARM_DOF];
    float end[ARM_DOF];
    int currentStep;

    int runState; // 0: stop, 1: running
    int goalType;
    int armId;
};

class RmArmDriverAndKinematics
{
public:
    RmArmDriverAndKinematics();
    ~RmArmDriverAndKinematics();

    int Init();
    static int ArmMove(const float points[USED_ARM_DOF], int armId);

    using ApiLogFunc = void(*)(const char* message, va_list args);
    void SetCustomApiLog(ApiLogFunc logFunc) {customApiLog_ = logFunc;}

    using RmArmStatusCallbackFun = void(*)(ArmStatusCallbackData *data);
    void RegArmStatusCallback(RmArmStatusCallbackFun callback) {
        armStatusCallback_ = callback;
    }

    void getAngleForCheckingInner(NodataTrajectory *trajectory, int timeStep, float *aimAngles);
    void getAngleForCheckingOffset(NodataTrajectory *trajectory, int timeStepOffset, float *aimAngles);
    int computeMovingTrajectory(NodataTrajectory *trajectory, const float *start, const float *end, int armId);
    int computeSteppingTrajectory(NodataTrajectory *trajectory, const float *start, const float *speed, int armId);
    int computePoseSteppingTrajectory(NodataTrajectory *trajectory, const float *start, const float *end, int armId);
    int updateTrajectory(NodataTrajectory *trajectory, float *aimAngles);
    void stopTrajectory(NodataTrajectory *trajectory);
    int continuePoseSteppingTrajectory(NodataTrajectory *trajectory, const float *nowAngles, const float *direct);

    int ForwardKinematics(float *jointAngles, float *endEffectorPose);
    int InverseKinematics(const float *endEffectorPose, float *jointAngles);

    static RM_Service *rmService_;

private:
    int initFlag_;
    static ApiLogFunc customApiLog_;
    static RmArmStatusCallbackFun armStatusCallback_;
    static rm_robot_handle *leftRobotHandle_;
    static rm_robot_handle *rightRobotHandle_;

    static void CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data);
    int InitHandle(rm_robot_handle **robotHandle_, const char *robot_ip_address, int robot_port, rm_realtime_push_config_t *config);
    static void api_log(const char* format, ...);
};

#endif // RM_DRIVER_KINEMATICS_HPP