#include <iostream>
#include <chrono>
#include <thread>
//#include<windows.h>//for windows
#include<unistd.h>//for linux
#include "DynamixelHandler.h"
#include <array>


// 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 _targetJointTorqueGripper = 1140;
float _proportionnalIncrement = 0.025f;
float _proportionnalIncrementAbove = 0.01f;
float _threshold= 1.9;
int waitTime = 100;
std::array<std::array<short unsigned int, 6>,9> notes_position = {{{511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}, {511, 511, 511, 511, 511, 511}}};
//short unsigned int notes_position[54] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54};
std::array<short unsigned int, 6> playingPosition = {{511, 410, 515, 515, 685, 589}}; //511, 310, 485, 515, 665, 592


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;
}

int convertJointCmdToAngles(float fJointCmd)
{ // y = ax + b
	float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
	float b = _minJointCmd - a * _minJointAngle;
	float fJointAngle = (fJointCmd - b)/a;
	return (int)fJointAngle;
}

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);
}

void goToPlayingPosition()
{
	std::vector<uint16_t> l_vTargetJointPosition;
	for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
		l_vTargetJointPosition.push_back(playingPosition[l_joint]);
	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
	/*std::vector<uint16_t> l_vTargetJointPosition;
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-45.0f));
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.0f));
	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(35.0f)); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);*/
}

std::array<short unsigned int,6> currentPos()
{
	std::array<short unsigned int, 6> myPosition = {{0, 1, 2, 3, 4, 5}};
	
	// read current joint position
	std::vector<uint16_t> l_vCurrentJointPosition;
	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
	
	//store current joint position to return it
	for (int l_joint=0; l_joint < myPosition.size(); l_joint++)
	{	
		myPosition[l_joint] = l_vCurrentJointPosition[l_joint];
	}
	
	// display current joint position
	std::cout << "vCurrentJointPosition= (";
	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
	std::cout << ")" << std::endl;
	
	return myPosition;
}

void teachTarget()
{
	_oDxlHandler.enableTorque(false);
	std::cout<<"===Disabling Torque==="<<std::endl;
	
	int noteNumber = 333;
	
	std::cout << "enter 555 when all the notes have been taught\n";
	std::cout << "enter note number (starting from 0):\n"; //allows to wait until user input, and specify which note is given
	std::cin >> noteNumber;
	
	while(noteNumber != 555)
	{
		while (noteNumber < 0 or (noteNumber > 9 and noteNumber != 555)) // test if the input is good
		{	
			std::cout << "wrong number: enter note number (between 0 and 9), or exit with 555:\n";
			std::cin >> noteNumber;
		}
		if (noteNumber != 555)
		{
		// read current joint position
			std::vector<uint16_t> l_vCurrentJointPosition;
			_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
			for (int l_joint=0; l_joint < notes_position[noteNumber].size(); l_joint++)
			{	
				notes_position[noteNumber][l_joint] = l_vCurrentJointPosition[l_joint];
				std::cout<<notes_position[noteNumber][l_joint]<<", ";
				std::cout<<noteNumber << ", ";
			}
			currentPos(); //print position		
		
			std::cout << "enter note number (starting from 0):\n"; //allows to wait until user input, and specify which note is given
			std::cin >> noteNumber;
		}
	}
	
	_oDxlHandler.enableTorque(true); //gives Hardware error
	std::cout<<"===Enabling Torque==="<<std::endl;
	goToPlayingPosition();
	
	return ;
}

int closeGripper()
{
	//read current joint 6 torque
	std::vector<uint16_t> l_vCurrentJointTorque;
	_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
	
	//read current joint 6 position
	std::vector<uint16_t> l_vCurrentJointPosition;
	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
	int joint6Position = l_vCurrentJointPosition[5];
	
	// display current joint 6 torque
	std::cout << "CurrentJoint6Torque= ";
	std::cout << l_vCurrentJointTorque[5] << std::endl;
	int joint6Torque = l_vCurrentJointTorque[5];
	
	int jointTorqueDiff = joint6Torque - _targetJointTorqueGripper;
	float angleIncrement = -jointTorqueDiff * _proportionnalIncrement;
	std::cout<< "increment= " << angleIncrement<< std::endl;
	float targetJoint6Position = joint6Position + angleIncrement;
	std::cout << "joint torque diff= " <<abs(jointTorqueDiff) << std::endl;
	
	while (abs(angleIncrement) > _threshold)
	{
		
		//read current joint 6 torque
		std::vector<uint16_t> l_vCurrentJointTorque;
		_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
		
		//read current joint 6 position
		std::vector<uint16_t> l_vCurrentJointPosition;
		_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
		int joint6Position = l_vCurrentJointPosition[5];
		
		// display current joint 6 torque
		std::cout << "vCurrentJoint6Torque= ";
		std::cout << l_vCurrentJointTorque[5] << std::endl;
		int joint6Torque = l_vCurrentJointTorque[5];
		
		int jointTorqueDiff = joint6Torque - _targetJointTorqueGripper;
		if (jointTorqueDiff < 0)
		{
			float angleIncrement = -jointTorqueDiff * _proportionnalIncrement;
			}
		else{
			float angleIncrement = 0;
		}
		std::cout<< "increment= " << angleIncrement<< std::endl;
		float targetJoint6Position = joint6Position + angleIncrement;
		std::cout << "joint torque diff= " <<jointTorqueDiff << std::endl;
		
		//set the position of each servomotor to 0
		std::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(90.0f));
		//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(90.0f));
		//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-90.0f));
		//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(-90.0f));
		//l_vTargetJointPosition.push_back(convertAnglesToJointCmd(45.0f));
		l_vTargetJointPosition.push_back(targetJoint6Position);
		_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
		std::cout << convertJointCmdToAngles(targetJoint6Position) << std::endl;
	}
		
	return (int)joint6Torque;
}

void playNote(int myNote)
{
	//goToPlayingPosition();
	
	std::vector<uint16_t> l_vTargetJointPosition;
	
	if (myNote >= 0 && myNote <= 9)
	{
		l_vTargetJointPosition.push_back(notes_position[myNote][0]);
		l_vTargetJointPosition.push_back(notes_position[myNote][1]);
		l_vTargetJointPosition.push_back(notes_position[myNote][2] + 200);
		l_vTargetJointPosition.push_back(notes_position[myNote][3]);
		l_vTargetJointPosition.push_back(notes_position[myNote][4]);
		l_vTargetJointPosition.push_back(playingPosition[5]); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
		
		_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
		
		std::this_thread::sleep_for(std::chrono::milliseconds(75));
		
		std::vector<uint16_t> l_vTargetJoint1Position;
		
		l_vTargetJoint1Position.push_back(notes_position[myNote][0]);
		l_vTargetJoint1Position.push_back(notes_position[myNote][1]);
		l_vTargetJoint1Position.push_back(notes_position[myNote][2]);
		l_vTargetJoint1Position.push_back(notes_position[myNote][3]);
		l_vTargetJoint1Position.push_back(notes_position[myNote][4]);
		l_vTargetJoint1Position.push_back(playingPosition[5]); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
		
		_oDxlHandler.sendTargetJointPosition(l_vTargetJoint1Position);
		
		std::this_thread::sleep_for(std::chrono::milliseconds(waitTime)); //time period in milliseconds
		
		std::vector<uint16_t> l_vTargetJoint2Position;
		
		l_vTargetJoint2Position.push_back(notes_position[myNote][0]);
		l_vTargetJoint2Position.push_back(notes_position[myNote][1]);
		l_vTargetJoint2Position.push_back(notes_position[myNote][2] + 200);
		l_vTargetJoint2Position.push_back(notes_position[myNote][3]);
		l_vTargetJoint2Position.push_back(notes_position[myNote][4]);
		l_vTargetJoint2Position.push_back(playingPosition[5]); //mettre pince à convertAnglesToJointCmd(38.0f) (à revérifier si ça ne change pas avec un poppy différent)
		
		_oDxlHandler.sendTargetJointPosition(l_vTargetJoint2Position);
	}
	else
	{
		goToPlayingPosition();
		
		std::this_thread::sleep_for(std::chrono::milliseconds(waitTime * 2)); //time period in milliseconds
	}
	/*switch(myNote){
		case 0:
			l_vTargetJointPosition.push_back(notes_position[0][0]);
			l_vTargetJointPosition.push_back(notes_position[0][1]);
			l_vTargetJointPosition.push_back(notes_position[0][2]);
			l_vTargetJointPosition.push_back(notes_position[0][3]);
			l_vTargetJointPosition.push_back(notes_position[0][4]);
			break;
		case 1:
			l_vTargetJointPosition.push_back(notes_position[1][0]);
			l_vTargetJointPosition.push_back(notes_position[1][1]);
			l_vTargetJointPosition.push_back(notes_position[1][2]);
			l_vTargetJointPosition.push_back(notes_position[1][3]);
			l_vTargetJointPosition.push_back(notes_position[1][4]);
			break;
		case 2:
			l_vTargetJointPosition.push_back(notes_position[2][0]);
			l_vTargetJointPosition.push_back(notes_position[2][1]);
			l_vTargetJointPosition.push_back(notes_position[2][2]);
			l_vTargetJointPosition.push_back(notes_position[2][3]);
			l_vTargetJointPosition.push_back(notes_position[2][4]);
			break;
		case 3:
			l_vTargetJointPosition.push_back(notes_position[3][0]);
			l_vTargetJointPosition.push_back(notes_position[3][1]);
			l_vTargetJointPosition.push_back(notes_position[3][2]);
			l_vTargetJointPosition.push_back(notes_position[3][3]);
			l_vTargetJointPosition.push_back(notes_position[3][4]);
			break;
		case 4:
			l_vTargetJointPosition.push_back(notes_position[4][0]);
			l_vTargetJointPosition.push_back(notes_position[4][1]);
			l_vTargetJointPosition.push_back(notes_position[4][2]);
			l_vTargetJointPosition.push_back(notes_position[4][3]);
			l_vTargetJointPosition.push_back(notes_position[4][4]);
			break;
		case 5:
			l_vTargetJointPosition.push_back(notes_position[5][0]);
			l_vTargetJointPosition.push_back(notes_position[5][1]);
			l_vTargetJointPosition.push_back(notes_position[5][2]);
			l_vTargetJointPosition.push_back(notes_position[5][3]);
			l_vTargetJointPosition.push_back(notes_position[5][4]);
			break;
		case 6:
			l_vTargetJointPosition.push_back(notes_position[6][0]);
			l_vTargetJointPosition.push_back(notes_position[6][1]);
			l_vTargetJointPosition.push_back(notes_position[6][2]);
			l_vTargetJointPosition.push_back(notes_position[6][3]);
			l_vTargetJointPosition.push_back(notes_position[6][4]);
			break;
		case 7:
			l_vTargetJointPosition.push_back(notes_position[7][0]);
			l_vTargetJointPosition.push_back(notes_position[7][1]);
			l_vTargetJointPosition.push_back(notes_position[7][2]);
			l_vTargetJointPosition.push_back(notes_position[7][3]);
			l_vTargetJointPosition.push_back(notes_position[7][4]);
			break;
		case 8:
			l_vTargetJointPosition.push_back(notes_position[8][0]);
			l_vTargetJointPosition.push_back(notes_position[8][1]);
			l_vTargetJointPosition.push_back(notes_position[8][2]);
			l_vTargetJointPosition.push_back(notes_position[8][3]);
			l_vTargetJointPosition.push_back(notes_position[8][4]);
			break;
		case 9:
			l_vTargetJointPosition.push_back(notes_position[9][0]);
			l_vTargetJointPosition.push_back(notes_position[9][1]);
			l_vTargetJointPosition.push_back(notes_position[9][2]);
			l_vTargetJointPosition.push_back(notes_position[9][3]);
			l_vTargetJointPosition.push_back(notes_position[9][4]);
			break;
		default:
			l_vTargetJointPosition.push_back(playingPosition[0]);
			l_vTargetJointPosition.push_back(playingPosition[1]);
			l_vTargetJointPosition.push_back(playingPosition[2]);
			l_vTargetJointPosition.push_back(playingPosition[3]);
			l_vTargetJointPosition.push_back(playingPosition[4]);
			break;
	}*/
}

void playMultipleNotes(std::string myMusic)
{
	for (char& c : myMusic)
	{
		std::cout<<c<<std::endl;
		playNote((c - '0'));
	}
	goToPlayingPosition();
}

void changeWaitTime()
{
	int myTime = 0;
	
	std::cout << std::endl;
	std::cout << "write the wait Time between each note in milliseconds" << std::endl;
	
	std::cin >> myTime;
	waitTime = myTime;
}

void glissendo()
{
	//good idea, but not working so much, might need to make a linear displacement instead of a joint one (but too much work for this project)
	std::string upOrDown = "u";
	
	std::cout << std::endl;
	std::cout << "Do you want a glissendo upwards or downwards ? u/d" << std::endl;
	std::cin >> upOrDown;
	
	if (upOrDown == "u" || upOrDown == "U")
	{
		std::vector<uint16_t> Target1JointPosition;
		Target1JointPosition.push_back(notes_position[0][0]);
		Target1JointPosition.push_back(notes_position[0][1]);
		Target1JointPosition.push_back(notes_position[0][2]);
		Target1JointPosition.push_back(notes_position[0][3]);
		Target1JointPosition.push_back(notes_position[0][4]);
		Target1JointPosition.push_back(playingPosition[5]); 
		
		_oDxlHandler.sendTargetJointPosition(Target1JointPosition);
		std::this_thread::sleep_for(std::chrono::milliseconds(150));
		
		std::vector<uint16_t> Target2JointPosition;
		Target2JointPosition.push_back(notes_position[2][0]);
		Target2JointPosition.push_back(notes_position[2][1]);
		Target2JointPosition.push_back(notes_position[2][2]);
		Target2JointPosition.push_back(notes_position[2][3]);
		Target2JointPosition.push_back(notes_position[2][4]);
		Target2JointPosition.push_back(playingPosition[5]); 
		
		_oDxlHandler.sendTargetJointPosition(Target2JointPosition);
		
		std::vector<uint16_t> Target3JointPosition;
		Target3JointPosition.push_back(notes_position[5][0]);
		Target3JointPosition.push_back(notes_position[5][1]);
		Target3JointPosition.push_back(notes_position[5][2]);
		Target3JointPosition.push_back(notes_position[5][3]);
		Target3JointPosition.push_back(notes_position[5][4]);
		Target3JointPosition.push_back(playingPosition[5]); 
		
		_oDxlHandler.sendTargetJointPosition(Target3JointPosition);
	}
	else if (upOrDown == "d" || upOrDown == "D")
	{
		std::vector<uint16_t> Target1JointPosition;
		Target1JointPosition.push_back(notes_position[5][0]);
		Target1JointPosition.push_back(notes_position[5][1]);
		Target1JointPosition.push_back(notes_position[5][2]);
		Target1JointPosition.push_back(notes_position[5][3]);
		Target1JointPosition.push_back(notes_position[5][4]);
		Target1JointPosition.push_back(playingPosition[5]); 
		
		_oDxlHandler.sendTargetJointPosition(Target1JointPosition);
		std::this_thread::sleep_for(std::chrono::milliseconds(150));
		
		std::vector<uint16_t> Target2JointPosition;
		Target2JointPosition.push_back(notes_position[2][0]);
		Target2JointPosition.push_back(notes_position[2][1]);
		Target2JointPosition.push_back(notes_position[2][2]);
		Target2JointPosition.push_back(notes_position[2][3]);
		Target2JointPosition.push_back(notes_position[2][4]);
		Target2JointPosition.push_back(playingPosition[5]); 
		
		_oDxlHandler.sendTargetJointPosition(Target2JointPosition);
		
		std::vector<uint16_t> Target3JointPosition;
		Target3JointPosition.push_back(notes_position[0][0]);
		Target3JointPosition.push_back(notes_position[0][1]);
		Target3JointPosition.push_back(notes_position[0][2]);
		Target3JointPosition.push_back(notes_position[0][3]);
		Target3JointPosition.push_back(notes_position[0][4]);
		Target3JointPosition.push_back(playingPosition[5]); 
		
		_oDxlHandler.sendTargetJointPosition(Target3JointPosition);
	}
	else
	{
		std::cout << "write only u for upwards, or d for downards" << std::endl;
	}
}

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;
	
	std::cout<<"===Initalization of the position==="<<std::endl; 
	std::array<short unsigned int, 6> myPosition = currentPos(); //check position, to add a test later
		
	goToPlayingPosition();
		
	std::cout<<"===Going to playing position==="<<std::endl;
	currentPos();
	std::cout<<"===Position: OK==="<<std::endl;
	
	//int gripperTorque = closeGripper();
	
	goToPlayingPosition();
	currentPos();
	teachTarget();
	
	std::string myMusic = "";
	std::cout << std::endl;
	std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
	std::cout<<"Write 'stop' to stop"<<std::endl;
	std::cin >> myMusic;
	while (myMusic != "stop")
	{
			if (myMusic == "home")
			{
				goToHomePosition();
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
			}
			else if (myMusic == "start")
			{
				goToPlayingPosition();
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
			}
			else if (myMusic == "teach")
			{
				teachTarget();
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
			}
			else if (myMusic == "wait")
			{
				changeWaitTime();
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
			}
			else if (myMusic == "glissendo")
			{
				glissendo();
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
			}
			else if (myMusic == "demo")
			{
				playMultipleNotes("0a4a4a32321a1a012a123a202a1");
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
				
			}
			else
			{
				playMultipleNotes(myMusic);
				std::cout << std::endl;
				std::cout<<"Write the notes you want to play (from 0 to 8), without spaces between them"<<std::endl;
				std::cout<<"Write 'stop' to stop"<<std::endl;
				std::cin >> myMusic;
			}
	}
	goToPlayingPosition();
	
	// wait 1s
	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	
	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
	_oDxlHandler.enableTorque(false);
	_oDxlHandler.closePort();
	std::cout << std::endl;
	
	return 0;
}