/* 
 * File: dig_out.c   
 * Author: E. Martin
 * Comments: 	digital output functions for control via modbus in MiniROV
 *
 *    
 * Revision history: Git via BitBucket 
 * 		https://bitbucket.org/mbari/minirov-piclightcontroller 
 * 
 * Copyright: 2016 MBARI, this is proprietary code, all rights reserved.
 *
 */
#include "dig_out.h"
#include "modbus_registers.h"
//#include "sys_timer.h"
#include <xc.h>
#include "sys_defs.h"

/* init registers */
void doInit()
{

}

/* power bit API */
void doSetBit(unsigned int addr, int state)
{
    /* state is non zero, set it to one */
    if ( state )
        state = 1;
    
    Nop();

    switch (addr)
    {
        case ISOPWR_CNTL_0: _LATD0  = state; break;
        case ISOPWR_CNTL_1: _LATC13 = state; break;
        case ISOPWR_CNTL_2: _LATC14 = state; break;
        case ISOPWR_CNTL_3: _LATD3  = state; break;
        case ISOPWR_CNTL_4: _LATD4  = state; break;
        case ISOPWR_CNTL_5: _LATD5  = state; break;
        case INSTR_CNTL_0:  _LATB12 = state; break;
        case INSTR_CNTL_1:  _LATB13 = state; break;
        case DIGITAL_OUT_0: _LATB14 = state; break;
        case DIGITAL_OUT_1: _LATB15 = state; break;
        case GF_CNTL_HIGH:  _LATD10 = state; break;
        case GF_CNTL_LOW:   _LATD11 = state; break;
        case VICOR_CNTL:    _LATF2  = state; break;
        //LATDBits.
        default: return;
    }
}

int doReadReg(unsigned int addr, unsigned short * val ) {
    
    unsigned short outval =0;
    
    if (addr == COIL_OUTPUT) {
        
        //ISOPOWERS

        outval |= (((doGetBit(ISOPWR_CNTL_0)==1)?1:0)   << 0);
        outval |= (((doGetBit(ISOPWR_CNTL_1)==1)?1:0)   << 1);
        outval |= (((doGetBit(ISOPWR_CNTL_2)==1)?1:0)   << 2);
        outval |= (((doGetBit(ISOPWR_CNTL_3)==1)?1:0)   << 3);
        outval |= (((doGetBit(ISOPWR_CNTL_4)==1)?1:0)   << 4);
        outval |= (((doGetBit(ISOPWR_CNTL_5)==1)?1:0)   << 5);

        //Instrument power status
        outval |= (((doGetBit(INSTR_CNTL_0)==1)?1:0)    << 6);
        outval |= (((doGetBit(INSTR_CNTL_1)==1)?1:0)    << 7);

        //Digital Output Status
        outval |= (((doGetBit(DIGITAL_OUT_0)==1)?1:0)   << 8);
        outval |= (((doGetBit(DIGITAL_OUT_1)==1)?1:0)   << 9);
        
        //GF Output control
        outval |= (((doGetBit(GF_CNTL_HIGH)==1)?1:0)    << 10);
        outval |= (((doGetBit(GF_CNTL_LOW)==1)?1:0)     << 11);

        //System vars and blinky
        //outval |= (((bBlinkyDebug==TRUE)?1:0)           << 12);
        outval |= (((doGetBlinky()==1)?1:0)             << 13);
        //outval |= (((bWaterAlarm==TRUE)?1:0)            << 14);
        outval |= (((doGetBit(VICOR_CNTL)==1)?1:0)        << 15);

        *val = outval;
        return 0;
    }
    else { return -1;}



}

int doGetBit(unsigned int addr)
{
    Nop();
    
    switch (addr)
    {
        case ISOPWR_CNTL_0: return (int)_RD0 ; break;
        case ISOPWR_CNTL_1: return (int)_RC13; break;
        case ISOPWR_CNTL_2: return (int)_RC14; break;
        case ISOPWR_CNTL_3: return (int)_RD3 ; break;
        case ISOPWR_CNTL_4: return (int)_RD4 ; break;
        case ISOPWR_CNTL_5: return (int)_RD5 ; break;
        case INSTR_CNTL_0:  return (int)_RB12; break;
        case INSTR_CNTL_1:  return (int)_RB13; break;
        case DIGITAL_OUT_0: return (int)_RB14; break;
        case DIGITAL_OUT_1: return (int)_RB15; break;
        case GF_CNTL_HIGH:  return (int)_RD10; break;
        case GF_CNTL_LOW:   return (int)_RD11; break;
        case VICOR_CNTL:    return (int)_RF2; break;
                
        default: return -1;
    }
}
void doSetBlinky(int state)
{
    Nop();
    
    if ( state )
        _LATB3 = 1;
    else
        _LATB3 = 0;
    
    Nop();
}

void doToggleBlinky() {
    Nop();
    if (doGetBlinky() == 1)
        _LATB3 = 0;
    else 
        _LATB3 = 1;
}

int doGetBlinky()
{
    Nop();
    return (int)_RB3;
}