#include "Vehicle.h"
#include "fdcan.h"
#include "CLS.h"
#include "CLS_BSP.h"
#include "BSP_GPIO.h"
#include "cmsis_os2.h"

static uint64_t last_unlock_message_time = UINT64_MAX;
static uint8_t car_can_brightness = 255;
static float car_can_speed = 0;
static int car_can_direction = 0;


void Vehicle_Setup_CAN() {


    FDCAN_FilterTypeDef sFilterConfig;
    sFilterConfig.IdType = FDCAN_STANDARD_ID;
    sFilterConfig.FilterIndex = 0;
    sFilterConfig.FilterType = CLS_BSP_CAN_FILTER_LIST;
    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1;
    sFilterConfig.FilterID1 = 0x391;
    sFilterConfig.FilterID2 = 0x395;
    HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig);

    sFilterConfig.FilterIndex = 1;
    sFilterConfig.FilterID1 = 0x351;
    sFilterConfig.FilterID2 = 0x635;
    HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig);

}


static void rx_unlock(uint8_t * RxData ) {
    if (RxData[1]  == 0x04)
        {
            // car was unlocked
            last_unlock_message_time = osKernelGetTickCount();
        } 
        else if (RxData[1]  ==0x80)
        {
            // car was locked
            if (!BSP_GPIO_K15isSet()) {
                NVIC_SystemReset();
            }
        }

}


static void rx_speed(uint8_t *  RxData) {
    // speed signal 
    // AA BB XX YY 00 00 00 00 
    // Speed (XX*(2^8)+(YY-1))/190
    // direction AA = 0x00 forward, 0x02 backward
    uint16_t speed = (RxData[2] << 8) + RxData[3];
    float speed_kmh = (speed - 1) / 190.0;
    car_can_speed = speed_kmh;
    car_can_direction = RxData[0];
    //ULOG_DEBUG("Speed: %f, Direction: %d", car_can_speed, car_can_direction);

}

static void rx_brightness(uint8_t* RxData) {
    // scale the brightness to 0 - 255 only using integer math
    car_can_brightness = ((uint32_t)RxData[0] * 255) / 100;
    //ULOG_DEBUG("Brightness: %d", car_can_brightness);
}

void Vehicle_Receive_CAN( FDCAN_RxHeaderTypeDef RxHeader, uint8_t* RxData) {

                if(RxHeader.Identifier == 0x391) {
                    rx_unlock(RxData);
                }

                if (RxHeader.Identifier == 0x395) {
                    rx_unlock(RxData);
                }


                if (RxHeader.Identifier == 0x351) {
                    rx_speed(RxData);
                }

                // brightness knob in 0 - 100
                if (RxHeader.Identifier == 0x635) {
                    rx_brightness(RxData);
                }

}



bool Vehicle_gotUnlockMessage() {
    return last_unlock_message_time != UINT64_MAX;
}



uint8_t Vehicle_Brightness() {
    return car_can_brightness;
}

float Vehicle_Speed() {
    return car_can_speed;
}

int Vehicle_DirectionIsForward() {
    return car_can_direction == 0;
}


static bool unlocked = false;

 
bool Vehicle_UnlockedSignal() {

    if (unlocked) {
        if(Vehicle_isK15On()) {
            unlocked = false;
            last_unlock_message_time = UINT64_MAX;
        }
    }

    if (!unlocked) {
        if (Vehicle_gotUnlockMessage()) {
            unlocked = true;
        }
    }

    return unlocked;
}