/*
 * power_ctrl.c
 *
 *  Created on: 2025Äê10ÔÂ17ÈÕ
 *      Author: LHYe200
 */


#include "power_ctrl.h"
#include "zf_driver_pit.h"
#include "power_out.h"
#include "pid.h"
#include "vofa_client.h"
#include "status_led.h"
#include "math.h"
#include "ina226.h"

// PID DC_OUT1_PID;
// PID DC_AMP1_PID;

uint32 timer_counter_1ms = 0;

// low_pass_filter_t vot_set_filter;

// float vot_set_filtered = 0.0f;

// moving_average_filter_t mov_avg_filter;
// Kalman_filter_t kalman_filter;
// Kalman_filter_t unscented_kalman_filter;
// Kalman_filter_t adaptive_kalman_filter;

// float kalman_q = 0.0001f;
// float kalman_r = 0.10f;

void Power_Ctrl_Init(void)
{
    Power_Out_Init();
    VOFA_Client_Init();
    INA226_Init();
    // PID_Init(&DC_OUT1_PID, 3.25, 0.00909, 0.0, 500.0, 36000.0, 24000.0);
    // PID_Init(&DC_OUT1_PID, 3.25, 0.00909, 0.0, 500.0, 50000.0, -50000.0);
    // PID_Init(&DC_AMP1_PID, 0.0, 0.0, 0.0, 10.0, 50000.0, -50000.0);
    // Init_lowPass_alpha(&vot_set_filter, 0.2f / 1000.0f, 10.0f);

    // Moving_Average_filter_init(&mov_avg_filter, 10);
    // Kalman_filter_init(&kalman_filter, kalman_q, kalman_r);
    // Unscented_Kalman_filter_init(&unscented_kalman_filter, kalman_q, kalman_r);
    // Adaptive_Kalman_filter_init(&adaptive_kalman_filter, kalman_q, kalman_r);
    
    pit_us_init(CCU60_CH0, 1000);
    
}

void Power_Ctrl_Enable_Output(uint8 out_index, uint8 enable)
{
    if(out_index < POWER_OUT_NUM)
    {
        Power_Out_Enable(&power_outs[out_index], enable);
    }
}

void Power_Ctrl_Loop_1ms(void)
{
    
    timer_counter_1ms++;
    // power_outs[4].set_voltage_V = Low_pass_filter(&vot_set_filter, vot_set_filtered);
    
    if(timer_counter_1ms % POWER_OUT_READ_TIME_MS == 0)
    {
        Power_Out_Read();

        // Moving_Average_filter(&mov_avg_filter, power_outs[4].config.v_adc);
        // Kalman_filter(&kalman_filter, mov_avg_filter.avg);
        // Unscented_Kalman_filter(&unscented_kalman_filter, power_outs[4].config.v_adc);
        // Adaptive_Kalman_filter(&adaptive_kalman_filter, power_outs[4].config.v_adc);
    }

//    if(power_outs[4].enabled)
//    {
//        // float err = power_outs[4].set_voltage_V - power_outs[4].status.voltage_V;
//        // float set_vot = (power_outs[4].set_voltage_V - power_outs[4].config.votK[1]) / power_outs[4].config.votK[0];
//        // float err = set_vot - power_outs[4].config.vot_adc_filter.lastYn;
//        // if(err < 0.0f)
//        // {
//            // err *= 5.0f;
//        // }
//        // PID_Loc_Ctrl(&DC_OUT1_PID, err);
//        float err = (power_outs[4].set_voltage_V - power_outs[4].config.votK[1]) / power_outs[4].config.votK[0] - kalman_filter.prevData;
//        if(fabsf(err) < 10.0f)
//        {
//            err = 0.0f;
//        }
//        else if(fabsf(err) < 282.4f)
//        {
//            err = err/fabsf(err)*(expf(fabsf(err)*0.02f)-1.0f);
//        }
//        PID_Loc_Ctrl(&DC_OUT1_PID, err);
//        // PID_Inc_Ctrl(&DC_OUT1_PID,power_outs[4].set_voltage_V - power_outs[4].status.voltage_V);
//        // if(power_outs[4].status.voltage_V > power_outs[4].set_voltage_V * 1.1f)
//        // {
//            // Power_Out_Set_PWM(&power_outs[4], 10000);
//        // }
//        // else if(power_outs[4].status.voltage_V < power_outs[4].set_voltage_V * 0.9f)
//        // {
//            // Power_Out_Set_PWM(&power_outs[4], 50000);
//        // }
//        // else
//        // {
//            // Power_Out_Set_PWM(&power_outs[4], 50000 + DC_OUT1_PID.output);
//        // }
//
//        Power_Out_Set_PWM(&power_outs[4], 50000 + DC_OUT1_PID.output);
//
//        // Power_Out_Set_PWM(&power_outs[4], 10000);
//        Flash_LED(STATUS_LED_7);
//    }
    
    if(timer_counter_1ms % INA226_READ_TIME_MS == 0)
    {
        INA226_Read();
        
        
    }
    // if(timer_counter_1ms % 3 == 0)
    // {
    //     PID_Loc_Ctrl(&DC_OUT1_PID,power_outs[4].set_voltage_V - power_outs[4].status.voltage_V);
    // }
    
    timer_counter_1ms %= (1 * 500);
}



void Power_Status_Upload(void)
{
    VOFA_Set_JustFloat_Data(0, power_outs[0].status.voltage_V);
    VOFA_Set_JustFloat_Data(1, power_outs[0].status.current_A);
    VOFA_Set_JustFloat_Data(2, power_outs[1].status.voltage_V);
    VOFA_Set_JustFloat_Data(3, power_outs[1].status.current_A);
    VOFA_Set_JustFloat_Data(4, power_outs[2].status.voltage_V);
    VOFA_Set_JustFloat_Data(5, power_outs[2].status.current_A);
    VOFA_Set_JustFloat_Data(6, power_outs[3].status.voltage_V);
    VOFA_Set_JustFloat_Data(7, power_outs[3].status.current_A);
    VOFA_Set_JustFloat_Data(8, power_outs[4].status.voltage_V);
    VOFA_Set_JustFloat_Data(9, power_outs[4].status.current_A);
    VOFA_Set_JustFloat_Data(10, power_outs[5].status.voltage_V);
    VOFA_Set_JustFloat_Data(11, power_outs[5].status.current_A);
    VOFA_Set_JustFloat_Data(12, power_outs[6].status.voltage_V);
    VOFA_Set_JustFloat_Data(13, power_outs[6].status.current_A);
    VOFA_Set_JustFloat_Data(14, power_outs[7].status.voltage_V);
    VOFA_Set_JustFloat_Data(15, power_outs[7].status.current_A);
    VOFA_Set_JustFloat_Data(16,ina226[0].result.voltage_V);
    VOFA_Set_JustFloat_Data(17,ina226[0].result.current_A);
    VOFA_Set_JustFloat_Data(18,ina226[1].result.voltage_V);
    VOFA_Set_JustFloat_Data(19,ina226[1].result.current_A);

    VOFA_Set_JustFloat_Data(22,vofa_last_data);
    VOFA_Send_Datas(27);
}



// void Power_Status_Upload(void)
// {
//     VOFA_Set_JustFloat_Data(0, power_outs[0].status.voltage_V);
//     VOFA_Set_JustFloat_Data(1, power_outs[0].status.current_A);
//     VOFA_Set_JustFloat_Data(2, power_outs[1].status.voltage_V);
//     VOFA_Set_JustFloat_Data(3, power_outs[1].status.current_A);
//     VOFA_Set_JustFloat_Data(4, power_outs[2].status.voltage_V);
//     VOFA_Set_JustFloat_Data(5, power_outs[2].status.current_A);
//     VOFA_Set_JustFloat_Data(6, power_outs[3].status.voltage_V);
//     VOFA_Set_JustFloat_Data(7, power_outs[3].status.current_A);
//     VOFA_Set_JustFloat_Data(8, power_outs[4].status.voltage_V);
//     VOFA_Set_JustFloat_Data(9, power_outs[4].status.current_A);
//     VOFA_Set_JustFloat_Data(10, power_outs[5].status.voltage_V);
//     VOFA_Set_JustFloat_Data(11, power_outs[5].status.current_A);
//     VOFA_Set_JustFloat_Data(12, power_outs[6].status.voltage_V);
//     VOFA_Set_JustFloat_Data(13, power_outs[6].status.current_A);
//     VOFA_Set_JustFloat_Data(14, power_outs[7].status.voltage_V);
//     VOFA_Set_JustFloat_Data(15, power_outs[7].status.current_A);
//     VOFA_Set_JustFloat_Data(16,vofa_last_data);
//     VOFA_Set_JustFloat_Data(17,DC_OUT1_PID.p_output);
//     VOFA_Set_JustFloat_Data(18,DC_OUT1_PID.i_output);
//     VOFA_Set_JustFloat_Data(19,DC_OUT1_PID.d_output);
//     VOFA_Set_JustFloat_Data(20,power_outs[4].config.vot_adc_filter.lastYn);
//     VOFA_Set_JustFloat_Data(21,DC_OUT1_PID.output);
//     VOFA_Set_JustFloat_Data(22,mov_avg_filter.avg);
//     VOFA_Set_JustFloat_Data(23,kalman_filter.prevData);
//     VOFA_Set_JustFloat_Data(24,unscented_kalman_filter.prevData);
//     VOFA_Set_JustFloat_Data(25,adaptive_kalman_filter.prevData);
//     VOFA_Set_JustFloat_Data(26,power_outs[4].config.v_adc);
//     VOFA_Send_Datas(27);
// }

void Power_Vofa_Set(void)
{
    // if(vofa_receive_new_data[1])
    // {
    //     vofa_receive_new_data[1] = 0;
    //     DC_OUT1_PID.kp = vofa_data[1];
    // }
    // if(vofa_receive_new_data[2])
    // {
    //     vofa_receive_new_data[2] = 0;
    //     DC_OUT1_PID.ki = vofa_data[2];
    // }
    // if(vofa_receive_new_data[3])
    // {
    //     vofa_receive_new_data[3] = 0;
    //     DC_OUT1_PID.kd = vofa_data[3];
    // }
    // if(vofa_receive_new_data[4])
    // {
    //     vofa_receive_new_data[4] = 0;
    //     power_outs[4].set_voltage_V = vofa_data[4];
    //     // vot_set_filtered = vofa_data[4];
    // }
    // if(vofa_receive_new_data[5])
    // {
    //     vofa_receive_new_data[5] = 0;
    //     Power_Out_Set_PWM(&power_outs[4], vofa_data[5]);
    // }
}
