#include "battery.h"
#include "ADBMS_Driver.h"
#include "NTC.h"
#include "config_ADBMS6830.h"
#include <string.h>

#define SWO_LOG_PREFIX "[BATTERY] "
#include "swo_log.h"

uint16_t min_voltage = 0xFFFF;
int16_t max_temp = -1;
int16_t cellTemps[N_BMS][N_CELLS];

HAL_StatusTypeDef battery_init(SPI_HandleTypeDef *hspi) { 
    auto ret = AMS_Init(hspi);
    if (ret.status != ADBMS_NO_ERROR) {
        debug_log(LOG_LEVEL_ERROR, "Failed to initialize BMS: %s",
                  ADBMS_Status_ToString(ret.status));
        if (ret.bms_id != -1) {
            debug_log_cont(LOG_LEVEL_ERROR, " (on BMS ID: %hd)", ret.bms_id);
        }
        return HAL_ERROR;
    }
    debug_log(LOG_LEVEL_INFO, "Battery initialized successfully");
    return HAL_OK;
}

HAL_StatusTypeDef battery_update() {
  auto ret = AMS_Idle_Loop();
  if (ret.status != ADBMS_NO_ERROR) {
    debug_log(LOG_LEVEL_ERROR, "Failed to update battery data: %s",
              ADBMS_Status_ToString(ret.status));
    if (ret.bms_id != -1) {
      debug_log_cont(LOG_LEVEL_ERROR, " (on BMS ID: %hd)", ret.bms_id);
    }
    return HAL_ERROR;
  }

  min_voltage = 0xFFFF;
  max_temp = -1;

  for (size_t i = 0; i < N_BMS; i++) {
    for (size_t j = 0; j < N_CELLS; j++) {
      if (modules[i].cellVoltages[j] > min_voltage) {
        min_voltage = modules[i].cellVoltages[j];
      }
    }
    for (size_t j = 0; j < 10; j++) { //10 GPIOs
      cellTemps[i][j] = ntc_mv_to_celsius(modules[i].auxVoltages[j]);

      if (cellTemps[i][j] > max_temp) {
        max_temp = cellTemps[i][j];
      }
    }
  }

  return HAL_OK;
}

void print_battery_info() {
  for (size_t i = 0; i < N_BMS; i++) {
    debug_log(LOG_LEVEL_INFO, "Module %d status:", i);

    // Print cell voltages in 4x4 format
    debug_log(LOG_LEVEL_INFO, "  Cell voltages (mV):");
    debug_log(LOG_LEVEL_INFO, "    C0:  %4d  C1:  %4d  C2:  %4d  C3:  %4d",
              modules[i].cellVoltages[0], modules[i].cellVoltages[1],
              modules[i].cellVoltages[2], modules[i].cellVoltages[3]);
    debug_log(LOG_LEVEL_INFO, "    C4:  %4d  C5:  %4d  C6:  %4d  C7:  %4d",
              modules[i].cellVoltages[4], modules[i].cellVoltages[5],
              modules[i].cellVoltages[6], modules[i].cellVoltages[7]);
    debug_log(LOG_LEVEL_INFO, "    C8:  %4d  C9:  %4d  C10: %4d  C11: %4d",
              modules[i].cellVoltages[8], modules[i].cellVoltages[9],
              modules[i].cellVoltages[10], modules[i].cellVoltages[11]);
    debug_log(LOG_LEVEL_INFO, "    C12: %4d  C13: %4d  C14: %4d  C15: %4d",
              modules[i].cellVoltages[12], modules[i].cellVoltages[13],
              modules[i].cellVoltages[14], modules[i].cellVoltages[15]);

    // Print GPIO values
    debug_log(LOG_LEVEL_INFO, "  GPIO voltages (mV):");
    debug_log(LOG_LEVEL_INFO,
              "    G0:  %4d  G1:  %4d  G2:  %4d  G3:  %4d  G4:  %4d",
              modules[i].auxVoltages[0], modules[i].auxVoltages[1],
              modules[i].auxVoltages[2], modules[i].auxVoltages[3],
              modules[i].auxVoltages[4]);
    debug_log(LOG_LEVEL_INFO,
              "    G5:  %4d  G6:  %4d  G7:  %4d  G8:  %4d  G9:  %4d",
              modules[i].auxVoltages[5], modules[i].auxVoltages[6],
              modules[i].auxVoltages[7], modules[i].auxVoltages[8],
              modules[i].auxVoltages[9]);

    // Print temperatures
    debug_log(LOG_LEVEL_INFO, "  GPIO as temperatures (°C):");
    debug_log(LOG_LEVEL_INFO,
              "    G0:  %4d  G1:  %4d  G2:  %4d  G3:  %4d  G4:  %4d",
              cellTemps[i][0], cellTemps[i][1], cellTemps[i][2],
              cellTemps[i][3], cellTemps[i][4]);
    debug_log(LOG_LEVEL_INFO,
              "    G5:  %4d  G6:  %4d  G7:  %4d  G8:  %4d  G9:  %4d",
              cellTemps[i][5], cellTemps[i][6], cellTemps[i][7],
              cellTemps[i][8], cellTemps[i][9]);

    debug_log(LOG_LEVEL_INFO,
              "  Internal temp: %d, VAnalog: %d, VDigital: %d, VRef: %d",
              modules[i].internalDieTemp, modules[i].analogSupplyVoltage,
              modules[i].digitalSupplyVoltage, modules[i].refVoltage);

    // Print error flags if any are set
    bool hasFlags = false;
    char flagBuffer[128] = "";
    char *bufPos = flagBuffer;

    if (modules[i].status.CS_FLT) {
      bufPos = stpcpy(bufPos, "CS_FLT ");
      hasFlags = true;
    }
    if (modules[i].status.SMED) {
      bufPos = stpcpy(bufPos, "SMED ");
      hasFlags = true;
    }
    if (modules[i].status.SED) {
      bufPos = stpcpy(bufPos, "SED ");
      hasFlags = true;
    }
    if (modules[i].status.CMED) {
      bufPos = stpcpy(bufPos, "CMED ");
      hasFlags = true;
    }
    if (modules[i].status.CED) {
      bufPos = stpcpy(bufPos, "CED ");
      hasFlags = true;
    }
    if (modules[i].status.VD_UV) {
      bufPos = stpcpy(bufPos, "VD_UV ");
      hasFlags = true;
    }
    if (modules[i].status.VD_OV) {
      bufPos = stpcpy(bufPos, "VD_OV ");
      hasFlags = true;
    }
    if (modules[i].status.VA_UV) {
      bufPos = stpcpy(bufPos, "VA_UV ");
      hasFlags = true;
    }
    if (modules[i].status.VA_OV) {
      bufPos = stpcpy(bufPos, "VA_OV ");
      hasFlags = true;
    }
    if (modules[i].status.THSD) {
      bufPos = stpcpy(bufPos, "THSD ");
      hasFlags = true;
    }
    if (modules[i].status.SLEEP) {
      bufPos = stpcpy(bufPos, "SLEEP ");
      hasFlags = true;
    }
    if (modules[i].status.SPIFLT) {
      bufPos = stpcpy(bufPos, "SPIFLT ");
      hasFlags = true;
    }
    if (modules[i].status.COMPARE) {
      bufPos = stpcpy(bufPos, "COMPARE ");
      hasFlags = true;
    }
    if (modules[i].status.VDE) {
      bufPos = stpcpy(bufPos, "VDE ");
      hasFlags = true;
    }
    if (modules[i].status.VDEL) {
      bufPos = stpcpy(bufPos, "VDEL ");
      hasFlags = true;
    }

    debug_log(LOG_LEVEL_INFO, "  Status flags: %s",
              hasFlags ? flagBuffer : "[none]");

    debug_log(LOG_LEVEL_INFO, "  Conversion counter: %d",
              modules[i].status.CCTS);

    // Check for over/under voltage
    if (modules[i].overVoltage || modules[i].underVoltage) {
      debug_log(LOG_LEVEL_WARNING,
                "  Module %d voltage issues - OV: 0x%08lX, UV: 0x%08lX", i,
                modules[i].overVoltage, modules[i].underVoltage);
    }

    debug_log(LOG_LEVEL_INFO, "  ---------------");
  }
}