/**
 * @file    sfra_f32.c
 * @brief   软件频率响应分析器 (SFRA) 库实现，完全兼容 TI SFRA_F32 接口。
 * @details 实现功率级传函 H(s)、系统开环传函 GH(s) 和闭环传函 CL(s) 的测量。
 *          支持浮点运算，自动检测上位机参数变化并更新频率向量。
 */

#include "sfra_f32.h"
#include <math.h>

/*============================================================================
 * 常量定义
 *============================================================================*/
#define TWO_PI          (2.0f * 3.14159265358979323846f)
#define RAD_TO_DEG      (180.0f / 3.14159265358979323846f)
#define INVALID_MAG_DB  (-200.0f)

/*============================================================================
 * 内部静态数据（单实例，非重入）
 *============================================================================*/
typedef struct {
    uint16_t active;                /**< 扫频是否激活 */
    uint16_t dtft_running;          /**< 当前频率点是否正在累积 DTFT */
    uint16_t freqIndex;             /**< 当前频率点索引 */
    uint16_t dataIndex;             /**< 当前频率点的采样计数器 */
    uint16_t dataCount;             /**< 当前频率点总采样数 */

    float32_t foi_rad;              /**< 每个采样的角度增量 (rad) */
    float32_t foi_sin;              /**< 当前采样点 sin 值 */
    float32_t foi_cos;              /**< 当前采样点 cos 值 */

    /* DTFT 累加器 (使用 e^{-jwt} 核) */
    float32_t dtft_real_inj;        /**< 注入信号 I 的实部 */
    float32_t dtft_imag_inj;        /**< 注入信号 I 的虚部 */
    float32_t dtft_real_fb;         /**< 反馈信号 Y 的实部 */
    float32_t dtft_imag_fb;         /**< 反馈信号 Y 的虚部 */
    float32_t dtft_real_ctrl;       /**< 控制输出 U 的实部 */
    float32_t dtft_imag_ctrl;       /**< 控制输出 U 的虚部 */

    /* 影子变量：用于检测上位机参数变更 */
    float32_t shadow_amplitude;     /**< 上次同步的注入幅度 */
    float32_t shadow_freqStart;     /**< 上次同步的起始频率 */
    float32_t shadow_freqStep;      /**< 上次同步的频率步进乘数 */
    int16_t   shadow_speed;         /**< 上次同步的扫频速度 */

    int16_t storeH;                 /**< 是否存储 H 向量 */
    int16_t storeGH;                /**< 是否存储 GH 向量 */
    int16_t storeCL;                /**< 是否存储 CL 向量 */
} SFRA_Internal;

static SFRA_Internal s_sfra = {0};

/*============================================================================
 * 辅助函数
 *============================================================================*/

/**
 * @brief   计算指定频率点需要累积的采样点数（基于 libsfra 的周期对齐策略）
 * @param   foi_hz      注入频率 (Hz)
 * @param   isrFreq     控制 ISR 频率 (Hz)
 * @param   speed       速度因子（越大扫频越慢，累积周期越多）
 * @return  采样点数
 */
static uint16_t calcDataCount(float32_t foi_hz, float32_t isrFreq, int16_t speed)
{
    uint16_t cycles;
    if (foi_hz < 10.0f)
        cycles = 10;
    else if (foi_hz < 100.0f)
        cycles = (uint16_t)ceilf(foi_hz);
    else
        cycles = 100;

    cycles = (uint16_t)((float32_t)cycles * (float32_t)speed);
    if (cycles < 4) cycles = 4;

    float32_t samples_per_cycle = isrFreq / foi_hz;
    return (uint16_t)ceilf(samples_per_cycle * cycles);
}

/**
 * @brief   复数除法：计算 (num_re + j*num_im) / (den_re + j*den_im)
 * @param   re_num     分子实部
 * @param   im_num     分子虚部
 * @param   re_den     分母实部
 * @param   im_den     分母虚部
 * @param   mag        输出幅值 (dB)
 * @param   phase_deg  输出相位 (度)
 */
static void complexDiv(float32_t re_num, float32_t im_num,
                       float32_t re_den, float32_t im_den,
                       float32_t *mag, float32_t *phase_deg)
{
    float32_t den_sq = re_den * re_den + im_den * im_den;
    if (den_sq < 1e-12f) {
        *mag = INVALID_MAG_DB;
        *phase_deg = 0.0f;
        return;
    }
    float32_t re = (re_num * re_den + im_num * im_den) / den_sq;
    float32_t im = (im_num * re_den - re_num * im_den) / den_sq;
    *mag = 10.0f * log10f(re * re + im * im);
    *phase_deg = atan2f(im, re) * RAD_TO_DEG;
}

/**
 * @brief   从指定索引开始重新生成频率向量（使用当前的 freqStart 和 freqStep）
 * @param   obj         SFRA 对象指针
 * @param   startIdx    起始索引（该点之前的频率点保持不变）
 */
static void regenerateFreqVectorFrom(SFRA_F32 *obj, uint16_t startIdx)
{
    if (!obj || !obj->freqVect || obj->vecLength <= 0) return;
    if (startIdx >= (uint16_t)obj->vecLength) return;

    for (int16_t i = startIdx; i < obj->vecLength; i++) {
        if (i == 0) {
            obj->freqVect[0] = obj->freqStart;
        } else {
            obj->freqVect[i] = obj->freqVect[i-1] * obj->freqStep;
        }
    }
}

/*============================================================================
 * 公开 API 实现
 *============================================================================*/

void SFRA_F32_reset(SFRA_F32 *obj)
{
    if (!obj) return;
    obj->state = 0;
    obj->status = 0;
    obj->freqIndex = 0;
    obj->start = 0;

    s_sfra.active = 0;
    s_sfra.dtft_running = 0;
    s_sfra.freqIndex = 0;
    s_sfra.dataIndex = 0;
    s_sfra.dataCount = 0;
    s_sfra.foi_rad = 0;
    s_sfra.foi_sin = 0;
    s_sfra.foi_cos = 0;
    s_sfra.dtft_real_inj = 0;
    s_sfra.dtft_imag_inj = 0;
    s_sfra.dtft_real_fb = 0;
    s_sfra.dtft_imag_fb = 0;
    s_sfra.dtft_real_ctrl = 0;
    s_sfra.dtft_imag_ctrl = 0;

    s_sfra.shadow_amplitude = obj->amplitude;
    s_sfra.shadow_freqStart = obj->freqStart;
    s_sfra.shadow_freqStep  = obj->freqStep;
    s_sfra.shadow_speed     = obj->speed;
}

void SFRA_F32_config(SFRA_F32 *obj,
                     float32_t isrFrequency,
                     float32_t injectionAmplitude,
                     int16_t noFreqPoints,
                     float32_t fraSweepStartFreq,
                     float32_t freqStep,
                     float32_t *h_magVect,
                     float32_t *h_phaseVect,
                     float32_t *gh_magVect,
                     float32_t *gh_phaseVect,
                     float32_t *cl_magVect,
                     float32_t *cl_phaseVect,
                     float32_t *freqVect,
                     int16_t speed)
{
    if (!obj) return;
    obj->isrFreq = isrFrequency;
    obj->amplitude = injectionAmplitude;
    obj->vecLength = noFreqPoints;
    obj->freqStart = fraSweepStartFreq;
    obj->freqStep = freqStep;
    obj->speed = speed;

    obj->h_magVect = h_magVect;
    obj->h_phaseVect = h_phaseVect;
    obj->gh_magVect = gh_magVect;
    obj->gh_phaseVect = gh_phaseVect;
    obj->cl_magVect = cl_magVect;
    obj->cl_phaseVect = cl_phaseVect;
    obj->freqVect = freqVect;

    obj->storeH   = (h_magVect && h_phaseVect) ? 1 : 0;
    obj->storeGH  = (gh_magVect && gh_phaseVect) ? 1 : 0;
    obj->storeCL  = (cl_magVect && cl_phaseVect) ? 1 : 0;

    s_sfra.storeH = obj->storeH;
    s_sfra.storeGH = obj->storeGH;
    s_sfra.storeCL = obj->storeCL;

    s_sfra.shadow_amplitude = injectionAmplitude;
    s_sfra.shadow_freqStart = fraSweepStartFreq;
    s_sfra.shadow_freqStep  = freqStep;
    s_sfra.shadow_speed     = speed;

    SFRA_F32_reset(obj);
}

void SFRA_F32_initFreqArrayWithLogSteps(SFRA_F32 *obj,
                                        float32_t fra_sweep_start_freq,
                                        float32_t freqStep)
{
    if (!obj || !obj->freqVect || obj->vecLength <= 0) return;
    obj->freqVect[0] = fra_sweep_start_freq;
    for (int16_t i = 1; i < obj->vecLength; i++) {
        obj->freqVect[i] = obj->freqVect[i-1] * freqStep;
    }
    s_sfra.shadow_freqStart = fra_sweep_start_freq;
    s_sfra.shadow_freqStep  = freqStep;
}

void SFRA_F32_resetFreqRespArray(SFRA_F32 *obj)
{
    if (!obj) return;
    uint16_t len = obj->vecLength;
    if (obj->storeH && obj->h_magVect && obj->h_phaseVect) {
        for (uint16_t i = 0; i < len; i++) {
            obj->h_magVect[i] = 0.0f;
            obj->h_phaseVect[i] = 0.0f;
        }
    }
    if (obj->storeGH && obj->gh_magVect && obj->gh_phaseVect) {
        for (uint16_t i = 0; i < len; i++) {
            obj->gh_magVect[i] = 0.0f;
            obj->gh_phaseVect[i] = 0.0f;
        }
    }
    if (obj->storeCL && obj->cl_magVect && obj->cl_phaseVect) {
        for (uint16_t i = 0; i < len; i++) {
            obj->cl_magVect[i] = 0.0f;
            obj->cl_phaseVect[i] = 0.0f;
        }
    }
}

void SFRA_F32_updateInjectionAmplitude(SFRA_F32 *obj, float32_t new_injection_amplitude)
{
    if (obj) {
        obj->amplitude = new_injection_amplitude;
        s_sfra.shadow_amplitude = new_injection_amplitude;
    }
}

/**
 * @brief   注入函数：生成正弦扰动并叠加到参考值上。
 * @param   ref     原始参考值
 * @return  叠加扰动后的参考值
 */
float SFRA_F32_inject(float ref)
{
    if (!s_sfra.active || !s_sfra.dtft_running) return ref;

    float32_t angle = s_sfra.foi_rad * s_sfra.dataIndex;
    s_sfra.foi_cos = cosf(angle);
    s_sfra.foi_sin = sinf(angle);
    return ref + s_sfra.shadow_amplitude * s_sfra.foi_cos;
}

/**
 * @brief   收集函数：在控制 ISR 中调用，累积 DTFT 数据。
 * @param   control_output   控制输出指针（例如占空比）
 * @param   feedback         反馈信号指针（例如 ADC 读数）
 */
void SFRA_F32_collect(float *control_output, float *feedback)
{
    if (!s_sfra.active || !s_sfra.dtft_running) return;
    if (!control_output || !feedback) return;

    float32_t ctrl = *control_output;
    float32_t fb   = *feedback;
    float32_t cosv = s_sfra.foi_cos;
    float32_t sinv = s_sfra.foi_sin;

    /* DTFT 采用 e^{-jwt} 核，虚部为负号 */
    s_sfra.dtft_real_fb   += fb * cosv;
    s_sfra.dtft_imag_fb   -= fb * sinv;

    s_sfra.dtft_real_ctrl += ctrl * cosv;
    s_sfra.dtft_imag_ctrl -= ctrl * sinv;

    float32_t inj = s_sfra.shadow_amplitude * cosv;
    s_sfra.dtft_real_inj += inj * cosv;
    s_sfra.dtft_imag_inj -= inj * sinv;

    s_sfra.dataIndex++;
    if (s_sfra.dataIndex >= s_sfra.dataCount) {
        s_sfra.dtft_running = 0;   /* 当前频率点累积完成 */
    }
}

/**
 * @brief   后台任务：状态机，管理扫频流程，计算传函并存储结果。
 * @param   obj     SFRA 对象指针
 * @note    传函定义：
 *          - H(s)  = Y / U        (功率级传函，用于开环模式)
 *          - GH(s) = Y / (I - Y)  (系统开环传函，闭环模式下测量)
 *          - CL(s) = Y / I        (系统闭环传函)
 */
void SFRA_F32_runBackgroundTask(SFRA_F32 *obj)
{
    if (!obj) return;

    /*------------------------------------------------------------------------
     * 1. 扫频未激活时，检测参数变化并更新频率向量
     *------------------------------------------------------------------------*/
    if (!s_sfra.active) {
        if (obj->amplitude != s_sfra.shadow_amplitude) {
            s_sfra.shadow_amplitude = obj->amplitude;
        }
        if (obj->speed != s_sfra.shadow_speed) {
            s_sfra.shadow_speed = obj->speed;
        }
        if (obj->freqStart != s_sfra.shadow_freqStart ||
            obj->freqStep  != s_sfra.shadow_freqStep) {
            regenerateFreqVectorFrom(obj, 0);
            s_sfra.shadow_freqStart = obj->freqStart;
            s_sfra.shadow_freqStep  = obj->freqStep;
        }
    }

    /*------------------------------------------------------------------------
     * 2. 启动新扫频
     *------------------------------------------------------------------------*/
    if (!s_sfra.active && obj->start) {
        s_sfra.active = 1;
        s_sfra.freqIndex = 0;
        obj->start = 0;
        obj->state = 1;
        obj->status = 1;
        obj->freqIndex = 0;

        if (obj->freqVect && obj->vecLength > 0) {
            float32_t freq = obj->freqVect[0];
            s_sfra.dataCount = calcDataCount(freq, obj->isrFreq, obj->speed);

            uint16_t cycles_raw;
            if (freq < 10.0f) cycles_raw = 10;
            else if (freq < 100.0f) cycles_raw = (uint16_t)ceilf(freq);
            else cycles_raw = 100;
            cycles_raw = (uint16_t)((float32_t)cycles_raw * obj->speed);
            if (cycles_raw < 4) cycles_raw = 4;

            s_sfra.foi_rad = TWO_PI * (float32_t)cycles_raw / (float32_t)s_sfra.dataCount;

            s_sfra.dataIndex = 0;
            s_sfra.dtft_real_inj = 0;
            s_sfra.dtft_imag_inj = 0;
            s_sfra.dtft_real_fb = 0;
            s_sfra.dtft_imag_fb = 0;
            s_sfra.dtft_real_ctrl = 0;
            s_sfra.dtft_imag_ctrl = 0;
            s_sfra.dtft_running = 1;
        }
        return;
    }

    if (!s_sfra.active) {
        obj->state = 0;
        obj->status = 0;
        return;
    }

    if (s_sfra.dtft_running) return;

    /*------------------------------------------------------------------------
     * 3. 当前频率点 DTFT 已完成，计算并存储各类传函
     *------------------------------------------------------------------------*/
    uint16_t idx = s_sfra.freqIndex;
    if (idx < (uint16_t)obj->vecLength) {
        float32_t mag, phase;

        /* 功率级传函 H(s) = Y / U */
        if (s_sfra.storeH && obj->h_magVect && obj->h_phaseVect) {
            complexDiv(s_sfra.dtft_real_fb, s_sfra.dtft_imag_fb,
                       s_sfra.dtft_real_ctrl, s_sfra.dtft_imag_ctrl,
                       &mag, &phase);
            obj->h_magVect[idx] = mag;
            obj->h_phaseVect[idx] = phase;
        }

        /* 开环传函 GH(s) = Y / (I - Y) */
        if (s_sfra.storeGH && obj->gh_magVect && obj->gh_phaseVect) {
            float32_t re_iy = s_sfra.dtft_real_inj - s_sfra.dtft_real_fb;
            float32_t im_iy = s_sfra.dtft_imag_inj - s_sfra.dtft_imag_fb;
            complexDiv(s_sfra.dtft_real_fb, s_sfra.dtft_imag_fb,
                       re_iy, im_iy, &mag, &phase);
            obj->gh_magVect[idx] = mag;
            obj->gh_phaseVect[idx] = phase;
        }

        /* 闭环传函 CL(s) = Y / I */
        if (s_sfra.storeCL && obj->cl_magVect && obj->cl_phaseVect) {
            complexDiv(s_sfra.dtft_real_fb, s_sfra.dtft_imag_fb,
                       s_sfra.dtft_real_inj, s_sfra.dtft_imag_inj,
                       &mag, &phase);
            obj->cl_magVect[idx] = mag;
            obj->cl_phaseVect[idx] = phase;
        }
    }

    /*------------------------------------------------------------------------
     * 4. 准备下一个频率点（如果参数已变化，则重新生成剩余频率向量）
     *------------------------------------------------------------------------*/
    s_sfra.freqIndex++;
    obj->freqIndex = s_sfra.freqIndex;

    if (s_sfra.freqIndex < (uint16_t)obj->vecLength) {
        /* 检测频率参数变化，重新生成剩余频率点 */
        if (obj->freqStart != s_sfra.shadow_freqStart ||
            obj->freqStep  != s_sfra.shadow_freqStep) {
            regenerateFreqVectorFrom(obj, s_sfra.freqIndex);
            s_sfra.shadow_freqStart = obj->freqStart;
            s_sfra.shadow_freqStep  = obj->freqStep;
        }
        if (obj->speed != s_sfra.shadow_speed) {
            s_sfra.shadow_speed = obj->speed;
        }

        float32_t freq = obj->freqVect[s_sfra.freqIndex];
        s_sfra.dataCount = calcDataCount(freq, obj->isrFreq, obj->speed);

        uint16_t cycles_raw;
        if (freq < 10.0f) cycles_raw = 10;
        else if (freq < 100.0f) cycles_raw = (uint16_t)ceilf(freq);
        else cycles_raw = 100;
        cycles_raw = (uint16_t)((float32_t)cycles_raw * obj->speed);
        if (cycles_raw < 4) cycles_raw = 4;

        s_sfra.foi_rad = TWO_PI * (float32_t)cycles_raw / (float32_t)s_sfra.dataCount;

        s_sfra.dataIndex = 0;
        s_sfra.dtft_real_inj = 0;
        s_sfra.dtft_imag_inj = 0;
        s_sfra.dtft_real_fb = 0;
        s_sfra.dtft_imag_fb = 0;
        s_sfra.dtft_real_ctrl = 0;
        s_sfra.dtft_imag_ctrl = 0;
        s_sfra.dtft_running = 1;
        obj->state = 1;
        obj->status = 1;
    } else {
        /* 扫频结束 */
        s_sfra.active = 0;
        obj->state = 0;
        obj->status = 2;
        obj->freqIndex = obj->vecLength;
    }
}