/*
	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 Calculate_Spline(PrVector3 xgoal);

static float tmp[6];

// 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");
//	printf("(1) Trapezoid Commutation \n");
	printf("(2) Sinusoid Commutation \n");
	printf("(3) Initialization with Index \n");
	printf("(4) Test HET \n");
	printf("(5) Test Encoders \n");
	printf("(6) Test Position Loop on EL1 \n");

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

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

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

		switch(mesg[0])
		{
			case 'h':
			if(strncmp(mesg,"home",4)==0)
			{
				tmp[0] = 90.0; tmp[1] = 10.0; tmp[2] = 0.0; tmp[3] = 0.0;

				printf("Going to HOME position \n");
				printf("You entered goal: %f,%f,%f,%f\n", tmp[0],tmp[1],
					tmp[2],tmp[3]);
			
				printf("Enter 'jgoto start' to start motion \n");
			}
			break;

			case 'r':
			if(strncmp(mesg,"ready",5)==0)
			{
				tmp[0] = 60.0; tmp[1] = 10.0; tmp[2] = 0.0; tmp[3] = -95.0;

				printf("Going to READY position \n");
				printf("You entered goal: %f,%f,%f,%f\n", tmp[0],tmp[1],
					tmp[2],tmp[3]);
			
				printf("Enter 'jgoto start' to start motion \n");
			}
			break;
			case 'f':
		
			if(strncmp(mesg,"force start",11)==0)
			{
				printf("Force Control Test Start \n");
				rt->user_mode = OS_FORCE;
	
				// HACK!!!!!	
				// TEST, OPERATIONAL POINT AT END EFFECTOR TIP
				rt->compliance_direction = 0;
				for(i=0;i<3;i++)
					rt->xgoal[i] = rt->xee[i];	
				// HACK!!!!!	
			}
			else if(strncmp(mesg,"force",5)==0)
			{
				sscanf(mesg+6,"%f", &tmp[0]);
				rt->desiredForce[2] = tmp[0];	
				// currently do force control in z-axis
		
				printf("Force Control Test \n");
				printf("Desired force: %4.1f \n", rt->desiredForce[2]);
			}

			// USAGE: enter 'float' to enter gravity compensation mode
			else if(strncmp(mesg,"float",5)==0)
			{
				printf("Entering float mode \n");
				rt->user_mode = GRAVITY_TEST;
			}	

			break;	// break for case 'f'
		
			case'j':
			{

			// USAGE: enter 'jgoto start'
			// After entering the 'jgoto # # # #' command below, enter 'jgoto start'
			// to start jgoto motion
			if(strncmp(mesg,"jgoto start",11)==0)
			{
				printf("Joint Space Trajectory Start! \n");	
				for(i=0;i<4;i++)
					rt->jgoal[i] = tmp[i]/57.3;
				
				rt->jgoal[4] = rt->q[i];
				printf("You entered goal: %f,%f,%f,%f\n", rt->jgoal[0],rt->jgoal[1],
					rt->jgoal[2],rt->jgoal[3]);

				rt->user_mode = JS_TEST;
			}

			// USAGE: enter 'jgoto J0 J1 J2 J3'
			// where J0,J1,J2,J3 are the DESIRED joint angles of SH1,SH2,SH3, and EL1
			// in degrees. THIS IS NOT A DISPLACEMENT COMMAND!!! 
			// Example: 'jgoto 90.0 10.0 0.0 -20.0' will command position of
			// 90, 10, 0, -20 degrees to joint SH1, SH2, SH3 and EL1 respectively.
			// If accidentally entering incorrect values, just redo the 'jgoto' command
			// with the correct numbers.
			
			else if(strncmp(mesg,"jgoto",5)==0)
			{
				sscanf(mesg+5,"%f %f %f %f", &tmp[0],&tmp[1],&tmp[2],&tmp[3]);
				printf("Joint Space Test \n");	
				printf("You entered goal: %f,%f,%f,%f\n", tmp[0],tmp[1],
					tmp[2],tmp[3]);
			
				printf("Enter 'jgoto start' to start motion \n");
			}

			// unused mode; Was used to test constant torque command to joint
			// for modelling friction 
			if(strncmp(mesg,"jtest",5)==0)
			{
				for(i=0;i<5;i++)
					tmp[i] = 0.0;
				printf("SH1 Test \n");	
			//	sscanf(mesg+5,"%f %f", &tmp[0],&tmp[1]);
				sscanf(mesg+5,"%f", &tmp[1]);
				printf("time %f \n", rt->time);
				for(i=0;i<5;i++)
					rt->qinc[i] = tmp[i]/57.3;
				
				rt->user_mode = FRICTION_TEST;
			}

			// USAGE: enter 'josc joint_number freq magnitude'
			// where 'joint_number' is joint number from 0 to 3
			// 'freq' is the oscillation frequency in HZ.
			// 'magnitude' is the oscillation magnitude in degrees.

			// Example: enter 'josc 0 1.0 10.0' to oscillate joint SH1
			// at 10 degrees amplitude and 1 HZ frequency. MAKE SURE
			// JOINT HAS ENOUGH CLEARANCE TO OSCILLATE. BEST DO THIS 
			// FROM THE MIDDLE OF THE JOINT WORKSPACE.
			if(strncmp(mesg,"josc",4)==0)
			{
				int jnumber;
				float freq, joffset;

				sscanf(mesg+4,"%d %f %f", &jnumber, &freq, &joffset);
				rt->jnumber = jnumber;
				rt->freq = freq*2*3.14;
				rt->joffset = joffset/57.3;
				printf("Joint Space Sinusoid Test, J:%d, %f Hz %f deg \n", 
					rt->jnumber, (rt->freq)/(2*3.14), joffset);
				rt->c0 = rt->q[jnumber];
				rt->c1 = (rt->freq);
				rt->c2 = (rt->freq)*(rt->c1);
				printf("Parameters (c0/c1/c2): %f, %f, %f \n",
					rt->c0, rt->c1, rt->c2);
				rt->xtime = rt->time;
				rt->user_mode = JS_OSC;
			}
			break;
			}
			case'e':
			{
			// USAGE: enter 'exit' to exit program.
			// This will stop the control program.
			if(strncmp(mesg,"exit",4)==0)
			{	
				rt->user_exit = 1;
				sm_functions->Write_To_SM(rt);
				printf("user entered exit: %d, %d \n",rt->user_exit,SM->user_exit); 
				printf("Exiting program \n");	
				pthread_mutex_destroy(&(SM->mutex));	
				exit(1);
			}
			break;
			}
			case't':
			{
			// currently an unused mode	
			if(strncmp(mesg,"trajectory",10)==0)
			{
				printf("Entering trajectory test mode \n");
		//		rt->user_mode = TRAJECTORY_TEST;
			}	
			
			break;
			}
			case'x':
			{
			
			// USAGE: 'xgoto start'
			// After entering 'xgoto # # #' below, do this command to start
			// cartesian motion
			if(strncmp(mesg,"xgoto start",11)==0)
			{
		//		for(i=0;i<3;i++)
		//			rt->xgoal[i] = rt->x[i] + tmp[i];	
				// TEST, USING EE-TIP AS CONTROL POINT	
				for(i=0;i<3;i++)
					rt->xgoal[i] = rt->xee[i] + tmp[i];	
				printf("Operational Space Trajectory Start \n");
				printf("You entered goal: %f,%f,%f\n", rt->xgoal[0],
					rt->xgoal[1],rt->xgoal[2]);
//				printf("Current positon:  %f,%f,%f\n", rt->x[0],
//					rt->x[1],rt->x[2]);
				printf("Current positon:  %f,%f,%f\n", rt->xee[0],
					rt->xee[1],rt->xee[2]);
				rt->user_mode = OS_TEST;
				rt->xtime = rt->time;
				printf("time: %f \n", rt->xtime);
			}
			
			// USAGE: 'xgoto x y z'
			// where 'x' is the x-axis coordinates displacement from current position
			// 'y' is the y-axis displacement
			// 'z' is the z-axis displacement
			// Enter values in meter.

			// Example:
			// Enter: 'xgoto 0.05 0 -0.05'
			// and the trajectory goal is 5 cm in the positive x direction and 
			// 5 cm in the negative z direction from the current xyz location.
			// To start motion after doing xgoto, enter 'xgoto start'.

			// If accidentally entering incorrect values, just redo the 'xgoto' command
			// with the correct numbers.
			else if(strncmp(mesg,"xgoto",5)==0)
			{
				PrVector3 xgoal;
				for(i=0;i<5;i++)
					tmp[i] = 0.0;
				sscanf(mesg+5,"%f %f %f", &tmp[0],&tmp[1],&tmp[2]);
				printf("Operational Space Test \n");	
				sm_functions->Read_SM(rt);
		//		printf("Current xyz: %f, %f, %f \n", rt->x[0],rt->x[1],
		//			rt->x[2]);
		//		for(i=0;i<3;i++)
		//			xgoal[i] = rt->x[i] + tmp[i];

				printf("Current xyz: %f, %f, %f \n", rt->xee[0],rt->xee[1],
					rt->xee[2]);
		
				for(i=0;i<3;i++)
					xgoal[i] = rt->xee[i] + tmp[i];

				printf("Goal xyz   : %f, %f, %f \n", xgoal[0],xgoal[1],xgoal[2]);
				printf("Type 'xgoto start' to start motion \n");
				Calculate_Spline(xgoal);

			}

			// USAGE: 'xhold #'
			// where # is the compliance direction: 
			// 0 - x axis
			// 1 - y axis
			// 2 - z axis
			// for example, enter 'xhold 0' to do x-axis compliance
			if(strncmp(mesg,"xhold",5)==0)
			{
				int compliance_direction;
				sscanf(mesg+5,"%d", &compliance_direction);
				if((compliance_direction < 0) || (compliance_direction > 2))
					compliance_direction = 1;
				// if user entered an invalid compliance direction (<0 or >2)
				// then the default direction is y-axis

				rt->compliance_direction = compliance_direction;

				// OLD ONE, OPERATIONAL POINT AT WRIST JOINT
//				for(i=0;i<3;i++)
//					rt->xgoal[i] = rt->x[i];	
				// TEST, OPERATIONAL POINT AT END EFFECTOR TIP
				for(i=0;i<3;i++)
					rt->xgoal[i] = rt->xee[i];	

				for(i=0;i<5;i++)
					rt->jgoal[i] = rt->q[i];	

				printf("Operational Space Test \n");	
				printf("Entering xhold mode %d \n", compliance_direction);
		//		printf("SH2 will be locked at %f degrees \n", 
		//			rt->jgoal[1]*57.3);
				rt->user_mode = OS_HOLD;

			}

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

}

// max cartesian velocities in m/s
static float dx_max[3]	= {0.06, 0.06, 0.06};

// Used to calculate cubic spline trajectories for cartesian motion ('xgoto')
void Calculate_Spline(PrVector3 xgoal)
{
	int i;
	float tf[3];
	rt->tf = 0.0;
	// calculate motion time and use the max calculated as the motion time
	for(i=0;i<3;i++)
	{
	//	tf[i] = abs((3.0*(xgoal[i] - rt->x[i]))/(dx_max[i]*2.0));
		tf[i] = abs((3.0*(xgoal[i] - rt->xee[i]))/(dx_max[i]*2.0));
//		printf("tf[%d]: %f, %f, %f \n", i, tf[i], xgoal[i], rt->x[i]);
		if(tf[i]>rt->tf)
			rt->tf = tf[i];		
	}
	printf("Motion to be achieved in: %f, parameters(a0/a2/a3):  \n", rt->tf);
	// get cubic spline parameters
	for(i=0;i<3;i++)
	{
//		rt->a0[i] = rt->x[i];
//		rt->a2[i] = 3.0*(xgoal[i] - rt->x[i])/(rt->tf*rt->tf);
//		rt->a3[i] = -2.0*(xgoal[i]- rt->x[i])/(rt->tf*rt->tf*rt->tf) ;
		rt->a0[i] = rt->xee[i];
		rt->a2[i] = 3.0*(xgoal[i] - rt->xee[i])/(rt->tf*rt->tf);
		rt->a3[i] = -2.0*(xgoal[i]- rt->xee[i])/(rt->tf*rt->tf*rt->tf) ;
//		printf("axis[%d]: goal:%f, x: %f ", i, xgoal[i], rt->x[i]);
//		printf("%f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
	}
}
