/************************************************************************/
/* Copyright 2009-2012 MBARI						*/
/************************************************************************/
/* Summary  : Remote serial port routines using SensorNode		*/
/* Filename : remSerial.c						*/
/* Author   : Robert Herlien (rah)					*/
/* Project  : xFOCE							*/
/* Revision: 1.0							*/
/* Created  : 09/19/2012 from Oasis4 remote.c				*/
/*									    */
/* 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:						*/
/* 19sep2012 rah - created from remote.c in Oasis4			*/
/************************************************************************/

#include <mbariTypes.h>			/* MBARI type definitions	*/
#include <mbariConst.h>			/* MBARI constants		*/
#include <modbus.h>			/* libmodbus include		*/
#include <modbus-rtu.h>			/* libmodbus include		*/
#include <remSerial.h>			/* defs for this module		*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>

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


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

MLocal SensorNodeStruct	sNode[SENSOR_NODES];

MLocal MBool		debug = FALSE;


/************************************************************************/
/* Function    : initRemoteLib						*/
/* Purpose     : Initialize this module					*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
Void initRemoteLib(Void)
{
    memset(sNode, 0, sizeof(sNode));

} /* initRemoteLib() */


Void	remLibSetDebug(MBool onoff)
{
    debug = onoff;
}


/************************************************************************/
/* Function    : openSensorNode						*/
/* Purpose     : Establish ModBus connection to SensorNode		*/
/* Inputs      : Device name, baud rate, slave ID			*/
/* Outputs     : SensorNodeHandle, or NULL if failed			*/
/************************************************************************/
SensorNodeHandle openSensorNode(char *dev, int baud, int slaveID)
{
    SensorNodeStruct	*snp;
    int		i;

    if ((slaveID <= 0) || (slaveID > 247))
    {
	errno = EINVAL;
	return(NULL);
    }

    for (i = 0; i < NumberOf(sNode); i++)
	if ((sNode[i].useCount > 0) && (sNode[i].slaveID == slaveID) &&
	    strcmp(dev, sNode[i].devName))
	{
	    sNode[i].useCount++;
	    return(&sNode[i]);
	}

    for (i = 0; i < NumberOf(sNode); i++)
	if (!sNode[i].useCount == 0)
	{
	    snp = &sNode[i];
	    if ((snp->ctx = modbus_new_rtu(dev, baud, 'N', 8, 1)) == NULL)
	    {
		if (debug)
		    fprintf(stderr, "Failed modbus_new_rtu(): %s\n",
			    modbus_strerror(errno));
		return(NULL);
	    }

	    if (modbus_set_slave(snp->ctx, slaveID) < 0)
	    {
		if (debug)
		    fprintf(stderr, "Failed modbus_set_slave(): %s\n",
			    modbus_strerror(errno));
		modbus_free(snp->ctx);
	    }

	    if (modbus_connect(snp->ctx) < 0)
	    {
		if (debug)
		    fprintf(stderr, "Failed modbus_connect(): %s\n",
			    modbus_strerror(errno));
		modbus_free(snp->ctx);
		return(NULL);
	    }

	    snp->useCount = 1;
	    strncpy(snp->devName, dev, DEVNAME_LEN);
	    return(snp);
	}

    return(NULL);
	
} /* openSensorConnection() */


/************************************************************************/
/* Function    : closeSensorNode					*/
/* Purpose     : Close ModBus connection to SensorNode			*/
/* Inputs      : SensorNodeHandle					*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
int	closeSensorNode(SensorNodeHandle snh)
{
    if (snh == NULL)
	return(ERROR);

    if (--snh->useCount == 0)
	modbus_close(snh->ctx);

    return(OK);

} /* closeSensorConnection() */


/************************************************************************/
/* Function    : openRemSerial						*/
/* Purpose     : Open a remote serial port				*/
/* Inputs      : SensorNodeHandle, serial port number			*/
/* Outputs     : RemSerHandle for opened port, or NULL if failed	*/
/************************************************************************/
RemSerHandle	openRemSerial(SensorNodeHandle snh, int port)
{
    RemSerPort	*rsp;
    int		rtn;

    if ((snh == NULL) || (port < 0) || (port >= SN_SER_PORTS))
    {
	errno = EINVAL;
	return(NULL);
    }

    if ((rsp = malloc(sizeof(RemSerPort))) == NULL)
    {
	if (debug)
	    fprintf(stderr, "Cannot malloc RemSerPort struct\n");
	errno = ENOMEM;
	return(NULL);
    }

    rsp->snp = snh;
    rsp->ctx = snh->ctx;
    rsp->port = port;
    rsp->bufPtr = rsp->rbuf;
    rsp->bufChars = 0;
    rsp->rxQueued = 0;

    if ((rtn = modbus_write_bit(snh->ctx, TxEnable(port), TRUE)) < 0)
    {
	if (debug)
	    fprintf(stderr, "Failed TxEnable on %s port %d:  %s\n",
		    snh->devName, port, modbus_strerror(errno));
	
	closeRemSerial(rsp);
	return(NULL);
    }

    return(rsp);

} /* openRemSerial() */


/************************************************************************/
/* Function    : closeRemSerial						*/
/* Purpose     : Close remote serial port				*/
/* Inputs      : RemSerHandle						*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
int	closeRemSerial(RemSerHandle rsh)
{
    if ((rsh == NULL) || (rsh->ctx == NULL))
	return(ERROR);

    modbus_write_bit(rsh->ctx, TxEnable(rsh->port), FALSE);
    free(rsh);
    return(OK);

} /* closeRemSerial() */


/************************************************************************/
/* Function    : remSerInit						*/
/* Purpose     : Set baud rate and mode for a remote serial port	*/
/* Inputs      : RemSerHandle, baud rate, parity ('N','E', or 'O'),	*/
/*		 stop bits (1 or 2)					*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
int remSerInit(RemSerHandle rsh, int baud, char parity, int stop)
{
    Nat16	regs[2];

    if ((rsh == NULL) || (rsh->ctx == NULL))
	return(ERROR);

    regs[0] = baud >> 2;

    switch(parity)
    {
      case 'O':
      case 'o':
	  regs[1] = REM_ODD_PTY;
	  break;

      case 'E':
      case 'e':
	  regs[1] = REM_EVEN_PTY;
	  break;
	
      default:
	  regs[1] = REM_NO_PTY;
	  break;
    }

    if (stop == 2)
	regs[1] |= REM_STOP2;

    return(modbus_write_registers(rsh->ctx, BaudReg(rsh->port), 2, regs));

} /* remSerInit() */


/************************************************************************/
/* Function    : remSerRead						*/
/* Purpose     : Read a remote serial port				*/
/* Inputs      : Remote serial handle, buffer ptr, length		*/
/* Outputs     : Number of chars read					*/
/************************************************************************/
Nat32 remSerRead(RemSerHandle rsh, char *buffer, Nat32 len)
{
    Nat32	bytesRead;
    char	*p;
    Nat16	readLen;
    int		rtn;

    if ((rsh == NULL) || (rsh->snp == NULL))
	return(0);

    for (p = buffer, bytesRead = 0; bytesRead < len; )
    {
	if (rsh->bufChars > 0)
	{
	    *p++ = *(rsh->bufPtr)++;
	    rsh->bufChars--;
	    bytesRead++;
	}
	else
	{
	    readLen = (len+1)/2;
	    if (readLen > SERREG_SIZE)
		readLen = SERREG_SIZE;
	    if (readLen < MIN_READ)
		readLen = MIN_READ;

	    rtn = modbus_read_registers(rsh->ctx, RxQReg(rsh->port),
				       readLen+1, &rsh->rxQueued);
	    if ((rtn != OK) || (rsh->rxQueued == 0))
		return(bytesRead);
	    rsh->bufPtr = rsh->rbuf;
	    rsh->bufChars = (rsh->rxQueued >= 2*readLen) ? 
				2*readLen : rsh->rxQueued;
	}
    }

    return(bytesRead);

} /* remSerRead() */


/************************************************************************/
/* Function    : remSerWrite						*/
/* Purpose     : Write a remote serial port				*/
/* Inputs      : Remote serial handle, buffer ptr, length		*/
/* Outputs     : Number of chars written				*/
/************************************************************************/
Nat32 remSerWrite(RemSerHandle rsh, const char *buffer, Nat32 len)
{
    Nat32	bytesWritten;
    const char	*p;
    Nat16	port, writeLen, writeQueuedReg, writeQueued, zeroCount;

    if ((rsh == NULL) || (rsh->ctx == NULL))
	return(0);

    port = rsh->port;
    bytesWritten = 0;
    zeroCount = 0;
    for (p = buffer; bytesWritten < len; )
    {
	writeLen = len - bytesWritten;
	if (writeLen > SERBUF_SIZE)
	    writeLen = SERBUF_SIZE;

	rsh->wbuf[0] = writeLen;
	memcpy(rsh->wbuf + 1, buffer, writeLen);

	if (modbus_write_and_read_registers(rsh->ctx, TxReqReg(port), 
					    (writeLen+3)/2, rsh->wbuf,
					    TxQueuedReg(port), 1, &writeQueuedReg) < 0)
	    return(bytesWritten);
	    
	writeQueued = writeQueuedReg & 0xff;
	bytesWritten += writeQueued;
	p += writeQueued;

	if (writeQueued < writeLen)
	    usleep(50000L);

	if (writeQueued == 0)
	{
	    if (++zeroCount > 10)
		return(bytesWritten);
	}
	else
	    zeroCount = 0;
    }

    return(bytesWritten);

} /* remSerWrite() */


/************************************************************************/
/* Function    : remSerDrain						*/
/* Purpose     : Wait for serial output stream to finish sending	*/
/* Inputs      : RemSerHandle, ticks to wait 				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno remSerDrain(RemSerHandle rsh, Nat32 tmout)
{
    Nat32	tmRemain;
    Nat16	status;
    int		rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))
	return(ERROR);

    for (tmRemain = tmout; tmRemain > 0; tmRemain -= 10)
    {
	if ((rtn = modbus_read_registers(rsh->ctx, SerStatusReg,
					 1, &status)) < 1)
	    return(ERROR);

	if (TxBusy(status, rsh->port) == 0)
	    return(OK);

	usleep(250000L);
    }

    return(OK);

} /* remSerDrain() */


/************************************************************************/
/* Function    : remSerIFlush						*/
/* Purpose     : Flush remote serial input stream			*/
/* Inputs      : RemSerHandle		 				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno remSerIFlush(RemSerHandle rsh)
{
    if ((rsh == NULL) || (rsh->ctx == NULL))
	return(ERROR);

    rsh->bufChars = 0;
    if (modbus_write_register(rsh->ctx,  RxCtlReg(rsh->port), REM_RX_FLUSH) == 1)
	return(OK);
    return(ERROR);

} /* remSerIFlush() */


/************************************************************************/
/* Function    : remSerOFlush						*/
/* Purpose     : Flush remote serial output stream			*/
/* Inputs      : RemSerHandle		 				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno remSerOFlush(RemSerHandle rsh)
{
    if ((rsh == NULL) || (rsh->ctx == NULL))
	return(ERROR);

    if (modbus_write_register(rsh->ctx,  RxCtlReg(rsh->port), REM_TX_FLUSH) == 1)
	return(OK);
    return(ERROR);

} /* remSerOFlush() */


/************************************************************************/
/* Function    : remSerRxCount						*/
/* Purpose     : Get number of queued Rx characters			*/
/* Inputs      : RemSerHandle		 				*/
/* Output      : Number of queued Rx characters				*/
/************************************************************************/
Int16 remSerRxCount(RemSerHandle rsh)
{
    return((rsh == NULL) ? 0 : rsh->rxQueued);

} /* remSerRxCount() */


/************************************************************************/
/* Function    : remSerBreak						*/
/* Purpose     : Send a break character to remote output stream		*/
/* Inputs      : RemSerHandle, break time in ms				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno remSerBreak(RemSerHandle rsh, Nat16 breakTime)
{
    if ( (rsh == NULL) || (rsh->ctx == NULL) )
        return(ERROR);
    
    /* set break bit */
    modbus_write_register(rsh->ctx, TxBreakReg(rsh->port), REM_BREAK);

    /* wait for break delay */
    usleep((useconds_t)breakTime * 1000L);

    /* clear break bit */
    if (modbus_write_register(rsh->ctx, TxBreakReg(rsh->port), 0) == 1)
	return(OK);
    return(ERROR);

} /* remSerBreak() */