#include "vehicle_control.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 uint8_t gShift_Status;//3 p 4 r 5 n 6 d

namespace iv {
struct msgunit
{
    char mstrmsgname[256];
    int mnBufferSize = 10000;
    int mnBufferCount = 1;
    void * mpa;
    std::shared_ptr<char> mpstrmsgdata;
    int mndatasize = 0;
    bool mbRefresh = false;
    bool mbImportant = false;
    int mnkeeptime = 100;
};
}

extern iv::msgunit shmRemoteCtrl;

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);

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

VehicleControlClient::~VehicleControlClient(void)
{

}

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();
            std::cout<<"throttle:"<<reply.throttlecmd()<<std::endl;
            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;
        }
        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;
    gShift_Status = shiftCMD;

    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);
    }
    else
    {
        xmsg.set_acc(0.0);
        xmsg.set_brake(100.0);
        xmsg.set_wheel(0.0);
        xmsg.set_shift(0);
    }

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

void VehicleControlClient::run()
{
    QTime xTime;
    xTime.start();
    uint64_t lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstrcontrolInterval.c_str());
    while (true)
    {
        if((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)
{

}

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;
        }
        return "changeCtrlMode RPC failed";
    }
}

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

void VehicleChangeCtrlModeClient::run()
{
    QTime xTime;
    xTime.start();
    uint64_t lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstrcontrolInterval.c_str());
    while (true)
    {
        if((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)
{

}

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();
    uint64_t lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstrcontrolInterval.c_str());
    while (true)
    {
        if((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;
    }
}