// 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_pppSatObs
 *
 * Purpose:    Satellite observations
 *
 * Author:     L. Mervart
 *
 * Created:    29-Jul-2014
 *
 * Changes:    
 *
 * -----------------------------------------------------------------------*/


#include <iostream>
#include <cmath>
#include <newmatio.h>

#include "pppSatObs.h"
#include "bncconst.h"
#include "pppEphPool.h"
#include "pppStation.h"
#include "bncutils.h"
#include "bncantex.h"
#include "pppObsPool.h"
#include "pppClient.h"

using namespace BNC_PPP;
using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
t_pppSatObs::t_pppSatObs(const t_satObs& pppSatObs) {
  _prn  = pppSatObs._prn;
  _time = pppSatObs._time;
  _outlier    = false;
  for (unsigned ii = 0; ii < pppSatObs._obs.size(); ii++) {
    _allObs.push_back(new t_frqObs(*pppSatObs._obs[ii]));
  }
  prepareObs();
}

// Destructor
////////////////////////////////////////////////////////////////////////////
t_pppSatObs::~t_pppSatObs() {
  for (unsigned ii = 0; ii < _allObs.size(); ii++) {
    delete _allObs[ii];
  }
}

// 
////////////////////////////////////////////////////////////////////////////
void t_pppSatObs::prepareObs() {
  _model.reset();
  _valid     = true;
  _validObs1 = 0;
  _validObs2 = 0;

  bool dualFreq = OPT->dualFreqRequired(_prn.system());

  // Select two pseudoranges and two phase observations
  // --------------------------------------------------
  const string preferredAttrib = "CWP_";
  for (unsigned iPref = 0; iPref < preferredAttrib.length(); iPref++) {
    string obsType1 = (preferredAttrib[iPref] == '_') ? string("1") : string("1") + preferredAttrib[iPref];
    string obsType2 = (preferredAttrib[iPref] == '_') ? string("2") : string("2") + preferredAttrib[iPref];
    if (_validObs1 == 0) {
      for (unsigned ii = 0; ii < _allObs.size(); ii++) {
        t_frqObs* obs = _allObs[ii];
        if (obs->_rnxType2ch == obsType1 && obs->_codeValid && obs->_phaseValid) {
          _validObs1 = obs;
        }
      }
    }
    if (dualFreq) {
      if (_validObs2 == 0) {
        for (unsigned ii = 0; ii < _allObs.size(); ii++) {
          t_frqObs* obs = _allObs[ii];
          if (obs->_rnxType2ch == obsType2 && obs->_codeValid && obs->_phaseValid) {
            _validObs2 = obs;
          }
        }
      }
    }
  }

  if (_validObs1 == 0 || (dualFreq && _validObs2 == 0)) {
    _valid = false;
    return;
  } 

  // Find Glonass Channel Number
  // ---------------------------
  if (_prn.system() == 'R') {
    _channel = PPP_CLIENT->ephPool()->getChannel(_prn);
  }
  else {
    _channel = 0;
  }

  // Copy raw observations
  // ---------------------
  _f1    = t_CST::f1(_prn.system(), _channel);
  _rawC1 = _validObs1->_code;
  _rawL1 = _validObs1->_phase * t_CST::c / _f1;
  if (dualFreq) {
    _f2    = t_CST::f2(_prn.system(), _channel);
    _rawC2 = _validObs2->_code;
    _rawL2 = _validObs2->_phase * t_CST::c / _f2;
  }
  else {
    _f2     = 0.0;
    _rawC2  = 0.0;
    _rawL2  = 0.0;
  }

  // Compute Satellite Coordinates at Time of Transmission
  // -----------------------------------------------------
  _xcSat.ReSize(4); _xcSat = 0.0;
  _vvSat.ReSize(4); _vvSat = 0.0;
  bool totOK  = false;
  ColumnVector satPosOld(4); satPosOld = 0.0;
  t_lc::type tLC = (dualFreq ? t_lc::cIF : t_lc::c1);
  double prange = obsValue(tLC);
  for (int ii = 1; ii <= 10; ii++) {
    bncTime ToT = _time - prange / t_CST::c - _xcSat[3];
    if (PPP_CLIENT->ephPool()->getCrd(_prn, ToT, _xcSat, _vvSat) != success) {
      _valid = false;
      return;
    }
    ColumnVector dx = _xcSat - satPosOld;
    dx[3] *= t_CST::c;
    if (dx.norm_Frobenius() < 1.e-4) {
      totOK = true;
      break;
    }
    satPosOld = _xcSat; 
  }
  if (totOK) {
    _model._satClkM = _xcSat[3] * t_CST::c; 
  }
  else {
    _valid = false;
  }
}

// 
////////////////////////////////////////////////////////////////////////////
t_irc t_pppSatObs::cmpModel(const t_pppStation* station) {

  // Reset all model values
  // ----------------------
  _model.reset();

  // Topocentric Satellite Position
  // ------------------------------
  ColumnVector rSat = _xcSat.Rows(1,3);
  ColumnVector rhoV = rSat - station->xyzApr(); 
  _model._rho = rhoV.norm_Frobenius();

  ColumnVector neu(3);
  xyz2neu(station->ellApr().data(), rhoV.data(), neu.data());

  _model._eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / _model._rho );
  if (neu[2] < 0) {
    _model._eleSat *= -1.0;
  }
  _model._azSat  = atan2(neu[1], neu[0]);

  // Satellite Clocks
  // ----------------
  _model._satClkM = _xcSat[3] * t_CST::c;

  // Receiver Clocks
  // ---------------
  _model._recClkM = station->dClk() * t_CST::c;

  // Sagnac Effect (correction due to Earth rotation)
  // ------------------------------------------------
  ColumnVector Omega(3);
  Omega[0] = 0.0;
  Omega[1] = 0.0;
  Omega[2] = t_CST::omega / t_CST::c;
  _model._sagnac = DotProduct(Omega, crossproduct(rSat, station->xyzApr()));

  // Antenna Eccentricity
  // --------------------
  _model._antEcc = -DotProduct(station->xyzEcc(), rhoV) / _model._rho;

  // Antenna Phase Center Offsets and Variations
  // -------------------------------------------
  if (PPP_CLIENT->antex()) {
    t_frequency::type frq1 = t_frequency::G1;
    t_frequency::type frq2 = t_frequency::G2;
    if (_prn.system() == 'R') {
      frq1 = t_frequency::R1;
      frq2 = t_frequency::R2;
    }
    bool found;
    _model._antPco1 = PPP_CLIENT->antex()->rcvCorr(frq1, station->antName(),
                                                _model._eleSat, found);
    _model._antPco2 = PPP_CLIENT->antex()->rcvCorr(frq2, station->antName(),
                                                _model._eleSat, found);
  }

  // Tropospheric Delay
  // ------------------
  _model._tropo = t_tropo::delay_saast(station->xyzApr(), _model._eleSat);

  // Phase Wind-Up
  // -------------
  _model._windUp = station->windUp(_time, _prn, rSat);

  // Code (and Phase) Biases
  // -----------------------
  const t_satBias* satBias = PPP_CLIENT->obsPool()->satBias(_prn);
  if (satBias) { 
    for (unsigned ii = 0; ii < satBias->_bias.size(); ii++) {
      const t_frqBias& bias = satBias->_bias[ii];
      if (_validObs1 && _validObs1->_rnxType2ch == bias._rnxType2ch) {
        _validObs1->_biasJumpCounter = satBias->_jumpCount;
        if (bias._codeValid) {
          _model._biasC1 = bias._code;
        }
        if (bias._phaseValid) {
          _model._biasL1 = bias._phase;
        }
      }
      if (_validObs2 && _validObs2->_rnxType2ch == bias._rnxType2ch) {
        _validObs2->_biasJumpCounter = satBias->_jumpCount;
        if (bias._codeValid) {
          _model._biasC2 = bias._code;
        }
        if (bias._phaseValid) {
          _model._biasL2 = bias._phase;
        }
      }
    }
  }

  // Tidal Correction
  // ----------------
  _model._tide = -DotProduct(station->tideDspl(), rhoV) / _model._rho;

  // Ionospheric Delay
  // -----------------
  // TODO

  // Ocean Loading
  // -------------
  // TODO

  // Set Model Set Flag
  // ------------------
  _model._set = true;

  return success;
}

// 
////////////////////////////////////////////////////////////////////////////
void t_pppSatObs::printModel() const {
  LOG.setf(ios::fixed);
  LOG << "MODEL for Satellite " << _prn.toString() << endl
      << "RHO:    " << setw(12) << setprecision(3) << _model._rho     << endl
      << "ELE:    " << setw(12) << setprecision(3) << _model._eleSat * 180.0 / M_PI << endl
      << "AZI:    " << setw(12) << setprecision(3) << _model._azSat  * 180.0 / M_PI << endl
      << "SATCLK: " << setw(12) << setprecision(3) << _model._satClkM << endl
      << "RECCLK: " << setw(12) << setprecision(3) << _model._recClkM << endl
      << "SAGNAC: " << setw(12) << setprecision(3) << _model._sagnac  << endl
      << "ANTECC: " << setw(12) << setprecision(3) << _model._antEcc  << endl
      << "PCO1:   " << setw(12) << setprecision(3) << _model._antPco1 << endl
      << "PCO2:   " << setw(12) << setprecision(3) << _model._antPco2 << endl
      << "TROPO:  " << setw(12) << setprecision(3) << _model._tropo   << endl
      << "WINDUP: " << setw(12) << setprecision(3) << _model._windUp  << endl
      << "BIASC1: " << setw(12) << setprecision(3) << _model._biasC1  << endl
      << "BIASC2: " << setw(12) << setprecision(3) << _model._biasC2  << endl
      << "BIASL1: " << setw(12) << setprecision(3) << _model._biasL1  << endl
      << "BIASL2: " << setw(12) << setprecision(3) << _model._biasL2  << endl
      << "TIDES:  " << setw(12) << setprecision(3) << _model._tide    << endl;

  //// beg test
  LOG << "PCO L3: " <<  setw(12) << setprecision(3) 
      << lc(t_lc::lIF, _model._antPco1, _model._antPco2, 0.0, 0.0) << endl;

  LOG << "WIND L3:" <<  setw(12) << setprecision(3) 
      << lc(t_lc::lIF, _model._windUp * t_CST::c / _f1, 
                       _model._windUp * t_CST::c / _f2, 0.0, 0.0) << endl;

  LOG << "OBS-CMP P3: " << _prn.toString() << " " 
      << setw(12) << setprecision(3) << obsValue(t_lc::cIF) << " "
      << setw(12) << setprecision(3) << cmpValue(t_lc::cIF) << " "
      << setw(12) << setprecision(3) << obsValue(t_lc::cIF) - cmpValue(t_lc::cIF) << endl;

  LOG << "OBS-CMP L3: " << _prn.toString() << " " 
      << setw(12) << setprecision(3) << obsValue(t_lc::lIF) << " "
      << setw(12) << setprecision(3) << cmpValue(t_lc::lIF) << " "
      << setw(12) << setprecision(3) << obsValue(t_lc::lIF) - cmpValue(t_lc::lIF) << endl;

  LOG << "OBS-CMP MW: " << _prn.toString() << " " 
      << setw(12) << setprecision(3) << obsValue(t_lc::MW) << " "
      << setw(12) << setprecision(3) << cmpValue(t_lc::MW) << " "
      << setw(12) << setprecision(3) << obsValue(t_lc::MW) - cmpValue(t_lc::MW) << endl;
  //// end test
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::obsValue(t_lc::type tLC) const {

  if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
    return 0.0;
  }

  return this->lc(tLC, _rawL1, _rawL2, _rawC1, _rawC2);
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::cmpValueForBanc(t_lc::type tLC) const {
  return cmpValue(tLC) - _model._rho - _model._sagnac - _model._recClkM;
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::cmpValue(t_lc::type tLC) const {

  if (!_validObs2 && t_lc::need2ndFreq(tLC)) {
    return 0.0;
  }

  // Non-Dispersive Part
  // -------------------
  double nonDisp = _model._rho    + _model._recClkM - _model._satClkM 
                 + _model._sagnac + _model._antEcc  + _model._tropo 
                 + _model._tide;

  // Add Dispersive Part
  // -------------------
  double L1 = nonDisp + _model._antPco1 - _model._biasL1 + _model._windUp * t_CST::c / _f1;
  double L2 = nonDisp + _model._antPco2 - _model._biasL2 + _model._windUp * t_CST::c / _f2;
  double C1 = nonDisp + _model._antPco1 - _model._biasC1;
  double C2 = nonDisp + _model._antPco2 - _model._biasC2;

  return this->lc(tLC, L1, L2, C1, C2);
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::lc(t_lc::type tLC, 
                    double L1, double L2, double C1, double C2,
                    ColumnVector* coeff) const {

  if (coeff) {
    coeff->ReSize(4);
    (*coeff) = 0.0;
  }

  if      (tLC == t_lc::l1) {
    if (coeff) (*coeff)(1) = 1.0;
    return L1;
  }
  else if (tLC == t_lc::l2) {
    if (coeff) (*coeff)(2) = 1.0;
    return L2;
  }
  else if (tLC == t_lc::c1) {
    if (coeff) (*coeff)(3) = 1.0;
    return C1;
  }
  else if (tLC == t_lc::c2) {
    if (coeff) (*coeff)(4) = 1.0;
    return C2;
  }
  else if (tLC == t_lc::lIF || tLC == t_lc::cIF) {
    double a1 =  _f1 * _f1 / (_f1 * _f1 - _f2 * _f2);
    double a2 = -_f2 * _f2 / (_f1 * _f1 - _f2 * _f2);
    if (tLC == t_lc::lIF) {
      if (coeff) {
        (*coeff)(1) = a1;
        (*coeff)(2) = a2;
      }
      return a1 * L1 + a2 * L2;
    }
    else {
      if (coeff) {
        (*coeff)(3) = a1;
        (*coeff)(4) = a2;
      }
      return a1 * C1 + a2 * C2;
    }
  }
  else if (tLC == t_lc::MW) {
    double a1 =  _f1 / (_f1 - _f2);
    double a2 = -_f2 / (_f1 - _f2);
    double a3 = -_f1 / (_f1 + _f2);
    double a4 = -_f2 / (_f1 + _f2);
    if (coeff) {
      (*coeff)(1) = a1;
      (*coeff)(2) = a2;
      (*coeff)(3) = a3;
      (*coeff)(4) = a4;
    }
    return a1 * L1 + a2 * L2 + a3 * C1 + a4 * C2;
  }
  else if (tLC == t_lc::CL) {
    if (coeff) {
      (*coeff)(1) = 0.5;
      (*coeff)(3) = 0.5;
    }
    return (C1 + L1) / 2.0;
  }

  return 0.0;
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::lambda(t_lc::type tLC) const {

  if      (tLC == t_lc::l1) {
    return t_CST::c / _f1;
  }
  else if (tLC == t_lc::l2) {
    return t_CST::c / _f2;
  }
  else if (tLC == t_lc::lIF) {
    return t_CST::c / (_f1 + _f2);
  }
  else if (tLC == t_lc::MW) {
    return t_CST::c / (_f1 - _f2);
  }
  else if (tLC == t_lc::CL) {
    return t_CST::c / _f1 / 2.0;
  }

  return 0.0;
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::sigma(t_lc::type tLC) const {

  ColumnVector sig(4);
  sig(1) = OPT->_sigmaL1;
  sig(2) = OPT->_sigmaL1;
  sig(3) = OPT->_sigmaC1;
  sig(4) = OPT->_sigmaC1;

  ColumnVector coeff(4);
  lc(tLC, sig(1), sig(2), sig(3), sig(4), &coeff);

  ColumnVector sp = SP(sig, coeff); // Schur product

  // Elevation-Dependent Weighting
  // -----------------------------
  double cEle = 1.0;
  if ( (OPT->_eleWgtCode  && t_lc::includesCode(tLC)) ||
       (OPT->_eleWgtPhase && t_lc::includesPhase(tLC)) ) {
    double eleD = eleSat()*180.0/M_PI;
    double hlp  = fabs(90.0 - eleD);
    cEle = (1.0 + hlp*hlp*hlp*0.000004);
  }

  return cEle * sp.norm_Frobenius();
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::maxRes(t_lc::type tLC) const {

  ColumnVector res(4);
  res(1) = OPT->_maxResL1;
  res(2) = OPT->_maxResL1;
  res(3) = OPT->_maxResC1;
  res(4) = OPT->_maxResC1;

  ColumnVector coeff(4);
  lc(tLC, res(1), res(2), res(3), res(4), &coeff);

  ColumnVector sp = SP(res, coeff); // Schur product

  return sp.norm_Frobenius();
}

// 
////////////////////////////////////////////////////////////////////////////
void t_pppSatObs::setRes(t_lc::type tLC, double res) {
  _res[tLC] = res;
}

// 
////////////////////////////////////////////////////////////////////////////
double t_pppSatObs::getRes(t_lc::type tLC) const {
  map<t_lc::type, double>::const_iterator it = _res.find(tLC);
  if (it != _res.end()) {
    return it->second;
  }
  else {
    return 0.0;
  }
}