#include "Dezibot.h"
#include "esp_pm.h" 
#include "esp_task_wdt.h"

Dezibot dezibot;

const uint16_t cycleTime = 20e3; 

esp_pm_lock_handle_t cpuFreqLock;
esp_pm_lock_handle_t apbFreqLock;
esp_pm_lock_handle_t lightSleepLock;
static bool pmLocksCreated = false;


void stress_task0(void *pvParameters) {
  // Register ourselves with the task watchdog
  if (esp_task_wdt_add(NULL) != ESP_OK) {
    Serial.println("Failed to add task 0 to TWDT");
  }
  while (1) {
    volatile uint32_t x = 0;
    for (uint32_t i = 0; i < 10000; i++) {
      x += i;
    }
    esp_task_wdt_reset();
  }
}

void stress_task1(void *pvParameters) {
  // Register ourselves with the task watchdog
  if (esp_task_wdt_add(NULL) != ESP_OK) {
    Serial.println("Failed to add task 0 to TWDT");
  }
  while (1) {
    volatile uint32_t x = 0;
    for (uint32_t i = 0; i < 10000; i++) {
      x += i;
    }
    esp_task_wdt_reset();
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ;
  }
  uint32_t Freq = getCpuFrequencyMhz();
  Serial.print("CPU Freq = ");
  Serial.print(Freq);
  Serial.println(" MHz");
  Freq = getXtalFrequencyMhz();
  Serial.print("XTAL Freq = ");
  Serial.print(Freq);
  Serial.println(" MHz");
  Freq = getApbFrequency();
  Serial.print("APB Freq = ");
  Serial.print(Freq);
  Serial.println(" Hz");
  // Create lock handles for controlling power management features
  // Parameter arg is always unused and should thus be set to 0
  // Ref: https://docs.espressif.com/projects/esp-idf/en/v3.3.6/api-reference/system/power_management.html#enumerations
  if (!pmLocksCreated) {
    bool cpuFreqLockSuccess = esp_pm_lock_create(ESP_PM_CPU_FREQ_MAX, 0, "CPU_FREQ_MAX", &cpuFreqLock);
    bool apbFreqLockSuccess = esp_pm_lock_create(ESP_PM_APB_FREQ_MAX, 0, "APB_FREQ_MAX", &apbFreqLock);
    bool lightSleepLockSuccess = esp_pm_lock_create(ESP_PM_NO_LIGHT_SLEEP, 0, "NO_LIGHT_SLEEP", &lightSleepLock);
    if ((cpuFreqLockSuccess && apbFreqLockSuccess && lightSleepLockSuccess)) {
      pmLocksCreated = true;
      Serial.println("Sucessfully created handles to control power management features.");
    } else {
      Serial.println("Failed to create handles to control power management features!");
    }
  } else {
    Serial.println("Powermanagement lock handles already created, skipping creation!");
  }
  // Configure Task Watchdog so it does not interfere
  esp_task_wdt_config_t twdt_config = {
    .timeout_ms = 30000,
    .idle_core_mask = (1 << portNUM_PROCESSORS) - 1,
    .trigger_panic = true
  };
  if (esp_task_wdt_reconfigure(&twdt_config) != ESP_OK) {
    Serial.println("Failed to reconfigure task watchdog!");
  }
  delay(500);
  Serial.flush();
  Serial.end();
}

/*
 * A function that prints to serial, setting up the serial peripheral beforehand, and shutting it down afterwards.
 * Do not use this on a regular basis, as it is probably very slow. It is useful for not having the serial peripheral
 * active at all times.
 *
 * @param str The text to print
 */
void setupAndCleanupSerialPrint(char *str) {
  Serial.begin(115200);
  while (!Serial) {
    ;
    ;
  }
  delay(10);
  Serial.print(str);
  delay(10);
  Serial.flush();
  delay(10);
  Serial.end();
}

void loop() {
  // Alternate between 20 Seconds at 100% CPU load, no artificial load but not sleeping, and normal behaviour (light sleep when there is nothing to do).
  setupAndCleanupSerialPrint("Beginning stress phase\n");
  TaskHandle_t core0StressTask = NULL;
  TaskHandle_t core1StressTask = NULL;
  xTaskCreatePinnedToCore(stress_task0, "CPU0Stress", 4096, NULL, 1, &core0StressTask, 0);
  xTaskCreatePinnedToCore(stress_task1, "CPU1Stress", 4096, NULL, 1, &core1StressTask, 1);
  vTaskDelay(pdMS_TO_TICKS(cycleTime));
  setupAndCleanupSerialPrint("Still alive after waiting cycleTime after setting up stress tasks...\n");
  esp_task_wdt_delete(core0StressTask);
  esp_task_wdt_delete(core1StressTask);
  vTaskDelete(core0StressTask);
  vTaskDelete(core1StressTask);
  // Now disable light sleep
  setupAndCleanupSerialPrint("Beginning idle with power management disabled\n");
  esp_pm_lock_acquire(cpuFreqLock);
  esp_pm_lock_acquire(apbFreqLock);
  esp_pm_lock_acquire(lightSleepLock);
  vTaskDelay(pdMS_TO_TICKS(cycleTime));
  // Restore auto light sleep and dynamic frequency scaling
  setupAndCleanupSerialPrint("Beginning idle with power management reenabled\n");
  esp_pm_lock_release(cpuFreqLock);
  esp_pm_lock_release(apbFreqLock);
  esp_pm_lock_release(lightSleepLock);
  vTaskDelay(pdMS_TO_TICKS(cycleTime));
}