#include "sfra_f32.h"
#include "sfra_test.h"
#include "libsfra.h"
#include "lowpass.h"
#include "sfra_gui_scicomms_driverlib.h"
#include <stdio.h>
#include "libsfra_ti_hal.h"

#define TI_SFRA 0
#define LIBSFRA 1
#define LIBSFRA_HAL_TI 2

#define SFRA_TYPE LIBSFRA_HAL_TI

// sfra变量定义
SFRA_F32 ti_sfra;

#define CONTROL_ISR_FREQUENCY ((float32_t)100 * 1000)  // 100KHz

#define SFRA_ISR_FREQ   CONTROL_ISR_FREQUENCY
#define SFRA_FREQ_START 10
//
// SFRA step Multiply = 10^(1/No of steps per decade(40))
//
#define SFRA_FREQ_STEP_MULTIPLY (float32_t)1.105
#define SFRA_AMPLITUDE (float32_t)0.1
#define SFRA_FREQ_LENGTH 100

float32_t plantMagVect[SFRA_FREQ_LENGTH];
float32_t plantPhaseVect[SFRA_FREQ_LENGTH];
float32_t olMagVect[SFRA_FREQ_LENGTH];
float32_t olPhaseVect[SFRA_FREQ_LENGTH];
float32_t clMagVect[SFRA_FREQ_LENGTH];
float32_t clPhaseVect[SFRA_FREQ_LENGTH];
float32_t freqVect[SFRA_FREQ_LENGTH];
//
//extern to access tables in ROM
//
extern long FPUsinTable[];


// libsfra变量定义
// Size of each SFRA result buffer field
#define SFRA_BUF_SIZE 100

typedef struct {
    float freq[SFRA_BUF_SIZE];  // In Hz, log-spaced
    float mag[SFRA_BUF_SIZE];   // In dB
    float phase[SFRA_BUF_SIZE]; // In degrees
} sfra_results_t;

// Holds measured bode plot
sfra_results_t libsfra_results;

// Declare the SFRA instance
sfra_t libsfra = {
    .config.isrFreq = CONTROL_ISR_FREQUENCY,    // Sampling frequency
    .config.freqStart = 10,                     // Start frequency
    .config.freqStep = 1.584893192461113,       // Frequency step (log-spaced)
    .config.vecLength = SFRA_BUF_SIZE,          // Number of sweeps
    .results.freqVect = libsfra_results.freq,      // Points to the frequency vector
    .results.magnitudeVect = libsfra_results.mag,	// Points to the magnitude vector
    .results.phaseVect = libsfra_results.phase     // Points to the phase vector
};


// hal层变量
sfra_t hal_sfra;



// lowpass filter
LowPassFilter_t lowPass_test;
#define FS CONTROL_ISR_FREQUENCY
#define FC 10.0f * 1000


// 通信串口，LED
#define SFRA_GUI_SCI_BASE SCIA_BASE
#define SFRA_GUI_VBUS_CLK DEVICE_LSPCLK_FREQ
#define SFRA_GUI_SCI_BAUDRATE 115200
#define SFRA_GUI_SCIRX_GPIO 43
#define SFRA_GUI_SCITX_GPIO 42
#define SFRA_GUI_SCIRX_GPIO_PIN_CONFIG GPIO_43_SCIRXDA
#define SFRA_GUI_SCITX_GPIO_PIN_CONFIG GPIO_42_SCITXDA

#define SFRA_GUI_LED_INDICATOR 1
#define SFRA_GUI_LED_GPIO 31
#define SFRA_GUI_LED_GPIO_PIN_CONFIG GPIO_31_GPIO31


void sfra_init()
{
    CPUTimer_setPeriod(CPUTIMER0_BASE,
                    (DEVICE_SYSCLK_FREQ / CONTROL_ISR_FREQUENCY) - 1);
    CPUTimer_startTimer(CPUTIMER0_BASE);

    LowPassFilter_Init(&lowPass_test, FS, FC);


#if SFRA_TYPE == TI_SFRA

    SFRA_F32_reset(&ti_sfra);
    SFRA_F32_config(&ti_sfra,
                    SFRA_ISR_FREQ,
                    SFRA_AMPLITUDE,
                    SFRA_FREQ_LENGTH,
                    SFRA_FREQ_START,
                    SFRA_FREQ_STEP_MULTIPLY,
                    plantMagVect,
                    plantPhaseVect,
                    olMagVect,
                    olPhaseVect,
                    clMagVect,
                    clPhaseVect,
                    freqVect,
                    1);
    SFRA_F32_resetFreqRespArray(&ti_sfra);
    SFRA_F32_initFreqArrayWithLogSteps(&ti_sfra,
                                        SFRA_FREQ_START,
                                        SFRA_FREQ_STEP_MULTIPLY);
    SFRA_GUI_config(SFRA_GUI_SCI_BASE,
                    SFRA_GUI_VBUS_CLK,
                    SFRA_GUI_SCI_BAUDRATE,
                    SFRA_GUI_SCIRX_GPIO,
                    SFRA_GUI_SCIRX_GPIO_PIN_CONFIG,
                    SFRA_GUI_SCITX_GPIO,
                    SFRA_GUI_SCITX_GPIO_PIN_CONFIG,
                    SFRA_GUI_LED_INDICATOR,
                    SFRA_GUI_LED_GPIO,
                    SFRA_GUI_LED_GPIO_PIN_CONFIG,
                    &ti_sfra,
                    SFRA_GUI_PLOT_GH_CL);

#elif SFRA_TYPE == LIBSFRA

    // libsfra
    sfra_init_all();

    sfra_start(&libsfra);

#elif SFRA_TYPE == LIBSFRA_HAL_TI

    // 初始化 HAL 适配层
    libsfra_ti_hal_init(&hal_sfra,
                        SFRA_ISR_FREQ,
                        SFRA_AMPLITUDE,
                        SFRA_FREQ_LENGTH,
                        SFRA_FREQ_START,
                        SFRA_FREQ_STEP_MULTIPLY,
                        plantMagVect,
                        plantPhaseVect,
                        olMagVect,
                        olPhaseVect,
                        clMagVect,
                        clPhaseVect,
                        freqVect);
    SFRA_GUI_config(SFRA_GUI_SCI_BASE,
                    SFRA_GUI_VBUS_CLK,
                    SFRA_GUI_SCI_BAUDRATE,
                    SFRA_GUI_SCIRX_GPIO,
                    SFRA_GUI_SCIRX_GPIO_PIN_CONFIG,
                    SFRA_GUI_SCITX_GPIO,
                    SFRA_GUI_SCITX_GPIO_PIN_CONFIG,
                    SFRA_GUI_LED_INDICATOR,
                    SFRA_GUI_LED_GPIO,
                    SFRA_GUI_LED_GPIO_PIN_CONFIG,
                    libsfra_ti_hal_get_adapter(),
                    SFRA_GUI_PLOT_GH_H);
                    
#endif
}


// ==================== 打印 LIBSFRA 结果（CSV 格式）====================
/**
 * 将 SFRA 测量结果通过串口输出为 CSV 格式
 * @param res       指向 sfra_results_t 结构体的指针
 * @param num_points 结果点数（即 SFRA_FREQ_LENGTH）
 */
void sfra_print_results_csv(sfra_results_t *res, int num_points)
{
    char lineBuf[64];
    int i;

    UARTprintf("Frequency(Hz),Magnitude(dB),Phase(deg)\n");

    for (i = 0; i < num_points; i++) {
        int pos = 0;
        // 频率
        pos += floatToStr5(res->freq[i], lineBuf + pos);
        lineBuf[pos++] = ',';
        // 增益 (dB)
        pos += floatToStr5(res->mag[i], lineBuf + pos);
        lineBuf[pos++] = ',';
        // 相位 (度)
        pos += floatToStr5(res->phase[i], lineBuf + pos);
        lineBuf[pos++] = '\n';
        lineBuf[pos] = '\0';
        UARTprintf(lineBuf);
    }
}


void sfra_task_run()
{
    DEVICE_DELAY_US(1.0f *1000);

#if SFRA_TYPE == TI_SFRA

    SFRA_F32_runBackgroundTask(&ti_sfra);
    SFRA_GUI_runSerialHostComms(&ti_sfra);

#elif SFRA_TYPE == LIBSFRA

    sfra_background_task(&libsfra);
    if(sfra_is_done(&libsfra))
    {
        sfra_print_results_csv(&libsfra_results, SFRA_BUF_SIZE);
        sfra_clear_done(&libsfra);
    }
    
#elif SFRA_TYPE == LIBSFRA_HAL_TI

    SFRA_F32_runBackgroundTask(libsfra_ti_hal_get_adapter());
    SFRA_GUI_runSerialHostComms(libsfra_ti_hal_get_adapter());

#endif

}

__interrupt void TIMER0_ISR()
{
    static float32_t input_dc = 0.8f;
    float32_t plant_input;
    float32_t plant_output;

#if (SFRA_TYPE == TI_SFRA) || (SFRA_TYPE == LIBSFRA_HAL_TI)
    plant_input = SFRA_F32_inject(input_dc);
#elif SFRA_TYPE == LIBSFRA
    plant_input = input_dc + 0.1f * sfra_inject(&libsfra);
#endif
    
    // 直通，用于测试SFRA，plant扫描结果应为0°，0db
    // plant_output = plant_input; 

    // 注入扫描lowpass
    plant_output = LowPassFilter_Run(&lowPass_test, plant_input);

#if (SFRA_TYPE == TI_SFRA) || (SFRA_TYPE == LIBSFRA_HAL_TI)
    SFRA_F32_collect(&plant_input, &plant_output);
#elif SFRA_TYPE == LIBSFRA
    sfra_monitor(&libsfra, plant_input, plant_output);
#endif

    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}
