/*
	Peter Thaulad
	03/30/2004

	Printer.cpp
	***********
	
	Process that only prints data to screen at a slower rate. Works ( and spawned by) 
	in conjunction with the OS control process (control.cpp). 

*/

#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 "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 "Definitions.h"
#include "Process.h"

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

// VARIABLE DECLARATIONS

#define HZ	200.0
#define NOJ	5

// CLASS OBJECTS DECLARATIONS
PrTimer timerReal(HZ);
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

// MAIN
void main(void)
{
	int i,j, counter = 0, exit_flag=0, sm_address;
	static int k=0,x=0;	
	FILE *memaddress;
	float t=0.0, time = 0.0;
	double time2=0.0, time3=0.0, time4 = 0.0, time5 = 0.0;
	PrVector tau(NOJ), tau_des(NOJ), temp(NOJ), q(NOJ), dq(NOJ), dq_virtual(NOJ);
	PrVector q_des(NOJ), dq_raw(NOJ), dq_des(NOJ);
	static PrVector q_old(NOJ), dq_old(NOJ);
	pthread_mutexattr_t attr;

	// Process init stuff
	process->Get_Background_pid();
	process->Set_Realtime_Priority();

	// init global shared memory
	printf("Opening Printer 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\nPrinting thread starts \n");
	printf("Printer Ver 06/14/04 \n\n ");
	timerReal.start();

	while(!tcischars(1))
	{
		// Poke control process to see if it's still alive
		if(kill(rt->pid,0) == -1)
		{
			printf("\nfrom printer.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));

		// change the below if statement to change printing frequency.
		// for example: doing if(counter == HZ) will print every second and 
		// doint if(counter == 0.5*HZ) will print every half second.
		if(counter == 2.0*HZ)
		{
			time4 = timerReal.time();
			// check process time
			// rt->time is time elapsed in control process
			// (time4-time5) is the delta time between printing
			// rt->honda_count is the servo loop count from Honda controller
	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
			printf("time:	 %6.1f\n", rt->time);
			time5 = time4;
/*	
			switch(rt->user_mode)
			{
				case(GRAVITY_TEST):
				{
					printf("Gravity mode \n");
			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
			//			rt->x[1], rt->x[2]);
					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
						rt->xee[1], rt->xee[2]);
					break;
				}
				case(FRICTION_TEST):
				{
					printf("Friction mode \n");
					break;
				}
				case(JS_TEST):
				{
					printf("Joint space mode \n");
					printf("qgoal(deg): %7.2f %7.2f %7.2f %7.2f %7.2f \n",
						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
						57.3*rt->jgoal[2],
						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
					break;
				}
				case(JS_OSC):
				{
					printf("Joint space oscillation mode \n");
					printf("qgoal(deg): %7.2f %7.2f %7.2f %7.2f %7.2f \n",
						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
						57.3*rt->jgoal[2],
						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
					break;
				}
				case(OS_TEST):
				{
					printf("OS track mode \n");
					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
						rt->x_des[1],rt->x_des[2]); 
		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
		//				rt->x[1], rt->x[2]);
					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
						rt->xee[1], rt->xee[2]);
					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
						rt->F[1], rt->F[2]);
					break;
				}
				case(OS_HOLD):
				{
					printf("OS compliance mode direction: %c \n", 
						'x'+rt->compliance_direction);
					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
						rt->xgoal[1],rt->xgoal[2]); 
			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
			//			rt->x[1], rt->x[2]);
					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
						rt->xee[1], rt->xee[2]);
					printf("Assisting Force desired: %7.3f %7.3f %7.3f \n",
							rt->desiredForce[0], rt->desiredForce[1],
							rt->desiredForce[2]);
					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
					rt->force[1], rt->force[2]);
					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
						rt->F[1], rt->F[2]);
					break;
				}

				case(OS_FORCE):
				{
					printf("Force desired: %7.3f %7.3f %7.3f \n",
							rt->desiredForce[0], rt->desiredForce[1],
							rt->desiredForce[2]);
					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
					rt->force[1], rt->force[2]);
					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
						rt->F[1], rt->F[2]);


					break;
				}

				case(IDLE):
				{
			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
			//			rt->x[1], rt->x[2]);
					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
						rt->xee[1], rt->xee[2]);
					printf("Idle mode \n");
					break;
				}
				case(TRAJECTORY_TEST):
				{
					printf("Trajectory mode \n");
					printf("qgoal(deg): %7.4f %7.4f %7.4f %7.4f %7.4f \n",
						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
						57.3*rt->jgoal[2],
						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
					break;
				}

			}	// end of switch statement

		*/
/*			printf("q(deg)	: %7.2f %7.2f %7.2f %7.2f %7.2f \n",
				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
				57.3*rt->q[3], 57.3*rt->q[4]);

			printf("qdes	: %7.2f %7.2f %7.2f %7.2f %7.2f \n",
				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
				57.3*rt->q_des[3], 57.3*rt->q_des[4]);

			printf("dq(deg)	: %7.2f %7.2f %7.2f %7.2f %7.2f \n",
				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
				57.3*rt->dq[3], 57.3*rt->dq[4]);

			printf("tau(N.m): %7.4f %7.4f %7.4f %7.4f %7.4f \n",
				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);

			printf("taudes	: %7.2f %7.2f %7.2f %7.2f %7.2f \n",
				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
				rt->tau_des[3], rt->tau_des[4]);

			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
					rt->force[1], rt->force[2]);

			printf("\n");
*/
			counter = 0;
		}	// end of printing if

		counter++;
		t += 1/HZ;
		time2 = timerReal.time();
//		printf("Print loop: %f Hz \n", 1.0/(time2-time3));
		time3 = time2;
		timerReal.wait();
	}	// end of while(!tcischars(1))
	
	pthread_mutex_destroy(&(SM->mutex));	

}

