#include "DSP2833x_Device.h"
#include "DSP28x_Project.h"
#include "Protocol/CAN.h"
#include "Protocol/CAN_data.h"
#include "Protocol/DigitalIO.h"
#include "Periphery.h"

Periphery::Periphery(): 
    _canb(canSpace::CANB),
    _digitalIO(),
    _zone0_ptr(reinterpret_cast<Uint16*>(0x4000)),
    _modbusSettingsMsg(),
    _modbusInit(false)
{
    _softVersion.DSP = 101;
    _softVersion.CPLD = 0;
}


void Periphery::config(){

    //--------------------------------------Init CAN interface----------------------------------------------------------
    _canb.initGpio();
    _canb.config();

    // Data Frame MBOX
    _canb.configRxMBox(canSpace::MODBUS_SETTINGS_MBOX, canSpace::MsgID(0x0), canSpace::MsgCtrlReg(0x6));   // Modbus settings
    // _canb.configTxMBox(canSpace::MODBUS_DATA_COMM_TO_CPU_MBOX, canSpace::MsgID(0x29), canSpace::MsgCtrlReg(0x26)); // Send Modbus data to CPU
    // _canb.configRxMBox(canSpace::MODBUS_DATA_CPU_TO_COMM_MBOX, canSpace::MsgID(0x28), canSpace::MsgCtrlReg(0x6));  // Receive Modbus data from CPU
    _canb.configTxMBox(canSpace::DIGITAL_INPUT_MBOX, canSpace::MsgID(0x30), canSpace::MsgCtrlReg(0x2));    // Receive DI
    _canb.configRxMBox(canSpace::DIGITAL_OUTPUT_MBOX, canSpace::MsgID(0x31), canSpace::MsgCtrlReg(0x2));   // Send DO

    // Remote frame MBOX
    _canb.configTxMBox(canSpace::COMM_VERSION_MBOX, canSpace::MsgID(0x1, false, true), canSpace::MsgCtrlReg(0));

    // Interrupts
    _canb.configSystemIsr(canSpace::I0EN_ENABLE |
                         canSpace::EPIM_ENABLE | canSpace::WLIM_ENABLE | canSpace::AAIM_ENABLE);


    //--------------------------------------Init XINTF interface----------------------------------------------------------
    InitXintf();


    //---------------------------------------Init DIO interface----------------------------------------------------------
    _digitalIO.setup(_zone0_ptr);
    _digitalIO.setMemoryOffset(1);

}


Uint16 Periphery::getVersionFPGA(){
    Uint16 data;
    data = *_zone0_ptr;
    return data;
}


void Periphery::updateVersionFPGA(){
    _softVersion.CPLD = getVersionFPGA() & 0x3FF;   // no more than 1023 (9.9.9) should be limit
    canSpace::CANMessage data;
    data.mdl.word.LOW_WORD = _softVersion.DSP;
    data.mdl.word.HI_WORD = _softVersion.CPLD;
    _canb.updateTXMessage(canSpace::COMM_VERSION_MBOX, data);
}


void Periphery::processDigitalInput(){
    Uint16 data = 0;
    // _digitalIO.readDigitalIO(data);
    data = 0x1234;
    _canb.transmitMsg(canSpace::DIGITAL_INPUT_MBOX, (data & 0x3FF)); // TODO check! может быть лишнее затирание битов?
}


void Periphery::processDigitalOutput(){
    if(_canb.isNewMessage(canSpace::DIGITAL_OUTPUT_MBOX)){
        canSpace::CANMessage data;
        _canb.receiveMsg(canSpace::DIGITAL_OUTPUT_MBOX, data);
        Uint16 temp;
        temp = data.mdl.all;
        // _digitalIO.writeDigitalIO(temp);
    }
}


void Periphery::initExternalModbus(){
    if (!_modbusInit && (_canb.isNewMessage(canSpace::MODBUS_SETTINGS_MBOX))) {
        _canb.receiveMsg(canSpace::MODBUS_SETTINGS_MBOX, _modbusSettingsMsg);
        _modbusInit = true;
    }
}