#include "CAN.h"

namespace canSpace {

void CAN::transmitMsg(Uint16 boxNumber, const CANMessage& message){
    if (boxNumber > 31) return;

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


void CAN::transmitMsg(Uint16 boxNumber, const Uint32& message){
    if (boxNumber > 31) return;

    Uint32 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 >> 16;
    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::transmitMsg(Uint16 boxNumber, const Uint32& message, const Uint16 dlc){
    if (boxNumber > 31) return;

    Uint32 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 >> 16;
    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 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);

    // TODO Add lenght changing?
    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){
    if (boxNumber > 31) return;

    volatile MBOX* p_MailBox(0);
    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;
}


int16 CAN::receiveMsg(Uint16 boxNumber, CANMessage& rxMessage){ // TODO faults just return -1
    if (boxNumber > 31) { return -1; }

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


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


bool CAN::isNewMessage(Uint16 boxNumber){
    if (boxNumber > 31) { return 0; }

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

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


}   // canSpace