#include "CAN.h"
#include "DSP2833x_ECan.h"

namespace can_space {

CAN::CAN(CAN_VARIANT canVariant) : 
    _canPort(canVariant),
    _p_CanRegs(0),
    _p_CanMBoxes(0),
    _p_CanMotoRegs(0),
    _p_CanMotsRegs(0)
{}


void CAN::initGpio(){
    if(_canPort == CANA) InitECanaGpio();
    else if (_canPort == CANB) InitECanbGpio();
}

void CAN::config(uint16_t baudrate, uint16_t flags){
    if (_canPort == CANA){
		EALLOW;
		SysCtrlRegs.PCLKCR0.bit.ECANAENCLK = 1;
		EDIS;
		_p_CanRegs = &ECanaRegs;
		_p_CanMBoxes = &ECanaMboxes;
        _p_CanMotoRegs = &ECanaMOTORegs;
        _p_CanMotsRegs = &ECanaMOTSRegs;
	}
	else if (_canPort == CANB){
		EALLOW;
		SysCtrlRegs.PCLKCR0.bit.ECANBENCLK = 1;
		EDIS;
		_p_CanRegs = &ECanbRegs;
		_p_CanMBoxes = &ECanbMboxes;
        _p_CanMotoRegs = &ECanbMOTORegs;
        _p_CanMotsRegs = &ECanbMOTSRegs;
	}
	else { return; }

    // Create a shadow register structure for the CAN control registers. This   // TODO add this into description
    // is needed, since only 32-bit access is allowed to these registers. 
    // 16-bit access to these registers could potentially corrupt the register
    // contents or return false data. This is especially true while writing 
    // to/reading from a bit (or group of bits) among bits 16 - 31

    EALLOW;

    // Configure eCAN RX and TX pins for CAN operation using eCAN regs
    _CanShadow.CANTIOC.all = _p_CanRegs->CANTIOC.all;
    _CanShadow.CANTIOC.bit.TXFUNC = 1;
    _p_CanRegs->CANTIOC.all = _CanShadow.CANTIOC.all;

    _CanShadow.CANRIOC.all = _p_CanRegs->CANRIOC.all;
    _CanShadow.CANRIOC.bit.RXFUNC = 1;
    _p_CanRegs->CANRIOC.all = _CanShadow.CANRIOC.all;


    // Configure eCAN for HECC mode - (reqd to access mailboxes 16 thru 31)
    // HECC mode also enables time-stamping feature
    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    _CanShadow.CANMC.bit.SCB = 1;
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;


    // Initialize all bits of 'Master Control Field' to zero
    // Some bits of MSGCTRL register come up in an unknown state. For proper 
    // operation, all bits (including reserved bits) of MSGCTRL must be 
    // initialized to zero
    _p_CanMBoxes->MBOX0.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX1.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX2.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX3.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX4.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX5.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX6.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX7.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX8.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX9.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX10.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX11.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX12.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX13.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX14.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX15.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX16.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX17.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX18.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX19.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX20.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX21.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX22.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX23.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX24.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX25.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX26.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX27.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX28.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX29.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX30.MSGCTRL.all = 0x00000000;
    _p_CanMBoxes->MBOX31.MSGCTRL.all = 0x00000000;

    // TAn, RMPn, GIFn bits are all zero upon reset and are cleared again
    // as a matter of precaution.
    _p_CanRegs->CANTA.all	= 0xFFFFFFFF;
    _p_CanRegs->CANRMP.all = 0xFFFFFFFF;
    _p_CanRegs->CANGIF0.all = 0xFFFFFFFF;
    _p_CanRegs->CANGIF1.all = 0xFFFFFFFF;


    // Configure bit timing parameters for eCANA
    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    _CanShadow.CANMC.bit.CCR = 1 ;
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;

    do { _CanShadow.CANES.all = _p_CanRegs->CANES.all; }
    while(_CanShadow.CANES.bit.CCE != 1 );

    // MSB - 0; LSB - 1
    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    if (flags & MSB_ENABLE){
        _CanShadow.CANMC.bit.DBO = 0;
    }
    else
    { _CanShadow.CANMC.bit.DBO = 1; }
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;

    _CanShadow.CANBTC.all = 0;

    // The following block for all 150 MHz SYSCLKOUT 
    // (75 MHz CAN clock) - default. Bit rate = 1 Mbps / 500 kbps / 250 kbps / 100 kbps
    switch (baudrate) {
        case 1000:
            _CanShadow.CANBTC.bit.BRPREG = 4;
            _CanShadow.CANBTC.bit.TSEG2REG = 3;
            _CanShadow.CANBTC.bit.TSEG1REG = 9;
        break;

        case 500:
            _CanShadow.CANBTC.bit.BRPREG = 9;
            _CanShadow.CANBTC.bit.TSEG2REG = 3;
            _CanShadow.CANBTC.bit.TSEG1REG = 9;
        break;

        case 250:
            _CanShadow.CANBTC.bit.BRPREG = 19;
            _CanShadow.CANBTC.bit.TSEG2REG = 3;
            _CanShadow.CANBTC.bit.TSEG1REG = 9;
        break;

        case 100:
            _CanShadow.CANBTC.bit.BRPREG = 49;
            _CanShadow.CANBTC.bit.TSEG2REG = 3;
            _CanShadow.CANBTC.bit.TSEG1REG = 9;
        break;

        default: return;
    }

    _CanShadow.CANBTC.bit.SAM = 1;
    _p_CanRegs->CANBTC.all = _CanShadow.CANBTC.all;

    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    _CanShadow.CANMC.bit.CCR = 0 ;
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;

    do { _CanShadow.CANES.all = _p_CanRegs->CANES.all; }
    while(_CanShadow.CANES.bit.CCE != 0 ); // Wait for CCE bit to be  cleared

    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    _CanShadow.CANMC.bit.ABO = 1;
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;

    // Disable all Mailboxes
    _p_CanRegs->CANME.all = 0;

    // Disable all interrupts
    _p_CanRegs->CANGIM.all = 0;
    _p_CanRegs->CANMIM.all = 0;

    //
    // Debug feature
    // Configure the eCAN for self test mode.
    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    if (flags & STM_ENABLE){
        _CanShadow.CANMC.bit.STM = 1;
    }
    else
    { _CanShadow.CANMC.bit.STM = 0; }
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;

    EDIS;
}


void CAN::configTxMBox(uint16_t boxNumber, const MsgID& configID, const MsgCtrlReg& configCtrlReg){
    if (boxNumber > 31) return;

    volatile MBOX* p_MailBox(0);
    p_MailBox = &(_p_CanMBoxes->MBOX0) + boxNumber;

    uint32_t mboxControl(0);
    mboxControl = 1ul << boxNumber;

    // Reset transmittion
    if (_CanShadow.CANTRS.all &= mboxControl)
    {
        _CanShadow.CANTRR.all = _p_CanRegs->CANTRR.all;
        _CanShadow.CANTRR.all |= mboxControl;
        _p_CanRegs->CANTRR.all = _CanShadow.CANTRR.all;
        do {_CanShadow.CANTRS.all = _p_CanRegs->CANTRS.all;}
        while((_CanShadow.CANTRS.all & mboxControl) != 0);      // Wait for TRS bit to be cleared
    }

    // Mailbox disable
    _CanShadow.CANME.all = _p_CanRegs->CANME.all;
    _CanShadow.CANME.all &= ~(mboxControl);
    _p_CanRegs->CANME.all = _CanShadow.CANME.all;

    // Write to the MSGID field
    p_MailBox->MSGID.all = configID.all;

    // Mailbox direction - transmit
    _CanShadow.CANMD.all = _p_CanRegs->CANMD.all;
    _CanShadow.CANMD.all &= ~(mboxControl);
    _p_CanRegs->CANMD.all = _CanShadow.CANMD.all;

    // Config MBOX control reg
    p_MailBox->MSGCTRL.all = configCtrlReg.all;

    // Mailbox enable
    _CanShadow.CANME.all = _p_CanRegs->CANME.all;
    _CanShadow.CANME.all |= mboxControl;
    _p_CanRegs->CANME.all = _CanShadow.CANME.all;
}


void CAN::configRxMBox(uint16_t boxNumber, const MsgID& configID, const MsgCtrlReg& configCtrlReg){
    if (boxNumber > 31) return;

    volatile MBOX* p_MailBox(0);
    p_MailBox = &(_p_CanMBoxes->MBOX0) + boxNumber;

    uint32_t mboxControl(0);
    mboxControl = 1ul << boxNumber;

    // Mailbox disable
    _CanShadow.CANME.all = _p_CanRegs->CANME.all;
    _CanShadow.CANME.all &= ~(mboxControl);
    _p_CanRegs->CANME.all = _CanShadow.CANME.all;

    // Write to the MSGID field
    p_MailBox->MSGID.all = 0x0;
    p_MailBox->MSGID.all = configID.all;

    // Write to DLC and RTR field in control reg
    p_MailBox->MSGCTRL.bit.DLC = configCtrlReg.bit.DLC;
    p_MailBox->MSGCTRL.bit.RTR = configCtrlReg.bit.RTR;

    // Configure Mailbox under test as a Receive mailbox
    _CanShadow.CANMD.all = _p_CanRegs->CANMD.all;
    _CanShadow.CANMD.all |= mboxControl;
    _p_CanRegs->CANMD.all = _CanShadow.CANMD.all;

    // Overwrite protection
    // If "ON" make sure that an additional mailbox is configured to store ’overflow’ messages.
    if(configCtrlReg.bit.OPC){
        _CanShadow.CANOPC.all = _p_CanRegs->CANOPC.all;
        _CanShadow.CANOPC.all |= mboxControl;
        _p_CanRegs->CANOPC.all = _CanShadow.CANOPC.all;
    }

    // Enable Mailbox
    _CanShadow.CANME.all = _p_CanRegs->CANME.all;
    _CanShadow.CANME.all |= mboxControl;
    _p_CanRegs->CANME.all = _CanShadow.CANME.all;
}


void CAN::configSystemIsr(uint32_t flags){
    EALLOW;
    _p_CanRegs->CANGIM.all = flags;
    EDIS;
}


void CAN::configMBoxIsr(uint16_t boxNumber){
    if (boxNumber > 31) return;

    uint32_t mboxControl(0);
    mboxControl = 1ul << boxNumber;

    EALLOW;
    _p_CanRegs->CANMIL.all |= mboxControl;
    _p_CanRegs->CANMIM.all |= mboxControl;
    EDIS;
}


void CAN::enableTimeOutControl(uint16_t boxNumber){
    if (boxNumber > 31) return;

    uint32_t mboxControl(0);

    mboxControl = _p_CanRegs->CANTOC.all;
    mboxControl |= 1ul << boxNumber;
    _p_CanRegs->CANTOC.all = mboxControl;
}


void CAN::disableTimeOutControl(uint16_t boxNumber){
    if (boxNumber > 31) return;

    uint32_t mboxControl(0);

    mboxControl = _p_CanRegs->CANTOC.all;
    mboxControl &= ~(1ul << boxNumber);
    _p_CanRegs->CANTOC.all = mboxControl;
}


} //can_space