//
// 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) < 1000) {
        return;
    }

    uint8_t data1[8] = {0x45};
    CAN_Transmit(300, data1, 8, &hcan1);
    can1_transmit_clock = TIM_Clock;
    return;

    CAN_TxHeaderTypeDef header;
    header.IDE = CAN_ID_STD;
    header.RTR = CAN_RTR_DATA;
    header.StdId = 0x444;
    header.DLC = 1;

    uint8_t data[32] = {};
    uint32_t mailbox = 0;

    // sending SOC, SOH and number of cells
    uint16_t id = 0x101;
    memcpy(data, &id, sizeof(id));
    memcpy(data + 2, &packState.SoC, sizeof(packState.SoC));
    memcpy(data + 6, &packState.SoCCapacityAh, sizeof(packState.SoCCapacityAh));
    memcpy(data + 10, &generalConfig->noOfCellsSeries, sizeof(generalConfig->noOfCellsSeries));
    header.DLC = 11;
    HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);

    // sending charge current, discharge current // TODO
    id = 0x102;
    memcpy(data, &id, sizeof(id));
    memcpy(data + 2, &packState.packCurrent, sizeof(packState.packCurrent));
    memcpy(data + 6, &packState.loCurrentLoadCurrent, sizeof(packState.loCurrentLoadCurrent));
    header.DLC = 10;
    HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);

    // sending BMS state, input state, output state // TODO
    id = 0x103;
    memcpy(data, &id, sizeof(id));
    memcpy(data + 2, &packState.cellVoltageLow, sizeof(packState.cellVoltageLow));
    memcpy(data + 6, &packState.cellVoltageAverage, sizeof(packState.cellVoltageAverage));
    memcpy(data + 10, &packState.cellVoltageHigh, sizeof(packState.cellVoltageHigh));
    header.DLC = 12;
    HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);

    // sending cell voltages
    id = 0x200;
    for (int cellPointer = 0;
         cellPointer < generalConfig->noOfCellsSeries * generalConfig->noOfParallelModules; ++cellPointer) {
        ++id;
        memcpy(data, &id, sizeof(id));
        float voltage = 0;
        if (packState.cellVoltagesIndividual[cellPointer].cellBleedActive)
            voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * -1000;
        else
            voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * 1000;
        memcpy(data + 2, &voltage, sizeof(voltage));
        header.DLC = 6;
        HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
    }

    // sending temperatures
    id = 0x300;
    for (int sensorPointer = 0; sensorPointer < NoOfTempSensors; ++sensorPointer) {
        ++id;
        memcpy(data, &id, sizeof(id));
        float temperature = packState.temperatures[sensorPointer];
        memcpy(data + 2, &temperature, sizeof(temperature));
        header.DLC = 6;
        HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
    }

    can1_transmit_clock = TIM_Clock;
}

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;
        }
    }
}