/************************************************************************/
/* Copyright 2015-2018 MBARI                                            */
/************************************************************************/
/* Summary  : BEDS2 Debug menu routine                                  */
/* Filename : bedsDiag.c												*/
/* Author   : Robert Herlien (rah)                                      */
/* Project  : BEDS2                                                     */
/* Revision: 1.0                                                        */
/* Created  : 03/09/2018 from o5debug.c (Oasis5)						*/
/*                                                                          */
/* 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:                                                */
/* 11feb2016 rah - created o5debug from mvdebug.c FOCE project			*/
/* 09mar2018 rah - created from o5debug.c on Oasis5						*/
/************************************************************************/

#include <mbariTypes.h>
#include <beds.h>
#include <bedsDiag.h>
#include <dig_io.h>
#include <rtc.h>
#include <clock.h>
#include <serial.h>
#include <picConfig.h>
#include <syslog.h>

#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <ctype.h>

typedef struct                  /****************************************/
{                               /* Struct to hold commands              */
    char  *name;                /* Command name                         */
    int   (*func)();            /* Function to execute                  */
    int   nparms;               /* Number of parms (0 or 1 only for now)*/
    UINT  parmMin;              /* Max value of parm (0 to parmMax)     */
    UINT  parmMax;              /* Max value of parm (0 to parmMax)     */
} CommandStruct;                /****************************************/


/********************************/
/*      Forward Declarations    */
/********************************/

int     printDebugMenu(void);
CommandStruct *getCommand(Nat32 *parmp);
int     bedsReset(void);
int     doSleep(Nat32 secs);
int     readRTC(void);
int     getTime(void);
int     setTime(void);
int     getTick(void);
int     readRTCReg(Nat32 reg);
int     writeRTCReg(Nat32 reg);
int     checkUsTmr(Nat32 parm);
int     checkMsTmr(Nat32 parm);
int     testCmd(Nat32 parm);
int     quitDiag(void);


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

MLocal char     inputBuf[256];

MLocal CommandStruct cmdTbl[] = {
    {"slp", doSleep, 1, 1, 99}, 
    {"res", bedsReset, 0, 0, 0}, 
    {"rr", readRTCReg, 1, 0, 19}, 
    {"rw", writeRTCReg, 1, 0, 19}, 
    {"rtc", readRTC, 0, 0, 0}, 
    {"time", getTime, 0, 0, 0}, 
    {"stime", setTime, 0, 0, 0}, 
    {"us", checkUsTmr, 1, 1, 99}, 
    {"ms", checkMsTmr, 1, 1, 99}, 
    {"tick", getTick, 0, 0, 0}, 
    {"h", printDebugMenu, 0, 0, 0},
    {"?", printDebugMenu, 0, 0, 0},
    {"q", quitDiag, 0, 0, 0},
    {"x", quitDiag, 0, 0, 0},
    {"test", testCmd, 1, 1, 250}
};


/************************************************************************/
/* Function	   : bedsDiagnostic											*/
/* Purpose	   : Hardware diagnostic routines for BEDS2					*/
/* Inputs	   : argc, argv												*/
/*				 drivers are spawned) or due to error.	pmask = 0-7 otherwise*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
CmdRtn	bedsDiagnostic(int argc, char **argv)
{
    Nat32                cmdParm;
    CommandStruct       *cmdp;

    printDebugMenu();

    while (TRUE)
    {
        cmdParm = 0;
        if ((cmdp = getCommand(&cmdParm)) != NULL)
        {
            printf("\r\n");
            if ((*cmdp->func)(cmdParm) == EXIT)
				return(OK);
        }
    }

	return(OK);

} /* o5Diagnostic() */


/************************************************************************/
/* Function    : printDebugMenu                                         */
/* Purpose     : Print the menu                                         */
/* Inputs      : None                                                   */
/* Outputs     : None                                                   */
/************************************************************************/
int printDebugMenu(void)
{
    printf("\r\nDEBUG MENU - Type a command:\r\n\n");
    printf("h for this help message\r\n");

    printf("slp[1-99] to sleep for number of seconds\r\n");
    printf("res to reset the CPU\r\n");
    printf("rtc to read date/time directly from the RTC\r\n");
    printf("time to read system time/date\r\n");
    printf("stime to set time/date into RTC\r\n");
    printf("rr to read one RTC register\r\n");
    printf("rw to write one RTC register\r\n");
	printf("us to test microsecond delay\r\n");
	printf("ms to test millisecond delay\r\n");
	printf("tick to get system ticker\r\n");
    return(OK);

} /* printDebugMenu() */


/********************************************************/
/*  Support routines for getCommand()                   */
/********************************************************/

/************************************************************************/
/* Function    :findCommand                                             */
/* Purpose     : Find a matching command								*/
/* Inputs      : Ptr to place to put parm								*/
/* Outputs     : Ptr to command to execute                              */
/************************************************************************/
MLocal CommandStruct  *findCommand(Nat32 *parmp)
{
    int         i, nameSize, rtn;
    Nat32        parm;
    CommandStruct *csp;

    for (i = 0, csp = cmdTbl; i < NumberOf(cmdTbl); i++, csp++)
    {
        nameSize = strlen(csp->name);

        if (strncasecmp(inputBuf, csp->name, nameSize) == 0)
        {
            if (csp->nparms == 0)
                return(csp);

			if ((sscanf(&inputBuf[nameSize], " %u", &parm) >= 1) && (parmp != NULL))
                *parmp = parm;
            
            return(csp);
        }
    }
    return(NULL);
}


/************************************************************************/
/* Function    : checkParm												*/
/* Purpose     : Range-check parm										*/
/* Inputs      : CommandStruct ptr, parm ptr							*/
/* Outputs     : CommandStruct ptr or NULL								*/
/************************************************************************/
MLocal CommandStruct *checkParm(CommandStruct *csp, Nat32 *parmp)
{
    if (csp->nparms == 0)
        return(csp);
    if (parmp == NULL)
        return(NULL);
    if ((*parmp >= csp->parmMin) && (*parmp <= csp->parmMax))
        return(csp);

    printf("\r\nBad parameter: %u\r\n", *parmp);
    return(NULL);
}

/* decDigits()      */
MLocal Nat32 decDigits(Nat32 n)
{
    Nat32 digits=1;

    while (n >= 10)
    {
        digits++;
        n /= 10;
    }

    return(digits);
}


/************************************************************************/
/* Function    : getCommand                                             */
/* Purpose     : Get a command                                          */
/* Inputs      : Serial port, ptr to place to put parm                  */
/* Outputs     : Ptr to command to execute                              */
/************************************************************************/
CommandStruct  *getCommand(Nat32 *parmp)
{
    int         nchars, nameSize, digits;
    char        *p, c;
    CommandStruct *csp;
	SerPort	port = getConsole();

    printf("\r\nEnter a command:  ");
    memset(inputBuf, 0, sizeof(inputBuf));
    nchars = 0;
    p = inputBuf;

    while( nchars < sizeof(inputBuf) - 1 )
        if ((c = ser_getc(port)) != ERROR)
        {
            ser_putc(port, c);

			*p = c;
            if ((c == '\n') || (c == '\r'))
            {
                *p = '\0';
                if ((csp = findCommand(parmp)) == NULL)
                    return(NULL);
                return(checkParm(csp, parmp));
            }

            nchars++;
            p++;
            *p = '\0';

            if ((csp = findCommand(parmp)) != NULL)
            {
                if (csp->nparms == 0)
                    return(csp);

                nameSize = strlen(csp->name);
                digits = decDigits(csp->parmMax);

                if (nchars >= nameSize + digits)
                    return(checkParm(csp, parmp));
                
                if ((csp->parmMax < 20) && (nchars >= nameSize+1) && (*parmp >= 2))
                    return(checkParm(csp, parmp));
            }
        }
	
    return(NULL);
}

/****************************************/
/* Helper functions                     */
/****************************************/

/************************************************************************/
/* Function    : getline                                                */
/* Purpose     : Get one line of user input                             */
/* Inputs      : Serial port number, buffer pointer, buf length         */
/* Outputs     : Length of returned line                                */
/************************************************************************/
int getline(int port, char *buf, int buflen)
{
    int         len, b;

    for (len = 0; len < buflen-1; )
    {
        if ((b = ser_getc(port)) != ERROR)
        {
            ser_putc(port, (Byte)b);
            buf[len++] = (Byte)b;

            if (b == '\r')
            {
                ser_putc(0, (Byte)'\n');
                buf[len] = '\0';
                return(len);
            }
        }
        else
			clkDelayMs(10);
    }

    buf[buflen] = '\0';
    return(buflen);
}


/********************************************************/
/* Functions to implement menu commands                 */
/********************************************************/

/************************************************************************/
/* Function    : bedsReset												*/
/* Purpose     : Reset the CPU											*/
/* Inputs      : None													*/
/* Outputs     : OK, but should never get there							*/
/************************************************************************/
int bedsReset(void)
{
	sysLog("Reset from diagnostic mode");
	picSoftReset();
    return(OK);
}


/************************************************************************/
/* Function    : doSleep                                                */
/* Purpose     : Command function to go to low-power sleep mode         */
/* Inputs      : Serial port, Seconds to sleep                          */
/* Outputs     : OK                                                     */
/************************************************************************/
int doSleep(Nat32 secs)
{
    printf("Going to sleep for %u seconds\r\n", secs);
    serWaitTxComplete(getConsole());

	digSerDisbl();
	digSerShdn();

    clkSleep(secs);
	digSerEnbl();
	digSerNoShdn();

    return(OK);
}


/************************************************************************/
/* Function    : readRTC                                                */
/* Purpose     : Command function to read RTC registers                 */
/* Inputs      : None													*/
/* Outputs     : OK                                                     */
/************************************************************************/
int readRTC(void)
{
    int         i, rtn;
    Byte        regs[16];

    if ((rtn = rtcReadRegs(RTC_SEC, 16, regs)) != SUCCESS)
    {
        printf("Error reading RTC regs, errno = %d\r\n", rtn);
    }

    for (i = 0; i < 16; i++)
    {
        printf("%02x ", (Nat32)(regs[i] & 0xff));
    }

    printf("\r\n\n%02d:%02d:%02d  %04u/%02d/%02d\r\n",
            (int)fromBCD((Nat16)regs[RTC_HOUR]), 
            (int)fromBCD((Nat16)regs[RTC_MIN]), 
            (int)fromBCD((Nat16)regs[RTC_SEC]), 
            (int)fromBCD((Nat16)regs[RTC_YEAR]) + 2000, 
            (int)fromBCD((Nat16)(regs[RTC_MON] & 0x1f)),
            (int)fromBCD((Nat16)regs[RTC_MDAY]));


    printf("rtcTime() returns %u\r\n", rtcTime(NULL));
	clkPrintTime(clkTime());
	newline();

    return(OK);
}

/************************************************************************/
/* Function    : getTime                                                */
/* Purpose     : Command function to get system time/date               */
/* Inputs      : None													*/
/* Outputs     : OK                                                     */
/************************************************************************/
int getTime(void)
{
    time_t      now;
    Nat16      nowMs;

    clkGetTime(&now, &nowMs);
    clkPrintTime(now);
    printf(".%03u\r\n", nowMs);

    printf("rtcTime() returns %u\r\n", rtcTime(NULL));
}


/************************************************************************/
/* Function    : getTick                                                */
/* Purpose     : Command function to get tick count                     */
/* Inputs      : None													*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     getTick(void)
{
    printf("Tick count = %ld\r\n", clkGetTick());
}


/************************************************************************/
/* Function    : setTime                                                */
/* Purpose     : Command function to set time/date into RTC             */
/* Inputs      : None													*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     setTime(void)
{
    Nat32        n, rtn, year, mon, day, hr, min, sec;
    Byte        regs[8];

    printf("Enter new time/date as either:\r\n");
    printf("\tyyyy/mm/dd hh:mm:ss    or\r\n");
    printf("\thh:mm:ss    or\r\n");
    printf("\thh:mm\r\n");

    if (getline(getConsole(), inputBuf, sizeof(inputBuf)) < 5)
        return(OK);

    n = sscanf(inputBuf, " %u/%u/%u %u:%u:%u", &year, &mon, &day, &hr, &min, &sec);
    if (n >= 5)
    {
        if (n == 5)
            sec = 0;
        else
            regs[0] = (Byte)toBCD(sec % 60);
        regs[1] = (Byte)toBCD(min % 60);
        regs[2] = (Byte)toBCD(hr % 24);
        regs[3] = 1;
        regs[4] = (Byte)toBCD(day % 31);
        regs[5] = (Byte)toBCD(mon % 12);
        regs[6] = (Byte)toBCD(year % 100);

        if ((rtn = rtcWriteRegs(RTC_SEC, 7, regs)) != SUCCESS)
        {
            printf("Error writing 7 RTC regs, errno = %d\r\n", rtn);
        }

        return(OK);
    }

    sec = 0;
    n = sscanf(inputBuf, " %u:%u:%u", &hr, &min, &sec);
    if (n >= 2)
    {
        regs[0] = (Byte)toBCD(sec % 60);
        regs[1] = (Byte)toBCD(min % 60);
        regs[2] = (Byte)toBCD(hr % 24);

        if ((rtn = rtcWriteRegs(RTC_SEC, 3, regs)) != SUCCESS)
            printf("Error writing 3 RTC regs, errno = %d\r\n", rtn);
        else
            printf("Setting time to %02d:%02d:%02d\r\n",
                    hr, min, sec);

        return(OK);
    }

    return(OK);
}


/************************************************************************/
/* Function    : setSysTime                                             */
/* Purpose     : Command function to set system date/time from RTC      */
/* Inputs      : None													*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     setSysTime(void)
{
#if 0
    time_t curTod = rtcTime(NULL);

    clkSetTime(curTod);
    clkPrintTime(curTod);
    printf("  %u\r\n", curTod);
#endif
}


/************************************************************************/
/* Function    : readRTCReg                                             */
/* Purpose     : Command function to read one RTC register              */
/* Inputs      : Reg number												*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     readRTCReg(Nat32 reg)
{
    Byte        rdat;
    Errno       rtn;

    if ((rtn = rtcReadRegs(reg, 1, &rdat)) != SUCCESS)
        printf("Error reading RTC Reg %u, errno %d\r\n",
                reg, rtn);
    else
        printf("RTC register %u = 0x%x\r\n",
                reg, (Nat32)rdat);
    
    return(OK);
}

/************************************************************************/
/* Function    : writeRTCReg                                            */
/* Purpose     : Command function to write one RTC register             */
/* Inputs      : Reg number												*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     writeRTCReg(Nat32 reg)
{
    Byte        wdat;
    Errno       rtn;

    printf("Enter value, in hex, to write to register %u:  ", reg);

    if (getline(getConsole(), inputBuf, sizeof(inputBuf)) < 1)
        return(OK);

	newline();
    wdat = strtoul(inputBuf, NULL, 16) & 0xff;

    printf("Writing 0x%x to register %u\r\n", (Nat32)wdat, reg);

    rtn = rtcWriteRegs(reg, 1, &wdat);

    printf("Return code was %d\r\n", rtn);

    return(OK);
}


/************************************************************************/
/* Function    : testCmd                                                */
/* Purpose     : Command function to test parser                        */
/* Inputs      : Parm													*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     testCmd(Nat32 parm)
{
	double	f = 2.0;

    printf("Test Command, parm = %u, digits = %u, f = %f\r\n", parm, decDigits(parm), f);
	serWaitTxComplete(getConsole());
    return(OK);
}

/************************************************************************/
/* Function    : checkUsTmr                                             */
/* Purpose     : Command function show timing of microsecond timer      */
/* Inputs      : Usecs to delay											*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     checkUsTmr(Nat32 parm)
{
    while(serRxCount(getConsole()) == 0)
    {
		digTest2Toggle();
        clkDelayUs(parm);
    }
}

/************************************************************************/
/* Function    : checkMsTmr                                             */
/* Purpose     : Command function show timing of millisecond timer      */
/* Inputs      : msecs to delay											*/
/* Outputs     : OK                                                     */
/************************************************************************/
int     checkMsTmr(Nat32 parm)
{
    while(serRxCount(getConsole()) == 0)
	{
		digTest2Toggle();
        clkDelayMs(parm);
	}
}


/************************************************************************/
/* Function    : quitDiag												*/
/* Purpose     : Exit the diagnostic									*/
/* Inputs      : None													*/
/* Outputs     : EXIT													*/
/************************************************************************/
int     quitDiag(void)
{
    return(EXIT);
}
