/* Robot arm sample program based on Burns & Wellings "Real-Time Systems
   and Programming Languages", ch. 9.5, vxWorks POSIX environment 
   Revision 2011 / B. Skaali                                            */      

/* run robot: shell command  robot, or spawn from shell  -->sp robot */ 

#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"

/* 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 */


/* ********************************************** */
/*                The FYS4220 ROBOT               */
/* ********************************************** */

/* Controller tasks priority */
#define CONTRTPRI 60

/* 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 coordinate ranges for robot */
#define	ROBOT_XMIN	0
#define ROBOT_XMAX	100
#define	ROBOT_YMIN	0
#define ROBOT_YMAX	100
#define	ROBOT_ZMIN	0
#define ROBOT_ZMAX	100
/* termination coordinate */
#define ROBOT_ZZZ	999

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

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, three copies are spawned for x, y and z === */
void	controller (dimension dim)
{
	int prio, current_axiscoord, next_axiscoord;
	mqd_t my_queue;
	char buf[DEFAULT_NBYTES];
	ssize_t len;

	/* initial coordinate position, note three concurrent copies of "controller" */ 
	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");
		exit(ERROR);
	    }
	    break;
	  case yplane:
	    if ((my_queue = mq_open(MQ_YPLANE, O_RDWR, 0, NULL))
		== (mqd_t)-1) {
		printf("\nYPLANE mq_open error");
		exit(ERROR);
	    }
	    break;
	  case zplane:
	    if ((my_queue = mq_open(MQ_ZPLANE, O_RDWR, 0, NULL))
		== (mqd_t)-1) {
		printf("\nZPLANE mq_open error");
		exit(ERROR);
	    }
	    break;
	  default:
	    return;
	};

	/* 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);
	    exit(OK);	    
	  }

	  /* 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 -------- */
/* WARNING - ERROR Handling                       */
/* Although all system calls are correctly tested */
/* for return status, the program will not recover*/
/* from an error return. Error handling in RT     */
/* is complicated, "a can of worms!"              */

STATUS robot (int argc, char **argv) 
{ 
	mqd_t mq_xplane, mq_yplane, mq_zplane; /* msg queue descriptors */
	mode_t mode=(mode_t)MODE; 		       /* msq mode, dummy */
	struct mq_attr ma;			           /* msq queue attributes */
	int xpid, ypid, zpid;
	char buf[DEFAULT_NBYTES];
	int coord_arrsize, x, y, z, i, mark_mq_error;

	/* set the required message queue attributes */
	ma.mq_flags = 0;
	ma.mq_maxmsg = 1;
	ma.mq_msgsize = nbytes;

	printf("\nRobot control initialization ...\n ");
	mark_mq_error = 0; /* set this flag if error */

	currPx = currPy = currPz = 0;
	newPx_reached = newPy_reached = newPz_reached = 0;
	
	/* 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);
	}

	/* Spawn the three controllers */
	if ((xpid = taskSpawn ("txcontr", CONTRTPRI, 0, 2000,
	    (FUNCPTR)controller, xplane, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) 
        { 
        	printf ("\ntaskSpawn of txcontr failed"); 
        	return (errno); 
        }
	if ((ypid = taskSpawn ("tycontr", CONTRTPRI, 0, 2000,
	    (FUNCPTR)controller, yplane, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) 
        { 
        	printf ("\ntaskSpawn of tycontr failed"); 
        	return (errno); 
        }
	if ((zpid = taskSpawn ("tzcontr", CONTRTPRI, 0, 2000,
	    (FUNCPTR)controller, zplane, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) 
        { 
        	printf ("\ntaskSpawn of tzcontr failed"); 
        	return (errno); 
        }

	#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("\nRobot 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[] */

	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

	if (mark_mq_error == 0)
	{
	  printf("\nRobot operation terminated OK\n\n");
	  return(OK);
	}
	else {
	  printf("\n- mq_send error marked, deleting controller tasks ...\n");
	/* force remove tasks after error */
	  taskDelete (xpid);
	  taskDelete (ypid);
	  taskDelete (zpid);
	  return(ERROR);
	}
}

/* here is where the move action is simulated */
/* simulate the duration of move with taskDelay(distance) */

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