/**
 * @author Rémi Heredero
 * @version 1.0.0
 * @date August 2023
 * @file can_message.c
 */

#include "can_message.h"
#include "car.h"
#include "../app/factory/factory.h"
#include "../middleware/can_interface.h"
#include "kartculator.h"

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

typedef union {
    struct {
        uint8_t byte0;
        uint8_t byte1;
    } separate;
    uint16_t full;
} BYTES_2;

typedef union{
    uint16_t data;
    struct{
        uint8_t data[2];
    }byteSplit;  
    struct{
        unsigned b0:1;
        unsigned b1:1;
        unsigned b2:1;
        unsigned b3:1;
        unsigned b4:1;
        unsigned b5:1;
        unsigned b6:1;
        unsigned b7:1;
        unsigned b8:1;
        unsigned b9:1;
        unsigned b10:1;
        unsigned b11:1;
        unsigned b12:1;
        unsigned b13:1;
        unsigned b14:1;
        unsigned b15:1;
    }bitSplit;
} BITFIELD16;

void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){
    BYTES_4 incomeData;
    incomeData.full = data;
    BYTES_4 revertData;
    revertData.separate.byte0 = incomeData.separate.byte3;
    revertData.separate.byte1 = incomeData.separate.byte2;
    revertData.separate.byte2 = incomeData.separate.byte1;
    revertData.separate.byte3 = incomeData.separate.byte0;
    BITFIELD16 bField;
    bField.data = (uint16_t) data;
    
    switch(idSender){
        
        /*********************
         * BROADCAST / DEBUG *
         *********************/
        case 0:
            if(idMsg == 0x0) { // CONTROL_SETUP
                //  steeringMode    eraseMemory -   controlAliveTime
                if (incomeData.separate.byte1) {
                    MEM_reset();
                }
                KART_CST.CONTROL_STEERING_MODE = incomeData.separate.byte0;
                KART_CST.CONTROL_ALIVE_TIME = incomeData.separate.byte3;
                MEM_write_1_byte(MEMADD_CONTROL_STEERING_MODE, KART_CST.CONTROL_STEERING_MODE);
                MEM_write_1_byte(MEMADD_CONTROL_ALIVE_TIME, KART_CST.CONTROL_ALIVE_TIME);
                ALIVE_setAliveTime(ALcontroller(), KART_CST.CONTROL_ALIVE_TIME);
                ALIVE_emitStart(ALcontroller(), 0, 0);
            }
            
            if(idMsg == 0x1) { // CONTROL_SPEED_FACTOR
                //  valHH   valH    valL    valLL
                KART_CST.CONTROL_SPEED_FACTOR = revertData.full;
                MEM_write_4_byte(MEMADD_CONTROL_SPEED_FACTOR, KART_CST.CONTROL_SPEED_FACTOR);
                
            }
            
            if(idMsg == 0x2) { // CONTROL_POWER_FACTOR
                //  valHH   valH    valL    valLL
                KART_CST.CONTROL_POWER_FACTOR = revertData.full;
                MEM_write_4_byte(MEMADD_CONTROL_POWER_FACTOR, KART_CST.CONTROL_POWER_FACTOR);
                CAN_Send(0, 5, KART_CST.CONTROL_POWER_FACTOR); 
            }
            
            if(idMsg == 0x3) { // CONTROL_STEERING_FACTOR
                //  valHH   valH    valL    valLL
                KART_CST.CONTROL_STEERING_FACTOR = revertData.full;
                MEM_write_4_byte(MEMADD_CONTROL_STEERING_FACTOR, KART_CST.CONTROL_STEERING_FACTOR);
                
            }
            
            if(idMsg == 0x4) { // CONTROL_SECURITY_PARAM
                //  maxSpeedFw  maxSpeedBw  -   -
                KART_CST.CONTROL_MAX_SPEED_FW = incomeData.separate.byte0;
                KART_CST.CONTROL_MAX_SPEED_BW = incomeData.separate.byte1;
                MEM_write_1_byte(MEMADD_CONTROL_MAX_SPEED_FW, KART_CST.CONTROL_MAX_SPEED_FW);
                MEM_write_1_byte(MEMADD_CONTROL_MAX_SPEED_BW, KART_CST.CONTROL_MAX_SPEED_BW);
                
            }
            
            if(idMsg == 0x5) { // CONTROL_SETUP_PARAM
                //  displayAliveTime    steeringAliveTime   -   -
                KART_CST.DISPLAY_ALIVE_TIME = incomeData.separate.byte0;
                KART_CST.STEERING_ALIVE_TIME = incomeData.separate.byte1;
                MEM_write_1_byte(MEMADD_DISPLAY_ALIVE_TIME, KART_CST.DISPLAY_ALIVE_TIME);
                MEM_write_1_byte(MEMADD_STEERING_ALIVE_TIME, KART_CST.STEERING_ALIVE_TIME);
                
                // TODO set alive times
                // start alives
                CM_DISPLAY_SETUP(NULL);
                CM_STEERING_SETUP(&ALWAYS0);
                
            }
            
            if(idMsg == 0x6) { // CONTROL_SETUP_PARAM_JOY
                //  joystickMode    joystickParam1  joystickParam2  joystickAliveTime
                KART_CST.JOYSTICK_MODE = incomeData.separate.byte0;
                KART_CST.JOYSTICK_PARAM1 = incomeData.separate.byte1;
                KART_CST.JOYSTICK_PARAM2 = incomeData.separate.byte2;
                KART_CST.JOYSTICK_ALIVE_TIME = incomeData.separate.byte3;
                MEM_write_1_byte(MEMADD_JOYSTICK_MODE, KART_CST.JOYSTICK_MODE);
                MEM_write_1_byte(MEMADD_JOYSTICK_PARAM1, KART_CST.JOYSTICK_PARAM1);
                MEM_write_1_byte(MEMADD_JOYSTICK_PARAM2, KART_CST.JOYSTICK_PARAM2);
                MEM_write_1_byte(MEMADD_JOYSTICK_ALIVE_TIME, KART_CST.JOYSTICK_ALIVE_TIME);
                
                ALIVE_setAliveTime(ALjoy(), KART_CST.JOYSTICK_ALIVE_TIME);
                ALIVE_emitStart(ALjoy(), 0, 0);
                CM_JOY_SETUP(NULL);
                
            }
            
            if(idMsg == 0x7) { // CONTROL_SETUP_PARAM_DRIVE
                //  driveAliveTime  driveSpeedTime  driveStopTime   -
                KART_CST.DRIVE_ALIVE_TIME = incomeData.separate.byte0;
                KART_CST.DRIVE_SPEED_TIME = incomeData.separate.byte1;
                KART_CST.DRIVE_STOP_TIME = incomeData.separate.byte2;
                MEM_write_1_byte(MEMADD_DRIVE_ALIVE_TIME, KART_CST.DRIVE_ALIVE_TIME);
                MEM_write_1_byte(MEMADD_DRIVE_SPEED_TIME, KART_CST.DRIVE_SPEED_TIME);
                MEM_write_1_byte(MEMADD_DRIVE_STOP_TIME, KART_CST.DRIVE_STOP_TIME);
                
                ALIVE_setAliveTime(&drive()->myChecker, KART_CST.DRIVE_ALIVE_TIME);
                ALIVE_emitStart(&drive()->myChecker, 0, 0);
                CM_DRIVE_SETUP(&ALWAYSFALSE);
                
            }
            
            if(idMsg == 0x8) { // CONTROL_SETUP_PARAM_BATTERY
                //  batteryVoltTime batteryCurrentTime  batteryEnergyTime   batteryAliveTime
                KART_CST.BATTERY_VOLT_TIME = incomeData.separate.byte0;
                KART_CST.BATTERY_CURRENT_TIME = incomeData.separate.byte1;
                KART_CST.BATTERY_ENERGY_TIME = incomeData.separate.byte2;
                KART_CST.BATTERY_ALIVE_TIME = incomeData.separate.byte3;
                MEM_write_1_byte(MEMADD_BATTERY_VOLT_TIME, KART_CST.BATTERY_VOLT_TIME);
                MEM_write_1_byte(MEMADD_BATTERY_CURRENT_TIME, KART_CST.BATTERY_CURRENT_TIME);
                MEM_write_1_byte(MEMADD_BATTERY_ENERGY_TIME, KART_CST.BATTERY_ENERGY_TIME);
                MEM_write_1_byte(MEMADD_BATTERY_ALIVE_TIME, KART_CST.BATTERY_ALIVE_TIME);
                
                // TODO set alive time
                // TODO start alive
                CM_SUPPLY_SETUP(NULL);
                
            }
            
            if(idMsg == 0x9) { // CONTROL_PARAM_MAX_CHANGES
                //  maxChangeSteering   maxChangeDrive  -   -
                KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING = incomeData.separate.byte0;
                KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE = incomeData.separate.byte1;
                MEM_write_1_byte(MEMADD_CONTROL_PARAM_MAX_CHANGE_STEERING, KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING);
                MEM_write_1_byte(MEMADD_CONTROL_PARAM_MAX_CHANGE_DRIVE, KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE);
            }

            break;
            
            
        /************
         * JOYSTICK *
         ************/
        case 2:
            if(idMsg == 0x1) { // JOY_MESURE
                //  posX    posY    button  -
                calcTorque(incomeData.separate.byte1);
                calcPosition(incomeData.separate.byte0);
                STEERING_emitPollDir(steering());
                
            }
            
            if(idMsg == 0xF) { // JOY_ALIVE
                //  -   -   -   -
                ALIVE_ISALIVE(ALjoy());
                if(ALjoy()->state == STAL_DEAD){
                    KART_CST.JOYSTICK_MODE = MEM_read_1_byte(MEMADD_JOYSTICK_MODE);
                    KART_CST.JOYSTICK_PARAM1 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM1);
                    KART_CST.JOYSTICK_PARAM2 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM2);
                    KART_CST.JOYSTICK_ALIVE_TIME = MEM_read_1_byte(MEMADD_JOYSTICK_ALIVE_TIME);
                    ALIVE_setAliveTime(ALjoy(), KART_CST.JOYSTICK_ALIVE_TIME);
                    ALIVE_emitResurrect(ALjoy(), 0, 0);
                    ALIVE_emitBorn(ALjoy(), 0, 0);
                    ALIVE_emitReady(ALjoy(), 0, 0);
                }
            }
            break;
            
            
        /***********
         * DISPLAY *
         ***********/
        case 3:
            if(idMsg == 0xF) { // DISPLAY_ALIVE
                //  powerMode   -   -   -
                // TODO display say ALIVE
                
            }
            break;
            
            
        /*********
         * DRIVE *
         *********/
        case 4:
            if(idMsg == 0x0) { // DRIVE_SPEED
                //  speedHH speedH  speedL  speedLL
                calcSpeed(revertData.full);
                DRIVE_emitPollSpeed(drive());
            }
            
            if(idMsg == 0xF) { // DRIVE_ALIVE
                //  statusH statusL -   -
                ALIVE_ISALIVE(&drive()->myChecker);
                if(drive()->myChecker.state == STAL_DEAD){
                    KART_CST.DRIVE_SPEED_TIME = MEM_read_1_byte(MEMADD_DRIVE_SPEED_TIME);
                    KART_CST.DRIVE_STOP_TIME = MEM_read_1_byte(MEMADD_DRIVE_STOP_TIME);
                    KART_CST.DRIVE_ALIVE_TIME = MEM_read_1_byte(MEMADD_DRIVE_ALIVE_TIME);
                    ALIVE_emitResurrect(&drive()->myChecker, 0, 0);
                    ALIVE_emitBorn(&drive()->myChecker, 0, 0);
                }
                
            }
            break;
            
            
        /************
         * STEERING *
         ************/
        case 5:
            if(idMsg == 0x1) { // STEERING_GET_CENTER
                //  valHH   valH    valL    valLL
                eKart.center = revertData.full;
                ALIVE_emitReady(&steering()->myChecker, 0, 0);
            }
            
            if(idMsg == 0x2) { // STEERING_GET_POSITION
                //  valHH   valH    valL    valLL
                
            }
            
            if(idMsg == 0xE) { // DRIVE_BRAKE
                //  status  -   -   -
                eKart.brake = incomeData.separate.byte0;
            }
            
            if(idMsg == 0xF) { // STEERING_ALIVE
                //  statusH statusL -   -
                ALIVE_ISALIVE(&steering()->myChecker);
                if(steering()->myChecker.state == STAL_DEAD) {
                    KART_CST.STEERING_ALIVE_TIME = MEM_read_1_byte(MEMADD_STEERING_ALIVE_TIME);
                    ALIVE_emitResurrect(&steering()->myChecker, 500, 0);
                    ALIVE_emitBorn(&steering()->myChecker, 1000, 0);
                    //ALIVE_emitReady(&steering()->myChecker, 5000, 0);
                }
                if(bField.bitSplit.b4) {
                    if (steering()->myChecker.state == STAL_BORN){
                        ALIVE_emitReady(&steering()->myChecker, 1000, 0);
                    }
                }
                
                
            }
            break;
            
            
        /**********
         * SUPPLY *
         **********/
        case 6:
            if(idMsg == 0x4) {
                DRIVE_startBehaviour(drive());
                STEERING_startBehaviour(steering());

                ALIVE_startBehaviourChecker(ALjoy());
                ALIVE_emitBorn(ALjoy(), 100, 0);
                ALIVE_emitReady(ALjoy(), 200, 0);
            }
            
            if(idMsg == 0xF) { // BATTERY_ALIVE
                //  -   -	-	-
                // TODO battery say ALIVE
                
            }
            break;
            
            
        /*************
         * UNDEFINED *
         *************/
        case 7:
            break;
    }
}

void CM_CONTROLLER_ALIVE(void* p) {
    //  -   -   -   -
    CAN_Send(0x0, 0xF, 0);
}

void CM_JOY_SETUP(void* p) {
    //  mode    parm1   param2  aliveTime
    BYTES_4 joy;
    joy.separate.byte0 = KART_CST.JOYSTICK_MODE;
    joy.separate.byte1 = KART_CST.JOYSTICK_PARAM1;
    joy.separate.byte2 = KART_CST.JOYSTICK_PARAM2;
    joy.separate.byte3 = KART_CST.JOYSTICK_ALIVE_TIME;
    CAN_Send(2, 0, joy.full);    
}
void CM_DISPLAY_SETUP(void* p) {
    //  reset   -   -   aliveTime
    BYTES_4 display;
    display.separate.byte0 = 0; // reset
    display.separate.byte3 = KART_CST.DISPLAY_ALIVE_TIME;
    CAN_Send(3, 0, display.full);    
}

void CM_DISPLAY_SPEED(void* p) {
    //  valH    valL    -   -
    BYTES_4 tmpData;
    // byte0 should be valH but isn't possible to go enough fast for use it
    tmpData.separate.byte0 = 0; 
    tmpData.separate.byte1 = *((uint8_t*) p);
    tmpData.separate.byte2 = 0;
    tmpData.separate.byte3 = 0;
    CAN_Send(3, 2, tmpData.full);    
}

void CM_DISPLAY_DIRECTION(void* p) {
    //  direction   -   -   -
    BYTES_4 tmpData;
    tmpData.separate.byte0 = (uint8_t) p;
    tmpData.separate.byte1 = 0;
    tmpData.separate.byte2 = 0;
    tmpData.separate.byte3 = 0;
    CAN_Send(3, 3, tmpData.full);
}

void CM_DRIVE_SETUP(void* p) {
    //  reset/init  speedTime   stopTime    aliveTime
    BYTES_4 tmpData;
    tmpData.separate.byte0 = (uint8_t) p;
    tmpData.separate.byte1 = KART_CST.DRIVE_SPEED_TIME;
    tmpData.separate.byte2 = KART_CST.DRIVE_STOP_TIME;
    tmpData.separate.byte3 = KART_CST.DRIVE_ALIVE_TIME;
    CAN_Send(4, 0, tmpData.full);
    
}

void CM_DRIVE_POWER(void* p) {
    //  valH    valL    -   -
    BYTES_2 torque;
    BYTES_4 tmpData;
    torque.full = *((int16_t*) p);
    tmpData.separate.byte0 = torque.separate.byte1;
    tmpData.separate.byte1 = torque.separate.byte0;
    tmpData.separate.byte2 = 0;
    tmpData.separate.byte3 = 0;
    CAN_Send(4, 1, tmpData.full);
}

void CM_STEERING_SETUP(void* p) {
    //  reset/init  homing  setCenter   aliveTime
    // TODO not working have to fix it
    BYTES_4 tmpData;
    uint8_t choice = *(uint8_t*) p;
    switch (choice) {
        case 1:
            tmpData.separate.byte0 = 1;
            tmpData.separate.byte1 = 0;
            tmpData.separate.byte2 = 0;
            break;
        case 2:
            tmpData.separate.byte0 = 0;
            tmpData.separate.byte1 = 1;
            tmpData.separate.byte2 = 0;
            break;
        case 3:
            tmpData.separate.byte0 = 0;
            tmpData.separate.byte1 = 0;
            tmpData.separate.byte2 = 1;
            break;
        default:
            tmpData.separate.byte0 = 0;
            tmpData.separate.byte1 = 0;
            tmpData.separate.byte2 = 0;
            break;
    }
    tmpData.separate.byte3 = KART_CST.STEERING_ALIVE_TIME; // 0 -> no alive
    CAN_Send(5, 0, tmpData.full);
}

void CM_STEERING_SET(void* p) {
    //  valHH   valH    valL    valLL
    BYTES_4 pPosition;
    pPosition.full = *((uint32_t*) p);
    BYTES_4 sPosition;
    sPosition.separate.byte0 = pPosition.separate.byte3;
    sPosition.separate.byte1 = pPosition.separate.byte2;
    sPosition.separate.byte2 = pPosition.separate.byte1;
    sPosition.separate.byte3 = pPosition.separate.byte0;
    CAN_Send(5, 1, sPosition.full);
}

void CM_SUPPLY_SETUP(void* p) {
    //  batteryVoltTime batteryCurrentTime  batteryEnergyTime   aliveTime
    BYTES_4 supply;
    supply.separate.byte0 = KART_CST.BATTERY_VOLT_TIME;
    supply.separate.byte1 = KART_CST.BATTERY_CURRENT_TIME;
    supply.separate.byte2 = KART_CST.BATTERY_ENERGY_TIME;
    supply.separate.byte3 = KART_CST.BATTERY_ALIVE_TIME;
    CAN_Send(6, 0, supply.full);
}