#include "bl_chargControl.h"
#include "parameter.h"
#include "comm_types.h"
#include "FM_GPIO.h"
#include "task.h"


static BOOL stopChargConditions(void);
static BOOL floatChargConditions(void);
static BOOL mpptChargConditions(void);
static BOOL constantVChargConditions(void);
static void mpptCharge(void);
static void constantVoltageCharge(void);
static void floatCharge(void);
static void mppt_constantVoltage(float InVoltage);
static void mppt_constantVoltageNoBatteryO(float OutVoltage);
static void mppt_constantVoltageO(float OutVoltage);

static void judgeYNBattery(void);
static void chargControlMode(void);
static void BatteryChargControl(void);
static void noBatteryChargControl(void);

static void setPIControlStep(float *PI_step);


static BOOL chargControlFlag = FALSE;
// static BOOL getChargControlFlag(void);
void setChargControlFlag(BOOL state);

void setPIControlStep(float *PI_step)
{
    if (*PI_step > PI_CONTROL_MAX) {
        *PI_step = PI_CONTROL_MAX;
    }

    else if (*PI_step < PI_CONTROL_MIN) {
        *PI_step = PI_CONTROL_MIN;
    }
}

/**
  * @brief  恒定输入电压
  * @param  InVoltage 需要控制到的输入电压
  * @retval
  *
  */
void mppt_constantVoltage(float InVoltage)
{
    static float kp = 0.005;
    static float ki = 0.00001;
    // static float solarInCircuitVoltage;
    static float error;
    static float stepPwm;

    // solarInCircuitVoltage = getSolarInCircuitVoltage();
//    error = InVoltage - getSolarInCircuitVoltage();    
    error = getSolarInCircuitVoltage() - InVoltage;
    stepPwm = kp * error + ki * getSolarInCircuitVoltage();

    setPIControlStep(&stepPwm);

    setDutyRatio((getDutyRatio() + stepPwm));
    // if (getMosTemperState() == mosTemperEnd) {        
    //     setDutyRatio((getDutyRatio() + stepPwm - 0.1));
    // } else {
    //     setDutyRatio((getDutyRatio() + stepPwm));
    // }
}

/**
  * @brief  恒定输出电压(无电池)
  * @param
  * @retval
  *
  */
void mppt_constantVoltageNoBatteryO(float OutVoltage)
{
    static float kp = 0.005;
    static float ki = 0.00001;
    static float outVolt;
    static float error;
    static float stepPwm;

    outVolt = getOutputVoltage();
    error = OutVoltage - outVolt;
    stepPwm = kp * error + ki * outVolt;
    setPIControlStep(&stepPwm);
    
    setDutyRatio((getDutyRatio() + stepPwm));
}

/**
  * @brief  恒定输出电压（输出检测端）
  * @param
  * @retval
  *
  */
void mppt_constantVoltageO(float OutVoltage)
{
    // static float lastVolt = 0;
    // static float lastStepPwm = 0;
    static float lastDutyRatio = 0;
    static float kp = 0.005;
    static float ki = 0.00001;
    static float outVolt;
    static float error;
    static float StepPwm;

    outVolt = getOutputVoltage();
    error = OutVoltage - outVolt;
    StepPwm = kp * error + ki * outVolt;
    setPIControlStep(&StepPwm);

    /* 当有电池时，输出电压的曲线是先上升后下降 */
    if (lastDutyRatio >= getDutyRatio()) {
//        if (lastVolt >= outVolt) {
        setDutyRatio((getDutyRatio() + StepPwm));
        // if (getMosTemperState() == mosTemperEnd) {
        //     setDutyRatio((getDutyRatio() + StepPwm - 0.1));
        // } else {
        //     setDutyRatio((getDutyRatio() + StepPwm));
        // }
//        } else {
//            g_controlParameter.dutyRatio -= StepPwm;
//        }
    } else {    
//        if (lastVolt >= outVolt) {
//            g_controlParameter.dutyRatio -= StepPwm;
//        } else {
//            g_controlParameter.dutyRatio += StepPwm;
//        }
        setDutyRatio((getDutyRatio() - StepPwm));        
        // if (getMosTemperState() == mosTemperEnd) {        
        //     setDutyRatio((getDutyRatio() + StepPwm - 0.1));
        // } else {
        //     setDutyRatio((getDutyRatio() + StepPwm));
        // }
    }

    // lastVolt = outVolt;
    // lastStepPwm = StepPwm;
    lastDutyRatio = getDutyRatio();
}

/**
  * @brief  通过扰动干扰法追踪最大功率点
  * @param
  * @retval
  *
  */
void mppt_readJust(void)
{
    /* 调节占空比 */
//    static float_t step1 = 0.01;
//    static float_t step2 = 0.003;
//    static float_t tempV = 0.2;
//    static float_t i = 0.005;
//    static uint16_t flag = 0;
//    static float_t lastSolarInCircuitVoltage = 0;
//    static float_t lastPower = 0;
//    flag++;
//    if (flag < 500) {
//        return;
//    }
//    flag = 0;
//
//    float_t SolarInCircuitVoltage = get_PV1_VOLT_IN();
//    float_t power = g_otherParameter.Output_Voltage * g_otherParameter.Charg_Current;
//
//    float_t voltageDifference = SolarInCircuitVoltage - lastSolarInCircuitVoltage;
//
//    /* 输出电压随占空比增加电压减小 */
//    if (power <= lastPower) {
//        if (lastSolarInCircuitVoltage <= SolarInCircuitVoltage) {
//            if (voltageDifference > tempV) {
//                g_controlParameter.dutyRatio += step2 + voltageDifference / i;
//            } else {
//                g_controlParameter.dutyRatio += step1 + voltageDifference / i;
//            }
//        } else {
//            if (voltageDifference < -tempV) {
//                g_controlParameter.dutyRatio -= step2 + voltageDifference / i;
//            } else {
//                g_controlParameter.dutyRatio -= step1 + voltageDifference / i;
//            }
//        }
//    } else {
//        if (lastSolarInCircuitVoltage <= SolarInCircuitVoltage) {
//            if (voltageDifference > tempV) {
//                g_controlParameter.dutyRatio -= step2 - voltageDifference / i;
//            } else {
//                g_controlParameter.dutyRatio -= step1 - voltageDifference / i;
//            }
//        } else {
//            if (voltageDifference < -tempV) {
//                g_controlParameter.dutyRatio += step2 - voltageDifference / i;
//            } else {
//                g_controlParameter.dutyRatio += step1 - voltageDifference / i;
//            }
//        }
//    }
//
//    lastPower = power;
//    lastSolarInCircuitVoltage = SolarInCircuitVoltage;
//
//    Set_duty_ratio(&g_controlParameter.dutyRatio);


    /* 调节电压，变步长调节 */
//    static float_t Power3 = 0;                     //上上次功率
//    static float_t Power2 = 0;                     //上次功率
//    static float_t Power1 = 0;                     //当前功率
//    static float_t power23 = 0;                    //上次和上上次功率的绝对值
//    static float_t power12 = 0;                    //当前功率和上次功率的绝对值
////    static float_t SolarInCircuitVoltage3 = 0;     //上上次太阳能板电压
//    static float_t SolarInCircuitVoltage2 = 0;     //上次太阳能板电压
//    static float_t SolarInCircuitVoltage1 = 0;     //当前太阳能板电压
//    static float_t SolarInCircuitVoltage12 = 0;    //当前太阳能板电压和上次太阳能板电压的绝对值
//    SolarInCircuitVoltage1 = get_PV1_VOLT_IN();
//    Power1 = g_otherParameter.Output_Voltage * g_otherParameter.Charg_Current;
//    static float_t power12Abs = 0;
//    static float_t power23Abs = 0;
//    static float_t SolarInCircuitVoltage12Abs = 0;
//    static float_t dk = 0;                              //变步长因子
//    static float_t stepV = 0;
//    static float_t SolarInCircuitV = 18;           //控制太阳能板的输出电压稳定在该值
//
//    static float_t kp = 0.005;
//    static float_t ki = 0.00001;
//
//    /* 延时一段时间才判断 */
//    static uint16_t flag = 0;
//    flag++;
//    if (flag < 1000) {
////        float_t pv1Volt = g_otherParameter.Solar_In_Circuit_Voltage;
//        float_t pv1Volt = SolarInCircuitVoltage1;
//        float_t error = pv1Volt - SolarInCircuitV;
//        float_t stepPwm = kp * error + ki * pv1Volt;
//
//        g_controlParameter.dutyRatio += stepPwm;
//
//        /* 过温保护 */
//        if (g_otherParameter.overTemperature == 0) {
//
//        } else if (g_otherParameter.overTemperature == 1) {
//            g_controlParameter.dutyRatio -= 0.1;
//        } else if (g_otherParameter.overTemperature == 2) {
//            g_controlParameter.dutyRatio -= 0.2;
//        } else if (g_otherParameter.overTemperature == 3) {
//            g_controlParameter.dutyRatio -= 0.3;
//        }
//
//        Set_duty_ratio(&g_controlParameter.dutyRatio);
//
//        return;
//    }
//    flag = 0;
//
//    power23 = Power2 - Power3;
//    if (power23 < 0) {
//        power23Abs = -power23;
//    } else {
//        power23Abs = power23;
//    }
//
//    power12 = Power1 - Power2;
//    if (power12 < 0) {
//        power12Abs = -power12;
//    } else {
//        power12Abs = power12;
//    }
//
////    SolarInCircuitVoltage23 = SolarInCircuitVoltage2 - SolarInCircuitVoltage3;
//
//    SolarInCircuitVoltage12 = SolarInCircuitVoltage1 - SolarInCircuitVoltage2;
//
//    dk = power12Abs / power23Abs;
//    stepV = dk * SolarInCircuitVoltage12Abs;
//
////    printf(" dk : %d/10000 \n",  (int)(dk * 10000));
//
//    if (power12 > 0) {
//        if (SolarInCircuitVoltage12 > 0) {
//            SolarInCircuitV = SolarInCircuitVoltage1 + stepV;
//        } else {
//            SolarInCircuitV = SolarInCircuitVoltage1 - stepV;
//        }
//    } else {
//        if (SolarInCircuitVoltage12 > 0) {
//            SolarInCircuitV = SolarInCircuitVoltage1 - stepV;
//        } else {
//            SolarInCircuitV = SolarInCircuitVoltage1 + stepV;
//        }
//    }
//
//    printf(" SolarInCircuitV : %d/100 \n",  (int)(SolarInCircuitV * 100));
//
//    if (SolarInCircuitV > 21) {
//        SolarInCircuitV = 21;
//    }
//    else if (SolarInCircuitV < 15) {
//        SolarInCircuitV = 15;
//    }
//
//    printf(" SolarInCircuitV : %d/100 \n",  (int)(SolarInCircuitV * 100));
//
//    Power3 = Power2;
//    Power2 = Power1;
////    SolarInCircuitVoltage3 = SolarInCircuitVoltage2;
//    SolarInCircuitVoltage2 = SolarInCircuitVoltage1;
//
////      float_t pv1Volt = g_otherParameter.Solar_In_Circuit_Voltage;
//    float_t pv1Volt = SolarInCircuitVoltage1;
//    float_t error = pv1Volt - SolarInCircuitV;
//    float_t stepPwm = kp * error + ki * pv1Volt;
//
//    g_controlParameter.dutyRatio += stepPwm;
//
//    /* 过温保护 */
//    if (g_otherParameter.overTemperature == 0) {
//
//    } else if (g_otherParameter.overTemperature == 1) {
//        g_controlParameter.dutyRatio -= 0.1;
//    } else if (g_otherParameter.overTemperature == 2) {
//        g_controlParameter.dutyRatio -= 0.2;
//    } else if (g_otherParameter.overTemperature == 3) {
//        g_controlParameter.dutyRatio -= 0.3;
//    }
//
//    Set_duty_ratio(&g_controlParameter.dutyRatio);
//
//    return;

    /* 调节电压，两个电压步调节 */
    static float Power = 0;
    static float totalPower = 0;
    static float powerData[50] = {0};
    static uint8_t powerIndex = 0;

    /* 获取50次的平均值 */
    totalPower -= powerData[powerIndex];
    powerData[powerIndex] = getOutputVoltage() * getChargCurrent();
    totalPower += powerData[powerIndex];
    powerIndex++;
    if (powerIndex >= 50) {
        powerIndex = 0;
    }

    static float lPower = 0;
    static float lLPower = 0;
//    static float lLLPower = 0;

    static float SolarInCircuitV = 17;           //控制太阳能板的输出电压稳定在该值,初始为17V
    // static float kp = 0.005;
    // static float ki = 0.00001;

    static float stepV1 = 0.2;
    static float stepV2 = 0.1;

    static uint8_t flag1 = 0;       //表明上次运算是加还是减

    /* 延时一段时间才判断 */
    static uint16_t flag = 0;
    flag++;
    if (flag < 200) {
        // float pv1Volt = getSolarInCircuitVoltage();
        // float error = pv1Volt - SolarInCircuitV;
        // float stepPwm = kp * error + ki * pv1Volt;

        // setDutyRatio((getDutyRatio() + stepPwm));
        // set_pwmDutyRatio(getDutyRatio());

        mppt_constantVoltage(SolarInCircuitV);

        return;
    }

    if (getMosTemperState() == mosTemperReduce) {
        SolarInCircuitV = 16;
    }
    

    flag = 0;
    Power = totalPower / 50.0f;

    static float powerT = 0;
    powerT = Power - lPower;
    if (powerT < 0) {
        powerT = -powerT;
    }

    //  if ((lPower + 0.8f < Power) && (lLPower + 0.8f < Power) && (lLLPower + 0.8f < Power)) {
    if ((lPower + 0.1f < Power) && (lLPower + 0.1f < Power)) {
    // if ((lPower + 0.3f < Power)) {
        if (powerT > 5) {
            if (flag1) {
                SolarInCircuitV += stepV1;
                flag1 = 1;
            } else {
                SolarInCircuitV -= stepV1;
                flag1 = 0;
            }
        } else {
            if (flag1) {
                SolarInCircuitV += stepV2;
                flag1 = 1;
            } else {
                SolarInCircuitV -= stepV2;
                flag1 = 0;
            }
        }
    // } else if ((lPower - 0.8f > Power) && (lLPower - 0.8f > Power) && (lLLPower - 0.8f > Power)) {
    } else if ((lPower - 0.1f > Power) && (lLPower - 0.1f > Power)) {
    // } else if ((lPower - 0.3f > Power)) {
        if (powerT > 5) {
            if (flag1) {
                SolarInCircuitV -= stepV1;
                flag1 = 0;
            } else {
                SolarInCircuitV += stepV1;
                flag1 = 1;
            }
        } else {
            if (flag1) {
                SolarInCircuitV -= stepV2;
                flag1 = 0;
            } else {
                SolarInCircuitV += stepV2;
                flag1 = 1;
            }
        }
    }

    if (SolarInCircuitV > 20.0f) {
        SolarInCircuitV = 20.0f;
    }
    else if (SolarInCircuitV < 16.0f) {
        SolarInCircuitV = 16.0f;
    }

    // lLLPower = lLPower;
    lLPower = lPower;
    lPower = Power;
}

/**
  * @brief  停止充电，并打开启动任务
  * @param
  * @retval
  *
  */
void endChargWork(void)
{
    setChargControlFlag(FALSE);
    setDutyRatioToZero();
    setMPPT_Mode(noWork);
    beginStartControlTask();
}

/**
  * @brief  停止充电，不开启启动任务
  * @param
  * @retval
  *
  */
void stopChargWork(void)
{
    setChargControlFlag(FALSE);
    setDutyRatioToZero();
    setMPPT_Mode(noWork);
}

/**
  * @brief  启动充电，开启启动任务
  * @param
  * @retval
  *
  */
void beginChargWork(void)
{
    beginStartControlTask();
}

/**
  * @brief  启动充电，直接软启动
  * @param
  * @retval
  *
  */
void startChargWork(void)
{
    beginSoftStartTask();
}


/**
  * @brief  判断达到停止充电的条件
  * @param
  * @retval TRUE    达到停止充电
  *         FALSE   未达到
  *
  */
BOOL stopChargConditions(void)
{
    if (getSolarInCircuitVoltage() < g_cfgParameter.stopSolarOutputCircuitV
            && getChargCurrent() < 1) {
        // log_info("in stopChargConditions stopChargWork");
        return TRUE;
    }

    return FALSE;
}

/**
  * @brief  判断达到浮充充电的条件
  * @param
  * @retval TRUE    达到
  *         FALSE   未达到
  *
  */
BOOL floatChargConditions(void)
{
    if ((g_cfgParameter.constantVoltageChargeV < getBatteryVoltage() && g_cfgParameter.floatI > getChargCurrent())) {
        return TRUE;
    }    
    
    return FALSE;
}

/**
  * @brief  判断达到最大功率充电的条件
  * @param
  * @retval TRUE    达到
  *         FALSE   未达到
  *
  */
BOOL mpptChargConditions(void)
{
    if (((g_cfgParameter.constantVoltageChargeV - 0.2f) > getBatteryVoltage())
            && (getChargCurrent() > 0.1f)) {
        return TRUE;
    }

    return FALSE;
}

/**
  * @brief  判断达到恒压充电的条件
  * @param
  * @retval TRUE    达到
  *         FALSE   未达到
  *
  */
BOOL constantVChargConditions(void)
{
    if ((g_cfgParameter.constantVoltageV < getBatteryVoltage())
           && ((g_cfgParameter.floatI + 0.1f) <= getChargBatteryCurrent())) {
        return TRUE;
    }
    
    return FALSE;
}

/**
  * @brief  判断充电控制的模式
  * @param
  * @retval      
  *
  */
void chargControlMode(void)
{
    if (stopChargConditions()) {
        endChargWork();
    }

    else if (getBatteryState() == FALSE) {
        setMPPT_Mode(noBattery);
    }

    else if (floatChargConditions()) {
        setMPPT_Mode(floatCharg);
    }

    else if (constantVChargConditions()) {
        setMPPT_Mode(constantVoltage);
    }

    else if (mpptChargConditions()) {
        setMPPT_Mode(MPPT);
    }    
}

/**
  * @brief  判断有无电池
  * @param
  * @retval
  *
  */
void judgeYNBattery(void)
{
    if (getBatteryVoltage() > 16 || getBatteryVoltage() < 10) {
        setBatteryState(FALSE);
        return;
    }
    // if (getOutputVoltage() > 16 || getOutputVoltage() < 10) {
    //     setBatteryState(FALSE);
    //     return;
    // }
}

/**
  * @brief  无电池时控制
  * @param
  * @retval      
  *
  */
void noBatteryChargControl(void)
{
    mppt_constantVoltageNoBatteryO(g_cfgParameter.FloatChargeV);
}

/**
  * @brief  最大功率充电
  * @param
  * @retval      
  *
  */
void mpptCharge(void)
{
    mppt_readJust();
}

/**
  * @brief  恒压充电
  * @param
  * @retval      
  *
  */
void constantVoltageCharge(void)
{
    mppt_constantVoltageO(g_cfgParameter.constantVoltageChargeV);
}

/**
  * @brief  浮充充电
  * @param
  * @retval      
  *
  */
 void floatCharge(void)
 {
    mppt_constantVoltageO(g_cfgParameter.FloatChargeV);
 }

/**
  * @brief  有电池时控制
  * @param
  * @retval      
  *
  */
void BatteryChargControl(void)
{
    switch(getMPPT_Mode()) {

    case MPPT:
        mpptCharge();
        // mppt_constantVoltage(17.0f);
        break;

    case constantVoltage:
        constantVoltageCharge();
        break;

    case floatCharg:
        floatCharge();
        break;

    default:
        setMPPT_Mode(noWork);
        // stopChargWork();
        endChargWork();
        break;
    }
}

BOOL getChargControlFlag(void)
{
    return chargControlFlag;
}

/**
  * @brief  设置充电控制的标志位
  * @param  TRUE    启动充电控制
  *         FALSE   关闭充电控制
  * @retval
  *
  */
void setChargControlFlag(BOOL state)
{
    if (state == TRUE || state == FALSE) {
        chargControlFlag = state;
        // debug_printf("chargControlFlag : %d", state);
    }

    if (state == TRUE) {
        chargRunLed(runLedChargMode);
        // debug_printf("setChargControlFlag is true");
    } else if (state == FALSE) {
        chargRunLed(runLedOtherMode);
        // debug_printf("setChargControlFlag is false");
    }
}

/**
  * @brief  完成充电控制
  * @param
  * @retval
  *
  */
void bl_chargControl(void)
{
    setBatteryVoltage();
    
    if (getChargControlFlag() == FALSE) {
        return;
    }

    // getCVData();
    judgeYNBattery();

    chargControlMode();

    if (getMPPT_Mode() == noWork) {
        return;
    }

    if (getBatteryState()) {
        BatteryChargControl();
    } else {
        noBatteryChargControl();
    }    
    // noBatteryChargControl();
}