/************************************************************************/
/* Copyright 2013 MBARI							*/
/************************************************************************/
/* Summary  : Menu-driven program to test snLib functions under Linux or Cygwin*/
/* Filename : snTest.c							*/
/* Author   : Robert Herlien (rah)					*/
/* Project  : xFOCE							*/
/* Revision: 1.0							*/
/* Created  : 05/28/2013						*/
/*									    */
/* 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:						*/
/* 28may2013 rah - created						*/
/************************************************************************/

#include <modbus.h>			/* libmodbus include		*/
#include <modbus-rtu.h>			/* libmodbus include		*/
#include <snTest.h>			/* defs for this module		*/
#include <sensorNode.h>			/* defs for SensorNode library	*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <ctype.h>
#include <getopt.h>
#include <termios.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  parmMax;		/* Max value of parm (0 to parmMax)	*/
} CommandStruct;		/****************************************/

typedef struct			/****************************************/
{				/* Struct to keep RemSerHandle		*/
    RemSerHandle rsh;		/* The serial handle (NULL if unopened)	*/
    int   baud;			/* Baud rate it's opened at		*/
    int   stop;			/* Stop bits				*/
    char  parity;		/* 'N', 'E', or 'O'			*/
} RemSerPort;			/****************************************/


#define READSIZE	20

#define NumberOf(arr)	(sizeof(arr) / sizeof(arr[0]))

/* Return codes from function execution		*/
#define CONTINUE 0
#define EXIT	(-2)


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

void	usage(char **argv);
void	rawMode(void);
int	menu(void);
CommandStruct *getCommand(int *parmp);
int	readSerial(SensorNodeHandle snh, int port);
int	writeSerial(SensorNodeHandle snh, int port);
int	enableSerial(SensorNodeHandle snh, int port);
int	disableSerial(SensorNodeHandle snh, int port);
int	iflushSerial(SensorNodeHandle snh, int port);
int	oflushSerial(SensorNodeHandle snh, int port);
int	serBreak(SensorNodeHandle snh, int port);
int	pwrOn(SensorNodeHandle snh, int port);
int	pwrOff(SensorNodeHandle snh, int port);
int	pwrStatus(SensorNodeHandle snh);
int	analogRead(SensorNodeHandle snh, int port);
int	analogAllRead(SensorNodeHandle snh);
int	analogParms(SensorNodeHandle snh, int port);
int	blinkyCmd(SensorNodeHandle, int parm);
int	serNum(SensorNodeHandle snh);
int	softwareRev(SensorNodeHandle snh);
int	configReg(SensorNodeHandle snh);
int	exitProg(void);
int	testCmd(SensorNodeHandle, int parm);


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

static MBool	snDebug = FALSE;
static int	slaveID = 1, baud = 115200;
static char	*modPort = "/dev/ttyS0";
static char	modPortBuf[16];
static char	inputBuf[256];

static RemSerPort	serPorts[SN_SER_PORTS];

static CommandStruct cmdTbl[] = {
    {"r", readSerial, 1, 2},    {"w", writeSerial, 1, 2},
    {"e", enableSerial, 1, 2},  {"d", disableSerial, 1, 2},
    {"if", iflushSerial, 1, 2}, {"of", oflushSerial, 1, 2},
    {"sb", serBreak, 1, 2},
    {"on", pwrOn, 1, 3},        {"off", pwrOff, 1, 3},
    {"p", pwrStatus, 0, 0},
    {"a8", analogAllRead, 0, 0},{"a", analogRead, 1, 7},
    {"ap", analogParms, 0, 0},
    {"b", blinkyCmd, 1, 2},
    {"sn", serNum, 0, 0}, {"sr", softwareRev, 0, 0}, 
    {"c", configReg, 0, 0}, 
    {"h", menu, 0, 0},
    {"x", exitProg, 0, 0},    {"q", exitProg, 0, 0},
    {"test", testCmd, 1, 6}
};


/************************************************************************/
/* Function    : main							*/
/* Purpose     : Main entry point					*/
/* Inputs      : argc, argv						*/
/* Outputs     : Application return code				*/
/************************************************************************/
int	main(int argc, char **argv)
{
    int			opt, parm;
    SensorNodeHandle	snh;
    CommandStruct	*cmdp;

    memset(serPorts, 0, sizeof(serPorts));

    while ((opt = getopt(argc, argv, "a:b:dhp:")) != -1)
	switch(opt)
	{
	  case 'a':
	      slaveID = atoi(optarg);
	      if ((slaveID < 1) || (slaveID > 247))
	      {
		  printf("Invalid slave ID: %d.  Setting to 1\n", slaveID);
		  slaveID = 1;
	      }
	      break;

	  case 'b':
	      baud = atoi(optarg);
	      break;

	  case 'd':
	      snDebug = TRUE;
	      break;

	  case 'p':
	      modPort = optarg;

	      /* Cygwin no longer likes COMn.  Translate to /dev/ttyS{n-1}	*/
	      if (strncasecmp(modPort, "COM", 3) == 0)
		  if (sscanf(modPort+3, "%d", &opt) > 0)
		  {
		      sprintf(modPortBuf, "/dev/ttyS%u", opt-1);
		      modPort = modPortBuf;
		  }
	      break;

	  case 'h':
	      usage(argv);
	      return(0);
	}

    printf("Testing Modbus port %s  baud %u  slave %u\n", modPort, baud, slaveID);

    initSensorNodeLib();
    snSetDebug(snDebug);

    if ((snh = openSensorNode(modPort, baud, slaveID)) == NULL)
    {
	printf("Failure in openSensorNode: errno %d %s\n", errno,
	       modbus_strerror(errno));
	exit(1);
    }

    rawMode();
    menu();

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

    return(0);

} /* main() */


/************************************************************************/
/* Function    : rawMode						*/
/* Purpose     : Put input into raw mode				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
void rawMode(void)
{
    struct termios	tio;

    tcgetattr(STDIN_FILENO, &tio);

    tio.c_lflag &= ~ICANON;
    tio.c_cc[VMIN] = 0;
    tio.c_cc[VTIME] = 0;

    tcsetattr(STDIN_FILENO, TCSANOW, &tio);

} /* rawMode() */


/************************************************************************/
/* Function    : usage							*/
/* Purpose     : Print usage message					*/
/* Inputs      : argv							*/
/* Outputs     : None							*/
/************************************************************************/
void usage(char **argv)
{
    printf("%s [-a <slaveAddr>] [-b <baud>] [-d] [-p <port>] [-h]\n\n",
	   argv[0]);
    printf("\t[-p <port>] Use serial port <port> to communicate with Modbus device\n");
    printf("\t\tDefaults to COM1\n");
    printf("\t[-a <slaveAddr>] Communicate with Modbus slave <slaveAddr>, 1-247\n");
    printf("\t\tDefaults to 1\n");
    printf("\t[-b <baud>] Set Modbus baud rate.  Defaults to 115200\n");
    printf("\t[-d] Print debug messages\n");
    printf("\t[-h] Print this message\n");

} /* usage() */


/************************************************************************/
/* Function    : menu							*/
/* Purpose     : Print the menu						*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
int menu(void)
{
    printf("\nType a command:\n\n");
    printf("h for this help message\n");
    printf("x to exit this program\n\n");
    printf("w[0-2] to write to a serial port\n");
    printf("r[0-2] to read from a serial port\n");
    printf("e[0-2] to enable a serial port\n");
    printf("d[0-2] to disable a serial port\n");
    printf("if[0-2] to flush serial input\n");
    printf("of[0-2] to flush serial output\n");
    printf("sb[0-2] to send a break to a serial port\n\n");

    printf("on[0-3] to turn on a power relay\n");
    printf("off[0-3] to turn off a power relay\n");
    printf("p to read the state of the power relays\n\n");

    printf("a[0-7] to read an analog port\n");
    printf("a8 to read all analog ports\n");
    printf("ap to set analog parameters\n\n");

    printf("sn to get board serial number\n");
    printf("sr to get board software revision number\n");
    printf("c to get board configuration register\n");

    printf("b[0-2] to set blinky LED (0=off, 1=on, 2=debug)\n\n");
    return(OK);

} /* menu() */


/************************************************************************/
/* Function    : getCommand						*/
/* Purpose     : Get a command						*/
/* Inputs      : Ptr to place to put parm				*/
/* Outputs     : Ptr to command to execute				*/
/************************************************************************/
CommandStruct  *getCommand(int *parmp)
{
    int		i, nr, nchars, nameSize;
    uint	parm;
    char	*p;
    CommandStruct *csp;

    printf("Enter a command:  ");
    fflush(stdout);
    memset(inputBuf, 0, sizeof(inputBuf));
    nchars = 0;
    p = inputBuf;

    while( nchars < sizeof(inputBuf) - 1 )
	if ((nr = read(STDIN_FILENO, p, 1)) > 0)
	{
	    if ((*p == '\n') || (*p == '\r') || (*p == 4))
		return(NULL);

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

	    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 ((nchars > nameSize) &&
			(sscanf(&inputBuf[nameSize], "%u", &parm) >= 1))
		    {
			if (parm <= csp->parmMax)
			{
			    *parmp = parm;
			    return(csp);
			}
			else
			{
			    printf("\nBad parameter: %u\n", parm);
			    return(NULL);
			}
		    }
		}
	    }
	}
	else
	    usleep(10000);

    return(NULL);
}


/************************************************************************/
/* Function    : exitProg						*/
/* Purpose     : Command function to exit the program			*/
/* Inputs      : None used						*/
/* Outputs     : EXIT return code					*/
/************************************************************************/
int	exitProg(void)
{
    return(EXIT);
}


/************************************************************************/
/* Function    : getSerHandle						*/
/* Purpose     : Get handle for remote serial port			*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : RemSerHandle, or NULL if error				*/
/************************************************************************/
RemSerHandle getSerHandle(SensorNodeHandle snh, int port)
{
    RemSerHandle rsh;

    if ((rsh = serPorts[port].rsh) == NULL)
    {
	if ((rsh = snOpenSerial(snh, port)) == NULL)
	{
	    printf("Failed snOpenSerial(), errno %d, %s\n",
		   errno, modbus_strerror(errno));
	    return(NULL);
	}

	if (snSerInit(rsh, 9600, 'N', 1) == ERROR)
	{
	    printf("Failed snSerInit(), errno %d, %s\n",
		   errno, modbus_strerror(errno));
	    return(NULL);
	}
	serPorts[port].rsh = rsh;
	serPorts[port].baud = 9600;
	serPorts[port].parity = 'N';
	serPorts[port].stop = 1;
    }

    //Prompt for new baud/stop/parity here
    //re-init if user wants a change

    printf("Using serial port %d at %d,%c,8,%d\n",
	   port, serPorts[port].baud, serPorts[port].parity,
	   serPorts[port].stop);

    return(rsh);
}


/************************************************************************/
/* Function    : readSerial						*/
/* Purpose     : Command function to read a serial port			*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	readSerial(SensorNodeHandle snh, int port)
{
    RemSerHandle rsh;
    uint32_t	nchars, i;

    if ((rsh = getSerHandle(snh, port)) == NULL)
	return(ERROR);

    printf("Reading serial port %d at 9600,N,8,1\n", port);
    printf("Type ^D on port %d to exit\n\n", port);

    while(TRUE)
    {
	if ((nchars = snSerRead(rsh, inputBuf, READSIZE)) > 0)
	{
	    if (snDebug && (nchars > 0))
		printf("Got %lu chars\n", (ulong)nchars);

	    write(STDOUT_FILENO, inputBuf, nchars);

	    for (i = 0; i < nchars; i++)
		if (inputBuf[i] == 4)
		    return(OK);
	}

	usleep(100000);    
//	sleep(1);
    }

    return(OK);
}


/************************************************************************/
/* Function    : getOneLine						*/
/* Purpose     : Internal getline()-like function			*/
/* Inputs      : buffer ptr, length					*/
/* Outputs     : Bytes read						*/
/* Comments    : Stops on CR, LF, or ^D					*/
/*               Inserts CR-LF at end of line				*/
/************************************************************************/
static int getOneLine(char *buf, size_t len)
{
    size_t	got;
    char	*p;

    for (p = buf, got = 0; got < len-3; )
    {
	if (read(STDIN_FILENO, p, 1) > 0)
	{
	    got++;

	    if (*p == 4)
		return(got);

	    if ((*p == '\r') || (*p == '\n'))
	    {
		*p++ = '\r';
		*p++ = '\n';
		*p = '\0';
		return(got+1);
	    }
	    p++;
	}
	else
	    usleep(100000);
    }

    *p++ = '\r';
    *p++ = '\n';
    *p = '\0';
    return(got+2);
}


/************************************************************************/
/* Function    : writeSerial						*/
/* Purpose     : Command function to write to a serial port		*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	writeSerial(SensorNodeHandle snh, int port)
{
    RemSerHandle rsh;
    int		nchars;

    if ((rsh = getSerHandle(snh, port)) == NULL)
	return(ERROR);

    printf("Writing serial port %d at 9600,N,8,1, line at a time\n", port);
    printf("^D to exit\n\n");

    memset(inputBuf, 0, sizeof(inputBuf));

    while(TRUE)
    {
	nchars = getOneLine(inputBuf, sizeof(inputBuf));

	if (strchr(inputBuf, 4) != NULL)
	    return(OK);

	while (nchars)
	    nchars -= snSerWrite(rsh, inputBuf, nchars);
    }

    return(OK);
}


/************************************************************************/
/* Function    : enableSerial						*/
/* Purpose     : Command function to write to a serial port		*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	enableSerial(SensorNodeHandle snh, int port)
{
    printf("Enabling serial port %d\n", port);
    return(OK);
}

/************************************************************************/
/* Function    : disableSerial						*/
/* Purpose     : Command function to write to a serial port		*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	disableSerial(SensorNodeHandle snh, int port)
{
    printf("Disabling serial port %d\n", port);
    return(OK);
}

int	iflushSerial(SensorNodeHandle snh, int port)
{
    printf("Flush serial input, port %d\n", port);
    return(OK);
}

int	oflushSerial(SensorNodeHandle snh, int port)
{
    printf("Flush serial output, port %d\n", port);
    return(OK);
}


/************************************************************************/
/* Function    : serBreak						*/
/* Purpose     : Command function to send a serial break		*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	serBreak(SensorNodeHandle snh, int port)
{
    printf("Serial break, port %d\n", port);
    return(OK);
}


/************************************************************************/
/* Function    : pwrOn							*/
/* Purpose     : Command function to turn on a power port		*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	pwrOn(SensorNodeHandle snh, int port)
{
    printf("Powering on port %d\n", port);
    snPower(snh, port, TRUE);
    return(OK);
}


/************************************************************************/
/* Function    : pwrOff							*/
/* Purpose     : Command function to turn on a power port		*/
/* Inputs      : SensorNodeHandle, Port Number				*/
/* Outputs     : Return code						*/
/************************************************************************/
int	pwrOff(SensorNodeHandle snh, int port)
{
    printf("Powering off port %d\n", port);
    snPower(snh, port, FALSE);
    return(OK);
}


/************************************************************************/
/* Function    : pwrStatus						*/
/* Purpose     : Command function to return power bit status		*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : Return code						*/
/************************************************************************/
int	pwrStatus(SensorNodeHandle snh)
{
    uint16_t	pwrVec;

    snGetPowerVector(snh, &pwrVec);
    printf("Power vector = 0x%x\n", (uint)pwrVec);
    return(OK);
}

/************************************************************************/
/* Function    : analogRead						*/
/* Purpose     : Command function to read analog channel		*/
/* Inputs      : SensorNodeHandle, analog port number			*/
/* Outputs     : Return code						*/
/************************************************************************/
int	analogRead(SensorNodeHandle snh, int port)
{
    uint16_t	cnt;

    if (snAnalogChan(snh, port, &cnt) < 0)
	printf("Error reading analog channel %d\n", port);
    else
	printf("Analog port %d returns %u counts, %.3f volts\n",
	       port, cnt, ((double)cnt * 2.5 / 65536.0));

    return(OK);
}

/************************************************************************/
/* Function    : analogAllRead						*/
/* Purpose     : Command function to read all 8 analog channels		*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : Return code						*/
/************************************************************************/
int	analogAllRead(SensorNodeHandle snh)
{
    uint16_t	cnt[8], i;

    if (snAnalogChans(snh, 0, 8, cnt) < 0)
	printf("Error reading analog channels\n");
    else
    {
	printf("Chan  Counts  Volts\n");
	for (i = 0; i < 8; i++)
	    printf("% d  %5u  %8.3f\n",
		   i, cnt[i], ((double)cnt[i] * 2.5 / 65536.0));
    }

    return(OK);
}


/************************************************************************/
/* Function    : analogParms						*/
/* Purpose     : Command function to set analog parms			*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : Return code						*/
/************************************************************************/
int	analogParms(SensorNodeHandle snh, int port)
{
    printf("Set parms for analog channel %d\n", port);
    return(OK);
}


/************************************************************************/
/* Function    : serNum							*/
/* Purpose     : Command function to get board serial number		*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : OK							*/
/************************************************************************/
int	serNum(SensorNodeHandle snh)
{
    uint16_t	serNum;

    snGetSerialNum(snh, &serNum);
    printf("Serial Number = %u\n", (uint)serNum);
    return(OK);
}


/************************************************************************/
/* Function    : softwareRev						*/
/* Purpose     : Command function to get board firmware revision	*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : OK							*/
/************************************************************************/
int	softwareRev(SensorNodeHandle snh)
{
    uint16_t	rev;

    snGetSoftwareRev(snh, &rev);
    printf("Software Rev = 0x%x\n", (uint)rev);
    return(OK);
}


/************************************************************************/
/* Function    : configReg						*/
/* Purpose     : Command function to get board configuration		*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : OK							*/
/************************************************************************/
int	configReg(SensorNodeHandle snh)
{
    uint32_t	cfg;

    snGetBoardConfig(snh, &cfg);
    printf("Board Config Reg = 0x%x\n", cfg);
    return(OK);
}


/************************************************************************/
/* Function    : blinkyCmd						*/
/* Purpose     : Command function to set blinky LED			*/
/* Inputs      : SensorNodeHandle, parm					*/
/* Outputs     : OK							*/
/************************************************************************/
int	blinkyCmd(SensorNodeHandle snh, int parm)
{
    snBlinky(snh, parm);
    return(OK);
}


/************************************************************************/
/* Function    : testCmd						*/
/* Purpose     : Command function to test parser			*/
/* Inputs      : SensorNodeHandle, parm					*/
/* Outputs     : OK							*/
/************************************************************************/
int	testCmd(SensorNodeHandle snh, int parm)
{
    printf("Test Command, parm = %d\n", parm);
    return(OK);
}