#include "pid.h"
#include <QtMath>


PID::PID()
{
    //-----------------------------------------
    // PID参数
    //-----------------------------------------

    kp = 4.0;
    ki = 0.02;
    kd = 2.0;

    integral = 0;
    lastError = 0;

    integralMax = 100;
}


//====================================================
// 加热控制
//====================================================

double PID::calculate(double target, double current)
{
    const double dt = 0.5;

    double error = target - current;

    //-----------------------------------------
    // 70℃以前
    //
    // 全功率快速升温
    //-----------------------------------------

    if(current <= 70.0)
    {
        integral = 0;
        lastError = error;

        return 100.0;
    }


    //-----------------------------------------
    // 70~77℃
    //
    // PID控温
    //-----------------------------------------

    if(current < 77.0)
    {

        //-----------------------------------------
        // 积分
        //-----------------------------------------

        integral += error * dt;

        integral = qBound(-integralMax,
                          integral,
                          integralMax);


        //-----------------------------------------
        // 微分
        //-----------------------------------------

        double derivative =
                (error - lastError) / dt;


        lastError = error;



        //-----------------------------------------
        // PID输出
        //-----------------------------------------

        double output =
                kp * error +
                ki * integral +
                kd * derivative;



        //-----------------------------------------
        // 71℃以后限制功率
        //-----------------------------------------

        if(current >= 72.0)
        {
            if(output > 50.0)
                output = 50.0;
        }


        //-----------------------------------------
        // 75℃以后进一步限制
        //-----------------------------------------

        if(current >= 75.0)
        {
            if(output > 20.0)
                output = 20.0;
        }


        return qBound(0.0,
                      output,
                      100.0);
    }



    //-----------------------------------------
    // 77℃保护
    //-----------------------------------------

    integral = 0;
    lastError = error;

    return 0.0;
}



//====================================================
// 第一组风扇控制
//====================================================

double PID::calculateFanDuty(double temp)
{
    double duty;


    //-----------------------------------------
    // 70℃以前
    //
    // 小风量均温
    //-----------------------------------------

    if(temp <= 70.0)
    {
        duty = 20.0;
    }


    //-----------------------------------------
    // 70~72℃
    //
    // 20 -> 60%
    //-----------------------------------------

    else if(temp <= 72.0)
    {
        duty =
        20.0 +
        (temp - 70.0) / 2.0 * 40.0;
    }


    //-----------------------------------------
    // 72~75℃
    //
    // 60 -> 100%
    //-----------------------------------------

    else if(temp <= 75.0)
    {
        duty =
        60.0 +
        (temp - 72.0) / 3.0 * 40.0;
    }


    //-----------------------------------------
    // 75℃以上
    //-----------------------------------------

    else
    {
        duty = 100.0;
    }


    return qBound(20.0,
                  duty,
                  100.0);
}



//====================================================
// 第二组风扇控制
// 保持原逻辑
//====================================================

double PID::calculateFanDuty1(double temp)
{
    double duty;


    //-----------------------------------------
    // 30℃以下
    //-----------------------------------------

    if(temp <= 30.0)
    {
        duty = 10.0;
    }


    //-----------------------------------------
    // 30~40℃
    //-----------------------------------------

    else if(temp <= 40.0)
    {
        duty =
        (temp - 30.0) / 10.0 * 40.0;
    }


    //-----------------------------------------
    // 40~45℃
    //-----------------------------------------

    else if(temp <= 45.0)
    {
        duty =
        50.0 +
        (temp - 40.0) / 5.0 * 50.0;
    }


    //-----------------------------------------
    // 45℃以上
    //-----------------------------------------

    else
    {
        duty = 100.0;
    }


    return qBound(0.0,
                  duty,
                  100.0);
}