#include <QCoreApplication>

#include <iostream>
#include <QDateTime>
#include <math.h>

#include <thread>
#include <QElapsedTimer>
#include "modulecomm.h"
#include "xmlparam.h"
#include "ivlog.h"
#include "ivfault.h"

iv::Ivlog * givlog;
iv::Ivfault * givfault;

#include "ultrasonic.pb.h"
#include "ultraarea.pb.h"

iv::ultrasonic::ultrasonic gobj;

void * gpa , * gpb;

QElapsedTimer gTime;
int min(int ids[],int len)
{
    int i = 1;
    int* p = &ids[0];
    for(; i < len; i++)
    {
        if (*p>ids[i]){
            p = &ids[i];
        }
   }
   return *p;
}

void ProcessData(iv::ultrasonic::ultrasonic xmsg)
{

        std::cout<<xmsg.sigobjdist_flmiddle()<<" ";//1
        std::cout<<xmsg.sigobjdist_frmiddle()<<"      ";//1
        std::cout<<xmsg.sigobjdist_frcorner()<<" ";//2
        std::cout<<xmsg.sigobjdist_frside()<<" ";//2
        std::cout<<xmsg.sigobjdist_rrside()<<" ";//2
        std::cout<<xmsg.sigobjdist_rrcorner()<<"      ";//2
        std::cout<<xmsg.sigobjdist_rrmiddle()<<" ";//3
        std::cout<<xmsg.sigobjdist_rlmiddle()<<"      ";//3
        std::cout<<xmsg.sigobjdist_rlcorner()<<" ";//4
        std::cout<<xmsg.sigobjdist_rlside()<<" ";//4
        std::cout<<xmsg.sigobjdist_flside()<<" ";//4
        std::cout<<xmsg.sigobjdist_flcorner()<<std::endl;//4
    iv::ultrasonic::ultraarea areas;
    areas.set_timestamp(xmsg.timestamp());

    if(xmsg.sigsensor_front_lm()==0 && xmsg.sigsensor_front_rm()==0){
        iv::ultrasonic::Area area;
        area.set_id(1);
        area.set_valid(false);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }else{
        int tmp[2];
        tmp[0] = xmsg.sigobjdist_flmiddle();
        tmp[1] = xmsg.sigobjdist_frmiddle();
        int min_dist = min(tmp,2);
        iv::ultrasonic::Area area;
        area.set_id(1);
        area.set_dist(min_dist*1.e-3);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }
    if(xmsg.sigsensor_front_rc()==0 && xmsg.sigsensor_front_rs()==0 && xmsg.sigsensor_rear_rs()==0 && xmsg.sigsensor_rear_rc()==0){
        iv::ultrasonic::Area area;
        area.set_id(2);
        area.set_valid(false);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }else{
        int tmp[4];
        tmp[0] = xmsg.sigobjdist_frcorner();
        tmp[1] = xmsg.sigobjdist_frside();
        tmp[2] = xmsg.sigobjdist_rrside();
        tmp[3] = xmsg.sigobjdist_rrcorner();
        int min_dist = min(tmp,4);
        iv::ultrasonic::Area area;
        area.set_id(2);
        area.set_dist(min_dist*1.e-3);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }
    if(xmsg.sigsensor_rear_rm()==0 && xmsg.sigsensor_rear_lm()==0){
        iv::ultrasonic::Area area;
        area.set_id(3);
        area.set_valid(false);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }else{
        int tmp[2];
        tmp[0] = xmsg.sigobjdist_rrmiddle();
        tmp[1] = xmsg.sigobjdist_rlmiddle();

        int min_dist = min(tmp,2);
        iv::ultrasonic::Area area;
        area.set_id(3);
        area.set_dist(min_dist*1.e-3);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }
    if(xmsg.sigsensor_rear_lc()==0 && xmsg.sigsensor_rear_ls()==0 && xmsg.sigsensor_front_ls()==0 && xmsg.sigsensor_front_lc()==0){
        iv::ultrasonic::Area area;
        area.set_id(4);
        area.set_valid(false);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }else{
        int tmp[4];
        tmp[0] = xmsg.sigobjdist_rlcorner();
        tmp[1] = xmsg.sigobjdist_rlside();
        tmp[2] = xmsg.sigobjdist_flside();
        tmp[3] = xmsg.sigobjdist_flcorner();
        int min_dist = min(tmp,4);
        iv::ultrasonic::Area area;
        area.set_id(4);
        area.set_dist(min_dist*1.e-3);
        iv::ultrasonic::Area * p = areas.add_area();
        p->CopyFrom(area);
    }

    std::cout<<"---------- "<<areas.area_size()<<" ---------"<<std::endl;
    for(int i=0;i<areas.area_size();i++){
        iv::ultrasonic::Area area = areas.area(i);
        std::cout<<area.id()<<": "<<area.dist()<<" "<<area.valid()<<std::endl;
    }
    std::string out = areas.SerializeAsString();
    iv::modulecomm::ModuleSendMsg(gpb,out.data(),out.length());
}

void ListenUltra(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1) return;
    iv::ultrasonic::ultrasonic xmsg;

//    givlog->verbose("vbox","rec rsu data");
    if(false == xmsg.ParseFromArray(strdata,nSize))
    {
        std::cout<<"Listenultra fail."<<std::endl;
        return;
    }
    ProcessData(xmsg);
//    qDebug("can size is %d",xmsg.rawmsg_size());
//    xt = QDateTime::currentMSecsSinceEpoch();
//    qDebug("latence = %ld ",xt-pic.time());

}

void threadstate()
{
    while(1)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
//        if(gnRadarState > -100)gnRadarState--;
//        if(gnRadarState > 0)
//        {
//            givfault->SetFaultState(0,0,"OK");
//        }
//        else
//        {
//            if(gnRadarState > -100)
//            {
//                givfault->SetFaultState(1,1,"无CAN数据");
//            }
//            else
//            {
//                givfault->SetFaultState(2,2,"无CAN数据");
//            }
//        }

    }
}
int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    std::string strmemultra = "sonar";
    std::string strmemsend = "ultra_area";

    givlog = new iv::Ivlog(strmemultra.data());
    givfault = new iv::Ivfault(strmemultra.data());

    givfault->SetFaultState(1,1,"初始化");
    givlog->info("ultra","Initialized");

    gpb = iv::modulecomm::RegisterSend(strmemsend.data(),100000,1);
    iv::modulecomm::RegisterRecv(strmemultra.data(), ListenUltra);

    std::thread threadfault(threadstate);


    return a.exec();
}