//#include <variant>
#include <string>
//#include <vector>
//#include <cstdlib>
//#include <ctime>

//#include <stdio.h>    // For fprintf and printf
//#include <string.h>   // For strerror and strrchr
//#include <errno.h>    // For errno
#include <stdint.h>  // For int64_t
#include "esp_task_wdt.h"
#include "esp_pm.h" // Include ESP-IDF power management API

//------------------------------------------- My Includes -------------------------------------------
//#include <string>     // For std::string
//#include <cstring>    // For strcmp
#include "freertos/FreeRTOS.h"
#include "freertos/task.h" // For vTask_DELAY and xTaskGetTickCount


void loop() {}

std::string string_toDotString(const char* string, size_t spacing) {
    // Temporary buffer to hold the formatted string
    size_t len = strlen(string);
    size_t segments = len / spacing;
    size_t mod = (len % spacing)  ;
    size_t s = 1;
    char buffer[len + segments+1];
    if (!mod == 0) {
        for (size_t pre = 0; pre < mod;pre++) {
            buffer[pre] = string[pre];
        }
    buffer[ mod ]= '.';   
    } else{
      s=0;
      mod--;
      len--;
    }

    for (size_t j = mod+1; j < len + segments ; j+= spacing) {
        for (size_t i = 0; i < spacing; i++) {
            buffer[j+i] = string[j+i-s];  
            }
    j++;
    s++;
    buffer[ j + spacing -1]= '.';
    }
    buffer[len + segments] = '\0';

    if (buffer[0] == '-' && buffer[1] == '.') {        
        buffer[len + segments] = '\0';

        // Shift characters to the left by one position starting from index 2
        for (int i = 1; buffer[i + 1] != '\0'; ++i) {
            buffer[i] = buffer[i + 1];
        }
		// null-terminated
        buffer[strlen(buffer) - 1] = '\0';
    }
    return buffer;
}

void cpu_setClock()
{
  esp_pm_config_t pm_config = {
      .max_freq_mhz = 240, // Maximum frequency 
      .min_freq_mhz = 240, // Minimum frequency 
      .light_sleep_enable = false                     // Disable light sleep to keep fixed frequency
  };

  esp_err_t err = esp_pm_configure(&pm_config);
  if (err != ESP_OK)
  {
    printf( "Failed to configure power management: %s\n", esp_err_to_name(err));
  }

  err = esp_pm_get_configuration(&pm_config);  
  if (err == ESP_OK)
  {
    printf("CPU frequency set to: %d/%d MHz\n",
              pm_config.min_freq_mhz,
              pm_config.max_freq_mhz
              );
    if (pm_config.light_sleep_enable == true) {
    printf("CPU light sleep: ON\n");
    }
    else {
    printf("CPU light sleep: OFF\n");
    }
    
  } else if (err == ESP_ERR_NOT_SUPPORTED) {
    printf("Cannot set Clock Frequency: Please enable:'CONFIG_PM_ENABLE=y' in sdkconfig\n");
  }
  else {
    printf("Failed to get power management configuration: %s\n", esp_err_to_name(err)); 
  }

}


void wdt_setTimeout(int timeoutMs)
{
  printf("Setting WDT timeout to: %d ms\n", timeoutMs);
  esp_err_t err;

  
  // Deinitialize the default WDT
  err = esp_task_wdt_deinit();
  if (err != ESP_OK && err != ESP_ERR_INVALID_STATE)
  {
    printf("Error deinitializing WDT: %s\n", esp_err_to_name(err));
    return; // Exit if deinitialization fails
  } 
  else {
    printf("WDT de-initialized successfully\n");
  }

  // ESP-IDF-specific WDT initialization
  esp_task_wdt_config_t config;
  config.timeout_ms = 9000;   // Set timeout to 10 seconds
  config.idle_core_mask = 0;   // Monitor all cores
  config.trigger_panic = true; // Trigger panic on timeout

  // Initialize watchdog with new configuration
  err = esp_task_wdt_init(&config);
  if (err != ESP_OK)
  {
    printf("Error initializing WDT in ESP-IDF: %s\n", esp_err_to_name(err));
    return ; // Exit if initialization fails
  } 
  else {
    printf("WDT initialized successfully\n");
  }


  // Add current task to WDT monitoring
  err = esp_task_wdt_add(NULL); // NULL is equivalent to xTaskGetCurrentTaskHandle()
  if (err != ESP_OK)
  {
    printf("Error adding task to WDT in ESP-IDF: %s\n", esp_err_to_name(err));
    return; // Exit if task addition fails
  }
  printf("Task added to WDT monitoring:\n");
  printf("Watchdog: ON\n");
  // m_wdtEnabled = true; // WDT was enabled successfully
}

#define TEST_RTOS 1


static const TickType_t watchdog_period = 5000 / portTICK_PERIOD_MS;
static const TickType_t performance_period = 1000 / portTICK_PERIOD_MS;
static const TickType_t biggerTask_period = 50 / portTICK_PERIOD_MS;
static const TickType_t smallTask_period = 20 / portTICK_PERIOD_MS;
static const int smallTask_iterations = 32;
    
#if TEST_RTOS
// Performance counter task
void task_perf(void *pvParameters) {
    printf("%s created on core: %d | every: %d ms \n", __FUNCTION__, xPortGetCoreID(), performance_period);
    uint32_t performance_lastResetTime = xTaskGetTickCount(); 
    uint32_t performanceCounter = 0;

while (true) {
    performanceCounter++;
    if (xTaskGetTickCount() - performance_lastResetTime >= performance_period) {
        printf("core: %d | RTOS Performance score: %lu|\n", xPortGetCoreID(), performanceCounter);
        performanceCounter = 0;
        performance_lastResetTime += performance_period;
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}
}


// bigger periodic task
void task_biggerTask(void *pvParameters) {
    printf("%s created on core: %d | every: %d ms\n", __FUNCTION__, xPortGetCoreID(), biggerTask_period);
    uint32_t biggerTask_lastResetTime = xTaskGetTickCount(); // Initialize smallTask_lastResetTime

    while (true) {
        // MyESP32::wdt_reset();
        // Check if 200 ms have passed since the last wake time
        if (xTaskGetTickCount() - biggerTask_lastResetTime >= biggerTask_period) {
            biggerTask_lastResetTime += biggerTask_period;
            // printf(".");
            // your big function here !!!
        }       
    }
}

// Small periodic task
void task_smallTask(void *pvParameters) {
    printf("%s created on core: %d | every: %d ms \n", __FUNCTION__, xPortGetCoreID(), smallTask_period);
    uint32_t smallTask_lastResetTime = xTaskGetTickCount(); // Initialize smallTask_lastResetTime

    while (true) {
        // MyESP32::wdt_reset();
        if (xTaskGetTickCount() - smallTask_lastResetTime >= smallTask_period) {
            smallTask_lastResetTime += smallTask_period; // Update smallTask_lastResetTime
            for (size_t i = 0; i < smallTask_iterations ; i++) {
                const char* test = "7864507604586ß3850297q3456403534ß85634q860345878455q34wertr092345";
                std::string result= string_toDotString(test,3);
            }
            putchar('.');
            // LOG_RAW(".");
        }       
    }
}


void setup() {
    Serial.begin(115200);
    printf("STARTING MAIN ......\n");
    printf("...\n");
    cpu_setClock();
    wdt_setTimeout(9000);

    // Create task_smallTask ----------
    xTaskCreatePinnedToCore(task_smallTask, "task_smallTask", 2048, NULL, 1, NULL, 0);

    // Create task_biggerTask ---------
    xTaskCreatePinnedToCore(task_biggerTask, "task_biggerTask", 4096, NULL, 1, NULL, 0);

    // Create task_performanceCounter -------------------
    xTaskCreatePinnedToCore(task_perf, "task_performance", 2048, NULL, 1, NULL, 0);

    vTaskPrioritySet(NULL, 9);
    while (true) {

      vTaskDelay(watchdog_period);
      esp_task_wdt_reset();
      printf("core: %d | RESET main\n", xPortGetCoreID());
    }
}
#else

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// For comparison, here's the superloop version (comment out the RTOS version when using this)

void setup() {
    Serial.begin(115200);  
    printf("STARTING MAIN ......\n");
    printf("...\n");
    cpu_setClock();
    wdt_setTimeout(9000);

    printf("watchdog_period=%lu\n", watchdog_period);
    printf("performance_period=%lu\n", performance_period);
    printf("biggerTask_period=%lu\n", biggerTask_period);
    printf("smallTask_period=%lu\n", smallTask_period);

    TickType_t performance_lastResetTime = xTaskGetTickCount();
    TickType_t smallTask_lastResetTime = xTaskGetTickCount();
    TickType_t biggerTask_lastResetTime = xTaskGetTickCount();
    TickType_t watchdog_lastResetTime = xTaskGetTickCount();
    TickType_t performanceCounter = 0;

    while (true) {
        //vTaskDelay(1);

        performanceCounter++;

        const TickType_t t = xTaskGetTickCount();

        // Watchdog reset
        if (t - watchdog_lastResetTime >= watchdog_period) {
            watchdog_lastResetTime += watchdog_period;
            esp_task_wdt_reset();
        }
        // Performance counter output
        if (t - performance_lastResetTime >= performance_period) {
            printf("core: %d | SUPERloop Performance score: %lu|\n", xPortGetCoreID(), performanceCounter);
            performanceCounter = 0;
            performance_lastResetTime += performance_period;
        }

         // bigger task
        if (t - biggerTask_period >= biggerTask_period) {
            // Your bigger task here
            biggerTask_lastResetTime += biggerTask_period;
        }

        // small task
        if (t - smallTask_lastResetTime >= smallTask_period) {
            for (size_t i = 0; i < smallTask_iterations ; i++) {
                const char* test = "7864507604586ß3850297q3456403534ß85634q860345878455q34wertr092345";
                std::string result= string_toDotString(test,3);
            }
            putchar('.');
            smallTask_lastResetTime += smallTask_period;
        }
    }
}
#endif