/**
 * @file Power.h
 * @author Phillip Kühne
 * @brief This component provides utilities for keeping track of power usage
 * consumption.
 * @version 0.1
 * @date 2024-11-23
 */

#include "Power.h"

void vTaskUpdatePowerState(void *pvParameters) {
  for (;;) {
    ESP_LOGV(TAG, "Updating Power State...");
    Power::updatePowerStateHandler();
    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

void Power::begin() {
  // Check if another instance of us already initialized the power scheduler,
  // if not, we will do it.
  ESP_LOGI(TAG, "Initializing Power Management");

  if (powerScheduler == nullptr) {
    ESP_LOGI(TAG, "Creating Power Scheduler");
    powerScheduler = &PowerScheduler::getPowerScheduler(
        PowerParameters::Battery::CELL_CURRENT_1C_MA,
        PowerParameters::Battery::CELL_CURRENT_2C_MA);
    Power::initPowerState();
    Power::recalculateCurrentBudgets();
    TaskHandle_t xHandle = NULL;
    xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
                tskIDLE_PRIORITY, &xHandle);
    configASSERT(xHandle);

    if (!(powerScheduler->tryAccquireCurrentAllowance(
            PowerParameters::PowerConsumers::ESP,
            PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
      ESP_LOGE(TAG, "Could not get power for ESP");
      Serial.println("Could not get power for ESP");
    }
  }
}

float Power::getFreeLimitCurrentBudget() {
  return powerScheduler->getFreeLimitCurrentBudget();
}

float Power::getFreeMaximumCurrentBudget() {
  return powerScheduler->getFreeMaximumCurrentBudget();
}

bool Power::tryAccquireCurrentAllowance(
    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
    uint16_t requestedDurationMs) {
  return powerScheduler->tryAccquireCurrentAllowance(consumer, neededCurrent,
                                                     requestedDurationMs);
}

void Power::releaseCurrent(PowerParameters::PowerConsumers consumer) {
  powerScheduler->releaseCurrent(consumer);
}

bool Power::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                    uint16_t neededCurrent,
                                    uint16_t maxSlackTimeMs,
                                    uint16_t requestedDurationMs) {
  return powerScheduler->waitForCurrentAllowance(
      consumer, neededCurrent, maxSlackTimeMs, requestedDurationMs);
}

void Power::beginPermanentDeepSleep(void) {
  return powerScheduler->beginPermanentDeepSleep();
}

float Power::getCurrentCurrent(void) {
  return powerScheduler->getCurrentCurrent();
}

void Power::recalculateCurrentBudgets(void) {
  return powerScheduler->recalculateCurrentBudgets();
}

float Power::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
  return powerScheduler->getConsumerCurrent(consumer);
}

float Power::getBatteryVoltage() {
  // Get the battery voltage from the ADC and convert it to a voltage
  // using the voltage divider.
  pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT);
  pinMode(PowerParameters::PinConfig::BAT_ADC, INPUT);
  // Enable voltage divider
  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH);
  // Allow voltage to stabilize
  delayMicroseconds(10);
  // Returns value between 0 and 4095 mapping to between 0 and 3.3V
  uint16_t batteryAdcValue = analogRead(PowerParameters::PinConfig::BAT_ADC);
  // Disable voltage divider
  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW);
  // Convert ADC value to voltage
  float batteryVoltage =
      (batteryAdcValue / 4095.0 * 3.3) *
      PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR;
  ESP_LOGD(TAG, "Battery ADC value: %d, Calculated voltage: %.2f",
           batteryAdcValue, batteryVoltage);
  return batteryVoltage;
}

float Power::getBatteryChargePercent() { return percentRemaining; }

float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }

float Power::getBatteryVoltageChargePercent() {
  // Directly get the battery voltage and calculate the charge state based on
  // the discharge curve.
  float batteryVoltage = getBatteryVoltage();
  float chargeState = 0;
  // Clamp edge cases
  if (batteryVoltage >=
      PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0]) {
    return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[0];
  }
  if (batteryVoltage <=
      PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES
          [PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1]) {
    return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES
        [PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1];
  }
  float p1_x, p1_y, p2_x, p2_y;
  for (int i = 0; i < PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS;
       i++) {
    if (batteryVoltage >=
        PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i]) {
      p1_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i];
      p1_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i];
      p2_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i + 1];
      p2_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i + 1];
      chargeState =
          ((p2_y - p1_y) / (p2_x - p1_x)) * (batteryVoltage - p1_x) + p1_y;
      return chargeState;
    }
  }
}

void Power::updatePowerStateHandler() {
  float currentCurrent = powerScheduler->getCurrentCurrent();
  int referenceCurrentMa =
      PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;

  // Calculate remaining battery charge in Coulombs based on current and time
  float coloumbsConsumedSinceLastUpdate =
      (currentCurrent / 1000) *
      ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);

  // Update coloumbs remaining
  coloumbsRemaining -= coloumbsConsumedSinceLastUpdate;

  float chargeState;

  // If current flow is close enough to reference, get battery charge state via
  // voltage curve
  if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
      (currentCurrent < (referenceCurrentMa * 1.4))) {
    // Get battery charge state from voltage curve
    chargeState = getBatteryVoltageChargePercent();
  } else {
    // Estimate battery charge state from charge consumption
    float oldChargeState = lastSOC[latestSoCIndex];
    chargeState =
        oldChargeState - ((coloumbsConsumedSinceLastUpdate /
                           PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
                          100);
  }

  addSoCSample(chargeState);

  // Update percentage remaining based on charge state average
  float sampleSum = 0;
  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
    sampleSum += lastSOC[i];
  }
  percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;

  // Update last update time
  lastPowerStateUpdate = xTaskGetTickCount();

  // Update the available current (changes based on battery state of charge)
  powerScheduler->recalculateCurrentBudgets();
  ESP_LOGV(TAG, "Current: %f mA, Charge: %f Coulombs, %d %%", currentCurrent,
           coloumbsRemaining, percentRemaining);
  return;
}

float Power::getMax3V3Current() {
  float u_bat = getBatteryVoltage();
  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA;
  float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
  constexpr float u_3v3 = 3.3;
  return (u_bat * i_bat * eta) / u_3v3;
}

void Power::addSoCSample(float soc) {
  latestSoCIndex =
      (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
  lastSOC[latestSoCIndex] = soc;
}

void Power::initPowerState(void) {
  // Initialize the power state
  lastPowerStateUpdate = xTaskGetTickCount();
  // TODO: Get initial battery charge state based on voltage, set coloumbs based
  // on that
  const float initialChargePercentages = getBatteryVoltageChargePercent();
  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
    lastSOC[i] = initialChargePercentages;
  }
  coloumbsRemaining = initialChargePercentages / 100 *
                      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
  constexpr float fullColoumbs =
      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
  percentRemaining = initialChargePercentages;
}

void Power::dumpPowerStatistics() {
  Serial.printf("======== Dezibot Power Statistics ========\r\n");
  Serial.printf("Current: %f mA\r\n", Power::getCurrentCurrent());
  Serial.printf("Battery Voltage: %f V\r\n", Power::getBatteryVoltage());
  Serial.printf("Battery Charge: %f %%\r\n", Power::getBatteryChargePercent());
  Serial.printf("Battery Charge: %f Coulombs\r\n", Power::getBatteryChargeCoulombs());
  Serial.printf("Max 3.3V Current in this state: %f mA\r\n", Power::getMax3V3Current());
  Serial.printf("=========================================\r\n");
}

void Power::dumpConsumerStatistics() {
  Serial.printf("======== Dezibot Consumer Statistics ========\r\n");
  Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::ESP));
  Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::WIFI));
  Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT));
  Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT));
  Serial.printf("LED_RGB_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_BOTTOM));
  Serial.printf("RGBW_SENSOR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
  Serial.printf("LED_IR_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM));
  Serial.printf("LED_IR_FRONT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
  Serial.printf("PT_IR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
  Serial.printf("PT_DL: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
  Serial.printf("LED_UV: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
  Serial.printf("DISPLAY_OLED: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
  Serial.printf("MOTOR_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
  Serial.printf("MOTOR_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
  Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::IMU));
  Serial.printf("=============================================\r\n");
}

int Power::latestSoCIndex = 0;
float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0};
TickType_t Power::lastPowerStateUpdate = 0;
float Power::coloumbsRemaining =
    PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
float Power::percentRemaining = 100.0;
PowerScheduler *Power::powerScheduler = nullptr;

Power::Power() {}