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



// 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];


// 通信串口，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



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


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

    LowPassFilter_Init(&lowPass_test, FS, FC);


    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);

}


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

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

}

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

    plant_input = SFRA_F32_inject(input_dc);

    
    // 直通，用于测试SFRA，plant扫描结果应为0°，0db
    // plant_output = plant_input; 

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

    SFRA_F32_collect(&plant_input, &plant_output);

    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}
