/* -------------------------------------------------------------------------
 * 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"
#include "pppClient.h"

using namespace BNC_PPP;
using namespace std;

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

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

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

  if (!_parlist) {
    _parlist = new t_pppParlist();
  }

  // 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);

  //--
  // Set Parameters
  // --------------
  _parlist->set(_epoTime, allObs, _obsPool->getRefSatMap());
  const vector<t_pppParam*>& params = _parlist->params();

  // Status Vector, Variance-Covariance Matrix
  // -----------------------------------------
  ColumnVector    xFltOld = _xFlt;
  SymmetricMatrix QFltOld = _QFlt;

  _QFlt.ReSize(_parlist->nPar()); _QFlt = 0.0;
  _xFlt.ReSize(_parlist->nPar()); _xFlt = 0.0;
  _x0.ReSize(_parlist->nPar());   _x0   = 0.0;
  for (unsigned ii = 0; ii < params.size(); ii++) {
    const t_pppParam* par1 = params[ii];
    _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++) {
        const t_pppParam* par2 = params[jj];
        int               jOld = par2->indexOld();
        if (jOld >= 0) {
          _QFlt[ii][jj] = QFltOld(iOld+1,jOld+1);
        }
      }
    }
  }
  predictCovCrdPart(QFltOld);

  // Init Datum Trafo
  // ----------------------------------------
  if ((OPT->_obsModelType == OPT->DCMcodeBias ||
       OPT->_obsModelType == OPT->DCMphaseBias) &&
      (_numEpoProcessing == 1)) {
    _numAllUsedLCs = 0;
    for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
      char system = OPT->systems()[iSys];
      _numAllUsedLCs += OPT->LCs(system).size();
      if (OPT->_pseudoObsIono && !epoch->pseudoObsIono()) {
        _numAllUsedLCs -= 1;  // GIM not used
      }
    }
    int maxObs = allObs.size() * _numAllUsedLCs;
    _datumTrafo->initAA(maxObs, _parlist->nPar());
  }

  // Pre-Process Satellite Systems separately
  // ----------------------------------------
  bool preProcessing = false;
  if (OPT->_obsModelType == OPT->DCMcodeBias ||
      OPT->_obsModelType == OPT->DCMphaseBias) {
    preProcessing = true;
    for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
      (iSys) ? _datumTrafo->setFirstSystem(false) : _datumTrafo->setFirstSystem(true);
      char system = OPT->systems()[iSys];
      if (OPT->_refSatRequired) {
        _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
        if (_obsPool->hasHistoricalRefSat(_refPrn)) {
          return failure;
        }
      }
      vector<t_pppSatObs*> obsVector;
      for (unsigned jj = 0; jj < allObs.size(); jj++) {
        if (allObs[jj]->prn().system() == system) {
          obsVector.push_back(allObs[jj]);
        }
      }
      if (processSystem(OPT->LCs(system), obsVector, _refPrn,
                        epoch->pseudoObsIono(), preProcessing) != success) {
        return failure;
      }
    }
    // refSat change required?
    // -----------------------
    if (_obsPool->refSatChangeRequired()) {
      _xFlt = xFltOld;
      _QFlt = QFltOld;
      return success;
    }
    else if (!_obsPool->refSatChangeRequired() &&
             _numEpoProcessing > 1) {
      datumTransformation(xFltOld, QFltOld);
    }
  }

  // Process Satellite Systems separately
  // ------------------------------------
  preProcessing = false;
  for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
    char system = OPT->systems()[iSys];
    if (OPT->_refSatRequired) {
      _refPrn = (_obsPool->getRefSatMapElement(system))->prn();
    }
    unsigned int num = 0;
    vector<t_pppSatObs*> obsVector;
    for (unsigned jj = 0; jj < allObs.size(); jj++) {
      if (allObs[jj]->prn().system() == system) {
        obsVector.push_back(allObs[jj]);
        num++;
      }
    }
    LOG << epoTimeStr << " SATNUM " << system << ' ' << right << setw(2) << num << endl;
    if (processSystem(OPT->LCs(system), obsVector, _refPrn,
                      epoch->pseudoObsIono(), preProcessing) != success) {
      return failure;
    }
  }

  // close epoch processing
  // ----------------------
  cmpDOP(allObs);
  _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);

  // Detect Cycle Slips
  // ------------------

  if (detectCycleSlips(LCs, obsVector, refPrn, preProcessing) != success) {
    return failure;
  }

  unsigned usedLCs = LCs.size(); //qDebug() << "usedLCs: " << usedLCs;
  if (OPT->_pseudoObsIono && !pseudoObsIonoAvailable) {
    usedLCs -= 1;  // GIM not used
  }
  ColumnVector               xSav       = _xFlt;
  SymmetricMatrix            QSav       = _QFlt;
  string                     epoTimeStr = string(_epoTime);
  const vector<t_pppParam*>& params     = _parlist->params();
  unsigned                   maxObs     = obsVector.size() * usedLCs;
  //unsigned                   maxObs     = 2 * usedLCs;

  if (OPT->_pseudoObsIono && pseudoObsIonoAvailable) { // stecDiff w.r.t refSat
    maxObs -= 1;
  }

  // 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, _parlist->nPar());
    ColumnVector          ll(maxObs);
    DiagonalMatrix        PP(maxObs); PP = 0.0;

    int iObs = -1;
    vector<t_pppSatObs*> usedObs;
    vector<t_lc::type>   usedTypes;
    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()) {continue;}
          ++iObs;
          usedObs.push_back(obs);
          usedTypes.push_back(tLC);
          for (unsigned iPar = 0; iPar < params.size(); 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));
        }
      }
    }

    if ((preProcessing && ! iOutlier) &&
        (OPT->_obsModelType == OPT->DCMcodeBias || OPT->_obsModelType == OPT->DCMphaseBias)) {
   	  _datumTrafo->updateIndices(maxObs);
      _datumTrafo->prepareAA(AA, _numEpoProcessing, _parlist->nPar());
      if (_obsPool->refSatChangeRequired()) { // from detectCycleSlips()
        return success;
      }
    }

    // Check number of observations, truncate matrices
    // -----------------------------------------------
    if (iObs == -1) {
      return failure;
    }
    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;
      if (!preProcessing) {
        LOG << epoTimeStr << " Outlier " << t_lc::toString(maxOutlierLC) << ' '
            << obs->prn().toString()                        << ' '
            << setw(8) << setprecision(4) << maxOutlier << endl;
      }
      for (unsigned iPar = 0; iPar < params.size(); iPar++) {
        t_pppParam* hlp = params[iPar];
        if (hlp->type() == t_pppParam::amb &&
            hlp->prn()  == obs->prn() &&
            hlp->tLC()  == usedTypes[maxOutlierIndex]) {
          par = hlp;
        }
      }
      if (preProcessing) {
        if (par && obs->prn() == refPrn) {
          _obsPool->setRefSatChangeRequired(true);
          break;
        }
      }
      else {
        if (par) {
          if (par->ambResetCandidate() || _obsPool->hasHistoricalRefSat(par->prn())) {
            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().substr(0,3) << ' '
                  << setw(8) << 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) {
  if (_obsPool->hasHistoricalRefSat(refPrn)) {
    return success;
  }

  double                  SLIP       = 20.0;  // slip threshold
  string                  epoTimeStr = string(_epoTime);
  const vector<t_pppParam*>& params  = _parlist->params();

  if (preProcessing) {
    SLIP += 10.0;
  }

  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];

        if (preProcessing && obs->prn() != refPrn) {continue;}

        // Check set Slips and Jump Counters
        // ---------------------------------
        bool slip = false;
        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(true);
          }
          else {
            resetAmb(obs->prn(), obsVector);
          }
        }
        // Check Pre-Fit Residuals
        // -----------------------
        else {
          ColumnVector AA(params.size());
          for (unsigned iPar = 0; iPar < params.size(); 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) {
            if (preProcessing) {
              _obsPool->setRefSatChangeRequired(true);
            }
            else {
              LOG << epoTimeStr << " cycle slip detected " << t_lc::toString(tLC) << ' '
                  << obs->prn().toString() << ' ' << setw(8) << setprecision(4) << vv << endl;
              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;
}

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

  _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 system = obs->prn().system();
      t_prn refPrn = t_prn();
      if (OPT->_refSatRequired) {
        refPrn = _obsPool->getRefSatMapElement(system)->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 (...) {
  }
}

// Compute various DOP Values
////////////////////////////////////////////////////////////////////////////
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;
    }
  }
}

// Compute datum transformation
////////////////////////////////////////////////////////////////////////////
void t_pppFilter::datumTransformation(const ColumnVector& xFltOld, const SymmetricMatrix& QFltOld) {

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


// Reset Ambiguity Parameter (cycle slip)
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::addInfiniteNoise(t_pppParam::e_type para) {
  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() == para) {
      int ind = par->indexNew();
      if (ind < 0) {
        return irc;
      }
      _QFlt(ind+1,ind+1) += par->sigma0() * par->sigma0();
      irc = success;
    }
  }
  return irc;
}