/**************************************************************************
**                                                                        *
**  FILE        :  mvp.c                                                  *
**                                                                        *
**  DESCRIPTION :  Program to control Buoyancy Engine and logging		  *
**                 instruments on MBARI Vertical Profiler                 *
**                                                                        *
**  COPYRIGHT   :  2000 Monterey Bay Aquarium Research Institute          *
**                                                                        *
**	REVISION HISTORY:													  *
**		1.0.0	Initial release (prm)									  *
**		1.1.0	Added yoyo command (prm)								  *
**		1.2.0	Added motor brake (prm)									  *
**		1.3.0	Added motor soft-start (prm)							  *
**		1.3.1	Improved accuracy of pot position to percent calculation. *
**				Added second argument to yo-yo command (prm)              *
**		1.4.0	Added interrupt sampling of pot position				  *
**		1.5.0	Moved all motor position checking to interrupt routine	  *
**																		  *
**************************************************************************/
#ifndef PRECOMPHDRS 
#include	<TT8.h>			/* Tattletale Model 8 Definitions */
#include	<tat332.h>		/* 68332 Tattletale (7,8) Hardware Definitions */
#include	<sim332.h>		/* 68332 System Integration Module Definitions */
#include	<qsm332.h>		/* 68332 Queued Serial Module Definitions */
#include	<tpu332.h>		/* 68332 Time Processing Unit Definitions */
#include	<dio332.h>		/* 68332 Digital I/O Port Pin Definitions */
#include	<tt8pic.h>		/* Model 8 PIC Parallel Slave Port Definitions */

#include	<tt8lib.h>		/* definitions and prototypes for Model 8 library */
#include	<userio.h>		/* common convenient user I/O routines */

#include	<assert.h>
#include	<ctype.h>
#include	<errno.h>
#include	<float.h>
#include	<limits.h>
#include	<locale.h>
#include	<math.h>
#include	<setjmp.h>
#include	<signal.h>
#include	<stdarg.h>
#include	<stddef.h>
#include	<stdio.h>
#include	<stdlib.h>
#include	<string.h>
#include	<time.h>
#endif 
 
//#include	"mvp.h"
#include	"pwm.h"

#define VERSION "1.5.0"
#define MXLINE  80			/* max command line length */
#define MXARGS	2			/* max number of command line arguments */
#define TRUE    1
#define FALSE   0
#define ON		1
#define OFF		0
#define FORWARD	1
#define REVERSE	0
#define NUMCOMMANDS (sizeof(commands)/sizeof(commands[0]))
#define CMD_BUF_SIZE    132
#define NO_VAL	32767		/* indicates no value for command argument */
#define PROMPT  "\nMVP> "
#define BS		0x08		/* backspace char */
#define MTR_CUR	0			/* A/D chan for motor current */
#define POT_POS	1			/* A/D chan for pot position */
#define MTR_PWM	15			/* TPU chan for motor pwm */
#define PWM_PER	400			/* PWM period, 250 ns per count for 16 MHz clk */
#define PWM_STEP	10		/* number of PWM counts to increase each 100 ms */
#define NUMSAMPS	16		/* number of AtoD samples to average */
#define LOLIM	614			/* lower limit of engine travel on 0-4095 scale */
#define UPLIM	3481		/* upper limit of engine travel on 0-4095 scale */
#define RANGE	(UPLIM-LOLIM)	/* total range of allowed motion */
#define HALF_PCT	RANGE/200	/* 0.5% of total range */
#define FIVE_PCT	RANGE/20	/* 5% of total range */
#define DELAY 2				/* delay time for periodic interrupt (102.4 msec) */




/* Define error conditions for buoyancy engine */
typedef enum {
	kNone,
	kCurrentTrip,
	kOperatorAbort,
	kPotError
} BEFault;

/* Define the routines that do the work for each command */
typedef enum {
	kEmpty,
	kUnknown,
	kSetEngPos,
	kReadEngPos,
	kSetModeYoyo,
	kSetClkTime,
	kReadClkTime,
	kExitToMon,
	kHelp
} CmdNum;

typedef struct {
	char    *theCommand;
	char    *theExplanation;
	CmdNum  theNumber;
} ACommand;

ACommand commands[] = {
	"SEP", "[0..100]         Set engine position", kSetEngPos,
	"REP", "                 Read engine position", kReadEngPos,
	"SMY", "[0..99][1..100]  Set mode yo-yo (hit any key to abort)", kSetModeYoyo,
	"SCT", "                 Set clock time", kSetClkTime,
	"RCT", "                 Read clock time", kReadClkTime,
	"ETM", "                 Exit to TOM8 monitor", kExitToMon,
	"HELP", NULL, kHelp,
	"?", NULL, kHelp,
};
short   cmdPointer, baseCommand;
char    cmdBuffer[CMD_BUF_SIZE+2];
char    waiting, loopMode = FALSE;
short   cmdArg[MXARGS];

short	gDesPosition;		/* destination position of buoyancy engine */
short	gActPosition;		/* actual position of buoyancy engine */
short	gDelta;				/* difference between actual and desired position */
short	gSign;				/* sign of gDelta */
short	gRunning;			/* TRUE if motor running */
short	gPWMHi;				/* PWM duty cycle */
BEFault	gFault;				/* buoyancy engine fault */
struct	tm	gTime;			/* date and time */
static int 	TimeOut;

const char wday[7][4] = {
	"Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"
};

const char month[12][4] = {
	"Jan", "Feb", "Mar", "Apr", "May", "Jun",
	"Jul", "Aug", "Sep", "Oct", "Nov", "Dec"
};

const short n_mdays[12] = {
	31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
};


/* function prototypes */
void Init(void);
void SetEngPos(short);
void ReadEngPos(void);
short Cnt2Pct(short);
short Pct2Cnt(short);
void SetModeYoyo(short, short);
void SetClkTime(void);
void ReadClkTime(void);
int QueryTime(struct tm*);
void SndInstr(int, char *);
int RcvInstr(int, char *, int);
void Help(void);
void EnaDCDrive(void);
void DisDCDrive(void);
void SoftStart(short *);
short AtoDReadAvg(short, short);
void GetLine(char *, int);
void DoCommand(void);
short ParseCommand(void);
void MakeWordUpper(char *);
void InitPeriodicTimerInterrupt(void);
void SetPeriodicTimer(int delay);
void ServicePeriodicTimerInterrupt(void);

void main(void)
{
	Init();							/* set up hardware and program variables */

	printf("\n\nMVP Controller version %s\n", VERSION);

	while(TRUE) {                   /* loop forever */
		printf(PROMPT);
		kbflush();                  /* clear command buffer */
		GetLine(cmdBuffer, CMD_BUF_SIZE);	/* get next command */
		do {
			DoCommand();            /* go do it */
			waiting = loopMode;
			while(waiting);        	/* pause if in loop mode */
		} while(!kbhit() && loopMode);  /* exit if any key is hit */
		loopMode = FALSE;
	} /* end while */

} /* end main */


/* Initialize hardware and program variables */
void Init(void) {

	InitTT8(NO_WATCHDOG, TT8_TPU);	/* setup Model 8 for running C programs */
	
	PConfOutp(E, 1);				/* config direction pin (Port E bit 1) as output */
	PConfOutp(E, 2);				/* config brake pin (Port E bit 2) as output */
	DisDCDrive();					/* disable the motor driver */
	PClear(E, 2);					/* apply motor brake */
	TPUSetPin(MTR_PWM, OFF);		/* config mtr enable pin (TPU bit 15) as output, and turn off */
	TPUSetupPWM(MTR_PWM, 0, PWM_PER, MiddlePrior); /* start pwm output at 0%*/
	InitPeriodicTimerInterrupt();	/* start periodic interrupt timer */ 
	SetPeriodicTimer(DELAY);		/* set PITR delay value */

	
} /* end function Init() */


/* Get a command line into the command buffer */
void GetLine(char s[], int lim)
{
	char c;
    int i;
    
    i = 0;
    while(--lim > 0 && (c = getchar()) && c != '\n') {
	    if(c == '\x08' || c == '\x10') {	/* if backspace or delete */
	    	if(i > 0) {				/* if not beginning of buffer */
            	i--;				/* throw previous char away */
			} else {
            	putchar('\x20');	/* print a space */
            } /* end if */
	        lim++;
		} else {
	    	s[i++] = c;				/* put char in buffer */
        } /* end if */
    } /* end while */
    s[i] = '\0';					/* mark end of line */
    printf("\n");					/* echo end of line */

} /* end function GetLine() */


/* Read a command from the command buffer and execute it */
void DoCommand(void)
{
	short   cmdNum;
	
	MakeWordUpper(cmdBuffer);
	cmdNum = ParseCommand();
	switch(cmdNum) {
		case kEmpty:
			break;

		case kUnknown:
			printf("Unknown command.");
			loopMode = FALSE;
			break;

		case kHelp:
			Help();
			break;

		case kSetEngPos:
			SetEngPos(cmdArg[0]);
            break;

		case kReadEngPos:
			ReadEngPos();
			break;
			
		case kSetModeYoyo:
			SetModeYoyo(cmdArg[0], cmdArg[1]);
			break;

		case kSetClkTime:
			SetClkTime();
			break;
			
		case kReadClkTime:
			ReadClkTime();
			break;
			
		case kExitToMon:
			ResetToMon();			/* Exit program */
			break;

		default:
			loopMode = FALSE;
			printf("Unrecognized command");
			
	} /* end switch */

} /* end function DoCommand */


/* Set the engine position, given a position on a 0-100 percent scale
	note: the engine position (actPosition) is read in an interrupt service routine */
void SetEngPos(short desPosition)
{
	short	pctPosition;
	
	gDesPosition = desPosition;
	if(gDesPosition >= 0 && gDesPosition <= 100) {
		gFault = kNone;						/* clear any existing faults */
		gPWMHi = 0;
		TPUChangePWM(MTR_PWM, gPWMHi, PWM_PER); /* set PWM to start value */
		PSet(E, 2);							/* release brake */
		DelayMilliSecs(50L);				/* wait for brake to release */
		EnaDCDrive();						/* power up the motor driver */
		DelayMilliSecs(1L);					/* wait for motor power transient to die */
		gDesPosition = Pct2Cnt(gDesPosition);	/* convert percent to count */
		gDelta = gDesPosition - gActPosition;	/* diff is desired pos minus actual pos */
		gSign = gDelta / abs(gDelta);
		if(gDelta > 0) {					/* EXTEND buoyancy engine */
			PSet(E, 1);						/* set direction forward */
		} else {							/* RETRACT buoyancy engine */
			PClear(E, 1);					/* set direction reverse */
		} /* end if */

		gRunning = TRUE;					/* start the motor running */
		while(gRunning);					/* wait until it stops */
		
		pctPosition = Cnt2Pct(gActPosition); /* convert count to percent */
		if(gFault == kCurrentTrip) printf("OVERCURRENT TRIP at %d percent\n", pctPosition);
		if(gFault == kOperatorAbort) printf("OPERATOR ABORT at %d percent\n", pctPosition);
		if(gFault == kPotError) printf("POSITION POT ERROR at %d percent\n", pctPosition);
	} else {
		printf("Position must be in range 0 to 100\n");
	} /* end if */

} /* end function SetEngPos() */


/* Read the engine position */
void ReadEngPos(void)
{
	short	potPosition;
	
	potPosition = Cnt2Pct(gActPosition); 	/* convert count to percent */
	printf("Engine position is %d percent\n", potPosition);
	
} /* end function ReadEngPos() */


/* Convert engine position count to percent extension
	Engine lower position limit (LOLIM) is 0%, upper limit (UPLIM) is 100% */
short Cnt2Pct(short count)
{
	return(((long)count + HALF_PCT - LOLIM) * 100 / RANGE); /* convert count to percent */

} /* end function Cnt2Pct() */


/* Convert engine percent extension to position count
	Engine lower position limit (LOLIM) is 0%, upper limit (UPLIM) is 100% */
short Pct2Cnt(short percent)
{
	return((long)percent * RANGE / 100 + LOLIM);	/* convert percent to count */

} /* end function Pct2Cnt() */


/* Go into yo-yo mode, cyling back and forth from given lower limit to upper limit */
void SetModeYoyo(short llim, short ulim)
{
	short	cycles = 0;
	
	if(llim >= 0 && llim <= 100 && ulim >= 1 && ulim <= 100) {
		while(TRUE)	{							/* do until operator keyboard abort */
			SetEngPos(ulim);					/* set engine to upper limit */
			if(gFault != kNone) break;
			DelayMilliSecs(1000L);				/* pause for a second */
			SetEngPos(llim);					/* set engine to lower limit */
			if(gFault != kNone) break;
			printf("%d yo-yo cycles completed\n", ++cycles);
			DelayMilliSecs(1000L);				/* wait for motor stop */
		} /* end while */
	} else {
		printf("Lower limit must be in range 0 to 99\nUpper limit must be in range 1 to 100\n");
	} /* end if */
	
} /* end function SetModeYoyo() */


/* Set the real-time clock */
void SetClkTime(void)
{
	struct	tm datetime;			/* date and time */
	
	if(!QueryTime(&datetime)) {
		printf("Clock not set\n");
		return;
	} /* end if */
	SetTimeTM(&datetime, NULL);

} /* end function SetClkTime() */


/* Read the real-time clock */
void ReadClkTime(void)
{
	struct tm* now;
	time_tt cTime;        

	cTime = ttmnow();				/* get the current time */

	now = localtime(&(cTime.secs)); /* convert it to a time structure */

	if(now->tm_year < 70) now->tm_year += 2000;
	else now->tm_year += 1900;    
	printf ("%s %s %2d %d @ %02d:%02d:%02d\n",  wday[now->tm_wday],
												month[now->tm_mon],
												now->tm_mday,
												now->tm_year,
												now->tm_hour,
												now->tm_min,
												now->tm_sec);
		
} /* end function ReadClkTime() */


/* Query the date and time from the user.
	Returns TRUE if date and time are okay, FALSE otherwise */
int QueryTime(struct tm* tm)
{
	int n, illegal;
	char buffer[64];
	
	printf ("Please use Universal Coordinated Time, NOT local time.\n");
	printf ("The time set operation will time out after 30 seconds of inactivity.\n");
	printf ("Hit <return> to abort the time set operation\n");
	printf ("Enter date and time (MM/DD/YY HH:MM:SS) ");
	fflush (stdout);
	
	//  wait no more than 30 seconds between input characters before
	//  timing out
	StopWatchStart();
	n = 0;
//	memset(buffer, 0, sizeof(buffer));		// clear buffer
	while((StopWatchTime() / 1e6) < 30)  {
		if(SerByteAvail()) {				// if char available
			StopWatchStart();
			buffer[n] = SerGetByte();		// put it in buffer
			if(buffer[n] != BS)  {			// if not backspace
				putchar(buffer[n]);			//	put echo char in screen buffer
				if(buffer[n] == '\r')		// if carriage return
					break;					//	we're done
				if(n < (sizeof(buffer)-1))	// if buffer isn't full
					n++;					//	point to next buffer position
				else
					break;  
			} else {						// char is a backspace
				if(n > 0) {					// if buffer not empty
					putchar (buffer[n]);	//	echo backspace
					buffer[n] = 0;			//	and delete last char
					n--;
				} /* end if */
			} /* end if */
			fflush (stdout);				// print echo char to screen 
		} /* end if */
	} /* end while */

	if((StopWatchTime() / 1e6) > 30) {
		printf("Timed out due to inactivity.\n");
		return FALSE;
	} /* end if */      

	buffer[n] = 0;							// terminate string
	putchar('\n');							// put carriage return in screen buffer
	fflush(stdout);							//	and print it
	
	//  we reached this point either because of a timeout or
	//  because the user hit a carriage return
	n = sscanf (buffer, "%d/%d/%d %d:%d:%d", &tm->tm_mon, &tm->tm_mday, &tm->tm_year,
												&tm->tm_hour, &tm->tm_min, &tm->tm_sec);
	if(n != 6) {
		printf ("Date not accepted - entry format is month/day/year hour:min:sec\n");
		return FALSE;
	} // end if
	//  correct month for zero-based reference used by computer
	tm->tm_mon--;

	//  allow multiple formats for the year
	if(tm->tm_year > 1970)
		tm->tm_year -= 1900;
	if((tm->tm_year < 70) && (tm->tm_year >= 0))
		tm->tm_year += 100;
		
	// verify that the entered date and time are legal
	illegal = FALSE;
	if((tm->tm_mon < 0) || (tm->tm_mon >= 12))
		illegal = TRUE;
	if((tm->tm_mday <= 0) || (tm->tm_mday > n_mdays[tm->tm_mon]))  {
		if(((tm->tm_year%4) != 0) || (tm->tm_mon != 1) || (tm->tm_mday != 29))
			illegal = TRUE;
	}       
	if((tm->tm_year < 0) || (tm->tm_year > 206))
		illegal = TRUE;
	if((tm->tm_hour < 0) || (tm->tm_hour > 23))
		illegal = TRUE;
	if((tm->tm_min < 0) || (tm->tm_min > 59))
		illegal = TRUE;
	if((tm->tm_sec < 0) || (tm->tm_sec > 59))
		illegal = TRUE;
	if(illegal) {
		printf ("Date not accepted - illegal date entered\n");
		return FALSE;
	}
	return TRUE;
} /* end function QueryTime() */


/* Write to the TPU serial output to an instrument */
void SndInstr(int instr, char *str)
{

	int n = 0;
	
	while(str[n] != 0) {
		TSerPutByte(instr, str[n++]);
	} /* end while */
	
} /* end function SndInstr() */


/* Read the TPU serial input from an instrument, with a given timeout */
int RcvInstr(int instr, char *buffer, int timeout)
{

	int n = 0;
	
	StopWatchStart();
	while((StopWatchTime() / 1e6) < timeout)  {
		if(TSerByteAvail(instr)) {			// if char available
			StopWatchStart();				// reset stopwatch
			buffer[n] = TSerGetByte(instr);		// put it in buffer
			putchar(buffer[n]);				//	put echo char in screen buffer
			if(buffer[n] == '\r')			// if carriage return
				break;						//	we're done
			if(n < (sizeof(buffer) - 1))	// if buffer isn't full
				n++;						//	point to next buffer position
			else
				break;  
			fflush (stdout);				// print echo char to screen 
		} /* end if */
	} /* end while */

	if((StopWatchTime() / 1e6) > timeout) {
		printf("No reply from instrument#%d\n", instr);
		return FALSE;
	} /* end if */      

	buffer[n] = 0;							// terminate string
	putchar('\n');							// put carriage return in screen buffer
	fflush(stdout);							//	and print it
	return TRUE;

} /* end function RcvInstr() */


/* Enable (power on) the DC Drive module */
void EnaDCDrive(void)
{

	PConfOutp(E, 0);				/* config Port E run/sense pin as output */
	PSet(E, 0);						/* set run/sense hi */
	DelayMilliSecs(2L);				/* delay for 2 ms to latch run/sense */
	PConfInp(E, 0);					/* config run/sense pin as input */

} /* end function EnaDCDrive() */


/* Disable (power off) the DC Drive module */
void DisDCDrive(void)
{

	PConfOutp(E, 0);				/* config Port E run/sense pin as output */
	PClear(E, 0);					/* set run/sense low */

} /* end function DisDCDrive() */


/* Soft-start the motor by ramping up the PWM signal each 10 ms until it reaches 100% */
void SoftStart(short *pwmHi)
{

	ulong now;
	static ulong then;

	now = TensMilliSecs();			/* get current time */
	if(now > then) {				/* have 10 ms passed? */
		*pwmHi += PWM_STEP;			/* increase PWM duty cycle */
		if(*pwmHi > PWM_PER) *pwmHi = PWM_PER; /* limit max value */
		TPUChangePWM(MTR_PWM, *pwmHi, PWM_PER); /* set new value */
		then = now;					/* save current time */
	} /* end if */

} /* end function SoftStart() */


/* Print the help message */
void Help(void)
{
	short	i;
	
	printf("Commands are:\n");
	for(i = 0; i < NUMCOMMANDS; i++) {
		printf(commands[i].theCommand);
		if(commands[i].theExplanation) {
			printf(commands[i].theExplanation);
		} /* end if */
		printf("\n");
	} /* end for */
	
} /* end function Help() */


/* Read the A/D chan numSamps times and average the results */
short AtoDReadAvg(short chan, short numSamps)
{
	unsigned short i, sum = 0;

	if(numSamps > 16) numSamps = 16;		/* limit max samples to prevent math overflow */
	for(i = 0; i < numSamps; i++) {
		sum += AtoDReadWord(chan) / 8;		/* right-justify 12-bit result, and sum */
	} /* end for */
	return sum / numSamps;

} /* end function AtoDReadAvg() */


/* Parse the string in the command buffer */
short ParseCommand(void)
{
	short   i, j, cmdLen;
	char    *p;
	short   returnValue;
	
	returnValue = kUnknown;
	for(i = 0; i < MXARGS; i++) {
		cmdArg[i] = NO_VAL;				/* set to invalid value */
    } /* end for */

	if(!cmdBuffer[0]) {
		returnValue = kEmpty;            /* Empty buffer */
	} else {
		for(i = j = 0; i < NUMCOMMANDS; i++) {
			cmdLen = strlen(commands[i].theCommand);
			if(strncmp(cmdBuffer, commands[i].theCommand, cmdLen) == 0) {
				returnValue = commands[i].theNumber;
				p = cmdBuffer + cmdLen;
				while(*p && j < MXARGS) {
					while(*p && isspace(*p)) p++; /* skip spaces before arg */
					sscanf(p, "%d", &cmdArg[j++]); /* read the argument */
					while(*p && !isspace(*p)) p++; /* skip to next arg */
                } /* end while */
				if(*(p-1) == '*') loopMode = TRUE;
				break;
			} /* end if */
		} /* end for */
	} /* end if */

	return returnValue;

} /* end function ParseCommand */


/* Convert a string to upper case */
void MakeWordUpper(char *p)
{
	for( ; *p && !isspace(*p); p++) *p = toupper(*p);
} /* end function MakeWordUpper() */


/* Set up the Periodic Timer interrupt */
void InitPeriodicTimerInterrupt(void)
{ 
	static ExcCFrame	esf;

	*PITR = 0;	/* set count to zero - disable timer */
	InstallHandler(ServicePeriodicTimerInterrupt, PIT_INT_VECTOR, &esf);
} /* end function InitPeriodicTimerInterrupt() */


/* Set the Periodic Interrupt Timer delay
	delay is in range 0-255
	period = (delay * 512 * 4) / 40,000 for TT8
	min is 51.2 msec, max is 13.056 sec
	if delay = 0 then stopped
	bit 8 turns on divide-by-512 prescaler */
void SetPeriodicTimer(int delay)
{
	*PITR = delay | 0x0100;	/* set count and enable interrupt */
} /* end function SetPeriodicTimer() */


/*	Service the Periodic Timer interrupt*/
void ServicePeriodicTimerInterrupt(void)
{ 
	if(TimeOut > 0) TimeOut--;
	gActPosition = AtoDReadAvg(POT_POS, NUMSAMPS);	/* read pot position */

	if(gRunning) {						/* if motor is running */
		if(gPWMHi < PWM_PER) {			/* if not at full speed */
			gPWMHi += PWM_STEP;			/*  increase PWM duty cycle */
			if(gPWMHi > PWM_PER) gPWMHi = PWM_PER; /* limit max value */
			TPUChangePWM(MTR_PWM, gPWMHi, PWM_PER); /* set new value */
		} /* end if */
		
		if(!Pin(E, 0)) gFault = kCurrentTrip;	/* check for overcurrent trip */
		if(kbhit()) gFault = kOperatorAbort;	/* check for operator abort */
		if(gActPosition > UPLIM + FIVE_PCT || gActPosition < LOLIM - FIVE_PCT) {
			gFault = kPotError;			/* check for position pot error */
		} /* end if */

		if(gDelta * gSign < 0 || gFault) {	/* if destination reached or fault occurred */
			DisDCDrive();				/* power down the motor driver */
			PClear(E, 2);				/* apply brake */
			gRunning = FALSE;
		} else {
			gDelta = gDesPosition - gActPosition;	/* update position */
		} /* end if */
	} /* end if */

} /* end function ServicePeriodicTimerInterrupt() */