#include "can.h"

#include "ADBMS_Driver.h"
#include "battery.h"
#include "imd_monitoring.h"
#include "isotp.h"
#include "isotp_log_backend.h"
#include "isotp_user_defs.h"
#include "log.h"
#include "main.h"
#include "shunt_monitoring.h"
#include "soc_estimation.h"
#include "ts_state_machine.h"

#include "can-halal.h"

#include <math.h>
#include <stdint.h>
#include <string.h>

static isotp_conn_id_t isotp_connection_id = 0;

bool isotp_transmit(uint16_t id, const uint8_t *data, size_t datalen) {
    return ftcan_transmit(id, data, datalen) == HAL_OK;
}

uint32_t isotp_get_time() { return HAL_GetTick(); }

void can_init(FDCAN_HandleTypeDef *handle) {
    ftcan_init(handle);
    ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);
    ftcan_add_filter(CAN_ID_AMS_IN, 0xFFF);
    ftcan_add_filter(CAN_ID_AMS_DETAILS_FC, 0xFFF);
    ftcan_add_filter(ISOTP_LOG_FC_CAN_ID, 0xFFF);

    auto const status = isotp_add_connection(CAN_ID_AMS_DETAILS_FC, CAN_ID_AMS_DETAILS, ISOTP_FLAGS_NONE);
    if (status < 0) {
        log_warning("Failed to add ISO-TP connection: %s", isotp_status_to_string((isotp_status_t)status));
    }
    isotp_connection_id = (isotp_conn_id_t)status;

    // ftcan_add_filter(CAN_ID_SLAVE_PANIC, 0xFFF);
    // ftcan_add_filter(CAN_ID_SLAVE_STATUS_BASE, 0xFF0);
    // ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
}

HAL_StatusTypeDef can_send_status() {
    uint8_t data[8];
    data[0] = ts_state.current_state | (sdc_closed << 7);
    data[1] = roundf(battery.pack.soc);                            // Use the battery struct now
    ftcan_marshal_unsigned(&data[2], battery.pack.min_voltage, 2); // Use the battery struct now
    ftcan_marshal_signed(&data[4], battery.pack.max_temp, 2);      // Use the battery struct now
    data[6] = imd_data.state | (imd_data.ok << 7);
    if (imd_data.r_iso < 0xFFF) {
        data[7] = imd_data.r_iso >> 4;
    } else {
        data[7] = 0xFF;
    }
    const HAL_StatusTypeDef ret = ftcan_transmit(CAN_ID_AMS_STATUS, data, sizeof(data));
    if (ret != HAL_OK) {
        return ret;
    }
    data[0] = (sdc_closed_nodelay << 0) | (ts_error << 1) | (hv_active << 2) | (neg_air_closed << 3) |
              (pos_air_closed << 4) | (precharge_closed << 5) | (pre_and_air_open << 6);
    return ftcan_transmit(CAN_ID_AMS_SIGNALS, data, 1);
}

HAL_StatusTypeDef can_send_details() {
    static uint8_t module_index = 0;
    static uint8_t data[103] = {}; // sizeof(BMS_Chip) + 10 + 1
    auto const module = &bms_data[module_index];
    auto data_ptr = &data[1];

    isotp_status_t status = isotp_try_add_message(isotp_connection_id, data, sizeof(data));
    switch (status) {
    case ISOTP_OK:
        break;
    case ISOTP_MESSAGE_ALREADY_IN_FLIGHT:
        return HAL_BUSY;
    default:
        log_warning("isotp_try_add_message failed: %s", isotp_status_to_string(status));
        return HAL_ERROR;
    }

    data[0] = module_index;
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->bmsID, 8);
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->status.CS_FLT, 2);
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->status.CCTS, 2);

    // Marshal status bits into a single byte
    uint8_t status_bits = 0;
    status_bits |= module->status.SMED << 0;
    status_bits |= module->status.SED << 1;
    status_bits |= module->status.CMED << 2;
    status_bits |= module->status.CED << 3;
    status_bits |= module->status.VD_UV << 4;
    status_bits |= module->status.VD_OV << 5;
    status_bits |= module->status.VA_UV << 6;
    status_bits |= module->status.VA_OV << 7;
    *(data_ptr++) = status_bits;

    // Marshal the rest of the status bits
    status_bits = 0;
    status_bits |= module->status.OSCCHK << 0;
    status_bits |= module->status.TMODCHK << 1;
    status_bits |= module->status.THSD << 2;
    status_bits |= module->status.SLEEP << 3;
    status_bits |= module->status.SPIFLT << 4;
    status_bits |= module->status.COMPARE << 5;
    status_bits |= module->status.VDE << 6;
    status_bits |= module->status.VDEL << 7;
    *(data_ptr++) = status_bits;

    // Marshal voltage flags
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->overVoltage, 4);
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->underVoltage, 4);

    // Marshal temperature and voltages
    data_ptr = ftcan_marshal_signed(data_ptr, module->internalDieTemp, 2);
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->analogSupplyVoltage, 2);
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->digitalSupplyVoltage, 2);
    data_ptr = ftcan_marshal_unsigned(data_ptr, module->refVoltage, 2);

    // Marshal cell voltages
    for (int i = 0; i < 16; i++) {
        data_ptr = ftcan_marshal_signed(data_ptr, module->cellVoltages[i], 2);
    }

    // Marshal auxiliary voltages
    for (int i = 0; i < 10; i++) {
        data_ptr = ftcan_marshal_signed(data_ptr, module->auxVoltages[i], 2);
    }

    // Marshal temperature data
    for (int i = 0; i < 10; i++) {
        data_ptr = ftcan_marshal_signed(data_ptr, battery.module[module_index].cellTemps[i], 2);
    }

    if ((status = isotp_add_message(isotp_connection_id, data, sizeof(data))) != ISOTP_OK) {
        log_warning("isotp_add_message failed unexpectedly: %s", isotp_status_to_string(status));
        return HAL_ERROR;
    }

    module_index = (module_index + 1) % N_BMS;
    return HAL_OK;
}

HAL_StatusTypeDef can_send_error(TSErrorKind kind, uint8_t arg) {
    uint8_t data[2];
    data[0] = kind;
    data[1] = arg;
    return ftcan_transmit(CAN_ID_AMS_ERROR, data, sizeof(data));
}

void ftcan_msg_received_cb(uint16_t id, size_t len, const uint8_t *data) {
    if ((id & 0xFF0) == CAN_ID_SHUNT_BASE) {
        shunt_handle_can_msg(id, data);
        return;
    }

    switch (id) {
    case ISOTP_LOG_FC_CAN_ID:
    case CAN_ID_AMS_DETAILS_FC:
        auto const status = isotp_handle_incoming(id, data, len);
        if (status != ISOTP_OK) {
            log_debug("Error when handling flow control: %s", isotp_status_to_string(status));
        }
        break;
    case CAN_ID_AMS_IN:
        if(len == 1){                       //check bc inverter sends 4 byte message to 0xB which causes discharge (prob)
            ts_sm_handle_ams_in(data);
        }
        break;
    default:
        break;
    }
}