/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: madesheng
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/
#include "decision.h"
#include "../common/message.h"
#include "decisiondatatype.h"
#include <strstream>
#include <string.h>
#include <canbus/AccPedalActCtrReq.h>
#include <canbus/BrkPedalActCtrReq.h>
#include <canbus/SftActCtrReq.h>
#include <canbus/StrActCtrReq.h>
#include <decision/localizationMsg.h>
#include <decision/rulesMsg.h>
#include <decision/controlMsg.h>
#include <stdlib.h>
#include <math.h>


namespace robot {
namespace decisionSpace {



std::string Decision::Name() const { return "decision"; }

int Decision::Init(){

    LOG(INFO)<<Name()<<": init() start...";
    node_handle_.reset(new ros::NodeHandle());
    DecisionDatabase::InitDB();
    iCommandCmd = CMD_UNDEFIED;// set undefied mode
    iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
    iPredictionPointsPerNumber = 5;
    uiCarMode = CTR_MD_REQ_ROBOT_UNDIFED; // undefied
    fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
    PlanRouteName = "siasun";
//    for (int i =0; i< 10; i++)
//        TestMPC();
    LOG(INFO)<<Name()<<" init() end.";
    return ROBOT_SUCCESS;

}

int Decision::Start(){
    LOG(INFO)<<Name()<<" start() start...";
    // under ros service server
    ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
                                                                &Decision::CommandRequestService, this);
    // under ros subscriber
    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
                                                          &Decision::RTKLocationCallback, this);

    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 1, &Decision::OBDCallback, this);

    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 1, &Decision::RCUModeCallback, this);

    // under ros publish
    Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 100);

    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 100);

    Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 100);

    // under ros service of client
    clientSftActCtrReq = node_handle_->serviceClient<canbus::SftActCtrReq>(robot::common::ShiftActCtrService);

    clientAccPedalActCtrReq = node_handle_->serviceClient<canbus::AccPedalActCtrReq>(robot::common::AccPedalActCtrService);
    clientBrkPedalActCtrReq = node_handle_->serviceClient<canbus::BrkPedalActCtrReq>(robot::common::BrkPedalActCtrService);

    clientStrActCtrReq = node_handle_->serviceClient<canbus::StrActCtrReq>(robot::common::SteerActCtrService);

    ros::AsyncSpinner spinner(3); // thread_count The number of threads to use.  A value of 0 means to use the number of processor cores
    spinner.start();
    ros::waitForShutdown();
    LOG(INFO)<<Name()<<"  start() end.";
    return ROBOT_SUCCESS;
}

void Decision::Stop(){
    LOG(INFO)<<Name()<<": stop start...";
    LOG(INFO)<<Name()<<": stop end.";
    return ;
}

//  执行自动驾驶模式和路径规划模式，自动驾驶模式踩刹车然后挂前进当；路径规划模式删除数据库记录
bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
                      decision::CommandModeCmd::Response &res)
{
    LOG(INFO)<<Name()<<"CommandRequestService get command ID:."<< req.Cmd;
    if (req.Cmd == CMD_AUTODRIE_START)
    {
        LoadPlanRoute();
        throttle_Last = -70; // 踩刹车到70%
        ros::Rate loop_rate(1);
        while(ros::ok())
        {
            if (SpeedControl(throttle_Last) == 1)
            {
                ros::Duration(2).sleep();  // sleep for two second
                LOG(INFO)<<Name()<<"CommandRequestService break control 70% excute success.";
                if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
                    break;
            }
            else
            {
                LOG(ERROR)<<Name()<<"CommandRequestService break control 70% excute error.";
                return false;
            }
            ros::spinOnce();
            loop_rate.sleep();
        }
        ptsx.clear();
        ptsy.clear();
        ptsx.push_back(0);
        ptsy.push_back(0);
        ptsx.push_back(0);
        ptsy.push_back(0);
        fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
    }
    else if (req.Cmd == CMD_PLAN_START)
    {
        DecisionDatabase::DeleteActionDirective(PlanRouteName.c_str());
        DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
        iAbandon = fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
    }

    res.CmdResult = iCommandCmd = req.Cmd;
    LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
    return true;
}


// 加载规划路径，同时把规划的路径轨迹和测试的用例发送给显示界面
void Decision::LoadPlanRoute()
{
    ilocalTotalNumbers = DecisionDatabase::countOfLocationRuleInfo();
    iRulesTotalNumbers = DecisionDatabase::countOfActionDirective();
    LOG(INFO)<<Name()<<" LoadPlanRoute select ilocalTotalNumbers: "<<ilocalTotalNumbers<<"  iRulesTotalNumbers: "<<iRulesTotalNumbers;
    std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(0, ilocalTotalNumbers);
    for (int i = 0; i< ilocalTotalNumbers; i++)
    {
        decision::localizationMsg localMsg;
        localMsg.Locationid = tempInfo[i].LocationID;
        localMsg.Longitude = tempInfo[i].LocationLon;
        localMsg.Latitude = tempInfo[i].LocationLat;
        localMsg.Height = tempInfo[i].LocationHeight;
        localMsg.LocationSpeed = tempInfo[i].LocationSpeed;
        Geometry_pub.publish(localMsg);
    }
    std::vector<ACTION_DIRECTIVE> tempAction = DecisionDatabase::QueryActionDirective(0, iRulesTotalNumbers);
    for (int i = 0; i< iRulesTotalNumbers; i++)
    {
        decision::rulesMsg ruleMsg;
        ruleMsg.DirectiveID = tempAction[i].DirectiveID;
        ruleMsg.DirectiveInfo = tempAction[i].DirectiveInfo;
        ruleMsg.DirectiveResult = tempAction[i].DirectiveResult;
        Rules_pub.publish(ruleMsg);
    }

}

// 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
{
    switch (iCommandCmd) {
    case CMD_PLAN_START:
        ExcutePlanRoute(msg);
        break;
    case CMD_AUTODRIE_START:
        ExcuteSelfDriving(msg);
        break;
    case CMD_PLAN_END:
        StopPlanRoute(msg);
        break;
    case CMD_AUTODRIE_END:
        CarAutoDriveStop();
        break;
    default:
        break;
    }
}


void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
{
    fCarNowSpeed = msg->VehSpd;
    LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
}

void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
{
   uiCarMode = msg->CtrMdStat;
   //LOG(INFO)<<Name()<< " car excute mode :"<<uiCarMode;
}

//自动生成测试用例
void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
{
    if (iAbandon< 40){ // 2 second excute
        iAbandon ++;
        return;
    }
    else{
        iAbandon = 0;
    }

    try {
        iLocalNowNumber ++;
        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
        iRulesNowNumber++;
        std::stringstream strRuleContent;
        if (iRulesNowNumber == 1)
            strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
        else{
            if (fCarNowSpeed > fCarLastSpeed){
                strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
            }
            else if (fCarNowSpeed < fCarLastSpeed){
                strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
            }
            else if (fCarNowSpeed == fCarLastSpeed){
                strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
            }
       }
       int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
                        strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
                        msg->Latitude, msg->Height, fCarNowSpeed);
       if (iRet < 0) {
            iLocalNowNumber--;
            iRulesNowNumber--;
       }
       else{
           decision::localizationMsg localMsg;
           localMsg.Locationid = iLocalNowNumber;
           localMsg.Longitude = msg->Longitude;
           localMsg.Latitude = msg->Latitude;
           localMsg.Height = msg->Height;
           localMsg.LocationSpeed = fCarNowSpeed;
           localMsg.AngleDeviated = msg->AngleDeviated;
           Geometry_pub.publish(localMsg);
       }
       fCarLastSpeed = fCarNowSpeed;

    }
    catch (ros::Exception ex)
    {
        LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
    }
}

// 自动生成测试用例停止
void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
{
     LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber;
     iCommandCmd = CMD_UNDEFIED;// set undefied mode
     iLocalNowNumber++;
     iRulesNowNumber++;
     std::stringstream strRuleContent;
     if (fCarNowSpeed > fCarLastSpeed){
         strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
     }
     else if (fCarNowSpeed < fCarLastSpeed){
         strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
     }
     else if (fCarNowSpeed == fCarLastSpeed){
         strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
     }
    int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
                     strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
                     msg->Height, fCarNowSpeed);
    if (iRet < 0) {
         LOG(ERROR)<<Name()<<"StopPlanRoute insert iLocalNowNumber = "<< iLocalNowNumber<< "  last record error";
         iLocalNowNumber--;
         iRulesNowNumber--;
    }
}

// 执行自动驾驶ＭＰＣ算法
void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
{
   // 判断当前RCU是否为自动驾驶模式
    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE){
        LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
        return;
    }
    LOG(INFO)<<Name()<<" ilocalTotalNumbers:"<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
    if (CarAutoDriveStop())
        return;
    int iPredictPoints = iPredictionPointsPerNumber;
    double px = msg->Longitude;
    double py = msg->Latitude;
    bool bRetLon = (((ptsx[1] - ptsx[0])>= 0)&&((px - ptsx[1])> 0))||
            (((ptsx[1] - ptsx[0])< 0)&&((px - ptsx[1])< 0));
    bool bRetLat = (((ptsy[1] - ptsy[0])>= 0)&&((py - ptsy[1])> 0))||
            (((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
    if (bRetLon|| bRetLat){
        std::vector<LOCATION_RULE_INFO> tempInfo;
        tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
        ptsx.clear();
        ptsy.clear();
        for (int i= 0; i< iPredictPoints; i++)
        {
           ptsx.push_back( tempInfo[i].LocationLon);
           ptsy.push_back( tempInfo[i].LocationLat);
        }
        mpc.ref_speed = tempInfo[1].LocationSpeed;
        iLocalNowNumber ++;
        UpdataCarRuleResults();
        LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route  altered.";
    }
    else{
        LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
    }
    double psi = msg->AngleDeviated + 90;
    double v =  (double)fCarNowSpeed;
    MPC_Controller(ptsx, ptsy, px, py, psi, v);

}

void Decision::TestMPC()
{
    std::vector<LOCATION_RULE_INFO> tempInfo;
    tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, 5);
    ptsx.clear();
    ptsy.clear();
    for (int i= 0; i< 5; i++)
    {
       ptsx.push_back( tempInfo[i].LocationLon);
       ptsy.push_back( tempInfo[i].LocationLat);
    }
    mpc.ref_speed = tempInfo[1].LocationSpeed;
    double psi = deg2rad(230 + 90);
    double v =  (double)fCarNowSpeed;
    MPC_Controller(ptsx, ptsy, ptsx[1], ptsy[1], psi, v);
}

// 自动驾驶停止
bool  Decision::CarAutoDriveStop()
{
    try{

        if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
            iCommandCmd = CMD_AUTODRIE_END;
            int breakCtr = -70; // 踩刹车到70%
            //int breakCtr = (iRulesTotalNumbers -iRulesNowNumber) *10 -70; // 踩刹车到70%

            //iLocalNowNumber ++;
            while(true){
                if (SpeedControl(breakCtr) == 1){
                    ros::Duration(2).sleep();  // sleep for two second
                    if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
                        break;
                }
            }
            if (iRulesNowNumber == iRulesTotalNumbers)
                return true;
            //if (iRulesNowNumber == iRulesTotalNumbers){
               //iCommandCmd = CMD_UNDEFIED;
               //return true;
           // }

            iRulesNowNumber ++;
            decision::rulesMsg ruleMsg;
            if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
            {
                 ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
            }
            else
            {
                ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
            }
            int iResult = ruleMsg.DirectiveResult;
            DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
            ruleMsg.DirectiveID = iRulesNowNumber;
            Rules_pub.publish(ruleMsg);
            return true;
        }
        else
            return false;
    }
    catch (ros::Exception ex)
    {
        LOG(ERROR)<<Name()<<":CarAutoDriveStop error."<< ex.what();
        return false;
    }
}


bool Decision::UpdataCarRuleResults()
{
    try{
        iRulesNowNumber ++;
        decision::rulesMsg ruleMsg;
        if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
        {
             ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
        }
        else
        {
            ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
        }
        int iResult = ruleMsg.DirectiveResult;
        DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
        ruleMsg.DirectiveID = iRulesNowNumber;
        Rules_pub.publish(ruleMsg);
        return true;
    }
    catch (ros::Exception ex)
    {
        LOG(ERROR)<<Name()<<"  UpdataCarRuleResults error."<< ex.what();
        return false;
    }
}


void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
                              double &px, double &py,  double &psi, double &v)
{
     // 转换坐标到当前车辆坐标
     Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
     Eigen::VectorXd Ptsx = waypoints.row(0);
     Eigen::VectorXd Ptsy = waypoints.row(1);
//     for (unsigned i=0; i < ptsx.size(); ++i) {
//         LOG(INFO)<<Name()<< " lon: "<< Ptsx(i)<< " lat: "<<Ptsy(i);
//     }
     // 获取状态值
     auto coeffs = polyfit(Ptsx, Ptsy, 3);
     double cte = evaluateCte(coeffs);
     double epsi = evaluateEpsi(coeffs);
     LOG(INFO)<<Name()<< " cte: "<< cte<< " epsi: "<<epsi;
     Eigen::VectorXd state(6);
     state << 0, 0, 0, v, cte, epsi;
     // 计算方向和油门，范围都在[-1, 1]之间
     // 这个地方有延迟，取第三个
     auto vars = mpc.Solve(state, coeffs);
     // 展示MPC预测的路径
     int steer_value = rad2deg(vars[0]);

     double acc = vars[1]; // 加速度
     int throttle_value = throttle_Last + acc*0.1*3.6*100;
//     if (throttle_Last>=0)
//         throttle_value =  abs(throttle_Now - throttle_Last) >=15? throttle_Last -15: throttle_Now;
//     else
//         throttle_value =  abs(throttle_Now - throttle_Last) >=15? throttle_Last +15: throttle_Now;
     throttle_Last = throttle_value;
     LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
     StreeControl(steer_value);
     SpeedControl(throttle_value);
     decision::controlMsg control;
     control.nowSpeed = fCarNowSpeed;
     control.refSpeed = mpc.ref_speed;
     control.steer =  steer_value;
     control.throttle = throttle_value;
     control.cte = cte;
     Control_pub.publish(control);
}

unsigned int Decision::SpeedControl(int &speed)
{
     canbus::AccPedalActCtrReq accsrv;
     canbus::BrkPedalActCtrReq brksrv;
     if (speed >0){
         accsrv.request.uiAccPsngCtrReq = speed *10;
         brksrv.request.uiBrkPsngCtrReq =0;
     }
     else{
         accsrv.request.uiAccPsngCtrReq = 0;
         brksrv.request.uiBrkPsngCtrReq = -speed *10;
     }
     accsrv.request.uiAccSpdCtrReq = 20;
     brksrv.request.uiBrkCtrMdReq = 1;
     brksrv.request.uiBrkSpdCtrReq = 90;
     brksrv.request.uiBrkFocCtrReq = 350;
     if (clientAccPedalActCtrReq.call(accsrv))
     {
         LOG(INFO)<<Name()<<" Speed Control service return success, value = "<<accsrv.response.uiReturn;
     }
     else
     {
         LOG(ERROR)<<Name()<<" Speed Control service return falil";
     }

     if (clientBrkPedalActCtrReq.call(brksrv))
     {
         LOG(INFO)<<Name()<<" Speed Control service return success, value = "<<brksrv.response.uiReturn;
     }
     else
     {
         LOG(ERROR)<<Name()<<" Speed Control service return falil";
     }
     int result = 0;
     if(brksrv.response.uiReturn == 1 && accsrv.response.uiReturn == 1)
     {
       result =1;
     }
    return result;
}


unsigned int Decision::StreeControl(int &streer)
{

     canbus::StrActCtrReq srv;
     srv.request.iStrAngCtrReq = streer *100;
     srv.request.uiStrCtrMdReq =1;
     srv.request.iStrSpdCtrReq = 290;
     srv.request.iStrTrqCtrReq = 50;
     if (clientStrActCtrReq.call(srv))
     {
         LOG(INFO)<<Name()<<" stree Control service return success, value = "<<srv.response.uiReturn;
     }
     else
     {
         LOG(ERROR)<<Name()<<" stree Control service return falil";
     }
     return srv.response.uiReturn;
}


unsigned int Decision::BreakCtrControl(unsigned int breakmode)
{
     canbus::SftActCtrReq srv;
     srv.request.uiSftPsngCtrReq = breakmode;
     srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
     srv.request.uiClchPsngCtrReq = 1;
     srv.request.iClchSpdCtrReq = 0;
     if (clientSftActCtrReq.call(srv))
     {
         LOG(INFO)<<Name()<<" break Control service return success, value = "<<srv.response.uiReturn;
     }
     else
     {
         LOG(ERROR)<<Name()<<" break Control service return falil";
     }
     return srv.response.uiReturn;
}

 // 根据输入值x,y解方程
 Eigen::VectorXd Decision::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
     assert(xvals.size() == yvals.size());
     assert(order >= 1 && order <= xvals.size() - 1);
     Eigen::MatrixXd A(xvals.size(), order + 1);
     for (int i = 0; i < xvals.size(); i++) {
         A(i, 0) = 1.0;
     }
     for (int j = 0; j < xvals.size(); j++) {
         for (int i = 0; i < order; i++) {
             A(j, i + 1) = A(j, i) * xvals(j);
         }
     }
     auto Q = A.householderQr();
     auto result = Q.solve(yvals);
     return result;
 }

// 计算CTE误差
 double Decision::evaluateCte(Eigen::VectorXd coeffs) {
   return polyeval(coeffs, 0);
 }

 // 计算Epsi误差
 double Decision::evaluateEpsi(Eigen::VectorXd coeffs) {
   return -atan(coeffs[1]);
 }

 // 把全局坐标转换为汽车坐标，以当前汽车的位置作为原点
 Eigen::MatrixXd Decision::transformGlobal2Vehicle(double x, double y, double psi,
        const vector<double> & ptsx, const vector<double> & ptsy) {
    assert(ptsx.size() == ptsy.size());
    unsigned len = ptsx.size();
    auto waypoints = Eigen::MatrixXd(2, len);
    for (auto i = 0; i < len; ++i) {
        waypoints(0, i) = cos(psi) * (ptsx[i] - x) + sin(psi) * (ptsy[i] - y);
        waypoints(1, i) = -sin(psi) * (ptsx[i] - x) + cos(psi) * (ptsy[i] - y);
    }
    return waypoints;
}

} // decisionspace
} // robot

