#include "imd_monitoring.h"
#include "log.h"
#include "main.h"
#include "soc_estimation.h"
#include "stm32h7xx_hal.h"

#include "shunt_monitoring.h"
#include "ts_state_machine.h"
#include "battery.h"

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

void print_master_status() {
  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 ------", COMMIT_BRANCH,
            COMMIT_HASH, COMPILE_DATE);

  swo_write( "\nGeneral:");
  swo_write( "  State:      %s", TSStateToString(ts_state.current_state));
  swo_write( "  Target:     %s", TSStateToString(ts_state.target_state));
  swo_write( "  Err Source: %s", ts_state.error_source == 0 ? "NONE" : (ts_state.error_source == 0b11 ? "SHUNT, SLAVE" : (ts_state.error_source == 0b01 ? "SHUNT" : "SLAVE")));
  swo_write("  Err type:   %s", ts_state.error_type == 0 ? "NONE" : (ts_state.error_type == 0x01 ? "SLAVE_TIMEOUT" : (ts_state.error_type == 0x02 ? "SLAVE_PANIC" : (ts_state.error_type == 0x03 ? "SHUNT_TIMEOUT" : (ts_state.error_type == 0x04 ? "SHUNT_OVERCURRENT" : (ts_state.error_type == 0x05 ? "SHUNT_OVERTEMP" : "UNKNOWN"))))));
  swo_write("  HV active:  %s", hv_active ? "YES" : "NO");

  swo_write("\nRelay positions:");
  swo_write("  SDC:       %s", sdc_closed ? "CLOSED" : "OPEN");
  swo_write("  Air-:      %s", neg_air_closed ? "CLOSED" : "OPEN");
  swo_write("  Air+:      %s", pos_air_closed ? "CLOSED" : "OPEN");
  swo_write("  Precharge: %s", precharge_closed ? "CLOSED" : "OPEN");
  swo_write("  Precharge/Air+ sense: %s", pre_and_air_open ? "HIGH" : "LOW");

  swo_write("\nIMD data:");
  swo_write("  State:      %s", IMDStateToString(imd_data.state));
  swo_write("  R_iso:      %lu kOhm", imd_data.r_iso);
  swo_write("  Frequency:  %lu Hz", imd_data.freq);
  swo_write("  Duty cycle: %lu %%", imd_data.duty_cycle);
  swo_write("  Last high:  %lu ms", imd_data.last_high);
  swo_write("  OK:         %s", imd_data.ok ? "YES" : "NO");

  swo_write("\nShunt data:");
  swo_write("  Voltage:    %ld mV", shunt_data.voltage_bat);
  swo_write("  Current:    %ld mA", shunt_data.current);
  swo_write("  Power:      %ld W", shunt_data.power);
  swo_write("  Energy:     %ld Ws", shunt_data.energy);
  swo_write("  Temp:       %ld °C", shunt_data.busbartemp / 10);
  swo_write("  Time delta: %lu ms", HAL_GetTick() - shunt_data.last_message);

  swo_write("\nBattery data:");
  swo_write("  Min/Max voltage: %d mV / %d mV", min_voltage, max_voltage);
  swo_write("  Min/Max temp:    %d °C / %d °C", min_temp, max_temp);
  swo_write("  SoC:            %.2f %%", current_soc);
  swo_write("  Module data: Min V | Max V | Std Dev | Min T | Max T");
  auto max_std_dev = 0.0f;
  for (size_t i = 0; i < N_BMS; i++) {
    if (module_std_deviation[i] > max_std_dev) {
      max_std_dev = module_std_deviation[i];
    }
  }
  for (size_t i = 0; i < N_BMS; i++) {
    #if USE_ANSI_ESCAPE_CODES
      #define COLOR_MIN "\033[38;5;75m" //blue for min
      #define COLOR_MAX "\033[38;5;203m" //red for max
      #define COLOR_RESET "\033[0m"
      swo_write("  %2zu: %s%5d mV%s | %s%5d mV%s | %s%.2f mV%s | %s%3d °C%s | %s%3d °C%s", i,
          (module_voltages[i].min == min_voltage) ? COLOR_MIN : "", (module_voltages[i].min), COLOR_RESET,
          (module_voltages[i].max == max_voltage) ? COLOR_MAX : "", (module_voltages[i].max), COLOR_RESET,
          (module_std_deviation[i] > max_std_dev) ? COLOR_MAX : "", module_std_deviation[i], COLOR_RESET,
          (module_temps[i].min == min_temp) ? COLOR_MIN : "", (module_temps[i].min), COLOR_RESET,
          (module_temps[i].max == max_temp) ? COLOR_MAX : "", (module_temps[i].max), COLOR_RESET);
    #else
    swo_write("  %2zu: %5d mV | %5d mV | %.2f mV | %3d °C | %3d °C", i,
              module_voltages[i].min, module_voltages[i].max,
              module_std_deviation[i],
              module_temps[i].min, module_temps[i].max);
    #endif
  }

  swo_write("\n------ Updated at %lu ------", HAL_GetTick());
}

