/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-10-16 15:14:15
 * @LastEditors: zjk 1553836110@qq.com
 * @LastEditTime: 2023-11-03 11:30:38
 * @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.cpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
#include"pidControl.hpp"
pidControl::pidControl()
{
    pidClear(pid_depth);
    pidClear(pid_heading);
    pidClear(pid_pitch);
    pidClear(pid_speed);
    setCurDepth(0,0);
    setDesDepth(0,0);
    setCurHeading(0,0);
    setDesHeading(0,0);
    setCurPitch(0,0);
    setDesPitch(0,0);
    setCurSpeed(0,0);
    setDesSpeed(0,0);
    const_thrust = 0;
}
int pidControl::step()
{
    if(!setDesiredValues())
    {
        //这里不对时间进行重置，保持故障状态
        pidClear(pid_depth);
        pidClear(pid_heading);
        pidClear(pid_pitch);
        pidClear(pid_speed);
        return -1; //Falut
    }
    if(has_override)
    {
        //由手动移交的控制权，需要对时间进行重置，防止进入故障状态
        pidClear(pid_depth);
        pidClear(pid_heading);
        pidClear(pid_pitch);
        pidClear(pid_speed);
        setCurDepth(0,0);
        setDesDepth(0,0);
        setCurHeading(0,0);
        setDesHeading(0,0);
        setCurPitch(0,0);
        setDesPitch(0,0);
        setCurSpeed(0,0);
        setDesSpeed(0,0);
        RepList["Waring"] = int(has_override);
        return 1; //Ready
    }
    RepList["Waring"] = int(has_override);
    string rpt = "";
    if(has_speedCtrl)
    {
        if(const_thrust == 0)
        {
            double speed_error = desired_speed.value - current_speed.value;
            desired_thrust.value = pidStep(speed_error, pid_speed);
            desired_thrust.time = current_time;
            //Limit(desired_thrust.value, max_thrust, 0);

            rpt = "PID_SPEED: ";
            rpt += " (Want):" + doubleToString(desired_speed.value);
            rpt += " (Curr):" + doubleToString(current_speed.value);
            rpt += " (Diff):" + doubleToString(speed_error);
            rpt += " (Delt):" + doubleToString(pid_speed.delta_output);
            rpt += " THRUST:" + doubleToString(desired_thrust.value);
            addPosting("PID_SPD_DEBUG", rpt);
            RepList["spd_pid"]["Want"] = doubleToString(desired_speed.value,2);
            RepList["spd_pid"]["Curr"] = doubleToString(current_speed.value,2);
            RepList["spd_pid"]["Diff"] = doubleToString(speed_error,2);
            RepList["spd_pid"]["Delt"] = doubleToString(pid_speed.delta_output,2);
            RepList["spd_pid"]["THRUST"] = doubleToString(desired_thrust.value,2);
        }
        else
        {
            desired_thrust.value = const_thrust;
            desired_thrust.time = current_time;
        }

    }
    if(desired_thrust.value <= dead_zone)
    {
        desired_rudder.value = 0;
        desired_rudder.time = current_time;
        desired_elevator.value = 0;
        desired_elevator.time = current_time;
        return true;
    }

    if(has_headCtrl)
    {   
        double heading_error = desired_heading.value - current_heading.value;
        heading_error = angle180(heading_error);
        desired_rudder.value = pidStep(heading_error, pid_heading);
        desired_rudder.time = current_time;
        
        //Limit(desired_rudder.value, max_rudder, -max_rudder);
        // Limit();

        rpt = "PID_COURSE: ";
        rpt += " (Want):" + doubleToString(desired_heading.value);
        rpt += " (Curr):" + doubleToString(current_heading.value);
        rpt += " (Diff):" + doubleToString(heading_error);
        rpt += " (Delt):" + doubleToString(pid_heading.delta_output);
        rpt += " RUDDER:" + doubleToString(desired_rudder.value);
        addPosting("PID_HDG_DEBUG", rpt);
        RepList["hdg_pid"]["Want"] = doubleToString(desired_heading.value,2);
        RepList["hdg_pid"]["Curr"] = doubleToString(current_heading.value,2);
        RepList["hdg_pid"]["Diff"] = doubleToString(heading_error,2);
        RepList["hdg_pid"]["Delt"] = doubleToString(pid_heading.delta_output,2);
        RepList["hdg_pid"]["RUDDER"] = doubleToString(desired_rudder.value,2);
    }
    if(has_depthCtrl)
    {
        double depth_error = desired_depth.value - current_depth.value;
        desired_pitch.value = -pidStep(depth_error, pid_depth);
        desired_pitch.time = current_time;
        //Limit(desired_pitch.value, max_pitch, -max_pitch);
        double pitch_error = desired_pitch.value - current_pitch.value;
        desired_elevator.value = pidStep(pitch_error, pid_pitch);
        desired_elevator.time = current_time;
        //Limit(desired_elevator.value, max_elevator, -max_elevator);
        
        rpt = "PID_DEPTH: ";
        rpt += " (Want):" + doubleToString(desired_depth.value);
        rpt += " (Curr):" + doubleToString(current_depth.value);
        rpt += " (Diff):" + doubleToString(depth_error);
        rpt += " ELEVATOR:" + doubleToString(desired_elevator.value);
        addPosting("PID_DEP_DEBUG", rpt);
        RepList["dep_pid"]["Want"] = doubleToString(desired_depth.value,2);
        RepList["dep_pid"]["Curr"] = doubleToString(current_depth.value,2);
        RepList["dep_pid"]["Diff"] = doubleToString(depth_error,2);
        RepList["dep_pid"]["Delt"] = doubleToString(pid_depth.delta_output,2);

        RepList["pth_pid"]["Want"] = doubleToString(desired_pitch.value,2);
        RepList["pth_pid"]["Curr"] = doubleToString(current_pitch.value,2);
        RepList["pth_pid"]["Diff"] = doubleToString(pitch_error,2);
        RepList["pth_pid"]["Delt"] = doubleToString(pid_pitch.delta_output,2);
        RepList["pth_pid"]["ELEVATOR"] = doubleToString(desired_elevator.value,2);
    }
    // Limit();
    return 0;
}

bool pidControl::setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid)
{
    // pid.kp = p + i + d;
    // pid.ki = -p - 2*d;
    // pid.kd = d;
    pid.kp = p;
    pid.ki = i;
    pid.kd = d;
    pid.limit_delta = limitDelta;
    pid.output = 0;
    pid.max_out = max;
    pid.min_out = min;
    return true;
}

double pidControl::pidStep(double error, pidInc &pid)
{
    double deltaOutPut = 0;
    pid.error_0 = error;
    deltaOutPut 
    = pid.kp * (pid.error_0 - pid.error_1                 ) 
    + pid.ki * (pid.error_0                               ) 
    + pid.kd * (pid.error_0 -2.0*pid.error_1 + pid.error_2);
    // RepList["W"]["d"] = deltaOutPut;
    // RepList["W"]["e1"] = pid.kp;
    // RepList["W"]["e2"] = pid.ki;
    // RepList["W"]["e3"] = pid.kd;
    Limit(deltaOutPut, pid.limit_delta, -pid.limit_delta);
    pid.output += deltaOutPut;
    Limit(pid.output, pid.max_out, pid.min_out);
    pid.delta_output = deltaOutPut;
    pid.error_2 = pid.error_1;
    pid.error_1 = pid.error_0;
    return pid.output;
}
// double  pidControl::picStep_p(double error, pidInc &pid)
// {
//     pid.delta_output = 
// }
bool pidControl::overrived(string svar)
{
    Controler::overrived(svar);
    return true;
}
//TODO: 快速检测当前参数是具有与当前控制器相关
bool pidControl::hasConfigSettings() const
{
    if(config_params.size() == 0)
        return(false);

    list<string>::const_iterator p;
    for(p=config_params.begin(); p!=config_params.end(); p++) 
    {
        string line = *p;
        string param = tolower(biteStringX(line, '='));

        // if(param == "yaw_pid_kp")
        //     return(true);
        // else if(param == "yaw_pid_kd")
        //     return(true);
        // else if(param == "yaw_pid_ki")
        //     return(true);
        // else if(param == "yaw_pid_integral_limit")
        //     return(true);
        // else if(param == "yaw_pid_ki_limit")
        //     return(true);
        return true;
    }

    return(false);
}

bool pidControl::setParam(char n, double param, pidInc *pid_t)
{
    if(pid_t == NULL)
        return false;
    
    switch (n)
    {
    case 'p':
        pid_t->kp = param;
        break;
    case 'i':
        pid_t->ki = param;
        break;
    case 'd':
        pid_t->kd = param;
        break;
    default:
        return false;
        break;
    }

    return true;

}
int pidControl::setParam(string filePath)
{
    Json::Value paramList = getConfig(filePath);
    if(paramList.empty())
        return 1;
    if(paramList["speed"].empty()       || paramList["pitch"].empty()
       || paramList["depth"].empty()    || paramList["pitch"].empty()
       || paramList["speedCol"].empty() ||  paramList["depthCol"].empty()
       || paramList["HeadingCol"].empty())
        return 2; //重要参数不全
    Json::Value param = paramList["speed"];
    if(!setParam(param, pid_speed))
        return 3;
    RepList["speed-param"] = param;
    param = paramList["heading"];
    if(!setParam(param, pid_heading))
        return 4;
    RepList["heading-param"] = param;
    param = paramList["pitch"];
    if(!setParam(param, pid_pitch))
        return 5;
    RepList["pitch-param"] = param;
    param = paramList["depth"];
    if(!setParam(param, pid_depth))
        return 6;
    RepList["depth-param"] = param;
    setSpeedControl(paramList["speedCol"].asBool());
    setDepthControl(paramList["depthCol"].asBool());
    setHeadingControl(paramList["HeadingCol"].asBool());
    //次要参数
    if(!paramList["dead_zone"].empty())
    {
        dead_zone = paramList["dead_zone"].asDouble();
        RepList["dead_zone"] = dead_zone;
    }
    if(!paramList["const_thrust"].empty())
    {
        const_thrust = paramList["const_thrust"].asDouble();
        RepList["const_thrust"] = const_thrust;
    }
    return 0;
}
bool pidControl::setParam(Json::Value param, pidInc &pid)
{
    if(param.empty())
        return false;
    if(param["Kp"].empty() || param["Ki"].empty() || param["Kd"].empty())
        return false;
    pid.kp = param["Kp"].asDouble();
    pid.ki = param["Ki"].asDouble();
    pid.kd = param["Kd"].asDouble();
    if(param["LimitDelta"].empty() || param["MaxOut"].empty() || param["MinOut"].empty())
        return false;
    pid.limit_delta = param["LimitDelta"].asDouble();
    pid.max_out = param["MaxOut"].asDouble();
    pid.min_out = param["MinOut"].asDouble();
    return true;
}