/* -------------------------------------------------------------------------
 * BKG NTRIP Client
 * -------------------------------------------------------------------------
 *
 * Class:      t_pppFilter
 *
 * Purpose:    Filter Adjustment
 *
 * Author:     L. Mervart
 *
 * Created:    29-Jul-2014
 *
 * Changes:
 *
 * -----------------------------------------------------------------------*/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <newmat.h>
#include <newmatio.h>
#include <newmatap.h>

#include "pppFilter.h"
#include "bncutils.h"
#include "pppParlist.h"
#include "pppObsPool.h"
#include "pppStation.h"

using namespace BNC_PPP;
using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
t_pppFilter::t_pppFilter(t_pppObsPool *obsPool) {
  _numSat = 0;
  _obsPool = obsPool;
  _refPrn = t_prn();
  _datumTrafo = new t_datumTrafo();
}

// Destructor
////////////////////////////////////////////////////////////////////////////
t_pppFilter::~t_pppFilter() {
  delete _datumTrafo;
}

// Process Single Epoch
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::processEpoch() {
  _numSat = 0;
  const double maxSolGap = 60.0;

  // Vector of all Observations
  // --------------------------
  t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
  if (!epoch) {
    return failure;
  }
  vector<t_pppSatObs*> &allObs = epoch->obsVector();

  // Time of the Epoch
  // -----------------
  _epoTime = epoch->epoTime();

  if (!_firstEpoTime.valid() || !_lastEpoTimeOK.valid()
      || (maxSolGap > 0.0 && _epoTime - _lastEpoTimeOK > maxSolGap)) {
    _firstEpoTime = _epoTime;
  }

  string epoTimeStr = string(_epoTime);

  const QMap<char, t_pppRefSat*> &refSatMap = epoch->refSatMap();

  //--
  // Set Parameters
  if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
    return failure;
  }

  if (OPT->_obsModelType == OPT->DCMcodeBias ||
  OPT->_obsModelType == OPT->DCMphaseBias) {
#ifdef BNC_DEBUG_PPP
  _parlist.printParams(_epoTime);
#endif
  }
  // Status Vector, Variance-Covariance Matrix
  // -----------------------------------------
  ColumnVector xFltOld = _xFlt;
  SymmetricMatrix QFltOld = _QFlt;
  setStateVectorAndVarCovMatrix(xFltOld, QFltOld);

  // Pre-Process Satellite Systems separately
  // ----------------------------------------
  bool preProcessing = false;
  if (OPT->_obsModelType == OPT->DCMcodeBias ||
  OPT->_obsModelType == OPT->DCMphaseBias) {
    preProcessing = true;
    const QList<char> &usedSystems = _parlist.usedSystems();
    for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
      char sys = usedSystems[iSys];
      _refPrn.set(sys, 0);
      if (OPT->_refSatRequired) {
        _refPrn = refSatMap[sys]->prn();
      }
      vector<t_pppSatObs*> obsVector;
      for (unsigned jj = 0; jj < allObs.size(); jj++) {
        if (allObs[jj]->prn().system() == sys) {
          obsVector.push_back(allObs[jj]);
        }
      }
      if (iSys == 0) {
        _datumTrafo->setFirstSystem(sys);
      }
      if (processSystem(OPT->LCs(sys), obsVector, _refPrn,
          epoch->pseudoObsIono(), preProcessing) != success) {
        LOG << sys << ": processSystem !=  success (pre-processing)" << endl;
        _xFlt = xFltOld;
        _QFlt = QFltOld;
        _obsPool->deleteLastEpoch();
        restoreState(2);
        return failure;
      }
    }
    // refSat change required?
    // -----------------------
    if (_obsPool->refSatChangeRequired()) {
      _xFlt = xFltOld;
      _QFlt = QFltOld;
      return success;
    } else if (!_obsPool->refSatChangeRequired()) {
      initDatumTransformation(allObs, epoch->pseudoObsIono());
    }
  }

  // Process Satellite Systems separately
  // ------------------------------------
  preProcessing = false;
  const QList<char> &usedSystems = _parlist.usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    _refPrn.set(sys, 0);
    if (OPT->_refSatRequired) {
      _refPrn = refSatMap[sys]->prn();
    }
    unsigned int num = 0;
    vector<t_pppSatObs*> obsVector;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == sys) {
        obsVector.push_back(allObs[jj]);
        ++num;
      }
    }
    if (iSys == 0 && OPT->_obsModelType == OPT->UncombPPP) {
      _datumTrafo->setFirstSystem(sys);
    }
    LOG << epoTimeStr << " SATNUM " << sys << ' ' << right << setw(2) << num
        << endl;
    if (processSystem(OPT->LCs(sys), obsVector, _refPrn, epoch->pseudoObsIono(),
        preProcessing) != success) {
      LOG << "processSystem !=  success (fin-processing)" << endl;
      if (OPT->_obsModelType == OPT->DCMcodeBias ||
          OPT->_obsModelType == OPT->DCMphaseBias) {
        _xFlt = xFltOld;
        _QFlt = QFltOld;
        _obsPool->deleteLastEpoch();
        restoreState(3);
      }
      return failure;
    }
  }

  // close epoch processing
  // ----------------------
  cmpDOP(allObs, refSatMap);
  _parlist.printResult(_epoTime, _QFlt, _xFlt);
  _lastEpoTimeOK = _epoTime; // remember time of last successful epoch processing
  return success;
}

// Process Selected LCs
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::processSystem(const vector<t_lc::type> &LCs,
    const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
    bool pseudoObsIonoAvailable, bool preProcessing) {
  LOG.setf(ios::fixed);
  char sys = refPrn.system();

  // Detect Cycle Slips
  // ------------------
  if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
    return failure;
  }
  if (preProcessing && _obsPool->refSatChangeRequired(sys)) {
    return success;
  }

  ColumnVector xSav = _xFlt;
  SymmetricMatrix QSav = _QFlt;
  string epoTimeStr = string(_epoTime);
  const vector<t_pppParam*> &params = _parlist.params();
  unsigned nPar = _parlist.nPar();

  unsigned usedLCs = LCs.size();
  if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
    usedLCs -= 1;  // GIM not used
  }
  // max Obs
  unsigned maxObs = obsVector.size() * usedLCs;

  if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
    maxObs -= 1; // pseudo obs iono with respect to refSat
  }

  // Outlier Detection Loop
  // ----------------------
  for (unsigned iOutlier = 0; iOutlier < maxObs; iOutlier++) {

    if (iOutlier > 0) {
      _xFlt = xSav;
      _QFlt = QSav;
    }

    // First-Design Matrix, Terms Observed-Computed, Weight Matrix
    // -----------------------------------------------------------
    Matrix AA(maxObs, nPar);
    ColumnVector ll(maxObs);
    DiagonalMatrix PP(maxObs);
    PP = 0.0;

    int iObs = -1;
    vector<t_pppSatObs*> usedObs;
    vector<t_lc::type> usedTypes;

    // Real Observations
    // =================
    double nSat = 0;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      t_pppSatObs *obs = obsVector[ii];
      if (iOutlier == 0 && !preProcessing) {
        obs->resetOutlier();
      }
      if (!obs->outlier()) {
        nSat++;
        for (unsigned jj = 0; jj < usedLCs; jj++) {
          const t_lc::type tLC = LCs[jj];
          if (tLC == t_lc::GIM) {
            continue;
          }
          ++iObs;
          usedObs.push_back(obs);
          usedTypes.push_back(tLC);
          for (unsigned iPar = 0; iPar < nPar; iPar++) {
            const t_pppParam *par = params[iPar];
            AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
          }
          ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
                   - DotProduct(_x0, AA.Row(iObs + 1));
          PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
        }
      }
    }

    // Check number of observations
    // ----------------------------
    if (iObs == -1) {
      LOG << " number of observations == " << iObs + 1 << "\n";
      return failure;
    }

    if ((!iOutlier) &&
        (OPT->_obsModelType == OPT->DCMcodeBias ||
         OPT->_obsModelType == OPT->DCMphaseBias) && (!preProcessing)) {
      _datumTrafo->updateIndices(sys, iObs + 1);
      _datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, _parlist.nPar()), 1);
    }

    // Pseudo Obs Iono
    // ================
    if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) {
      for (unsigned ii = 0; ii < obsVector.size(); ii++) {
        t_pppSatObs *obs = obsVector[ii];
        if (!obs->outlier()) {
          for (unsigned jj = 0; jj < usedLCs; jj++) {
            const t_lc::type tLC = LCs[jj];
            if (tLC == t_lc::GIM && !obs->isReference()) {
              ++iObs;
            } else {
              continue;
            }
            usedObs.push_back(obs);
            usedTypes.push_back(tLC);
            for (unsigned iPar = 0; iPar < nPar; iPar++) {
              const t_pppParam *par = params[iPar];
              AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
            }
            ll[iObs] = obs->obsValue(tLC) - obs->cmpValue(tLC)
                     - DotProduct(_x0, AA.Row(iObs + 1));
            PP[iObs] = 1.0 / (obs->sigma(tLC) * obs->sigma(tLC));
          }
        }
      }
    }

    // Truncate matrices
    // -----------------
    AA = AA.Rows(1, iObs + 1);
    ll = ll.Rows(1, iObs + 1);
    PP = PP.SymSubMatrix(1, iObs + 1);

    // Kalman update step
    // ------------------
    kalman(AA, ll, PP, _QFlt, _xFlt);

    // Check Residuals
    // ---------------
    ColumnVector vv = AA * _xFlt - ll;
    double maxOutlier = 0.0;
    int maxOutlierIndex = -1;
    t_lc::type maxOutlierLC = t_lc::dummy;
    for (unsigned ii = 0; ii < usedObs.size(); ii++) {
      const t_lc::type tLC = usedTypes[ii];
      double res = fabs(vv[ii]);
      if (res > usedObs[ii]->maxRes(tLC)) {
        if (res > fabs(maxOutlier)) {
          maxOutlier = vv[ii];
          maxOutlierIndex = ii;
          maxOutlierLC = tLC;
        }
      }
    }

    // Mark outlier or break outlier detection loop
    // --------------------------------------------
    if (maxOutlierIndex > -1) {
      t_pppSatObs *obs = usedObs[maxOutlierIndex];
      t_pppParam *par = 0;
      for (unsigned iPar = 0; iPar < nPar; iPar++) {
        t_pppParam *hlp = params[iPar];
        if (hlp->type() == t_pppParam::amb && hlp->prn() == obs->prn()
            && hlp->tLC() == usedTypes[maxOutlierIndex]) {
          par = hlp;
        }
      }
      if (preProcessing) {
        // for refSats no ambiguity parameter exists
        if ((obs->prn() == refPrn)
            && (t_lc::toString(maxOutlierLC) == "l1" ||
                t_lc::toString(maxOutlierLC) == "l2")) {
          _obsPool->setRefSatChangeRequired(sys, true);
          LOG << epoTimeStr << " Outlier ("
              << ((preProcessing) ? "pre-processing) " : "fin-processing) ")
              << t_lc::toString(maxOutlierLC) << ' ' << obs->prn().toString()
              << ' ' << setw(8) << setprecision(4) << maxOutlier << endl;
          break;
        } else {
          obs->setOutlier();
        }
      } else {    // fin-processing
        LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
            << obs->prn().toString() << ' ' << setw(8) << setprecision(4)
            << maxOutlier << endl;
        if (par) {
          //if ( par->ambResetCandidate() || (OPT->_obsModelType == OPT->DCMcodeBias ||    OPT->_obsModelType == OPT->DCMphaseBias) ) {
            resetAmb(par->prn(), obsVector, &QSav, &xSav);
          //}
          //else {
          //  par->setAmbResetCandidate();
          //  obs->setOutlier();
          //}
        }
        else {
          obs->setOutlier();
        }
      }
    }
    // Print Residuals
    // ---------------
    else {
      if (!preProcessing) {
        for (unsigned jj = 0; jj < LCs.size(); jj++) {
          for (unsigned ii = 0; ii < usedObs.size(); ii++) {
            const t_lc::type tLC = usedTypes[ii];
            t_pppSatObs *obs = usedObs[ii];
            if (tLC == LCs[jj]) {
              obs->setRes(tLC, vv[ii]);
              LOG << epoTimeStr << " RES " << left << setw(3)
                  << t_lc::toString(tLC) << right << ' '
                  << obs->prn().toString();
              LOG << setw(9) << setprecision(4) << vv[ii] << endl;
            }
          }
        }
      }
      break;
    }
  }
  return success;
}

// Cycle-Slip Detection
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::detectCycleSlips(const vector<t_lc::type> &LCs,
    const vector<t_pppSatObs*> &obsVector, const t_prn &refPrn,
    bool preProcessing) {
  const double SLIP = 100.0;
  char sys = refPrn.system();
  string epoTimeStr = string(_epoTime);
  const vector<t_pppParam*> &params = _parlist.params();
  unsigned nPar = _parlist.nPar();

  for (unsigned ii = 0; ii < LCs.size(); ii++) {
    const t_lc::type &tLC = LCs[ii];
    if (t_lc::includesPhase(tLC)) {
      for (unsigned iObs = 0; iObs < obsVector.size(); iObs++) {
        const t_pppSatObs *obs = obsVector[iObs];

        // Check set Slips and Jump Counters
        // ---------------------------------
        bool slip = false;

        // in pre-processing only the reference satellite has to be checked
        if (preProcessing && obs->prn() != refPrn) {
          continue;
        }

        if (obs->slip()) {
          LOG << epoTimeStr << "cycle slip set (obs) " << obs->prn().toString()
              << endl;
          slip = true;
        }

        if (_slips[obs->prn()]._obsSlipCounter != -1
            && _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
          LOG << epoTimeStr << "cycle slip set (obsSlipCounter) "
              << obs->prn().toString() << endl;
          slip = true;
        }
        if (!preProcessing) {
          _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();
        }
        if (_slips[obs->prn()]._biasJumpCounter != -1
            && _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
          LOG << epoTimeStr << "cycle slip set (biasJumpCounter) "
              << obs->prn().toString() << endl;
          slip = true;
        }
        if (!preProcessing) {
          _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();
        }
        // Slip Set
        // --------
        if (slip) {
          if (preProcessing) {
            _obsPool->setRefSatChangeRequired(sys, true);
          } else {
            resetAmb(obs->prn(), obsVector);
          }
        }
        /* Check Pre-Fit Residuals
        // -----------------------
        else {
          if (refPrn != t_prn()) {
            return success;
          }
          ColumnVector AA(nPar);
          for (unsigned iPar = 0; iPar < nPar; iPar++) {
            const t_pppParam *par = params[iPar];
            AA[iPar] = par->partial(_epoTime, obs, tLC, refPrn);
          }
          double ll = obs->obsValue(tLC) - obs->cmpValue(tLC)
                    - DotProduct(_x0, AA);
          double vv = DotProduct(AA, _xFlt) - ll;
          if (fabs(vv) > SLIP) {
            LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC)
                << ' ' << obs->prn().toString() << ' ' << setw(8)
                << setprecision(4) << vv << endl;
            if (preProcessing) {
              _obsPool->setRefSatChangeRequired(sys, true);
            } else {
              resetAmb(obs->prn(), obsVector);
            }
          }
        }*/
      }
    }
  }
  return success;
}

// Reset Ambiguity Parameter (cycle slip)
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::resetAmb(t_prn prn, const vector<t_pppSatObs*> &obsVector,
    SymmetricMatrix *QSav, ColumnVector *xSav) {

  t_irc irc = failure;
  vector<t_pppParam*> &params = _parlist.params();
  for (unsigned iPar = 0; iPar < params.size(); iPar++) {
    t_pppParam *par = params[iPar];
    if (par->type() == t_pppParam::amb && par->prn() == prn) {
      int ind = par->indexNew();
      t_lc::type tLC = par->tLC();
      LOG << string(_epoTime) << " RESET " << par->toString() << endl;
      delete par;
      par = new t_pppParam(t_pppParam::amb, prn, tLC, &obsVector);
      par->setIndex(ind);
      params[iPar] = par;
      for (unsigned ii = 1; ii <= params.size(); ii++) {
        _QFlt(ii, ind + 1) = 0.0;
        if (QSav) {
          (*QSav)(ii, ind + 1) = 0.0;
        }
      }
      _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
      if (QSav) {
        (*QSav)(ind + 1, ind + 1) = _QFlt(ind + 1, ind + 1);
      }
      _xFlt[ind] = 0.0;
      if (xSav) {
        (*xSav)[ind] = _xFlt[ind];
      }
      _x0[ind] = par->x0();
      irc = success;
    }
  }

  return irc;
}

// Add infinite noise to iono
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::addNoiseToPar(t_pppParam::e_type parType, char sys) {
  t_irc irc = failure;
  vector<t_pppParam*> &params = _parlist.params();
  for (unsigned iPar = 0; iPar < params.size(); iPar++) {
    t_pppParam *par = params[iPar];
    if (par->type() == parType && par->prn().system() == sys) {
      int ind = par->indexNew();
      LOG << string(_epoTime) << " ADD NOISE TO " << par->toString() << endl;
      par->setIndex(ind);
      _QFlt(ind + 1, ind + 1) = par->sigma0() * par->sigma0();
      irc = success;
    }
  }

  return irc;
}

// Compute various DOP Values
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::cmpDOP(const vector<t_pppSatObs*> &obsVector,
    const QMap<char, t_pppRefSat*> &refSatMap) {

  _dop.reset();

  try {
    const unsigned numPar = 4;
    Matrix AA(obsVector.size(), numPar);
    _numSat = 0;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      t_pppSatObs *obs = obsVector[ii];
      char sys = obs->prn().system();
      t_prn refPrn = t_prn();
      if (OPT->_refSatRequired) {
        refPrn = refSatMap[sys]->prn();
      }
      if (obs->isValid() && !obs->outlier()) {
        ++_numSat;
        for (unsigned iPar = 0; iPar < numPar; iPar++) {
          const t_pppParam *par = _parlist.params()[iPar];
          AA[_numSat - 1][iPar] = par->partial(_epoTime, obs, t_lc::c1, refPrn);
        }
      }
    }
    if (_numSat < 4) {
      return;
    }
    AA = AA.Rows(1, _numSat);
    SymmetricMatrix NN;
    NN << AA.t() * AA;
    SymmetricMatrix QQ = NN.i();

    _dop.H = sqrt(QQ(1, 1) + QQ(2, 2));
    _dop.V = sqrt(QQ(3, 3));
    _dop.P = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3));
    _dop.T = sqrt(QQ(4, 4));
    _dop.G = sqrt(QQ(1, 1) + QQ(2, 2) + QQ(3, 3) + QQ(4, 4));
  } catch (...) {
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::predictCovCrdPart(const SymmetricMatrix &QFltOld) {

  const vector<t_pppParam*> &params = _parlist.params();
  if (params.size() < 3) {
    return;
  }

  bool first = (params[0]->indexOld() < 0);

  SymmetricMatrix Qneu(3);
  Qneu = 0.0;
  for (unsigned ii = 0; ii < 3; ii++) {
    const t_pppParam *par = params[ii];
    if (first) {
      Qneu[ii][ii] = par->sigma0() * par->sigma0();
    } else {
      Qneu[ii][ii] = par->noise() * par->noise();
    }
  }

  const t_pppStation *sta = PPP_CLIENT->staRover();
  SymmetricMatrix Qxyz(3);
  covariNEU_XYZ(Qneu, sta->ellApr().data(), Qxyz);

  if (first) {
    _QFlt.SymSubMatrix(1, 3) = Qxyz;
  } else {
    double dt = _epoTime - _firstEpoTime;
    if (dt < OPT->_seedingTime) {
      _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3);
    } else {
      _QFlt.SymSubMatrix(1, 3) = QFltOld.SymSubMatrix(1, 3) + Qxyz;
    }
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::setStateVectorAndVarCovMatrix(const ColumnVector &xFltOld,
    const SymmetricMatrix &QFltOld) {

  const vector<t_pppParam*> &params = _parlist.params();
  unsigned nPar = params.size();

  _QFlt.ReSize(nPar);
  _QFlt = 0.0;
  _xFlt.ReSize(nPar);
  _xFlt = 0.0;
  _x0.ReSize(nPar);
  _x0 = 0.0;

  for (unsigned ii = 0; ii < nPar; ii++) {
    t_pppParam *par1 = params[ii];
    if (QFltOld.size() == 0) {
      par1->resetIndex();
    }
    _x0[ii] = par1->x0();
    int iOld = par1->indexOld();
    if (iOld < 0) {
      _QFlt[ii][ii] = par1->sigma0() * par1->sigma0(); // new parameter
    } else {
      _QFlt[ii][ii] = QFltOld[iOld][iOld] + par1->noise() * par1->noise();
      _xFlt[ii] = xFltOld[iOld];
      for (unsigned jj = 0; jj < ii; jj++) {
        t_pppParam *par2 = params[jj];
        int jOld = par2->indexOld();
        if (jOld >= 0) {
          _QFlt[ii][jj] = QFltOld(iOld + 1, jOld + 1);
        }
      }
    }
  }
  predictCovCrdPart(QFltOld);
}

// Compute datum transformation
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::datumTransformation(
    const QMap<char, t_pppRefSat*> &refSatMap) {

  // get last epoch
  t_pppObsPool::t_epoch *epoch = _obsPool->lastEpoch();
  if (!epoch) {
    LOG << "t_pppFilter::datumTransformation: !lastEpoch" << endl;
    return failure;
  }
  _epoTime = epoch->epoTime();
  LOG.setf(ios::fixed);
  LOG << string(_epoTime) << " DATUM TRANSFORMATION " << endl;

  vector<t_pppSatObs*> &allObs = epoch->obsVector();

  const QMap<char, t_pppRefSat*> &refSatMapLastEpoch = epoch->refSatMap();

  bool pseudoObsIono = epoch->pseudoObsIono();

  // reset old and set new refSats in last epoch (ambiguities/GIM)
  // =============================================================
  if (resetRefSatellitesLastEpoch(allObs, refSatMap, refSatMapLastEpoch)
      != true) {
    LOG << "datumTransformation: resetRefSatellitesLastEpoch != success"
        << endl;
    return failure;
  }

  if (OPT->_obsModelType == OPT->UncombPPP) {
    _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);
    return success;
  }

  // set AA2
  // =======
  if (_parlist.set(_epoTime, allObs, refSatMap) != success) {
    return failure;
  }

#ifdef BNC_DEBUG_PPP
  _parlist.printParams(_epoTime);
#endif

  const QList<char> &usedSystems = _parlist.usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    t_prn refPrn = refSatMap[sys]->prn();
    vector<t_pppSatObs*> obsVector;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == sys) {
        allObs[jj]->resetOutlier();
        obsVector.push_back(allObs[jj]);
      }
    }
    if (iSys == 0) {
      _datumTrafo->setFirstSystem(sys);
    }
    vector<t_lc::type> LCs = OPT->LCs(sys);
    unsigned usedLCs = LCs.size();
    if (OPT->_pseudoObsIono && !pseudoObsIono) {
      usedLCs -= 1;  // GIM not used
    }
    // max Obs
    unsigned maxObs = obsVector.size() * usedLCs;

    if (OPT->_pseudoObsIono && pseudoObsIono) {
      maxObs -= 1; // pseudo obs iono with respect to refSat
    }

    const vector<t_pppParam*> &params = _parlist.params();
    unsigned nPar = _parlist.nPar();

    Matrix AA(maxObs, nPar);

    // Real Observations
    // -----------------
    int iObs = -1;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      t_pppSatObs *obs = obsVector[ii];
      for (unsigned jj = 0; jj < usedLCs; jj++) {
        const t_lc::type tLC = LCs[jj];
        if (tLC == t_lc::GIM) {
          continue;
        }
        ++iObs;
        for (unsigned iPar = 0; iPar < nPar; iPar++) {
          const t_pppParam *par = params[iPar];
          AA[iObs][iPar] = par->partial(_epoTime, obs, tLC, refPrn);
        }
      }
    }

    if (!iObs) {
      continue;
    }
    _datumTrafo->updateIndices(sys, iObs + 1); //LOG << "AA Ncols/Nrows: " << AA.Ncols() << "/" << AA.Nrows() << "  nPar: "  << nPar << endl;    //LOG << "AA.SubMatrix(1 .. " << iObs+1 << " , 1 .. " <<  nPar << ")" << endl;
    if (_datumTrafo->prepareAA(AA.SubMatrix(1, iObs + 1, 1, nPar), 2)
        != success) {
      return failure;
    }
  }
  _datumTrafo->updateNumObs();

  // Datum Transformation
  // ====================
#ifdef BNC_DEBUG_PPP
  //LOG << "AA1\n"; _datumTrafo->printMatrix(_datumTrafo->AA1(), _datumTrafo->numObs(), _datumTrafo->numPar());
  //LOG << "AA2\n"; _datumTrafo->printMatrix(_datumTrafo->AA2(), _datumTrafo->numObs(), _datumTrafo->numPar());
#endif
  if (_datumTrafo->computeTrafoMatrix() != success) {
    return failure;
  }
#ifdef BNC_DEBUG_PPP
  //LOG << "D21" << endl; _datumTrafo->printMatrix(_datumTrafo->D21(), _datumTrafo->numObs(), _datumTrafo->numPar());
#endif
  ColumnVector xFltOld = _xFlt;
  SymmetricMatrix QFltOld = _QFlt;

  _QFlt << _datumTrafo->D21() * QFltOld * _datumTrafo->D21().t();
  _xFlt = _datumTrafo->D21() * xFltOld;

#ifdef BNC_DEBUG_PPP
  LOG << "xFltOld:\n" << xFltOld << endl;
  LOG << "xFlt   :\n" << _xFlt   << endl;
#endif

  // Reset Ambiguities after Datum Transformation
  // ============================================
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    t_prn refPrnOld = refSatMapLastEpoch[sys]->prn();
    t_prn refPrnNew = refSatMap[sys]->prn();
    if (refPrnNew != refPrnOld) {
      resetAmb(refPrnOld, allObs);/*
      if (resetAmb(refPrnOld, allObs) == success) {
        if (OPT->_obsModelType == OPT->DCMcodeBias) {
          addNoiseToPar(t_pppParam::ion, sys);
        } else if (OPT->_obsModelType == OPT->DCMphaseBias) {
          if        (sys == 'G') {
            addNoiseToPar(t_pppParam::pBiasG1, sys);
            addNoiseToPar(t_pppParam::pBiasG2, sys);
          } else if (sys == 'R') {
            addNoiseToPar(t_pppParam::pBiasR1, sys);
            addNoiseToPar(t_pppParam::pBiasR2, sys);
          } else if (sys == 'E') {
            addNoiseToPar(t_pppParam::pBiasE1, sys);
            addNoiseToPar(t_pppParam::pBiasE2, sys);
          } else if (sys == 'C') {
            addNoiseToPar(t_pppParam::pBiasC1, sys);
            addNoiseToPar(t_pppParam::pBiasC2, sys);
          }
        }
      }*/
    }
  }

  // switch AA2 to AA1
  // =================
  _datumTrafo->switchAA();

  _obsPool->putEpoch(_epoTime, allObs, pseudoObsIono, refSatMap);

  return success;
}

// Init datum transformation
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::initDatumTransformation(
    const std::vector<t_pppSatObs*> &allObs, bool pseudoObsIono) {
  unsigned trafoObs = 0;
  const QList<char> &usedSystems = _parlist.usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    char sys = usedSystems[iSys];
    int satNum = 0;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == sys) {
        satNum++;
      }
    }
    // all LCs
    unsigned realUsedLCs = OPT->LCs(sys).size();
    // exclude pseudo obs GIM
    if (OPT->_pseudoObsIono && !pseudoObsIono) {
      realUsedLCs -= 1;
    }

    trafoObs += satNum * realUsedLCs;

    if (OPT->_pseudoObsIono && pseudoObsIono) {
      trafoObs -= 1; // pseudo obs iono with respect to refSat
    }
  }
  _datumTrafo->setNumObs(trafoObs);
  _datumTrafo->setNumPar(_parlist.nPar());
  _datumTrafo->initAA();
}

//
//////////////////////////////////////////////////////////////////////////////
bool t_pppFilter::resetRefSatellitesLastEpoch(
    std::vector<t_pppSatObs*> &obsVector,
    const QMap<char, t_pppRefSat*> &refSatMap,
    const QMap<char, t_pppRefSat*> &refSatMapLastEpoch) {
  bool resetRefSat;
  // reference satellite definition per system
  const QList<char> &usedSystems = _parlist.usedSystems();
  for (int iSys = 0; iSys < usedSystems.size(); iSys++) {
    resetRefSat = false;
    char sys = usedSystems[iSys];
    t_prn newPrn = refSatMap[sys]->prn();
    t_prn oldPrn = refSatMapLastEpoch[sys]->prn();
#ifdef BNC_DEBUG_PPP
    if (oldPrn != newPrn) {
      LOG << "oldRef: " << oldPrn.toString() << " => newRef " <<  newPrn.toString() << endl;
    }
#endif
    vector<t_pppSatObs*>::iterator it = obsVector.begin();
    while (it != obsVector.end()) {
      t_pppSatObs *satObs = *it;
      if (satObs->prn() == newPrn) {
        resetRefSat = true;
        satObs->setAsReference();
      } else if (satObs->prn() == oldPrn) {
        satObs->resetReference();
      }
      it++;
    }
    if (!resetRefSat) {
      _obsPool->setRefSatChangeRequired(sys, true);
      return resetRefSat;
    }
  }
  return resetRefSat;
}