/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-10-12 09:52:27
 * @LastEditors: zjk 1553836110@qq.com
 * @LastEditTime: 2023-11-07 12:01:57
 * @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
// #define DEBUG
#include"MotionControler.hpp"
//TODO：增加启用哪个控制器功能
bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
{
  AppCastingMOOSApp::OnNewMail(NewMail);
  MOOSMSG_LIST::iterator p;
  for(p=NewMail.begin(); p!=NewMail.end(); p++)
  {
    CMOOSMsg &msg = *p;

    string key  = msg.m_sKey;
    string sval = msg.m_sVal;
    double dval = msg.m_dfVal;
    double dfT;
    msg.IsSkewed(m_curr_time, &dfT);
    if(fabs(dfT) < ok_skew) 
    {
      if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
        pengine.overrived(sval);
      else if(key == "DESIRED_HEADING")
        pengine.setDesHeading(dval, MOOSTime());
      else if(key == "DESIRED_SPEED")
        pengine.setDesSpeed(dval, MOOSTime());
      else if(key == "DESIRED_DEPTH") 
        pengine.setDesDepth(dval, MOOSTime());
      else if(key == "NAV_HEADING")
        pengine.setCurHeading(angle360(dval), MOOSTime());
      else if(key == "NAV_SPEED")
        pengine.setCurSpeed(dval, MOOSTime());
      else if(key == "NAV_DEPTH")
        pengine.setCurDepth(dval, MOOSTime());
      else if(key == "NAV_PITCH")
        pengine.setCurPitch(dval, MOOSTime());      
      else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
      {
        int e = pengine.setParam(configFilePath);
        pengine.setOverride(true);
        if(e != 0)
          faultCode = 10 + e;
        else
          faultCode = 0;
      }
    }
  }
    return true;
}
bool MotionControler::Iterate()
{
    bool a;
    AppCastingMOOSApp::Iterate();
    pengine.updateTime(m_curr_time);
    int i = pengine.step();
    switch (i)
    {
      case 0:
        RepList["State : "] = "Run";
        break;
      case 1:
        RepList["State : "] = "Ready";
        break;      
      case -1:
        RepList["State : "] = "Fault";
        faultCode = 1;//信息超时
        break;  
      default:
        break;
    }
    postPengineResults();
    postPenginePostings();
    postCharStatus();
    Notify(MSG_FALUT,faultCode);
    AppCastingMOOSApp::PostReport(); 
    return true;
}
bool MotionControler::OnConnectToServer()
{
    registerVariables();
    return true;
}
bool MotionControler::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();
  pengine.setStartTime(MOOSTime());
  STRING_LIST sParams;
  m_MissionReader.GetConfiguration(GetAppName(), sParams);
  //pengine.setConfigParams(sParams);
  //bool handled = true;
  STRING_LIST::iterator p;
  for(p=sParams.begin(); p!=sParams.end(); p++) {
      string orig  = *p;
      string line  = (orig); //识别大小写
      string param = biteStringX(line, '=');
      string value = line;
      double dval  = atof(value.c_str());
      if(param == "config_file")
        configFilePath = value;
      else if(param == "tardy_helm_thresh")
        pengine.setTardyHelm(dval);
      else if(param == "tardy_nav_thresh")
        pengine.setTardyNav(dval);
      else if(param == "cheak_stalensee")
        pengine.setCheakStalensee(value);
      else if(param == "AppTick")
        setFrequency = dval;
      else if(param == "delta_freqency")
        frequency_delta = dval;
      else
        reportUnhandledConfigWarning(orig);
  }
  int e = pengine.setParam(configFilePath);
  if(e != 0)
  {
    faultCode = 10 + e;
    return false;
  }
  return true;
}

void MotionControler::registerVariables()
{
      AppCastingMOOSApp::RegisterVariables();

      Register("NAV_HEADING", 0);
      Register("NAV_SPEED", 0);
      Register("NAV_DEPTH", 0);
      Register("NAV_PITCH", 0);
      Register("DESIRED_HEADING", 0);
      Register("DESIRED_SPEED", 0);
      Register("DESIRED_DEPTH", 0);
      Register("PID_VERBOSE", 0);
      Register("SPEED_FACTOR", 0);
      Register("MOOS_MANUAL_OVERIDE", 0);
      Register("MOOS_MANUAL_OVERRIDE", 0);
      Register(MSG_ReadConfig,0);
}
bool MotionControler::buildReport()
{
  double frequency = pengine.getFrequency();
  double delta_freq =  100.0*(setFrequency - frequency) / setFrequency;
  if(abs(delta_freq) > frequency_delta)
    faultCode = 2;

  m_msgs << "Frequency Delta  : " << frequency  << endl;
  m_msgs << "PID has_control  : " << boolToString(pengine.hasControl()) << endl;
  m_msgs << "Config File Path : " << configFilePath << endl;
  m_msgs << "S : H : D        : | " << intToString(pengine.hasSpdCtrl())+ " | " 
  << intToString(pengine.hasHdgCtrl())+" | " 
  << intToString(pengine.hasDphCtrl())+" |" << endl;

  RepList["to BS"] = colVar;
  RepList["PID"] = pengine.getReport()["W"];
  string rep = Json::writeString(RepJsBuilder, RepList);
  m_msgs << rep << endl;

  return(true);
}

void MotionControler::postPenginePostings()
{
    vector<VarDataPair> m_postings = pengine.getPostings();
    for(unsigned int i=0; i<m_postings.size(); i++)
    {
        VarDataPair pair = m_postings[i];
        string var = pair.get_var();
        if(pair.is_string() && pair.get_sdata_set())
        Notify(var, pair.get_sdata());
        else if(!pair.is_string() && pair.get_ddata_set())
        Notify(var, pair.get_ddata());
    }
    pengine.clearPostings();
}
void MotionControler::postPengineResults()
{
  bool all_stop = true;
  if(pengine.hasControl())
      all_stop = false;

  if(all_stop) 
  {
    if(allstop_posted)
      return;
    Notify("DESIRED_RUDDER", 0.0);
    Notify("DESIRED_THRUST", 0.0);
    // if(pengine.hasDphCtrl())
    Notify("DESIRED_ELEVATOR", 0.0);
    postColVarToBS(0,0,0);
    allstop_posted = true;
  }
  else 
  {
    int T=0,S=0,R=0;
    if(pengine.hasHdgCtrl())
    {
      Notify("DESIRED_RUDDER", pengine.getDesiredRudder());
      R = (int)pengine.getDesiredRudder();
    }
    if(pengine.hasSpdCtrl())
    {
      Notify("DESIRED_THRUST", pengine.getDesiredThrust());
      T = (int)pengine.getDesiredThrust();
    }
      
    if(pengine.hasDphCtrl())
    {
      Notify("DESIRED_ELEVATOR", pengine.getDesiredElevator());
      S = pengine.getDesiredElevator();
    }
    postColVarToBS(T,S,R);
    allstop_posted = false;
  }
}
void MotionControler::postCharStatus()
{
  if(!verbose)
    return;

  if(pengine.hasControl())
    cout << "$" << flush;
  else 
    cout << "*" << flush;
}
void MotionControler::postColVarToBS(int T, int S, int R)
{
  string s_msg;
  colVar["mode"] = 2;
  colVar["thrust"] = T;
  colVar["rudder"] = R;
  colVar["elevator"] = S;
  s_msg = Json::writeString(RepJsBuilder,colVar);
  Notify(MSG_TO_BS, s_msg);
}