#include <stdio.h>
#include <stdatomic.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "esp_log.h"

// 调试宏
#define SHOW_TIME_TOKEN 1 // 耗时测试开关
#ifdef SHOW_TIME_TOKEN
#define RECURSIVE_NUM 100 // 递归进入和退出临界区次数
#endif

#define COUT_TO_NS(x) ((x * 1000) / 160)

static struct
{
    atomic_int suspend_count;
    TaskHandle_t task_handle;
} cores_suspend_handle[portNUM_PROCESSORS] = {0};

void cores_suspend(void *arg)
{
    while (1)
    {
        while (atomic_load(&cores_suspend_handle[xPortGetCoreID()].suspend_count) > 0)
        {
            __asm__ __volatile__("nop");
        }
        vTaskSuspend(NULL);
    }
}

void enter_critical(void)
{
    if (atomic_fetch_add(&cores_suspend_handle[xPortGetCoreID()].suspend_count, 1) == 0)
    {
        // 唤醒其他核的高优先级任务
        for (int i = 0; i < portNUM_PROCESSORS; i++)
        {
            if (i == xPortGetCoreID())
            {
                continue;
            }
            vTaskResume(cores_suspend_handle[i].task_handle);
        }
        vTaskSuspendAll();
    }
}

void exit_critical(void)
{
    if (atomic_fetch_sub(&cores_suspend_handle[xPortGetCoreID()].suspend_count, 1) == 1)
    {
        xTaskResumeAll();
    }
}

/* 竞争测试，验证互斥 */
static int shared_data = 0;

void taskA(void *pvParameters)
{
    int cnt = 0;
    while (1)
    {
        enter_critical();

        int before = shared_data;
        shared_data++;

        exit_critical();
        if (shared_data != before + 1)
        {
            ESP_LOGE("TaskA", "发生竞争");
        }
        else
        {
            if (++cnt % 100 == 0)
            {
                ESP_LOGI("TaskA", "未发生竞争");
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

void taskB(void *pvParameters)
{
    int cnt = 0;
    while (1)
    {
        enter_critical();
        enter_critical();
        // 临界区开始

        int before = shared_data;
        shared_data++;
        // 临界区结束

        exit_critical();
        exit_critical();

        if (shared_data != before + 1)
        {
            ESP_LOGE("TaskB", "发生竞争");
        }
        else
        {
            if (++cnt % 100 == 0)
            {
                ESP_LOGI("TaskB", "未发生竞争");
            }
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

int app_main(void)
{
    // 初始化
    for (int i = 0; i < portNUM_PROCESSORS; i++)
    {
        xTaskCreatePinnedToCore(cores_suspend, "cores_suspend", configMINIMAL_STACK_SIZE, NULL, configMAX_PRIORITIES - 1, &cores_suspend_handle[i].task_handle, i);
    }

    // 竞争测试
    xTaskCreatePinnedToCore(taskA, "TaskA", 4096, NULL, 5, NULL, 0);
    xTaskCreatePinnedToCore(taskB, "TaskB", 4096, NULL, 6, NULL, 1);

#ifdef SHOW_TIME_TOKEN
    // 单次耗时测试
    uint32_t start_time = esp_cpu_get_cycle_count();
    enter_critical();
    int32_t enter_time = (esp_cpu_get_cycle_count() - start_time);
    start_time = esp_cpu_get_cycle_count();
    exit_critical();
    int32_t exit_time = (esp_cpu_get_cycle_count() - start_time);

    ESP_LOGI("单次耗时", "进入时间：%ld ns, 退出时间：%ld ns", COUT_TO_NS(enter_time), COUT_TO_NS(exit_time));

    printf("\r\n");

    // 递归测试
    start_time = esp_cpu_get_cycle_count();
    for (int i = 0; i < RECURSIVE_NUM; i++)
    {
        enter_critical();
    }
    enter_time = (esp_cpu_get_cycle_count() - start_time);
    start_time = esp_cpu_get_cycle_count();
    for (int i = 0; i < RECURSIVE_NUM; i++)
    {
        exit_critical();
    }
    exit_time = (esp_cpu_get_cycle_count() - start_time);

    ESP_LOGI("递归耗时", "递归次数：%d, 进入总时间:%ld ns, 退出总时间: %ld ns", RECURSIVE_NUM, COUT_TO_NS(enter_time), COUT_TO_NS(exit_time));
#endif
    return 0;
}