/* -------------------------------------------------------------------------
 * BKG NTRIP Client
 * -------------------------------------------------------------------------
 *
 * Class:      t_pppEphPool
 *
 * Purpose:    Buffer with satellite ephemerides
 *
 * Author:     L. Mervart
 *
 * Created:    29-Jul-2014
 *
 * Changes:
 *
 * -----------------------------------------------------------------------*/

#include <iostream>
#include <iomanip>
#include "pppEphPool.h"
#include "pppInclude.h"
#include "pppClient.h"
#include "bncephuser.h"

using namespace BNC_PPP;
using namespace std;

//
/////////////////////////////////////////////////////////////////////////////
// Bad, outdated and unhealthy ephemerides are never used. Ephemerides from
// RINEX navigation files (post-processing) never pass bncEphUser's checks and
// stay "unchecked", so the health flag and the plausibility of the satellite
// distance are checked here directly as well.
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::putEphemeris(t_eph* eph) {
  if (!eph) {
    return;
  }
  int iPrn = eph->prn().toInt();
  if (iPrn == 0) { // satellite number out of range
    delete eph;
  }
  else if (eph->checkState() == t_eph::unhealthy || eph->isUnhealthy()) {
    _satEphPool[iPrn].setUnhealthy(eph);
  }
  else if (eph->checkState() == t_eph::unchecked && !bncEphUser::radialDistanceOk(eph)) {
    LOG << "bad ephemeris " << eph->prn().toString() << ' ' << string(eph->TOC())
        << " - implausible satellite distance, ignored" << endl;
    delete eph;
  }
  else if (eph->checkState() != t_eph::bad &&
           eph->checkState() != t_eph::outdated) {
    _satEphPool[iPrn].putEphemeris(_maxQueueSize, eph);
  }
  else {
    delete eph;
  }
}

//
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::putOrbCorrection(t_orbCorr* corr) {
  if (corr) {
    _satEphPool[corr->_prn.toInt()].putOrbCorrection(corr);
    if (OPT->_logMode > t_pppOptions::normal) {
      LOG << "orbCorr " << string(corr->_time) << ' ' << corr->_prn.toString() << endl;
    }
  }
}

//
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::putClkCorrection(t_clkCorr* corr) {
  if (corr) {
    _satEphPool[corr->_prn.toInt()].putClkCorrection(corr);
    if (OPT->_logMode > t_pppOptions::normal) {
      LOG.setf(ios::fixed);
      LOG << "clkCorr " << string(corr->_time) << ' ' << corr->_prn.toString() << ' '
          << setw(7) << setprecision(3) << corr->_dClk * t_CST::c << endl;
    }
  }
}

//
/////////////////////////////////////////////////////////////////////////////
t_irc t_pppEphPool::getCrd(const t_prn& prn, const bncTime& tt,
                             ColumnVector& xc, ColumnVector& vv) const {
  return _satEphPool[prn.toInt()].getCrd(tt, xc, vv);
}

//
/////////////////////////////////////////////////////////////////////////////
int t_pppEphPool::getChannel(const t_prn& prn) const {
  return _satEphPool[prn.toInt()].getChannel();
}

//
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::t_satEphPool::putEphemeris(unsigned maxQueueSize, t_eph* eph) {
  if (_ephs.empty() || eph->isNewerThan(_ephs.front())) {
    _ephs.push_front(eph);
    if (maxQueueSize > 0 && _ephs.size() > maxQueueSize) {
      delete _ephs.back();
      _ephs.pop_back();
    }
  }
  else {
    delete eph;
  }
}

// A newer ephemeris declaring the satellite unhealthy: rejecting it alone
// would leave the older, healthy ephemerides in the pool, and the satellite
// would still be used with them - so they are all discarded, and the
// satellite stays unused until a newer healthy ephemeris arrives.
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::t_satEphPool::setUnhealthy(t_eph* eph) {
  if (_ephs.empty() || eph->isNewerThan(_ephs.front())) {
    if (!_ephs.empty()) {
      LOG << "unhealthy ephemeris " << eph->prn().toString() << ' ' << string(eph->TOC())
          << " - older ephemerides of this satellite discarded" << endl;
    }
    for (unsigned ii = 0; ii < _ephs.size(); ii++) {
      delete _ephs[ii];
    }
    _ephs.clear();
  }
  delete eph;
}

//
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::t_satEphPool::putOrbCorrection(t_orbCorr* corr) {
  if (_ephs.empty()) {
    return;
  }

  for (unsigned ii = 0; ii < _ephs.size(); ii++) {
    t_eph* eph = _ephs[ii];
    if (eph->IOD() == corr->_iod) {
      eph->setOrbCorr(corr);
    }
    else {
      LOG << "putOrbCorrection " << corr->_prn.toString() << " corrIOD = " << corr->_iod << " ephIODs = ";
      for (auto* e : _ephs) LOG << e->IOD() << " ";
      LOG << endl;
    }
  }
  delete corr;
}

//
/////////////////////////////////////////////////////////////////////////////
void t_pppEphPool::t_satEphPool::putClkCorrection(t_clkCorr* corr) {
  if (_ephs.empty()) {
    return;
  }

  for (unsigned ii = 0; ii < _ephs.size(); ii++) {
    t_eph* eph = _ephs[ii];
    if (eph->IOD() == corr->_iod) {
      eph->setClkCorr(corr);
    }
    else {
      LOG << "putClockCorrection " << corr->_prn.toString() << " corrIOD = " << corr->_iod << " ephIODs = ";
      for (auto* e : _ephs) LOG << e->IOD() << " ";
      LOG << endl;
    }
  }
  delete corr;
}

//
/////////////////////////////////////////////////////////////////////////////
t_irc t_pppEphPool::t_satEphPool::getCrd(const bncTime& tt, ColumnVector& xc,
                                           ColumnVector& vv) const {
  if (_ephs.empty()) {
    return failure;
  }

  for (unsigned ii = 0; ii < _ephs.size(); ii++) {
    const t_eph* eph = _ephs[ii];
    t_irc irc = eph->getCrd(tt, xc, vv, OPT->useOrbClkCorr());
    if (irc == success) {
        if (outDatedBcep(eph, tt)) {
          continue;
      }
      return irc;
    }
  }
  return failure;
}

//
/////////////////////////////////////////////////////////////////////////////
int t_pppEphPool::t_satEphPool::getChannel() const {
  if (!_ephs.empty()) {
    return _ephs[0]->slotNum();
  }
  return 0;
}