#include "mainwindow.h"
#include "ui_mainwindow.h"

iv::Ivlog *givlog;
unsigned int gv2xEn = false;

void ListenV2xStEn(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//    if(nSize < sizeof(iv::v2x::v2xStEn))
//    {
//        givlog->error("v2x","ListenV2xStEn size error %d",nSize,sizeof(iv::v2x::v2xStEn));
//        return;
//    }

    iv::v2x::v2xStEn xv2xStEnMsg;
    if(!xv2xStEnMsg.ParseFromArray(strdata,nSize))
    {
        givlog->error("iv::v2x::v2xStEn::ListenV2xStEn parse error");
        return;
    }
    gv2xEn = xv2xStEnMsg.v2xsten();

    givlog->info("v2x", "v2x enable st: %d", gv2xEn);
}

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    //Start Get init param
    QString strpath = QCoreApplication::applicationDirPath();
    qDebug()<<strpath;
        strpath = strpath + "/v2xTcpClient.xml";

    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    std::string strCarVIN = xp.GetParam("carVIN","catarc001");
    mscarID = QString::fromStdString(strCarVIN);
    std::string strHostIP = xp.GetParam("hostIP","47.95.196.28");
    std::string strHostPort = xp.GetParam("hostPort","12123");
    std::string strStationCount = xp.GetParam("stationCount","20");
    int count = std::stoi(strStationCount,nullptr,10);
    StationGps location;
    for(int i = 0; i < count; i++)
    {
        std::string strLatName = "lat" + std::to_string(i);
        std::string strLonName = "lon" + std::to_string(i);
        std::string strLatValue = xp.GetParam(strLatName,"0");
        std::string strLonValue = xp.GetParam(strLonName,"0");

        location.lat = std::stod(strLatValue,0);
        location.lon = std::stod(strLonValue,0);

        std::cout<<strLatName<<strLonName<<" lat:"<<strLatValue<<" lon:"<<strLonValue<<location.lat<<" "<<location.lon<<std::endl;
        mstationGps.append(location);
    }//END get init param

    socket=new QTcpSocket();
    ui->lineEdit_ip->setText(QString::fromStdString(strHostIP));
    ui->lineEdit_port->setText(QString::fromStdString(strHostPort));
    ui->pushButton_connect->setEnabled(false);

    givlog = new iv::Ivlog("v2x");
    connect(socket, &QTcpSocket::readyRead, this, &MainWindow::socket_Read_Data);
    connect(socket, &QTcpSocket::disconnected, this, &MainWindow::socket_Disconnected);

    /* ShareMemory Register BEGIN */
    //ShareMem: v2x
    mpMemV2xSend = iv::modulecomm::RegisterSend("v2x",1000,3);
    //ShareMem: v2x使能状态
    iv::modulecomm::RegisterRecv("v2xStEn", ListenV2xStEn);
    //ShareMem: v2x使能状态请求
    mpMemV2xStSend = iv::modulecomm::RegisterSend("v2xStReq",1000,1);
    //ShareMem: 车辆地盘状态
    ModuleFun funchassis =std::bind(&MainWindow::UpdateChassis,this,std::placeholders::_1,\
                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,\
                                    std::placeholders::_5);
    mpMemchassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis);
    //ShareMem: gps
    ModuleFun fungpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1, \
                                   std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpMemGps = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);

    ModuleFun funlidarObs =std::bind(&MainWindow::UpdateLidarObs,this,std::placeholders::_1, \
                                   std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpMemLidarObs = iv::modulecomm::RegisterRecvPlus("lidar_track",funlidarObs);

    ModuleFun funRadarObs =std::bind(&MainWindow::UpdateRadarObs,this,std::placeholders::_1, \
                                   std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpMemRadarObs = iv::modulecomm::RegisterRecvPlus("radar",funRadarObs);

    /* ShareMemory Register END */

    shareV2xStReqMsg();
    //heart beat,jiaolili,20210207
    m_bEnablePlatform=false;
    m_bIsConnect=false;
    m_iHeartbeatCount=0;
    QTimer *timer = new QTimer(this);
    connect(timer,SIGNAL(timeout()),SLOT(heartBeat()));
    timer->start(200);
    ///////////////////////////////////////////

}

MainWindow::~MainWindow()
{
    delete this->socket;
    delete ui;
}
void MainWindow::heartBeat()
{
    if(gv2xEn) {
        if(!m_bEnablePlatform) {
            connectPlatform();
        }
        if(m_bIsConnect) {
            upDataStream();
        } else {
            connectPlatform();
        }

    } else {
        if(m_bEnablePlatform) {
            disconnectPlatform();
        }
    }
    m_bEnablePlatform = gv2xEn;
}
void MainWindow::upDataStream()
{
    m_iHeartbeatCount++;
    if(m_iHeartbeatCount%4==0) {
        upVehicleStatus();
        return;
    }
    if(m_iHeartbeatCount%4==1) {
        upHardwareStatus();
        return;
    }
    if(m_iHeartbeatCount%4==2) {
        upObstacleDataStatus();
        return;
    }
    if(m_iHeartbeatCount%4==3) {
        upSoftwareStatus();
        return;
    }

}
void MainWindow::socket_Read_Data()
{
    QByteArray buffer;
    //读取缓冲区数据
    buffer = socket->readAll();

    if(!buffer.isEmpty())
    {
        //QString str = QString::fromLocal8Bit(buffer);
        QString str = QString(buffer);

//        ui->textEdit_messages->insertPlainText("服务器消息："+str+"\n");
        QStringList list = str.split(",");
        int length=list.size();
//        if(str.contains("FFCC")) {
//            QString len=QString::number(length);
//            ui->textEdit_messages->insertPlainText("服务器消息："+str+"\n");
//            ui->textEdit_messages->insertPlainText("服务器消息："+len+"\n");

//        }
        if(length>=4) {
            if(checkVehicle(list[2])) {
                ui->textEdit_messages->insertPlainText("服务器消息："+str+"\n");
                ui->textEdit_messages->insertPlainText("服务器消息：vehicle id is ok!\n");
                int downstream_id =getDownStreamId(list[0]);
                switch (downstream_id) {
                    case StopCommand:
                        ProStopCommand(list[3]);
                        break;
                    case AutoPilotControl:
                        if(length>4) {
                            ProAutoPilotControl(list);
                        }
                        break;
                    case StationCommand:
                        ProStationCommand(list[3]);
                        break;
                    default:
                        break;


                }
            } else {
                ui->textEdit_messages->insertPlainText("服务器消息：vehicle id is wrong!\n");
            }

        }
    }
}
//云平台急停指令
void MainWindow::ProStopCommand(QString str)
{
    int tmp;
    iv::v2x::v2x msgV2xProto;
    msgV2xProto.Clear();
    QStringList list=str.split("]");
    if(list.size()>0) {
        tmp = list[0].toInt();
        if(tmp==0) {
            msgV2xProto.set_emergencystop(0);
            ui->textEdit_messages->insertPlainText("服务器消息：vehicle emergency stop cancel!\n");
            shareV2xProtoMsg(msgV2xProto);
        }
        else if(tmp==1) {
            msgV2xProto.set_emergencystop(1);
            ui->textEdit_messages->insertPlainText("服务器消息：vehicle emergency stop enable!\n");
            shareV2xProtoMsg(msgV2xProto);
        }
    }
}
//云平台工作模式&坐标索引指令
void MainWindow::ProAutoPilotControl(QStringList list)
{
    iv::v2x::v2x msgV2xProto;
    iv::v2x::stationsGPS *stGps;
    msgV2xProto.Clear();
    int tmp = list[3].toInt();
    int length=list.size();
    int stationId;
    if(tmp==1) {
        msgV2xProto.set_carmode(1);
        if(length>5) {
            QStringList tmp_list0=list[4].split("[");
            list[4]=tmp_list0[1];
            QStringList tmp_list1=list[length-1].split("]]");
            list[length-1]=tmp_list1[0];
            for(int i=4;i<length;i++) {
                stationId = list[i].toInt() - 1;
                msgV2xProto.add_stationid(stationId);

                stGps=msgV2xProto.add_stgps();
                stGps->set_lat(mstationGps.at(stationId).lat);
                stGps->set_lon(mstationGps.at(stationId).lon);
                ui->textEdit_messages->insertPlainText("服务器消息：car station has "+list[i]+"\n");
            }
        } else {
            ui->textEdit_messages->insertPlainText("服务器消息：car station at least 2!\n");
        }

    } else {
        msgV2xProto.set_carmode(0);
        ui->textEdit_messages->insertPlainText("服务器消息：car autoDrive mode cancle!\n");
    }
    shareV2xProtoMsg(msgV2xProto);
}
//云平台站点停止指令
void MainWindow::ProStationCommand(QString str)
{
    int tmp;
    iv::v2x::v2x msgV2xProto;
    msgV2xProto.Clear();
    QStringList list=str.split("]");
    if(list.size()>0) {
        tmp = list[0].toInt();
        if(tmp==0) {
            msgV2xProto.set_stationstop(1);//站点启动
            shareV2xProtoMsg(msgV2xProto);
            ui->textEdit_messages->insertPlainText("服务器消息：vehicle station start!\n");
        }
        else if(tmp==1) {
            msgV2xProto.set_stationstop(0);//下一站停车
            shareV2xProtoMsg(msgV2xProto);
            ui->textEdit_messages->insertPlainText("服务器消息：vehicle station top!\n");
        }
    }
}

bool MainWindow::checkVehicle(QString str)
{
    return true;
}
int MainWindow::getDownStreamId(QString str)
{
    int downstream_id=-1;
    if(str.contains("FFAA")) {
        ui->textEdit_messages->insertPlainText("服务器消息：StopCommand!\n");
        downstream_id=1;
        return downstream_id;
    }
    if(str.contains("FFCC")) {
        ui->textEdit_messages->insertPlainText("服务器消息：AutoPilotControl!\n");
        downstream_id=2;
        return downstream_id;
    }
    if(str.contains("FFBB")) {
        ui->textEdit_messages->insertPlainText("服务器消息：StationCommand!\n");
        downstream_id=3;
        return downstream_id;
    }
    return downstream_id;
}
void MainWindow::socket_Disconnected()
{

    //修改按键文字
    ui->pushButton_connect->setText("connect");
    ui->textEdit_messages->insertPlainText("服务器消息：Disconnected");
}
void MainWindow::connectPlatform()
{
    QString IP;
    int port;
    //获取IP地址
    IP = ui->lineEdit_ip->text();
    //获取端口号
    port = ui->lineEdit_port->text().toInt();
    ui->textEdit_messages->setText("");
    ui->textEdit_messages->insertPlainText("正在连接"+ui->lineEdit_ip->text()+":"+ui->lineEdit_port->text()+"\n");
    //取消已有的连接
    socket->abort();
    //连接服务器
    socket->connectToHost(IP, port);
    //等待连接成功
    if(!socket->waitForConnected(30000)) {
        ui->textEdit_messages->insertPlainText("连接失败\n");
        return;
    }
    m_bIsConnect = true;
    ui->textEdit_messages->insertPlainText("连接成功\n");
    //修改按键文字
    ui->pushButton_connect->setText("disconnect");
}

void MainWindow::disconnectPlatform()
{
    //断开连接
    ui->textEdit_messages->setText("断开连接\n");
    socket->disconnectFromHost();
    //修改按键文字
    ui->pushButton_connect->setText("connect");
    m_bIsConnect=false;
}
void MainWindow::on_pushButton_connect_clicked()
{
    if(ui->pushButton_connect->text() == tr("connect"))
    {
        QString IP;
        int port;

        //获取IP地址
        IP = ui->lineEdit_ip->text();
        //获取端口号
        port = ui->lineEdit_port->text().toInt();
        ui->textEdit_messages->setText("");
        ui->textEdit_messages->insertPlainText("正在连接"+ui->lineEdit_ip->text()+":"+ui->lineEdit_port->text()+"\n");
        //取消已有的连接
        socket->abort();
        //连接服务器
        socket->connectToHost(IP, port);
        //等待连接成功
        if(!socket->waitForConnected(30000))
        {
            QMessageBox::warning(this,tr("消息"),tr("连接失败！请重新连接"),QMessageBox::Yes);
            ui->textEdit_messages->insertPlainText("连接失败\n");
            return;
        }
        ui->textEdit_messages->insertPlainText("连接成功\n");
        QMessageBox::information(this,tr("消息"),tr("连接成功"),QMessageBox::Yes);


        //修改按键文字
        ui->pushButton_connect->setText("disconnect");
    }
    else
    {
        //断开连接
        ui->textEdit_messages->setText("断开连接\n");
        socket->disconnectFromHost();
        //修改按键文字
        ui->pushButton_connect->setText("connect");
    }
}


void MainWindow::on_textEdit_messages_textChanged()
{
    ui->textEdit_messages->moveCursor(QTextCursor::End);
}

QString MainWindow::getTimeStamp()
{
    QDateTime time = QDateTime::currentDateTime();
    int current_timestamp=time.toTime_t();
    QString str=QString::number(current_timestamp);
    return  str;
}

void MainWindow::shareV2xProtoMsg(iv::v2x::v2x msgV2xProto)
{
    int nsize = msgV2xProto.ByteSize();
    char * strdata = new char[msgV2xProto.ByteSize()];
    if(msgV2xProto.SerializePartialToArray(strdata,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpMemV2xSend,strdata,nsize);
    }
    givlog->info("v2x","share v2x controll Msg");
    delete strdata;
}
//请求v2状态，是否接收v2x的控制，由ui发送
void MainWindow::shareV2xStReqMsg()
{
    iv::v2x::v2xStReq x;
    x.set_v2xstreq(1);
    int nsize = x.ByteSize();
    char * str = new char[nsize];
    if(x.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpMemV2xStSend,str,nsize);
    }
    else
    {
        givlog->error("v2x","send require error");
    }
    givlog->info("v2x","send st req");
    delete str;
}

void MainWindow::upVehicleStatus()
{
    QString time_stamp=getTimeStamp();
    QString test="['CCFF',"+time_stamp+",'"+mscarID+"',"+QString::number(micarMode)+","+QString::number(mfspeed)+","+ \
            QString::number(mfsteerAngle)+","+ QString::number(miSOC)+","+ QString::number(micarError)+","+ \
            QString::number(mierrorNum)+","+QString::number(mflon)+","+ QString::number(mflat)+","+ \
            QString::number(mfheading)+"]";
    //QByteArray bytes = test.toUtf8();
    QByteArray bytes = test.toLatin1();
    socket->write(bytes);    
}
void MainWindow::upHardwareStatus()
{
    QString time_stamp=getTimeStamp();
    //QString test="['DDFF',"+time_stamp+",[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[[0,0],[0,0]],[2,0]]";
    QString test="['DDFF',"+time_stamp+",["+ \
            QString::number(mistRadar)+","+getHardwareType(mistRadar)+"],["+ \
            QString::number(mistLidar)+","+getHardwareType(mistLidar)+"],["+ \
            QString::number(mistSonic)+","+getHardwareType(mistSonic)+"],["+ \
            QString::number(mistCamera)+","+getHardwareType(mistCamera)+"],["+ \
            QString::number(mistmic)+","+getHardwareType(mistmic)+"],["+ \
            QString::number(mistGPS)+","+getHardwareType(mistGPS)+"],[["+ \
            QString::number(mistCanRadar)+","+getHardwareType(mistCanRadar)+"],["+ \
            QString::number(mistCanCar)+","+getHardwareType(mistCanCar)+"]],["+ \
            "2,0]]";
    //QByteArray bytes = test.toUtf8();
    QByteArray bytes = test.toLatin1();
    socket->write(bytes);
}

QString MainWindow::getHardwareType(int state)
{
    QString type;
    int iType=-1;
    if(state==1) {
        iType=11;
    } else if(state==0) {
        iType=0;
    } else {
        ui->textEdit_messages->insertPlainText("ModelError消息：Hardware state get error!!!\n");
    }
    type=QString::number(iType);
    return type;
}

void MainWindow::upObstacleDataStatus()
{
    QString time_stamp=getTimeStamp();
    //QString test="['EEFF',"+time_stamp+",[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]]";
    QString test="['EEFF',"+time_stamp+ \
            getObstacleData(0)+getObstacleData(1)+ \
            +"]";

    //QByteArray bytes = test.toUtf8();
    QByteArray bytes = test.toLatin1();
    socket->write(bytes);
}

QString MainWindow::getObstacleData(int type)
{
    QString message="";
    ObsInfo obs;
    int i;
    if(type==0) {
        for(i=0;i<miRadarObsCount;i++) {
            obs=mqRadarObsInfo[i];
            message += ",["+QString::number(obs.type)+","+QString::number(obs.lon,'f',9)+","+QString::number(obs.lat,'f',9)+"]";
        }
    } else if(type==1) {
        for(i=0;i<miLidarObsCount;i++) {
            obs=mqLidarObsInfo[i];
            message += ",["+QString::number(obs.type)+","+QString::number(obs.lon,'f',9)+","+QString::number(obs.lat,'f',9)+"]";
        }
    } else {
        ui->textEdit_messages->insertPlainText("ProcessError消息：Obstacle data type get error!!!\n");
    }
    ui->textEdit_messages->insertPlainText("Obstacle Message: "+message);
    return message;
}

void MainWindow::upSoftwareStatus()
{
    QString time_stamp=getTimeStamp();
    QString test="['FFFF',"+time_stamp+",[1,10,1,10,1,10,1,10],[0,0,0,0,0,0,0,0,0,0,0],[0,0,0,0,0,0]]";
    //QByteArray bytes = test.toUtf8();
    QByteArray bytes = test.toLatin1();
    socket->write(bytes);
}

void MainWindow::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    mistCanCar = 0;
    iv::chassis xchassis;
    static int ncount = 0;
    if(!xchassis.ParseFromArray(strdata,nSize))
    {
        std::cout<<"v2xTcpClient::UpdateChassis ParseFrom Array Error."<<std::endl;
        return;
    }

    if(xchassis.has_drivemode())
    {
        micarMode = xchassis.drivemode();
    }
    if(xchassis.has_vel())
        mfspeed = xchassis.vel();
    if(xchassis.has_angle_feedback())
        mfsteerAngle = xchassis.angle_feedback()/870.0*28;
    if((xchassis.has_soc()))
    {
        miSOC = xchassis.soc();
    }
}

//接收GPS数据
void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,\
                              const QDateTime * dt,const char * strmemname)
{

    iv::gps::gpsimu  xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
        return;
    }

    mflat = xgpsimu.lat();
    mflon = xgpsimu.lon();
    mfheading = xgpsimu.heading();
    mpdata.gps_lat = xgpsimu.lat();
    mpdata.gps_lng = xgpsimu.lon();
    mpdata.ins_heading_angle = xgpsimu.heading();
    mpdata.rtk_status = xgpsimu.rtk_state();
    mpdata.ins_status = xgpsimu.ins_state();
    mpdata.vel_D = xgpsimu.vd();    //地向速度,单位(米/秒)
    mpdata.vel_E = xgpsimu.ve();    //东向速度,单位(米/秒)
    mpdata.vel_N = xgpsimu.vn();    //北向速度,单位(米/秒)

 //   mfspeed = sqrt(pow(data->vel_E,2)+pow(data->vel_N,2)) * 3.6;  //double pow(double x, double y) 返回 x 的 y 次幂，即 xy。
    mistGPS = 0;
}

int toUnicode(const char* str)
{
    return str[0] + (str[1] ? toUnicode(str + 1) : 0);
}

constexpr inline int U(const char* str)
{
    return str[0] + (str[1] ? U(str + 1) : 0);
}
int GetTypeString(const char* obsType)
{
  switch (toUnicode(obsType))
  {
    case U("unknown"):
      return 0;
    case U("car"):
      return 1;
    case U("bike"):
      return 5;
    case U("pedestrian"):
      return 3;
    default:
      return 0;
  }
}
void MainWindow::UpdateLidarObs(const char * strdata,const unsigned int nSize,const unsigned int index, \
                           const QDateTime * dt,const char * strmemname)
{

//    std::vector<iv::lidar::object>  lidarobjvec;
//    strtolidarobj(lidarobjvec,strdata,nSize);
    mqLidarObsInfo.clear();
    iv::lidar::objectarray lidarobjvec;
    std::string in;
    in.append(strdata,nSize);
    lidarobjvec.ParseFromString(in);
    givlog->verbose("v2xLidarObs","obj size is %d ",lidarobjvec.obj_size());
    miLidarObsCount = lidarobjvec.obj_size();
    ObsInfo xobsInfo;
    double x,y;
    iv::GPS_INS gps_ins;
    for(int i = 0; i < miLidarObsCount; i++)
    {
        x = lidarobjvec.obj(i).centroid().x();
        y = lidarobjvec.obj(i).centroid().y();
        GaussProjCal(mpdata.gps_lng, mpdata.gps_lat, &mpdata.gps_x, &mpdata.gps_y);
        gps_ins = iv::decition::Coordinate_UnTransfer(x, y, mpdata);
        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &gps_ins.gps_lng, &gps_ins.gps_lat);
        xobsInfo.lat = gps_ins.gps_lat;
        xobsInfo.lon = gps_ins.gps_lng;
        std::string str = lidarobjvec.obj(i).type_name();
        xobsInfo.type = GetTypeString(str.data());
        mqLidarObsInfo.append(xobsInfo);
    }
    mistLidar = 0;
}

void MainWindow::UpdateRadarObs(const char * strdata,const unsigned int nSize,const unsigned int index, \
                           const QDateTime * dt,const char * strmemname)
{
    mqRadarObsInfo.clear();
    static qint64 oldrecvtime;
    iv::radar::radarobjectarray xradararray;
    if(!xradararray.ParseFromArray(strdata,nSize))
    {
        givlog->warn("ADCIntelligentVehicle::UpdateRADAR Parse Error.");
        return;
    }

    //   gIvlog->verbose("radar time is %ld",QDateTime::currentMSecsSinceEpoch());

    if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)
    {
        givlog->warn("radar interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
    }

    oldrecvtime = QDateTime::currentMSecsSinceEpoch();

    miRadarObsCount = xradararray.obj_size();

    givlog->verbose("radarobs count %ld",miRadarObsCount);
    ObsInfo xobsInfo;
    double x,y;
    iv::GPS_INS gps_ins;
    for(int i = 0; i < miRadarObsCount; i++)
    {
        x = xradararray.obj(i).x();
        y = xradararray.obj(i).y();
        GaussProjCal(mpdata.gps_lng, mpdata.gps_lat, &mpdata.gps_x, &mpdata.gps_y);
        gps_ins = iv::decition::Coordinate_UnTransfer(x, y, mpdata);
        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &gps_ins.gps_lng, &gps_ins.gps_lat);
        xobsInfo.lat = gps_ins.gps_lat;
        xobsInfo.lon = gps_ins.gps_lng;
        xobsInfo.type = 0;
        mqRadarObsInfo.append(xobsInfo);
    }
    mistRadar = 0;
    mistCanRadar = 0;
//    mistLidar = 0;
}