#include "ivdriver_gps_hcp2.h"

#include <math.h>

namespace iv {

ivdriver_gps_hcp2::ivdriver_gps_hcp2(std::string strxmlpath)
{
    loadxmlparam(strxmlpath);
    mTime.start();
}

int ivdriver_gps_hcp2::decode(iv::gps::gpsimu & xgpsimu)
{
    int xpos = mstrBuffer.indexOf('\n');
    QString strsen;
    if(xpos>0)
    {

        strsen = mstrBuffer.left(xpos+1);
        mstrBuffer.remove(0,xpos+1);
    }
    else
    {
        return 0;
    }

    QStringList strlistrmc;

RETRYDECODE:
    strlistrmc = strsen.split(",");

   if(strlistrmc.size() < 23)return -1;
   if(strlistrmc.at(0) != "$GPCHC")return -2;

   if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
   {
       int nPos = strsen.indexOf('$',10);
       if(nPos > 0)
       {
           QString strnewsen = strsen.right(strsen.size()-nPos);
//           qDebug("new sen is %s",strnewsen.toLatin1().data());
           strsen = strnewsen;
           goto RETRYDECODE;
       }
       return -3;
   }

   double fheading,fLat,fLon,fVel,fPitch,fRoll;
   double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
   int nsv1,nsv2;
   int gpsweek,gpstime;
   int insstate,rtkstate;
   QString strx = strlistrmc.at(3);
   fheading = strx.toDouble();

   strx = strlistrmc.at(1);
   gpsweek = strx.toInt();

   strx = strlistrmc.at(2);
   gpstime = strx.toInt();

   strx = strlistrmc.at(12);
   fLat = strx.toDouble();

   strx  = strlistrmc.at(13);
   fLon = strx.toDouble();

   strx = strlistrmc.at(14);
   fHgt = strx.toDouble();

   double ve,vn,vu;
   strx = strlistrmc.at(15);
   ve = strx.toDouble();

   strx = strlistrmc.at(16);
   vn = strx.toDouble();

   strx = strlistrmc.at(17);
   vu = strx.toDouble();

   fVel = sqrt(ve*ve + vn* vn);

   if((mTime.elapsed()-mOldTime) >= 100)
   {
       if(mOldTime == 0)
       {
           mfCalc_acc = 0;
           mfOldVel = fVel;
           mOldTime = mTime.elapsed();
       }
       else
       {
           mfCalc_acc = (fVel - mfOldVel)/((mTime.elapsed() - mOldTime)*0.001);
           mfOldVel = fVel;
           mOldTime = mTime.elapsed();
       }
   }

   strx = strlistrmc.at(4);
   fPitch = strx.toDouble();

   strx = strlistrmc.at(5);
   fRoll = strx.toDouble();

   strx = strlistrmc.at(6);
   gyro_x = strx.toDouble();

   strx = strlistrmc.at(7);
   gyro_y = strx.toDouble();

   strx = strlistrmc.at(8);
   gyro_z = strx.toDouble();

   strx = strlistrmc.at(9);
   acc_x = strx.toDouble();

   strx = strlistrmc.at(10);
   acc_y = strx.toDouble();

   strx = strlistrmc.at(11);
   acc_z = strx.toDouble();

   strx = strlistrmc.at(19);
   nsv1 = strx.toInt();

   strx = strlistrmc.at(20);
   nsv2 = strx.toInt();

   strx = strlistrmc.at(21);
   char strstate[3];
   strstate[2] = 0;
   if(strx.size() >= 2)
   {

       memcpy(strstate,strx.data(),2);
  //     qDebug(strstate);
       char xstate;
       xstate = strstate[0];
       rtkstate = 4;
       int xs;
       if(xstate == '0')xs = 0;
       if(xstate == '1')xs = 3;
       if(xstate == '2')xs = 4;
       if(xstate == '3')xs = 8;
       if(xstate == '4')xs = 6;
       if(xstate == '5')xs = 5;
       if(xstate == '6')xs = 3;
       if(xstate == '7')xs = 4;
       if(xstate == '8')xs = 5;
       if(xstate == '9')xs = 5;

       rtkstate = xs;
//       if(mstate == 0)minsstate = 0;
//       else minsstate = 4;

       xstate = strstate[1];
       if(xstate == '0')insstate = 3;
       else insstate = 4;
       if(rtkstate == 0)insstate = 3;
   }

   xgpsimu.set_vd(vu);
   xgpsimu.set_ve(ve);
   xgpsimu.set_vn(vn);
   xgpsimu.set_lat(fLat);
   xgpsimu.set_lon(fLon);
   xgpsimu.set_heading(fheading);
   xgpsimu.set_state(4);
   xgpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());
   xgpsimu.set_roll(fRoll);
   xgpsimu.set_pitch(fPitch);

   xgpsimu.set_rtk_state(rtkstate);
   xgpsimu.set_ins_state(insstate);

   xgpsimu.set_height(fHgt);
   xgpsimu.set_satnum1(nsv1);
   xgpsimu.set_satnum2(nsv2);
   xgpsimu.set_gpsweek(gpsweek);
   xgpsimu.set_gpstime(gpstime);
   xgpsimu.set_gyro_x(gyro_x);
   xgpsimu.set_gyro_y(gyro_y);
   xgpsimu.set_gyro_z(gyro_z);
   xgpsimu.set_acce_x(acc_x);
   xgpsimu.set_acce_y(acc_y);
   xgpsimu.set_acce_z(acc_z);
   xgpsimu.set_acc_calc(mfCalc_acc);

   return 1;


}

}