/**
 * @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"

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

void CM_processIncome(uint8_t idSender, uint8_t idMsg, uint32_t data){
    switch(idSender){
        
        /*********************
         * BROADCAST / DEBUG *
         *********************/
        case 0:
            if(idMsg == 0x0) { // CONTROL_SETUP
                //  steeringMode    -   -   controlAliveTime
                BYTES_4 tmpData;
                tmpData.full.bytes = data;
                CAR_CST.CONTROL_STEERING_MODE = tmpData.separate.byte0;
                CAR_CST.CONTROL_ALIVE_TIME = tmpData.separate.byte3;
                MEM_write_1_byte(MEMADD_CONTROL_STEERING_MODE, CAR_CST.CONTROL_STEERING_MODE);
                MEM_write_1_byte(MEMADD_CONTROL_ALIVE_TIME, CAR_CST.CONTROL_ALIVE_TIME);
                ALIVE_setAliveTime(ALcontroller(), CAR_CST.CONTROL_ALIVE_TIME);
            }
            
            if(idMsg == 0x1) { // CONTROL_SPEED_FACTOR
                //  valHH   valH    valL    valLL
                
            }
            
            if(idMsg == 0x2) { // CONTROL_POWER_FACTOR
                //  valHH   valH    valL    valLL
                
            }
            
            if(idMsg == 0x3) { // CONTROL_STEERING_FACTOR
                //  valHH   valH    valL    valLL
                
            }
            
            if(idMsg == 0x4) { // CONTROL_SECURITY_PARAM
                //  maxSpeedFw  maxSpeedBw  -   -
                
            }
            
            if(idMsg == 0x5) { // CONTROL_SETUP_PARAM
                //  displayAliveTime    steeringAliveTime   -   -
                
            }
            
            if(idMsg == 0x6) { // CONTROL_SETUP_PARAM_JOY
                //  joystickMode    joystickParam1  joystickParam2  joystickAliveTime
                BYTES_4 tmpData;
                tmpData.full.bytes = data;
                CAR_CST.JOYSTICK_MODE = tmpData.separate.byte0;
                CAR_CST.JOYSTICK_PARAM1 = tmpData.separate.byte1;
                CAR_CST.JOYSTICK_PARAM2 = tmpData.separate.byte2;
                CAR_CST.JOYSTICK_ALIVE_TIME = tmpData.separate.byte3;
                MEM_write_1_byte(MEMADD_JOYSTICK_MODE, CAR_CST.JOYSTICK_MODE);
                MEM_write_1_byte(MEMADD_JOYSTICK_PARAM1, CAR_CST.JOYSTICK_PARAM1);
                MEM_write_1_byte(MEMADD_JOYSTICK_PARAM2, CAR_CST.JOYSTICK_PARAM2);
                MEM_write_1_byte(MEMADD_JOYSTICK_ALIVE_TIME, CAR_CST.JOYSTICK_ALIVE_TIME);
                
                ALIVE_setAliveTime(ALjoy(), CAR_CST.JOYSTICK_ALIVE_TIME);
                CM_JOY_SETUP(NULL);
                
            }
            
            if(idMsg == 0x7) { // CONTROL_SETUP_PARAM_DRIVE
                //  driveAliveTime  driveSpeedTime  driveStopTime   -
                
            }
            
            if(idMsg == 0x8) { // CONTROL_SETUP_PARAM_BATTERY
                //  batteryVoltTime batteryCurrentTime  batteryEnergyTime   batteryAliveTime
                
            }

            break;
            
            
        /************
         * JOYSTICK *
         ************/
        case 2:
            if(idMsg == 0x1) { // JOY_MESURE
                //  posX    posY    button  -
                
            }
            
            if(idMsg == 0xF) { // JOY_ALIVE
                //  -   -   -   -
                ALIVE_ISALIVE(ALjoy());
                if(ALjoy()->state == STAL_DEAD){
                    CAR_CST.JOYSTICK_MODE = MEM_read_1_byte(MEMADD_JOYSTICK_MODE);
                    CAR_CST.JOYSTICK_PARAM1 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM1);
                    CAR_CST.JOYSTICK_PARAM2 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM2);
                    CAR_CST.JOYSTICK_ALIVE_TIME = MEM_read_1_byte(MEMADD_JOYSTICK_ALIVE_TIME);
                    ALIVE_setAliveTime(ALjoy(), CAR_CST.JOYSTICK_ALIVE_TIME);
                    ALIVE_emitBorn(ALjoy(), 0, 0);
                    ALIVE_emitReady(ALjoy(), 100, 0);
                }
            }
            break;
            
            
        /***********
         * DISPLAY *
         ***********/
        case 3:
            if(idMsg == 0xF) { // DISPLAY_ALIVE
                //  powerMode   -   -   -
                
            }
            break;
            
            
        /*********
         * DRIVE *
         *********/
        case 4:
            if(idMsg == 0x0) { // DRIVE_SPEED
                //  speedHH speedH  speedL  speedLL
                
            }
            
            if(idMsg == 0xF) { // DRIVE_ALIVE
                //  statusH statusL -   -
                
            }
            break;
            
            
        /************
         * STEERING *
         ************/
        case 5:
            if(idMsg == 0x1) { // STEERING_GET_CENTER
                //  valHH   valH    valL    valLL
                
            }
            
            if(idMsg == 0x2) { // STEERING_GET_POSITION
                //  valHH   valH    valL    valLL
                
            }
            
            if(idMsg == 0xF) { // STEERING_ALIVE
                //  statusH statusL -   -
                
            }
            break;
            
            
        /**********
         * SUPPLY *
         **********/
        case 6:
            if(idMsg == 0xF) { // BATTERY_ALIVE
                //  -   -	-	-
                
            }
            break;
            
            
        /*************
         * UNDEFINED *
         *************/
        case 7:
            break;
    }
}

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

void CM_JOY_SETUP(void* p) {
    BYTES_4 joy;
    joy.separate.byte0 = CAR_CST.JOYSTICK_MODE;
    joy.separate.byte1 = CAR_CST.JOYSTICK_PARAM1;
    joy.separate.byte2 = CAR_CST.JOYSTICK_PARAM2;
    joy.separate.byte3 = CAR_CST.JOYSTICK_ALIVE_TIME;
    CAN_Send(2, 0, joy.full.bytes);
    
}
void CM_DISPLAY_SETUP(void* p) {
    
}

void CM_DISPLAY_SPEED(void* p) {
    
}

void CM_DISPLAY_DIRECTION(void* p) {
    
}

void CM_DRIVE_SETUP(void* p) {
    
}

void CM_DRIVE_POWER(void* p) {
    
}

void CM_STEERING_SETUP(void* p) {
    
}

void CM_STEERING_SET(void* p) {
    
}

void CM_SETUP_CONTROL(void* p) {
    
}