#include <cmath>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <algorithm>
#include <newmatio.h>
#include "parlist.h"
#include "satobs.h"

#include "station.h"
#include "bncutils.h"
#include "bncconst.h"
#include "pppClient.h"

using namespace BNC;
using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
t_param::t_param(e_type type, const t_prn& prn, t_lc::type tLC,
                 const vector<t_satObs*>* obsVector) {

  _type     = type;
  _prn      = prn;
  _tLC      = tLC;
  _x0       = 0.0;
  _indexOld = -1;
  _indexNew = -1;
  _noise    = 0.0;
  _ambInfo  = 0;

  switch (_type) {
   case crdX:
     _epoSpec = true;
     _sigma0  = OPT->_sigCrd[0];
     break;
   case crdY:
     _epoSpec = true;
     _sigma0  = OPT->_sigCrd[1];
     break;
   case crdZ:
     _epoSpec = true;
     _sigma0  = OPT->_sigCrd[2];
     break;
   case clkR:
     _epoSpec = true;
     _sigma0  = 1000.0;
     break;
   case amb:
     _ambInfo = new t_ambInfo();
     if (obsVector) {
       for (unsigned ii = 0; ii < obsVector->size(); ii++) {
         const t_satObs* obs = obsVector->at(ii);
         if (obs->prn() == _prn) {
           double offGG = 0;
           if (_prn.system() == 'R' && tLC != t_lc::MW) {
             offGG = PPP_CLIENT->offGG();
           }
           _x0 = floor((obs->obsValue(tLC) - offGG - obs->cmpValue(tLC)) / obs->lambda(tLC) + 0.5);
           break;
         }
       }
     }
     _epoSpec = false;
     _sigma0  = 100.0;
     break;
   case offGG:
     _epoSpec = true;
     _sigma0  = 1000.0;
     _x0      = PPP_CLIENT->offGG();
     break;
   case trp:
     _epoSpec = false;
     _sigma0  = OPT->_sigTropo;
     _noise   = OPT->_noiseTropo;
     break;
   case bias:
     _epoSpec = true;
     _sigma0  = 10.0;
     break;
  }
}

// Destructor
////////////////////////////////////////////////////////////////////////////
t_param::~t_param() {
  delete _ambInfo;
}

// 
////////////////////////////////////////////////////////////////////////////
double t_param::partial(const bncTime& /* epoTime */, const t_satObs* obs, 
                        const t_lc::type& tLC) const {

  // Special Case - Melbourne-Wuebbena
  // ---------------------------------
  if (tLC == t_lc::MW && _type != amb && _type != bias) {
    return 0.0;
  }

  const t_station* sta  = PPP_CLIENT->staRover();
  ColumnVector     rhoV = sta->xyzApr() - obs->xc().Rows(1,3);

  switch (_type) {
  case crdX:
    return (sta->xyzApr()[0] - obs->xc()[0]) / rhoV.norm_Frobenius();
  case crdY:
    return (sta->xyzApr()[1] - obs->xc()[1]) / rhoV.norm_Frobenius();
  case crdZ:
    return (sta->xyzApr()[2] - obs->xc()[2]) / rhoV.norm_Frobenius();
  case clkR:
    return 1.0;
  case offGG:
    return (obs->prn().system() == 'R') ? 1.0 : 0.0;
  case amb:
    if (obs->prn() == _prn) {
      if      (tLC == _tLC) {
        return (obs->lambda(tLC));
      }
      else if (tLC == t_lc::lIF && _tLC == t_lc::MW) {
        return obs->lambda(t_lc::lIF) * obs->lambda(t_lc::MW) / obs->lambda(t_lc::l2);
      }
      else {
        ColumnVector coeff(4);
        obs->lc(tLC, 0.0, 0.0, 0.0, 0.0, &coeff);
        if      (_tLC == t_lc::l1) {
          return obs->lambda(t_lc::l1) * coeff(1); 
        }
        else if (_tLC == t_lc::l2) {
          return obs->lambda(t_lc::l2) * coeff(2); 
        }
      }
    }
    return 0.0;
  case trp:
    return 1.0 / sin(obs->eleSat()); 
  case bias:
    if (tLC == _tLC && obs->prn().system() == _prn.system()) {
      return 1.0;
    }
    else {
      return 0.0;
    }
  }

  return 0.0;
}

// 
////////////////////////////////////////////////////////////////////////////
string t_param::toString() const {
  stringstream ss;
  switch (_type) {
  case crdX:
    ss << "CRD_X";
    break; 
  case crdY:
    ss << "CRD_Y";
    break;
  case crdZ:
    ss << "CRD_Z";
    break;
  case clkR:
    ss << "CLK        ";
    break;
  case amb:
    ss << "AMB " << left << setw(3) << t_lc::toString(_tLC) << right << ' ' << _prn.toString();
    break;
  case offGG:
    ss << "OGG        ";
    break;
  case trp:
    ss << "TRP        ";
    break;
  case bias:
    ss << "BIAS " << _prn.system() << ' ' << left << setw(3) << t_lc::toString(_tLC);
    break;
  }
  return ss.str();
}

// Constructor
////////////////////////////////////////////////////////////////////////////
t_parlist::t_parlist() {
}

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

// 
////////////////////////////////////////////////////////////////////////////
t_irc t_parlist::set(const bncTime& epoTime, const vector<t_lc::type>& ambLCs, 
                     const vector<t_satObs*>& obsVector) {

  // Remove some Parameters
  // ----------------------
  vector<t_param*>::iterator it = _params.begin();
  while (it != _params.end()) {
    t_param* par = *it;

    bool remove = false;

    if      (par->epoSpec()) {  
      remove = true;
    }

    else if (par->type() == t_param::amb) {
      if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 120.0)) {
        remove = true;
      }
    }

    else if (par->type() == t_param::amb) {
      if (par->lastObsTime().valid() && (epoTime - par->lastObsTime() > 3600.0)) {
        remove = true;
      }
    }

    if (remove) {
      delete par;
      it = _params.erase(it);
    }
    else {
      ++it;
    }
  }

  // Check whether parameters have observations
  // ------------------------------------------
  for (unsigned ii = 0; ii < _params.size(); ii++) {
    t_param* par = _params[ii];
    if (par->prn() == 0) {
      par->setLastObsTime(epoTime);
      if (par->firstObsTime().undef()) {
        par->setFirstObsTime(epoTime);
      }
    }
    else {
      for (unsigned jj = 0; jj < obsVector.size(); jj++) {
        const t_satObs* satObs = obsVector[jj];
        if (satObs->prn() == par->prn()) {
          par->setLastObsTime(epoTime);
          if (par->firstObsTime().undef()) {
            par->setFirstObsTime(epoTime);
          }
          break;
        }
      }
    }
  } 

  // Required Set of Parameters
  // --------------------------
  vector<t_param*> required;

  // Coordinates
  // -----------
  required.push_back(new t_param(t_param::crdX, t_prn(), t_lc::dummy));
  required.push_back(new t_param(t_param::crdY, t_prn(), t_lc::dummy));
  required.push_back(new t_param(t_param::crdZ, t_prn(), t_lc::dummy));

  // Receiver Clock
  // --------------
  required.push_back(new t_param(t_param::clkR, t_prn(), t_lc::dummy));

  // GPS-Glonass Clock Offset
  // ------------------------
  if (OPT->useGlonass()) {
    required.push_back(new t_param(t_param::offGG, t_prn(), t_lc::dummy));
  }

  // Receiver Biases
  // ---------------
  for (unsigned ii = 0; ii < OPT->_estBias.size(); ii++) {
    const t_options::t_optBias& optBias = OPT->_estBias[ii];
    required.push_back(new t_param(t_param::bias, t_prn(optBias._system, 1), optBias._tLC));
  }

  // Troposphere
  // -----------
  if (OPT->estTropo()) {
    required.push_back(new t_param(t_param::trp, t_prn(), t_lc::dummy));
  }

  // Ambiguities
  // -----------
  for (unsigned ii = 0; ii < ambLCs.size(); ii++) {
    const t_lc::type& tLC = ambLCs[ii];
    for (unsigned jj = 0; jj < obsVector.size(); jj++) {
      const t_satObs* satObs = obsVector[jj];
      required.push_back(new t_param(t_param::amb, satObs->prn(), tLC, &obsVector));
    }
  }

  // Check if all required parameters are present
  // --------------------------------------------
  for (unsigned ii = 0; ii < required.size(); ii++) {
    t_param* parReq = required[ii];

    bool found = false;
    for (unsigned jj = 0; jj < _params.size(); jj++) {
      t_param* parOld = _params[jj];
      if (parOld->isEqual(parReq)) {
        found = true;
        break;
      }
    }
    if (found) {
      delete parReq;
    }
    else {
      _params.push_back(parReq);
    }
  }

  // Set Parameter Indices
  // ---------------------
  sort(_params.begin(), _params.end(), t_param::sortFunction);

  for (unsigned ii = 0; ii < _params.size(); ii++) {
    t_param* par = _params[ii];
    par->setIndex(ii);
    for (unsigned jj = 0; jj < obsVector.size(); jj++) {
      const t_satObs* satObs = obsVector[jj];
      if (satObs->prn() == par->prn()) {
        par->setAmbEleSat(satObs->eleSat());
        par->stepAmbNumEpo();
      }
    }
  }

  return success;
}

// 
////////////////////////////////////////////////////////////////////////////
void t_parlist::printResult(const bncTime& epoTime, const SymmetricMatrix& QQ,
                            const ColumnVector& xx, double ambFixRate) const {

  string epoTimeStr = string(epoTime);

  LOG << endl;

  t_param* parX = 0;
  t_param* parY = 0;
  t_param* parZ = 0;
  for (unsigned ii = 0; ii < _params.size(); ii++) {
    t_param* par = _params[ii];
    if      (par->type() == t_param::crdX) {
      parX = par;
    }
    else if (par->type() == t_param::crdY) {
      parY = par;
    }
    else if (par->type() == t_param::crdZ) {
      parZ = par;
    }
    else {
      int ind = par->indexNew();
      LOG << epoTimeStr << ' ' << par->toString() << ' '
          << setw(10) << setprecision(4) << par->x0() << ' '
          << showpos << setw(10) << setprecision(4) << xx[ind] << noshowpos << " +- "
          << setw(8)  << setprecision(4) << sqrt(QQ[ind][ind]);
      if (par->type() == t_param::amb) {
        LOG << " el = " << setw(6) << setprecision(2) << par->ambEleSat() * 180.0 / M_PI
            << " epo = " << setw(4) << par->ambNumEpo();
      }
      LOG << endl;
    }
  }
  
  if (parX && parY && parZ) {
    const t_station* sta = PPP_CLIENT->staRover();

    ColumnVector xyz(3);
    xyz[0] = xx[parX->indexNew()];
    xyz[1] = xx[parY->indexNew()];
    xyz[2] = xx[parZ->indexNew()];

    ColumnVector neu(3);
    xyz2neu(sta->ellApr().data(), xyz.data(), neu.data());

    SymmetricMatrix QQxyz = QQ.SymSubMatrix(1,3);

    SymmetricMatrix QQneu(3);
    covariXYZ_NEU(QQxyz, sta->ellApr().data(), QQneu);

    LOG << epoTimeStr
        << " X = " << setprecision(4) << sta->xyzApr()[0] + xyz[0] << " +- "
        << setprecision(4) << sqrt(QQxyz[0][0])
                                   
        << " Y = " << setprecision(4) << sta->xyzApr()[1] + xyz[1] << " +- "
        << setprecision(4) << sqrt(QQxyz[1][1])

        << " Z = " << setprecision(4) << sta->xyzApr()[2] + xyz[2] << " +- "
        << setprecision(4) << sqrt(QQxyz[2][2])

        << " dN = " << setprecision(4) << neu[0] << " +- "
        << setprecision(4) << sqrt(QQneu[0][0])

        << " dE = " << setprecision(4) << neu[1] << " +- "
        << setprecision(4) << sqrt(QQneu[1][1])

        << " dU = " << setprecision(4) << neu[2] << " +- "
        << setprecision(4) << sqrt(QQneu[2][2]);
    if (ambFixRate > 0.0) {
      LOG << " fix "; 
    }
    else {
      LOG << " flt "; 
    }
    LOG << int(100*ambFixRate) << " %\n";
  }
}