/****************************************************************************/
/* Copyright 2009-2013 MBARI.                                               */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Written by Mike Risi for Respirometer project in 2009                    */
/* Modified by Bob Herlien for SensorNode for xFOCE, May 2013               */
/****************************************************************************/
#include "p24Fxxxx.h"
#include <uart.h>
#include <stdio.h>
#include <stdlib.h>

#include "inst_port.h"
#include "buffer.h"
#include "serial.h"
#include "dig_io.h"
#include "sys_timer.h"


/***************************** Buffer Variables *****************************/
ByteBuffer txFgBuff[SER_PORT_TOTAL];

#define TX_FG_BUFF_SIZE 512

unsigned char txFgBuffData[SER_PORT_TOTAL][TX_FG_BUFF_SIZE];

static unsigned char txWriteRequest[SER_PORT_TOTAL];
static unsigned char txWriteQueued[SER_PORT_TOTAL];
static unsigned int  rxAvailSnapshot[SER_PORT_TOTAL];

static int pwrState = 0;
static BOOL dcAlwaysOn = FALSE;


/***************************** Local Functions ******************************/

int instPortRxFlush(int port);
int instPortTxFlush(int port);

unsigned int getRxConfig(int port, unsigned int offset);
unsigned int getRxStatus(int port, unsigned int offset);

unsigned int getTxStatus(int port, unsigned int offset);
unsigned int getTxConfig(int port, unsigned int offset);

void setRxConfig(int port, unsigned int offset, unsigned int reg);
void setTxConfig(int port, unsigned int offset, unsigned int reg);

unsigned int getRxData(int port, unsigned int offset);
void putTxData(int port, unsigned int offset, unsigned int reg);


void instPortInit()
{
    int i;

    for (i = 0; i < SER_PORT_TOTAL; ++i)
    {
        txFgBuff[i].data = txFgBuffData[i];
        txFgBuff[i].head = 0;
        txFgBuff[i].tail = 0;
        txFgBuff[i].ptr_mask = (TX_FG_BUFF_SIZE - 1);

        /* clear all write requests */
        txWriteRequest[i] = 0;

	/* Clear rxAvail snapshots  */
	rxAvailSnapshot[i] = 0;
    }
}

/***************************** Local Functions ******************************/


void instPwrInit(int leaveAlone)
{
    int i;

    if (leaveAlone)
    {
	DC_CTRL = 1;	/* Need DC on to read state of power relays	*/	
	tmrDelayMs(250);

	pwrState = instPwrGet();
    }
    else	    /**** Turn off all instruments  ****/
    {
	for (i = 0; i < PWR_PORT_TOTAL; ++i)
	    instPortPwrSet(i, 0);

	pwrState = 0;
    }

    /* If all relays off, turn off DC converter	*/
    if (pwrState == 0)
	DC_CTRL = 0;
}


void instPortPwrSet(int port, int state)
{
    if (port >= PWR_PORT_TOTAL)
	return;

    if (state)
    {
	switch (port)
        {
	  case PWR_PORT_0:
	      INSTR_1_ON = 1;
	      tmrDelayMs(RELAY_MS);
	      INSTR_1_ON = 0;
	      break;
	  case PWR_PORT_1:
	      INSTR_2_ON = 1;
	      tmrDelayMs(RELAY_MS);
	      INSTR_2_ON = 0;
	      break;
	  case PWR_PORT_2:
	      INSTR_3_ON = 1;
	      tmrDelayMs(RELAY_MS);
	      INSTR_3_ON = 0;
	      break;
	  case PWR_PORT_3:
	      INSTR_4_ON = 0;
	      tmrDelayMs(2*RELAY_MS);
	      INSTR_4_ON = 1;
	      break;
	}

	pwrState |= (1 << port);
    }
    else
    {
	switch (port)
        {
	  case PWR_PORT_0:
	      INSTR_1_OFF = 1;
	      tmrDelayMs(RELAY_MS);
	      INSTR_1_OFF = 0;
	      break;
	  case PWR_PORT_1:
	      INSTR_2_OFF = 1;
	      tmrDelayMs(RELAY_MS);
	      INSTR_2_OFF = 0;
	      break;
	  case PWR_PORT_2:
	      INSTR_3_OFF = 1;
	      tmrDelayMs(RELAY_MS);
	      INSTR_3_OFF = 0;
	      break;
	  case PWR_PORT_3:
	      INSTR_4_OFF = 0;
	      tmrDelayMs(2*RELAY_MS);
	      INSTR_4_OFF = 1;
	      break;
	}

	pwrState &= ~(1 << port);
    }

    pwrState &= PWR_PORT_MASK;

// If any instruments on, turn on master DC.  Else turn it off.
    DC_CTRL = (pwrState || dcAlwaysOn) ? 1 : 0;
}

int instPortPwrGet(int port)
{
    if (port >= PWR_PORT_TOTAL)
	return(FALSE);

    return((PORTF & (1<<port)) ? TRUE : FALSE);
}

int instPwrGet(void)
{
    return(PORTF & PWR_PORT_MASK);
}

void instSetDCalwaysOn(BOOL state)
{
    dcAlwaysOn = state;
// Turn DC on/off as necessary
    DC_CTRL = (pwrState || dcAlwaysOn) ? 1 : 0;
}

BOOL instGetDCalwaysOn(void)
{
    return(dcAlwaysOn);
}

void instPortTxTask()
{
    int i, bg_full;
    BYTE b;
    ByteBuffer* fg_buff;
//    char	dbgBuf[32];

    for (i = 0; i < SER_PORT_TOTAL; ++i)
    {
        /* if you are not in a break state put bytes in the TX FIFO */
        if ( !serGetBreak(i) )
        {
            fg_buff = &txFgBuff[i];

            bg_full = serIsTxFull(i);

            /* get a byte from the foreground buffer and queue it */
            while ( (!bg_full) && bufAvailable(fg_buff) )
            {
//		sprintf(dbgBuf, "Sending char\r\n");
//		serPutString(0, dbgBuf);
                /* if you get a byte from the foreground 
                buffer put it in the background buffer */
                if ( bufGet(&b, fg_buff) )
                {
//		    sprintf(dbgBuf, "Sending %x\r\n", (int)b);
//		    serPutString(0, dbgBuf);
                    /* if there was a byte in the 
                    foreground buffer queue it for transmission */
                    serPutByte(i, b);
                }

                /* see if there is still room in the bg_buff */
                bg_full = serIsTxFull(i);
            }
        }
    }
}

void instPortClearWriteReq()
{
    txWriteRequest[SER_PORT_0] = 0;
    txWriteRequest[SER_PORT_1] = 0;
    txWriteRequest[SER_PORT_2] = 0;
    
    return;
}


void instSnapshotRxAvail()
{
    int		i;

    for (i = 0; i < SER_PORT_TOTAL; i++)
	rxAvailSnapshot[i] = (unsigned int)serRxAvailable(i);
}

unsigned int instPortReadReg(unsigned int address)
{
    int port;
    unsigned int offset;
    unsigned int status;

    if ( address == SERIAL_STAT )
    {
        /* clear out status register */
        status = 0;

        /* set global serial err bits */
        if ( serGetErrFlag(SER_PORT_0) )
            status |= ERR_0_BIT; 

        if ( serGetErrFlag(SER_PORT_1) )
            status |= ERR_1_BIT; 

        if ( serGetErrFlag(SER_PORT_2) )
            status |= ERR_2_BIT; 

        /* set rx data buffered bit */
        if ( serRxAvailable(SER_PORT_0) > 0 )
            status |= RX0_DATA_BIT; 

        if ( serRxAvailable(SER_PORT_1) > 0 )
            status |= RX1_DATA_BIT; 

        if ( serRxAvailable(SER_PORT_2) > 0 )
            status |= RX2_DATA_BIT; 

        /* set tx data buffered bit */
        if ( bufAvailable(&txFgBuff[SER_PORT_0]) )
            status |= TX0_DATA_BIT; 

        if ( bufAvailable(&txFgBuff[SER_PORT_1]) )
            status |= TX1_DATA_BIT; 

        if ( bufAvailable(&txFgBuff[SER_PORT_2]) )
            status |= TX2_DATA_BIT; 
    
        /* return the status */
        return status;
    }

    /* calculate the offset and port */
    if ( (address > (SER_HOLDING_0 - 1)) && (address < SER_HOLDING_1) )
    {
        port = SER_PORT_0;
        offset = address - SER_HOLDING_0;
    }
    else if ( (address > (SER_HOLDING_1 - 1)) && (address < SER_HOLDING_2) )
    {
        port = SER_PORT_1;
        offset = address - SER_HOLDING_1;
    }
    else if ( (address > (SER_HOLDING_2 - 1)) && (address < SER_HOLDING_3) )
    {
        port = SER_PORT_2;
        offset = address - SER_HOLDING_2;
    }
    else
    {
        return 0;
    }

    switch ( offset )
    {
        /* read RX config register */
        case 0x00: return getRxConfig(port, offset);
        case 0x01: return getRxConfig(port, offset);
        case 0x02: return getRxConfig(port, offset);
        case 0x03: return getRxConfig(port, offset);
        /* read RX status register */
        case 0x04: return getRxStatus(port, offset);
        case 0x05: return getRxStatus(port, offset);
        case 0x06: return getRxStatus(port, offset);
        case 0x07: return getRxStatus(port, offset);
        /* read TX status register */
        case 0x80: return getTxStatus(port, offset);
        case 0x81: return getTxStatus(port, offset);
        case 0x82: return getTxStatus(port, offset);
        case 0x83: return getTxStatus(port, offset);
        /* read TX config register */
        case 0x84: return getTxConfig(port, offset);
        case 0x85: return getTxConfig(port, offset);
        case 0x86: return getTxConfig(port, offset);
        case 0x87: return getTxConfig(port, offset);
        default: 
	    if ((offset >= 0x08) && (offset <= 0x7f))
		return getRxData(port, offset);
	    else
		return(0);
    }
}

void instPortWriteReg(unsigned int address, unsigned int reg)
{
    int port;
    unsigned int offset;

    /* calculate the offset and port */
    if ( (address > (SER_HOLDING_0 - 1)) && (address < SER_HOLDING_1) )
    {
        port = SER_PORT_0;
        offset = address - SER_HOLDING_0;
    }
    else if ( (address > (SER_HOLDING_1 - 1)) && (address < SER_HOLDING_2) )
    {
        port = SER_PORT_1;
        offset = address - SER_HOLDING_1;
    }
    else if ( (address > (SER_HOLDING_2 - 1)) && (address < SER_HOLDING_3) )
    {
        port = SER_PORT_2;
        offset = address - SER_HOLDING_2;
    }
    else
    {
        return;
    }

    switch ( offset )
    {
        /* write RX config register */
        case 0x00: return setRxConfig(port, offset, reg);
        case 0x01: return setRxConfig(port, offset, reg);
        case 0x02: return setRxConfig(port, offset, reg);
        case 0x03: return setRxConfig(port, offset, reg);
        /* you can't write RX status registers, just return */
        case 0x04: return;
        case 0x05: return;
        case 0x06: return;
        case 0x07: return;
        /* you can't write TX status registers, just return */
        case 0x80: return;
        case 0x81: return;
        case 0x82: return;
        case 0x83: return;
        /* write TX config register */
        case 0x84: return setTxConfig(port, offset, reg);
        case 0x85: return setTxConfig(port, offset, reg);
        case 0x86: return setTxConfig(port, offset, reg);
        case 0x87: return setTxConfig(port, offset, reg);
        default: return putTxData(port, offset, reg);
    }
}

int instPortRxFlush(int port)
{
    if ( (port < 0) || (port > (SER_PORT_TOTAL - 1)) )
        return -1;

    serRxFlush(port);

    return 0;
}

int instPortTxFlush(int port)
{
    ByteBuffer* fg_buff;

    if ( (port < 0) || (port > (SER_PORT_TOTAL - 1)) )
        return -1;

    fg_buff = &txFgBuff[port];
    bufClear(fg_buff);
    serTxFlush(port);

    return 0;
}

#define STOP_BIT        0x0001
#define DATA_PAR_BIT    0x0006
#define ENABLE_BIT      0x0008

/* get RX/TX config and status resgisters */
unsigned int getRxConfig(int port, unsigned int offset)
{
    int stop;
    int data_par;
    unsigned int reg = 0;
    unsigned long baud;

    /* get the serial port params */
    serGetConfig(port, &baud, &data_par, &stop);

    /* if you're not into the TX registers, bail */
    switch ( offset )
    {
        case 0x00:  /* return the baud div 4 here */
                    return (baud / 4);

        case 0x01:  /* check data_par, stop, and enable bits */
                    reg = 0;

                    /* set data_par value */
                    reg = data_par;
                    reg <<= 1;
                    reg &= DATA_PAR_BIT;

                    /* set stop bits */
                    if ( stop == S_2 )
                        reg |= STOP_BIT;


                    /* always set enable hi for now */
                    if ( serGetEnable(port) )
                        reg |= ENABLE_BIT;

                    /* return the populated register */
                    return reg;
        
                    /* the following registers always read zero */
        case 0x02:  return 0;
        case 0x03:  return 0;
        default: return 0;
    }
    
    return 0;
}

#define FLUSH_RX_BIT    0x20
#define PERR_BIT        0x10
#define FERR_BIT        0x08
#define OERR_BIT        0x04
#define XERR_BIT        0x02
//#define YERR_BIT        0x01		/* FG overflow no longer possible */

unsigned int getRxStatus(int port, unsigned int offset)
{
    unsigned int err_flags = 0;

    switch ( offset )
    {
        case 0x04:  /* return error flags */
                    if ( serGetPERR(port) )
                        err_flags |= PERR_BIT;

                    if ( serGetFERR(port) )
                        err_flags |= FERR_BIT;

                    if ( serGetOERR(port) )
                        err_flags |= OERR_BIT;

                    if ( serGetXERR(port) )
                        err_flags |= XERR_BIT;

                    return err_flags;

        case 0x05:  return 0;
        case 0x06:  return 0;
        case 0x07:  return(rxAvailSnapshot[port]);
        default: break;
    }
    
    return 0;
}

unsigned int getTxStatus(int port, unsigned int offset)
{
    unsigned int reg;

    switch ( offset )
    {
        case 0x80:  return 0;
        case 0x81:  return 0;

        case 0x82:  /* set TX_BYTES_QUEUED in full */
                    reg = bufAvailable(&txFgBuff[port]);
                    return reg;

        case 0x83:  /* stuff TX_QUEUE_FREE in hi byte */
                    reg = TX_FG_BUFF_SIZE - 1;
                    reg -= bufAvailable(&txFgBuff[port]);
                    if ( reg > 0xFF ) reg = 0xFF;
                    reg <<= 8;
                    /* stuff TX_WRITE_ACCEPT in hi byte */
                    reg += (0x00FF & txWriteQueued[port]);
                    return reg;
        default: break;
    }
    
    return 0;
}

#define FLUSH_TX_BIT    0x20
#define BREAK_TX_BIT    0x01

unsigned int getTxConfig(int port, unsigned int offset)
{
    switch ( offset )
    {
        case 0x84:  /* FLUSH_TX always reads as zero */
                    return 0;

        case 0x85:  /* return break bit status */
                    if ( serGetBreak(port) )
                        return BREAK_TX_BIT;
                    else
                        return 0;
        
        case 0x86:  /* unused register */
                    return 0;
        
        case 0x87:  /* note: txWriteRequest is always cleared 
                    after write operation, so return zero  */
                    return 0;
        default: break;
    }
    
    return 0;
}

/* set RX/TX config resgisters */
void setRxConfig(int port, unsigned int offset, unsigned int reg)
{
    int stop;
    int data_par;
    unsigned long baud;
    
    unsigned char hi_byte;
    unsigned char lo_byte;

    hi_byte = (unsigned char)(reg >> 8);
    lo_byte = (unsigned char)(0x00FF & reg);

    /* get the current serial port configuration */
    serGetConfig(port, &baud, &data_par, &stop);

    /* if you're not into the TX registers, bail */
    switch ( offset )
    {
        case 0x00:  /* set serial div 4 here */
                    baud = (4 * reg);
                    serSetConfig(port, baud, data_par, stop);
                    break;

        case 0x01:  /* set data and parity values */
                    data_par = (DATA_PAR_BIT & reg);
                    data_par >>= 1;
                    
                    /* set stop value */
                    if ( reg & STOP_BIT )
                        stop = S_2;
                    else
                        stop = S_1;
                    
                    /* set the serial config values */
                    serSetConfig(port, baud, data_par, stop);
                    
                    /* enable/disable serial port */
                    if ( reg & ENABLE_BIT )
                        serSetEnable(TRUE);
                    else
                        serSetEnable(FALSE);
                    break;

        case 0x02:  /* clear error flags */
                    if ( lo_byte & FLUSH_RX_BIT )
                        instPortRxFlush(port);

                    if ( lo_byte & PERR_BIT )
                        serClrPERR(port);

                    if ( lo_byte & FERR_BIT )
                        serClrFERR(port);

                    if ( lo_byte & OERR_BIT )
                        serClrOERR(port);

                    if ( lo_byte & XERR_BIT )
                        serClrXERR(port);

                    break;

        case 0x03:  break;
        
        default: break;
    }
    
    return;
}

void setTxConfig(int port, unsigned int offset, unsigned int reg)
{
    unsigned char hi_byte;
    unsigned char lo_byte;

    hi_byte = (unsigned char)(reg >> 8);
    lo_byte = (unsigned char)(0x00FF & reg);
    
    /* if you're not into the TX registers, bail */
    switch ( offset )
    {
        case 0x84:  /* flush the tx buffers */
                    if ( lo_byte & FLUSH_TX_BIT )
                    {
                        bufClear(&txFgBuff[port]);
                        serTxFlush(port);
                    }
                    break;

        case 0x85:  /* set of clear break bit */ 
                    if ( reg & BREAK_TX_BIT )
                        serSetBreak(port, TRUE);
                    else
                        serSetBreak(port, FALSE);
                    break;

        case 0x86:  break;

        case 0x87:  /* accept the write request */
                    txWriteRequest[port] = lo_byte; 
                    txWriteQueued[port] = 0;
                    break;
        default: break;
    }
    
    return;
}
/* get RX data */
unsigned int getRxData(int port, unsigned int offset)
{
    unsigned char b;
    unsigned int reg, rxByteNum;

    /* if you're past the last RX data reg return zero */
    if ( offset > 0x7F )
        return 0;

    rxByteNum = 2 * (offset - 0x08);

    if ((rxAvailSnapshot[port] > rxByteNum) && serGetByte(port, &b))
    {
        reg = b;
        reg <<= 8;
    }
    else
    {
        return 0;
    }
    
    if ((rxAvailSnapshot[port] > rxByteNum+1) && serGetByte(port, &b))
    {
        reg += (0x00FF & b);
        return reg;
    }
    else
    {
        return reg;
    }
}

/* set TX data */
void putTxData(int port, unsigned int offset, unsigned int reg)
{
    unsigned char hi_byte;
    unsigned char lo_byte;

    /* if you're not into the TX registers, bail */
    if ( offset < 0x88 )
        return;

    hi_byte = (unsigned char)(reg >> 8);
    lo_byte = (unsigned char)(0x00FF & reg);
    
    if ( txWriteRequest[port] > txWriteQueued[port] )
        if ( bufPut(hi_byte, &txFgBuff[port]) )
            ++txWriteQueued[port];

    if ( txWriteRequest[port] > txWriteQueued[port] )
        if ( bufPut(lo_byte, &txFgBuff[port]) )
            ++txWriteQueued[port];
}