/****************************************************************************/
/* Copyright 2009 MBARI.                                                    */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
#include "p24Fxxxx.h"
#include <uart.h>
#include <stdio.h>
#include <stdlib.h>

#include "buffer.h"
#include "serial.h"

#include "sys_defs.h"
#include "config.h"

/***************************** Buffer Variables *****************************/
ByteBuffer rxFgBuff[INST_PORT_TOTAL];
ByteBuffer txFgBuff[INST_PORT_TOTAL];

#define RX_FG_BUFF_SIZE 512
#define TX_FG_BUFF_SIZE 256

unsigned char rxFgBuffData[INST_PORT_TOTAL][RX_FG_BUFF_SIZE];
unsigned char txFgBuffData[INST_PORT_TOTAL][TX_FG_BUFF_SIZE];

static unsigned char txWriteRequest[INST_PORT_TOTAL];
static unsigned char txWriteQueued[INST_PORT_TOTAL];

/* status variables for serial ports */
static int fgOverFlowFlag[INST_PORT_TOTAL];

void instPortInit() {
    int i;

    for (i = 0; i < INST_PORT_TOTAL; ++i) {
        rxFgBuff[i].data = rxFgBuffData[i];
        rxFgBuff[i].head = 0;
        rxFgBuff[i].tail = 0;
        rxFgBuff[i].ptr_mask = (RX_FG_BUFF_SIZE - 1);

        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 the overflow flag */
        fgOverFlowFlag[i] = FALSE;
    }
}

/***************************** Buffer Variables *****************************/


/***************************** 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 instPortWriteCfg(int port);

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

void instPortRxTask() {
    int i, gotbyte, putbyte;
    unsigned char b;
    ByteBuffer* fg_buff;

    for (i = 0; i < INST_PORT_TOTAL; ++i) {
        /* get a pointer to the foreground buffer */
        fg_buff = &rxFgBuff[i];

        /* get a byte from the background buffer */
        gotbyte = serGetByte(i, &b);

        while (gotbyte) {
            /* if you got a byte, put it in the forground buffer */
            putbyte = bufPut(b, fg_buff);

            if (putbyte) {

                /* there is still room in the
                foreground buffer get another byte */
                gotbyte = serGetByte(i, &b);
            } else {
                /* if the foreground buffer
                is full, set the over flow flag */
                fgOverFlowFlag[i] = TRUE;
                gotbyte = FALSE;
            }
        }
    }
}

void instPortTxTask() {
    int i, bg_full;
    unsigned char b;
    ByteBuffer* fg_buff;

    for (i = 0; i < INST_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)) {
                /* if you get a byte from the foreground
                buffer put it in the background buffer */
                if (bufGet(&b, fg_buff)) {
                    /* 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() {
#if ENABLE_PORT_0
    txWriteRequest[INST_PORT_0] = 0;
#endif

#if ENABLE_PORT_1
    txWriteRequest[INST_PORT_1] = 0;
#endif

#if ENABLE_PORT_2
    txWriteRequest[INST_PORT_2] = 0;
#endif

    return;
}

/* SERIAL_STAT bits */
#define ERR_2_BIT       0x0400
#define RX2_DATA_BIT    0x0200
#define TX2_DATA_BIT    0x0100

#define ERR_1_BIT       0x0040
#define RX1_DATA_BIT    0x0020
#define TX1_DATA_BIT    0x0010

#define ERR_0_BIT       0x0004
#define RX0_DATA_BIT    0x0002
#define TX0_DATA_BIT    0x0001


#define SERIAL_STAT     0x02FF

#define SER_HOLDING_0   0x0300//changed because the other registers now live in 0x200
#define SER_HOLDING_1   0x0400
#define SER_HOLDING_2   0x0500
/* note: the registers below don't exist, just used for offset calcs */
#define SER_HOLDING_3   0x0600


static char dbg_buff[64];
static int inst_rd_debug = 0;

void dumpReadReg(unsigned int address) {
    if (!inst_rd_debug)
        return;

    sprintf(dbg_buff, "instPortReadReg(0x%04X)\r\n", address);
    serPutString(CONSOLE_PORT, dbg_buff);
}

void dumpReadOffset(int port, unsigned int offset) {
    if (!inst_rd_debug)
        return;

    sprintf(dbg_buff, "port = %d\r\n", port);
    serPutString(CONSOLE_PORT, dbg_buff);

    sprintf(dbg_buff, "offset = 0x%04X\r\n", offset);
    serPutString(CONSOLE_PORT, dbg_buff);
}

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

    dumpReadReg(address);

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

        /* set global serial err bits */
#if ENABLE_PORT_0
        if (serGetErrFlag(INST_PORT_0) || fgOverFlowFlag[INST_PORT_0])
            status |= ERR_0_BIT;
#endif

#if ENABLE_PORT_1
        if (serGetErrFlag(INST_PORT_1) || fgOverFlowFlag[INST_PORT_1])
            status |= ERR_1_BIT;
#endif

#if ENABLE_PORT_2
        if (serGetErrFlag(INST_PORT_2) || fgOverFlowFlag[INST_PORT_2])
            status |= ERR_2_BIT;
#endif

        /* set rx data buffered bit */
#if ENABLE_PORT_0
        if (bufAvailable(&rxFgBuff[INST_PORT_0]))
            status |= RX0_DATA_BIT;
#endif

#if ENABLE_PORT_1
        if (bufAvailable(&rxFgBuff[INST_PORT_1]))
            status |= RX1_DATA_BIT;

#endif

#if ENABLE_PORT_2
        if (bufAvailable(&rxFgBuff[INST_PORT_2]))
            status |= RX2_DATA_BIT;
#endif

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

#if ENABLE_PORT_1
        if (bufAvailable(&txFgBuff[INST_PORT_1]))
            status |= TX1_DATA_BIT;
#endif

#if ENABLE_PORT_2
        if (bufAvailable(&txFgBuff[INST_PORT_2]))
            status |= TX2_DATA_BIT;
#endif
        /* return the status */
        return status;
    }

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

    dumpReadOffset(port, offset);

    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: return getRxData(port, offset);
    }
}

static int inst_wr_debug = 0;

void dumpWriteReg(unsigned int address, unsigned int reg) {
    if (!inst_wr_debug)
        return;


    sprintf(dbg_buff, "instPortWriteReg(0x%04X, 0x%04X)\r\n", address, reg);
    serPutString(CONSOLE_PORT, dbg_buff);
}

void dumpWriteOffset(int port, unsigned int offset, unsigned int reg) {
    if (!inst_wr_debug)
        return;

    sprintf(dbg_buff, "port = %d\r\n", port);
    serPutString(CONSOLE_PORT, dbg_buff);

    sprintf(dbg_buff, "offset = 0x%04X\r\n", offset);
    serPutString(CONSOLE_PORT, dbg_buff);

    sprintf(dbg_buff, "reg = 0x%04X\r\n", reg);
    serPutString(CONSOLE_PORT, dbg_buff);

}

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

    dumpWriteReg(address, reg);

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

    dumpWriteOffset(port, offset, reg);

    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) {
    ByteBuffer* fg_buff;

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

    fg_buff = &rxFgBuff[port];
    bufClear(fg_buff);
    serRxFlush(port);

    return 0;
}

int instPortTxFlush(int port) {
    ByteBuffer* fg_buff;

    if ((port < 0) || (port > (INST_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
#define SAVE_CFG_BIT    0x0001

/* 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;

            /* always leave save config to zero on read*/

            /* 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

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;

            if (fgOverFlowFlag[port])
                err_flags |= YERR_BIT;

            return err_flags;

        case 0x05: return 0;
        case 0x06: return 0;
        case 0x07: return (unsigned int) bufAvailable(&rxFgBuff[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 * (unsigned long)reg);
            switch (baud) {
                case CFG_INST_BAUD_4800: 
                case CFG_INST_BAUD_9600: 
                case CFG_INST_BAUD_14400: 
                case CFG_INST_BAUD_19200:
                case CFG_INST_BAUD_38400:
                case CFG_INST_BAUD_57600: 
                case CFG_INST_BAUD_115200: 
                    serSetConfig(port, baud, data_par, stop);
                    break;
                default: return;
            }
            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);

            if (lo_byte & YERR_BIT)
                fgOverFlowFlag[port] = FALSE;

            break;

        case 0x03:
            /* commit the current settings to config */
            if (reg & SAVE_CFG_BIT)
                instPortWriteCfg(port);
            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;

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

    if (bufGet(&b, &rxFgBuff[port])) {
        reg = b;
        reg <<= 8;
    } else {
        return 0;
    }

    if (bufGet(&b, &rxFgBuff[port])) {
        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];
}

/*Grab and save instrument port data to the configuration memory */
void instPortWriteCfg(int port) {
    ConfigData configData;
    int stop;
    int data_par;
    unsigned long baud;

     cfgRead(&configData);

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

    // we need to translate BRGHtoBaud values to longs compatible with our cfg constants.
     switch ( baud ) {
         case 4807:   baud = 4800L; break;
         case 9615:   baud = 9600L; break;
         case 14492:  baud = 14400L; break;
         case 19230:  baud = 19200L; break;
         case 38461:  baud = 38400L; break;
         case 58823:  baud = 58600L; break;
         case 125000: baud = 115200; break;
         default: return;
     }
    //now change
    switch (port) {

#if ENABLE_PORT_0
        case INST_PORT_0:
            configData.inst0Baud = baud;
            configData.inst0Parity = data_par;
            configData.inst0Stop = stop;
            break;
#endif
#if ENABLE_PORT_1
        case INST_PORT_1:
            configData.inst1Baud = baud;
            configData.inst1Parity = data_par;
            configData.inst1Stop = stop;
            break;
#endif
        default: return;
    }

    //write changes
    if (cfgWrite(&configData) == 0) //success
    {

    }

}