#include <iostream>
#include <chrono>
#include <thread>
#include "DynamixelHandler.h"

using namespace std;

// Global variables
DynamixelHandler _oDxlHandler;
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()
{ 
	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);
}

void gripperControl(){
    while(true){
        int slot = 2;
        std::vector<uint16_t> l_vCurrentJointTorque;
        _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
        int angle = 0;
        int var = l_vCurrentJointTorque[slot];
        cout<<"initial torque value = "<<var<<endl;
                while (var < 2000){
                    int var = l_vCurrentJointTorque[slot];
                    vector<uint16_t> l_vTargetJointPosition;
                    for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
                    {
                        l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
                    }
                    l_vTargetJointPosition.push_back(convertAnglesToJointCmd(angle));
                    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
                    _oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
                    cout<<"torque value: "<<l_vCurrentJointTorque[slot]<<endl;
                    angle ++;
                }
                cout<<"done!!!!!!!!!!!"<<endl;
                break;
    }

}

void inputEndEffector(int Theta){
    while(true){
        cout<<"angle: "<<endl;
        cin >> Theta;
        if (Theta > -90 && Theta < 30){
            vector<uint16_t> l_vTargetJointPosition;
            for (int l_joint = 0; l_joint < (_nbJoints-1); l_joint++)
            {
                    l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
            }
            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(Theta));
            _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
        }
        else{
            cout<<"angle outisde of range";
        }
    }
}

void print_position(const char* state)
{
	cout<< state << endl;
	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	// read current joint position
	vector<uint16_t> l_vCurrentJointPosition;
	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
	cout << "vCurrentJointPosition= (" << endl;
	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
		cout << l_vCurrentJointPosition[l_joint] << ", ";
		cout << ")" << endl;
	// wait 1s
	this_thread::sleep_for(chrono::milliseconds(1000));
}

int main()
{ 
	cout << "===Initialization of the Dynamixel Motor communication====" << endl;
	_oDxlHandler.setDeviceName(_poppyDxlPortName);
	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
	_oDxlHandler.openPort();
	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
	_oDxlHandler.enableTorque(true);
	cout << endl;
	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	// display current joint position
        print_position("=========BEFORE MOVING========");
        goToHomePosition();
        print_position("=========AFTER MOVING=========");
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        //move only end effector angle
        gripperControl();
	cout << "===Closing the Dynamixel Motor communication====" << endl;
	_oDxlHandler.enableTorque(false);
	_oDxlHandler.closePort();
	cout << endl;
	return 0;
}