// Part of BNC, a utility for retrieving decoding and
// converting GNSS data streams from NTRIP broadcasters.
//
// Copyright (C) 2007
// German Federal Agency for Cartography and Geodesy (BKG)
// http://www.bkg.bund.de
// Czech Technical University Prague, Department of Geodesy
// http://www.fsv.cvut.cz
//
// Email: euref-ip@bkg.bund.de
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation, version 2.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

/* -------------------------------------------------------------------------
 * BKG NTRIP Client
 * -------------------------------------------------------------------------
 *
 * Class:      t_pppRunRealTime
 *
 * Purpose:    Single Real-Time PPP Client
 *
 * Author:     L. Mervart
 *
 * Created:    29-Jul-2014
 *
 * Changes:    
 *
 * -----------------------------------------------------------------------*/


#include <iostream>
#include <iomanip>
#include <string.h>
#include <map>

#include "pppRunRealTime.h"
#include "bnccore.h"
#include "bncephuser.h"

using namespace BNC_PPP;
using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
t_pppRunRealTime::t_pppRunRealTime(const t_pppOptions* opt) {
  _opt = opt;
  connect(this, SIGNAL(newMessage(QByteArray,bool)), 
          BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
  if (_opt->_realTime) {
    connect(BNC_CORE->caster(), SIGNAL(newObs(QByteArray, QList<t_obs>)),
            this, SLOT(slotNewObs(QByteArray, QList<t_obs>)));

    connect(BNC_CORE, SIGNAL(newEphGPS(gpsephemeris)),
            this, SLOT(slotNewEphGPS(gpsephemeris)));
  
    connect(BNC_CORE, SIGNAL(newEphGlonass(glonassephemeris)),
            this, SLOT(slotNewEphGlonass(glonassephemeris)));
  
    connect(BNC_CORE, SIGNAL(newEphGalileo(galileoephemeris)),
            this, SLOT(slotNewEphGalileo(galileoephemeris)));

    connect(BNC_CORE, SIGNAL(newCorrections(QStringList)),
            this, SLOT(slotNewCorrections(QStringList)));

    connect(this, SIGNAL(newPosition(bncTime, double, double, double)),
            BNC_CORE, SIGNAL(slotNewPosition(bncTime, double, double, double)));

    for (unsigned iPrn = 0; iPrn <= t_prn::MAXPRN; iPrn++) {
      _lastOrbCorrIOD[iPrn]   = -1;
      _lastClkCorrValue[iPrn] = 0.0;
    }

    _pppClient = new t_pppClient(_opt);
  }
  else {
    throw t_except("t_pppRunRealTime: post-processing not yet implemented");
  }
}

// Destructor
////////////////////////////////////////////////////////////////////////////
t_pppRunRealTime::~t_pppRunRealTime() {
}

// 
////////////////////////////////////////////////////////////////////////////
void t_pppRunRealTime::slotNewEphGPS(gpsephemeris gpseph) {
  QMutexLocker locker(&_mutex);
  t_ephGPS eph;
  eph.set(&gpseph);
  _pppClient->putEphemeris(&eph);
}

// 
////////////////////////////////////////////////////////////////////////////
void t_pppRunRealTime::slotNewEphGlonass(glonassephemeris gloeph) {
  QMutexLocker locker(&_mutex);
  t_ephGlo eph;
  eph.set(&gloeph);
  _pppClient->putEphemeris(&eph);
}
  
// 
////////////////////////////////////////////////////////////////////////////
void t_pppRunRealTime::slotNewEphGalileo(galileoephemeris galeph) {
  QMutexLocker locker(&_mutex);
  t_ephGal eph;
  eph.set(&galeph);
  _pppClient->putEphemeris(&eph);
}

//
////////////////////////////////////////////////////////////////////////////
void t_pppRunRealTime::slotNewObs(QByteArray staID, QList<t_obs> obsList) {
  QMutexLocker locker(&_mutex);

  if (string(staID.data()) != _opt->_roverName) {
    return;
  }

  // Loop over all obsevations (possible different epochs)
  // -----------------------------------------------------
  QListIterator<t_obs> it(obsList);
  while (it.hasNext()) {
    const t_obs& oldObs = it.next();
    t_satObs* newObs = new t_satObs;
   
    newObs->_prn.set(oldObs.satSys, oldObs.satNum);
    newObs->_time.set(oldObs.GPSWeek, oldObs.GPSWeeks);

    // Find the corresponding data epoch or create a new one
    // -----------------------------------------------------
    t_epoData* epoch = 0;
    deque<t_epoData*>::const_iterator it;
    for (it = _epoData.begin(); it != _epoData.end(); it++) {
      if (newObs->_time == (*it)->_time) {
        epoch = *it;
        break;
      }
    }
    if (epoch == 0) {
      if (_epoData.empty() || newObs->_time > _epoData.back()->_time) {
        epoch = new t_epoData;
        epoch->_time = newObs->_time;
        _epoData.push_back(epoch);
      }
    }

    // Fill the new observation and add it to the corresponding epoch
    // --------------------------------------------------------------
    if (epoch != 0) {
      epoch->_satObs.push_back(newObs);
      map<string, t_frqObs*> frqObsMap;
      for (unsigned iEntry = 0; iEntry < GNSSENTRY_NUMBER; iEntry++) {
        string hlp(oldObs.rnxStr(iEntry).toAscii().data());
        if (hlp.length() == 3) {
          char   obsType    = hlp[0];
          string rnxType2ch = hlp.substr(1);
          if (obsType == 'C' || obsType == 'L') {
            t_frqObs* frqObs = 0;
            if (frqObsMap.find(rnxType2ch) == frqObsMap.end()) {
              frqObs                = new t_frqObs();
              frqObsMap[rnxType2ch] = frqObs;
              frqObs->_rnxType2ch   = rnxType2ch;
              newObs->_obs.push_back(frqObs);
            }
            else {
              frqObs = frqObsMap[rnxType2ch];
            }
            if      (obsType == 'C') {
              frqObs->_code      = oldObs._measdata[iEntry];
              frqObs->_codeValid = true;
            }
            else if (obsType == 'L') {
              frqObs->_phase      = oldObs._measdata[iEntry];
              frqObs->_phaseValid = true;
            }
          }
        }
      }
    }
  }

  // Process the oldest epochs
  // ------------------------
  while (_epoData.size() && _epoData.front()->_time < _lastClkCorrTime + 10.0) {

    const vector<t_satObs*>& satObs = _epoData.front()->_satObs;

    t_output output;
    _pppClient->processEpoch(satObs, &output);

    if (!output._error && _opt->xyzAprRoverSet()) {
      ColumnVector dx(3);
      dx[0] = _opt->_xyzAprRover[0] - output._xyzRover[0];
      dx[1] = _opt->_xyzAprRover[1] - output._xyzRover[1];
      dx[2] = _opt->_xyzAprRover[2] - output._xyzRover[2];
      emit newPosition (output._epoTime, dx[0], dx[1], dx[2]);
    }

    delete _epoData.front(); _epoData.pop_front();
    
    emit newMessage(QByteArray(output._log.c_str()), true);
  }
}
    
// 
////////////////////////////////////////////////////////////////////////////
void t_pppRunRealTime::slotNewCorrections(QStringList corrList) {
  QMutexLocker locker(&_mutex);

  if (_opt->_corrMount.empty()) {
    return;
  }

  // Check the Mountpoint (source of corrections)
  // --------------------------------------------
  QMutableListIterator<QString> itm(corrList);
  while (itm.hasNext()) {
    QStringList hlp = itm.next().split(" ");
    if (hlp.size() > 0) {
      QString mountpoint = hlp[hlp.size()-1];
      if (mountpoint != QString(_opt->_corrMount.c_str())) {
        itm.remove();     
      }
    }
  }

  if (corrList.size() == 0) {
    return;
  }

  vector<t_orbCorr*> orbCorr;
  vector<t_clkCorr*> clkCorr;
  vector<t_satBias*> satBias;

  QListIterator<QString> it(corrList);
  while (it.hasNext()) {
    QString line = it.next();

    QTextStream in(&line);
    int     messageType;
    int     updateInterval;
    int     GPSweek;
    double  GPSweeks;
    QString prn;
    in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;

    if ( t_corr::relevantMessageType(messageType) ) {
      t_corr corr;
      corr.readLine(line);
      if      (messageType == COTYPE_GPSCOMBINED || messageType == COTYPE_GLONASSCOMBINED ||
               messageType == COTYPE_GPSORBIT    || messageType == COTYPE_GLONASSORBIT    ) {
        t_orbCorr* cc = new t_orbCorr();
        cc->_prn.set(corr.prn.toAscii().data());
        cc->_iod       = corr.iod;
        cc->_time      = corr.tRao;
        cc->_system    = 'R';
        cc->_xr[0]     = corr.rao[0]; 
        cc->_xr[1]     = corr.rao[1]; 
        cc->_xr[2]     = corr.rao[2]; 
        cc->_dotXr[0]  = corr.dotRao[0]; 
        cc->_dotXr[0]  = corr.dotRao[1]; 
        cc->_dotXr[0]  = corr.dotRao[2]; 
        orbCorr.push_back(cc);

        _lastOrbCorrIOD[cc->_prn.toInt()] = cc->_iod;
      }
      else if (messageType == COTYPE_GPSCOMBINED || messageType == COTYPE_GLONASSCOMBINED ||
               messageType == COTYPE_GPSCLOCK    || messageType == COTYPE_GLONASSCLOCK    ) {
        t_clkCorr* cc = new t_clkCorr();
        cc->_prn.set(corr.prn.toAscii().data());
        cc->_iod         = corr.iod;
        cc->_time        = corr.tClk;
        cc->_dClk        = corr.dClk;
        cc->_dotDClk     = corr.dotDClk;
        cc->_dotDotDClk  = corr.dotDotDClk;
        cc->_clkPartial  = 0.0;
        if (messageType == COTYPE_GPSCLOCK || messageType == COTYPE_GLONASSCLOCK) {
          int lastIOD = _lastOrbCorrIOD[cc->_prn.toInt()];
          if (lastIOD != -1) {
            cc->_iod = lastIOD;
          }
          else {
            delete cc;
            cc = 0;
          }
        }
        if (cc) {
          clkCorr.push_back(cc);        
          if (_lastClkCorrTime.undef() || cc->_time > _lastClkCorrTime) {
            _lastClkCorrTime = cc->_time;
          }
        }
      }
    }
    else if ( messageType == BTYPE_GPS || messageType == BTYPE_GLONASS ) { 
      t_bias bias;
      bias.readLine(line);
    }
  }

  _pppClient->putOrbCorrections(orbCorr); 
  _pppClient->putClkCorrections(clkCorr); 
  _pppClient->putBiases(satBias);   

  for (unsigned ii = 0; ii < orbCorr.size(); ii++) {
    delete orbCorr[ii];
  }
  for (unsigned ii = 0; ii < clkCorr.size(); ii++) {
    delete clkCorr[ii];
  }
  for (unsigned ii = 0; ii < satBias.size(); ii++) {
    delete satBias[ii];
  }
}