/* Robot arm sample program based on Burns & Wellings "Real-Time Systems
   and Programming Languages", ch. 9.5, vxWorks POSIX environment
   POSIX pthreads environment
   Run program with shell command   -->sp main   !!
   Revision Sep 2011, B. Skaali
*/

#include "vxWorks.h"
#include "taskLib.h"
#include "sysLib.h"
#include "mqueue.h" 
#include "fcntl.h"
#include "errno.h"
#include "time.h"
#include "tickLib.h"
#include "stdio.h"
#include "stdlib.h"
#include "pthread.h"
#include "logLib.h"

/* Place running x,y,z coordinates as globals for StethoScope sampling */
int	currPx, currPy, currPz;
int newPx_reached, newPy_reached, newPz_reached;

#define ROBOT_STETHOSCOPE

#ifdef ROBOT_STETHOSCOPE

#define  RTI_VXWORKS

int robotTid = 0;
int robotSamplingRate= 60;
int robotUseAuxClock = 1;
int robotScopeIndex = 0;
int robotverbosity = 0;
int robotnoServer = 0;
int robotreqDataBufSize = 8000;
int robotnoCollection = 0;


/* >>>>>>>>>>>>>>>> Robot Stethoscope <<<<<<<<<<<<<<<<<< */
/* based on :											 */
/* ref. vxdemo.c, v 1.42 2002/07/18 22:24:57 sreenath Exp                 */
/* (c) Copyright Real-Time Innovations, Inc., 1999.  All rights reserved. */
/* StethoScope files, copied from ...\scopetools-5.4\target\ */
/*                           to   ...\vxworks-6.2\target\h\  */

#include "math.h"
#include "scope/scope.h"
#include "rtilib/vxVersions.h"
#include "rtilib/rti_types.h"
#include "rtilib/rti_endian.h"

float Time = 0.0;
float Dt = 0.005;

SEM_ID SampleSemaphore = NULL;
int ScopeIndex = -1;
int ScopeHandle = -1;

static int ScopeDemoShutdownRequest = 0;
static int ScopeNoServer = 0;

void (*SampleHookFunc)(void *) = NULL;
void *SampleHookParam = NULL;

void SampleHookSet( void (*func)(void *), void *param )
{
	SampleHookFunc = func;
	SampleHookParam = param;
}

static
void Sample(float t, int noCollection)
{
	if (noCollection == 0) { ScopeCollectSignals(ScopeIndex); }
	if (SampleHookFunc != NULL) {
	(*SampleHookFunc)(SampleHookParam);
	}
}

static
void InstallRobotSignals(int num)
{
	/* Install ROBOT signals for debugging */
	ScopeInstallSignal ("currX", "n/a", &currPx, "int", ScopeIndex);
	ScopeInstallSignal ("currY", "n/a", &currPy, "int", ScopeIndex);
	ScopeInstallSignal ("currZ", "n/a", &currPz, "int", ScopeIndex);	
}

static
/* This routine simply lets the sampler run.  An alternative, asychronous
   sampling strategy is to simply call ScopeCollectSignals() from this routine
   (at interrupt level).  However, this scheme is probably more indicative of
   the way most users will implement Scope. */
int ScopeDemoTimerInterrupt(void)
{
	semGive(SampleSemaphore);
	return(0);		/*  sysAuxClkConnect should take VOIDFUNCPTR */
}

static
/* Called by main loop to clean up mem usage */
void ScopeDemoShutdownInternal(void)
{
	if (robotTid != 0) 
	{
		if (SampleSemaphore != NULL) 
		{
			/* Must be aux clk, so clean up. */
			sysAuxClkDisable();
			sysAuxClkConnect(NULL, 0);
			semDelete(SampleSemaphore);
			SampleSemaphore = NULL;
		}
		if (ScopeNoServer == 0) 
		{
			ScopeShutdown(ScopeIndex);
		} else 
		{
			#if VXWORKS_AE_VERSION_1_0_OR_BETTER
				ScopeRemoveMultipleSignals("", ScopeIndex);
			#else
				ScopeRemoveMultipleSignals("*", ScopeIndex);
			#endif
			ScopeNoServer = 0;
		}
		ScopeIndex = -1;
		robotTid = 0;
	}
	/* Reset flag */
	ScopeDemoShutdownRequest = 0;
}

static
/* This routine generates the signals, and does the sampling.
 * It attaches a semaphore to the Aux clock iff useAuxClock is non-zero.
 */
int ScopeDemoSampler(int requestedSR, int useAuxClock, int noCollection)
{
	int nTicksBwSamples = 0;
	float actualSR;
	if (useAuxClock) 
	{
		SampleSemaphore = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);
	}

	/* Connect the timer to the auxillary clock interrupt. */
	if (useAuxClock && (sysAuxClkConnect(ScopeDemoTimerInterrupt, 0) == OK)) 
	{
		sysAuxClkEnable();
		sysAuxClkRateSet(requestedSR);
		/* hardware may not be exact */
		actualSR = sysAuxClkRateGet();
	} else 
	{
		actualSR = sysClkRateGet();
		if (actualSR < requestedSR) 
		{
/* Requested rate (requestedSR) is higher than  maximum achievable rate.*/
			nTicksBwSamples = 1;
		} else 
		{
			nTicksBwSamples = actualSR / requestedSR;
			actualSR = requestedSR;
		}
	}

	/* Tell scope the actual sample rate (hardware may not be exact). */
	ScopeChangeSampleRateInt((int) actualSR, ScopeIndex);

	if((actualSR/requestedSR > 1.1) || (actualSR/requestedSR < 0.9)) 
	{
		printf("ScopeDemo: requested rate (%f Hz) not \n"
		   "achievable, actual sampling rate is %f Hz\n",
		   (float) requestedSR, actualSR);
	}

	Dt = 1./actualSR;
	while(!ScopeDemoShutdownRequest) 
	{
		if (useAuxClock) 
		{
			semTake(SampleSemaphore, WAIT_FOREVER);
		} else 
		{
			taskDelay(nTicksBwSamples);
		}
		Time += Dt;
		Sample(Time, noCollection);
	}
	ScopeDemoShutdownInternal();
	return(0);
}


/* ARGUMENTS ScopeDemo :
   If useAuxClock is TRUE, use the Aux clock. Otherwise (or if there is no
	  Aux clock, use taskDelay.
   scopeIndex is the StethoScope index to use. It must be coordinated with
	  the GUI.
   verbosity controls the amount of warning and debug messages.  0 means
	  only errors will be printed.  Higher values causes more output.
   If reqDataBufSize is non-zero, use that value rather then the default (51k)
   If noServer is true, then this demo will not start a Scope server.  Assumes
	 one is already started.
   If noCollection is non-zero then this demo will not call ScopeCollectSignals
	 since it is assumed that another process is collecting.
*/

STATUS ScopeDemo( int useAuxClock, int scopeIndex, int verbosity,
		int reqDataBufSize, int noServer, int noCollection )
{
	int samplingRate = robotSamplingRate;     /* Hz */
	int dataBufSize, signalBufSize, eventBufSize;

	/* Initialize */
	if (ScopeIndex > 0) 
	{
		return 2001;
	}

	if (reqDataBufSize == 0) 
	{
		dataBufSize = 4000;
	} else 
	{
		dataBufSize = reqDataBufSize;
	}
	signalBufSize = 7200;
	eventBufSize = 7200;
	printf("ScopeDemo noServer = %d\n", noServer);
	if (noServer == 0) 
	{
		/* Initialize an index. */
		ScopeIndex = ScopeInitServer(dataBufSize, signalBufSize, verbosity,
					 scopeIndex);
		if (ScopeIndex < 0) 
		{
			printf("ScopeInitServer failed, return code = %d\n", ScopeIndex);
			return 2002;
		}

	/* Attach an event buffer to that index. */
		ScopeHandle = ScopeEventsAttach(eventBufSize, ScopeIndex);
		if(ScopeHandle == 0) 
		{
			printf("ScopeEventsAttach failed, exiting!\n");
			return 2003;
		}

	} else 
	{
		ScopeIndex = scopeIndex;
		ScopeNoServer = 1;
	}
 
	ScopeIndex = scopeIndex;

	/* OK, now install the robot signals */
	InstallRobotSignals(0);
	/* and spawn the Sampler */
	robotTid = taskSpawn("ScopeRobot", 100, VX_FP_TASK|VX_STDIO, 0x4000, 
		  (int (*)()) ScopeDemoSampler,
		  samplingRate, useAuxClock, noCollection,0,0,0,0,0,0,0);
	if (robotTid == ERROR) return ERROR;	
	return OK;
}

void ScopeDemoShutdown(void)
{
	if (robotTid != 0) {
	ScopeDemoShutdownRequest = 1;
	}
}
#endif /*ROBOT_STETHOSCOPE */

/* POSIX message queues */
#define	MQ_XPLANE "mq_xplane"
#define	MQ_YPLANE "mq_yplane"
#define	MQ_ZPLANE "mq_zplane"
#define MODE	 0
#define MSGPRI	31

/* permitted robot coordinate ranges */
#define	ROBOT_XMIN	0
#define ROBOT_XMAX	100
#define	ROBOT_YMIN	0
#define ROBOT_YMAX	100
#define	ROBOT_ZMIN	0
#define ROBOT_ZMAX	100
/* end coordinate */
#define ROBOT_ZZZ	999

/* assume the coordinate can be represented as int in 4 characters */
#define	DEFAULT_NBYTES	4
int nbytes = DEFAULT_NBYTES;

pthread_attr_t	attributes;
pthread_t		xp, yp, zp;

typedef		enum {xplane, yplane, zplane} dimension;

int	move_arm (dimension D, int P, int newP);

/* clockticks to move arm coordinate is taskDelay(TDSCALE * length) */
#define TDSCALE	2
 
/* arrays with a few robot arm x,y,z coordinates */
int xa_coord[15] = {0, 7,   5, 15, 30, 12, 25, 0, 18, 14, 17,  9, 15, 0,ROBOT_ZZZ};
int ya_coord[15] = {0, 20, 33, 42, 50, 36, 47, 0, 32, 26, 41, 49, 20, 0,ROBOT_ZZZ};
int za_coord[15] = {0, 35, 55, 66, 90, 77, 62, 0, 89, 50, 69, 77, 59, 0,ROBOT_ZZZ};


/* === Robot controller code - executed as three threads --- */
void	controller (dimension *dim)
{
	int prio, current_axiscoord, next_axiscoord;
	int status = OK;

	mqd_t my_queue;
	char buf[DEFAULT_NBYTES];
	ssize_t len;

	/* initial coordinate positions for x, y and z */ 
	current_axiscoord = 0;

	/* open appropriate message queue */
	switch( *dim) {
	  case xplane:
	    if ((my_queue = mq_open(MQ_XPLANE, O_RDWR, 0, NULL))
		== (mqd_t)-1) {
		printf("\nXPLANE mq_open error");
		status = ERROR; pthread_exit ( &status);
	    }
	    break;
	  case yplane:
	    if ((my_queue = mq_open(MQ_YPLANE, O_RDWR, 0, NULL))
		== (mqd_t)-1) {
		printf("\nYPLANE mq_open error");
		status = ERROR; pthread_exit ( &status);
	    }
	    break;
	  case zplane:
	    if ((my_queue = mq_open(MQ_ZPLANE, O_RDWR, 0, NULL))
		== (mqd_t)-1) {
		printf("\nZPLANE mq_open error");
		status = ERROR; pthread_exit ( &status);
	    }
	    break;
	  default:
	    return;
	};

	logMsg (" Robot thread starting up\n", 0,0,0,0,0,0);

	/* keep the robot working */	
	while (TRUE)
	{
	  /* read message */
	  len = mq_receive (my_queue, &buf[0], nbytes, &prio);
	  if (len == ERROR) exit(ERROR);
	
	  /* quick and dirty convert from char to int for 32-bit architecture, from B&W  */
	  /* otherwise sprintf() from int to char, and atoi() from char to int           */
	  next_axiscoord = *((int*)(&buf[0]));

	/* test for robot sleep message */
	  if (next_axiscoord == ROBOT_ZZZ) {
	    switch(*dim) {
	  	case xplane:
		  printf("\ntick %ld: X shutdown", tickGet());
		  break;
	  	case yplane:
		  printf("\ntick %ld: Y shutdown", tickGet());
		  break;
	  	case zplane:
		  printf("\ntick %ld: Z shutdown", tickGet());
		  break;
		default:
		  printf("\nIllegal dim value\n");
	    }
	    /* shut down, close message queue */
	    mq_close (my_queue);
		pthread_exit ( &status);	    
	  }
	
	  /* move arm to new position and update */
	  move_arm(*dim, current_axiscoord, next_axiscoord);
	  current_axiscoord = next_axiscoord;
	}
}

/* end Robot Controller  */

/* ------------ set up and run the robot -------- */	
/* spawn   main   by the shell command     sp main
   otherwise the shell will hang after termination, why so? */

int main()
{
	mqd_t mq_xplane, mq_yplane, mq_zplane;  /* msg queue descriptors */
	mode_t mode=(mode_t)MODE; 		        /* mode, dummy */
	struct mq_attr ma;		            	/* queue attributes */

	dimension X, Y, Z;
	void *result;

 	char buf[DEFAULT_NBYTES];
	int coord_arrsize, x, y, z, i, mark_mq_error; 
      
	X = xplane;
	Y = yplane;
	Z = zplane;

	/* set the required message queue attributes */
	ma.mq_flags = 0;
	ma.mq_maxmsg = 1;
	ma.mq_msgsize = nbytes;	
	
	/* Create and open message queues */
	if ((mq_xplane = mq_open (MQ_XPLANE, O_RDWR|O_CREAT, mode, &ma))
		== (mqd_t)-1) {
		return(1001);
	}
	if ((mq_yplane = mq_open (MQ_YPLANE, O_RDWR|O_CREAT, mode, &ma))
		== (mqd_t)-1) {
		return(1002);
	}
	if ((mq_zplane = mq_open (MQ_ZPLANE, O_RDWR|O_CREAT, mode, &ma))
		== (mqd_t)-1) {
		return(1003);
	}

	/* create the threads,  set default attributes */
	if (pthread_attr_init (&attributes) != 0)
	  exit (ERROR);

	if (pthread_create (&xp, &attributes, (void *)controller, &X) != 0)
	  exit (ERROR);

	if (pthread_create (&yp, &attributes, (void *)controller, &Y) != 0)
	  exit (ERROR);

	if (pthread_create (&zp, &attributes, (void *)controller, &Z) != 0)
	  exit (ERROR);


	#ifdef ROBOT_STETHOSCOPE
	if ( ScopeDemo( robotUseAuxClock, robotScopeIndex, robotverbosity, 
		robotreqDataBufSize, robotnoServer , robotnoCollection ) != OK)
	{
		printf("robot ScopeDemo call failed \n");			
		return ERROR;
	}
	#endif 
	
	taskDelay (50);			/* let the controllers initialize */
	printf("Robot is starting up\n");


	/* now it's time to let the robot do something useful */
	/* a coordinate is an integer occupying 4 bytes of buf[] */

	mark_mq_error = 0;
	coord_arrsize = sizeof(xa_coord)/sizeof(int);
	while (TRUE) {
	  for (i = 0; i < coord_arrsize; i++) {

	    printf("\nRT-clock tick %ld", tickGet());

	    /* send new set of x,y,z coordinates */
	    printf(": next x-y-z =%3d %3d %3d", xa_coord[i],ya_coord[i],za_coord[i]);;
	    x = xa_coord[i];
	    if (x != ROBOT_ZZZ)
		    if ((x < ROBOT_XMIN)||(x > ROBOT_XMAX)) break;
	    *(int*)(&buf[0]) = x;
	    if (mq_send (mq_xplane, &buf[0], nbytes, MSGPRI) != OK) {
		  printf("\nmq_send error %d", errno);
		  mark_mq_error++;
	    }
	    y = ya_coord[i];
	    if (y != ROBOT_ZZZ)
	    	if ((y < ROBOT_YMIN)||(y > ROBOT_YMAX)) break;
	    *(int*)(&buf[0]) = y;
	    if (mq_send (mq_yplane, &buf[0], nbytes, MSGPRI) != OK) {
		  printf("\nmq_send error %d", errno);
		  mark_mq_error++;
	    }
	    z = za_coord[i];
	    if (z != ROBOT_ZZZ)
	    	if ((z < ROBOT_ZMIN)||(z > ROBOT_ZMAX)) break;
	    *(int*)(&buf[0]) = z;
	    if (mq_send (mq_zplane, &buf[0], nbytes, MSGPRI) != OK) {
		  printf("\nmq_send error %d", errno);
		  mark_mq_error++;
	    }
	  }

	  /* simple-minded termination, assuming that last array
	       element before ROBOT_ZZZ has been sent to controllers */
	  taskDelay (100);
	  break;
	}

	/* delete the message queues */
	printf("\ntick %ld: removing the message queues", tickGet());
	mq_unlink (MQ_XPLANE);
	mq_unlink (MQ_YPLANE);
	mq_unlink (MQ_ZPLANE);
	
	#ifdef ROBOT_STETHOSCOPE
		/* wait some to finish and delete StethoScope task */
		taskDelay(150);
		printf("\nStethoScope taskDelete = %d\n", taskDelete (robotTid) );
	#endif


	printf("\nRobot operation terminated with:");
	if (mark_mq_error == 0)
	  printf("\n SUCCESS\n");
	else
	  printf("\n- mq_send error marked\n");

	pthread_join (xp, (void **)&result);
	pthread_join (yp, (void **)&result);
	pthread_join (zp, (void **)&result);
	
	return 0;
}

int	move_arm (dimension D, int P, int newP)
{
	int n, inc, delay_steps;
	if (newP != P)
	{
		delay_steps = abs(newP-P);
		/* increment or decrement robot coordinate */		
		inc = 1;
		if (newP < P) inc = -1;

		switch (D) {
			case xplane:
				currPx = P;
				for (n = 1; n <= delay_steps; n++)
				{
					taskDelay (TDSCALE);
					currPx = currPx + inc;
				}
				newPx_reached = newP;
#ifdef ROBOT_STETHOSCOPE
				ScopeEventsCollect(ScopeHandle, 1, "Next X reached", 
							&newPx_reached, RTI_INT32ID);
#endif
				break;
			case yplane:
				currPy = P;
				for (n = 1; n <= delay_steps; n++)
				{
					taskDelay (TDSCALE);
					currPy = currPy + inc;
				}
				newPy_reached = newP;
#ifdef ROBOT_STETHOSCOPE
				ScopeEventsCollect(ScopeHandle, 1, "Next Y reached", 
						&newPy_reached, RTI_INT32ID);
#endif
				break;
			case zplane:
				currPz = P;
				for (n = 1; n <= delay_steps; n++)
				{
					taskDelay (TDSCALE);
					currPz = currPz + inc;
				}
				newPz_reached = newP;
#ifdef ROBOT_STETHOSCOPE
				ScopeEventsCollect(ScopeHandle, 1, "Next Z reached", 
						&newPz_reached, RTI_INT32ID);
#endif
			break;
		} /* end switch*/
	} /* end test newP vs. P */
		
	/* print tick time and coordinate for action executed */
	printf("\ntickGet %ld: ", tickGet());
	switch(D) {
	  case xplane:
		printf("moved to x=%d", newP);
		break;
	  case yplane:
		printf("moved to       y=%d", newP);
		break;
	  case zplane:
		printf("moved to             z=%d", newP);
		break;
	}
	return(OK);
}