#include "CAN.h"
#include "DSP2833x_Device.h"
#include <cstddef>

namespace canSpace {

void CAN::transmitMsg(Uint16 boxNumber, const CANMessage& message){   
    Uint32 mboxControl(0);
    mboxControl = 1ul << boxNumber;

    volatile MBOX* p_MailBox(NULL);
    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;

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

    p_MailBox->MSGCTRL.bit.DLC = message.msgctrl.bit.DLC;
    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;

    do { CanShadow_.CANTA.all = p_CanRegs_->CANTA.all; }    // TODO add tx error somewhere here
    while((CanShadow_.CANTA.all & mboxControl) == 0 );// Wait for TA1 bit to be set

    CanShadow_.CANTA.all = 0;
    CanShadow_.CANTA.all |= mboxControl;    // Clear TA1
    p_CanRegs_->CANTA.all = CanShadow_.CANTA.all;
}

void CAN::updateTXMessage(Uint16 boxNumber, const CANMessage& message){ // TODO check trs bit and set it at the end if it was here
    volatile MBOX* p_MailBox(NULL);
    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;

    p_CanRegs_->CANMC.all |= (128 + boxNumber);

    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;
}


void CAN::sendRemoteRequest(Uint16 boxNumber){
    volatile MBOX* p_MailBox(NULL);
    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;

    Uint32 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;
}


bool CAN::receiveMsg(Uint16 boxNumber, CANMessage& rxMessage){ // TODO faults just return -1
    Uint32 mboxControl(0);
    mboxControl = 1ul << boxNumber;

    volatile MBOX* p_MailBox(NULL);
    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;

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

    p_CanRegs_->CANRMP.all &= mboxControl;

    rxMessage.msgctrl.all = 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 -1;
    //  set_fault();
    }

    return 0;
}


bool CAN::isNewMessage(){
    return static_cast<bool>(p_CanRegs_->CANRMP.all);
}


bool CAN::isNewMessage(Uint16 boxNumber){
    Uint32 mboxControl(0);
    mboxControl = 1ul << boxNumber;

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


/* CANMessage CAN::receiveMsg(Uint16 boxNumber){
    Uint32 mboxControl(0);
    mboxControl = 1ul << boxNumber;

    volatile MBOX* p_MailBox(NULL);
    p_MailBox = &(p_CanMBoxes_->MBOX0) + boxNumber;
    CANMessage rxMessage;

    p_CanRegs_->CANRMP.all &= mboxControl;

    rxMessage.MSGCTRL.all = 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) {
        counter_fault_rx++;     // TODO delete after debug
    //  set_fault();
    }

    return rxMessage;
} */


}   // canSpace