#include "vehicle_control.h"

#include <math.h>

#include "modulecomm.h"
#include "remotectrl.pb.h"

extern std::string gstrserverip;
extern std::string gstrcontrolPort;
extern std::string gstrcontrolInterval;
extern std::string gstruploadMapInterval;
extern std::string gstrid;
extern std::string gstrplateNumber;

extern char stryamlpath[256];

extern uint8_t gCtrlMode_Status; //0 auto 1 remote 2 stop 3 platform

using org::jeecg::defsControl::grpc::Empty; ///< other message
using org::jeecg::defsControl::grpc::GPSPoint;
using org::jeecg::defsControl::grpc::MapPoint;

using org::jeecg::defsControl::grpc::CtrlMode; ///< other enum
using org::jeecg::defsControl::grpc::ShiftStatus;

VehicleControlClient::VehicleControlClient(std::shared_ptr<Channel> channel)
{
    stub_ = VehicleControl::NewStub(channel);

    dec_yaml(stryamlpath);
    shmRemoteCtrl.mpa = iv::modulecomm::RegisterSend(shmRemoteCtrl.mstrmsgname,shmRemoteCtrl.mnBufferSize,shmRemoteCtrl.mnBufferCount);
}

VehicleControlClient::~VehicleControlClient(void)
{
    if(shmRemoteCtrl.mpa != nullptr)iv::modulecomm::Unregister(shmRemoteCtrl.mpa);
    requestInterruption();
    while(this->isFinished() == false);
}

void VehicleControlClient::dec_yaml(const char *stryamlpath)
{
    YAML::Node config;
    try
    {
        config = YAML::LoadFile(stryamlpath);
    }
    catch(YAML::BadFile &e)
    {
        std::cout<<e.what()<<std::endl;
        std::cout<<"yaml file load fail."<<std::endl;
        return;
    }
    catch(YAML::ParserException &e)
    {
        std::cout<<e.what()<<std::endl;
        std::cout<<"yaml file is malformed."<<std::endl;
        return;
    }

    std::string strmsgname;

    if(config["remote_ctrl"])
    {
        if(config["remote_ctrl"]["msgname"]&&config["remote_ctrl"]["buffersize"]&&config["remote_ctrl"]["buffercount"])
        {
            strmsgname = config["remote_ctrl"]["msgname"].as<std::string>();
            strncpy(shmRemoteCtrl.mstrmsgname,strmsgname.data(),255);
            shmRemoteCtrl.mnBufferSize = config["remote_ctrl"]["buffersize"].as<int>();
            shmRemoteCtrl.mnBufferCount = config["remote_ctrl"]["buffercount"].as<int>();
            std::cout << "RemoteCtrl:" << shmRemoteCtrl.mstrmsgname << "," << shmRemoteCtrl.mnBufferSize << "," << shmRemoteCtrl.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "remotectrl";
        strncpy(shmRemoteCtrl.mstrmsgname,strmsgname.data(),255);
        shmRemoteCtrl.mnBufferSize = 10000;
        shmRemoteCtrl.mnBufferCount = 1;
    }

    return;
}

std::string VehicleControlClient::vehicleControl(void)
{
    // Data we are sending to the server.
    Empty request;
    request.set_id(gstrid);

    // Container for the data we expect from the server.
    ControlReply reply;

    // Context for the client. It could be used to convey extra information to
    // the server and/or tweak certain RPC behaviors.
    ClientContext context;
    gpr_timespec timespec;
    timespec.tv_sec = 1;
    timespec.tv_nsec = 0;
    timespec.clock_type = GPR_TIMESPAN;
    context.set_deadline(timespec);

    // The actual RPC.
    Status status = stub_ -> vehicleControl(&context,request,&reply);

    // Act upon its status.
    if (status.ok()) {
        if(reply.id() == gstrid)
        {
            shiftCMD = reply.shiftcmd();
            steeringWheelAngleCMD = reply.steeringwheelanglecmd();
            throttleCMD = reply.throttlecmd();
            brakeCMD = reply.brakecmd();
        }
        else
        {
//            std::cout<<"id:"<<reply.id()<<std::endl;
//            if(throttleCMD > 0)throttleCMD -= 1.5;
//            if(throttleCMD <= 0)throttleCMD = 0;
//            if(brakeCMD > 0)brakeCMD -= 1.5;
//            if(brakeCMD <= 0)brakeCMD = 0;
        }
        return "vehicleControl RPC successed";
    } else {
        std::cout << status.error_code() << ": " << status.error_message()
                  << std::endl;
        if(status.error_code() == 4)
        {
            std::cout << "vehicleControl RPC connect timeout" << std::endl;
        }
        shiftCMD = ShiftStatus::SHIFT_DRIVE;
        steeringWheelAngleCMD = 0.0;
        throttleCMD = 0.0;
        brakeCMD = 100.0;
        return "vehicleControl RPC failed";
    }
}

void VehicleControlClient::updateControlData(void)
{
#if 0
    std::cout<<"shift:"<<shiftCMD<<std::endl;
    std::cout<<"steeringWheelAngle:"<<steeringWheelAngleCMD<<std::endl;
    std::cout<<"throttle:"<<throttleCMD<<std::endl;
    std::cout<<"brake:"<<brakeCMD<<std::endl;
#endif
//    std::cout<<"\n"<<"shift:"<<shiftCMD<<std::endl;
//    std::cout<<"steeringWheelAngle:"<<steeringWheelAngleCMD<<std::endl;
//    std::cout<<"throttle:"<<throttleCMD<<std::endl;
//    std::cout<<"brake:"<<brakeCMD<<"\n"<<std::endl;

    iv::remotectrl xmsg;
    if(modeCMD == CtrlMode::CMD_REMOTE || modeCMD == CtrlMode::CMD_CLOUD_PLATFORM)
        xmsg.set_ntype(iv::remotectrl::CtrlType::remotectrl_CtrlType_REMOTE);
    else if(modeCMD == CtrlMode::CMD_AUTO)
        xmsg.set_ntype(iv::remotectrl::CtrlType::remotectrl_CtrlType_AUTO);
    else if(modeCMD == CtrlMode::CMD_EMERGENCY_STOP)
        xmsg.set_ntype(iv::remotectrl::CtrlType::remotectrl_CtrlType_STOP);

    if(modeCMD == CtrlMode::CMD_REMOTE || modeCMD == CtrlMode::CMD_CLOUD_PLATFORM)
    {
        xmsg.set_acc(throttleCMD);
        xmsg.set_brake(brakeCMD);
        xmsg.set_wheel((-1)*steeringWheelAngleCMD/5.5f);

        if(shiftCMD == org::jeecg::defsControl::grpc::ShiftStatus::SHIFT_DRIVE)
            xmsg.set_shift(1);
        else if(shiftCMD == org::jeecg::defsControl::grpc::ShiftStatus::SHIFT_REVERSE)
            xmsg.set_shift(-1);
        else
            xmsg.set_shift(0);
    }
    else if(modeCMD == CtrlMode::CMD_AUTO)
    {
        xmsg.set_acc(0.0);
        xmsg.set_brake(0.0);
        xmsg.set_wheel(0.0);
        xmsg.set_shift(0); //real shift decided by controller
    }
    else // CMD_STOP
    {
        xmsg.set_acc(0.0);
        xmsg.set_brake(100.0);
        xmsg.set_wheel(0.0);
        xmsg.set_shift(0); //real shift decided by controller
    }

    int ndatasize = xmsg.ByteSize();
    char * str = new char[ndatasize];
    std::shared_ptr<char> pstr;pstr.reset(str);
    if(!xmsg.SerializeToArray(str,ndatasize))
    {
        std::cout<<"updateControlData serialize error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(shmRemoteCtrl.mpa,str,ndatasize);
}

void VehicleControlClient::run()
{
    QTime xTime;
    xTime.start();
    int lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstrcontrolInterval.c_str());
    while (!QThread::isInterruptionRequested())
    {
        if(abs(xTime.elapsed() - lastTime)>=interval)
        {
            std::string reply = vehicleControl();
//            std::cout<< reply <<std::endl;
            updateControlData();
            lastTime = xTime.elapsed();
        }
    }
}

void VehicleControlClient::ctrlMode_Changed_Slot(org::jeecg::defsControl::grpc::CtrlMode ctrlMode)
{
    modeCMD = ctrlMode;
}

VehicleChangeCtrlModeClient::VehicleChangeCtrlModeClient(std::shared_ptr<Channel> channel)
{
    stub_ = VehicleControl::NewStub(channel);
}

VehicleChangeCtrlModeClient::~VehicleChangeCtrlModeClient(void)
{
    requestInterruption();
    while(this->isFinished() == false);
}

std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
{
    // Data we are sending to the server.
    Empty request;
    request.set_id(gstrid);

    // Container for the data we expect from the server.
    CtrlModeReply reply;

    // Context for the client. It could be used to convey extra information to
    // the server and/or tweastd::cout<<"shift:"<<shiftCMD<<std::endl;k certain RPC behaviors.
    ClientContext context;
    gpr_timespec timespec;
    timespec.tv_sec = 1;
    timespec.tv_nsec = 0;
    timespec.clock_type = GPR_TIMESPAN;
    context.set_deadline(timespec);

    // The actual RPC.
    Status status = stub_ -> changeCtrlMode(&context,request,&reply);

    // Act upon its status.
    if (status.ok()) {
        if(reply.id() == gstrid)
        {
            modeCMD = reply.modecmd();
        }
        return "changeCtrlMode RPC successed";
    } else {
        std::cout << status.error_code() << ": " << status.error_message()
                  << std::endl;
        if(status.error_code() == 4)
        {
            std::cout << "vehicleControl RPC connect timeout" << std::endl;
        }
        modeCMD = CtrlMode::CMD_EMERGENCY_STOP;
        return "changeCtrlMode RPC failed";
    }
}

void VehicleChangeCtrlModeClient::updateCtrolMode(void)
{
//    std::cout<<"modeCMD:"<<modeCMD<<std::endl;
    gCtrlMode_Status = modeCMD;
    emit ctrlMode_Changed(modeCMD);
}

void VehicleChangeCtrlModeClient::run()
{
    QTime xTime;
    xTime.start();
    int lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstrcontrolInterval.c_str()) + 5; // move 5 ms to avoid ctrl data request
    while (!QThread::isInterruptionRequested())
    {
        if(abs(xTime.elapsed() - lastTime)>=interval)
        {
            std::string reply = changeCtrlMode();
//            std::cout<< reply <<std::endl;
            updateCtrolMode();
            lastTime = xTime.elapsed();
        }
    }
}

VehicleUploadMapClient::VehicleUploadMapClient(std::shared_ptr<Channel> channel)
{
    stub_ = VehicleControl::NewStub(channel);
}

VehicleUploadMapClient::~VehicleUploadMapClient(void)
{
    requestInterruption();
    while(this->isFinished() == false);
}

std::string VehicleUploadMapClient::uploadMap(void)
{
    // Data we are sending to the server.
    Empty request;
    request.set_id(gstrid);

    // Container for the data we expect from the server.
    UploadMapReply reply;

    // Context for the client. It could be used to convey extra information to
    // the server and/or tweak certain RPC behaviors.
    ClientContext context;
    gpr_timespec timespec;
    timespec.tv_sec = 2;
    timespec.tv_nsec = 0;
    timespec.clock_type = GPR_TIMESPAN;
    context.set_deadline(timespec);

    // The actual RPC.
    Status status = stub_ -> UploadMap(&context,request,&reply);

    // Act upon its status.
    if (status.ok()) {
        if(reply.id() == gstrid)
        {
//            std::cout<<reply.id()<<std::endl;
            isNeedMap = false;
            patrolPathID = "noPath";
            POIPoints.clear();

            isNeedMap = reply.isneedmap();
//            std::cout<<reply.isneedmap()<<std::endl;
            patrolPathID = reply.patrolpathid();
//            std::cout<<reply.patrolpathid()<<std::endl;
            for(int i = 0;i < reply.mappoints_size();i++)
            {
                POIPoints.append(reply.mappoints(i));
//                std::cout<<reply.mappoints(i).index()<<std::endl;
            }
        }
        return "UploadMap RPC successed";
    } else {
        std::cout << status.error_code() << ": " << status.error_message()
                  << std::endl;
        if(status.error_code() == 4)
        {
            std::cout << "vehicleControl RPC connect timeout" << std::endl;
        }
        return "UploadMap RPC failed";
    }
}

void VehicleUploadMapClient::updateMapPOIData(void)
{
    std::cout<<"isNeedMap:"<<isNeedMap<<std::endl;
    std::cout<<"patrolPathID:"<<patrolPathID<<std::endl;
    emit patrolPOI_Recieved(patrolPathID);
}

void VehicleUploadMapClient::run()
{
    QTime xTime;
    xTime.start();
    int lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstruploadMapInterval.c_str());
    while (!QThread::isInterruptionRequested())
    {
        if(abs(xTime.elapsed() - lastTime)>=interval)
        {
            if(isNeedMap == false)
            {
                std::string reply = uploadMap();
//                std::cout<< reply <<std::endl;
                if(isNeedMap == true)
                {
                    updateMapPOIData();
                }
            }
            lastTime = xTime.elapsed();
        }
    }
}

void VehicleUploadMapClient::uploadPath_Finished_Slot(std::string pathID)
{
    std::cout<<"Path ID:"<<pathID<<" upload finished"<<std::endl;
    if(isNeedMap == true)
    {
        isNeedMap = false;
    }
}