/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-10-16 14:05:16
 * @LastEditors: zjk 1553836110@qq.com
 * @LastEditTime: 2023-11-02 14:31:16
 * @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.hpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
#ifndef __COBTROLLER_H
#define __CONTROLLER_H
#include<iostream>
#include<vector>
#include "VarDataPair.h"
#include "MBUtils.h"
#include "AngleUtils.h"
#include <list>
#include "json/json.h"
#include <fstream>
#include <map>

// #include "VarDataPair.h"

using namespace std;

typedef struct
{
    double value;
    double time;
} vlaTim;

class Controler
{
    public:
    Controler();
    ~Controler(){};

    virtual int step(){return 0;}
    virtual bool setConfigParams(){return false;}
    virtual bool overrived(string sval);

    virtual bool handleYawSettings(){return false;}
    virtual bool handleSpeedSettings(){return false;};
    virtual bool handleDepthSettings(){return false;};
    virtual bool hasConfigSettings() const{return false;};
    // bool setError(double err);

    bool setDesSpeed(double spd, double tim);
    bool setDesDepth(double dph, double tim);
    bool setDesPitch(double pth, double tim);
    bool setDesHeading(double hdg, double tim);

    bool setCurSpeed(double spd, double tim);
    bool setCurDepth(double dph, double tim);
    bool setCurPitch(double pth, double tim);
    bool setCurHeading(double hdg, double tim);
    // bool setCurTime(double tim){current_time = tim;}
    void updateTime(double tim){current_time = tim;}
    double getCurTime(){return current_time;}
    void setStartTime(double tim){start_time = tim;}
    void setOverride(bool v){has_override = v;}
    void setTardyHelm(double t){ tardy_helm_thresh = t;}
    void setTardyNav(double t){ tardy_nav_thresh = t;}
    void setCheakStalensee(string s);
    void setConstThrust(double v){const_thrust = v;}
    void setDeadZone(double v){dead_zone=v;}
    void setDepthControl(bool v){has_depthCtrl=v;}
    void setSpeedControl(bool v){has_speedCtrl=v;}
    void setHeadingControl(bool v){has_headCtrl=v;}
    bool setDesiredValues();

    // bool setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator);
    bool setCtrl(bool speed, bool heading, bool depth);
    bool Limit(double &a, double max, double min);

    bool checkForStaleness();
    void addPosting(std::string var, std::string sval);
    void addPosting(std::string var, double val);
    vector<VarDataPair> getPostings() {return(postings);}
    void clearPostings() {postings.clear();}
    bool ClearValTim(vlaTim &a);
    list<std::string> setConfigParams(std::list<std::string>); //使用MOOS风格配置参数函数
    Json::Value getConfig(string path);

    double getFrequency() const;
    bool hasControl(){return(!has_override);}
    bool hasSpdCtrl(){return has_speedCtrl;}
    bool hasDphCtrl(){return has_depthCtrl;}
    bool hasHdgCtrl(){return has_headCtrl;}

    double getDesiredRudder() const   {return(desired_rudder.value);}
    double getDesiredThrust() const   {return(desired_thrust.value);}
    double getDesiredElevator() const {return(desired_elevator.value);}
    // bool setErrorCap(int c){Error_capacity = c;}

    inline void initVariable();
    list<string> getConfigParams(){return config_params;}
    vector<string> getConfigErrors(){return config_errors;}
    vector<string> getConfigInfo(){return config_info;}
    vector<string> getRunMsg(){return runMsg;}
    Json::Value getReport(){return RepList;}
    void ClearRunMsg(){runMsg.clear();}

    protected:
    vlaTim desired_speed;
    vlaTim desired_heading;
    vlaTim desired_depth;
    vlaTim desired_pitch;

    vlaTim current_speed;
    vlaTim current_heading;
    vlaTim current_pitch;
    vlaTim current_depth;
    double current_time;
    double start_time;

    vlaTim desired_thrust;
    vlaTim desired_rudder;
    vlaTim desired_elevator;

    double limit_thrust[2];
    double limit_rudder[2];
    double limit_elevator[2];
    double max_rudder;
    double max_thrust;
    double max_pitch;
    double max_elevator;

    bool has_speedCtrl = true;
    bool has_headCtrl = true;
    bool has_depthCtrl = true;
    bool has_override = true;
    bool cheakStalensee = true; 

    vector<VarDataPair> postings;
    unsigned int iterations;

    double tardy_helm_thresh;
    double tardy_nav_thresh;
    double const_thrust;
    double dead_zone;
    


    double is_rad = false;

    list<string> config_params;
    vector<string> config_errors;
    vector<string> config_info;
    vector<string> runMsg;
    Json::Value RepList;  
};

#endif