// Windows version
#include <windows.h>
#include <stdio.h>
#include <conio.h>
#include <iostream>
#include <math.h>
#include <algorithm>
#include <dynamixel.h>

#pragma comment(lib, "dynamixel.lib")

// Default setting
#define DEFAULT_PORTNUM		4 // COM4
#define DEFAULT_BAUDNUM		1 // 1Mbps


void PrintCommStatus(int CommStatus);
void PrintErrorCode();

void movedyn(int moveto_ID, double moveto_TARGET, double moveto_SPEED)	//moves dynamixel moveto_ID to position moveto_TARGET at speed moveto_SPEED (in radians)
{
	int TARGET = 511 + floor(moveto_TARGET*195.6+0.5);;					//511 is position 0 of dynamixel for the angle computation
	int SPEED = min(1023, floor(moveto_SPEED*85.693+0.5));				//1023 is the dynamixel maximum speed
	dxl_write_word( moveto_ID, 32, SPEED );
	dxl_write_word( moveto_ID, 30, TARGET );
}

void moveleg(int leg_ID[3], double moveleg_TARGET[3], double movespeed_TIME)
{
	int CommStatus;
	double angle_TARGET[3] = { 0, 0, 0};								//corresponding values of the 3 dynamixel
	int dyn_TARGET[3] = { 0, 0, 0};
	int dyn_SPEED[3] = { 511, 511, 511};
	double x = 64.5;													//internal parameter (horizontal distance between the second dynamixel axis and the leg tip)
	int i =0;

	if (moveleg_TARGET[0]>73.5)											//the first dynamixel needs a specific case if the tip goes "behind" the axis
		angle_TARGET[0] = atan2(moveleg_TARGET[1]-36, moveleg_TARGET[0]-73.5);
	else
		angle_TARGET[0] = atan2(36-moveleg_TARGET[1], 73.5-moveleg_TARGET[0]);

	x = ( moveleg_TARGET[0]-73.5)/ cos( angle_TARGET[0])-44;
	angle_TARGET[2] = asin(( x*x+moveleg_TARGET[2]*moveleg_TARGET[2]-12902)/12061);
	angle_TARGET[1] = -atan2(moveleg_TARGET[2], x)+ atan2( -93.5*cos (angle_TARGET[2]), 64.5+93.5*sin (angle_TARGET[2]));
	//printf( "%f   %f   %f   %f\n", x, angle[0], angle[1], angle[2]);

	dyn_TARGET[0] = 511 + floor(angle_TARGET[0]*195.6 + 0.5);
	dyn_TARGET[1] = 441 + floor(angle_TARGET[1]*195.6 + 0.5);			//offsets are the angles between the kinematics lines and the dynamixel 0 positions
	dyn_TARGET[2] = 419 + floor(angle_TARGET[2]*195.6 + 0.5);

	if (movespeed_TIME>0)
		for( i=0; i<3; i++ )
			{
				dyn_SPEED[i] = min ( 1023, floor(0.439 * abs(dyn_TARGET[i] - dxl_read_word( leg_ID[i], 30))/movespeed_TIME + 0.5));
			}
	else																//just in case the leg is set to go back in time
		for( i=0; i<3; i++ )
			{
				dyn_SPEED[i] = 1023;
			}

	dxl_set_txpacket_id(BROADCAST_ID);									//setting up speed
		dxl_set_txpacket_instruction(INST_SYNC_WRITE);
		dxl_set_txpacket_parameter(0, 32);
		dxl_set_txpacket_parameter(1, 2);
		for( i=0; i<3; i++ )
		{
			dxl_set_txpacket_parameter(2+3*i, i+1);
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(dyn_SPEED[i]));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(dyn_SPEED[i]));
		}
		dxl_set_txpacket_length(13);
		dxl_txrx_packet();

	dxl_set_txpacket_id(BROADCAST_ID);									//moving the leg
		dxl_set_txpacket_instruction(INST_SYNC_WRITE);
		dxl_set_txpacket_parameter(0, 30);
		dxl_set_txpacket_parameter(1, 2);
		for( i=0; i<3; i++ )
		{
			dxl_set_txpacket_parameter(2+3*i, leg_ID[i]);
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(dyn_TARGET[i]));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(dyn_TARGET[i]));
		}
		dxl_set_txpacket_length(13);
		dxl_txrx_packet();
}

int main()
{
	int CommStatus;
	int leg_0[3] = { 1, 2, 3};
	double position_0[3] = {188.5, 36, -115};							//spider leg tip position from CM5 center
	char c = 0;

	// Open device
	if( dxl_initialize(DEFAULT_PORTNUM, DEFAULT_BAUDNUM) == 0 )
	{
		printf( "Failed to open USB2Dynamixel!\n" );
		printf( "Press any key to terminate...\n" );
		getch();
		return 0;
	}
	else
		printf( "Succeed to open USB2Dynamixel!\n" );

	while(1)
	{
		printf( "Press any key to continue!(press ESC to quit)\n" );
		c = getch();
		//printf( "%d\n", c);
		if(c == 27)
			break;

		switch (c)
		{
		case 122:
			position_0[0] = position_0[0]+10;
			break;
		case 115:
			position_0[0] = position_0[0]-10;
			break;
		case 113:
			position_0[1] = position_0[1]+10;
			break;
		case 100:
			position_0[1] = position_0[1]-10;
			break;
		case 97:
			position_0[2] = position_0[2]+10;
			break;
		case 101:
			position_0[2] = position_0[2]-10;
			break;

		}
		
		while( dxl_read_byte( leg_0[0], 46 ) + dxl_read_byte( leg_0[1], 46 ) + dxl_read_byte( leg_0[2], 46 ) > 0 )
		{
		}

		moveleg(leg_0, position_0, 0.2);

		CommStatus = dxl_get_result();

		if( CommStatus != COMM_RXSUCCESS )
		{
			PrintCommStatus(CommStatus);
			break;
		}

	}

	// Close device
	dxl_terminate();
	printf( "Press any key to terminate...\n" );
	getch();
	return 0;
}

// Print communication result
void PrintCommStatus(int CommStatus)
{
	switch(CommStatus)
	{
	case COMM_TXFAIL:
		printf("COMM_TXFAIL: Failed transmit instruction packet!\n");
		break;

	case COMM_TXERROR:
		printf("COMM_TXERROR: Incorrect instruction packet!\n");
		break;

	case COMM_RXFAIL:
		printf("COMM_RXFAIL: Failed get status packet from device!\n");
		break;

	case COMM_RXWAITING:
		printf("COMM_RXWAITING: Now recieving status packet!\n");
		break;

	case COMM_RXTIMEOUT:
		printf("COMM_RXTIMEOUT: There is no status packet!\n");
		break;

	case COMM_RXCORRUPT:
		printf("COMM_RXCORRUPT: Incorrect status packet!\n");
		break;

	default:
		printf("This is unknown error code!\n");
		break;
	}
}

// Print error bit of status packet
void PrintErrorCode()
{
	if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
		printf("Input voltage error!\n");

	if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
		printf("Angle limit error!\n");

	if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
		printf("Overheat error!\n");

	if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
		printf("Out of range error!\n");

	if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
		printf("Checksum error!\n");

	if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
		printf("Overload error!\n");

	if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
		printf("Instruction code error!\n");
}