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

#define DEBUG_CHANNEL 2 // channel to output messages on
#include "swo_log_backend.h"

void print_battery_info() {
  auto backend = swo_log_get_backend();

  if (!backend->is_enabled(LOG_LEVEL_INFO)) {
    return; // No need to print if the backend is not enabled for this log level
  }

  log_message_t log_msg = {};

  #define swo_write(...) \
    snprintf(log_msg.message, sizeof(log_msg.message), __VA_ARGS__); \
    backend->write(&log_msg); \
    
  // clear the console
  backend->clear();

    swo_write("------ AMS_Master on %s (%s), compiled at %s ------\n", COMMIT_BRANCH,
              COMMIT_HASH, COMPILE_DATE);

    for (size_t i = 0; i < N_BMS; i++) {
      swo_write("Module %d status:", i);
      swo_write("  BMS ID: 0x%08lx%08lx", (uint32_t)(modules[i].bmsID >> 32), (uint32_t)(modules[i].bmsID & 0xFFFFFFFF));
  
      // Print cell voltages in 4x4 format
      swo_write("  Cell voltages (mV):");
      swo_write("    C0:  %4d  C1:  %4d  C2:  %4d  C3:  %4d",
                modules[i].cellVoltages[0], modules[i].cellVoltages[1],
                modules[i].cellVoltages[2], modules[i].cellVoltages[3]);
      swo_write("    C4:  %4d  C5:  %4d  C6:  %4d  C7:  %4d",
                modules[i].cellVoltages[4], modules[i].cellVoltages[5],
                modules[i].cellVoltages[6], modules[i].cellVoltages[7]);
      swo_write("    C8:  %4d  C9:  %4d  C10: %4d  C11: %4d",
                modules[i].cellVoltages[8], modules[i].cellVoltages[9],
                modules[i].cellVoltages[10], modules[i].cellVoltages[11]);
      swo_write("    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
      swo_write("  GPIO voltages (mV):");
      swo_write(
                "    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]);
      swo_write(
                "    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
      swo_write("  GPIO as temperatures (°C):");
      swo_write(
                "    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]);
      swo_write(
                "    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]);
  
      swo_write(
                "  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;
      }

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

      if (modules[i].status.CS_FLT) { // Print out which ADCs are faulting
        swo_write("Comparison fault on ADC/Cell(s): ");
        for (ssize_t j = 0; j < 16; j++) {
          if (modules[i].status.CS_FLT & (1u << j)) {
            swo_write("%d ", j);
          }
        }
      }

      swo_write("  Conversion counter: %d",
                modules[i].status.CCTS);
    }
    swo_write("\n------ Updated at %lu ------", HAL_GetTick());
  }