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


#include "power_out.h"
#include "zf_driver_adc.h"
#include "zf_driver_pwm.h"
#include "zf_driver_gpio.h"


Power_Out_t power_outs[POWER_OUT_NUM];


void Power_Out_Single_Init(Power_Out_t *power_out, int16 vot_channel, int16 amp_channel, float votK[2], float ampK[2], int16 enable_pin, uint8 can_adjust, int16 vot_pwm_channel)
{
    if(enable_pin != POWER_OUT_FUNCTION_DISABLE)
    {
        gpio_init((gpio_pin_enum)enable_pin, GPO, 0, GPO_PUSH_PULL);
    }
    power_out->config.voltage_read_channel = vot_channel;
    power_out->config.current_read_channel = amp_channel;
    Init_lowPass_alpha(&power_out->config.vot_filter, POWER_OUT_READ_TIME_MS / 1000.0f, 10.0f);
    Init_lowPass_alpha(&power_out->config.amp_filter, POWER_OUT_READ_TIME_MS / 1000.0f, 10.0f);
    Init_lowPass_alpha(&power_out->config.vot_adc_filter, POWER_OUT_READ_TIME_MS / 1000.0f, 100.0f);
    power_out->config.votK[0] = votK[0];
    power_out->config.votK[1] = votK[1];
    power_out->config.ampK[0] = ampK[0];
    power_out->config.ampK[1] = ampK[1];
    power_out->config.enable_pin = enable_pin;
    power_out->config.can_adjust = can_adjust;
    power_out->config.vot_pwm_channel = vot_pwm_channel;

    power_out->status.voltage_V = 0.0f;
    power_out->status.current_A = 0.0f;
    power_out->status.power_W = 0.0f;
    power_out->status.energy_J = 0.0f;
    power_out->status.quantity_C = 0.0f;
    memset(power_out->status.past_voltage_V, 0, sizeof(power_out->status.past_voltage_V));
    memset(power_out->status.past_current_A, 0, sizeof(power_out->status.past_current_A));

    power_out->set_voltage_V = VOT_ADJUST_DEF;
    power_out->set_current_A = AMP_ADJUST_DEF;
    power_out->max_power_W = POW_ADJUST_DEF;
    power_out->enabled = 0;
    if(vot_channel != POWER_OUT_FUNCTION_DISABLE)
    {
        adc_init((adc_channel_enum)vot_channel, ADC_12BIT);
    }
    if(amp_channel != POWER_OUT_FUNCTION_DISABLE)
    {
        adc_init((adc_channel_enum)amp_channel, ADC_12BIT);
    }

    if(vot_pwm_channel != POWER_OUT_FUNCTION_DISABLE)
    {
        pwm_init((pwm_channel_enum)vot_pwm_channel, VOT_PWM_FREQUENCY_HZ, 10000);
    }
}

void Power_Out_Init()
{
    int16 vot_channel_list[POWER_OUT_NUM] = POWER_OUT_VOT_ADC_CHANNEL;
    int16 amp_channel_list[POWER_OUT_NUM] = POWER_OUT_AMP_ADC_CHANNEL;
    int16 vot_pwm_channel_list[POWER_OUT_NUM] = POWER_OUT_VOT_PWM_CHANNEL;
    int16 enable_pin_list[POWER_OUT_NUM] = POWER_OUT_ENABLE_PIN;
    float votK[POWER_OUT_NUM][2] = POWER_OUT_VOT_ADC_K;
    float ampK[POWER_OUT_NUM][2] = POWER_OUT_AMP_ADC_K;
    
    uint8 counter;
    for(counter = 0; counter < POWER_OUT_NUM; counter++)
    {
        uint8 can_adjust = vot_pwm_channel_list[counter] != -1 ? 1 : 0;
        Power_Out_Single_Init(&power_outs[counter], vot_channel_list[counter], amp_channel_list[counter], votK[counter], ampK[counter], enable_pin_list[counter], can_adjust, vot_pwm_channel_list[counter]);
    }
}

void Power_Out_Enable(Power_Out_t *power_out, uint8 enable)
{
    if(power_out->config.enable_pin != POWER_OUT_FUNCTION_DISABLE)
    {
        gpio_set_level((gpio_pin_enum)power_out->config.enable_pin, enable);
    }
    power_out->enabled = enable;
}

void Power_Out_Set_PWM(Power_Out_t *power_out, uint32 duty_cycle)
{
    if(power_out->config.can_adjust && power_out->config.vot_pwm_channel != POWER_OUT_FUNCTION_DISABLE)
    {
        pwm_set_duty((pwm_channel_enum)power_out->config.vot_pwm_channel, duty_cycle);
    }
}

void Power_Out_Single_Read(Power_Out_t *power_out)
{
    float tmp = 0.0f;
    if(power_out->config.voltage_read_channel != POWER_OUT_FUNCTION_DISABLE)
    {
        // tmp = (float)adc_convert((adc_channel_enum)power_out->config.voltage_read_channel);
        tmp = (float)adc_mean_filter_convert((adc_channel_enum)power_out->config.voltage_read_channel,2);
        Low_pass_filter(&power_out->config.vot_adc_filter, tmp);
        power_out->config.v_adc = tmp;
        power_out->status.voltage_V = Low_pass_filter(&power_out->config.vot_filter, tmp) * power_out->config.votK[0] + power_out->config.votK[1];
    }
    else
    {
        power_out->status.voltage_V = power_out->set_voltage_V;
    }
    if(power_out->config.current_read_channel != POWER_OUT_FUNCTION_DISABLE)
    {
        // tmp = (float)adc_convert((adc_channel_enum)power_out->config.current_read_channel);
        tmp = (float)adc_mean_filter_convert((adc_channel_enum)power_out->config.current_read_channel,2);
        power_out->status.current_A = Low_pass_filter(&power_out->config.amp_filter, tmp) * power_out->config.ampK[0] + power_out->config.ampK[1];
        power_out->status.current_A = power_out->status.current_A > 0.0f ? power_out->status.current_A : 0.0f;
    }
    else
    {
        power_out->status.current_A = power_out->set_current_A;
    }

    power_out->status.power_W = power_out->status.voltage_V * power_out->status.current_A;
    uint8 counter;
    for(counter = 0; counter < 9; counter++)
    {
        power_out->status.past_voltage_V[counter] = power_out->status.past_voltage_V[counter+1];
        power_out->status.past_current_A[counter] = power_out->status.past_current_A[counter+1];
    }
    power_out->status.past_voltage_V[9] = power_out->status.voltage_V;
    power_out->status.past_current_A[9] = power_out->status.current_A;

    power_out->status.energy_J += (power_out->status.past_voltage_V[9] * power_out->status.past_current_A[9] + power_out->status.past_voltage_V[8] * power_out->status.past_current_A[8]) * (POWER_OUT_READ_TIME_MS / 2000.0f);
    power_out->status.quantity_C += (power_out->status.past_current_A[9] + power_out->status.past_current_A[8]) * (POWER_OUT_READ_TIME_MS / 2000.0f);

}

void Power_Out_Read()
{
    uint8 counter;
    for(counter = 0; counter < POWER_OUT_NUM; counter++)
    {
        Power_Out_Single_Read(&power_outs[counter]);
    }
}
