#include "ivdriver_gps.h"

#include <iostream>

#include <math.h>

namespace iv {

ivdriver_gps::ivdriver_gps()
{
    mudpSocketGPSIMU = new QUdpSocket();


}

void ivdriver_gps::loadxmlparam(std::string strxmlpath)
{
    iv::xmlparam::Xmlparam xp(strxmlpath);

    mstrportname = xp.GetParam("devname","/dev/ttyUSB0");
    mstrmsgname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
    mnbaudrate = xp.GetParam("baudrate",230400);

}

void ivdriver_gps::ShareMsg(gps::gpsimu &xgpsimu)
{
    char * strser;
    bool bser;
    int nbytesize;
    nbytesize = xgpsimu.ByteSize();
    strser = new char[nbytesize];
    bser = xgpsimu.SerializeToArray(strser,nbytesize);
    if(bser)
        iv::modulecomm::ModuleSendMsg(mpagpsimu,strser,nbytesize);
    else
    {
        std::cout<<"gpsimu serialize error."<<std::endl;
//        givlog->error("gpsimu serialize error.");
//        gfault->SetFaultState(1, 0, "gpsimu serialize err");
    }
    delete strser;
}

void ivdriver_gps::broadcastmsg(gps::gpsimu &xgpsimu)
{
    unsigned char strSend[72];
    int i;
    for(i=0;i<72;i++)strSend[i] = 0x0;
    strSend[0] = 0xE7;

    strSend[21] = xgpsimu.ins_state();
    strSend[68] = xgpsimu.rtk_state();
    strSend[67] = 0;
    strSend[62] = 62;

    double fV = xgpsimu.lat();
    fV = fV/(180.0/M_PI);
    memcpy(strSend+23,&fV,8);
    fV = xgpsimu.lon();fV = fV/(180.0/M_PI);memcpy(strSend+31,&fV,8);
    float fF =0;memcpy(strSend+39,&fF,4);

    int iV;
    char * piV = (char *)&iV;
    piV++;
    fV = xgpsimu.ve();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+43,piV,3);
    fV = xgpsimu.vn();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+46,piV,3);
    fV = xgpsimu.vd();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+49,piV,3);

    fV = xgpsimu.heading()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+52,piV,3);
    fV = xgpsimu.pitch()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+55,piV,3);
    fV = xgpsimu.roll()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+58,piV,3);

    double facc_x,facc_y,facc_z;
    facc_x = xgpsimu.acce_x();
    facc_y = xgpsimu.acce_y();
    facc_z = xgpsimu.acce_z();

    fV = facc_x;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+3,piV,3);
    fV = facc_y;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+6,piV,3);
    fV = facc_z;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+9,piV,3);

    fV = xgpsimu.gyro_x();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+12,piV,3);
    fV = xgpsimu.gyro_y();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+15,piV,3);
    fV = xgpsimu.gyro_z();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+18,piV,3);


    char c;
    c = strSend[0];
    for(i=1;i<71;i++)c = c+strSend[i];
    strSend[71]= c;

    mudpSocketGPSIMU->writeDatagram((char *)strSend,72,QHostAddress::Broadcast, 3000);


}

void ivdriver_gps::modulerun()
{

    std::cout<<"register msg name is "<<mstrmsgname<<std::endl;
    mpagpsimu = iv::modulecomm::RegisterSend(mstrmsgname.data(),10000,1);

    m_serialPort_GPS = new QSerialPort();
    m_serialPort_GPS->setPortName(mstrportname.data());
    m_serialPort_GPS->setBaudRate(230400);
    m_serialPort_GPS->setParity(QSerialPort::NoParity);
    m_serialPort_GPS->setDataBits(QSerialPort::Data8);
    m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
    m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
    m_serialPort_GPS->setReadBufferSize(0);




    mbrun = true;

    bool bPortOpen = false;

OPENDEVICE:
    bPortOpen = false;
    std::cout<<" Open Device."<<std::endl;
    while(mbrun)
    {
        if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
        {
            bPortOpen = true;
            break;
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
        std::cout<<"retry open "<<mstrportname<<" baudrate is "<<mnbaudrate<<std::endl;
    }

    int nnotrecv = 0;
    while(mbrun)
    {
        QByteArray ba = m_serialPort_GPS->readAll();

        if(ba.length() > 0)
        {
            nnotrecv = 0;
            mstrBuffer.push_back(ba);
            iv::gps::gpsimu xgpsimu;
            int nrtn = decode(xgpsimu);
            while(1 == nrtn)
            {
                ShareMsg(xgpsimu);
                broadcastmsg(xgpsimu);
                nrtn = decode(xgpsimu);
            }
            //Decode
        }
        else
        {
            nnotrecv++;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }

        if(nnotrecv == 1000)
        {
            std::cout<<"not recv data."<<std::endl;
        }
        if(nnotrecv > 3000)
        {
            m_serialPort_GPS->close();
            goto OPENDEVICE;
        }
    }

    if(bPortOpen)m_serialPort_GPS->close();
    delete m_serialPort_GPS;
    iv::modulecomm::Unregister(mpagpsimu);

}

bool ivdriver_gps::checknmeasen(const char *strsen, const unsigned int nlen)
{
    if(nlen< 4)return false;

    int i;
    char check;
    int nstarpos = -1;
    check = strsen[1]^strsen[2];
    for(i=3;i<nlen;i++)
    {
        if(strsen[i] == '*')
        {
            nstarpos = i;
            break;
        }
        check = check^strsen[i];
    }
    if(nstarpos < 0)return false;
    if(nstarpos >(nlen -3))return false;
    char strcheck[3];
    int ncheck = check;
    snprintf(strcheck,3,"%02X",ncheck);
    char strsencheck[3];
    strsencheck[2] = 0;
    strsencheck[0] = strsen[nstarpos + 1];
    strsencheck[1] = strsen[nstarpos + 2];
    if(strncmp(strcheck,strsencheck,2) == 0)
    {
   //     qDebug("  r sen is %s",strsen);
        return true;
    }

//    givlog->verbose("cal check is %s, sen check is %s",strcheck,strsencheck);

//    qDebug("  sen is %s",strsen);
    return false;
}

}