#include <iostream>
#include <chrono>
#include <thread>
#include "DynamixelHandler.h"
// Global variables
DynamixelHandler _oDxlHandler;
std::string _poppyDxlPortName = "/dev/ttyUSB0";
float _poppyDxlProtocol = 2.0;
int _poppyDxlBaudRate = 1000000;
int _nbJoints = 6;
float _minJointCmd = 0;
float _maxJointCmd = 1023;
float _minJointAngle = -180.0f;
float _maxJointAngle = 180.0f;

int convertAnglesToJointCmd(float fJointAngle)
{ // y = ax + b
float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
float b = _minJointCmd - a * _minJointAngle;
float jointCmd = a * fJointAngle + b;
return (int)jointCmd;
}
void goToHomePosition()
{ std::vector<uint16_t> l_vTargetJointPosition;
for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}

float convertJointCmdToAngle(int jointCmd) {
    const float jointCmdRange = 65535.0f; // range of the joint command (0 to 65535)
    const float angleRange = 360.0f; // range of the joint angle (-180 to 180)
    float jointCmdMapped = (jointCmd / jointCmdRange) * angleRange; // map the joint command value to the angle range
    if (jointCmdMapped > 180.0f) {
        jointCmdMapped -= 360.0f; // ensure that the angle is between -180 and 180
    }
    return jointCmdMapped;
}


void gripperControl(float targetAngle)
{
    const int jointIndex = 5; // index of the 6th motor (starts from 0)
    const float kP = 0.1f; // proportional gain

    // Read current joint torque
    std::vector<uint16_t> l_vCurrentJointTorque;
    _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);

    // Calculate error and proportional control
    float error = targetAngle - convertJointCmdToAngle(l_vCurrentJointTorque[jointIndex]);
    float jointCmd = convertAnglesToJointCmd(convertJointCmdToAngle(l_vCurrentJointTorque[jointIndex]) + kP * error);

    // Set the target joint position for the 6th motor
    std::vector<uint16_t> l_vTargetJointPosition;
    for (int l_joint = 0; l_joint < _nbJoints; l_joint++) {
        if (l_joint == jointIndex) {
            l_vTargetJointPosition.push_back(jointCmd);
        }
        else {
            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
        }
    }
    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}





int main()
{

    std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
_oDxlHandler.setDeviceName(_poppyDxlPortName);
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
_oDxlHandler.openPort();
_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
_oDxlHandler.enableTorque(true);
std::cout << std::endl;
// read current joint position
std::vector<uint16_t> l_vCurrentJointPosition;
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
// display current joint position
std::cout << "vCurrentJointPosition= (" << std::endl;
for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
std::cout << l_vCurrentJointPosition[l_joint] << ", ";
std::cout << ")" << std::endl;
// wait 1s
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
goToHomePosition();
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
gripperControl(l_vCurrentJointPosition[5]);
std::this_thread::sleep_for(std::chrono::milliseconds(2000));

std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
_oDxlHandler.enableTorque(false);
_oDxlHandler.closePort();
std::cout << std::endl;
return 0;
}