#include "Periphery.h"
#include "Protocol/CAN.h"

Periphery::Periphery() : 
    _cana(can_space::CANA),
    _digitalIO(),
    _zone6_ptr(reinterpret_cast<uint16_t*>(0x100000)),
    _scib(ScibRegs),
    _modbusRegCounter(0),
    modbusRTU(CpuTimer2, _scib),
    _error(false)
{
    _softVersion.DSP = 202;
    _softVersion.CPLD = 0;
}


void Periphery::config(){

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

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

    // Remote frame MBOX
    _cana.configTxMBox(can_space::COMM_VERSION_MBOX, can_space::MsgID(0x1, false, true), can_space::MsgCtrlReg(0x4));

    _cana.enableTimeOutControl(can_space::MODBUS_SETTINGS_MBOX);
    _cana.setTimeOutValue(can_space::MODBUS_SETTINGS_MBOX, 1000);

    // Interrupts
    _cana.configSystemIsr(can_space::I0EN_ENABLE |
                          can_space::EPIM_ENABLE | can_space::WLIM_ENABLE | can_space::WDIM_ENABLE);// | can_space::AAIM_ENABLE);


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


    //---------------------------------------Init DIO interface----------------------------------------------------------
    _digitalIO.setup(_zone6_ptr);
    _digitalIO.setMemoryOffset(interface::DISCRETE_DATA_OFFSET);


    //---------------------------------------Init SCI interface----------------------------------------------------------    
    DSP28335::SCISetup sciBSetup;
    sciBSetup.config.baudrate = SCIB_BAUDRATE_DEFAULT;
    sciBSetup.config.parity   = SCIB_PARITY_DEFAULT;
    sciBSetup.config.stopbits = SCIB_STOPBITS_DEFAULT;
    sciBSetup.config.lenght   = SCIB_LENGHT_DEFAULT;
    sciBSetup.gpio_setup      = SCIB_GPIO_SETUP_DEFAULT;

    _scib.setup(sciBSetup);


    //------------------------------------------Init Modbus----------------------------------------------------------
    MODBUSRTU::modbusSetup.gpio_re_de_setup = &DSP28335::GPIO::gpio_scib_re_de_setup;
    MODBUSRTU::modbusSetup.gpio_driver_enable = &DSP28335::GPIO::gpio_scib_re_de_set;
    MODBUSRTU::modbusSetup.gpio_receiver_enable = &DSP28335::GPIO::gpio_scib_re_de_clear;
    modbusRTU.setup(MODBUSRTU::modbusSetup);
}


uint16_t Periphery::getVersionFPGA(){
    uint16_t data;
    data = *_zone6_ptr + interface::SOFT_VERSION_OFFSET;
    return data & 0x3FF;    // no more than 1023. (9.9.9) should be limit
}


void Periphery::updateVersionFPGA(){
    _softVersion.CPLD = getVersionFPGA();
    _message.mdl.word.LOW_WORD = _softVersion.DSP;
    _message.mdl.word.HI_WORD = _softVersion.CPLD;
    _cana.updateTXMessage(can_space::COMM_VERSION_MBOX, _message);
}


void Periphery::receiveModbusParameters(){
    if (_cana.isNewMessage(can_space::MODBUS_SETTINGS_MBOX)) {
        _cana.receiveMsg(can_space::MODBUS_SETTINGS_MBOX, _message);

        MODBUSRTU::modbusConfiguration.node_id = _message.mdl.byte.BYTE0;
        MODBUSRTU::modbusConfiguration.config.lenght = static_cast<DSP28335::SCICharLenght>(_message.mdl.byte.BYTE1);
        MODBUSRTU::modbusConfiguration.config.baudrate = static_cast<DSP28335::SCIBaudRate>(_message.mdl.word.LOW_WORD);
        MODBUSRTU::modbusConfiguration.config.parity = static_cast<DSP28335::SCIParity>(_message.mdh.byte.BYTE4);
        MODBUSRTU::modbusConfiguration.config.stopbits = static_cast<DSP28335::SCIStopBits>(_message.mdh.byte.BYTE5);

        modbusRTU.configure(MODBUSRTU::modbusConfiguration);

        // Future needs (maybe)
        // Some bits are set before initialization process completed (CPU already at work for example).
        // They should be reset for not to get false interrupt
        // ECanaRegs.CANAA.all = 0xFFFFFFFF;

        _modbusRegCounter = 0;
    }
}


void Periphery::getModbusConfiguration(){
    _cana.resetTimeStampCounter();
    _cana.clearTimeOutFlag(can_space::MODBUS_SETTINGS_MBOX);
    bool requestIsSent = false;

    while(!_cana.isNewMessage(can_space::MODBUS_SETTINGS_MBOX)){
        if(_cana.isTimeOut(can_space::MODBUS_SETTINGS_MBOX)){
            if (!requestIsSent) {
                _cana.resetTimeStampCounter();
                _cana.clearTimeOutFlag(can_space::MODBUS_SETTINGS_MBOX);

                _message.mdl.word.LOW_WORD = _softVersion.DSP;
                _message.mdl.word.HI_WORD = _softVersion.CPLD;
                _cana.transmitMsg(can_space::COMM_VERSION_MBOX, _message);

                requestIsSent = true;
            }
            else{
                _cana.clearTimeOutFlag(can_space::MODBUS_SETTINGS_MBOX);
                _cana.disableTimeOutControl(can_space::MODBUS_SETTINGS_MBOX);
                _error = true;
                return;
            }
        }
    }
    _cana.disableTimeOutControl(can_space::MODBUS_SETTINGS_MBOX);
    
    // Future needs (maybe)
    // Some bits are set before initialization process completed (CPU already at work for example).
    // They should be reset for not to get false interrupt
    // ECanaRegs.CANAA.all = 0xFFFFFFFF;

    _cana.receiveMsg(can_space::MODBUS_SETTINGS_MBOX, _message);
    MODBUSRTU::modbusConfiguration.node_id = _message.mdl.byte.BYTE0;
    MODBUSRTU::modbusConfiguration.config.lenght = static_cast<DSP28335::SCICharLenght>(_message.mdl.byte.BYTE1);
    MODBUSRTU::modbusConfiguration.config.baudrate = static_cast<DSP28335::SCIBaudRate>(_message.mdl.word.LOW_WORD);
    MODBUSRTU::modbusConfiguration.config.parity = static_cast<DSP28335::SCIParity>(_message.mdh.byte.BYTE4);
    MODBUSRTU::modbusConfiguration.config.stopbits = static_cast<DSP28335::SCIStopBits>(_message.mdh.byte.BYTE5);

    modbusRTU.configure(MODBUSRTU::modbusConfiguration);
    _modbusRegCounter = 0;
}


#pragma CODE_SECTION("ramfuncs");
void Periphery::processDigitalInput(){
    uint16_t data = 0;
    _digitalIO.readDigitalIO(data);
    data = ~data;
    _cana.transmitMsg(can_space::DIGITAL_INPUT_MBOX, data);
}


#pragma CODE_SECTION("ramfuncs");
void Periphery::processDigitalOutput(){
    if(_cana.isNewMessage(can_space::DIGITAL_OUTPUT_MBOX)){
        _cana.receiveMsg(can_space::DIGITAL_OUTPUT_MBOX, _message);
        uint16_t data = ~_message.mdl.word.LOW_WORD;
        _digitalIO.writeDigitalIO(data);
    }
}


#pragma CODE_SECTION("ramfuncs");
void Periphery::sendModbusDataToCPU(){
    _message.mdl.all = 0;   // TODO delete maybe?
    _message.mdh.all = 0;

    WEINBUS::WeinbusTableRegister reg;
    reg = modbusRTU.dataHandler.outputRegisters.get_register_cursor(_modbusRegCounter);

    if(modbusRTU.dataHandler.outputRegisters.address_range(reg.get_address())){
        _message.mdl.all = reg.get_address();
        reg.read(_message.mdh.all);
    }
    else {
        _modbusRegCounter = 0;
        reg = modbusRTU.dataHandler.outputRegisters.get_register_cursor(_modbusRegCounter);

        _message.mdl.all = reg.get_address();
        reg.read(_message.mdh.all);
    }
    _modbusRegCounter++;

    _cana.transmitMsg(can_space::MODBUS_DATA_COMM_TO_CPU_MBOX, _message);
}


#pragma CODE_SECTION("ramfuncs");
void Periphery::receiveCpuModbusData(){
    if (_cana.isNewMessage(can_space::MODBUS_DATA_CPU_TO_COMM_MBOX)) {
        _cana.receiveMsg(can_space::MODBUS_DATA_CPU_TO_COMM_MBOX, _message);

        WEINBUS::WeinbusTableRegister reg;

        uint32_t addr = _message.mdl.all;
        reg = modbusRTU.dataHandler.inputRegisters.get_register_cursor(addr - modbusRTU.dataHandler.inputRegisters.get_start_address());
        reg.write_data(_message.mdh.all);
    }
}


void Periphery::canSystemInterruptHandle(){
    _cana.busErrorInterruptFlagsReset();
    _error = true;
}