#include "libsfra_ti_hal.h"
#include <string.h>

// 静态适配器实例
static SFRA_F32 g_sfra_adapter;
static sfra_t *g_libsfra = NULL;

// 标志，用于检测 start 边沿
static int16_t g_last_start = 0;

// 需要同步的参数（上位机通过命令修改）
static float32_t g_last_amplitude;

// 内部函数：同步 libsfra 参数
static void sync_parameters(void) {

    if (g_sfra_adapter.amplitude != g_last_amplitude) {
        g_last_amplitude = g_sfra_adapter.amplitude;
    }

    if(g_sfra_adapter.freqStart != g_libsfra->config.freqStart){
        g_libsfra->config.freqStart = g_sfra_adapter.freqStart;
    }
    
    if(g_sfra_adapter.freqStep != g_libsfra->config.freqStep){
        g_libsfra->config.freqStep = g_sfra_adapter.freqStep;
    }

}

// 检查 start 标志并启动扫频
static void check_start_flag(void) {
    
    if (g_sfra_adapter.start == 1 && g_last_start == 0) {
        sfra_start(g_libsfra);
        g_sfra_adapter.start = 0;   // 清除，避免重复触发
    }
    g_last_start = g_sfra_adapter.start;
}

// 更新状态和频率索引
static void update_status(void) {
    
    g_sfra_adapter.freqIndex = g_libsfra->internal_state.freqIndex;
    
    if (sfra_is_running(g_libsfra)) {
        g_sfra_adapter.status = 1;   // 运行中
    } else if (sfra_is_done(g_libsfra)) {
        g_sfra_adapter.status = 2;   // 完成
        sfra_clear_done(g_libsfra);
    } else {
        g_sfra_adapter.status = 0;   // 空闲
    }
}



// 注入函数，返回 ref + 扰动
float SFRA_F32_inject(float ref) {
    
    float perturb = sfra_inject(g_libsfra);   // 返回 -1..1
    return ref + g_sfra_adapter.amplitude * perturb;
}

// 收集函数
void SFRA_F32_collect(float *control_output, float *feedback) {

    sfra_monitor(g_libsfra, *control_output, *feedback);
}

// 后台任务
void SFRA_F32_runBackgroundTask(SFRA_F32 *obj) {
    (void)obj;  // 不使用参数，直接用全局
    sync_parameters();
    check_start_flag();
    sfra_background_task(g_libsfra);
    update_status();
}

// ------------------------------------------------------------
// 初始化适配器
// ------------------------------------------------------------
void libsfra_ti_hal_init(sfra_t *libsfra,
                        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
                        )
{
    g_libsfra = libsfra;
    g_libsfra->config.isrFreq = isrFrequency;
    g_libsfra->config.freqStart = fraSweepStartFreq;
    g_libsfra->config.freqStep = freqStep;
    g_libsfra->config.vecLength = noFreqPoints;

    g_libsfra->results.freqVect = freqVect;
    g_libsfra->results.magnitudeVect = h_magVect;
    g_libsfra->results.phaseVect = h_phaseVect;

    

    // 清空适配器结构体
    memset(&g_sfra_adapter, 0, sizeof(SFRA_F32));

    // 配置指针
    g_sfra_adapter.freqVect    = freqVect;
    g_sfra_adapter.h_magVect   = h_magVect;
    g_sfra_adapter.h_phaseVect = h_phaseVect;
    // H(s) 和 G(s)H(s) 的数据存储在同一个数组中
    g_sfra_adapter.gh_magVect  = h_magVect;
    g_sfra_adapter.gh_phaseVect= h_phaseVect;
    g_sfra_adapter.cl_magVect  = cl_magVect;
    g_sfra_adapter.cl_phaseVect= cl_phaseVect;

    g_sfra_adapter.vecLength   = noFreqPoints;
    g_sfra_adapter.isrFreq     = isrFrequency;
    g_sfra_adapter.amplitude   = injectionAmplitude;
    g_sfra_adapter.freqStart   = fraSweepStartFreq;
    g_sfra_adapter.freqStep    = freqStep;
    g_sfra_adapter.speed       = 1;   // 默认速度
    g_sfra_adapter.storeH      = 1;   // 存储 plant 数据
    g_sfra_adapter.storeGH     = 0;
    g_sfra_adapter.storeCL     = 0;

    g_sfra_adapter.start   = 0;
    g_sfra_adapter.state   = 0;
    g_sfra_adapter.status  = 0;
    g_sfra_adapter.freqIndex = 0;

    // 同步内部影子变量
    g_last_amplitude = injectionAmplitude;
    g_last_start = 0;
}

SFRA_F32* libsfra_ti_hal_get_adapter(void) {
    return &g_sfra_adapter;
}