/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-10-16 14:04:53
 * @LastEditors: zjk 1553836110@qq.com
 * @LastEditTime: 2023-11-02 15:17:38
 * @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.cpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
#include"Controler.hpp"

Controler::Controler()
{
    // Error_capacity = 10;
    initVariable();
}

void Controler::initVariable()
{
    ClearValTim(desired_speed);
    ClearValTim(desired_heading);
    ClearValTim(desired_depth);
    ClearValTim(desired_pitch);

    ClearValTim(current_speed);
    ClearValTim(current_heading);
    ClearValTim(current_pitch);
    ClearValTim(current_depth);
    current_time = 0;
    start_time = 0;

    ClearValTim(desired_thrust);
    ClearValTim(desired_rudder);
    ClearValTim(desired_elevator);

    iterations = 0;
    start_time = 0.0;

    tardy_helm_thresh = 2.0;
    tardy_nav_thresh  = 2.0;
    // current_error = 0;
    // last_error = 0;
    // Error.clear();
}

bool Controler::setDesSpeed(double spd, double tim)
{
    desired_speed.value = spd;
    desired_speed.time = tim;
    return true;
}
bool Controler::setDesDepth(double dph, double tim)
{
    desired_depth.value = dph;
    desired_depth.time = tim;
    return true;
}
bool Controler::setDesPitch(double pth, double tim)
{
    desired_pitch.value = pth;
    desired_pitch.time = tim;
    return true;
}
bool Controler::setDesHeading(double hdg, double tim)
{
    if(is_rad)
        hdg = radToDegrees(hdg);

    desired_heading.value = angle180(hdg);
    desired_heading.time = tim;
    return true;
}
bool Controler::setCurSpeed(double spd, double tim)
{
    current_speed.value = spd;
    current_speed.time = tim;
    return true;
}
bool Controler::setCurDepth(double dph, double tim)
{
    current_depth.value = dph;
    current_depth.time = tim;
    return true;
}
bool Controler::setCurPitch(double pth, double tim)
{
    current_pitch.value = pth;
    current_pitch.time = tim;
    return true;
}
bool Controler::setCurHeading(double hdg, double tim)
{
    current_heading.value = hdg;
    current_heading.time = tim;
    return true;
}
bool Controler::setDesiredValues()
{
    iterations++;

    desired_thrust.value = 0;
    desired_rudder.value = 0;
    desired_elevator.value = 0;

    //=============================================================
    // Part 1: Ensure all nav and helm messages have been received 
    // for first time. If not, we simply wait, without declaring an
    // override. After all messages have been received at least
    // once, staleness is checked below.
    //=============================================================
    runMsg.push_back("desired_heading-time:" + doubleToString(desired_heading.time));
    runMsg.push_back("desired_speed-time:" + doubleToString(desired_speed.time));
    runMsg.push_back("current_heading-time:" + doubleToString(current_heading.time));
    runMsg.push_back("current_speed-time:" + doubleToString(current_speed.time));
    runMsg.push_back("desired_depth-time:" + doubleToString(desired_depth.time));
    runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
    runMsg.push_back("current_pitch-time:" + doubleToString(current_pitch.time));
    // runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));

    if(has_speedCtrl)
    {
        if((desired_heading.time == 0) || (desired_speed.time == 0))
            return true;
    }
    if(has_headCtrl)
    {
        if((current_heading.time == 0) || (current_speed.time == 0))
            return true;
    }
    if(has_depthCtrl) 
    {
        if(desired_depth.time == 0)
            return true;
        if((current_depth.time == 0) || (current_pitch.time == 0))
            return true;
    }
    //=============================================================
    // Part 2: Critical info staleness (if not in simulation mode)
    //=============================================================
    //TODO: 验证信息过时操作
    if(cheakStalensee) 
    {
        bool is_stale = checkForStaleness();
        if(is_stale) 
        {
            has_override = true;
            return false;
        }
    }

    //=============================================================
    // Part 3: Set the actuator values. Note: If desired thrust is 
    // zero others desired values will automatically be zero too.
    //=============================================================
    
    // m_desired_thrust = setDesiredThrust();

    // if(m_desired_thrust > 0)
    // m_desired_rudder = setDesiredRudder();

    // if(m_desired_thrust > 0 && m_depth_control) 
    // m_desired_elevator = setDesiredElevator();
    return true;
}
//TODO： 添加操控权判断
bool Controler::overrived(string sval)
{
    if(tolower(sval) == "true")
    {
        if(has_override == false)
            addPosting("HAS_CONTROL", "false");    
        has_override = true;
    }
    else if(tolower(sval) == "false") 
    {
    // Upon lifting an override, the timestamps are reset. New values
    // for all will need to be received before desired_* outputs will
    // be produced.  If we do not reset, it's possible we may
    // interpret older pubs as being stale but they may have been
    // paused also during an override.
        if(has_override == true) 
        {
            desired_speed.time = 0;
            desired_heading.time = 0;
            desired_depth.time = 0;
            desired_pitch.time = 0;

            current_speed.time = 0;
            current_heading.time = 0;
            current_pitch.time = 0;
            current_depth.time = 0;
        }
        has_override = false;
        addPosting("HAS_CONTROL", "true");
    }
    return true;
}

bool Controler::checkForStaleness()
{
    bool is_stale = false;

    // =========================================================
    // Part 1: Check for Helm staleness
    // =========================================================
    double hdg_delta = (current_time - desired_heading.time);
    if(hdg_delta > tardy_helm_thresh) {
    string staleness = doubleToStringX(hdg_delta,3);
    addPosting("PID_STALE", "Stale DesHdg:" + staleness);
    is_stale = true;
    }
    double spd_delta = (current_time - desired_speed.time);
    if(spd_delta > tardy_helm_thresh) {
    string staleness = doubleToStringX(spd_delta,3);
    addPosting("PID_STALE", "Stale DesSpd:" + staleness);
    is_stale = true;
    }

    // =========================================================
    // Part 2B: Check for Nav staleness
    // =========================================================
    double nav_hdg_delta = (current_time - current_heading.time);
    if(nav_hdg_delta > tardy_nav_thresh) {
    string staleness = doubleToStringX(nav_hdg_delta,3);
    addPosting("PID_STALE", "Stale NavHdg:" + staleness);
    is_stale = true;
    }
    double nav_spd_delta = (current_time - current_speed.time);
    if(nav_spd_delta > tardy_nav_thresh) {
    string staleness = doubleToStringX(nav_spd_delta,3);
    addPosting("PID_STALE", "Stale NavSpd:" + staleness);
    is_stale = true;
    }
    // =========================================================
    // Part 2C: If depth control, check for Helm Depth staleness 
    // =========================================================
    if(has_depthCtrl) {
    double dep_delta = (current_time - desired_depth.time);
    if(dep_delta > tardy_helm_thresh) {
        string staleness = doubleToStringX(dep_delta,3);
        addPosting("PID_STALE", "Stale DesDepth:" + staleness);
        is_stale = true;
    }
    }

    // =========================================================
    // Part 2D: If depth control, check for Nav Depth staleness 
    // =========================================================
    if(has_depthCtrl) {    
    double nav_dep_delta = (current_time - current_depth.time);
    if(nav_dep_delta > tardy_nav_thresh) {
        string staleness = doubleToStringX(nav_dep_delta,3);
        addPosting("PID_STALE", "Stale NavDep:" + staleness);
        is_stale = true;
    }
    double nav_pit_delta = (current_time - current_pitch.time);
    if(nav_pit_delta > tardy_nav_thresh) {
        string staleness = doubleToStringX(nav_pit_delta,3);
        addPosting("PID_STALE", "Stale NavPitch:" + staleness);
        is_stale = true;
    }
    }
    return(is_stale);
}

// bool Controler::setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator)
// {
//     limit_thrust[0] = min_thrush;
//     limit_thrust[1] = max_thrush;

//     limit_rudder[0] = min_rubber;
//     limit_rudder[1] = max_rudder;

//     limit_elevator[0] = min_elevator;
//     limit_elevator[1] = max_elevator;
// }

bool Controler::Limit(double &a, double max, double min)
{
    a = (a < min) ? min : a;
    a = (a > max) ? max : a;
    return true;
}

bool Controler::setCtrl(bool speed, bool heading, bool depth)
{
    has_speedCtrl = speed;
    has_headCtrl = heading;
    has_depthCtrl = depth;
    return true;
}

void Controler::addPosting(std::string var, std::string sval)
{
    VarDataPair pair(var, sval);
    postings.push_back(pair);
}

void Controler::addPosting(std::string var, double val)
{
    VarDataPair pair(var, val);
    postings.push_back(pair);
}

double Controler::getFrequency() const
{
    double elapsed = current_time - start_time;

    double frequency = 0;
    if(elapsed > 0) 
    frequency = ((double)(iterations)) / elapsed;

    return(frequency);
}

bool Controler::ClearValTim(vlaTim &a)
{
    if(&a == NULL)
        return false;
    else
    {
        a.value = 0;
        a.time = 0;
    }

}
//TODO: 控制器参数配置函数
list<std::string> Controler::setConfigParams(std::list<std::string>)
{
  list<string> unhandled_params;
  list<string>::iterator p;
//   for(p=param_lines.begin(); p!=param_lines.end(); p++) {
//     string orig  = *p;
//     string line  = tolower(orig);
//     string param = biteStringX(line, '=');
//     if(param == "speed_factor")
//       m_config_params.push_front(orig);
//     else if(param == "simulation")
//       m_config_params.push_front(orig);
//     else if(param == "sim_instability")
//       m_config_params.push_front(orig);
//     else if(param == "tardy_helm_threshold")
//       m_config_params.push_front(orig);
//     else if(param == "tardy_nav_threshold")
//       m_config_params.push_front(orig);
//     else if(param == "yaw_pid_kp")
//       m_config_params.push_front(orig);
//     else if(param == "yaw_pid_kd")
//       m_config_params.push_front(orig);
//     else if(param == "yaw_pid_ki")
//       m_config_params.push_front(orig);
//     else if(param == "yaw_pid_integral_limit")
//       m_config_params.push_front(orig);
//     else if(param == "yaw_pid_ki_limit")
//       m_config_params.push_front(orig);
//     else if(param == "maxrudder")
//       m_config_params.push_front(orig);
//     else if(param == "heading_debug")
//       m_config_params.push_front(orig);

//     else if(param == "speed_pid_kp")
//       m_config_params.push_front(orig);
//     else if(param == "speed_pid_kd")
//       m_config_params.push_front(orig);
//     else if(param == "speed_pid_ki")
//       m_config_params.push_front(orig);
//     else if(param == "speed_pid_integral_limit")
//       m_config_params.push_front(orig);
//     else if(param == "maxthrust")
//       m_config_params.push_front(orig);
//     else if(param == "speed_debug")
//       m_config_params.push_front(orig);

//     else if(param == "depth_control")
//       m_config_params.push_front(orig);
//     else if(param == "z_to_pitch_pid_kp")
//       m_config_params.push_front(orig);
//     else if(param == "z_to_pitch_pid_kd")
//       m_config_params.push_front(orig);
//     else if(param == "z_to_pitch_pid_ki")
//       m_config_params.push_front(orig);
//     else if(param == "z_to_pitch_pid_integral_limit")
//       m_config_params.push_front(orig);
//     else if(param == "maxpitch")
//       m_config_params.push_front(orig);
//     else if(param == "depth_debug")
//       m_config_params.push_front(orig);

//     else if(param == "pitch_pid_kp")
//       m_config_params.push_front(orig);
//     else if(param == "pitch_pid_kd")
//       m_config_params.push_front(orig);
//     else if(param == "pitch_pid_ki")
//       m_config_params.push_front(orig);
//     else if(param == "pitch_pid_integral_limit")
//       m_config_params.push_front(orig);
//     else if(param == "maxelevator")
//       m_config_params.push_front(orig);

//     else
//       unhandled_params.push_front(orig);    
//   }
  return(unhandled_params);
}

Json::Value Controler::getConfig(string path)
{  
    ifstream ifs;
    ifs.open(path, ios::in);
    Json::Reader taskConfigureReader;
    Json::Value inputJsonValue;
    taskConfigureReader.parse(ifs, inputJsonValue);
    ifs.close();
    return inputJsonValue;
}

void Controler::setCheakStalensee(string s)
{
    if(s == "true")
        cheakStalensee = true;
    else
        cheakStalensee = false;
}