/** 
 * COPYRIGHT 2016 MBARI
 *
 *
 *
 *
 */
#include "mcc_generated_files/mcc.h"
#include "AD567x.h"
#include "serial.h"
#include "analoginput.h"
#include "dig_in.h"
#include "dig_out.h"
#include "parser.h"
#include "errors.h"
#include "config.h"
#include "inst_port.h"

#include <stdio.h>
#include <string.h>

#define DELAY 15000

/***************************** Modbus includes ******************************/
#include "mb.h"
#include "mbport.h" 
/***************************** Modbus includes ******************************/
#include "sys_defs.h"

/****************************** Modbus defines ******************************/
#include "modbus_registers.h"

/****************************** Modbus variables ****************************/
static BOOL bGotModbusFrame = FALSE;

static BOOL bWaterAlarm = FALSE;
static BOOL bBlinkyDebug = FALSE;
static unsigned short usBlinkyState = 0;
static BOOL escapeSequenceFailed = FALSE;
//static unsigned short usDACValues[8];

/** 
 * Function Prototypes
 */

int gotConfigEscape();
int getCommand(char * cmd);
int processCommands();

/*
   Main application
   */
int main(void) {

    //unsigned int iDebug = 0x00;
    ConfigData configData;
    
    //unsigned int iDebug;
    // initialize the device

    SYSTEM_Initialize();
    AD567X_Initialize();
    serInit();
    analogInit();
    instPortInit();

    // ONE TIME TO ADJUST
    //cfgDefault();
    
    /* Config Initializations */
    if ( cfgRead(&configData) != 0 )
    {
        /* set the defaults */
        cfgDefault();

        /* reread the correct default config */
        cfgRead(&configData);
    }

    /* Change Instrument Ports to match config */
    serSetConfig(INST_PORT_0, configData.inst0Baud, configData.inst0Parity, configData.inst0Stop);
    serSetConfig(INST_PORT_1, configData.inst1Baud, configData.inst1Parity, configData.inst1Stop);

    unsigned int total;
    total = 0;
    usBlinkyState = 0;

    //Prior to modbus initialization, we need to determine the address.
    unsigned char ucMBAddress = hwBoardAddress(); 

    /* Initialize the Modbus Stack 
     * Please note that the stop bits are set down in the freemodbus lib, 
     * currently at 2 stop bits.*/
    eMBInit(MB_RTU, ucMBAddress, 0, 19200, MB_PAR_NONE);
    
    eMBEnable();
    
    eMBPoll();
    
    
    /** Main Loop ******************************************************/
    
    for (;;) {

        /* Modbus Callbacks */
        eMBPoll(); 
        
        /* Water Alarm Test */
        if (IO_H2OSense_GetValue() ==1 ) {
            bWaterAlarm = TRUE;
        }
        
        /* Instrument Port Tasks */
        /* start polling instrument port 
        tasks once modbus messages have started */
        if ( bGotModbusFrame || escapeSequenceFailed )
        {
            /* make sure the instrument port RX and TX queues are handled */
            instPortRxTask();
            instPortTxTask();

        } else {
            /* wait for escape sequence and go to config */
            if (gotConfigEscape()) {
                /* Console Callback */
                processCommands();
            }
                /* blink blinky */
            if (++total == 20000) {
                    /* clear the counter */
                total = 0;
                doToggleBlinky();
            }
            
        }
//        /* Periodic Functions */
//        if (1) { // Periodic light blinking, and other debug tests at a slowed rate.
//            if (++total > 20000) {
//                total = 0;
//                if (bBlinkyDebug == TRUE)
//                    doToggleBlinky();
//                
//                //test AIN0
//                iDebug = analogReadReg(ANALOG_IN_0);
//                sprintf(sDebug, "AIN ANALOG_IN_0: %u\r\n",iDebug);
//                serPutString(CONSOLE_PORT,sDebug);
//
//                //test AIN1
//                iDebug = analogReadReg(ANALOG_IN_1);
//                sprintf(sDebug, "AIN ANALOG_IN_1: %u\r\n",iDebug);
//                serPutString(CONSOLE_PORT,sDebug);
//
//                //test AIN2
//                iDebug = analogReadReg(ANALOG_IN_2);
//                sprintf(sDebug, "AIN ANALOG_IN_2: %u\r\n",iDebug);
//                serPutString(CONSOLE_PORT,sDebug);
//
//                //test AIN3
//                iDebug = analogReadReg(ANALOG_IN_3);
//                sprintf(sDebug, "AIN ANALOG_IN_3: %u\r\n",iDebug);
//                serPutString(CONSOLE_PORT,sDebug);
//
//                //test AMB_TEMP
//                iDebug = analogReadReg(AMB_TEMP);
//                sprintf(sDebug, "AIN AMB_TEMP: %u\r\n",iDebug);
//                serPutString(CONSOLE_PORT,sDebug);
//
//                //test GF_CURRENT
//                iDebug = analogReadReg(GF_CURRENT);
//                sprintf(sDebug, "AIN GF_CURRENT: %u\r\n",iDebug);
//                serPutString(CONSOLE_PORT,sDebug);
//
//                //test DI's 
//                sprintf(sDebug,"DI0: %u DI1: %u CN0: %u CN1: %u H20: %u \r\n",
//                        discreteReadReg(DIGITAL_IN_0),
//                        discreteReadReg(DIGITAL_IN_1),
//                        discreteReadReg(CN_IN_0),
//                        discreteReadReg(CN_IN_1),
//                        discreteReadReg(H2O_SENSE));
//                serPutString(CONSOLE_PORT,sDebug);



//            }
//        } else {
//        }

    }

    return -1;
}

int gotConfigEscape()
{
   unsigned char b;
   static int char_count = 0;

   /* check for a byte */
   if ( serGetByte(CONSOLE_PORT, &b) )
   {
      /* if its a 'U' inc the char counter */
      if ( b == 'U')
      { 
         ++char_count;
      }
      else
      {
         /* if its not a 'U' the escape sequence failed */
         escapeSequenceFailed = TRUE;
         return FALSE;
      }

      /* if you get 5 'U' chars in a row you've escaped into config land */
      if ( char_count == 5 )
         return TRUE;
   }

   return FALSE;
}

eMBErrorCode
eMBRegInputCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs) {
    eMBErrorCode eStatus = MB_ENOERR;
        uint16_t  retval;
        //char msg[128];
        
        /* set the modbus frame received flag to true */
        bGotModbusFrame = TRUE;
        if ( (bBlinkyDebug == FALSE) )
            doToggleBlinky();
    
        if( ( usAddress >= REG_INPUT_START )
            && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
        {
            while (usNRegs >0) {
                switch(usAddress) {
                    case ANALOG_IN_0:
                    case ANALOG_IN_1:
                    case ANALOG_IN_2:
                    case ANALOG_IN_3:
                    case AMB_TEMP:
                    case GF_CURRENT:
                        retval = analogReadReg(usAddress);

                        //sprintf(msg,"Read Address:x%hX Value %u\n\r",usAddress,retval);
                        //serPutString(CONSOLE_PORT,msg);
                        *pucRegBuffer++ = (unsigned char)(retval >> 8);
                        *pucRegBuffer++ = (unsigned char)(retval & 0xFF);
                        break;
                    case DISCRETE_INPUTS:
                        //Return a register stuffed with bits set for the digital inputs
                        retval = discreteReadAll();
                        retval |= (((bWaterAlarm == TRUE) ? 1 : 0) << 4);

                        *pucRegBuffer++ = (unsigned char)(retval >> 8);
                        *pucRegBuffer++ = (unsigned char)(retval & 0xFF);
                        break;
                    default: return MB_ENOREG;

                }
                
                usNRegs--;   
                usAddress++;
            }
            /*iRegIndex = ( int )( usAddress - usRegInputStart );
            while( usNRegs > 0 )
            {
                *pucRegBuffer++ =
                    ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 );
                *pucRegBuffer++ =
                    ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF );
                iRegIndex++;
                usNRegs--;
            }*/
            
        }
        else
        {
            eStatus = MB_ENOREG;
        }

    return eStatus;
}


eMBErrorCode
eMBRegHoldingCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs,
        eMBRegisterMode eMode) {
    eMBErrorCode eStatus = MB_ENOERR;
    USHORT loByte;
    USHORT hiByte;
    USHORT reg;

    /* set the modbus frame received flag to true */
    bGotModbusFrame = TRUE;
    if ((bBlinkyDebug == FALSE))
        doToggleBlinky();

    if (((usAddress < REG_HOLDING_COMMON_START ) || 
        (usAddress + usNRegs > (REG_HOLDING_COMMON_START+REG_HOLDING_COMMON_NREGS))) &&
            ( (usAddress < INST_HOLDING_START) || (usAddress > INST_HOLDING_END) ) &&
            ( (usAddress < REG_INPUT_START) || (usAddress > REG_INPUT_START+REG_INPUT_NREGS))
            ) {
        return MB_ENOREG;
    }

    /* read mode */
    if (eMode == MB_REG_READ) {
        
        // Input Registers
            
            if ( (usAddress >= REG_INPUT_START) &&
                    (usAddress <= REG_INPUT_START+REG_INPUT_NREGS) )
            {
                return eMBRegInputCB(pucRegBuffer,usAddress,usNRegs);
            }

        while( usNRegs > 0 )
        {
            /* set the reg to zero just in case there is no representation */
            reg = 0;
            
            //COMMON HOLDING REGS
            if ((usAddress >= REG_HOLDING_COMMON_START) && 
                    ((usAddress) < 
                     (REG_HOLDING_COMMON_START + REG_HOLDING_COMMON_NREGS))) {
                switch (usAddress) {
                    case LED_DAC_0:
                    case LED_DAC_1:
                    case LED_DAC_2:
                    case LED_DAC_3:
                    case LED_DAC_4:
                    case LED_DAC_5:
                    case DAC_OUT_6:
                    case DAC_OUT_7:
                        reg = AD567X_ReadBackEnable(usAddress-LED_DAC_0);
                        break;
                    case COIL_OUTPUT: //Return the Coil Output Register
                        doReadReg(usAddress,&reg);
                        reg |= (((bBlinkyDebug==TRUE)?1:0)           << 12);
                        
                        break;
                }
            }

            // Instrument Ports

            if ( (usAddress >= INST_HOLDING_START) && 
                    (usAddress <= INST_HOLDING_END) )
            {
                reg = instPortReadReg(usAddress);
            }
            
            
            /* assign it to the reg buffer */
            *pucRegBuffer++ = (unsigned char)( reg >> 8 );
            *pucRegBuffer++ = (unsigned char)( reg & 0xFF );

            /* increment register index and address */
            --usNRegs;
            ++usAddress;
        }
    }

    /* write mode */
    if (eMode == MB_REG_WRITE) {
        
                /* grab a register */
                while( usNRegs > 0 )
                {
                    /* get a register from the reg buffer */
                    hiByte = *(pucRegBuffer++);
                    hiByte <<= 8;
                    loByte = *(pucRegBuffer++);
        
                    reg = ((hiByte & 0xFF00) + (loByte & 0x00FF));
        
                    /* write the register */
                    //COMMON HOLDING REGS
                    if ((usAddress >= REG_HOLDING_COMMON_START) && 
                            ((usAddress) < (REG_HOLDING_COMMON_START + REG_HOLDING_COMMON_NREGS)))
                    {
                        switch (usAddress) {
                            case LED_DAC_0:
                            case LED_DAC_1:
                            case LED_DAC_2:
                            case LED_DAC_3:
                            case LED_DAC_4:
                            case LED_DAC_5:
                            case DAC_OUT_6:
                            case DAC_OUT_7:
                                AD567X_WriteToAndUpdateChannel(usAddress-LED_DAC_0,reg);
                                break;
                            case COIL_OUTPUT:
                                //0
                                doSetBit(ISOPWR_CNTL_0,(reg & (1 << 0)));
                                //1
                                doSetBit(ISOPWR_CNTL_1,(reg & (1 << 1)));
                                //2
                                doSetBit(ISOPWR_CNTL_2,(reg & (1 << 2)));
                                //3
                                doSetBit(ISOPWR_CNTL_3,(reg & (1 << 3)));
                                //4
                                doSetBit(ISOPWR_CNTL_4,(reg & (1 << 4)));
                                //5
                                doSetBit(ISOPWR_CNTL_5,(reg & (1 << 5)));
                                //6
                                doSetBit(INSTR_CNTL_0 ,(reg & (1 << 6)));
                                //7
                                doSetBit(INSTR_CNTL_1 ,(reg & (1 << 7)));
                                //8
                                doSetBit(DIGITAL_OUT_0,(reg & (1 << 8)));
                                //9
                                doSetBit(DIGITAL_OUT_1,(reg & (1 << 9)));
                                //10
                                doSetBit(GF_CNTL_HIGH ,(reg & (1 << 10)));
                                //11
                                doSetBit(GF_CNTL_LOW  ,(reg & (1 << 11)));
                                //12
                                bBlinkyDebug = (reg & (1 << 12))? TRUE:FALSE;
                                //13
                                doSetBlinky((reg & (1<<13)));
                                //14
                                if (reg & (1 << 14)) bWaterAlarm = FALSE;
                                //15 
                                doSetBit(VICOR_CNTL,   (reg & (1 <<15)));
                                break;
                        }
                    }
                    
                    if ( (usAddress >= INST_HOLDING_START) && 
                         (usAddress <= INST_HOLDING_END) )
                    {
                        instPortWriteReg(usAddress, reg);
                    }
        
                    /* increment register index and address */
                    --usNRegs;
                    ++usAddress;
                }
        
//                /* If a write is ending then all Tx write 
//               transactions are over, so clear all txWriteRequest registers. */
//               instPortClearWriteReq();
    }

    return eStatus;
}

int isBitSet(int bit, UCHAR* buff) {
    switch (bit % 8) {
        case 0: return (buff[(bit / 8)] & 0x01);
        case 1: return (buff[(bit / 8)] & 0x02);
        case 2: return (buff[(bit / 8)] & 0x04);
        case 3: return (buff[(bit / 8)] & 0x08);
        case 4: return (buff[(bit / 8)] & 0x10);
        case 5: return (buff[(bit / 8)] & 0x20);
        case 6: return (buff[(bit / 8)] & 0x40);
        case 7: return (buff[(bit / 8)] & 0x80);
        default: return 0;
    }
}

void bitSet(int bit, UCHAR* buff, int val) {
    UCHAR mask;

    /* set the mask */
    switch (bit % 8) {
        case 0: mask = 0x01;
            break;
        case 1: mask = 0x02;
            break;
        case 2: mask = 0x04;
            break;
        case 3: mask = 0x08;
            break;
        case 4: mask = 0x10;
            break;
        case 5: mask = 0x20;
            break;
        case 6: mask = 0x40;
            break;
        case 7: mask = 0x80;
            break;
        default: return;
    }

    /* set or clear the bit depending on the val */
    if (val)
        buff[(bit / 8)] |= mask;
    else
        buff[(bit / 8)] &= ~mask;

    return;
}

eMBErrorCode
eMBRegCoilsCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils,
        eMBRegisterMode eMode) {
    int i, coil, val;

    /* set the modbus frame received flag to true */
    bGotModbusFrame = TRUE;
    if ((bBlinkyDebug == FALSE))
        doToggleBlinky();
    
    /* only coils 1 - 17 are represented */
    if ((usAddress < REG_COIL_START) ||
            (usAddress + usNCoils) > (REG_COIL_START + REG_COIL_NREGS))
        return MB_ENOREG;

    /* perform a coil write(s) */
    if (eMode == MB_REG_WRITE) {
        for (i = 0; i < usNCoils; ++i) {
            /* calculate the coil number */
            coil = i + usAddress;

            /* see if the bit is set for that coil */
            val = isBitSet(i, pucRegBuffer);

            /* take the appropriate action for that coil */
            switch (coil) {
                 /* digital output bits */
                case ISOPWR_CNTL_0:
                case ISOPWR_CNTL_1:
                case ISOPWR_CNTL_2:
                case ISOPWR_CNTL_3:
                case ISOPWR_CNTL_4:
                case ISOPWR_CNTL_5:
                case INSTR_CNTL_0: 
                case INSTR_CNTL_1: 
                case DIGITAL_OUT_0:
                case DIGITAL_OUT_1:
                case GF_CNTL_HIGH:
                case GF_CNTL_LOW:   
                case VICOR_CNTL:
                                    doSetBit(coil, val);  break;
                case BLINKY_DEBUG:  bBlinkyDebug = val; break;
                case BLINKY_STATE:  usBlinkyState=val;   break;
                case H2O_ALARM_CLR: //
                    if (val)
                        bWaterAlarm = FALSE;
                    break;
               
                default: break;
            }
        }
        
        /* set blinky based on debug state */
        if ( bBlinkyDebug )
           doSetBlinky(usBlinkyState);

        return MB_ENOERR;
    }


    /* perform a coil read(s) */
    if (eMode == MB_REG_READ) {
        /* clear all of the coil bytes in the buffer */
        memset(pucRegBuffer, 0, ((usNCoils / 8) + 1));

        for (i = 0; i < usNCoils; ++i) {
            /* calculate the coil number */
            coil = i + usAddress;

            switch (coil) {
                    /* digital outpt bits */
                case ISOPWR_CNTL_0:
                case ISOPWR_CNTL_1:
                case ISOPWR_CNTL_2:
                case ISOPWR_CNTL_3:
                case ISOPWR_CNTL_4:
                case ISOPWR_CNTL_5:
                case INSTR_CNTL_0: 
                case INSTR_CNTL_1: 
                case DIGITAL_OUT_0:
                case DIGITAL_OUT_1:
                case GF_CNTL_HIGH:
                case GF_CNTL_LOW:   
                case VICOR_CNTL:    
                                    bitSet(i, pucRegBuffer, doGetBit(coil)); break;
                case BLINKY_DEBUG:  bitSet(i, pucRegBuffer, bBlinkyDebug); break;
                case BLINKY_STATE:  bitSet(i, pucRegBuffer, doGetBlinky()); break;
                case H2O_ALARM_CLR: bitSet(i, pucRegBuffer, 0); break; //this is a momentary coil, and purely software, so always let it return zero.
                    //TODO REALLY INSTRUMENT PORTS
                    //                /* instrument port xcvr bits */
                    //                case  9: bitSet(i, pucRegBuffer, serPwrGet(0)); break;
                    //                case 10: bitSet(i, pucRegBuffer, serPwrGet(1)); break;
                    //                case 11: bitSet(i, pucRegBuffer, serPwrGet(2)); break;
                    //                /* instrument port power bits */
                    //                case 12: bitSet(i, pucRegBuffer, serXcvrGet(0)); break;
                    //                case 13: bitSet(i, pucRegBuffer, serXcvrGet(1)); break;
                    //                case 14: bitSet(i, pucRegBuffer, serXcvrGet(2)); break;
                    //                case 15: bitSet(i, pucRegBuffer, usBlinkyDebug); break;
                    //                case 16: bitSet(i, pucRegBuffer, usBlinkyState); break;
                default: break;
            }
        }

        return MB_ENOERR;
    }

    return MB_EIO;
}

eMBErrorCode
eMBRegDiscreteCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete) {
    
    int dinput, i;
    
    /* set the modbus frame received flag to true */
    bGotModbusFrame = TRUE;
    if ((bBlinkyDebug == FALSE))
        doToggleBlinky();
    
    /* only discrete inputs 1-6 are represented */
    if ((usAddress < REG_DISCRETE_START) ||
            (usAddress + usNDiscrete) > (REG_DISCRETE_START + REG_DISCRETE_NREGS))
        return MB_ENOREG;

    memset(pucRegBuffer, 0, ((usNDiscrete / 8) + 1));

        
    for (i = 0; i<usNDiscrete; i++) {
        dinput = i +usAddress;
        switch(dinput) {
		
            case DIGITAL_IN_0: 
                bitSet(i, pucRegBuffer, discreteReadReg(dinput)); 
                break;
            case DIGITAL_IN_1: 
                bitSet(i, pucRegBuffer, discreteReadReg(dinput));
                break;
            case CN_IN_0: 
                bitSet(i, pucRegBuffer, discreteReadReg(dinput)); 
                break;
            case CN_IN_1: 
                bitSet(i, pucRegBuffer, discreteReadReg(dinput)); 
                break;
            case H2O_SENSE:
                if (bWaterAlarm == TRUE) {//There's an alarm, that must be cleared by a coil write.
                    bitSet(i, pucRegBuffer, 1);
                }
                else
                {
                    // There is no fault
                    bitSet(i,pucRegBuffer,0);
                }
                break;

            default: break;    
        }
    }
    
    return MB_ENOERR;
}

#define ECHO_MODE 1
#define MAX_CHARS 32
int getCommand(char * cmd)
{
    static int char_count = 0;
    static char line[MAX_CHARS] = "";
    const unsigned char terminator = '\r';
    int got_line;
    unsigned char b;
    int i;

    got_line = FALSE;

    if (serGetByte(CONSOLE_PORT, &b))
    {
        if (ECHO_MODE)
        {
            serPutByte(CONSOLE_PORT, b);
            if (b == '\r') serPutByte(CONSOLE_PORT,'\n');
            //serPutByte(CONSOLE_PORT, 'b');

        }

        if (b != terminator) 
        {
            if ((b!='\b') && (char_count < MAX_CHARS))
                line[char_count++] = b;
            else if (char_count > 0)
                --char_count;

            cmd[0] = '\0';
        }
        else
        {
            got_line = TRUE;
            line[char_count] = '\0';
            for (i = 0; i <= char_count; i++)
                cmd[i] = line[i];
            char_count = 0;
        }
    }

    return got_line;
}

int processCommands() 
{
    int ret_val;
    static char cmd_buff[MAX_CHARS];
    char err_buff[32];
    static char* cmd_ptr;
    char sDebug[128];
    
    // Show a sign in banner
    serPutString(CONSOLE_PORT, "\r\r\r");
    Nop();
    
    sprintf(sDebug, "\r\nMBARI MiniROV LightController %s, %s \r\n", __DATE__, __TIME__);
    serPutString(CONSOLE_PORT, sDebug);
    
    for(;;)
    {
        // get command
        if ( !getCommand(cmd_buff) ) {
            ret_val = ERR_NO_LINE;
        }
        else {
            /* Skip white space here */
            cmd_ptr = prsSkip(cmd_buff, " \t");

            /* If the cmd from getCommand was empty then return here. */
            if ( !strlen(cmd_ptr) )
                ret_val = ERR_EMPTY_LINE;
            else
                ret_val = prsParse(cmd_ptr, cmdsRoot, ERR_NO_COMMAND);
        }

        /* display ERR code or RDY prompt */
        if ( ret_val != ERR_NO_LINE)
        {
            if ((ret_val != ERR_SUCCESS) && (ret_val != ERR_EMPTY_LINE))
            {
                sprintf(err_buff,"ERR %04d\r\n", ret_val);
                serPutString(CONSOLE_PORT, err_buff);
            }

            serPutString(CONSOLE_PORT, "RDY\r\n");
           
        }

        // Let's sample TODO this will end up being a test for the temp sensor next.  
//                sprintf(sDebug, "AIN GF_CURRENT: %u\r\n",analogReadReg(GF_CURRENT));
//               serPutString(CONSOLE_PORT,sDebug);

    }
    return ret_val;
}

/**
  End of File
  */