#include <driver/gptimer.h>
#include "servo.hpp"
#include "defines.h"
#include "nvs_flash.h"
#include "NimBLEDevice.h"
#include "WiFi.hpp"
#include "setup.hpp"
#include "socketIO.hpp"
#include "encoder.hpp"
#include "calibration.hpp"
#include "esp_pm.h"
#include "mainEventLoop.hpp"
#include "max17048.h"
#include "bms_test.hpp"
#include "servo_test.hpp"
#include "encoder_test.hpp"

// Global encoder instances
Encoder* topEnc = new Encoder(ENCODER_PIN_A, ENCODER_PIN_B);
Encoder* bottomEnc = new Encoder(InputEnc_PIN_A, InputEnc_PIN_B);

void mainApp() {
  esp_err_t ret = nvs_flash_init(); // change to secure init logic soon!!
  // 2. If NVS is full or corrupt (common after flashing new code), erase and retry
  if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
    ESP_ERROR_CHECK(nvs_flash_erase());
    ret = nvs_flash_init();
  }
  ESP_ERROR_CHECK(ret);

  main_event_queue = xQueueCreate(10, sizeof(main_event_type_t));
  
  WiFi::init();
  Calibration::init();
  
  // Initialize encoders
  topEnc->init();
  bottomEnc->init();
  servoInit();
  max17048_init();
  printf("beforeSetup\n");
  setupAndCalibrate();
  printf("afterSetup\n");

  xTaskCreate(wakeTimer, "wakeTimer", 2048, NULL, 5, &wakeTaskHandle);

  mainEventLoop();
}

void pm_init() {
    esp_pm_config_t pm_config = {
        .max_freq_mhz = 160,        // Max CPU frequency
        .min_freq_mhz = 80,         // Min CPU frequency (DFS)
        .light_sleep_enable = true  // ALLOW CPU to power down during idle
    };
    ESP_ERROR_CHECK(esp_pm_configure(&pm_config));
}

extern "C" void app_main() {
  pm_init();
  mainApp();
  // servo_test();
}