#ifndef _PID_SOURCE_
#define _PID_SOURCE_

#include <iostream>
#include <cmath>
#include <QDebug>
#include "pid.h"

using namespace std;

#define STAGE_INIT (0)
#define STAGE_LOW  (1)
#define STAGE_MID  (2)
#define STAGE_HIGH (3)

class PIDImpl
{
    public:
        PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki);
        PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki, double maxI);
        PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki, double maxI, double maxIstep, double maxDstep);
        ~PIDImpl();
        double calculate( double setpoint, double pv );
        double calculate_v2( double setpoint, double pv );
        void reset();

    private:
        double _dt;
        double _max;
        double _min;
        double _Kp;
        double _Kd;
        double _Ki;
        double _pre_error;
        double _integral;
        double _maxI;
        double _maxIstep;
        double _maxDstep;
        double _pre_output;

        double _start_pv;
        double _critical_output;
        int    _stage;
};

PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki)
{
    pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
}

PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki, double maxI)
{
    pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki,maxI);
}

PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki, double maxI, double maxIstep, double maxDstep)
{
    pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki,maxI, maxIstep, maxDstep);
}

double PID::calculate( double setpoint, double pv )
{
    return pimpl->calculate(setpoint,pv);
}

double PID::calculate_v2( double setpoint, double pv )
{
    return pimpl->calculate_v2(setpoint,pv);
}

void PID::reset()
{
    return pimpl->reset();
}

PID::~PID()
{
    delete pimpl;
}


/**
 * Implementation
 */
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki) :
    _dt(dt),
    _max(max),
    _min(min),
    _Kp(Kp),
    _Kd(Kd),
    _Ki(Ki),
    _pre_error(0),
    _integral(0)
{
    _maxI = 0;
    _maxIstep = 0;
    _maxDstep = 0;

    _pre_output = 0;
    _start_pv = 0;
    _critical_output = 0;
    _stage = STAGE_INIT;
}

/**
 * Implementation
 */
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki, double maxI) :
    _dt(dt),
    _max(max),
    _min(min),
    _Kp(Kp),
    _Kd(Kd),
    _Ki(Ki),
    _pre_error(0),
    _integral(0),
    _maxI(maxI)
{
    _maxIstep = 0;
    _maxDstep = 0;
    _pre_output = 0;
    _start_pv = 0;
    _critical_output = 0;
    _stage = STAGE_INIT;
}

/**
 * Implementation
 */
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki, double maxI, double maxIstep, double maxDstep) :
    _dt(dt),
    _max(max),
    _min(min),
    _Kp(Kp),
    _Kd(Kd),
    _Ki(Ki),
    _pre_error(0),
    _integral(0),
    _maxI(maxI),
    _maxIstep(maxIstep),
    _maxDstep(maxDstep)
{
    _pre_output = 0;
    _start_pv = 0;
    _critical_output = 0;
    _stage = STAGE_INIT;
}


double PIDImpl::calculate( double setpoint, double pv )
{

    // Calculate error
    double error = setpoint - pv;

    // Proportional term
    double Pout = _Kp * error;

    // Integral term
    if(setpoint < 0.05){
        _integral += error * _dt;
    }


    double Iout = _Ki * _integral;

    //积分限幅
    if(_maxI > 0){
        if(Iout > _maxI){
            Iout = _maxI;
        }else if(Iout < -_maxI){
            Iout = -_maxI;
        }
    }


    // Derivative term
    double derivative = (error - _pre_error) / _dt;
    double Dout = _Kd * derivative;

     qDebug("calculate : pOut : %f, Iout: %f, Dout: %f ", Pout, Iout, Dout);
    // Calculate total output
    //double output = Pout + Iout + Dout;
    double output = Pout + Iout ;//+ Dout;

    // Save error to previous error
    _pre_error = error;

    if(_maxIstep > 0 && output - _pre_output > _maxIstep){
        output = _pre_output + _maxIstep;
    }else if(_maxDstep > 0 && _pre_output - output > _maxDstep){
        output = _pre_output - _maxDstep;
    }

    // Restrict to max/min
    if( output > _max )
        output = _max;
    else if( output < _min )
        output = _min;

    _pre_output = output;

    return output;
}

double PIDImpl::calculate_v2( double setpoint, double pv )
{

    // Calculate error
    double error = setpoint - pv;

    // Proportional term
    double Pout = _Kp * error;

    // Integral term
    _integral += error * _dt;

    double Iout = _Ki * _integral;

    // Derivative term
    double derivative = (error - _pre_error) / _dt;
    double Dout = _Kd * derivative;

     qDebug("calculate_V2 : pOut : %f, Iout: %f, Dout: %f ", Pout, Iout, Dout);
    // Calculate total output
    //double output = Pout + Iout + Dout;
    double output = Pout + Iout ;//+ Dout;

    // Restrict to max/min
    if( output > _max )
        output = _max;
    else if( output < _min )
        output = _min;

    // Save error to previous error
    _pre_error = error;

    //belows add by avansguo
    if(STAGE_INIT == _stage){
        _start_pv = pv;
        _stage = STAGE_LOW;
    }else{
        if(pv < _start_pv){
            _start_pv = pv;
            qDebug("calculate _start_pv: %f ", _start_pv);
        }
    }

    if(_critical_output < 1.0){
        if(derivative <= (-0.02)){ //压力开始增加了，记录前值作为临界值
            _critical_output = _pre_output;
            qDebug("calculate _critical_output: %f ", _critical_output);
        }
    }

    double _distance = 0;
    if(setpoint - _start_pv < 0.5){
        _distance = 1;
    }else{
        _distance = error/(setpoint - _start_pv);
    }


    if(_distance <= 0.2){
        output = _critical_output + (_distance+0.1)*( _pre_output - _critical_output);
    }else if((_distance > 0.2) &&(_distance <= 0.5)){

        if(derivative > (-0.02)){ //压力不变或在下降

        }else if(derivative < (-0.2)){
            output = _pre_output+Dout;
            _integral = output;
        }{
            output = _pre_output;
            _integral = output;
        }

    }else{

        if(derivative < (-0.2)){
            output = _pre_output+Dout;
            _integral = output;
        }else if(derivative > (-0.02)){ //压力不变或在下降
            //解决当 开始压力与初始压力差距太小， 积分太慢的问题
            if(output-_pre_output < 2){
                output = _pre_output+2;
                _integral = output;
            }
        }else{
            output = _pre_output;
            _integral = output;
        }

        //

    }

    _pre_output = output;

    return output;
}

void PIDImpl::reset()
{
    _pre_error = 0;
    _integral = 0;
    _pre_output = 0;
    _start_pv = 0;
    _critical_output = 0;
    _stage = STAGE_INIT;
}

PIDImpl::~PIDImpl()
{
}

#endif