#include <QCoreApplication>

#include <QTime>
#include <QMutex>
#include <QTimer>
#include "xmlparam.h"
#include "modulecomm.h"
#include "ivbacktrace.h"
#include "ivversion.h"
#include "decition.pb.h"
#include "chassis.pb.h"
#include <thread>
#include "include/car_control.h"

CarControl car_control_module;

void * gpadecition;

iv::brain::decition gdecition_def;
iv::brain::decition gdecition;

int gnDecitionNum = 0; //when is zero,send default;
const int gnDecitionNumMax = 100;

static QMutex gMutex;

bool gstatus;
GearPrkgAssistReq ggearSetVal;
GearLevelIndicate ggearRealVal;
ChassisErrCode gchassErr;
StsMach gstsMach;
float gsteerDeg, gspeed;
double lastspeedSetVal = 0;
double lastEpsSetVal = 0;

GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;

static void ShareChassis(void * pa,iv::chassis  * pchassis)
{
    char * str;
    int ndatasize = pchassis->ByteSize();
    str = new char[ndatasize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(!pchassis->SerializeToArray(str,ndatasize))
    {
        std::cout<<"ShareChassis Error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(pa,str,ndatasize);

}

void executeDecition(const iv::brain::decition decition)
{
    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;

    GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
    double speedSetVal = 0;
    double EpsSetVal = 0;

    if(decition.has_gear()){
        switch (decition.gear()) {
        case 1:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
            gearSetVal=GearPrkgAssistReq::kTargetGearP;
            break;
        case 2:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
            gearSetVal=GearPrkgAssistReq::kTargetGearR;
            break;
        case 3:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
            gearSetVal=GearPrkgAssistReq::kTargetGearD;
            break;
        default:
            //car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
            gearSetVal=GearPrkgAssistReq::kNoRequest;;
            break;
        }
        lastgearSetVal=gearSetVal;
    }
    else
    {
        gearSetVal=lastgearSetVal;
    }

    if(decition.has_accelerator()){
        speedSetVal=decition.accelerator();
        lastspeedSetVal=speedSetVal;
        //speedSetVal=0.1;
       // car_control_module.set_target_acc_mps2(decition.accelerator());
//        car_control_module.set_target_acc_mps2(0.1);
    }
    else
    {
        speedSetVal=lastspeedSetVal;
    }

    if(decition.has_wheelangle())
    {
       // car_control_module.set_target_pinion_ag_in_deg(0.0);
        EpsSetVal=decition.wheelangle();
        lastEpsSetVal=EpsSetVal;
       // EpsSetVal=0.0;//
  // car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
    }
    else
    {
        EpsSetVal=lastEpsSetVal;
    }
    // gearSetVal=GearPrkgAssistReq::kTargetGearD;
    car_control_module.set_target_gear(gearSetVal);
    car_control_module.set_target_acc_mps2(speedSetVal);
    car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);

    if(decition.has_leftlamp() && decition.leftlamp()==true)
        car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
    else if(decition.has_rightlamp() && decition.rightlamp()==true)
        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
    else
        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);

    /*
    void set_lat_lgt_ctrl_req(bool req);  // 握手请求, true:请求握手， false:退出握手
    void set_target_gear(GearPrkgAssistReq tar);  // 目标档位请求
    bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
    void set_target_pinion_ag_in_deg(float32_t ang_req);// 设置目标方向盘角度请求， -450°至+450°（车辆实测范围值），方向盘左正，右负。
    void set_target_acc_mps2(float32_t tar_acc);// 目标加减速度值。
    void set_turn_light_status(TurnLightIndicReq req);// 转向灯控制请求
*/
}




void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    qint64 oldtime;
    iv::brain::decition xdecition;

    if(!xdecition.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenDecition parse error."<<std::endl;
        return;
    }

    if(xdecition.gear() != 3)
    {
        qDebug("not D");
    }
    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
        // qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
        oldtime = QDateTime::currentMSecsSinceEpoch();
    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.unlock();
    gnDecitionNum = gnDecitionNumMax;
    std::cout<<"update decision. "<<std::endl;
}


void sendthread()
{
    iv::brain::decition xdecition;
//    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
//    car_control_module.sm_task_20ms();  // 线控状态机函数

    while(1)
    {
        if(gnDecitionNum <= 0)
        {
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            gMutex.lock();
            xdecition.CopyFrom(gdecition);
            gMutex.unlock();
            gnDecitionNum--;
        }

        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();

        if(bstatus == true)
        {
            //            std::cout<<"di pan ke kong "<<std::endl;
            executeDecition(xdecition);
        }
        else
        {
            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
            car_control_module.set_lat_lgt_ctrl_req(true);
        }

        car_control_module.sm_task_20ms();  // 线控状态机函数
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
}

void * gpa;

void recvthread()
{
    iv::chassis xchassis;
    int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
    while(1)
    {
        gstatus = car_control_module.is_lat_lgt_ctrl_active();
        ggearSetVal = car_control_module.get_setted_tar_gear();
        ggearRealVal = car_control_module.get_cur_disp_gear();
        gchassErr = car_control_module.get_chassis_err_state();
        gstsMach = car_control_module.get_chassis_statemachine_sts();
        gspeed = car_control_module.get_current_vehicle_spd_in_ms();
        gsteerDeg = car_control_module.get_current_steer_ang_in_deg();

        std::cout<<"FeedBack: current_vehicle_spd_in_ms is "<<gspeed<<"err code :"<<(int)gchassErr<<std::endl;


        xchassis.set_angle_feedback(gsteerDeg);

        ShareChassis(gpa,&xchassis);
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        /*
            bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
            ChassisErrCode get_chassis_err_state();// 底盘错误状态码
            StsMach get_chassis_statemachine_sts(); // 内部状态机运行状态
            float32_t get_current_steer_ang_in_deg();// 当前方向盘实际角度
            float32_t get_current_vehicle_spd_in_ms();// 当前车辆实际车速，单位ｍ／ｓ
            GearPrkgAssistReq get_setted_tar_gear(); // 获取当前设定目标档位值
            GearLevelIndicate get_cur_disp_gear(); // 当前实际显示档位状态
        */
    }
}

int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("controller_Geely_xingyueL");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

    if(argc < 2)
        strpath = strpath + "/controller_Geely_xingyueL.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

    //    car_control_module.set_lat_lgt_ctrl_req(true);
    //    car_control_module.sm_task_20ms();  // 线控状态机函数

    //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);

    iv::xmlparam::Xmlparam xp(strpath.toStdString());
    std::string gstrmemdecition = xp.GetParam("dection","deciton");
    std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");

    gpa = iv::modulecomm::RegisterSend(strchassismsgname.data(),1000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);

    std::thread xthread(sendthread);
//    std::thread myxthread(recvthread);

    return a.exec();
}