/* 两把互斥锁的 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;
}