/*
 * mppt_control.c
 *
 *  Created on: 2024年6月29日
 *      Author: psx
 */

#include "mppt_control.h"
#include "collect_Conversion.h"
#include "pwm.h"
#include "inflash.h"
#include "gpio.h"
#include "sl_protocol.h"
#include "task.h"

#include "uart_dev.h"

static void TrickleCharge(void);
static void ConstantCurrentCharge(void);
static void ConstantVoltageCharge(void);
static void FloatingCharge(void);
//static void NoBatteryCharge(void);


/* 占空比 */
float g_duty_ratio = 0.8;
/* 用于确定工作模式 */
//static uint8_t modeFlag = 2;

/**
  * @brief  得到输出的功率
  * @param
  * @retval OutputPower 输出功率
  */
static float Get_OutputPower(void)
{
    static float OutputPower;
    static float V_out, I_out;

    V_out = get_PV_VOLT_OUT();
    I_out = get_CHG_CURR();

    OutputPower = V_out * I_out;

    printf(" V = %d/100, I = %d/10000, OutputPower = %d/10000 \r\n",
            (int)(V_out*100), (int)(I_out * 10000), (int)(OutputPower * 10000));

    return OutputPower;
}

/**
  * @brief  使用的为扰动干扰法,调整输出电压,使功率输出最大
  * @param
  * @retval
  */
/* pwm占空比调节步长 */
const float step1_pwm = 0.01;
const float step2_pwm = 0.005;
//#define array_num 10
void mppt_readJust(void)
{
//    static float last_duty_ratio = 0.5;
//    static float now_duty_ratio;
//    static float last_OutputPower;
//    static float now_OutputPower;
//    static float step_pwm = step1_pwm;
//
//    last_OutputPower = Get_OutputPower();
//
//    printf(" duty_ratio = %d/1000 \r\n", (int)(last_duty_ratio * 1000));
//
//    /* 正向调节查看功率是否会变大 */
//    now_duty_ratio = last_duty_ratio + step_pwm;
//    if (now_duty_ratio > 1) {
//        now_duty_ratio = 1;
//    }
//    Set_duty_ratio(now_duty_ratio);
//    now_OutputPower = Get_OutputPower();
//    if (now_OutputPower > last_OutputPower) {
//        printf(" now_OutputPower > last_OutputPower1 \r\n");
//        last_duty_ratio = now_duty_ratio;
//        return;
//    }
//
//    /* 负向调节查看功率是否会变大 */
//    now_duty_ratio = last_duty_ratio - step_pwm;
//    if (now_duty_ratio < 0) {
//        now_duty_ratio = 0;
//    }
//    Set_duty_ratio(now_duty_ratio);
//    now_OutputPower = Get_OutputPower();
//    if (now_OutputPower > last_OutputPower) {
//        printf(" now_OutputPower > last_OutputPower2 \r\n");
//        last_duty_ratio = now_duty_ratio;
//        return;
//    }
//
//    /* 正负向调节功率均未变大，此时设置功率为原来的点 */
//    Set_duty_ratio(last_duty_ratio);
//    step_pwm = step2_pwm;

    static float last_duty_ratio = 0.5;
    static float last_OutputPower;
    static float now_OutputPower;
    static float step_pwm = step1_pwm;

    last_OutputPower = Get_OutputPower();

    printf(" duty_ratio = %d/1000 \r\n", (int)(last_duty_ratio * 1000));

    /* 正向调节查看功率是否会变大 */
    g_duty_ratio = last_duty_ratio + step_pwm;

    Set_duty_ratio(&g_duty_ratio);
    now_OutputPower = Get_OutputPower();
    if (now_OutputPower > last_OutputPower) {
        printf(" now_OutputPower > last_OutputPower1 \r\n");
        last_duty_ratio = g_duty_ratio;
        return;
    }

    /* 负向调节查看功率是否会变大 */
    g_duty_ratio = last_duty_ratio - step_pwm;
    Set_duty_ratio(&g_duty_ratio);
    now_OutputPower = Get_OutputPower();
    if (now_OutputPower > last_OutputPower) {
        printf(" now_OutputPower > last_OutputPower2 \r\n");
        last_duty_ratio = g_duty_ratio;
        return;
    }

    /* 正负向调节功率均未变大，此时设置功率为原来的点 */
    g_duty_ratio = last_duty_ratio;
    Set_duty_ratio(&g_duty_ratio);
    step_pwm = step2_pwm;
}

void printf_data(void)
{
    printf("\n");
//    get_CHG_CURR();
//    get_PV_VOLT_OUT();
//    get_DSG_CURR();
//    get_PV1_VOLT_IN();
//    get_PV_VOLT_IN1();
//    get_MOSFET_Temper();
//    get_PV2_VOLT_IN();

    printf("\n");
}

//float_t get_capturedata(float_t (*fun)(void))
//{
//    float_t temp1;
//    float_t temp[3];
//
//    for (int i = 0; i < 3; ++i) {
//        temp[i] = fun();
////        Delay_Us(1);
//    }
//
//    if (temp[0] > temp[1]) {
//        temp1 = temp[0];
//        temp[0] = temp[1];
//        temp[1] = temp1;
//    }
//
//    if (temp[0] > temp[2]) {
//        temp1 = temp[0];
//        temp[0] = temp[2];
//        temp[2] = temp1;
//        if (temp[1] > temp[2]) {
//            temp1 = temp[1];
//            temp[1] = temp[2];
//            temp[2] = temp1;
//        }
//    }
//
//    return temp[1];
//}

//uint16_t get_mpptMode(void)
//{
//    return (uint16_t)modeFlag;
//}

/**
  * @brief  恒定输入电压
  * @param
  * @retval
  *
  */
void mppt_constantVoltage(float InVoltage)
{
//    static uint8_t ConstantVoltageFlag = 1;
//    float PV1_V = get_PV1_VOLT_IN();
//
//    if (ConstantVoltageFlag) {
//        if (PV1_V > InVoltage) {
//            g_duty_ratio += step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio -= step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (PV1_V - InVoltage < 0.1) {
//            ConstantVoltageFlag = 0;
//        }
//    } else {
//        if (PV1_V > InVoltage) {
//            g_duty_ratio += step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio -= step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (PV1_V - InVoltage > 0.1) {
//            ConstantVoltageFlag = 1;
//        }
//    }

    static float_t kp = 0.005;
    static float_t ki = 0.00001;
//    static float_t allError = 0;

//    float_t error = (get_PV1_VOLT_IN()) - InVoltage;
//    float_t error = InVoltage - (get_PV2_VOLT_IN());
//    allError += error;
//    printf("111\n");

//    float_t pv1Volt = get_capturedata(get_PV1_VOLT_IN);

//    float_t pv1Volt = get_PV1_VOLT_IN();
    float_t pv1Volt = g_Mppt_Para.Input_Voltage;
//    printf("volt in : %d \n", pv1Volt);
    float_t error = pv1Volt - InVoltage;
//    float_t error = InVoltage - pv1Volt;
    float_t stepPwm = kp * error + ki * pv1Volt;
    g_duty_ratio += stepPwm;

//    printf("setPwm : %d/10000 \n", (int)(stepPwm * 10000));
//    printf("setPwm : %d/10000 \n", (int)(stepPwm * 10000));
//    printf("g_duty_ratio : %d/10000 \n", (int)(g_duty_ratio * 10000));

    Set_duty_ratio(&g_duty_ratio);
}

/**
  * @brief  恒定输出电压(电池)
  * @param
  * @retval
  *
  */
void mppt_constantVoltageB(float OutVoltage)
{
//    static uint8_t ConstantVoltageFlag = 1;
//    float PV1_V = get_PV_VOLT_OUT();
//
//    if (ConstantVoltageFlag) {
//        if (PV1_V > OutVoltage) {
//            g_duty_ratio -= step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio += step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (PV1_V - OutVoltage < 0.1) {
//            ConstantVoltageFlag = 0;
//        }
//    } else {
//        if (PV1_V > OutVoltage) {
//            g_duty_ratio -= step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio += step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (PV1_V - OutVoltage > 0.1) {
//            ConstantVoltageFlag = 1;
//        }
//    }

//    static float_t kp = 0.0005;
//    static float_t ki = 0.000001;
    static float_t kp = 0.005;
    static float_t ki = 0.00001;
//    static float_t kp = 0.1;
//    static float_t ki = 0.001;

//    float_t outVolt = get_PV_VOLT_OUT();
    float_t outVolt = g_Mppt_Para.Battery_Voltage;
//    float_t outVolt = voltOut;
//    float_t error = outVolt - OutVoltage;
    float_t error = OutVoltage - outVolt;
    float_t stepPwm = kp * error + ki * outVolt;
    g_duty_ratio += stepPwm;

//    printf("setPwm : %d/10000 \n", (int)(stepPwm * 10000));

    Set_duty_ratio(&g_duty_ratio);

}

/**
  * @brief  恒定输出电压（输出检测端）
  * @param
  * @retval
  *
  */
void mppt_constantVoltageO(float OutVoltage)
{
//    static uint8_t ConstantVoltageFlag = 1;
//    float PV1_V = get_PV_VOLT_OUT();
//
//    if (ConstantVoltageFlag) {
//        if (PV1_V > OutVoltage) {
//            g_duty_ratio -= step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio += step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (PV1_V - OutVoltage < 0.1) {
//            ConstantVoltageFlag = 0;
//        }
//    } else {
//        if (PV1_V > OutVoltage) {
//            g_duty_ratio -= step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio += step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (PV1_V - OutVoltage > 0.1) {
//            ConstantVoltageFlag = 1;
//        }
//    }

//    static float_t kp = 0.0005;
//    static float_t ki = 0.000001;
    static float_t kp = 0.005;
    static float_t ki = 0.00001;
//    static float_t kp = 0.1;
//    static float_t ki = 0.001;

//    float_t outVolt = get_PV_VOLT_OUT();
    float_t outVolt = g_Mppt_Para.Output_Voltage;
//    float_t outVolt = voltOut;
//    float_t error = outVolt - OutVoltage;
    float_t error = OutVoltage - outVolt;
    float_t stepPwm = kp * error + ki * outVolt;
    g_duty_ratio += stepPwm;

//    printf("setPwm : %d/10000 \n", (int)(stepPwm * 10000));

    Set_duty_ratio(&g_duty_ratio);

}

/**
  * @brief  后端电池钳位，恒定输出电流
  * @param
  * @retval
  *
  */
void mppt_constantCurrentO(float outCurrent)
{
//    static uint8_t ConstantCurrent = 1;
//    float out_I = get_CHG_CURR();
//
//    if (ConstantCurrent) {
//        if (out_I > outCurrent) {
//            g_duty_ratio -= step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio += step1_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (out_I - outCurrent < 0.1) {
//            ConstantCurrent = 0;
//        }
//    }
//
//    else {
//        if (out_I > outCurrent) {
//            g_duty_ratio -= step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        } else {
//            g_duty_ratio += step2_pwm;
//            Set_duty_ratio(&g_duty_ratio);
//        }
//
//        if (out_I - outCurrent > 0.1) {
//            ConstantCurrent = 1;
//        }
//    }

    static float_t kp = 0.005;
    static float_t ki = 0.00005;

//    static float_t last_CHG_CURR = 0;
//    static float_t flag = 1;
//    static float_t last_OutputPower = 0;

//    float_t outCurr = get_CHG_CURR();

    float_t outCurr = g_Mppt_Para.Charg_Current;
//    float_t OutputPower = outCurr * get_PV_VOLT_OUT();
    float_t error = outCurrent - outCurr;
//    float_t error = outCurr - outCurrent;
    float_t stepPwm = kp * error + ki * outCurr;

//    if (flag) {
//        if (OutputPower > last_OutputPower) {
//            g_duty_ratio += stepPwm;
//            flag = 1;
//        } else {
//            g_duty_ratio -= stepPwm;
//            flag = 0;
//        }
//    } else {
//        if (OutputPower > last_OutputPower) {
//            g_duty_ratio -= stepPwm;
//            flag = 0;
//        } else {
//            g_duty_ratio += stepPwm;
//            flag = 1;
//        }
//    }
//
//    last_OutputPower = OutputPower;

    g_duty_ratio += stepPwm;

//    printf("setPwm : %d/10000 \n", (int)(stepPwm * 10000));
//    printf("g_duty_ratio : %d/10000 \n", (int)(g_duty_ratio * 10000));

    Set_duty_ratio(&g_duty_ratio);

//    last_CHG_CURR = outCurr;
//    if (stepPwm > 0 && (last_CHG_CURR > outCurr)) {
//        flag = 1;
//    } else {
//        flag = 0;
//    }
}

/**
  * @brief  涓流充电
  * @param
  * @retval
  *
  */
void TrickleCharge(void)
{
    static float_t TrickleChargeC;
    static uint8_t onlyOnce = 1;
    if (onlyOnce) {
        TrickleChargeC = (float_t)g_slConfigInfo.trickleChargeC / 100;
        onlyOnce = 0;
    }
//    printf("Trickle\n");
    mppt_constantCurrentO(TrickleChargeC + g_Mppt_Para.Discharg_Current);
}

/**
  * @brief  恒流充电（大电流充电），mppt最大功率充电
  * @param
  * @retval
  *
  */
void ConstantCurrentCharge(void)
{
//    mppt_readJust();
    mppt_constantVoltage(18);
//    printf("ConstantCurrent\n");

}

/**
  * @brief  恒压充电
  * @param
  * @retval
  *
  */
void ConstantVoltageCharge(void)
{
    static float_t ConstantVoltageChargeV;
    static uint8_t onlyOnce = 1;
    if (onlyOnce) {
        ConstantVoltageChargeV = (float_t)g_slConfigInfo.constantVoltageChargeV / 100;
        onlyOnce = 0;
    }

    mppt_constantVoltageO(ConstantVoltageChargeV);
}

/**
  * @brief  浮充充电
  * @param
  * @retval
  *
  */
void FloatingCharge(void)
{
//    static uint32_t num = 0;
//    static uint32_t numLenFlag;
//    static uint8_t onlyOnce = 1;
//    if (onlyOnce) {
//        numLenFlag = g_slConfigInfo.FloatTime * 1000;
//        onlyOnce = 0;
//    }

//    printf("float\n");
//    TIM_SetCompare4(TIM4, 0);

//    if (numLenFlag == ++num) {
//        num = 0;
//        for (int var = 0; var < 10; ++var) {
//            ConstantVoltageCharge();
//        }
//        g_Mppt_Para.MPPT_Mode = CONSTANTVOLTAGE;
//        printf("float\n");
//    }

    static float_t FloatChargeV;
    static uint8_t onlyOnce = 1;
    if (onlyOnce) {
        FloatChargeV = (float_t)g_slConfigInfo.FloatV / 100;
        onlyOnce = 0;
    }
    mppt_constantVoltageO(FloatChargeV);

}

/**
  * @brief  没有电池时，恒定输出一个电压
  * @param
  * @retval
  *
  */
void NoBatteryCharge(void)
{
//    static float_t NoBatteryChargeV;
//    static uint8_t onlyOnce = 1;
//    if (onlyOnce) {
//        NoBatteryChargeV = (float_t)g_slConfigInfo.noBatteryChargeV / 100;
//        onlyOnce = 0;
//    }

    mppt_constantVoltageO(14.2);

//    if ((g_Mppt_Para.Battery_Voltage - NoBatteryChargeV > 0.2 && g_Mppt_Para.Charg_Current < 0.1)
//            || (NoBatteryChargeV - g_Mppt_Para.Battery_Voltage > 0.1 && g_Mppt_Para.Charg_Current > 0.5)) {
//        ConstantCurrentCharge();
//        g_Mppt_Para.MPPT_Mode = CONSTANTCURRENT;
//    }

//    if (!overTemperature) {
//        if (!(g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current < 0.3
//                && g_Mppt_Para.Discharg_Current - g_Mppt_Para.Charg_Current < 0.3)) {
//            ConstantCurrentCharge();
//            g_Mppt_Para.MPPT_Mode = CONSTANTCURRENT;
//        }
//    }

}

void MpptContorl(void)
{
    switch(g_Mppt_Para.MPPT_Mode) {
    case TRICKLE:
//        printf("111\n");
        TrickleCharge();
        break;

    case CONSTANTCURRENT:
//        printf("222222\n");
        ConstantCurrentCharge();
//        ConstantVoltageCharge();
        break;

    case CONSTANTVOLTAGE:
//        printf("333333333\n");
        ConstantVoltageCharge();
        break;

    case FLOAT:
//        printf("444444444444\n");
        FloatingCharge();
        break;

//    case NoBattery:
////        printf("555555555555555\n");
//        NoBatteryCharge();
//        break;

    default:
        break;
    }
}

void MpptMode(void)
{
//    printf("vout : %d /100 \n", (int)(g_Mppt_Para.Battery_Voltage * 100));
//    printf("iout : %d /1000 \n", (int)(g_Mppt_Para.Charg_Current * 1000));
//    printf("in checkSolarOpenCircuitVoltage v: %d/100 \n", (int)(g_Mppt_Para.Solar_Open_Circuit_Voltage * 100));

    static float ConstantCurrentV;
    static float ConstantVoltageV;
    static float FloatI;
    static float StopSolarOpenCircuitV;

    /* 赋值仅执行一次 */
    static uint8_t only_once = 1;
    if (only_once) {
        ConstantCurrentV = (float)g_slConfigInfo.constantCurrentV / 100;
        ConstantVoltageV = (float)g_slConfigInfo.constantVoltageV / 100;
        FloatI = (float)g_slConfigInfo.floatI / 100;
        printf("FloatI: %d / 100 \n", (int)(FloatI * 100));
        StopSolarOpenCircuitV = (float)g_slConfigInfo.stopSolarOpenCircuitV / 100;
        only_once = 0;
    }

////    if (g_Mppt_Para.Battery_Voltage > 16 || g_Mppt_Para.Battery_Voltage < 8
////            || modeFlag == NoBattery) {
////        modeFlag = NoBattery;
//    if (g_Mppt_Para.Battery_Voltage > 16 || g_Mppt_Para.Battery_Voltage < 8
//            || g_Mppt_Para.MPPT_Mode == NoBattery) {
//        g_Mppt_Para.MPPT_Mode = NoBattery;
//        return;
//    }
//
////    if (((ConstantVoltageV < g_Mppt_Para.Battery_Voltage) &&
////            (FloatI > g_Mppt_Para.Charg_Current)) || modeFlag == FLOAT) {
////        modeFlag = FLOAT;
//    if (((ConstantVoltageV < g_Mppt_Para.Battery_Voltage) &&
//            (FloatI > g_Mppt_Para.Charg_Current)) || g_Mppt_Para.MPPT_Mode == FLOAT) {
//        g_Mppt_Para.MPPT_Mode = FLOAT;
//        return;
//    }
//
//    if (((ConstantCurrentV + 0.4) < g_Mppt_Para.Battery_Voltage) &&
//            ((ConstantVoltageV - 0.4) >= g_Mppt_Para.Battery_Voltage)) {
////        modeFlag = CONSTANTCURRENT;
//        g_Mppt_Para.MPPT_Mode = CONSTANTCURRENT;
//        return;
//    }
//
//    if ((ConstantVoltageV < g_Mppt_Para.Battery_Voltage) &&
//            (FloatI + 0.1 <= g_Mppt_Para.Charg_Current)) {
////        modeFlag = CONSTANTVOLTAGE;
//        g_Mppt_Para.MPPT_Mode = CONSTANTVOLTAGE;
//        return;
//    }
//
//    if (ConstantCurrentV > g_Mppt_Para.Battery_Voltage) {
////        modeFlag = TRICKLE;
//        g_Mppt_Para.MPPT_Mode = TRICKLE;
//        return;
//    }


//    if (g_Mppt_Para.Battery_Voltage > 16 || g_Mppt_Para.Battery_Voltage < 8
//            || g_Mppt_Para.MPPT_Mode == NoBattery) {
//        g_Mppt_Para.MPPT_Mode = NoBattery;
//        return;
//    }

//    if (g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current < 0.05
//            || g_Mppt_Para.Discharg_Current - g_Mppt_Para.Charg_Current < 0.05) {
//        g_Mppt_Para.MPPT_Mode = NoBattery;
//        return;
//    }
//
//    if (((ConstantVoltageV < g_Mppt_Para.Battery_Voltage) &&
//            (FloatI > g_Mppt_Para.Charg_Current)) || g_Mppt_Para.MPPT_Mode == FLOAT) {
//        g_Mppt_Para.MPPT_Mode = FLOAT;
//        return;
//    }
//
//    if (((ConstantCurrentV + 0.4) < g_Mppt_Para.Battery_Voltage) &&
//            ((ConstantVoltageV - 0.4) >= g_Mppt_Para.Battery_Voltage)) {
//        g_Mppt_Para.MPPT_Mode = CONSTANTCURRENT;
//        return;
//    }
//
//    if ((ConstantVoltageV < g_Mppt_Para.Battery_Voltage) &&
//            (FloatI + 0.1 <= g_Mppt_Para.Charg_Current)) {
//        g_Mppt_Para.MPPT_Mode = CONSTANTVOLTAGE;
//        return;
//    }
//
//    if (ConstantCurrentV > g_Mppt_Para.Battery_Voltage) {
//        g_Mppt_Para.MPPT_Mode = TRICKLE;
//        return;
//    }



    if (g_Mppt_Para.Input_Voltage < StopSolarOpenCircuitV
            && (g_Mppt_Para.Discharg_Current >= g_Mppt_Para.Charg_Current
                    || g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current < 0.05)) {
        g_Mppt_Para.MPPT_Mode = NoWork;
        printf("nowork \n");
        stop_mpptWork();
        TimeSliceOffset_Register(&m_startMpptControl, Task_startMpptControl
                , startMpptControl_reloadVal, startMpptControl_offset);
        return;
    }


//    if (((g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current < 0.03
//            && g_Mppt_Para.Discharg_Current - g_Mppt_Para.Charg_Current < 0.03)
//            && (g_Mppt_Para.Battery_Voltage < ConstantVoltageV - 1
//            || g_Mppt_Para.Battery_Voltage > ConstantVoltageV + 1))
//            || g_Mppt_Para.MPPT_Mode == NoBattery) {
//        g_Mppt_Para.MPPT_Mode = NoBattery;
//        return;
//    }

//    if (((ConstantVoltageV < g_Mppt_Para.Battery_Voltage) &&
//            (FloatI > g_Mppt_Para.Charg_Current)) || g_Mppt_Para.MPPT_Mode == FLOAT) {
//        g_Mppt_Para.MPPT_Mode = FLOAT;
//        return;
//    }
    if ((g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current < 0.05
            && g_Mppt_Para.Discharg_Current - g_Mppt_Para.Charg_Current < 0.05)
            || g_Mppt_Para.Battery_Voltage > 16 || g_Mppt_Para.Battery_Voltage < 8) {
//            || g_Mppt_Para.MPPT_Mode == NoBattery) {
//        g_Mppt_Para.MPPT_Mode = NoBattery;
//        g_Mppt_Para.MPPT_Mode = CONSTANTVOLTAGE;
        g_Mppt_Para.MPPT_Mode = FLOAT;

//        printf("Charg_Current : %d/100 \n", (int)(g_Mppt_Para.Charg_Current * 100));
//        printf("Discharg_Current : %d/100 \n", (int)(g_Mppt_Para.Discharg_Current * 100));
//        printf("Battery_Voltage : %d/100 \n", (int)(g_Mppt_Para.Battery_Voltage * 100));

        g_batteryState = 0;

        return;
    }

    if ((((ConstantCurrentV + 0.2) < g_Mppt_Para.Battery_Voltage)
            &&((ConstantVoltageV - 0.2) >= g_Mppt_Para.Battery_Voltage))
            &&(g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current > 0.1)) {
        g_Mppt_Para.MPPT_Mode = CONSTANTCURRENT;
        return;
    }
//    if ((g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current > 4)) {
//        g_Mppt_Para.MPPT_Mode = CONSTANTCURRENT;
//        return;
//    }

//    if (((ConstantVoltageV < g_Mppt_Para.Battery_Voltage)
//            &&(FloatI + 0.1 <= g_Mppt_Para.Charg_Current))
//            || (FloatI + 0.1 <= g_Mppt_Para.Discharg_Current)) {
////            || (g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current < 0.03
////               && g_Mppt_Para.Discharg_Current - g_Mppt_Para.Charg_Current < 0.03)) {
//
////        printf("mppt mode \n");
////        printf(" vout : %d/100 \n", (int)(g_Mppt_Para.Battery_Voltage * 100));
//        g_Mppt_Para.MPPT_Mode = CONSTANTVOLTAGE;
//        return;
//    }

    if (((ConstantVoltageV < g_Mppt_Para.Battery_Voltage)
            &&(FloatI + 0.1 <= g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current))) {
        g_Mppt_Para.MPPT_Mode = CONSTANTVOLTAGE;
        return;
    }

    if ((((ConstantVoltageV < g_Mppt_Para.Battery_Voltage)
            && (FloatI > g_Mppt_Para.Charg_Current))
            && (FloatI > g_Mppt_Para.Discharg_Current))
            || g_Mppt_Para.MPPT_Mode == FLOAT) {
        g_Mppt_Para.MPPT_Mode = FLOAT;
        return;
    }

    if (ConstantCurrentV > g_Mppt_Para.Battery_Voltage) {
        g_Mppt_Para.MPPT_Mode = TRICKLE;
        return;
    }

}

void findMiNDutyRatio(void)
{
    static uint8_t num = 100;
    if (0.05 < get_CHG_CURR()) {
        num -= 1;
        TIM_SetCompare4(TIM4, num);
    }
    else {
        printf("min duty ratio : %d/200 \n", num);
    }
}


void test(void)
{
//    mppt_readjust();
//    Get_OutputPower();

//    mppt_constantVoltage(18);

//    findMiNDutyRatio();
//    MpptContorl();

//    printf_data();
//    void MpptContorl();

//    mppt_constantVoltageO(12);

//    FloatingCharge();
//    mppt_readJust();

//    mppt_constantCurrentO(1);


//    if (g_interruptNum < 5) {
//        g_interruptNum++;
//        return;
//    }
//    g_Mppt_Para.Charg_Current = get_capturedata(get_CHG_CURR);
//    g_Mppt_Para.Discharg_Current = get_capturedata(get_DSG_CURR);
//    g_Mppt_Para.Output_Voltage = get_capturedata(get_PV_VOLT_OUT);
//    g_Mppt_Para.Battery_Voltage = g_Mppt_Para.Output_Voltage;
    g_Mppt_Para.Charg_Current = get_CHG_CURR();
    g_Mppt_Para.Discharg_Current = get_DSG_CURR();
    g_Mppt_Para.Output_Voltage = get_PV_VOLT_OUT();
    g_Mppt_Para.Input_Voltage  = get_PV1_VOLT_IN();

    if (g_Mppt_Para.Discharg_Current == 0 && g_Mppt_Para.Charg_Current == 0) {
        return;
    }
//    g_Mppt_Para.Battery_Voltage = g_Mppt_Para.Output_Voltage;

//    static float_t Volt = 0.7;
//    static float_t Curr = 5.5;
//    static float_t loopImpedance;
//    static uint8_t onlyone = 1;
//    if (onlyone) {
//        loopImpedance = (float_t)g_slConfigInfo.loopImpedance / 100;
//    }


    static float_t inBatteryCurr;
    inBatteryCurr = g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current;
    if (inBatteryCurr > 0.1) {
        g_Mppt_Para.Battery_Voltage = g_Mppt_Para.Output_Voltage - inBatteryCurr * g_impedance;
    } else {
        g_Mppt_Para.Battery_Voltage = g_Mppt_Para.Output_Voltage;
    }


    if (g_batteryState == 0 && inBatteryCurr > 0.1 && g_Mppt_Para.Output_Voltage < 14.2) {
        printf("int g_batteryState : %d\n", g_batteryState);
        g_batteryState = 1;
        TimeSliceOffset_Register(&m_impedanceCalculation, Task_impedanceCalculation
                , impedanceCalculation_reloadVal, impedanceCalculation_reloadVal);
    }

//    mppt_constantVoltage(18);

//    if (g_Mppt_Para.MPPT_Mode == CONSTANTCURRENT
//            || g_Mppt_Para.MPPT_Mode == CONSTANTVOLTAGE) {
//
//        g_Mppt_Para.Battery_Voltage = get_capturedata(get_PV_VOLT_OUT)
//                - g_impedance * (g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current);
//    } else {
//        g_Mppt_Para.Battery_Voltage = get_capturedata(get_PV_VOLT_OUT);
//    }

//    voltOut = get_capturedata(get_PV_VOLT_OUT);
//    g_Mppt_Para.Battery_Voltage = voltOut - g_impedance * (g_Mppt_Para.Charg_Current - g_Mppt_Para.Discharg_Current);


//    ConstantVoltageCharge();
//    return;

    if (!overTemperature) {
        MpptMode();
        MpptContorl();
    }


//    mppt_constantVoltageO(12);

//    static uint32_t run_num = 0;
//    if (1000 < run_num++) {
//        FloatingCharge();
//        run_num = 1200;
//        printf("in floatcharge \n");
//        return;
//    }
//    mppt_readJust();
//    mppt_constantCurrentO(1.2);


//    int16_t var = 0;
//    char buff[4];
//    for (var = 0; var < 100; ++var) {
//        sprintf(buff, "%3d:", var);
//        uart_dev_write(g_gw485_uart4_handle, buff, sizeof(buff));
//        uart_dev_write(g_gw485_uart4_handle, "1234567890\n", sizeof("1234567890\n"));
//        Delay_Ms(1);
//    }
//    uart_dev_write(g_gw485_uart4_handle, "\n\n\n\n\n\n", sizeof("\n\n\n\n\n\n"));
}