//
// Created by enik on 03.04.23.
//

#include "can_messenger.h"
#include "can.h"
//#include "modCommands.h"
#include "modPowerElectronics.h"

#include <string.h>

#include "../RingBuffer/RingBuffer.h"

extern uint64_t TIM_Clock;

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

uint64_t can1_transmit_clock = 0;
uint64_t can2_transmit_clock = 0;

typedef struct CAN_MSG_t {
    u16_t id;
    u8_t len;
    u8_t msg[8];
} CAN_MSG_t;

static volatile CAN_MSG_t can1_transmit_q_msg[45];  // 540 bytes
static volatile CAN_MSG_t can2_transmit_q_msg[10];  // 120 bytes
static volatile RINGBUF_t can1_ring;
static volatile RINGBUF_t can2_ring;

static volatile CAN_MSG_t can1_buf = {0,};      // Buf for 1 packet
static volatile CAN_MSG_t can2_buf = {0,};      // Buf for 1 packet

static volatile CAN_MSG_t can1_tx_buf = {0,};      // Buf for 1 TX packet
static volatile CAN_MSG_t can2_tx_buf = {0,};      // Buf for 1 TX packet

extern modPowerElectronicsPackStateTypedef packState;
extern modConfigGeneralConfigStructTypedef *generalConfig;

static volatile bool dummy_soc = 0;

static volatile bool threshold_request = 0;
static volatile bool active_request = 0;

void can_msgr_init() {
    RingBuf_Init((void *) &can1_transmit_q_msg, 45, sizeof(CAN_MSG_t), (RINGBUF_t *) &can1_ring);
    RingBuf_Init((void *) &can2_transmit_q_msg, 10, sizeof(CAN_MSG_t), (RINGBUF_t *) &can2_ring);
}

/**
 * @brief Make SoC for charging = 100%
 */
void can_set_dummy_soc(void){
    dummy_soc = 1;
}


bool get_dummy_soc(void){
    return dummy_soc;
}

void transmitCan1Packet() {
    if ((TIM_Clock - can1_transmit_clock) < 500)
    {
        return;
    }

    can1_transmit_clock = TIM_Clock;

    // SOC and SOH (TODO)
    can1_buf.id = 0x101;
    can1_buf.len = 8;
    memcpy(can1_buf.msg, &packState.SoC, sizeof(packState.SoC));
    memcpy(can1_buf.msg + 4, &packState.SoCCapacityAh, sizeof(packState.SoCCapacityAh));
    RingBuf_CellPut(&can1_buf, &can1_ring);

    // pack voltage and current
    can1_buf.id = 0x102;
    can1_buf.len = 8;
    memcpy(can1_buf.msg, &packState.packVoltage, sizeof(packState.packVoltage));
    memcpy(can1_buf.msg + 4, &packState.packCurrent, sizeof(packState.packCurrent));
    RingBuf_CellPut(&can1_buf, &can1_ring);

    // inputs and outputs state
    can1_buf.id = 0x103;
    can1_buf.len = 8;

    const uint8_t input0 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_7);
    const uint8_t input1 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_8);
    const uint8_t input2 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_9);

    const uint8_t output0 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_0);
    const uint8_t output1 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_1);
    const uint8_t output2 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_2);
    const uint8_t output3 = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_3);

    memcpy(can1_buf.msg + 0, &input0, sizeof(input0));
    memcpy(can1_buf.msg + 1, &input1, sizeof(input1));
    memcpy(can1_buf.msg + 2, &input2, sizeof(input2));

    memcpy(can1_buf.msg + 4, &output0, sizeof(output0));
    memcpy(can1_buf.msg + 5, &output1, sizeof(output1));
    memcpy(can1_buf.msg + 6, &output2, sizeof(output2));
    memcpy(can1_buf.msg + 7, &output3, sizeof(output3));
    
    RingBuf_CellPut(&can1_buf, &can1_ring);

    // number of cells and temperature sensors
    can1_buf.id = 0x104;
    can1_buf.len = 8;
    const uint32_t numberOfCells = generalConfig->noOfCellsSeries * generalConfig->noOfParallelModules;
    const uint32_t numberOfTemperatures = generalConfig->cellMonitorICCount * generalConfig->noOfTempSensorPerModule;
    memcpy(can1_buf.msg, &numberOfCells, sizeof(numberOfCells));
    memcpy(can1_buf.msg + 4, &numberOfTemperatures, sizeof(numberOfTemperatures));
    RingBuf_CellPut(&can1_buf, &can1_ring);

    // cell voltages
    can1_buf.id = 0x201;
    can1_buf.len = 8;

    for (int i = 0; i < numberOfCells; i += 2)
    {
        for (int j = 0; j < 2; ++j)
        {
            const int index = i + j;
            float voltage = 0;
            if (packState.cellVoltagesIndividual[index].cellBleedActive)
                voltage = packState.cellVoltagesIndividual[index].cellVoltage * -1000;
            else
                voltage = packState.cellVoltagesIndividual[index].cellVoltage * 1000;
            
            memcpy(can1_buf.msg + j * 4, &voltage, sizeof(voltage));
        }

        if (i + 2 > numberOfCells)
            can1_buf.len = 4;

        RingBuf_CellPut(&can1_buf, &can1_ring);
        can1_buf.id++;
    }

    // temperatures
    can1_buf.id = 0x301;
    can1_buf.len = 8;

    for (int i = 0; i < numberOfTemperatures; i += 2)
    {
        for (int j = 0; j < 2; ++j)
        {
            float temperature = packState.temperatures[i + j];
            memcpy(can1_buf.msg + j * 4, &temperature, sizeof(temperature));
        }

        if (i + 2 > numberOfTemperatures)
            can1_buf.len = 4;

        RingBuf_CellPut(&can1_buf, &can1_ring);
        can1_buf.id++;
    }
}

int getIndexBySoc() {
    const float soc = packState.SoC;
    if (soc <= 0) {
        return 0;
    } else if (soc <= 10 && soc > 0) {
        return 1;
    } else if (soc <= 20 && soc > 10) {
        return 2;
    } else if (soc <= 30 && soc > 20) {
        return 3;
    } else if (soc <= 40 && soc > 30) {
        return 4;
    } else if (soc <= 50 && soc > 40) {
        return 5;
    } else if (soc <= 60 && soc > 50) {
        return 6;
    } else if (soc <= 70 && soc > 60) {
        return 7;
    } else if (soc <= 80 && soc > 70) {
        return 8;
    } else if (soc <= 95 && soc > 80) {
        return 9;
    } else if (soc > 95) {
        return 10;
    }

    return -1;
}

int getIndexByTemperature() {
    const float temp = packState.tempBatteryAverage;
    if (temp < 0) {
        return 0;
    } else if (temp < 2 && temp >= 0) {
        return 1;
    } else if (temp < 7 && temp >= 2) {
        return 2;
    } else if (temp < 15 && temp >= 7) {
        return 3;
    } else if (temp < 20 && temp >= 15) {
        return 4;
    } else if (temp < 45 && temp >= 20) {
        return 5;
    } else if (temp < 50 && temp >= 45) {
        return 6;
    } else if (temp < 55 && temp >= 50) {
        return 7;
    } else if (temp >= 55) {
        return 8;
    }

    return -1;
}

u16_t get_table_current() {
    // sending table current and end-of-charge current command
    const int socIndex = dummy_soc ? 10 : getIndexBySoc();
    const int temperatureIndex = getIndexByTemperature();
    float tableCurrent = 0;
    if (socIndex != -1 && temperatureIndex != -1) {
        float tableValue = generalConfig->externalChargeCurrentTable[temperatureIndex][socIndex];
        float pureCurrent = generalConfig->externalChargeUnitTable[temperatureIndex][socIndex];
        return (u16_t) roundf(pureCurrent ? tableValue : generalConfig->batteryCapacity * tableValue);
    }
    return 0;
}

void can2_send_status() {
    // id - CAN_BMS_STATUS
    // len - 7
    // msg[0:0] = 0: permission, 1: end_charge, 2: charging
    // msg[1:2] = set_curr
    // msg[3:6] = end_voltage

    u8_t b0 = 0;
    b0 |= (packState.chargeAllowed & 0b1) << 0; // bit 0
    b0 |= ((packState.SoC > 95) & 0b1) << 1;    // bit 1
    b0 |= (charge_switch_state & 0b1) << 2;     // bit 2

    u16_t curr = get_table_current();
    const float endVoltage = generalConfig->cellHardOverVoltage * (float) generalConfig->noOfCellsSeries;

    can2_buf.id = CAN_BMS_STATUS;
    can2_buf.len = 7;
    can2_buf.msg[0] = b0;   // set msg[0]
    memcpy((void *) can2_buf.msg + sizeof(b0), &curr, sizeof(curr));               // set msg[1]
    memcpy((void *) can2_buf.msg + sizeof(b0) + sizeof(curr), &endVoltage, sizeof(endVoltage)); // set msg[3]
    RingBuf_CellPut(&can2_buf, &can2_ring);
}

void can2_send_volt_threshold() {
    // id - CAN_BMS_VOLT_THRESH
    // len - 8
    // msg[0:3] = start_voltage
    // msg[4:7] = end_voltage

    // TODO: Check it
    const float startVoltage = generalConfig->cellHardUnderVoltage * (float) generalConfig->noOfCellsSeries;
    const float endVoltage = generalConfig->cellHardOverVoltage * (float) generalConfig->noOfCellsSeries;

    can2_buf.id = CAN_BMS_VOLT_THRESH;
    can2_buf.len = 8;
    memcpy((void *) can2_buf.msg, &startVoltage, sizeof(startVoltage));
    memcpy((void *) can2_buf.msg + sizeof(startVoltage), &endVoltage, sizeof(endVoltage));
    RingBuf_CellPut(&can2_buf, &can2_ring);
}

void can2_send_volt_alarm() {
    // id - CAN_BMS_VOLT_ALARM
    // len - 8
    // msg[0:3] = volt_alarm_lo
    // msg[4:7] = volt_alarm_up

    // TODO: Check it
    const float startFaultVoltage = generalConfig->cellLCSoftUnderVoltage * (float) generalConfig->noOfCellsSeries;
    const float endFaultVoltage = generalConfig->cellSoftOverVoltage * (float) generalConfig->noOfCellsSeries;

    can2_buf.id = CAN_BMS_VOLT_ALARM;
    can2_buf.len = 8;
    memcpy((void *) can2_buf.msg, &startFaultVoltage, sizeof(startFaultVoltage));
    memcpy((void *) can2_buf.msg + sizeof(startFaultVoltage), &endFaultVoltage, sizeof(endFaultVoltage));
    RingBuf_CellPut(&can2_buf, &can2_ring);
}

void can2_send_current() {
    // id - CAN_BMS_CURRENT
    // len - 8
    // msg[0:1] = [u16] start_current
    // msg[2:3] = [u16] start_current_interval
    // msg[4:5] = [u16] set_current
    // msg[6:7] = [u16] end_current

    u16_t chargeStartingCurrent = 15; // TODO move to generalConfig
    u16_t chargeStartingCurrentInterval = 20; // in seconds TODO move to generalConfig
    u16_t curr = get_table_current();
    u16_t chargeEndingCurrent = 5; // TODO move to generalConfig

    can2_buf.id = CAN_BMS_CURRENT;
    can2_buf.len = 8;
    memcpy((void *) can2_buf.msg, &chargeStartingCurrent, sizeof(chargeStartingCurrent));
    memcpy((void *) can2_buf.msg + sizeof(chargeStartingCurrent),
           &chargeStartingCurrentInterval,
           sizeof(chargeStartingCurrentInterval));
    memcpy((void *) can2_buf.msg + sizeof(chargeStartingCurrent) + sizeof(chargeStartingCurrentInterval),
           &curr,
           sizeof(curr));
    memcpy((void *) can2_buf.msg + sizeof(chargeStartingCurrent) + sizeof(chargeStartingCurrentInterval) + sizeof(curr),
           &chargeEndingCurrent,
           sizeof(chargeEndingCurrent));

    RingBuf_CellPut(&can2_buf, &can2_ring);
}

void can2_send_active() {
    // id - CAN_BMS_ACT_VAL
    // len - 6
    // msg[0:3] = [float] sum_voltage
    // msg[4:5] = [u16] sum_current

    can2_buf.id = CAN_BMS_ACT_VAL;
    can2_buf.len = 6;
    memcpy((void *) can2_buf.msg, &packState.packVoltage, sizeof(packState.packVoltage));
    memcpy((void *) can2_buf.msg + sizeof(packState.packVoltage),
           &packState.packCurrent, sizeof(packState.packCurrent));
    RingBuf_CellPut(&can2_buf, &can2_ring);
}


void transmitCan2Packet() {
    if (threshold_request) {
        // Send 501 - status, 503 - volt. threshold, 504 - volt. alarm, 505 - current
        can2_send_status();         // 501
        can2_send_volt_threshold(); // 503
        can2_send_volt_alarm();     // 504
        can2_send_current();        // 505

        threshold_request = 0;
    }
    if (active_request){
        // send 507 - active values
        can2_send_active();
        active_request = 0;
    }

    if ((TIM_Clock - can2_transmit_clock) < 10) {
        return;
    }
    can2_transmit_clock = TIM_Clock;

    can2_send_status();
}

void transmitCanPacketFromQueueCan1(void) {
    if (!HAL_CAN_GetTxMailboxesFreeLevel(&hcan1)) return; // if no free mailboxes
    u16_t len = 0;
    RingBuf_Available(&len, &can1_ring);
    if (!len) return;

    RingBuf_CellRead(&can1_tx_buf, &can1_ring);
    CAN_Transmit(can1_tx_buf.id, can1_tx_buf.msg, can1_tx_buf.len, &hcan1);
}

u32_t ctr;

void transmitCanPacketFromQueueCan2(void) {
    if (!HAL_CAN_GetTxMailboxesFreeLevel(&hcan2)) return; // if no free mailboxes
    u16_t len = 0;
    RingBuf_Available(&len, &can2_ring);
    if (!len) return;

//    volatile u32_t delta = HAL_GetTick() - ctr;
//    ctr = HAL_GetTick();
//
//    __NOP();

    RingBuf_CellRead(&can2_tx_buf, &can2_ring);
    CAN_Transmit(can2_tx_buf.id, can2_tx_buf.msg, can2_tx_buf.len, &hcan2);
}


void can2_receive_from_irq(CAN_RxHeaderTypeDef *hdr, uint8_t *data) {
    // If remote frame with STD ID
    if (hdr->RTR == CAN_RTR_REMOTE && hdr->IDE == CAN_ID_STD) {
        if (hdr->StdId == CAN_BMS_THRESH_REQ) {  // if Threshold request
            threshold_request = 1;
        } else if (hdr->StdId == CAN_BMS_ACT_REQ) {  // if Active request
            active_request = 1;
        }
    }
}