/*
	Peter Thaulad
	04/05/2004

	user.cpp
	***********

	Process that takes user input and set user modes and goal positions accordingly. Spawned by
	control.cpp and use shared memory.	

*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <termios.h>
#include <math.h>
#include <sys/sched.h>
#include <sys/types.h>
#include <pthread.h>
#include <string.h>

#include "libpuma.h"
#include "matrix/PrMatrix.h"
#include "matrix/PrVector.h"
#include "matrix/PrVector6.h"
#include "matrix/PrVector3.h"
#include "matrix/PrTransform.h"
#include "driver/PrTimer.h"
#include "SharedMemory.h"
#include "Process.h"
#include "Definitions.h"

#include <unistd.h>
#include <sys/sched.h>

// CLASS OBJECTS DECLARATIONS
sharedMemory *SM = new sharedMemory;		// global shared memory object
sharedMemory *rt;		// local shared memory object
Process *process = new Process();
SM_Functions *sm_functions = new SM_Functions();

// FUNCTION PROTOTYPES
void calcSpline(float qgoal[]);
void calcSplineVel(float qgoal[]);

static float tmp[6];

float qMax[3] = {74.0, 15.0, 120.0};
float qMin[3] = {-74.0, -120.0, 10.0}; 


// MAIN
void main(void)
{
	int i, exit_flag=0, sm_address;
	pthread_mutexattr_t attr;
	FILE *memaddress;

	pid_t pid = getpid();
	setprio(pid,15);

	// init global shared memory
	printf("Opening User Data Shared Memory \n");
	sm_functions->Open_Shared_Memory();
	memaddress = fopen("smaddress.txt","r");
	fscanf(memaddress,"%d", &sm_address);

	sm_functions->Map_Shared_Memory(sm_address);

	// init shared memory mutex protection
	pthread_mutexattr_init(&attr);
	pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
	pthread_mutex_init(&(SM->mutex), &attr);	

	// init local shared memory
	rt = sm_functions->Init_Local_SM(rt);

	printf("\n\nUser thread starts \n \n");

	printf("SEA-3DOF BLDC Test Program \n");
	printf("Enter command: \n");
	// current existing modes:
	// Encoders init process
	// Joint servo control

	while(!tcischars(1))
	{
		// Poke control process to see if it's still alive
		if(kill(rt->pid,0) == -1)
		{
			printf("\nfrom user.cpp: control process died. Exiting... \n \n");
			pthread_mutex_destroy(&(SM->mutex));	
			exit(1);
		}


		printf("Enter command: > \n");
		char mesg[100];
		memset(mesg,NULL,100);
		read(1,mesg,sizeof(mesg));
		printf("You entered: %s\n",mesg);

		pthread_mutex_lock(&(SM->mutex));
		sm_functions->Read_SM(rt);
		pthread_mutex_unlock(&(SM->mutex));

		switch(mesg[0])
		{
				case 't':
				{
					if(strncmp(mesg,"test max",8) == 0)
					{
						rt->userMode = TEST_MAX;
						printf("Testing custom MAX547 Driver \n");

					}
					break;
				}
				case 'd':
				{
					if(strncmp(mesg,"display on",10) == 0)
					{
						rt->userMode = DISPLAY_INFO;
						printf("Displaying raw spring torque data \n");

					}
					
					if(strncmp(mesg,"display off",10) == 0)
					{
						rt->userMode = IDLE;
						printf("Turning off display \n");

					}
					break;
				}
				case'e':
				{
					// USAGE: enter 'exit' to exit program.
					// This will stop the control program.
					if(strncmp(mesg,"exit",4)==0)
					{	
						rt->userExit = 1;
						sm_functions->Write_To_SM(rt);
						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
						printf("Exiting program \n");	
						pthread_mutex_destroy(&(SM->mutex));	
						exit(1);
					}

					if(strncmp(mesg,"encoder",7)==0)
					{
							rt->userMode = ENCODER_TEST;
							printf("Entering encoder test function \n");
					}

					break;
				}	// end of case 'e'

				case 'f':
				{

					if(strncmp(mesg,"float",5) == 0)
					{
						if(rt->userMode != FLOAT)
						{
							rt->xtime = rt->time;
							printf("Float mode start time: %f \n", rt->xtime);
						}
						rt->userMode = FLOAT;
						printf("Apply closed-loop torque control \n");
					}
					
					break;
				}
				case 'i':
				{
					if(strncmp(mesg,"init pos",8) == 0)
					{
						rt->userMode = INIT_POSITION;
						printf("Running encoders initialization sequence \n");

					}
					break;
				}

				// goal is entered in the following order:
				// #, #, #: [WA1], [SH1], [EL1]
				case 'j':
				{
					if(strncmp(mesg,"jpos",4) == 0)
					{
						printf("Current pos: %6.3f, %6.3f, %6.3f\n",
									rt->q[0], rt->q[1], rt->q[2]); 
					}
					else if(strncmp(mesg,"jgoto start",11) == 0)
					{
						rt->userMode = JOINT_SERVO;
						printf("Joint servo test start \n");
						// only update goal positions when typing "jgoto start"

						rt->qgoal[0] = tmp[0];
						rt->qgoal[1] = tmp[1];
						rt->qgoal[2] = tmp[2];
						printf("Current pos: %6.3f, %6.3f, %6.3f Goal pos: %6.3f, %6.3f, %6.3f \n",
									rt->q[0], rt->q[1], rt->q[2], 
									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
						rt->xtime = rt->time;
						printf("Time: %f \n", rt->xtime);
					}
					else if (strncmp(mesg,"jgoto",5) == 0)
					{
						sscanf(mesg+5,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);

						// CHECK IF GOAL POS ARE WITHIN JOINT LIMITS
						int i;
						for(i=0;i<3;i++)
						{
							if(tmp[i] > qMax[i])
								tmp[i] = qMax[i];
							else if(tmp[i] < qMin[i])
								tmp[i] = qMin[i];
						}
						printf("Current pos: %6.3f, %6.3f, %6.3f\n",
									rt->q[0], rt->q[1], rt->q[2]);
						printf("Goal pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);

				//		calcSpline(rt->qgoal);
						calcSpline(tmp);
						printf("Type 'jgoto start' to start motion \n");
					}
					break;
				}
		
				// goal is entered in the following order:
				// #, #, #: [WA1], [SH1], [EL1]
				// Make sure there's enough space for the robot to move
				case 'v':
				{
					if(strncmp(mesg,"velgoto start",13) == 0)
					{
						rt->userMode = VEL_SERVO;
						printf("Vel servo test start \n");
						// only update goal positions when typing "jgoto start"
						rt->qgoal[0] = rt->q[0];
						rt->qgoal[1] = tmp[1];
						rt->qgoal[2] = rt->q[2];
						printf("Current pos: %6.3f, %6.3f, %6.3f Goal pos: %6.3f, %6.3f, %6.3f \n",
									rt->q[0], rt->q[1], rt->q[2], 
									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
						rt->xtime = rt->time;
						printf("Time: %f \n", rt->xtime);
					}
					else if (strncmp(mesg,"velgoto",7) == 0)
					{
						sscanf(mesg+7,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);

						printf("Current pos: %6.3f, %6.3f, %6.3f\n",
									rt->q[0], rt->q[1], rt->q[2]);
						printf("Goal pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);

				//		calcSplineVel(rt->qgoal);
						calcSplineVel(tmp);
						printf("Type 'vgoto start' to start motion \n");
					}
					break;
				}
				case 'r':
				{
					if (strncmp(mesg,"ready one",9) == 0)
					{
						tmp[0] = 0.0;
						tmp[1] = 0.0;
						tmp[2] = 90.0;	
						printf("Goal set at position one \n");
						printf("Current pos: %6.3f, %6.3f, %6.3f\n",
									rt->q[0], rt->q[1], rt->q[2]);
						printf("Goal pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);
						calcSpline(tmp);
						printf("Type 'jgoto start' to start motion \n");
					}
					else if (strncmp(mesg,"ready two",9) == 0)
					{
						tmp[0] = 0.0;
						tmp[1] = -50.0;
						tmp[2] = 40.0;	
						printf("Goal set at position two \n");
						printf("Current pos: %6.3f, %6.3f, %6.3f\n",
									rt->q[0], rt->q[1], rt->q[2]);
						printf("Goal pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);
						calcSpline(tmp);
						printf("Type 'jgoto start' to start motion \n");
					}
					else if (strncmp(mesg,"ready home",10) == 0)
					{
						tmp[0] = -70.0;
						tmp[1] = -00.0;
						tmp[2] = 20.0;	
						printf("Goal set at home position \n");
						printf("Current pos: %6.3f, %6.3f, %6.3f\n",
									rt->q[0], rt->q[1], rt->q[2]);
						printf("Goal pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);
						calcSpline(tmp);
						printf("Type 'jgoto start' to start motion \n");
					}
					break;
				}
				case 'm':
				{
					if(strncmp(mesg,"mini on",7) == 0)
					{
						rt->miniMode = TRUE;
						printf("mini motor activated\n");
					}
					else if (strncmp(mesg,"mini off",8) == 0)
					{
						rt->miniMode = FALSE;
						printf("mini motor deactivated\n");
					}
					break;
				}
				case 's':
				{

					break;
				}	// end of case 's'


		}	// end of switch statement	

		pthread_mutex_lock(&(SM->mutex));
		sm_functions->Write_To_SM(rt);
		pthread_mutex_unlock(&(SM->mutex));
	}	// end of while(!tcischars(1))
	
	pthread_mutex_destroy(&(SM->mutex));	

}

// Used to calculate cubic spline trajectories for cartesian motion ('xgoto')
void calcSpline(float qgoal[])
{
	int i;
	float tf[3];
	float dqmax = 20.0;
	// calculate motion time and use the max calculated as the motion time
	for(i=0;i<3;i++)
	{
		rt->tf[i] = 0.0;
		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
		if(tf[i]> rt->tf[i])
			rt->tf[i]= tf[i];		
	}
	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);

	// get cubic spline parameters
	for(i=0;i<3;i++)
	{
		if(rt->tf[i] > 0.0)
		{
			rt->a0[i] = rt->q[i];
			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
		}
		else
		{
			rt->a0[i] = 0.0;
			rt->a2[i] = 0.0;
			rt->a3[i] = 0.0;
		}
		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
	}
}
// Used to calculate cubic spline trajectories for cartesian motion ('xgoto')
void calcSplineVel(float qgoal[])
{
	int i;
	float tf[3];
	float dqmax[3] = {0.0, 95.0, 10.0};
	// calculate motion time and use the max calculated as the motion time
	for(i=0;i<3;i++)
	{
		rt->tf[i] = 0.0;
		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax[i]*2.0));
		if(tf[i]> rt->tf[i])
			rt->tf[i]= tf[i];		
	}
	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);

	// get cubic spline parameters
	for(i=0;i<3;i++)
	{
		if(rt->tf[i] > 0.0)
		{
			rt->a0[i] = rt->q[i];
			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
		}
		else
		{
			rt->a0[i] = 0.0;
			rt->a2[i] = 0.0;
			rt->a3[i] = 0.0;
		}
		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
	}
}
