/****************************************************************************/
/* Copyright 2004-2016 MBARI												*/
/****************************************************************************/
/* Summary	: Diagnostic routines to debug OASIS5 software					*/
/* Filename : swdiag.c														*/
/* Author	: Robert Herlien (rah)											*/
/* Project	: OASIS5 Mooring/Instrument Controller							*/
/* $Revision$															*/
/* Created	: 08/09/2016 rah												*/
/*																			*/
/* MBARI provides this documentation and code "as is", with no warranty,	*/
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium	*/
/* Research Institute to assist in its use, correction, modification, or	*/
/* enhancement. This information should not be published or distributed to	*/
/* third parties without specific written permission from MBARI.			*/
/*																			*/
/****************************************************************************/
/* Modification History:													*/
/* 11aug2004 rah - created OASIS3 version from OASIS usrcmds.c				*/
/* 09aug2016 rah - created OASIS5 version from OASIS3 swdiag.c				*/
/* $Log$
 */
/****************************************************************************/

#include <mbariTypes.h>					/* MBARI type definitions			*/
#include <mbariConst.h>					/* MBARI constants					*/
#include <oasis.h>						/* OASIS controller definitions		*/
#include <debug.h>
#include <dig_io.h>						/* OASIS I/O definitions			*/
#include <olist.h>						/* OASIS linked list definitions	*/
#include <serial.h>						/* OASIS serial I/O definitions		*/
#include <sem.h>						/* OASIS semaphore definitions		*/
#include <otask.h>						/* OASIS task dispatcher			*/
#include <malloc.h>						/* OASIS malloc routines			*/
#include <log.h>						/* OASIS data logging definitions	*/
#include <drvr.h>						/* OASIS driver functions			*/
#include <modbus.h>						/* For Modbus testing				*/
#include <modbusio.h>					/* For Modbus testing				*/
#include <remote.h>						/* Remote I/O library				*/
#include <swdiag.h>						/* Software diagnostic func protos	*/
#include <utils.h>						/* OASIS Utility routines			*/
#include <timer.h>						/* OASIS Clock module				*/
#include <usrRemoteCmds.h>

#include <FreeRTOS.h>					/* FreeRTOS definitions				*/
#include <task.h>						/* FreeRTOS/include task.h			*/
#include <istdio.h>						/* Standard I/O						*/
#include <time.h>						/* Standard time defs				*/


/********************************/
/*		External Data			*/
/********************************/

Extern Nat32	modbusDebug;
Extern Errno	modErrno;
Extern Parm_t	con_dsc;				/* Disconnect character for CONNECT */
Extern Parm_t	con_tmout;				/* Timeout during CONNECT			*/
Extern Parm_t	brkchr;					/* Character that sends break		*/
Extern Nat32	pwr_perm;				/* Devices to leave powered on		*/

#define BUFSIZE			2048
#define REM_BREAK_TIME	1000			/* Length of serial break in ms		*/


/********************************/
/*		Module Local Data		*/
/********************************/

MLocal BaseType_t	testThreadNum = 0;
MLocal Semaphore	testSem;
MLocal MBool		frozen = FALSE, semCreated = FALSE;

typedef struct
{
	SerDesc		serD;
	Parm_t		parm2;
	Parm_t		loops;
} TaskTestStruct;


/************************************************************************/
/* Function	   : printTaskState											*/
/* Purpose	   : Print the state of a task								*/
/* Inputs	   : TaskHandle_t											*/
/* Outputs	   : None													*/
/************************************************************************/
void printTaskState(TaskHandle_t td)
{
	TaskState		tstate;
	SemID			parm;
	TickType_t		ticks;

	switch(eTaskGetState(td))
	{
	  case eReady:
		  xputs("Ready\n");
		  break;

	  case eRunning:
		  xputs("Running\n");
		  break;

	  case eBlocked:
		  tstate = (TaskState)pvTaskGetThreadLocalStoragePointer(td, State);
		  parm = pvTaskGetThreadLocalStoragePointer(td, StateParm);

		  if (tstate == PENDING)
		  {
			  xprintf("Pending on %s\n",
					  (strlen(parm->sem_name) == 0) ? "semaphore" : parm->sem_name);
		  }
		  else if (tstate == DELAY)
		  {
			  ticks = (TickType_t)parm - xTaskGetTickCount();
			  xprintf("Delayed %u.%02u sec\n",
					  ticks/TICKS_PER_SEC,
					  (ticks % TICKS_PER_SEC)/(TICKS_PER_SEC/100));
		  }
		  else
			  xputs("Blocked\n");
		  break;

	  case eSuspended:
		  xputs("Suspended\n");
		  break;

	  case eDeleted:
		  xputs("Deleted\n");
		  break;
	}
}


#ifdef DEBUG_SYSTEM_CMD

/************************************************************************/
/* Function	   : showTaskList											*/
/* Purpose	   : Show List of tasks										*/
/* Inputs	   : Buffer													*/
/* Outputs	   : None													*/
/************************************************************************/
MLocal void showTaskList(char *buf)
{
	TaskHandle_t	td;
	Nat32			i, nTasks;
	TaskStatus_t	*stsp;

	stsp = (TaskStatus_t *)buf;
	nTasks = uxTaskGetSystemState(stsp, BUFSIZE/sizeof(TaskStatus_t), NULL);

	for (i = 0; i < nTasks; i++)
	{
		xprintf("%-12s addr %x  taskNum %u  prio %u  ",
				stsp[i].pcTaskName, stsp[i].xHandle,
				stsp[i].xTaskNumber, stsp[i].uxCurrentPriority);
		printTaskState(stsp[i].xHandle);

		xprintf("             Stack Base %x  Stack High Water %u\n",
				stsp[i].pxStackBase, (Nat32)stsp[i].usStackHighWaterMark);
	}

	xprintf("\n");

} /* showTaskList() */


/************************************************************************/
/* Function	   : showSemList											*/
/* Purpose	   : Show List of all semaphores in system					*/
/* Outputs	   : None													*/
/************************************************************************/
MLocal void showSemList(char *buf)
{
	Reg Node	*np;
	LstHead		*semList;
	Reg SemID	sem;
	TaskHandle_t owner;
	Nat32		len;

	len = 0;
	semList = semGetSemList();
	lockList(semList);

	len = isprintf(buf, "%d Semaphores:           Name    Owner\n",
				   list_count(semList));

	for (np = list_first(semList); np != NULLNODE; np = list_next(np))
	{
		sem = (SemID)np;
		len += isprintf(buf + len, "    0x%x %16s ", sem, sem->sem_name);

		owner = xSemaphoreGetMutexHolder(sem->semHandle);
		
		if (owner != NULL)
			len += isprintf(buf + len, "%s", pcTaskGetName(owner));

		strcat(buf, "\n");
		len++;
	}

	unlockList(semList);
	xprintf("%s\n", buf);

} /* showSemList() */


/************************************************************************/
/* Function	   : showSystem												*/
/* Purpose	   : Show state of system: dispatcher, etc					*/
/* Inputs	   : None													*/
/* Outputs	   : OK														*/
/************************************************************************/
CmdRtn showSystem(ParmMask_t pmask, Parm_t skipSem)
{
	char	*buf;

	if ((buf = pmalloc(BUFSIZE)) == NULL)
		return(ERROR);

	xprintf("Tick = %u  ", xTaskGetTickCount());
	memCheck();
	xputs("\n");

	showTaskList(buf);

	if (!((pmask & 1) && skipSem))
		showSemList(buf);

	free(buf);
	return(OK);
 
} /* showSystem() */

#endif /* DEBUG_SYSTEM_CMD */


/************************************************************************/
/* Function	   : testThread												*/
/* Purpose	   : Test thread to put msgs out to serial port				*/
/* Inputs	   : TaskTestStruct											*/
/* Outputs	   : None													*/
/************************************************************************/
MLocal void testThread(void *parm)
{
	TaskTestStruct *tsp;
	int			loop;
	Port_t		serPort;
	time_t		secs;
	Nat16		ms;
	struct tm	*tmptr;
	char		*name;

	tsp = (TaskTestStruct *)parm;
	vTaskSetThreadLocalStoragePointer(NULL, ConsoleP, &tsp->serD);
	serPort = tsp->serD.serport;
	name = pcTaskGetName(NULL);

	if (isPhysSer(serPort))
	{
		getSerialSem(serPort);				/* Get serial sem			*/
		pwrBitOn(serPort);					/* Enables serial relay		*/
		digSerEnable(serPort);				/* Enables serial Tx		*/
		serPortInit(serPort, 9600, tsp->serD.sermode);
	}
	else
	{
		if (remOpenVirtSer(serPort, FALSE) != OK)
			return;

		remVirtSerInit(serPort, 9600, tsp->serD.sermode);

		remPower(serPort, TRUE);			/* Turn on device				*/
	}
	

	xprintf("%s(%u, %u, %u) create\n", name,
			serPort, tsp->parm2, tsp->loops);

	for (loop = 0; loop < tsp->loops; loop++)
	{
		clkGetTime(&secs, &ms);
		tmptr = gmtime(&secs);

		xprintf("%s loop %u at %02u:%02u:%02u.%03u\n",
				name, loop, tmptr-> tm_hour,
				tmptr->tm_min, tmptr->tm_sec, ms);

		xdrain_ser(10);
		delay_secs(tsp->parm2);
	}

	xprintf("%s(%u, %u, %u) exit\n", name,
			serPort, tsp->parm2, tsp->loops);

	if (isPhysSer(serPort))
	{
		releaseSerialSem(serPort);			/* Release serial sem		*/
		digSerDisable(serPort);				/* Disable serial Tx		*/
		serPortInit(serPort, 9600, tsp->serD.sermode);
	}
	else
		remCloseVirtSer(serPort);			/* Close remote serial		*/
	
	if ((pwr_perm & (1L<<serPort)) == 0)
		pwrBitOff(serPort);					/* Release serial relay		*/

	free(tsp);

} /* testThread() */


/************************************************************************/
/* Function	   : startTestThread										*/
/* Purpose	   : UserIF routine to start a test thread to put msgs to ser*/
/* Inputs	   : Parm Mask, serial port, delay, number of loops			*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
CmdRtn startTestThread(ParmMask_t pmask, Parm_t serPort, Parm_t delay, Parm_t loops)
{
	TaskTestStruct		*ts;
	char				*name;

	if ((pmask & 1) != 1)
	{
		xputs("Usage: TestTask <serPort> [delay_secs] [loops]\n");
		return(ERROR);
	}

	name = pmalloc(sizeof(TaskTestStruct) + 32);
	ts = (TaskTestStruct *)name;
	name += sizeof(TaskTestStruct);

	isnprintf(name, 32, "testThread%u", testThreadNum++);
	ts->serD.serport = serPort;
	ts->serD.sermode = (NO_PTY | BIT8 | STOP1 | FLOW | AUTOCR);
	ts->parm2 = (pmask & 2) ? delay : 10;
	ts->loops = (pmask & 4) ? loops : 20;

	task_create(testThread, ts, name, MISC_STACK_WORDS, DRVR_PRIO);
	return(OK);

} /* startTestThread() */


/************************************************************************/
/* Function	   : testDelay												*/
/* Purpose	   : Test task_delay()										*/
/* Inputs	   : O3 ticks												*/
/* Outputs	   : None													*/
/************************************************************************/
CmdRtn testDelay(ParmMask_t pmask, Parm_t ticks)
{
	TickType_t	stTick, elapsed;

	if ((pmask & 1) != 1)
	{
		xputs("Usage: delay <ticks>\n");
		return(ERROR);
	}

	stTick = xTaskGetTickCount();
	task_delay(ticks);
	elapsed = xTaskGetTickCount() - stTick;

	xprintf("Elapsed time = %u.%03u secs\n",
			elapsed/TICKS_PER_SEC, (elapsed%TICKS_PER_SEC)*MS_PER_TICK);

	return(OK);

} /* testDelay() */


/************************************************************************/
/* Function	   : turnOffCoils											*/
/* Purpose	   : Turn off coils on all slaves in test					*/
/* Inputs	   : ModbusHandle, first slave, number of slaves			*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
MLocal void turnOffCoils(ModBusHandle mh, RemID *rp, Nat16 nslaves,
						 Nat16 firstCoil, Nat16 nCoils)
{
	Nat16		firstSlave, slave;
	Byte		coils = 0;

	firstSlave = rp->slaveID;

	for (slave = firstSlave; slave < (firstSlave + nslaves); slave++)
	{
		rp->slaveID = slave;
		writeCoils(mh, rp, firstCoil, nCoils, &coils);
	}

	rp->slaveID = firstSlave;

} /* turnOffCoils() */


/************************************************************************/
/* Function	   : modbusTest												*/
/* Purpose	   : Modbus Test											*/
/* Inputs	   : Parm Mask, serial port (bus), slave ID, number of slaves, mode*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
CmdRtn modbusTest(ParmMask_t pmask, Parm_t bus, Parm_t testSlave, Parm_t numSlaves)
{
	Nat32		loop;
	Nat16		firstSlave, slave, nslaves;
	Nat16		firstCoil, nCoils;
	Errno		ret;
	ModBusHandle mh;
	Byte		coil1, coilRead, coilWrite;
	Nat32		errs = 0;
	ModBusType	busType;
	RemID		*rp, remID;

	if ((pmask & 1) != 1)
	{
		xputs("Usage: ModTest <serPort>, [slave], [nslaves]\n");
		return(ERROR);
	}

	firstSlave = (pmask & 2) ? testSlave : 1;
	nslaves = (pmask & 4) ? numSlaves : 1;

	if (bus <= 0)
	{
		busType = MB_SPI;
		firstSlave = 1;
		nslaves = 1;
		if (bus < 0)
			bus = 1;
		firstCoil = TxEnable(0);
		nCoils = 6;
	}
	else
	{
		busType = MB_SER;
		firstCoil = 1;
		nCoils = 5;
	}

	if ((rp = findRemoteID(busType, bus, firstSlave)) == NULL)
	{
		xprintf("Can't find slave %u on bus %u\n", firstSlave, bus);
		return(ERROR);
	}

	memcpy(&remID, rp, sizeof(RemID));

	if ((mh = modBusOpen(rp)) == NULL)
	{
		xprintf("Error in modBusOpen()\n");
		return(ERROR);
	}

	xprintf("ModTest(%s bus %u, %u, %u)\n", busTypeStr(busType), bus,
			firstSlave, nslaves);

	task_delay(TICKS_PER_SEC/2);
	xflush_ser(TICKS_PER_SEC/10);

	turnOffCoils(mh, &remID, nslaves, firstCoil, nCoils);

	for (loop = 0; xRxAvail() == 0; loop++)
	{
		for (slave = firstSlave; slave < (firstSlave+nslaves); slave++)
		{
			remID.slaveID = slave;
			if ((ret = readCoils(mh, &remID, firstCoil, nCoils, &coilRead)) != OK)
			{
				clkPrintTime(NULL);
				xprintf(" Error in readCoils(): slave %u loop %u rtn = 0x%x (%s)\n",
						slave, loop, ret, modBusErrString(ret));
				errs++;
			}
			else if (modbusDebug >= 3)
				xprintf("readCoils() loop %u return value = 0x%x, coils = 0x%x\n", 
						loop, ret, coilRead);

			if (slave == firstSlave)
				coil1 = coilRead;
			coilWrite = coilRead + 1;
			
			if ((ret = writeCoils(mh, &remID, firstCoil, nCoils, &coilWrite)) != OK)
			{
				clkPrintTime(NULL);
				xprintf(" Error in writeCoils(): slave %u loop %u rtn = 0x%x (%s)\n",
						slave, loop, ret, modBusErrString(ret));
				errs++;
			}
			else if (modbusDebug >= 3)
				xprintf("writeCoils() loop %u return value = 0x%x\n", loop, ret);

			if (coilRead != coil1)
			{
				xprintf("Coils out of sync: ID %u = %02x, ID %u = %02x\n",
						firstSlave, coil1, slave, coilRead);
				xprintf("Clearing all coil\n");
				remID.slaveID = firstSlave;
				turnOffCoils(mh, &remID, nslaves, firstCoil, nCoils);
				break;
			}
		}

		if ((modbusDebug >= 1) && (loop%20 == 0))
		{
			clkPrintTime(NULL);
			xprintf(" Loop %u, %u errors\n", loop, errs);
		}

		task_delay(TICKS_PER_SEC/20);
	}

	xprintf("ModTest(%s bus %u, %u, %u) done, %u loops, %u errors\n",
			busTypeStr(busType), bus, firstSlave, nslaves, loop, errs);

	modBusClose(mh);
	return(OK);

} /* modbusTest() */


/************************************************************************/
/* Function	   : remSerConnect											*/
/* Purpose	   : Open a connection to remote serial port				*/
/* Inputs	   : Remote Serial Handle									*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
CmdRtn remSerConnect(RemSerHandle rsh)
{
	Nat32		i, n, rtn;
	Nat32		startTick;
#define REMBUFSIZE 32
	char		buffer[REMBUFSIZE];

	startTick = tmrGetTicks();

	while ((tmrGetTicks() - startTick) < con_tmout*TICKS_PER_SEC)
	{
		if ((n = xgetn_tmout(buffer, REMBUFSIZE, 0)) > 0)
		{
			startTick = tmrGetTicks();			/* Reset timeout		*/

			for (i = 0; i < n; i++)
			{
				if (buffer[i] == con_dsc)
					return(OK);
				else if (buffer[i] == brkchr)
				{
					remSerBreak(rsh, REM_BREAK_TIME);
					buffer[i] = '\0';
				}
			}

			if ((rtn = remSerWrite(rsh, buffer, n)) != n)
			{
				xprintf("remSerWrite() error: request %u bytes, wrote %u, modErrno = %d (%s)\n",
						n, rtn, modErrno, modBusErrString(modErrno));
				remSerClose(rsh);
				return(ERROR);
			}
		}

		if ((n = remSerRead(rsh, buffer, REMBUFSIZE)) > 0)
		{
			startTick = tmrGetTicks();			/* Reset timeout		*/
			xputn(buffer, n);
		}

		task_delay(TICKS_PER_SEC/50);			/* Run loop at 50 Hz   */

	} /* for */

	xputs("TIMEOUT\n");
	return(OK);

} /* remSerConnect() */


/************************************************************************/
/* Function	   : remConnect												*/
/* Purpose	   : Send test string to remote serial port					*/
/* Inputs	   : Parm Mask, serial port, slave ID, number of slaves, mode*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
CmdRtn remConnect(ParmMask_t pmask, Parm_t bus, Parm_t slave, Parm_t port)
{
	RemSerHandle rsh;
	ModBusType	busType;
	Errno		rtn;

	if ((pmask & 7) != 7)
	{
		xprintf("remCon <bus> <slave> <port> (bus 0 for DBs)\n");
		return(ERROR);
	}

	if (bus == 0)
	{
		busType = MB_SPI;
		slave = 1;
		bus = (port < UARTS_PER_DB) ? 0 : 1;
		if (bus == 1)
			port -= UARTS_PER_DB;
	}
	else
		busType = MB_SER;

	xprintf("Connect to remote serial port on %s bus %u slave %u port %u\n",
			busTypeStr(busType), bus, slave, port);
	
	if ((rsh = findRemSerHandle(busType, bus, slave, port)) == NULL)
	{
		xprintf("Error finding remote serial port: modErrno = %d (%s)\n",
				modErrno, modBusErrString(modErrno));
		return(modErrno);
	}

	if (usrSemWait(&rsh->sem) != OK)
		return(OK);

	if ((rtn = openRemSerHandle(rsh, TRUE)) != OK)
	{
		xprintf("Error opening remote serial port: errno = %d (%s)\n",
				rtn, modBusErrString(rtn));
		return(rtn);
	}

	xputs("Connected\n");
	rtn = remSerConnect(rsh);
	remSerClose(rsh);
	return(rtn);

} /* remConnect() */


/************************************************************************/
/* Function	   : taskKillTest											*/
/* Purpose	   : Test task creation and killing task that owns semaphore*/
/* Inputs	   : None													*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
MLocal void taskKillTestTask(void *parm)
{
	SemID			sem;
	TaskHandle_t	td;
	char			*tname;

	sem = (SemID)parm;
	td = xTaskGetCurrentTaskHandle();
	tname = pcTaskGetName(td);

	xprintf("Task 0x%x (%s) alive\n", td, tname);
	xputs("Prior to malloc(16384)  ");
	memCheck();

	pmalloc(16384);
	xprintf("After malloc()  ");
	memCheck();

	xprintf("Task %s about to take SemID 0x%x (%s)\n",
			tname, sem, sem->sem_name);

	task_delay(TICKS_PER_SEC);

	semTake(sem);
	semTake(sem);

	task_delay(300 * TICKS_PER_SEC);

} /* taskKillTestTask() */


#ifdef DEBUG_KILL

/************************************************************************/
/* Function	   : taskKillTest											*/
/* Purpose	   : Test task creation and killing task that owns semaphore*/
/* Inputs	   : None													*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Int16 taskKillTest(Void)
{
	static char 	*killTaskName = "killTestTask";
	TaskHandle_t	*testTd, *tdLookup;

	if (!semCreated)
	{
		semCreateMutex(&testSem);
		semName(&testSem, "TestSem");
		semCreated = TRUE;
	}

	testTd = task_create(taskKillTestTask, &testSem, killTaskName,
						 MISC_STACK_WORDS, DRVR_PRIO);

	task_delay(TICKS_PER_SEC);

	if ((tdLookup = xTaskGetHandle(killTaskName)) != testTd)
	{
		xprintf("task_find error: %s created at 0x%x, xTaskGetHandle() returns 0x%x\n",
				killTaskName, testTd, tdLookup);
	}

	xprintf("Created semaphore 0x%x (%s) and task 0x%x (%s).\n",
			&testSem, testSem.sem_name, testTd, pcTaskGetName(testTd));
	xdrain_ser(TICKS_PER_SEC/4);
	xputs("Hit any key to continue\n");
	xgetc();

	xprintf("Killing 0x%x.  Sem owner is 0x%x\n",
			testTd, xSemaphoreGetMutexHolder(testSem.semHandle));

	taskKill(testTd);						/* Kill task				*/
//	vTaskDelete(testTd);
	task_delay(TICKS_PER_SEC);

	xprintf("Sem owner 0x%x\n", xSemaphoreGetMutexHolder(testSem.semHandle));
	xprintf("After taskKill  ");
	memCheck();

	return(OK);

} /* taskKilltest() */

#endif /* DEBUG_KILL */


/************************************************************************/
/* Function	   : usrEcho												*/
/* Purpose	   : Test command-line args									*/
/* Inputs	   : Parm Mask, string, string, string, string				*/
/* Outputs	   : OK														*/
/************************************************************************/
CmdRtn usrEcho(ParmMask_t pmask, Parm_t s1, Parm_t s2, char *s3, char *s4)
{
	if (pmask & 1)
		xprintf("Arg 1: %u\n", s1);

	if (pmask & 2)
		xprintf("Arg 2: %u\n", s2);

	if (pmask & 4)
		xprintf("Arg 3: %s\n", s3);

	if (pmask & 8)
		xprintf("Arg 4: %s\n", s4);

	return(OK);

} /* usrEcho() */


/************************************************************************/
/* Function	   : echoAll												*/
/* Purpose	   : Test command-line args									*/
/* Inputs	   : Parm Mask, string, string, string, string				*/
/* Outputs	   : OK														*/
/************************************************************************/
CmdRtn echoAll(ParmMask_t pmask, char *s1, char *s2, char *s3, char *s4)
{
	if (pmask & 1)
		xprintf("Arg 1: %s\n", s1);

	if (pmask & 2)
		xprintf("Arg 2: %s\n", s2);

	if (pmask & 4)
		xprintf("Arg 3: %s\n", s3);

	if (pmask & 8)
		xprintf("Arg 4: %s\n", s4);

	return(OK);

} /* echoAll() */