/**
 * @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"
static portMUX_TYPE mux;

void Power::begin() {
  // Check if another instance of us already initialized the power scheduler,
  // if not, we will do it.
  if (powerScheduler == nullptr) {
    powerScheduler = &PowerScheduler::getPowerScheduler();

    if (!(powerScheduler->tryAccquireCurrentAllowance(
            PowerParameters::PowerConsumers::ESP,
            PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
      Serial.println("Alledgedly not enough power available to reserve the "
                     "ESP32s base power consumption. Something is wrong.");
      return;
    }
  }
}

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

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

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

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

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

float Power::getBatteryVoltage() {
  // Get the battery voltage from the ADC and convert it to a voltage
  // using the voltage divider.
  portENTER_CRITICAL(&mux);
  // Enable voltage divider
  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH);
  // Returns value between 0 and 4095 mapping to between 0 and 3.3V
  uint16_t batteryAdcValue =
      analogRead(PowerParameters::PinConfig::BAT_ADC) *
      (PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR);
  // Disable voltage divider
  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW);
  portEXIT_CRITICAL(&mux);
  // Convert ADC value to voltage
  float batteryVoltage =
      (batteryAdcValue * 3.3 / 4095) *
      PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR;
  return batteryVoltage;
}

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

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



int Power::getBatteryVoltageChargePercent() {
  // 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 {
    // Calculate battery charge state from Charge consumption
    float oldChargeState = lastSOC[latestSoCIndex];
    float 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();

  return;

}

float Power::getMax3V3Current() {
  float u_bat = getBatteryVoltage();
  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C;
  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;
}

PowerScheduler *Power::powerScheduler = nullptr;

Power::Power() {
  // Initialize the power scheduler
  powerScheduler = &PowerScheduler::getPowerScheduler();
  // Initialize the mutex
  mux = portMUX_INITIALIZER_UNLOCKED;
}