// 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_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() {
  _parlist = 0;
}

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

// Process Single Epoch
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::processEpoch(t_pppObsPool* obsPool) {

  _numSat     = 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();
  
  // Set Parameters
  // --------------
  _parlist->set(_epoTime, allObs);
  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);
        }
      }
    }
  }

  // Process Satellite Systems separately
  // ------------------------------------
  for (unsigned iSys = 0; iSys < OPT->systems().size(); iSys++) {
    char system = OPT->systems()[iSys];
    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) != success ) {
      return failure;
    }
  }
   
  cmpDOP(allObs);

  _parlist->printResult(_epoTime, _QFlt, _xFlt);

  return success;
}

// Process Selected LCs
////////////////////////////////////////////////////////////////////////////
t_irc t_pppFilter::processSystem(const vector<t_lc::type>& LCs, 
                                 const vector<t_pppSatObs*>& obsVector) {

  LOG.setf(ios::fixed);

  // Detect Cycle Slips
  // ------------------
  if (detectCycleSlips(LCs, obsVector) != success) {
    return failure;
  }

  ColumnVector            xSav       = _xFlt;
  SymmetricMatrix         QSav       = _QFlt;
  string                  epoTimeStr = string(_epoTime);
  const vector<t_pppParam*>& params     = _parlist->params();
  unsigned                maxObs     = obsVector.size() * LCs.size();
    
  // 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);
    UpperTriangularMatrix Sl(maxObs); Sl = 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()) {
        Matrix CC(LCs.size(), 4);
        for (unsigned jj = 0; jj < LCs.size(); jj++) {
          const t_lc::type tLC = LCs[jj];
          ++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);
          }
          ll[iObs]       = obs->obsValue(tLC) - obs->cmpValue(tLC) - DotProduct(_x0, AA.Row(iObs+1));
          if (LCs.size() > 1) {
            ColumnVector coeff(4);
            obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
            CC[jj][0] = coeff[0] * obs->sigma(t_lc::l1);
            CC[jj][1] = coeff[1] * obs->sigma(t_lc::l2);
            CC[jj][2] = coeff[2] * obs->sigma(t_lc::c1);
            CC[jj][3] = coeff[3] * obs->sigma(t_lc::c2);
          }
          else {
            Sl[iObs][iObs] = obs->sigma(tLC);
          }
        }
        if (LCs.size() > 1) {
          SymmetricMatrix QQ; QQ << CC * CC.t();
          Sl.SymSubMatrix(iObs-LCs.size()+2, iObs+1) = Cholesky(QQ).t();
        }
      }
    }

    // Check number of observations, truncate matrices
    // -----------------------------------------------
    if (iObs+1 < OPT->_minObs) {
      return failure;
    }
    AA = AA.Rows(1, iObs+1);
    ll = ll.Rows(1, iObs+1);
    Sl = Sl.SymSubMatrix(1, iObs+1);

    // Kalman update step
    // ------------------
    DiagonalMatrix PP(iObs+1); PP = 0.0;
    for (int ii = 1; ii <= iObs+1; ii++) {
      PP(ii,ii) = 1.0 / (Sl(ii,ii) * Sl(ii,ii));
    }
    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;
      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 (par) {
        if (par->ambResetCandidate()) {
          resetAmb(par->prn(), obsVector, &QSav, &xSav);
        }
        else {
          par->setAmbResetCandidate();
          obs->setOutlier();
        }
      }
      else {
        obs->setOutlier();
      }
    }

    // Print Residuals
    // ---------------
    else {
      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() << ' ' 
                << 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 double            SLIP       = 20.0;  // slip threshold
  string                  epoTimeStr = string(_epoTime);
  const vector<t_pppParam*>& params     = _parlist->params();

  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;

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

        if (_slips[obs->prn()]._obsSlipCounter != -1 &&
            _slips[obs->prn()]._obsSlipCounter != obs->slipCounter()) {
          LOG << "cycle slip set (obsSlipCounter)" << endl;
          slip = true;
        }
        _slips[obs->prn()]._obsSlipCounter = obs->slipCounter();

        if (_slips[obs->prn()]._biasJumpCounter != -1 &&
            _slips[obs->prn()]._biasJumpCounter != obs->biasJumpCounter()) {
          LOG << "cycle slip set (biasJumpCounter)" << endl;
          slip = true;
        }
        _slips[obs->prn()]._biasJumpCounter = obs->biasJumpCounter();

        // Slip Set
        // --------  
        if (slip) {
          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);
          }
          
          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;
            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];
      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);
        }
      }
    }
    if (_numSat < 4) {
      return;
    }
    AA = AA.Rows(1, _numSat);
    SymmetricMatrix NN; NN << AA.t() * AA;  
    SymmetricMatrix QQ = NN.i();
    
    _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 (...) {
  }
}