#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_task(void *pvParameters) {
    // Register with task watchdog
    ESP_ERROR_CHECK(esp_task_wdt_add(NULL));
    
    // Variables for load generation
    volatile uint32_t x = 0;
    TickType_t last_wake_time = xTaskGetTickCount();
    
    while (1) {
        // Compute-intensive period
        for (uint32_t i = 0; i < 10000; i++) {
            x += i;
            // Mix in some memory operations
            if (i % 100 == 0) {
                // Force cache misses occasionally
                void* temp = malloc(32);
                if (temp) {
                    free(temp);
                }
            }
        }
        
        // Reset watchdog
        ESP_ERROR_CHECK(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();
}

void blip_io(int times) {
  constexpr int ioPin = 17;
  pinMode(ioPin, OUTPUT);
  for(int i = 0; i<times; i++) {
    digitalWrite(ioPin,1);
    delay(1);
    digitalWrite(ioPin,0);
    delay(1);
  }
}

/*
 * 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);
  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");
  blip_io(1);
  TaskHandle_t core0StressTask = NULL;
  TaskHandle_t core1StressTask = NULL;
  xTaskCreatePinnedToCore(stress_task, "CPU0Stress", 4096, NULL, 1, &core0StressTask, 0);
  xTaskCreatePinnedToCore(stress_task, "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");
  blip_io(2);
  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");
  blip_io(3);
  esp_pm_lock_release(cpuFreqLock);
  esp_pm_lock_release(apbFreqLock);
  esp_pm_lock_release(lightSleepLock);
  vTaskDelay(pdMS_TO_TICKS(cycleTime));
}