// converting GNSS data streams from NTRIP broadcasters.
//
// Copyright (C) 2007
// German Federal Agency for Cartography and Geodesy (BKG)
// http://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:      bncEphUser
 *
 * Purpose:    Base for Classes that use Ephemerides
 *
 * Author:     L. Mervart
 *
 * Created:    27-Jan-2011
 *
 * Changes:
 *
 * -----------------------------------------------------------------------*/

#include <cmath>
#include <iostream>

#include "bncephuser.h"
#include "bnccore.h"
#include "bncsettings.h"
#include "RTCM3/ephEncoder.h"

using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
bncEphUser::bncEphUser(bool connectSlots, unsigned maxQueueSize) : _maxQueueSize(maxQueueSize), _voting(false) {
  if (connectSlots) {
    connect(BNC_CORE, SIGNAL(newGPSEph(t_ephGPS)),
            this, SLOT(slotNewGPSEph(t_ephGPS)), Qt::DirectConnection);

    connect(BNC_CORE, SIGNAL(newGlonassEph(t_ephGlo)),
            this, SLOT(slotNewGlonassEph(t_ephGlo)), Qt::DirectConnection);

    connect(BNC_CORE, SIGNAL(newGalileoEph(t_ephGal)),
            this, SLOT(slotNewGalileoEph(t_ephGal)), Qt::DirectConnection);

    connect(BNC_CORE, SIGNAL(newSBASEph(t_ephSBAS)),
            this, SLOT(slotNewSBASEph(t_ephSBAS)), Qt::DirectConnection);

    connect(BNC_CORE, SIGNAL(newBDSEph(t_ephBDS)),
            this, SLOT(slotNewBDSEph(t_ephBDS)), Qt::DirectConnection);
  }
}

// Ephemerides of the streams in the general option 'Ephemeris streams' only
////////////////////////////////////////////////////////////////////////////
void bncEphUser::useEphMounts(const QString& user) {
  bncSettings settings;
  _ephMounts = settings.value("ephMount").toString().split(QRegularExpression("[,\\s]+"),
                                                            Qt::SkipEmptyParts);
  if (!_ephMounts.isEmpty()) {
    BNC_CORE->message((user.trimmed() + ": ephemerides of " + _ephMounts.join(", ") + " only").toLatin1(), true, "bce", t_logRecord::info);
  }
}

// Destructor
////////////////////////////////////////////////////////////////////////////
bncEphUser::~bncEphUser() {
  QMapIterator<QString, deque<t_eph*> > it(_eph);
  while (it.hasNext()) {
    it.next();
    const deque<t_eph*>& qq = it.value();
    for (unsigned ii = 0; ii < qq.size(); ii++) {
      delete qq[ii];
    }
  }
  qDeleteAll(_votes);
}

// New GPS Ephemeris
////////////////////////////////////////////////////////////////////////////
void bncEphUser::slotNewGPSEph(t_ephGPS eph) {
  if (fromEphMount(eph)) {
    putNewEph(&eph, true);
  }
}

// New Glonass Ephemeris
////////////////////////////////////////////////////////////////////////////
void bncEphUser::slotNewGlonassEph(t_ephGlo eph) {
  if (fromEphMount(eph)) {
    putNewEph(&eph, true);
  }
}

// New Galileo Ephemeris
////////////////////////////////////////////////////////////////////////////
void bncEphUser::slotNewGalileoEph(t_ephGal eph) {
  if (fromEphMount(eph)) {
    putNewEph(&eph, true);
  }
}

// New SBAS Ephemeris
////////////////////////////////////////////////////////////////////////////
void bncEphUser::slotNewSBASEph(t_ephSBAS eph) {
  if (fromEphMount(eph)) {
    putNewEph(&eph, true);
  }
}

// New BDS Ephemeris
////////////////////////////////////////////////////////////////////////////
void bncEphUser::slotNewBDSEph(t_ephBDS eph) {
  if (fromEphMount(eph)) {
    putNewEph(&eph, true);
  }
}

//
////////////////////////////////////////////////////////////////////////////
t_irc bncEphUser::putNewEph(t_eph* eph, bool realTime) {

  QMutexLocker locker(&_mutex);

  if (eph == 0) {
    return failure;
  }

  // satellite outside the range supported by BNC (e.g. G39)
  if (eph->prn().toInt() == 0) {
    return failure;
  }

  t_eph* newEph = clone(eph);
  if (newEph == 0) {
    return failure;
  }

  QString prn(newEph->prn().toInternalString().c_str());

  // data set received before, e.g. from a slower stream after a newer one
  if (_eph.contains(prn)) {
    const deque<t_eph*>& qq = _eph[prn];
    for (unsigned ii = 0; ii < qq.size(); ii++) {
      if (qq[ii]->sameDataSet(eph)) {
        delete newEph;
        if (_voting && realTime) {
          vote(prn, ii, eph);
        }
        return failure;
      }
    }
  }

  const t_eph* ephOld = ephLast(prn);

  if (ephOld &&
      (ephOld->checkState() == t_eph::bad ||
       ephOld->checkState() == t_eph::outdated )) {
    ephOld = 0;
  }

  if ( (ephOld == 0) || eph->isNewerThan(ephOld)) {
    checkEphemeris(eph, realTime);
  }
  else {
    delete newEph;
    return failure;
  }

  if (eph->checkState() != t_eph::bad &&
      eph->checkState() != t_eph::outdated) {
    deque<t_eph*>& qq = _eph[prn];
    newEph->setCheckState(eph->checkState());
    qq.push_back(newEph);
    if (_voting && realTime) {
      t_votes* votes = new t_votes;
      votes->_content.append(votingContent(newEph));
      votes->_eph.append(clone(newEph));
      votes->_station[newEph->receptStaID()] = 0;
      _votes[newEph] = votes;
    }
    if (qq.size() > _maxQueueSize) {
      delete _votes.take(qq.front());
      delete qq.front();
      qq.pop_front();
    }
    ephBufferChanged();
    return success;
  }
  else {
    delete newEph;
    return failure;
  }
}

// Copy of an ephemeris of any system
////////////////////////////////////////////////////////////////////////////
t_eph* bncEphUser::clone(const t_eph* eph) {
  if (const t_ephGPS*  e = dynamic_cast<const t_ephGPS*>(eph))  return new t_ephGPS(*e);
  if (const t_ephGlo*  e = dynamic_cast<const t_ephGlo*>(eph))  return new t_ephGlo(*e);
  if (const t_ephGal*  e = dynamic_cast<const t_ephGal*>(eph))  return new t_ephGal(*e);
  if (const t_ephSBAS* e = dynamic_cast<const t_ephSBAS*>(eph)) return new t_ephSBAS(*e);
  if (const t_ephBDS*  e = dynamic_cast<const t_ephBDS*>(eph))  return new t_ephBDS(*e);
  return 0;
}

// Content compared in the vote: the uploaded RTCM3 message without CRC and,
// for GLONASS, without the fields differing legitimately between receivers
////////////////////////////////////////////////////////////////////////////
QByteArray bncEphUser::votingContent(const t_eph* eph) {
  unsigned char buffer[80];
  int size = 0;
  if      (const t_ephGPS*  e = dynamic_cast<const t_ephGPS*>(eph))  size = t_ephEncoder::RTCM3(*e, buffer);
  else if (const t_ephGlo*  e = dynamic_cast<const t_ephGlo*>(eph))  size = t_ephEncoder::RTCM3(*e, buffer);
  else if (const t_ephGal*  e = dynamic_cast<const t_ephGal*>(eph))  size = t_ephEncoder::RTCM3(*e, buffer);
  else if (const t_ephSBAS* e = dynamic_cast<const t_ephSBAS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
  else if (const t_ephBDS*  e = dynamic_cast<const t_ephBDS*>(eph))  size = t_ephEncoder::RTCM3(*e, buffer);
  if (size <= 6) {
    return QByteArray();
  }
  if (eph->system() == t_eph::GLONASS) {
    // message bits (after the 3 header bytes) of tk and of the system time
    // offsets tau_c and tau_GPS: receivers take the latter from the string 5
    // of any satellite, they differ within the same data set
    const int field[3][2] = {{27, 12}, {293, 32}, {330, 22}};
    for (int ii = 0; ii < 3; ii++) {
      for (int bit = 24 + field[ii][0]; bit < 24 + field[ii][0] + field[ii][1]; bit++) {
        buffer[bit / 8] &= ~(0x80 >> (bit % 8));
      }
    }
  }
  return QByteArray((const char*) buffer, size - 3);
}

// Names of the fields in which two vote contents (RTCM3 messages) differ
////////////////////////////////////////////////////////////////////////////
namespace {
struct t_field {const char* name; int nbits;};
const t_field fields1019[] = {{"type",12},{"sv",6},{"week",10},{"URA",4},{"L2code",2},{"IDOT",14},{"IODE",8},
  {"toc",16},{"af2",8},{"af1",16},{"af0",22},{"IODC",10},{"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},
  {"Cus",16},{"sqrtA",32},{"toe",16},{"Cic",16},{"Omega0",32},{"Cis",16},{"i0",32},{"Crc",16},{"omega",32},
  {"OmegaDot",24},{"TGD",8},{"health",6},{"L2P",1},{"fit",1},{0,0}};
const t_field fields1020[] = {{"type",12},{"sv",6},{"fc",5},{"Cn",1},{"A",1},{"P1",2},{"tk",12},{"Bn",1},{"P2",1},
  {"tb",7},{"xv",24},{"x",27},{"xa",5},{"yv",24},{"y",27},{"ya",5},{"zv",24},{"z",27},{"za",5},{"P3",1},
  {"gamma",11},{"P",2},{"ln3",1},{"tau",22},{"dtau",5},{"En",5},{"P4",1},{"FT",4},{"NT",11},{"M",2},{"add",1},
  {"NA",11},{"tauc",32},{"N4",5},{"tauGPS",22},{"ln5",1},{0,0}};
const t_field fields1042[] = {{"type",12},{"sv",6},{"week",13},{"URAI",4},{"IDOT",14},{"AODE",5},{"toc",17},
  {"a2",11},{"a1",22},{"a0",24},{"AODC",5},{"Crs",18},{"dn",16},{"M0",32},{"Cuc",18},{"e",32},{"Cus",18},
  {"sqrtA",32},{"toe",17},{"Cic",18},{"Omega0",32},{"Cis",18},{"i0",32},{"Crc",18},{"omega",32},{"OmegaDot",24},
  {"TGD1",10},{"TGD2",10},{"SatH1",1},{0,0}};
const t_field fields1044[] = {{"type",12},{"sv",4},{"toc",16},{"af2",8},{"af1",16},{"af0",22},{"IODE",8},
  {"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},{"Cus",16},{"sqrtA",32},{"toe",16},{"Cic",16},{"Omega0",32},
  {"Cis",16},{"i0",32},{"Crc",16},{"omega",32},{"OmegaDot",24},{"IDOT",14},{"L2code",2},{"week",10},{"URA",4},
  {"health",6},{"TGD",8},{"IODC",10},{"fit",1},{0,0}};
#define GAL_COMMON {"type",12},{"sv",6},{"week",12},{"IODnav",10},{"SISA",8},{"IDOT",14},{"toc",14},{"af2",6}, \
  {"af1",21},{"af0",31},{"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},{"Cus",16},{"sqrtA",32},{"toe",14}, \
  {"Cic",16},{"Omega0",32},{"Cis",16},{"i0",32},{"Crc",16},{"omega",32},{"OmegaDot",24},{"BGDa",10}
const t_field fields1045[] = {GAL_COMMON,{"E5aHS",2},{"E5aDVS",1},{0,0}};
const t_field fields1046[] = {GAL_COMMON,{"BGDb",10},{"E5bHS",2},{"E5bDVS",1},{"E1BHS",2},{"E1BDVS",1},{0,0}};

unsigned long long getBits(const QByteArray& buf, int pos, int nbits) {
  unsigned long long val = 0;
  for (int bit = pos; bit < pos + nbits; bit++) {
    val = (val << 1) | ((static_cast<unsigned char>(buf[bit / 8]) >> (7 - bit % 8)) & 1);
  }
  return val;
}
}

QString bncEphUser::differingFields(const QByteArray& c1, const QByteArray& c2) {
  if (c1.size() != c2.size() || c1.size() < 5) {
    return "content";
  }
  const t_field* fields = 0;
  switch (getBits(c1, 24, 12)) {
    case 1019: fields = fields1019; break;
    case 1020: fields = fields1020; break;
    case 1042: fields = fields1042; break;
    case 1044: fields = fields1044; break;
    case 1045: fields = fields1045; break;
    case 1046: fields = fields1046; break;
  }
  if (!fields) {
    return "content";
  }
  QStringList names;
  int pos = 24;
  for (int ii = 0; fields[ii].name; ii++) {
    if (pos + fields[ii].nbits > 8 * c1.size()) {
      break;
    }
    if (getBits(c1, pos, fields[ii].nbits) != getBits(c2, pos, fields[ii].nbits)) {
      names << fields[ii].name;
    }
    pos += fields[ii].nbits;
  }
  return names.isEmpty() ? QString("content") : names.join(" ");
}

// Further copy of the buffered data set qq[index] received from a stream
////////////////////////////////////////////////////////////////////////////
void bncEphUser::vote(const QString& prn, unsigned index, const t_eph* eph) {
  deque<t_eph*>& qq = _eph[prn];
  t_votes* votes = _votes.value(qq[index], 0);
  if (!votes) {
    return;
  }
  QByteArray content = votingContent(eph);
  int iVar = votes->_content.indexOf(content);
  if (iVar < 0) {
    iVar = votes->_content.size();
    votes->_content.append(content);
    votes->_eph.append(clone(eph));
  }
  votes->_station[eph->receptStaID()] = iVar;

  QVector<int> count(votes->_content.size(), 0);
  QMapIterator<QString, int> it(votes->_station);
  while (it.hasNext()) {
    it.next();
    count[it.value()]++;
  }
  int iBest = votes->_current;
  for (int ii = 0; ii < count.size(); ii++) {
    if (count[ii] > count[iBest]) {
      iBest = ii;
    }
  }
  if (iBest == votes->_current) {
    return;
  }

  t_eph* oldEph = qq[index];
  t_eph* newEph = clone(votes->_eph[iBest]);
  if (!radialDistanceOk(newEph)) {
    delete newEph;
    return;
  }
  if      (newEph->isUnhealthy()) {
    newEph->setCheckState(t_eph::unhealthy);
  }
  else if (oldEph->checkState() == t_eph::unhealthy) {
    newEph->setCheckState(t_eph::ok);
  }
  else {
    newEph->setCheckState(oldEph->checkState());
  }

  QStringList stations;
  for (int ii = 0; ii < count.size(); ii++) {
    QStringList hlp;
    QMapIterator<QString, int> is(votes->_station);
    while (is.hasNext()) {
      is.next();
      if (is.value() == ii) {
        hlp << is.key();
      }
    }
    if (!hlp.isEmpty()) {
      stations << (ii == iBest ? "+ " : "- ") + hlp.join(" ");
    }
  }
  BNC_CORE->message(QString("ephemeris vote %1 %2 %3: copy of %4 replaced by that of %5 (%6 of %7 streams; %8; differing: %9)")
                    .arg(prn.left(3))
                    .arg(oldEph->TOC().datestr().c_str())
                    .arg(oldEph->TOC().timestr(0).c_str())
                    .arg(oldEph->receptStaID())
                    .arg(newEph->receptStaID())
                    .arg(count[iBest])
                    .arg(votes->_station.size())
                    .arg(stations.join(", "))
                    .arg(differingFields(votes->_content[votes->_current], votes->_content[iBest])).toLatin1(),
                    false, "bce", t_logRecord::info);

  _votes.remove(oldEph);
  delete oldEph;
  qq[index] = newEph;
  votes->_current = iBest;
  _votes[newEph] = votes;
  ephBufferChanged();
}

//
////////////////////////////////////////////////////////////////////////////
bool bncEphUser::radialDistanceOk(const t_eph* eph) {
  ColumnVector xc(6);
  ColumnVector vv(3);
  if (eph->getCrd(eph->TOC(), xc, vv, false) != success) {
    return false;
  }
  double rr = xc.Rows(1,3).NormFrobenius();

  const double MINDIST = 2.e7;
  const double MAXDIST = 6.e7;
  if (rr < MINDIST || rr > MAXDIST || std::isnan(rr)) {
    return false;
  }
  return true;
}

//
////////////////////////////////////////////////////////////////////////////
void bncEphUser::checkEphemeris(t_eph* eph, bool realTime) {

  if (!eph) {
    return;
  }

  // Check whether the epoch is too far away the current time
  // --------------------------------------------------------
  if (realTime) {
    QDateTime now = currentDateAndTimeGPS();
    bncTime   currentTime(now.toString(Qt::ISODate).toStdString());
    if (outDatedBcep(eph, currentTime)) {
      eph->setCheckState(t_eph::outdated);
      return;
    }
  }

  // Check health status
  // -------------------
  if (eph->isUnhealthy()) {
    eph->setCheckState(t_eph::unhealthy);
    return;
  }

  // GPS/QZSS: IODE and IODC of the same upload
  // -------------------------------------------
  const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
  if (ephGPS && !ephGPS->iodeIodcConsistent()) {
    eph->setCheckState(t_eph::bad);
    return;
  }

  // Simple Check - check satellite radial distance
  // ----------------------------------------------
  if (!radialDistanceOk(eph)) {
    eph->setCheckState(t_eph::bad);
    return;
  }
  ColumnVector xc(6);
  ColumnVector vv(3);
  eph->getCrd(eph->TOC(), xc, vv, false);

  // Check consistency with older ephemeris
  // --------------------------------------
  const double MAXDIFF_INIT = 10.0;
  const double MINDIFF      = 0.005;
  QString      prn          = QString(eph->prn().toInternalString().c_str());
  t_eph*       ephL         = ephLast(prn);

  // maxdiff adapted to individual systems
  char   sys     = eph->prn().system();
  double MAXDIFF = MAXDIFF_INIT;
  if (sys == 'C' ||
      sys == 'S'    ) {
    MAXDIFF *= 10.0;
  }

  if (ephL && ephL->checkState() != t_eph::bad) {
    // Update of ephemeris data sets after an outage: if the time since the
    // last data set exceeds the system's update interval, the last one is
    // outdated and a consistency check against it (extrapolated over the
    // whole gap) is meaningless - it would reject the first new data set
    // after every gap. The new data set stays unchecked and serves as
    // reference for the next one.
    // -----------------------------------------------------------------
    double dt = eph->TOC() - ephL->TOC();

    // update interval: 2h
    if      (eph->system() == t_eph::GPS     && dt >  7200.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }
    // update interval: 3h
    else if (eph->system() == t_eph::Galileo && dt > 10800.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }
    // updated every 30 minutes + 5 min
    else if (eph->system() == t_eph::GLONASS && dt >  2100.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }
    // updated every ?
    else if (eph->system() == t_eph::QZSS    && dt >  3600.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }
    // maximum update interval: 300 sec
    else if  (eph->system() == t_eph::SBAS   && dt >   600.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }
    // updates 1h + 5 min
    else if  (eph->system() == t_eph::BDS    && dt >  3900.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }
    // update interval: up to 24 hours
    else if  (eph->system() == t_eph::NavIC  && dt > 86400.0) {
      ephL->setCheckState(t_eph::outdated);
      return;
    }

    ColumnVector xcL(6);
    ColumnVector vvL(3);
    if (ephL->getCrd(eph->TOC(), xcL, vvL, false) != success) {
      ephL->setCheckState(t_eph::bad);
      return;
    }

    double diff  = (xc.Rows(1,3) - xcL.Rows(1,3)).NormFrobenius();
    double diffC = fabs(xc(4) - xcL(4)) * t_CST::c;

    if (diff  < MAXDIFF &&
        diffC < MAXDIFF) {
      if (diff  < MINDIFF &&
          diffC < MINDIFF &&
          ephL->checkState() == t_eph::ok) {
        // to prevent the same data sets with different TOC values
        eph->setCheckState(t_eph::bad);
      }
      else {
        eph->setCheckState(t_eph::ok);
        ephL->setCheckState(t_eph::ok);
      }
    }
    else {
      if (ephL->checkState() == t_eph::ok) {
        eph->setCheckState(t_eph::bad);
      }
    }
  }
}

