/**
 * @author R�mi Heredero
 * @version 1.0.0
 * @date August 2023
 * @file can_interface.c
 */

#include "can_interface.h"
#include "../mcc_generated_files/ecan.h"


typedef union {
    struct {
        uint8_t sender;
        uint8_t recipient;
        uint8_t message;
        uint32_t data;
    } full;
    struct {
        uint8_t sender;
        uint8_t recipient;
        uint8_t message;
        uint8_t data0;
        uint8_t data1;
        uint8_t data2;
        uint8_t data3;
    } separate;
} CAN_MESSAGE;

typedef union {
    struct {
        uint8_t byte0;
        uint8_t byte1;
        uint8_t byte2;
        uint8_t byte3;
    } separate;
    uint32_t full;
} CAN_4_BYTES;

void CAN_init(){
    CAN_myself.receiveCan = NULL;
    CAN_myself.sender = 0;
}

void CAN_startBehaviour(){
    POST(&CAN_myself, &CAN_processEvent, evCAinit, 0, 0);
}

bool CAN_processEvent(Event* ev) {
    bool processed = false;
    CAN* me = (CAN*)Event_getTarget(ev);
    CAN_STATES oldState = me->state;
    evIDT evid = Event_getId(ev);
    
    uint64_t data = Event_getData(ev);
    CAN_4_BYTES tmpData;
    uCAN_MSG canMsg;
    uint32_t canData = (uint32_t) data;
    uint8_t idMsg;
    uint8_t idRecipient;
    uint8_t idSender;
    
        
    switch (me->state) {        // onState
        case STCA_INIT:
            if (ev->id == evCAinit) {
                me->state = STCA_PROCESS;
            }
            break;
        
        case STCA_PROCESS:
            
            // New message arrive
            if (ev->id == evCAnewMsg) {
                if (me->receiveCan != NULL) {
                    data = data>>32;

                    tmpData.full = data;
                    idMsg = tmpData.separate.byte0;
                    idMsg = idMsg >> 4;
                    idMsg = idMsg & 0xF;
                    idRecipient = tmpData.separate.byte1;
                    idRecipient = idRecipient & 0xF;
                    idSender = tmpData.separate.byte1;
                    idSender = idSender >> 4;

                    me->receiveCan(idSender, idMsg, false, canData);
                }
            }
            
            if (ev->id == evCAnewRTR) {
                if (me->receiveCan != NULL) {
                    data = data>>32;

                    tmpData.full = data;
                    idMsg = tmpData.separate.byte0;
                    idMsg = idMsg >> 4;
                    idMsg = idMsg & 0xF;
                    idRecipient = tmpData.separate.byte1;
                    idRecipient = idRecipient & 0xF;
                    idSender = tmpData.separate.byte1;
                    idSender = idSender >> 4;

                    me->receiveCan(idSender, idMsg, true, canData);
                }
            }
            
            // Send a message
            if (ev->id == evCAsend4) {
                canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B;    // standard
                canMsg.frame.dlc = 4;                               // 4 bytes to send
                canMsg.frame.rtr = 0;                               // no remote frame
                canMsg.frame.data0 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data1 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data2 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data3 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.id = (uint32_t) data;
                CAN_transmit(&canMsg);
            }
            
            if (ev->id == evCAsend2) {
                canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B;    // standard
                canMsg.frame.dlc = 2;                               // 4 bytes to send
                canMsg.frame.rtr = 0;                               // no remote frame
                canMsg.frame.data0 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data1 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data2 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data3 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.id = (uint32_t) data;
                CAN_transmit(&canMsg);
            }
            
            if (ev->id == evCAsend1) {
                canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B;    // standard
                canMsg.frame.dlc = 1;                               // 4 bytes to send
                canMsg.frame.rtr = 0;                               // no remote frame
                canMsg.frame.data0 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data1 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data2 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data3 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.id = (uint32_t) data;
                CAN_transmit(&canMsg);
            }
            
            if (ev->id == evCAsend0) {
                canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B;    // standard
                canMsg.frame.dlc = 0;                               // 4 bytes to send
                canMsg.frame.rtr = 0;                               // no remote frame
                canMsg.frame.data0 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data1 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data2 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data3 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.id = (uint32_t) data;
                CAN_transmit(&canMsg);
            }
            
            if (ev->id == evCAsendRTR) {
                canMsg.frame.idType = dSTANDARD_CAN_MSG_ID_2_0B;    // standard
                canMsg.frame.dlc = 0;                               // 4 bytes to send
                canMsg.frame.rtr = 1;                               // no remote frame
                canMsg.frame.data0 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data1 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data2 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.data3 = (uint8_t) data;
                data = data >> 8;
                canMsg.frame.id = (uint32_t) data;
                CAN_transmit(&canMsg);
            }
            break;
    }

    if(oldState != me->state){
        switch (oldState) {     // onExit
            case STCA_INIT:
                break;
            
            case STCA_PROCESS:
                break;
        }

        switch (me->state) {    // onEntry
            case STCA_INIT:
                break;
            
            case STCA_PROCESS:
                break;
        }

        processed = true;
    }
    return processed;
}

/*************
 * Callbacks *
 *************/

void CAN_onReceiveCan(CAN_CALLBACK f) {
    CAN_myself.receiveCan = f;
}

/************
 * EMITTERS *
 ************/

void CAN_newMsg() {
    uint64_t data;
    uCAN_MSG canMsg;
    CAN_receive(&canMsg);
    data = canMsg.frame.id;
    data = data<<12;
    data = data | canMsg.frame.data3;
    data = data<<8;
    data = data | canMsg.frame.data2;
    data = data<<8;
    data = data | canMsg.frame.data1;
    data = data<<8;
    data = data | canMsg.frame.data0;
    if(canMsg.frame.rtr) {
        POST(&CAN_myself, &CAN_processEvent, evCAnewRTR, 0, data);
    } else {
        POST(&CAN_myself, &CAN_processEvent, evCAnewMsg, 0, data);
    }
}

void CAN_Send(uint8_t idRecipient, uint8_t idMsg, uint32_t data) {
    uint64_t tmpData = CAN_myself.sender;
    tmpData = (tmpData<<4) | idRecipient;
    tmpData = (tmpData<<4) | idMsg;
    tmpData = (tmpData<<32) | data;
    POST(&CAN_myself, &CAN_processEvent, evCAsend4, 0, tmpData);
}

void CAN_send_4_bytes(uint8_t idRecipient, uint8_t idMsg, uint8_t byte0, uint8_t byte1, uint8_t byte2, uint8_t byte3) {
    uint64_t tmpData = CAN_myself.sender;
    tmpData = (tmpData<<4) | idRecipient;
    tmpData = (tmpData<<4) | idMsg;
    tmpData = (tmpData<<8) | byte3;
    tmpData = (tmpData<<8) | byte2;
    tmpData = (tmpData<<8) | byte1;
    tmpData = (tmpData<<8) | byte0;
    POST(&CAN_myself, &CAN_processEvent, evCAsend4, 0, tmpData);
}

void CAN_send_2_bytes(uint8_t idRecipient, uint8_t idMsg, uint16_t data) {
    uint64_t tmpData = CAN_myself.sender;
    tmpData = (tmpData<<4) | idRecipient;
    tmpData = (tmpData<<4) | idMsg;
    tmpData = (tmpData<<32) | data;
    POST(&CAN_myself, &CAN_processEvent, evCAsend2, 0, tmpData);
}

void CAN_send_1_byte(uint8_t idRecipient, uint8_t idMsg, uint8_t data) {
    uint64_t tmpData = CAN_myself.sender;
    tmpData = (tmpData<<4) | idRecipient;
    tmpData = (tmpData<<4) | idMsg;
    tmpData = (tmpData<<32) | data;
    POST(&CAN_myself, &CAN_processEvent, evCAsend1, 0, tmpData);
}

void CAN_send_0_byte(uint8_t idRecipient, uint8_t idMsg) {
    uint64_t tmpData = CAN_myself.sender;
    tmpData = (tmpData<<4) | idRecipient;
    tmpData = (tmpData<<4) | idMsg;
    tmpData = tmpData<<32;
    POST(&CAN_myself, &CAN_processEvent, evCAsend0, 0, tmpData);
}

void CAN_send_rtr(uint8_t idRecipient, uint8_t idMsg) {
    uint64_t tmpData = CAN_myself.sender;
    tmpData = (tmpData<<4) | idRecipient;
    tmpData = (tmpData<<4) | idMsg;
    tmpData = tmpData<<32;
    POST(&CAN_myself, &CAN_processEvent, evCAsendRTR, 0, tmpData);
}

/***********
 * SETTERS *
 ***********/

void CAN_setSender(uint8_t idSender) {
    CAN_myself.sender = idSender;
}