/*
	Peter Thaulad
	03/30/2004

	main.cpp
	***********
	
*/

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

#include "driver/PrSTGDefn.h"
#include "driver/PrTimer.h"

#include "matrix/PrMatrix.h"
#include "matrix/PrVector.h"
#include "matrix/PrVector6.h"
#include "matrix/PrVector3.h"
#include "matrix/PrTransform.h"

#include "SharedMemory.h"
#include "Process.h"
#include "Definitions.h"
#include "control.h"
#include "max547Driver.h"

// Definitions
//#define HZ		1000.0	// control thread servo loop
#define HZ		500.0	// control thread servo loop
#define TICKTOSECOND	1000.0
#define DEGTORAD	1/57.3

#define PRINT_TO_SCREEN	0
#define SAVE_DATA_FILES	1
#define SINGLE		0 

// CLASS OBJECTS DECLARATIONS
PrTimer servoTimer(HZ);

// Shared Memory and Threading classes
sharedMemory *SM = new sharedMemory;
sharedMemory *rt;
SM_Functions *sm_functions = new SM_Functions();
Process *process = new Process();

// motor control class
ServoToGo *stgPtr = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);
//ServoToGo *stgPtr2 = new ServoToGo(STG2_ARM_IRQ, STG2_ARM_ADDRESS);
dm2Control *arm3dof = new dm2Control(stgPtr);

#define TEST_MAIN	1

#if TEST_MAIN
void main(void)
{
	static int lcounter = 0;
	int i,j, exit_flag = 0, tick;
	int userExit=0;
	static long int counter = 0;
	float t=0.0, time=0.0, temp10, timeFloat;
	double time2 = 0.0, time3 = 0.0, runtime = 0.0;
	FILE *mem_loc;
	static float timeStart = 0.0;
	float qdes_, dqdes_;
	PrVector x(3);
	PrVector q(6);
	float xdes_[3], dxdes_[3];
	
	pthread_mutexattr_t attr;
	printf("\nSEA 3DOF Main Program Test\n\n\n");

	// Initialize shared memory
	SM = sm_functions->Create_Shared_Memory();
	// write address of SM to file so other process
	// can locate SM
	mem_loc = fopen("smaddress.txt","w");
	fprintf(mem_loc,"%x",SM);
	fclose(mem_loc);

	sm_functions->Init_Shared_Memory();
	rt = sm_functions->Init_Local_SM(rt);

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

	// set process priority to max
	pid_t pid = getpid();
	setprio(pid, 63);
	rt->pid = pid;

	servoTimer.start();
	for(i=0;i<100;i++)
	{
		arm3dof->readPosition();
		arm3dof->calcPosition();
		servoTimer.wait();
	}

	arm3dof->calcRotationMatrix();
	// arm3dof->initForceSensorOffset();

	arm3dof->resetIndex();
	arm3dof->calcPreloadSpringTorque();

	rt->userMode = IDLE;
	sm_functions->Write_To_SM(rt);

	// spawn slower external processes that print and take user inputs at slower rate
//	process->Spawn_RT();

	timeStart = servoTimer.time();
	printf("timeStart: %f \n", timeStart);

	int START_COUNT = 4000;
	int dataOut = -START_COUNT;

	// MAIN CONTROL LOOP
	while(!tcischars(1) && (rt->userExit != 1))
	{
//		printf("mode %d \n", rt->userMode);
//		printf("mode %d \n", rt->miniMode);
		// read local shared memory and check user input
		sm_functions->Read_SM(rt);
		userExit = rt->userExit;

//		printf("exit: %d \n", rt->userExit);

		arm3dof->readLoadAccel();
		arm3dof->readLOHET();						// read LOHET and calculate spring torque
		arm3dof->readPosition();
		arm3dof->calcPosition();
		arm3dof->calcJacobian();
		arm3dof->calcRotationMatrix();
		arm3dof->calcX();
		//arm3dof->readForceSensor();

		rt->time = servoTimer.time() - timeStart;
		time = rt->time;
		
		arm3dof->setTime(time);
		arm3dof->setUserMode(rt->userMode);

		// DECIDE WHAT TO DO BASED ON USER INPUT (FROM USER.CPP)
		switch(rt->userMode)
		{

				case(NEWFLOAT):
				{
					arm3dof->readPosition();
					arm3dof->calcPosition();
					arm3dof->gravcomp_settorque();

					arm3dof->macroControlNew();
                    
					break;
				}
				case (DISPLAY_INFO):
				{
					arm3dof->calcPosition();
					arm3dof->displayInfo2();	
					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
					// save data to buffer
					arm3dof->saveInfo();

					break;
				}

				case (TEST_MAX):
				{
					arm3dof->dacRight->writeData(dataOut);	

					dataOut += 5;

					if(dataOut > START_COUNT)
						dataOut = -START_COUNT;
					break;
				}

				case (FLOAT):
				{
					timeFloat = rt->time - rt->xtime;

					//copy from above, outside the switch case
					arm3dof->readLoadAccel();
					arm3dof->readLOHET();						// read LOHET and calculate spring torque
					arm3dof->readPosition();
					arm3dof->calcPosition();
					arm3dof->calcJacobian();
					arm3dof->calcRotationMatrix();
					arm3dof->calcX();
					//
					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
					arm3dof->gravityCompensationRight();	// for RIGHT arml

					arm3dof->calcJointTorqueDes(rt->userMode);
					
					arm3dof->calcBaseTorqueDes(timeFloat);
					
					arm3dof->modulateLoadTorqueCommand(timeFloat);	// modulate torque command so that initially it doesnt step
					//John: is modulate ... neccessary since in MacroControlNew we dont just use BaseTorque from calcBaseTorqueDes
					
					//John: we need to read LOHET data before doing something crazy in macroControlNew
					//arm3dof->readLOHET();     //added by John 
					arm3dof->macroControlNew();	

		//			if(rt->miniMode == TRUE)
						arm3dof->miniControl(rt->miniMode);
		//			else
		//				arm3dof->zeroMiniOutput();
					
					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();

					break;
				}
				case (FLOAT_FORCE):
				{
					timeFloat = rt->time - rt->xtime;
				
					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
					arm3dof->gravityCompensationRight();	// for RIGHT arml
					arm3dof->setDesiredForce();
					arm3dof->opSpaceControl(rt->miniMode,rt->userMode);

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(timeFloat);
					arm3dof->modulateLoadTorqueCommand(timeFloat);	// modulate torque command so that initially it doesnt step
					arm3dof->macroControlNew();	

					arm3dof->miniControl(rt->miniMode);

					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();

					break;
				}

				case (JOINT_SERVO):
				{
					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
				
					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					arm3dof->calcTraj(runtime);
					arm3dof->setJointControlMode(POS_MODE);
			//		arm3dof->jointControlNonDynamic(rt->miniMode);
					arm3dof->jointControlDynamic(rt->miniMode);

					arm3dof->gravityCompensationRight();	// for RIGHT arml

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
			
					arm3dof->macroControlNew();	
		
					arm3dof->miniControl(rt->miniMode);

					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (JOINT_OSC):
				{
					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
			
					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					if(runtime < 5.0/(rt->oscFreq/(2.0*PI)))
					{
						qdes_ = rt->c0 + rt->oscAmp*(1-cos((rt->oscFreq)*runtime));
						dqdes_ = rt->c1*rt->oscAmp*sin((rt->oscFreq)*runtime);
					}
					else if(runtime > 5.0/(rt->oscFreq/(2.0*PI)))
					{
						qdes_ = rt->q[rt->jNumber];
						dqdes_ = 0.0;
					}

	
					arm3dof->calcOscTraj(runtime, rt->jNumber, qdes_, dqdes_, rt->qinit);

					arm3dof->setJointControlMode(POS_MODE);
			//		arm3dof->jointControlNonDynamic(rt->miniMode);
					arm3dof->jointControlDynamic(rt->miniMode);

					arm3dof->gravityCompensationRight();	// for RIGHT arml

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
					arm3dof->readLOHET();
					arm3dof->macroControlNew();	
					arm3dof->miniControl(rt->miniMode);
		

					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (OS_SERVO):
				{
					arm3dof->setXSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->xgoal);
					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
			

					arm3dof->gravityCompensationRight();	// for RIGHT arml
		
					// PST temp comment out to do impact safety experiment	
					arm3dof->calcXTraj(runtime);
					arm3dof->opSpaceControl(rt->miniMode, rt->userMode);

					// this is used for impact safety experiment... pure velocity control mode
		//			arm3dof->calcXTrajVel(runtime);
		//			arm3dof->opSpaceControlImpact(rt->miniMode, rt->userMode);

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
			
					arm3dof->macroControlNew();	
					arm3dof->miniControl(rt->miniMode);
		
					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (OS_HOLD):
				{

					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
			
					arm3dof->gravityCompensationRight();	// for RIGHT arml
			
					arm3dof->opSpaceHoldParameters(rt->xgoal, rt->complianceAxis);
					arm3dof->setDesiredForce();
					arm3dof->opSpaceControl(rt->miniMode, rt->userMode);

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
			
					arm3dof->macroControlNew();	
					arm3dof->miniControl(rt->miniMode);

					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (OS_OSC):
				{
					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					if(runtime < 4.0/(rt->oscFreq/(2.0*PI)))
					{
						for(i=0;i<3;i++)
						{
							xdes_[i] = rt->xinit[i] + rt->oscAmp*(1-cos((rt->oscFreq)*runtime));
							dxdes_[i] = rt->c1*rt->oscAmp*sin((rt->oscFreq)*runtime);
						}
					}
					else if(runtime > 4.0/(rt->oscFreq/(2.0*PI)))
					{
						for(i=0;i<3;i++)
						{
							xdes_[i] = rt->x[i];
							dxdes_[i] = 0.0;
						}
					}
	
					arm3dof->calcXOscTraj(runtime, rt->jNumber, xdes_, dxdes_, rt->xinit);
			
					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
					arm3dof->gravityCompensationRight();	// for RIGHT arml

					arm3dof->opSpaceControl(rt->miniMode, rt->userMode);

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
		
					arm3dof->macroControlNew();	
					arm3dof->miniControl(rt->miniMode);
		
					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (OS_CONTACT_CONTROL):
				{

					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
			
					arm3dof->gravityCompensationRight();	// for RIGHT arml
			
					arm3dof->setDesiredContactForce();
					arm3dof->opSpaceControl(rt->miniMode, rt->userMode);

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
			
					arm3dof->macroControlNew();	
					arm3dof->miniControl(rt->miniMode);

					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (OS_VWALL):
				{

					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					// runtime is the amount of time the motion will be achieved in

					arm3dof->calcAllJacobians();
					arm3dof->calcAMatrix();
					arm3dof->calcLambdaMatrix();
			
					arm3dof->gravityCompensationRight();	// for RIGHT arml
		
					arm3dof->virtualWallControl(rt->miniMode);	

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
			
					arm3dof->macroControlNew();	
					arm3dof->miniControl(rt->miniMode);

					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}
				case (VEL_SERVO):
				{
				#if 0
					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
					arm3dof->calcTrajVel(runtime);
					arm3dof->setJointControlMode(VEL_MODE);
					arm3dof->jointControlNonDynamic(rt->miniMode);

					arm3dof->gravityCompensationRight();	// for RIGHT arml

					arm3dof->calcJointTorqueDes(rt->userMode);
					arm3dof->calcBaseTorqueDes(runtime);
					arm3dof->macroControlNew();	

					if(rt->miniMode == TRUE)
						arm3dof->miniControl();
					else
						arm3dof->zeroMiniOutput();
				#endif
					arm3dof->displayInfo();
					// save data to buffer
					arm3dof->saveInfo();
					break;
				}

				case (ENCODER_TEST):
				{
	//					printf("running encoder test \n");
						break;
				}

				case (IDLE):
				{
					break;
				}

		}	// end of switch statement
		// END OF DECIDE WHAT TO DO BASED ON USER INPUT (FROM USER.CPP)

		// save data to buffer
//		arm3dof->saveInfo();

		q = arm3dof->getQ();
		x = arm3dof->getX();

		for(i=0;i<NOJ;i++)
		{
			rt->q[i] = q[i];
			if(i<3)
				rt->x[i] = x[i];
		}

		// write to shared memory
		sm_functions->Write_To_SM(rt);

		tick = servoTimer.wait();

		//if(tick > 0)
				//printf("Missed %2d tick \n", tick);
	}

	arm3dof->dacRight->shutdown();
	printf("SEA3DOF Process Terminated \n");

#if 1
	// only perform mini shutdown when the mini is turned on
	if(rt->miniMode == TRUE)
	{
		printf("shutting down mini \n");
		while(!arm3dof->shutdownMotors())
		{
			arm3dof->saveInfo();
			servoTimer.wait();
		}
	}
#endif


	// Housekeeping functions to do before quitting program
	rt->exit_flag = 1;
	sm_functions->Write_To_SM(rt);

	// UNCOMMENT TO SAVE DATA TO FILES
//	arm3dof->saveBuffer();

	//	delay(1000);
//	process->Kill_RT_Process();
//	delay(1000);
	// Destroy mutex 
	pthread_mutex_destroy(&(SM->mutex));

	printf("Deleting arm3dof object \n");
	delete arm3dof;
	printf("Object deleted \n");
}

#endif