#include <iostream>
#include <iomanip>
#include <stdlib.h>
#include <string.h>
#include <stdexcept>

#include "pppMain.h"
#include "ephpool.h"
#include "obspool.h"
#include "satbias.h"
#include "genconst.h"
#include "utils.h"
#include "station.h"
#include "tides.h"
#include "antex.h"
#include "filter.h"

using namespace BNC;
using namespace std;

// Global Variable
//////////////////////////////////////////////////////////////////////////////
t_pppMain* pppMain = 0;

// Constructor
//////////////////////////////////////////////////////////////////////////////
t_pppMain::t_pppMain() {
  _opt      = 0;
  _output   = 0;
  _antex    = 0;
  _log      = new ostringstream();
  _ephPool  = new t_ephPool();
  _obsPool  = new t_obsPool();
  _staRover = new t_station(e_rover);
  _staBase  = new t_station(e_base);
  _filter   = new t_filter();
}

// Destructor
//////////////////////////////////////////////////////////////////////////////
t_pppMain::~t_pppMain() {
  delete _log;
  delete _opt;
  delete _ephPool;
  delete _obsPool;
  delete _staRover;
  delete _staBase;
  delete _antex;
  delete _filter;
  clearObs();
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::setOptions(const t_pppOpt* opt) {
  delete _opt;
  _opt = new t_options(opt);

  delete _antex; _antex = 0;
  if (!_opt->antexFileName().empty()) {
    _antex = new t_antex(_opt->antexFileName());
  }

  _opt->print();
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::putGPSEphemeris(const t_ephGPS* eph) {
  _ephPool->putEphemeris(new t_ephGPS(*eph));
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::putGloEphemeris(const t_ephGlo* eph) {
  _ephPool->putEphemeris(new t_ephGlo(*eph));
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::putOrbCorrections(int numCorr, const t_pppOrbCorr* corr) {
  for (int ii = 0; ii < numCorr; ii++) {
    _ephPool->putOrbCorrection(new t_orbCorr(corr[ii]));
  }
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::putClkCorrections(int numCorr, const t_pppClkCorr* corr) {
  for (int ii = 0; ii < numCorr; ii++) {
    _ephPool->putClkCorrection(new t_clkCorr(corr[ii]));
  }
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::putBiases(int numBiases, const t_pppSatBiases* biases) {
  for (int ii = 0; ii < numBiases; ii++) {
    _obsPool->putBiases(new t_satBias(biases[ii]));
  }
}

// 
//////////////////////////////////////////////////////////////////////////////
t_irc::irc t_pppMain::prepareObs(int numSat, const t_pppSatObs* satObs,
                                 vector<t_satObs*>& obsVector, t_time& epoTime) {
  // Default 
  // -------
  epoTime.reset();

  // Create vector of valid observations
  // -----------------------------------
  int numValidGPS = 0;
  for (int ii = 0; ii < numSat; ii++) {
    char system = satObs[ii]._satellite._system;
    if (system == 'G' || (system == 'R' && OPT->useGlonass())) {
      t_satObs* satObs = new t_satObs(satObs[ii]);
      if (satObs->isValid()) {
        obsVector.push_back(satObs);
        if (satObs->prn().system() == 'G') {
          ++numValidGPS;
        }
      }
      else {
        delete satObs;
      }
    }
  }

  // Check whether data are synchronized, compute epoTime
  // ----------------------------------------------------
  const double MAXSYNC = 0.05; // synchronization limit
  double meanDt = 0.0;
  for (unsigned ii = 0; ii < obsVector.size(); ii++) {
    const t_satObs* satObs = obsVector.at(ii);
    if (epoTime.undef()) {
      epoTime = satObs->time();
    }
    else {
      double dt = satObs->time() - epoTime;
      if (fabs(dt) > MAXSYNC) {
        LOG << "t_pppMain::prepareObs asynchronous observations" << endl;
        return t_irc::failure;
      }
      meanDt += dt;
    }
  }
  epoTime += meanDt / obsVector.size();

  return t_irc::success;
}

// Compute the Bancroft position, check for blunders
//////////////////////////////////////////////////////////////////////////////
t_irc::irc t_pppMain::cmpBancroft(const t_time& epoTime, 
                                  vector<t_satObs*>& obsVector,
                                  ColumnVector& xyzc, bool print) {

  t_lc::type tLC = (OPT->dualFreqRequired() ? t_lc::cIF : t_lc::c1);

  while (true) {
    Matrix BB(obsVector.size(), 4);
    int iObs = -1;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      const t_satObs* satObs = obsVector.at(ii);
      if ( satObs->isValid() && satObs->prn().system() == 'G' &&
           (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) {
        ++iObs;   
        BB[iObs][0] = satObs->xc()[0];
        BB[iObs][1] = satObs->xc()[1];
        BB[iObs][2] = satObs->xc()[2];
        BB[iObs][3] = satObs->obsValue(tLC) - satObs->cmpValueForBanc(tLC);
      }
    }
    if (iObs + 1 < OPT->minobs()) {
      LOG << "t_pppMain::cmpBancroft not enough observations" << endl;
      return t_irc::failure;
    }
    BB = BB.Rows(1,iObs+1);
    bancroft(BB, xyzc);

    xyzc[3] /= t_genConst::c;

    // Check Blunders
    // --------------
    const double BLUNDER = 100.0;
    double   maxRes      = 0.0;
    unsigned maxResIndex = 0;
    for (unsigned ii = 0; ii < obsVector.size(); ii++) {
      const t_satObs* satObs = obsVector.at(ii);
      if ( satObs->isValid() && satObs->prn().system() == 'G' &&
           (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) {
        ColumnVector rr = satObs->xc().Rows(1,3) - xyzc.Rows(1,3);
        double res = rr.norm_Frobenius() - satObs->obsValue(tLC) 
          - (satObs->xc()[3] - xyzc[3]) * t_genConst::c;
        if (fabs(res) > maxRes) {
          maxRes      = fabs(res);
          maxResIndex = ii;
        }
      }
    }
    if (maxRes < BLUNDER) {
      if (print) {
        LOG.setf(ios::fixed);
        LOG << string(epoTime) << " BANCROFT:"        << ' '
            << setw(14) << setprecision(3) << xyzc[0] << ' '
            << setw(14) << setprecision(3) << xyzc[1] << ' '
            << setw(14) << setprecision(3) << xyzc[2] << ' '
            << setw(14) << setprecision(3) << xyzc[3] * t_genConst::c << endl << endl;
      }
      break;
    }
    else {
      t_satObs* satObs = obsVector.at(maxResIndex);
      LOG << "t_pppMain::cmpBancroft outlier " << satObs->prn().toString()
          << " " << maxRes << endl;
      delete satObs;
      obsVector.erase(obsVector.begin() + maxResIndex);
    }
  }

  return t_irc::success;
}

// Compute A Priori GPS-Glonass Offset
//////////////////////////////////////////////////////////////////////////////
double t_pppMain::cmpOffGG(vector<t_satObs*>& obsVector) {

  t_lc::type tLC   = (OPT->dualFreqRequired() ? t_lc::cIF : t_lc::c1);
  double     offGG = 0.0;

  if (OPT->useGlonass()) {
    while (true) {
      offGG = 0.0;
      bool outlierFound = false;
      unsigned nObs  = 0;
      for (unsigned ii = 0; ii < obsVector.size(); ii++) {
        t_satObs* satObs = obsVector.at(ii);
        if ( !satObs->outlier() && satObs->isValid() && satObs->prn().system() == 'R' &&
             (!satObs->modelSet() || satObs->eleSat() >= OPT->minEle()) ) {
          ++nObs;
          double ll = satObs->obsValue(tLC) - satObs->cmpValue(tLC);
          if (fabs(ll) > 1000.0) {
            satObs->setOutlier();
            outlierFound = true;
           LOG << "t_pppMain::cmpOffGG outlier " << satObs->prn().toString()
               << " " << ll << endl;
          }
          offGG += ll;
        }
      }
      if (nObs > 0) {
        offGG = offGG / nObs;
      }
      else {
        offGG = 0.0;
      }
      if (!outlierFound) {
        break;
      }
    }
  }

  return offGG;
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::initOutput(t_pppOutput* output) {
  _output = output;
  _output->_epoTime._mjd = 0;
  _output->_epoTime._sec = 0.0;
  _output->_numSat       = 0;
  _output->_pDop         = 0.0;
  _output->_ambFixRate   = 0.0;
  _output->_log          = 0;
  _output->_error        = false;
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::clearObs() {
  for (unsigned ii = 0; ii < _obsRover.size(); ii++) {
    delete _obsRover.at(ii);
  }
  _obsRover.clear();
  for (unsigned ii = 0; ii < _obsBase.size(); ii++) {
    delete _obsBase.at(ii);
  }
  _obsBase.clear();
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::finish(t_irc::irc irc) {

  clearObs();

  _output->_epoTime._mjd = _epoTimeRover.mjd();
  _output->_epoTime._sec = _epoTimeRover.daysec();

  if (irc == t_irc::success) {
    _output->_xyzRover[0] = _staRover->xyzApr()[0] + _filter->x()[0];
    _output->_xyzRover[1] = _staRover->xyzApr()[1] + _filter->x()[1];
    _output->_xyzRover[2] = _staRover->xyzApr()[2] + _filter->x()[2];
    copy(&_filter->Q().data()[0], &_filter->Q().data()[6], _output->_covMatrix);
    _output->_numSat     = _filter->numSat();
    _output->_pDop       = _filter->PDOP();
    _output->_ambFixRate = _filter->ambFixRate();
    _output->_error = false;
  }
  else {
    _output->_error = true;
  }
  if (OPT->logLevel() > 0) {
    _output->_log = strdup(_log->str().c_str());
  }
  delete _log; _log = new ostringstream();
}

// 
//////////////////////////////////////////////////////////////////////////////
t_irc::irc t_pppMain::cmpModel(t_station* station, const ColumnVector& xyzc,
                               vector<t_satObs*>& obsVector) {

  t_time time;
  if (station->roverBase() == e_rover) {
    time = _epoTimeRover;
    station->setName(OPT->roverName());
    station->setAntName(OPT->antNameRover());
    if (OPT->xyzAprRoverSet()) {
      station->setXyzApr(OPT->xyzAprRover());
    }
    else {
      station->setXyzApr(xyzc.Rows(1,3));
    }
    station->setNeuEcc(OPT->neuEccRover());
  }
  else {
    time = _epoTimeBase;
    station->setName(OPT->baseName());
    station->setAntName(OPT->antNameBase());
    station->setXyzApr(OPT->xyzAprBase());
    station->setNeuEcc(OPT->neuEccBase());
  }

  // Receiver Clock
  // --------------
  station->setDClk(xyzc[3]);

  // US Restriction
  // -------------- 
  station->checkRestriction(time);

  // Tides
  // -----
  station->setTideDspl(t_tides::displacement(time, station->xyzApr()));
  
  // Observation model
  // -----------------
  vector<t_satObs*>::iterator it = obsVector.begin();
  while (it != obsVector.end()) {
    t_satObs* satObs = *it;
    satObs->cmpModel(station);
    if (satObs->isValid() && satObs->eleSat() >= OPT->minEle()) {
      ++it;
    }
    else {
      delete satObs;
      it = obsVector.erase(it);
    }
  }

  return t_irc::success;
}

// 
//////////////////////////////////////////////////////////////////////////////
t_irc::irc t_pppMain::createDifferences() {

  vector<t_satObs*>::iterator it = _obsRover.begin();
  while (it != _obsRover.end()) {
    t_satObs* obsR = *it;
    t_satObs* obsB = 0;
    for (unsigned ii = 0; ii < _obsBase.size(); ii++) {
      if (_obsBase[ii]->prn() == obsR->prn()) {
        obsB = _obsBase[ii];
        break;
      }
    }
    if (obsB) {
      obsR->createDifference(obsB);
      ++it;
    }
    else {
      delete obsR;
      it = _obsRover.erase(it);
    }
  }

  return t_irc::success;
}

// 
//////////////////////////////////////////////////////////////////////////////
void t_pppMain::processEpoch(int numSatRover, const t_pppSatObs* satObsRover,
                             t_pppOutput* output) {

  try {
    initOutput(output);

    // Prepare Observations of the Rover
    // ---------------------------------    
    if (prepareObs(numSatRover, satObsRover, _obsRover, 
                    _epoTimeRover) != t_irc::success) {
      return finish(t_irc::failure);
    }

    LOG << "\nResults of Epoch ";
    if (!_epoTimeRover.undef()) LOG << string(_epoTimeRover);
    LOG << "\n--------------------------------------\n";
 
    for (int iter = 1; iter <= 2; iter++) {
      ColumnVector xyzc(4); xyzc = 0.0;
      bool print = (iter == 2);
      if (cmpBancroft(_epoTimeRover, _obsRover, xyzc, print) != t_irc::success) {
        return finish(t_irc::failure);
      }
      if (cmpModel(_staRover, xyzc, _obsRover) != t_irc::success) {
        return finish(t_irc::failure);
      }
    }

    _offGG = cmpOffGG(_obsRover);

    // Prepare Observations of the Base
    // --------------------------------    
    if (OPT->isDifferential()) {
      if (prepareObs(numSatBase, satObsBase, _obsBase, 
                     _epoTimeBase) != t_irc::success) {
        return finish(t_irc::failure);
      }
      ColumnVector xyzc(4); xyzc = 0.0;
      if (cmpModel(_staBase, xyzc, _obsBase) != t_irc::success) {
        return finish(t_irc::failure);
      }
      if (cmpBancroft(_epoTimeBase, _obsBase, xyzc, true) != t_irc::success) {
        return finish(t_irc::failure);
      }
      _staBase->setDClk(xyzc[3]);

      _offGG -= cmpOffGG(_obsBase);

      // Create single-differences of observations
      // -----------------------------------------
      if (createDifferences() != t_irc::success) {
        return finish(t_irc::failure);
      }
      for (unsigned ii = 0; ii < _obsBase.size(); ii++) {
        delete _obsBase[ii];
      }
      _obsBase.clear();
    }

    // Store last epoch of data
    // ------------------------    
    _obsPool->putEpoch(_epoTimeRover, _obsRover);

    // Process Epoch in Filter
    // -----------------------
    if (_filter->processEpoch(_obsPool) != t_irc::success) {
      return finish(t_irc::failure);
    }
  }
  catch (Exception& exc) {
    LOG << exc.what() << endl;
    return finish(t_irc::failure);
  }
  catch (string& msg) {
    LOG << "Exception: " << msg << endl;
    return finish(t_irc::failure);
  }
  catch (logic_error exc) {
    LOG << exc.what() << endl;
    return finish(t_irc::failure);
  }
  catch (const char* msg) {
    LOG << msg << endl;
    return finish(t_irc::failure);
  }
  catch (...) {
    LOG << "unknown exception" << endl;
    return finish(t_irc::failure);
  }

  return finish(t_irc::success);
}