/************************************************************************/
/* Copyright 2009-2013 MBARI						*/
/************************************************************************/
/* Summary  : Remote serial/power/analog I/O using SensorNode		*/
/* Filename : snLib.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 <snLib.h>			/* defs for this module		*/
#include <sensorNode.h>			/* Library func defns		*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>

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

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

MLocal SensorNodeStruct	sNode[SENSOR_NODES];

MLocal MBool		snDebug = FALSE;

/****************************************/
/* MUTEX (Thread Safety) definitions	*/
/****************************************/

//#define MUTEX

#ifdef MUTEX
MLocal pthread_mutex_t	snMutex;
#define INIT_MUTEX()	pthread_mutex_init(&snMutex, NULL)
#define LOCK()		pthread_mutex_lock(&snMutex)
#define UNLOCK()	pthread_mutex_unlock(&snMutex)
#else
#define INIT_MUTEX()
#define LOCK()
#define UNLOCK()
#endif




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

    INIT_MUTEX();

} /* initSensorNodeLib() */


/************************************************************************/
/* Function    : snSetDebug						*/
/* Purpose     : Turn debug messages on/off				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
Void	snSetDebug(MBool onoff)
{
    snDebug = 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);
    }

    LOCK();

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

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

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

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

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

    errno = ENOMEM;
    UNLOCK();
    return(NULL);
	
} /* openSensorConnection() */


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

    LOCK();
    if (--snh->useCount == 0)
    {
	modbus_close(snh->ctx);
	snh->ctx = NULL;
    }

    UNLOCK();
    return(OK);

} /* closeSensorConnection() */


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

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

    LOCK();
    rsp = &snh->serPorts[port];
    rsp->snp = snh;
    rsp->ctx = snh->ctx;
    rsp->port = port;
    
    if (rsp->useCount == 0)
    {
	rsp->useCount++;
	rsp->bufPtr = rsp->rbuf;
	rsp->bufChars = 0;
	rsp->rxQueued = 0;
    }

    rtn = modbus_write_bit(snh->ctx, TxEnable(port), TRUE);
    UNLOCK();

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

    return(rsp);

} /* snOpenSerial() */


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

    LOCK();

    if (--rsh->useCount == 0)
    {
	modbus_write_bit(rsh->ctx, TxEnable(rsh->port), FALSE);
	rsh->snp = NULL;		/*Just in case someone reuses rsh*/
	rsh->ctx = NULL;
    }

    UNLOCK();
    return(OK);

} /* snCloseSerial() */


/************************************************************************/
/* Function    : snSerInit						*/
/* 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						*/
/************************************************************************/
Errno snSerInit(RemSerHandle rsh, int baud, char parity, int stop)
{
    Nat16	regs[2];
    Errno	rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))	
    {
	errno = EINVAL;
	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;

    LOCK();
    rtn = modbus_write_registers(rsh->ctx, BaudReg(rsh->port), 2, regs);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snSerInit() */


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

    if ((rsh == NULL) || (rsh->snp == NULL))
    {
	errno = EINVAL;
	return(0);
    }

    LOCK();
    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))
	    {
		UNLOCK();
		return(bytesRead);
	    }

	    rsh->bufPtr = rsh->rbuf;
	    rsh->bufChars = (rsh->rxQueued >= 2*readLen) ? 
				2*readLen : rsh->rxQueued;
	}
    }

    UNLOCK();
    return(bytesRead);

} /* snSerRead() */


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

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
	errno = EINVAL;
	return(0);
    }

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

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

	rtn = modbus_write_and_read_registers(rsh->ctx, TxReqReg(port), 
					      (writeLen+3)/2, rsh->wbuf,
					      TxQueuedReg(port), 1, &writeQueuedReg);
	UNLOCK();

	if (rtn < 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);

} /* snSerWrite() */


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

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
	errno = EINVAL;
	return(ERROR);
    }

    for (tmRemain = tmout; tmRemain > 0; tmRemain -= 10)
    {
	LOCK();
	rtn = modbus_read_registers(rsh->ctx, SerStatusReg, 1, &status);
	UNLOCK();

	if (rtn < 1)
	    return(ERROR);

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

	usleep(250000L);
    }

    return(OK);

} /* snSerDrain() */


/************************************************************************/
/* Function    : remSerIFlush						*/
/* Purpose     : Flush remote serial input stream			*/
/* Inputs      : RemSerHandle		 				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno snSerIFlush(RemSerHandle rsh)
{
    int	rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rsh->bufChars = 0;
    rtn = modbus_write_register(rsh->ctx,  RxCtlReg(rsh->port), REM_RX_FLUSH);
    UNLOCK();
    
    return((rtn > 0) ? OK : ERROR);

} /* snSerIFlush() */


/************************************************************************/
/* Function    : remSerOFlush						*/
/* Purpose     : Flush remote serial output stream			*/
/* Inputs      : RemSerHandle		 				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno snSerOFlush(RemSerHandle rsh)
{
    int	rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_write_register(rsh->ctx,  RxCtlReg(rsh->port), REM_TX_FLUSH);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snSerOFlush() */


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

} /* snSerRxCount() */


/************************************************************************/
/* Function    : snSerBreak						*/
/* Purpose     : Send a break character to remote output stream		*/
/* Inputs      : RemSerHandle, break time in ms				*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno snSerBreak(RemSerHandle rsh, Nat16 breakTime)
{
    int	rtn;

    if ( (rsh == NULL) || (rsh->ctx == NULL) )
    {
	errno = EINVAL;
	return(ERROR);
    }
    
    /* set break bit */
    LOCK();
    modbus_write_register(rsh->ctx, TxBreakReg(rsh->port), REM_BREAK);
    UNLOCK();

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

    /* clear break bit */
    LOCK();
    rtn = modbus_write_register(rsh->ctx, TxBreakReg(rsh->port), 0);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snSerBreak() */


/************************************************************************/
/* Function    : snSerSetTxEnbl						*/
/* Purpose     : Set/Clear Tx Enable for SensorNode serial port		*/
/* Inputs      : RemSerHandle, Boolean TRUE to set TxEn, FALSE to clear	*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno	snSerSetTxEnbl(RemSerHandle rsh, MBool enbl)
{
    int rtn;

    if ( (rsh == NULL) || (rsh->ctx == NULL) )
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_write_bit(rsh->ctx, TxEnable(rsh->port), (int)enbl);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snSerSetTxEnbl() */


/***********************************/
/*   Remote Power Functions	   */
/***********************************/
/************************************************************************/
/* Function    : snPower						*/
/* Purpose     : Turn remote power bit on/off on SensorNode		*/
/* Inputs      : SensorNodeHandle, Pwr bit num, Boolean TRUE/FALSE for on/off*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno	snPower(SensorNodeHandle snh, Nat16 port, MBool onoff)
{
    int rtn;

    if ((snh == NULL) || (port >= SN_PWR_PORTS))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_write_bit(snh->ctx, PowerBit(port), (int)onoff);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snPower() */


/************************************************************************/
/* Function    : snPowerIsOn						*/
/* Purpose     : Return status of power bit on/off on SensorNode	*/
/* Inputs      : SensorNodeHandle, Pwr bit num				*/
/* Outputs     : 1 if on, 0 if off, ERROR (-1) if I/O error		*/
/************************************************************************/
Int16		snPowerIsOn(SensorNodeHandle snh, Nat16 port)
{
    int		rtn;
    Byte	val;

    if ((snh == NULL) || (port >= SN_PWR_PORTS))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_read_bits(snh->ctx, PowerBit(port), 1, &val);
    UNLOCK();

    if (rtn < 1)
	return(ERROR);

    return(val ? 1 : 0);

} /* snPowerIsOn() */


/***********************************/
/*   Remote A/D Functions	   */
/***********************************/
/************************************************************************/
/* Function    : snAnalog						*/
/* Purpose     : Read analog value from SensorNode			*/
/* Inputs      : SensorNodeHandle, Analog channel, ptr for return value	*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno	snAnalog(SensorNodeHandle snh, Nat16 chan, Nat16 *val)
{
    int		rtn;

    if ((snh == NULL) || (chan >= SN_AD_CHANS))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(snh->ctx, AnalogReg(chan), 1, val);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snAnalog() */


/************************************************************************/
/* Function    : snSetAnalogParms					*/
/* Purpose     : Set analog averaging parameters			*/
/* Inputs      : SensorNodeHandle, Analog channel, num avgs, ms btwn avgs*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno	snSetAnalogParms(SensorNodeHandle snh, Nat16 chan, Nat16 navg, Nat16 avgMs)
{
    int		rtn;
    Nat16	val[2];

    if ((snh == NULL) || (chan >= SN_AD_CHANS) ||
	(navg == 0) || (navg * avgMs > 500))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    val[0] = navg;
    val[1] = avgMs;
    return(modbus_write_registers(snh->ctx, AnalogAvgReg(chan), 2, val));
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snSetAnalogParms() */


/************************************************************************/
/* Function    : snGetAnalogParms					*/
/* Purpose     : Get analog averaging parameters			*/
/* Inputs      : SensorNodeHandle, Analog channel, ptrs to return data	*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno	snGetAnalogParms(SensorNodeHandle snh, Nat16 chan, Nat16 *navg, Nat16 *avgMs)
{
    Errno	rtn;
    Nat16	val[2];

    if ((snh == NULL) || (chan >= SN_AD_CHANS))
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(snh->ctx, AnalogAvgReg(chan), 2, val);
    UNLOCK();
    *navg = val[0];
    *avgMs = val[1];

    return((rtn > 0) ? OK : ERROR);

} /* snGetAnalogParms() */


/************************************************************************/
/* Function    : snAnalogUse16bit					*/
/* Purpose     : Set SensorNode board to use 16 or 10 bit converter	*/
/* Inputs      : SensorNodeHandle, Boolean TRUE to use ADS8344 converter*/
/* Outputs     : Errno							*/
/************************************************************************/
Errno	snAnalogUse16bit(SensorNodeHandle snh, MBool use16bit)
{
    int		rtn;

    if (snh == NULL)
    {
	errno = EINVAL;
	return(ERROR);
    }

    LOCK();
    rtn = modbus_write_bit(snh->ctx, ADCSwitch, (int)use16bit);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* snAnalogUse16bit() */
