#include "motor_timer.h"

#include <math.h>
#include "driver/gptimer.h"
#include "esp_attr.h"
#include "esp_check.h"
#include "esp_log.h"

static const char *TAG_FOC = "foc_timer";                       //打印标签
static gptimer_handle_t s_foc_timer = NULL;                     //定时器timer句柄
static foc_timer_isr_cb_t s_foc_timer_user_cb = NULL;           //用户回调函数--FOC计算
static void *s_foc_timer_user_data = NULL;                      //用户回调传入一个私有参数
static uint32_t s_foc_timer_freq_hz = FOC_TIMER_DEFAULT_HZ;     //定时器频率  3K hz
static bool s_foc_timer_started = false;                        //记录定时器启动标记

static bool IRAM_ATTR foc_timer_on_alarm_cb(gptimer_handle_t timer,
                                            const gptimer_alarm_event_data_t *edata,
                                            void *user_data)
{
    (void)timer;
    (void)edata;
    (void)user_data;

    if (s_foc_timer_user_cb) {
        return s_foc_timer_user_cb(s_foc_timer_user_data);
    }
    return false;
}

static esp_err_t foc_timer_apply_alarm_locked(uint32_t freq_hz)
{
    if (freq_hz == 0 || s_foc_timer == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    uint64_t alarm_count = 1000000ULL / freq_hz;
    if (alarm_count == 0) {
        alarm_count = 1;
    }

    gptimer_alarm_config_t alarm_config = {
        .reload_count = 0,
        .alarm_count = alarm_count,
        .flags.auto_reload_on_alarm = true,
    };
    return gptimer_set_alarm_action(s_foc_timer, &alarm_config);
}

esp_err_t foc_timer_init(uint32_t freq_hz, foc_timer_isr_cb_t cb, void *user_data)
{
    if (freq_hz == 0) {
        freq_hz = FOC_TIMER_DEFAULT_HZ;
    }

    if (s_foc_timer != NULL) {
        s_foc_timer_user_cb = cb;
        s_foc_timer_user_data = user_data;
        s_foc_timer_freq_hz = freq_hz;
        return foc_timer_apply_alarm_locked(freq_hz);
    }

    gptimer_config_t timer_config = {
        .clk_src = GPTIMER_CLK_SRC_DEFAULT,
        .direction = GPTIMER_COUNT_UP,
        .resolution_hz = 1000000,
    };
    ESP_RETURN_ON_ERROR(gptimer_new_timer(&timer_config, &s_foc_timer), TAG_FOC, "create gptimer failed");

    gptimer_event_callbacks_t cbs = {
        .on_alarm = foc_timer_on_alarm_cb,
    };
    ESP_RETURN_ON_ERROR(gptimer_register_event_callbacks(s_foc_timer, &cbs, NULL), TAG_FOC, "register gptimer callback failed");

    s_foc_timer_user_cb = cb;
    s_foc_timer_user_data = user_data;
    s_foc_timer_freq_hz = freq_hz;
    s_foc_timer_started = false;

    ESP_RETURN_ON_ERROR(foc_timer_apply_alarm_locked(freq_hz), TAG_FOC, "set gptimer alarm failed");

    esp_err_t err = gptimer_enable(s_foc_timer);
    if ((err != ESP_OK) && (err != ESP_ERR_INVALID_STATE)) {
        return err;
    }

    ESP_LOGI(TAG_FOC, "FOC timer init done, freq=%lu Hz", (unsigned long)s_foc_timer_freq_hz);
    return ESP_OK;
}

esp_err_t foc_timer_set_frequency(uint32_t freq_hz)
{
    if (freq_hz == 0 || s_foc_timer == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    bool restart = s_foc_timer_started;
    if (restart) {
        ESP_RETURN_ON_ERROR(gptimer_stop(s_foc_timer), TAG_FOC, "stop gptimer before retune failed");
        s_foc_timer_started = false;
    }

    ESP_RETURN_ON_ERROR(foc_timer_apply_alarm_locked(freq_hz), TAG_FOC, "retune gptimer alarm failed");
    s_foc_timer_freq_hz = freq_hz;

    if (restart) {
        ESP_RETURN_ON_ERROR(gptimer_start(s_foc_timer), TAG_FOC, "restart gptimer failed");
        s_foc_timer_started = true;
    }

    ESP_LOGI(TAG_FOC, "FOC timer frequency set to %lu Hz", (unsigned long)s_foc_timer_freq_hz);
    return ESP_OK;
}

esp_err_t foc_timer_start(void)
{
    if (s_foc_timer == NULL) {
        return ESP_ERR_INVALID_STATE;
    }
    if (s_foc_timer_started) {
        return ESP_OK;
    }

    ESP_RETURN_ON_ERROR(gptimer_set_raw_count(s_foc_timer, 0), TAG_FOC, "reset gptimer count failed");

    esp_err_t err = gptimer_start(s_foc_timer);
    if ((err != ESP_OK) && (err != ESP_ERR_INVALID_STATE)) {
        return err;
    }

    s_foc_timer_started = true;
    ESP_LOGI(TAG_FOC, "FOC timer started");
    return ESP_OK;
}

esp_err_t foc_timer_stop(void)
{
    if (s_foc_timer == NULL) {
        return ESP_ERR_INVALID_STATE;
    }
    if (!s_foc_timer_started) {
        return ESP_OK;
    }

    ESP_RETURN_ON_ERROR(gptimer_stop(s_foc_timer), TAG_FOC, "stop gptimer failed");
    s_foc_timer_started = false;
    ESP_LOGI(TAG_FOC, "FOC timer stopped");
    return ESP_OK;
}

uint32_t foc_timer_get_frequency(void)
{
    return s_foc_timer_freq_hz;
}