/* 两把互斥锁的 FreeRTOS 实现。静态分配（`xSemaphoreCreateMutexStatic`）——
   两把锁的数量是编译期定死的，没有理由去堆上要内存，也就没有「创建失败」这条路径。

   ⚠️ 用的是 mutex 不是 binary semaphore：mutex 有**优先级继承**。
   t_safety 是最高优先级且每 20 ms 就要总线，低优先级的 t_link 拿着 I²C 锁去读遥测时
   如果没有继承，t_safety 会被一个中优先级任务无限期挡在门外（经典优先级反转）。 */
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include "hal_lock.h"

static StaticSemaphore_t s_buf[HAL_LOCK_COUNT];
static SemaphoreHandle_t s_mtx[HAL_LOCK_COUNT];
static uint32_t          s_timeouts[HAL_LOCK_COUNT];

hal_err_t hal_lock_init(void)
{
    for (int i = 0; i < HAL_LOCK_COUNT; i++) {
        if (s_mtx[i] == NULL) {
            s_mtx[i] = xSemaphoreCreateMutexStatic(&s_buf[i]);
            s_timeouts[i] = 0;
        }
    }
    return HAL_OK;
}

bool hal_lock_held(hal_lock_id_t id)
{
    if (id < 0 || id >= HAL_LOCK_COUNT || s_mtx[id] == NULL) {
        return false;
    }
    return xSemaphoreGetMutexHolder(s_mtx[id]) == xTaskGetCurrentTaskHandle();
}

hal_err_t hal_lock_take(hal_lock_id_t id, uint32_t timeout_ms)
{
    if (id < 0 || id >= HAL_LOCK_COUNT || s_mtx[id] == NULL) {
        return HAL_ERR_STATE;
    }
    /* 已经持有 = 重复加锁；持有另一把 = 嵌套。两者都是写错了，见 hal_lock.h。 */
    for (int i = 0; i < HAL_LOCK_COUNT; i++) {
        if (hal_lock_held((hal_lock_id_t)i)) {
            return HAL_ERR_STATE;
        }
    }
    const TickType_t ticks =
        (timeout_ms == HAL_LOCK_FOREVER) ? portMAX_DELAY : pdMS_TO_TICKS(timeout_ms);
    if (xSemaphoreTake(s_mtx[id], ticks) != pdTRUE) {
        s_timeouts[id]++;
        return HAL_ERR_TIMEOUT;
    }
    return HAL_OK;
}

void hal_lock_give(hal_lock_id_t id)
{
    configASSERT(id >= 0 && id < HAL_LOCK_COUNT && s_mtx[id] != NULL);
    configASSERT(hal_lock_held(id)); /* 不是自己拿的就别放 */
    (void)xSemaphoreGive(s_mtx[id]);
}

uint32_t hal_lock_timeouts(hal_lock_id_t id)
{
    return (id >= 0 && id < HAL_LOCK_COUNT) ? s_timeouts[id] : 0;
}

/* ---- 临界区（不是第三把锁，见 hal_lock.h） -------------------------------
   `portMUX_TYPE` + `portENTER_CRITICAL()`：单核上是关中断，多核上再加一个
   自旋锁。ESP32-S3 是双核，两个核上的任务同时入队是真会发生的，所以这里
   必须是 portMUX 而不是 `portDISABLE_INTERRUPTS()`。

   ⚠️ 关中断的时间要短到能一眼数完 —— evlog 的入队里只有一次 memcpy 和两个
   下标自增。往里面加任何一行「顺手」的活（取时间戳、算校验、写 flash）
   之前，先想清楚它会不会把 20 ms 的巡检节拍抖出去。 */
static portMUX_TYPE s_crit = portMUX_INITIALIZER_UNLOCKED;

void hal_crit_enter(void) { portENTER_CRITICAL(&s_crit); }
void hal_crit_exit(void) { portEXIT_CRITICAL(&s_crit); }