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

Periphery::Periphery() : 
    _canb(canSpace::CANA),
    _digitalIO(),
    _zone6_ptr(reinterpret_cast<Uint16*>(0x100000)),
    _scib(ScibRegs),
    _modbusSetup(),
    _modbusConfiguration(),
    _crc(),
    _intervalMeasure(CpuTimer2),
    _modbusPort(_scib, _intervalMeasure, _crc),
    _ASUTP(_crc),
    _modbusInitFlag(false)
{
    _softVersion.DSP = 202;
    _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(_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----------------------------------------------------------
    _modbusSetup.gpio_re_de_setup = &DSP28335::GPIO::gpio_scib_re_de_setup;
    _modbusSetup.gpio_driver_enable = &DSP28335::GPIO::gpio_scib_re_de_set;
    _modbusSetup.gpio_receiver_enable = &DSP28335::GPIO::gpio_scib_re_de_clear;

    _intervalMeasure.reset();
    _intervalMeasure.set_magic(19);

    _modbusPort.setup(_modbusSetup);

    // modbus_port_configuration.node_id         = 0x5;
    // modbus_port_configuration.config.baudrate = DSP28335::BR9600;
    // modbus_port_configuration.config.parity   = DSP28335::NO;
    // modbus_port_configuration.config.stopbits = DSP28335::ONE;
    // modbus_port_configuration.config.lenght   = DSP28335::LEN8;

    // modbus_port.configure(modbus_port_configuration);

    // clear_array((uint16_t *)hmi.rxStack, sizeof(hmi.rxStack)/sizeof(uint16_t));
    // clear_array((uint16_t *)hmi.txStack, sizeof(hmi.txStack)/sizeof(uint16_t));

    _modbusPort.setRXBuffer((uint16_t*)_ASUTP.rxStack, &_ASUTP.rxLength);
    _modbusPort.setTXBuffer((uint16_t*)_ASUTP.txStack, &_ASUTP.txLength);

}


Uint16 Periphery::getVersionFPGA(){
    Uint16 data;
    data = *_zone6_ptr + interface::SOFT_VERSION_OFFSET;
    return data;
}


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


void Periphery::processDigitalInput(){
    Uint16 data = 0;
    _digitalIO.readDigitalIO(data);
    _canb.transmitMsg(canSpace::DIGITAL_INPUT_MBOX, (data));
}


void Periphery::processDigitalOutput(){
    if(_canb.isNewMessage(canSpace::DIGITAL_OUTPUT_MBOX)){
        _canb.receiveMsg(canSpace::DIGITAL_OUTPUT_MBOX, _message);
        Uint16 data = _message.mdl.word.LOW_WORD;
        _digitalIO.writeDigitalIO(data);
    }
}


bool Periphery::isModbusInit() const{
    return _modbusInitFlag;
}


void Periphery::receiveModbusParameters(){
    if (!_modbusInitFlag && (_canb.isNewMessage(canSpace::MODBUS_SETTINGS_MBOX))) { // TODO check for init is incorect here maybe
        _canb.receiveMsg(canSpace::MODBUS_SETTINGS_MBOX, _message);

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

        _modbusPort.configure(_modbusConfiguration);

        _modbusInitFlag = true; // TODO modbus init function must be here
    }
}


void Periphery::test_init_hmi_buffers()
{
    //
    // hmi writeable registers
    _ASUTP.inputRegisters.set(WEINBUS::INPUTREGISTERS, 400);
    _ASUTP.inputRegisters.add( 0, &test_hmi_float_reg_400.f);
    _ASUTP.inputRegisters.add( 1, &(float&)test_hmi_float_reg_401.f);
    _ASUTP.inputRegisters.add( 2, &(float&)test_hmi_float_reg_402.f);
    _ASUTP.inputRegisters.add( 3, &(float&)test_hmi_float_reg_403.f);
    _ASUTP.inputRegisters.add( 4, &(float&)test_hmi_float_reg_404.f);
    //
    // hmi readable registers
    _ASUTP.outputRegisters.set(WEINBUS::OUTPUTREGISTERS, 400);
    _ASUTP.outputRegisters.add( 0 , &(float&)test_hmi_float_reg_400.f);
    _ASUTP.outputRegisters.add( 1, &(float&)test_hmi_float_reg_401.f);
    _ASUTP.outputRegisters.add( 2, &(float&)test_hmi_float_reg_402.f);
    _ASUTP.outputRegisters.add( 3, &(float&)test_hmi_float_reg_403.f);
    _ASUTP.outputRegisters.add( 4, &(float&)test_hmi_float_reg_404.f);
    //
}//

// void Periphery::addInputRegFloat(uint16_t addr, float& param){
//     // if(_ASUTP.inputRegisters.address_range(addr)){
//     //     return;
//     // }
//     _ASUTP.inputRegisters.add( addr, &param);
//     _ASUTP.outputRegisters.add( addr, &param);
// }

// void Periphery::modbusExecute(){
//     if(_modbusPort.compare_state(MODBUSRTU::BREAK))
//     {
//         _modbusPort.port_reset();
//         //
//     }
//     else
//     {
//         _modbusPort.execute();
//         _ASUTP.execute();
//         //
//     }//if else
// }


// void Periphery::setModbusBuffers(){
//     _modbusPort.setRXBuffer((uint16_t*)_ASUTP.rxStack, &_ASUTP.rxLength);
//     _modbusPort.setTXBuffer((uint16_t*)_ASUTP.txStack, &_ASUTP.txLength);
// }