/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-10-16 15:14:26
 * @LastEditors: zjk 1553836110@qq.com
 * @LastEditTime: 2023-11-02 18:03:14
 * @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.hpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
#ifndef _PIDCONTROL_H
#define _PIDCONTROL_H
#include"Controler.hpp"

typedef struct
{
    double kp;
    double ki;
    double kd;

    double error_0;
    double error_1;
    double error_2;

    double limit_delta;
    double max_out;
    double min_out;

    double delta_output;
    double output;
} pidInc;


class pidControl : public Controler
{
private:
    /* data */
    pidInc pid_speed;
    pidInc pid_heading;
    pidInc pid_pitch;
    pidInc pid_depth;

    double current_error;
    double last_error;
    vector<double> Error;
    int Error_capacity;

public:
    pidControl(/* args */);
    ~pidControl(){};

    int step();
    
    bool setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid);
    bool setParam(char n, double param, pidInc *pid_t);
    int setParam(string filePath);
    bool setParam(Json::Value param, pidInc &pid);

    bool hasConfigSettings() const;
    
    double pidStep(double error, pidInc &pid);
    //double picStep_p(double error, pidInc &pid);

    bool overrived(string svar);

    inline void pidClear(pidInc &pid)
    {
        pid.error_0 = 0;
        pid.error_1 = 0;
        pid.error_2 = 0;
        pid.delta_output = 0;
        pid.output = 0;
    }

};

#endif