/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: madesheng
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/
#ifndef ROBOT_ROBOT_DECISION_H
#define ROBOT_ROBOT_DECISION_H



#include "../common/robotapp.h"
#include "ros/ros.h"
#include "../common/sqlite3helper.h"
#include "../common/mpc.h"
#include "decisiondatabase.h"
#include <decision/CommandModeCmd.h>
#include <localization/localMsg.h>
#include <canbus/obdMsg.h>
#include <perception/si2Msg.h>
#include <vector>
#include <math.h>
//#include <eigen3/Eigen/Core>
//#include <eigen3/Eigen/QR>
#include "../common/Eigen-3.3/Eigen/Core"
#include "../common/Eigen-3.3/Eigen/QR"

namespace robot {
namespace decisionSpace {

#define ROBOT_SUCCESS     0
#define ROBOT_FAILTURE   -1



// 角度和弧度之间的转换
constexpr double pi() {
    return M_PI;
}

/**
* @class Decision
*
* @brief decision module main class.
* 本类是决策类
* It collect the control data from other model and send decison message to other model.
*/

class Decision: public robot::common::RobotApp{

public:
    Decision(){

    }
    /**
    * decision节点名称
    */
    std::string Name() const override;

    /**
    * decision模块初始化
    * @brief module initialization function
    * @return initialization status
    */
    int Init() override;

    /**
    * decision模块启动
    * @brief module start function
    * @return start status
    */
    int Start() override;

    /**
    * decision模块的停止
    * @brief module stop function.
    * @return start status
    */
    void Stop() override;

private:
    /**
    * RTK定位回调函数
    */
    void RTKLocationCallback(const localization::localMsg::ConstPtr& msg);

    /**
    * OBD车辆信息回调函数
    */
    void OBDCallback(const canbus::obdMsg::ConstPtr& msg);

    /**
    * RCU当前模式回调函数
    */
    void RCUModeCallback(const perception::si2Msg::ConstPtr& msg);

    /**
    * ICU当前执行命令服务
    */
    bool CommandRequestService(decision::CommandModeCmd::Request &req,
                        decision::CommandModeCmd::Response &res);

    /**
    * 执行保存规划路径
    */
    void ExcutePlanRoute(const localization::localMsg::ConstPtr& msg);

    /**
    * 停止规划路径
    */
    void StopPlanRoute(const localization::localMsg::ConstPtr& msg);

    /**
    * 执行自动驾驶任务
    */
    void ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg);


    void TestMPC();
    //速度控制
    unsigned int SpeedControl(int &speed);
    //方向控制
    unsigned int StreeControl(int &streer);
    //档位控制
    unsigned int BreakCtrControl(unsigned int breakmode);
    //加载规划路径
    void LoadPlanRoute();
    //车辆自动驾驶停止
    bool CarAutoDriveStop();
    //更新测试用例结果
    bool UpdataCarRuleResults();

    void MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
                        double &px, double &py,  double &psi, double &v);


    inline double deg2rad(double x) {
        return x * pi() / 180;
    }
    inline double rad2deg(double x) {
        return x * 180 / pi();
    }

    // 根据系数和x值获取y值
    inline double polyeval(Eigen::VectorXd coeffs, double x) {
        double result = 0.0;
        for (int i = 0; i < coeffs.size(); i++) {
            result += coeffs[i] * pow(x, i);
        }
        return result;
    }

    // 根据输入值x,y解方程
    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);

    // 计算CTE误差
    double evaluateCte(Eigen::VectorXd coeffs);
    // 计算Epsi误差
    double evaluateEpsi(Eigen::VectorXd coeffs);

    // 把全局坐标转换为汽车坐标，以当前汽车的位置作为远点
    Eigen::MatrixXd transformGlobal2Vehicle(double x, double y, double psi,
           const vector<double> & ptsx, const vector<double> & ptsy);

    /**
    * @brief timer callback function
    */
    void OnTimer(const ros::TimerEvent &event);

private:

    int iAbandon;

    std::string PlanRouteName;

    int32_t iCommandCmd; // excute mode 0: undefined 1: plan start 2: auto-drive start  3: plan stop 4: auto-drive stop

    float_t fCarNowSpeed;

    float_t fCarLastSpeed; //plan

    int64_t iLocalNowNumber; // plan or auto drive localization numbers

    int64_t ilocalTotalNumbers;

    int64_t iRulesNowNumber; // plan or auto drive rule numbers

    int64_t iRulesTotalNumbers;

    int32_t iPredictionPointsPerNumber;

    unsigned int uiCarMode;

    robot::common::MPC mpc;

    ros::Publisher Rules_pub;

    ros::Publisher Geometry_pub;

    ros::Publisher Control_pub;

    ros::ServiceClient clientAccPedalActCtrReq;
    ros::ServiceClient clientBrkPedalActCtrReq;

    ros::ServiceClient clientStrActCtrReq;

    ros::ServiceClient clientSftActCtrReq;

    std::vector<double> ptsy;
    std::vector<double> ptsx;

    std::vector<double> vPx;
    std::vector<double> vPy;

    int throttle_Last = 0;
    int steer_Last = 0;

};


}
}
#endif // ROBOT_ROBOT_DECISION_H
