#include "CAN.h"

namespace can_space {

#pragma CODE_SECTION("ramfuncs");
void CAN::transmitMsg(uint16_t boxNumber, const CANMessage& message, bool writeDataLength){
    if (boxNumber > 31) return;

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

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

    p_MailBox->MDH.all = 0x0;
    p_MailBox->MDL.all = 0x0;

    if (writeDataLength) p_MailBox->MSGCTRL.bit.DLC = message.dataLength;
    p_MailBox->MDH.all = message.mdh.all;
    p_MailBox->MDL.all = message.mdl.all;

    // Set TRS for mailbox
    _CanShadow.CANTRS.all = 0;
    _CanShadow.CANTRS.all |= mboxControl;   
    _p_CanRegs->CANTRS.all = _CanShadow.CANTRS.all;

    // Wait for TA bit to be set
    // do { _CanShadow.CANTA.all = _p_CanRegs->CANTA.all; }
    // while((_CanShadow.CANTA.all & mboxControl) == 0 );

    // // Clear TA (transmit acknowledge bit)
    // _CanShadow.CANTA.all = 0;
    // _CanShadow.CANTA.all |= mboxControl;
    // _p_CanRegs->CANTA.all = _CanShadow.CANTA.all;
}


#pragma CODE_SECTION("ramfuncs");
void CAN::transmitMsg(uint16_t boxNumber, const uint64_t& message, const uint16_t dlc){
    if (boxNumber > 31) return;

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

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

    p_MailBox->MDH.all = 0x0;
    p_MailBox->MDL.all = 0x0;

    p_MailBox->MSGCTRL.bit.DLC = dlc;
    p_MailBox->MDH.all = message >> 32;
    p_MailBox->MDL.all = message;

    // Set TRS for mailbox
    _CanShadow.CANTRS.all = 0;
    _CanShadow.CANTRS.all |= mboxControl;   
    _p_CanRegs->CANTRS.all = _CanShadow.CANTRS.all;

    // Wait for TA bit to be set
    // do { _CanShadow.CANTA.all = _p_CanRegs->CANTA.all; }
    // while((_CanShadow.CANTA.all & mboxControl) == 0 );

    // Clear TA (transmit acknowledge bit)
    // _CanShadow.CANTA.all = 0;
    // _CanShadow.CANTA.all |= mboxControl;
    // _p_CanRegs->CANTA.all = _CanShadow.CANTA.all;
}


#pragma CODE_SECTION("ramfuncs");
void CAN::transmitMsg(uint16_t boxNumber, const uint64_t& message){
    if (boxNumber > 31) return;

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

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

    p_MailBox->MDH.all = 0x0;
    p_MailBox->MDL.all = 0x0;
    
    p_MailBox->MDH.all = message >> 32;
    p_MailBox->MDL.all = message;

    // Set TRS for mailbox
    _CanShadow.CANTRS.all = 0;
    _CanShadow.CANTRS.all |= mboxControl;   
    _p_CanRegs->CANTRS.all = _CanShadow.CANTRS.all;

    // Wait for TA bit to be set
    // do { _CanShadow.CANTA.all = _p_CanRegs->CANTA.all; }
    // while((_CanShadow.CANTA.all & mboxControl) == 0 );

    // Clear TA (transmit acknowledge bit)
    // _CanShadow.CANTA.all = 0;
    // _CanShadow.CANTA.all |= mboxControl;
    // _p_CanRegs->CANTA.all = _CanShadow.CANTA.all;
}


void CAN::updateTXMessage(uint16_t boxNumber, const CANMessage& message){ // TODO check trs bit and set it at the end if it was here. Once the TRS bit is set for a mailbox and then data is changed in the mailbox using the CDR
                                                                        // bit, the CAN module fails to transmit the new data and transmits the old data instead. To avoid this,
                                                                        // reset transmission in that mailbox using the TRRn bit and set the TRSn bit again. The new data is
                                                                        // then transmitted.
    if (boxNumber > 31) return;

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

    // Set change data request (CDR bit + MBOX number)
    _p_CanRegs->CANMC.all |= (128 + boxNumber);

    p_MailBox->MSGCTRL.bit.DLC = message.dataLength;
    p_MailBox->MDL.all = message.mdl.all;
    p_MailBox->MDH.all = message.mdh.all;

    _CanShadow.CANMC.all = _p_CanRegs->CANMC.all;
    _CanShadow.CANMC.bit.CDR = 0;
    _p_CanRegs->CANMC.all = _CanShadow.CANMC.all;
}


#pragma CODE_SECTION("ramfuncs");
void CAN::sendRemoteRequest(uint16_t boxNumber){
    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;

    p_MailBox->MSGCTRL.bit.RTR = 1;

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

    // Set TRS for mailbox
    _CanShadow.CANTRS.all = 0;
    _CanShadow.CANTRS.all |= mboxControl;
    _p_CanRegs->CANTRS.all = _CanShadow.CANTRS.all;
}


#pragma CODE_SECTION("ramfuncs");
int16_t CAN::receiveMsg(uint16_t boxNumber, CANMessage& rxMessage){
    if (boxNumber > 31) { return -1; }

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

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

    bool isNewMessageInBox = _p_CanRegs->CANRMP.all & mboxControl;
    if(!isNewMessageInBox) return -2;

    _p_CanRegs->CANRMP.all &= mboxControl;

    rxMessage.dataLength = p_MailBox->MSGCTRL.all;
    rxMessage.mdl.all = p_MailBox->MDL.all;
    rxMessage.mdh.all = p_MailBox->MDH.all;

    bool newMessage;
    bool lostMessage;

    newMessage = _p_CanRegs->CANRMP.all & mboxControl;
    lostMessage = _p_CanRegs->CANRML.all & mboxControl;

    if(newMessage || lostMessage) {
        return -3;
    }

    return 0;
}


#pragma CODE_SECTION("ramfuncs");
bool CAN::isNewMessage(){
    return static_cast<bool>(_p_CanRegs->CANRMP.all);
}


#pragma CODE_SECTION("ramfuncs");
bool CAN::isNewMessage(uint16_t boxNumber){
    if (boxNumber > 31) { return 0; }

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

    bool isNewMessageInBox = _p_CanRegs->CANRMP.all & mboxControl;
    return isNewMessageInBox;
}


void CAN::resetTimeStampCounter(){
    EALLOW;
    _p_CanRegs->CANTSC = 0;
    EDIS;
}


void CAN::setTimeOutValue(uint16_t boxNumber, uint32_t canBitsNumber){
    if (boxNumber > 31) { return; }

    volatile uint32_t* p_MailBox(0);
    p_MailBox = &(_p_CanMotoRegs->MOTO0) + boxNumber;

    *p_MailBox = canBitsNumber;
}


#pragma CODE_SECTION("ramfuncs");
bool CAN::isTimeOut(uint16_t boxNumber){
    bool mBOXTimeOut = false;
    mBOXTimeOut = _p_CanRegs->CANTOS.all & (1ul << boxNumber);
    return mBOXTimeOut;
}


void CAN::clearTimeOutFlag(uint16_t boxNumber){
    uint32_t clearFlag = 0;
    clearFlag = 1ul << boxNumber;
    _p_CanRegs->CANTOS.all = clearFlag;
}


void CAN::busErrorInterruptFlagsReset(){
    _CanShadow.CANGIF0.all = 0;
    _CanShadow.CANGIF0.bit.WLIF0 = 1;
    _CanShadow.CANGIF0.bit.EPIF0 = 1;
    _CanShadow.CANGIF0.bit.WDIF0 = 1;

    _p_CanRegs->CANGIF0.all = _CanShadow.CANGIF0.all;
}


}   // can_space