#include "HalPowerManager.h"

#include <Logging.h>
#include <WiFi.h>
#include <esp_sleep.h>

#include "HalGPIO.h"

void HalPowerManager::begin() {
  pinMode(BAT_GPIO0, INPUT);
  normalFreq = getCpuFrequencyMhz();
  modeMutex = xSemaphoreCreateMutex();
  assert(modeMutex != nullptr);
}

void HalPowerManager::setPowerSaving(bool enabled) {
  if (normalFreq <= 0) {
    return;
  }

  if (enabled) {
    if (WiFi.getMode() != WIFI_MODE_NULL) {
      enabled = false;
    }
    xSemaphoreTake(modeMutex, portMAX_DELAY);
    const LockMode mode = currentLockMode;
    xSemaphoreGive(modeMutex);
    if (mode == NormalSpeed) {
      enabled = false;
    }
  }

  if (enabled && !isLowPower) {
    LOG_DBG("PWR", "Going to low-power mode");
    if (!setCpuFrequencyMhz(LOW_POWER_FREQ)) {
      LOG_DBG("PWR", "Failed to set CPU frequency = %d MHz", LOW_POWER_FREQ);
      return;
    }
  }
  if (!enabled && isLowPower) {
    LOG_DBG("PWR", "Restoring normal CPU frequency");
    if (!setCpuFrequencyMhz(normalFreq)) {
      LOG_DBG("PWR", "Failed to set CPU frequency = %d MHz", normalFreq);
      return;
    }
  }
  isLowPower = enabled;
}

// RAII Lock implementation

HalPowerManager::Lock::Lock() {
  xSemaphoreTake(powerManager.modeMutex, portMAX_DELAY);
  powerManager.currentLockMode = NormalSpeed;
  valid = true;
  if (powerManager.isLowPower) {
    powerManager.setPowerSaving(false);
  }
  xSemaphoreGive(powerManager.modeMutex);
}

HalPowerManager::Lock::~Lock() {
  if (!valid) return;
  xSemaphoreTake(powerManager.modeMutex, portMAX_DELAY);
  powerManager.currentLockMode = None;
  xSemaphoreGive(powerManager.modeMutex);
}

void HalPowerManager::startDeepSleep(HalGPIO& gpio) const {
  // Ensure that the power button has been released to avoid immediately turning back on if you're holding it
  while (gpio.isPressed(HalGPIO::BTN_POWER)) {
    delay(50);
    gpio.update();
  }
  // Arm the wakeup trigger *after* the button is released
  esp_deep_sleep_enable_gpio_wakeup(1ULL << InputManager::POWER_BUTTON_PIN, ESP_GPIO_WAKEUP_GPIO_LOW);
  // Enter Deep Sleep
  esp_deep_sleep_start();
}

int HalPowerManager::getBatteryPercentage() const {
  static const BatteryMonitor battery = BatteryMonitor(BAT_GPIO0);
  return battery.readPercentage();
}