/*
	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 calcXSpline(float qgoal[]);
void calcXSplineVel(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 force",11) == 0)
					{
						if(rt->userMode != FLOAT_FORCE)
						{
							rt->xtime = rt->time;
							printf("Force Float mode start time: %f \n", rt->xtime);
						}
						rt->userMode = FLOAT_FORCE;
						printf("Apply closed-loop torque control plus force control \n");
					}
					else 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");
					}
					else if(strncmp(mesg,"fcontrol",8) == 0)
					{
						rt->userMode = OS_CONTACT_CONTROL;
						printf("Starting contact force 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)
					{
						calcSpline(tmp);
						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 ready",11) == 0)
					{
						tmp[0] = -51.0;
						tmp[1] = -15.0;
						tmp[2] = 72.0;
						
						printf("Going to ready 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");
					}
					else if (strncmp(mesg,"jgoto home",10) == 0)
					{
						tmp[0] = -70.0;
						tmp[1] = 0.0;
						tmp[2] = 12.0;
							
						printf("Going to 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");
					}
					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");
					}
					else if(strncmp(mesg,"josc start",10) == 0)
					{
						rt->c0 = rt->q[rt->jNumber];
						rt->c1 = rt->oscFreq;
						rt->c2 = (rt->c1)*(rt->c1);

						rt->qinit[0] = rt->q[0];
						rt->qinit[1] = rt->q[1];
						rt->qinit[2] = rt->q[2];

						rt->userMode = JOINT_OSC;
						rt->xtime = rt->time;
						printf("Time: %f \n", rt->xtime);
						printf("Joint %d initial q: %7.3f \n", rt->jNumber, rt->c0);
					}
					else if(strncmp(mesg,"josc0",5) == 0)
					{
						sscanf(mesg+5,"%f ", &tmp[0]);

						rt->jNumber = 0;
						rt->oscAmp = 5.0;
						rt->oscFreq = tmp[0]*2.0*PI;
					//	sscanf(mesg+5,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);

					//	rt->jNumber = int(tmp[0]);
					//	rt->oscAmp = tmp[1];
					//	rt->oscFreq = tmp[2]*2.0*PI;

						printf("Joint %d will osc at amplitude of %5.2f deg and freq of %4.3f Hz \n",
									rt->jNumber, rt->oscAmp, rt->oscFreq/(2.0*PI));

					}
					else if(strncmp(mesg,"josc1",5) == 0)
					{
						sscanf(mesg+5,"%f ", &tmp[0]);

						rt->jNumber = 1;
						rt->oscAmp = -5.0;
						rt->oscFreq = tmp[0]*2.0*PI;
					//	sscanf(mesg+4,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);

					//	rt->jNumber = int(tmp[0]);
					//	rt->oscAmp = tmp[1];
					//	rt->oscFreq = tmp[2]*2.0*PI;

						printf("Joint %d will osc at amplitude of %5.2f deg and freq of %4.3f Hz \n",
									rt->jNumber, rt->oscAmp, rt->oscFreq/(2.0*PI));

					}
					else if(strncmp(mesg,"josc2",5) == 0)
					{
						sscanf(mesg+5,"%f ", &tmp[0]);

						rt->jNumber = 2;
						rt->oscAmp = 5.0;
						rt->oscFreq = tmp[0]*2.0*PI;
					//	sscanf(mesg+4,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);

					//	rt->jNumber = int(tmp[0]);
					//	rt->oscAmp = tmp[1];
					//	rt->oscFreq = tmp[2]*2.0*PI;

						printf("Joint %d will osc at amplitude of %5.2f deg and freq of %4.3f Hz \n",
									rt->jNumber, rt->oscAmp, rt->oscFreq/(2.0*PI));

					}			

					break;
				}
				case 'n':
				{
					if(strncmp(mesg,"newfloat",8) == 0)
					{
						rt->userMode = NEWFLOAT;
						
						printf("Entering FLOAT Mode \n\n");

					}
									

					break;
				}
		
				case 'x':
				{
					if(strncmp(mesg,"xpos",4) == 0)
					{
						printf("Current ee pos (x,y,z): %6.3f, %6.3f, %6.3f\n",
									rt->x[0], rt->x[1], rt->x[2]); 
					}
					else if(strncmp(mesg,"xgoto start",11) == 0)
					{
						rt->userMode = OS_SERVO;
						printf("OpSpace servo test start \n");
						// only update goal positions when typing "xgoto start"

						rt->xgoal[0] = tmp[0];
						rt->xgoal[1] = tmp[1];
						rt->xgoal[2] = tmp[2];
						printf("Current ee pos: %6.3f, %6.3f, %6.3f Goal ee pos: %6.3f, %6.3f, %6.3f \n",
									rt->x[0], rt->x[1], rt->x[2], 
									rt->xgoal[0], rt->xgoal[1], rt->xgoal[2]);
						calcXSpline(tmp);
						rt->xtime = rt->time;
						printf("Time: %f \n", rt->xtime);
					}
					else if (strncmp(mesg,"xgoto1",6) == 0)
					{
						tmp[0] = 0.4857;
						tmp[1] = 0.1548;
					//	tmp[2] = -0.3033;
						tmp[2] = -0.2133;
					
						printf("Current ee pos: %6.3f, %6.3f, %6.3f\n",
									rt->x[0], rt->x[1], rt->x[2]);
						printf("Goal ee pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);

						calcXSpline(tmp);
						printf("Type 'xgoto start' to start motion \n");
					}
					else if (strncmp(mesg,"xgoto",5) == 0)
					{
						sscanf(mesg+5,"%f %f %f", &tmp[0], &tmp[1], &tmp[2]);

						// CHECK IF GOAL POS ARE WITHIN LIMITS
						if(tmp[0] < 0.2)
							tmp[0] == 0.2;
						if(tmp[2] < -0.76)
							tmp[2] == -0.76;
					
						printf("Current ee pos: %6.3f, %6.3f, %6.3f\n",
									rt->x[0], rt->x[1], rt->x[2]);
						printf("Goal ee pos: %6.3f, %6.3f, %6.3f \n",
									tmp[0], tmp[1], tmp[2]);

						calcXSpline(tmp);
						printf("Type 'xgoto start' to start motion \n");
					}
					else if(strncmp(mesg,"xosc start",10) == 0)
					{
						rt->c1 = rt->oscFreq;
						rt->c2 = (rt->c1)*(rt->c1);

						rt->xinit[0] = rt->x[0];
						rt->xinit[1] = rt->x[1];
						rt->xinit[2] = rt->x[2];

						rt->userMode = OS_OSC;
						rt->xtime = rt->time;
						printf("Time: %f \n", rt->xtime);
						printf("xyz initial: %7.4f %7.4f %7.4f \n",
								rt->xinit[0], rt->xinit[1], rt->xinit[2]);
					}
					else if(strncmp(mesg,"xosc0",5) == 0)
					{
						sscanf(mesg+5,"%f ", &tmp[0]);

						rt->jNumber = 1;
						rt->oscAmp = 0.075;
						rt->oscFreq = tmp[0]*2.0*PI;

						printf("EE x-z pos will osc at amplitude of %5.3f m and freq of %4.3f Hz \n",
									rt->oscAmp, rt->oscFreq/(2.0*PI));

					}
					else if(strncmp(mesg,"xhold",5) == 0)
					{
						int axis = 0;	// default compliance axis
						sscanf(mesg+5,"%d ", &axis);
			
						if((axis < 0) || (axis > 2))
							axis = 0;
			
						rt->complianceAxis = axis;
		
						rt->xgoal[0] = rt->x[0];				
						rt->xgoal[1] = rt->x[1];				
						rt->xgoal[2] = rt->x[2];	
			
						printf("Entering OS HOLD mode \n");
						printf("Compliance axis: %d \n", rt->complianceAxis);			
						printf("xyz initial: %7.4f %7.4f %7.4f \n",
								rt->xgoal[0], rt->xgoal[1], rt->xgoal[2]);
							
						rt->xtime = rt->time;
						printf("os-hold mode start time: %f \n", rt->xtime);
						rt->userMode = OS_HOLD;
					}

					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");
					}
					else if (strncmp(mesg,"vwall",5) == 0)
					{
						printf("Entering virtual wall demo... \n");
						printf("Make sure end-effector clear the impact plane!!! \n");
						rt->userMode = OS_VWALL;

					}
					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 tfmax = 0.0;
	float dqmax = 30.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];		

		if(tf[i]>tfmax)
			tfmax = tf[i];
	}

	rt->tf[0] = tfmax;
	rt->tf[1] = tfmax;
	rt->tf[2] = tfmax;
	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]);
	}
}

void calcXSpline(float xgoal[])
{
	int i;
	float tf[3];
	float tfmax = 0.0;
	float dxmax = 0.2;	// max velocity in m/s
	// was using 0.5 for impact test
//	float dxmax = 0.9;	// max velocity in m/s	==> max vel. used for impact test
	// 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*(xgoal[i] - rt->x[i]))/(dxmax*2.0));
		if(tf[i]> rt->tf[i])
			rt->tf[i]= tf[i];		

		if(tf[i]>tfmax)
			tfmax = tf[i];
	}

	rt->tf[0] = tfmax;
	rt->tf[1] = tfmax;
	rt->tf[2] = tfmax;

	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->x[i];
			rt->a2[i] = 3.0*(xgoal[i] - rt->x[i])/(rt->tf[i]*rt->tf[i]);
			rt->a3[i] = -2.0*(xgoal[i]- rt->x[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]: xgoal:%7.2f, q: %7.2f; ", i, xgoal[i], rt->x[i]);
		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
	}
}

void calcXSplineVel(float xgoal[])
{
	int i;
	float tf[3];
	float tfmax = 0.0;
	float dxmax = 0.3;	// max velocity in m/s
	// 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*(xgoal[i] - rt->x[i]))/(dxmax*2.0));
		if(tf[i]> rt->tf[i])
			rt->tf[i]= tf[i];		

		if(tf[i]>tfmax)
			tfmax = tf[i];
	}

	rt->tf[0] = tfmax;
	rt->tf[1] = tfmax;
	rt->tf[2] = tfmax;

	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->x[i];
			rt->a2[i] = 3.0*(xgoal[i] - rt->x[i])/(rt->tf[i]*rt->tf[i]);
			rt->a3[i] = -2.0*(xgoal[i]- rt->x[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]: xgoal:%7.2f, q: %7.2f; ", i, xgoal[i], rt->x[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]);
	}
}