/**
 * @file GPS solver base
 */
/*
 * Copyright (c) 2020, M.Naruoka (fenrir)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice,
 *   this list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * - Neither the name of the naruoka.org nor the names of its contributors
 *   may be used to endorse or promote products derived from this software
 *   without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef __GPS_SOLVER_BASE_H__
#define __GPS_SOLVER_BASE_H__

#include <vector>
#include <map>
#include <utility>
#include <deque>
#include <algorithm>

#include <cmath>
#include <cstring>
#include <cstdlib>

#include "param/matrix.h"
#include "param/bit_array.h"
#include "GPS.h"

template <class FloatT>
struct GPS_Solver_Base {
  virtual ~GPS_Solver_Base() {}

  typedef FloatT float_t;
  typedef Matrix<float_t> matrix_t;

  typedef int prn_t;

  typedef GPS_SpaceNode<float_t> space_node_t;
  typedef typename space_node_t::gps_time_t gps_time_t;

  typedef typename space_node_t::xyz_t xyz_t;
  typedef typename space_node_t::llh_t llh_t;
  typedef typename space_node_t::enu_t enu_t;

  struct pos_t {
    xyz_t xyz;
    llh_t llh;
    matrix_t ecef2enu() const {
      float_t buf[3][3];
      llh.rotation_ecef2enu(buf);
      return matrix_t(3, 3, (float_t *)buf);
    }
  };

  typedef std::vector<std::pair<prn_t, float_t> > prn_obs_t;

  static prn_obs_t difference(
      const prn_obs_t &operand, const prn_obs_t &argument,
      const FloatT &scaling = FloatT(1)) {
    prn_obs_t res;
    for(typename prn_obs_t::const_iterator it(operand.begin()), it_end(operand.end()); it != it_end; ++it){
      for(typename prn_obs_t::const_iterator it2(argument.begin()), it2_end(argument.end()); it2 != it2_end; ++it2){
        if(it->first != it2->first){continue;}
        res.push_back(std::make_pair(it->first, (it->second - it2->second) * scaling));
        break;
      }
    }
    return res;
  }

  struct measurement_items_t {
    enum {
      L1_PSEUDORANGE,
      L1_DOPPLER,
      L1_CARRIER_PHASE,
      L1_RANGE_RATE,
      L1_PSEUDORANGE_SIGMA, // standard deviation(sigma)
      L1_DOPPLER_SIGMA,
      L1_CARRIER_PHASE_SIGMA,
      L1_RANGE_RATE_SIGMA,
      L1_SIGNAL_STRENGTH_dBHz,
      L1_LOCK_SEC,
      L1_FREQUENCY,
      MEASUREMENT_ITEMS_PREDEFINED,
    };
  };
  typedef std::map<prn_t, std::map<int, float_t> > measurement_t;

  struct measurement_item_set_t {
    struct {
      int i, i_sigma;
    } pseudorange, doppler, carrier_phase, range_rate;
    int signal_strength, lock_sec;
  };
  static const measurement_item_set_t L1CA;

  struct measurement_util_t {
    static prn_obs_t gather(
        const measurement_t &measurement,
        const typename measurement_t::mapped_type::key_type &key,
        const FloatT &scaling = FloatT(1)){
      prn_obs_t res;
      for(typename measurement_t::const_iterator it(measurement.begin()), it_end(measurement.end());
          it != it_end; ++it){
        typename measurement_t::mapped_type::const_iterator it2(it->second.find(key));
        if(it2 == it->second.end()){continue;}
        res.push_back(std::make_pair(it->first, it2->second * scaling));
      }
      return res;
    }
    static void merge(
        measurement_t &measurement,
        const prn_obs_t &new_item,
        const typename measurement_t::mapped_type::key_type &key) {
      for(typename prn_obs_t::const_iterator it(new_item.begin()), it_end(new_item.end());
          it != it_end; ++it){
        measurement[it->first].insert(std::make_pair(key, it->second));
      }
    }
  };

  /**
   * Find value corresponding to key from key-value map
   * of measurement_t::mapped_type
   * @param values key-value map
   * @param key key
   * @param buf buffer into which found value is stored
   * @return (float_t *) When value is found, pointer of buf will be returned.
   * Otherwise, NULL is returned.
   */
  static const float_t *find_value(
      const typename measurement_t::mapped_type &values,
      const typename measurement_t::mapped_type::key_type &key,
      float_t &buf) {
    typename measurement_t::mapped_type::const_iterator it;
    if((it = values.find(key)) != values.end()){
      return &(buf = it->second);
    }
    return NULL;
  }

  struct range_error_t {
    enum {
#define make_entry(name) \
  name, \
  MASK_ ## name = (1 << name), \
  DUMMY_ ## name = name
      make_entry(RECEIVER_CLOCK),
      make_entry(SATELLITE_CLOCK),
      make_entry(IONOSPHERIC),
      make_entry(TROPOSPHERIC),
#undef make_entry
      NUM_OF_ERRORS,
    };
    int unknown_flag;
    float_t value[NUM_OF_ERRORS];
    static const range_error_t not_corrected;
  };

  // TODO These range and rate functions will be overridden in subclass to support multi-frequency
  /**
   * Extract range information from measurement per satellite
   * @param values measurement[prn]
   * @param buf buffer into which range is stored
   * @param error optional argument in which error components of range will be returned
   * @return If valid range information is found, the pointer of buf will be returned; otherwise NULL
   */
  virtual const float_t *range(
      const typename measurement_t::mapped_type &values, float_t &buf,
      range_error_t *error = NULL) const {
    if(error){
      *error = range_error_t::not_corrected;
    }
    return find_value(values, measurement_items_t::L1_PSEUDORANGE, buf);
  }

  virtual const float_t *range_sigma(
      const typename measurement_t::mapped_type &values, float_t &buf) const {
    return find_value(values, measurement_items_t::L1_PSEUDORANGE_SIGMA, buf);
  }

  virtual const float_t *rate(
      const typename measurement_t::mapped_type &values, float_t &buf) const {
    const float_t *res;
    if((res = find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){

    }else if((res = find_value(values, measurement_items_t::L1_DOPPLER, buf))){
      // Fall back to doppler
      buf *= -space_node_t::L1_WaveLength();
    }
    return res;
  }

  virtual const float_t *rate_sigma(
      const typename measurement_t::mapped_type &values, float_t &buf) const {
    const float_t *res;
    if((res = find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){

    }else if((res = find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, buf))){
      // Fall back to doppler
      buf *= space_node_t::L1_WaveLength();
    }
    return res;
  }

  struct range_corrector_t {
    virtual ~range_corrector_t() {}
    virtual bool is_available(const gps_time_t &t) const {
      return false;
    }
    virtual float_t *calculate(
        const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
        float_t &buf) const = 0;
  };
  static const struct no_correction_t : public range_corrector_t {
    bool is_available(const gps_time_t &t) const {
      return true;
    }
    float_t *calculate(
        const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos,
        float_t &buf) const {
      return &(buf = 0);
    }
  } no_correction;
  struct range_correction_t : public std::deque<const range_corrector_t *> {
    typedef std::deque<const range_corrector_t *> super_t;
    range_correction_t() : super_t() {}
    const range_corrector_t *select(const gps_time_t &t) const {
      for(typename super_t::const_iterator it(super_t::begin()), it_end(super_t::end());
          it != it_end; ++it){
        if((*it)->is_available(t)){return *it;}
      }
      return NULL;
    }
    float_t operator()(
        const gps_time_t &t, const pos_t &usr_pos, const enu_t &sat_rel_pos) const {
      float_t res;
      for(typename super_t::const_iterator it(super_t::begin()), it_end(super_t::end());
          it != it_end; ++it){
        if((*it)->calculate(t, usr_pos, sat_rel_pos, res)){return res;}
      }
      return 0;
    }
    void remove(const typename super_t::value_type &v){
      std::remove(super_t::begin(), super_t::end(), v);
    }
    void add(const typename super_t::value_type &v){
      remove(v);
      super_t::push_front(v);
    }
  };

  /**
   * Select appropriate solver, this is provision for GNSS extension
   * @param prn satellite number
   * @return self, however, it will be overridden by a subclass
   */
  virtual const GPS_Solver_Base<FloatT> &select(const prn_t &prn) const {
    return *this;
  }

  struct relative_property_t {
    float_t weight; ///< How useful this information is. square value is required; thus, only positive value activates the other values.
    float_t range_corrected; ///< corrected range just including delay, and excluding receiver/satellite error
    float_t range_residual; ///< residual range
    float_t rate_relative_neg; /// relative rate
    float_t los_neg[3]; ///< line of site vector from satellite to user
  };

  /**
   * Calculate relative range and rate information to a satellite
   * This function will be overridden by a subclass to provide appropriate implementation
   *
   * @param prn satellite number
   * @param measurement measurement (per satellite) containing pseudo range
   * @param receiver_error (temporal solution of) receiver clock error in meter
   * @param time_arrival time when signal arrive at receiver
   * @param usr_pos (temporal solution of) user position
   * @param usr_vel (temporal solution of) user velocity
   * @return (relative_property_t) relative information
   */
  virtual relative_property_t relative_property(
      const prn_t &prn,
      const typename measurement_t::mapped_type &measurement,
      const float_t &receiver_error,
      const gps_time_t &time_arrival,
      const pos_t &usr_pos,
      const xyz_t &usr_vel) const {
    static const relative_property_t invalid = {0};
    return invalid;
  }

  struct user_pvt_t {
    enum {
      ERROR_NO = 0,
      ERROR_UNSOLVED,
      ERROR_INVALID_IONO_MODEL,
      ERROR_INSUFFICIENT_SATELLITES,
      ERROR_POSITION_LS,
      ERROR_POSITION_NOT_CONVERGED,
      ERROR_DOP,
      ERROR_VELOCITY_SKIPPED,
      ERROR_VELOCITY_INSUFFICIENT_SATELLITES,
      ERROR_VELOCITY_LS,
    } error_code;
    gps_time_t receiver_time;
    pos_t user_position;
    float_t receiver_error;
    enu_t user_velocity_enu;
    float_t receiver_error_rate;
    struct dop_t {
      float_t g, p, h, v, t;
      dop_t() {}
      dop_t(const matrix_t &C_enu)
          : g(std::sqrt(C_enu.trace())),
          p(std::sqrt(C_enu.partial(3, 3).trace())),
          h(std::sqrt(C_enu.partial(2, 2).trace())),
          v(std::sqrt(C_enu(2, 2))),
          t(std::sqrt(C_enu(3, 3))) {}
    } dop;
    unsigned int used_satellites;
    typedef BitArray<0x400> satellite_mask_t;
    satellite_mask_t used_satellite_mask; ///< bit pattern(use=1, otherwise=0), PRN 1(LSB) to 32 for GPS

    user_pvt_t()
        : error_code(ERROR_UNSOLVED),
          receiver_time(),
          user_position(), receiver_error(0),
          user_velocity_enu(), receiver_error_rate(0) {}

    bool position_solved() const {
      switch(error_code){
        case ERROR_NO:
        case ERROR_VELOCITY_SKIPPED:
        case ERROR_VELOCITY_INSUFFICIENT_SATELLITES:
        case ERROR_VELOCITY_LS:
          return true;
        default:
          return false;
      }
    }
    bool velocity_solved() const {
      return error_code == ERROR_NO;
    }
  };

  struct options_t {
    template <class OptionsT, class BaseSolverT = GPS_Solver_Base<float_t> >
    struct merge_t : public BaseSolverT::options_t, OptionsT {
      merge_t() : BaseSolverT::options_t(), OptionsT() {}
      merge_t(
          const typename BaseSolverT::options_t &opt_super,
          const OptionsT &opt = OptionsT())
          : BaseSolverT::options_t(opt_super), OptionsT(opt) {}
    };

    /**
     * Flags to invalidate specific satellite
     * Its index will be adjusted for PRN.
     */
    template <int prn_begin, int prn_end>
    struct exclude_prn_t
        : public BitArray<prn_end - prn_begin + 1, unsigned int> {
      typedef BitArray<prn_end - prn_begin + 1, unsigned int> super_t;
      exclude_prn_t() : super_t() {
        super_t::clear();
      }
      bool operator[](const int &prn) const {
        return super_t::operator[](prn - prn_begin);
      }
      using super_t::set;
      void set(const int &prn, const bool &bit = true) {
        super_t::set(prn - prn_begin, bit);
      }
      using super_t::reset;
      void reset(const int &prn) {set(prn, false);}
      std::vector<int> excluded() const {
        std::vector<int> res(super_t::indices_one());
        for(std::vector<int>::iterator it(res.begin()), it_end(res.end());
            it != it_end; ++it){
          *it += prn_begin;
        }
        return res;
      }
    };
  };

  options_t available_options() const {
    return options_t();
  }

  options_t available_options(const options_t &opt_wish) const {
    return opt_wish;
  }

  options_t update_options(const options_t &opt_wish){
    return opt_wish;
  }

protected:
  template <class MatrixT>
  struct linear_solver_t {
    MatrixT G; ///< Design matrix
    /**
     * Weighting (diagonal) matrix corresponding to inverse of covariance,
     * whose (i, j) element is assumed to be 1/sigma_{i}^2 (i == j) or 0 (i != j)
     */
    MatrixT W;
    MatrixT delta_r; ///< Observation delta, i.e., observation minus measurement (y)
    linear_solver_t(const MatrixT &G_, const MatrixT &W_, const MatrixT &delta_r_)
        : G(G_), W(W_), delta_r(delta_r_) {}
    /**
     * Transform coordinate of design matrix G
     * y = G x + v = G (R^{-1} x') + v = (G R^t) x' + v = G' x' + v,
     * where R x = x' and R is a rotation matrix.
     * For example, x is in ECEF with G being calculated in ECEF,
     * and if x' in ENU is required,
     * then, R is the ecef2enu matrix, because x' = ecef2enu x.
     *
     * @param G_ original design matrix
     * @param rotation_matrix 3 by 3 rotation matrix
     * @return transformed design matrix G'
     */
    static matrix_t rotate_G(const MatrixT &G_, const matrix_t &rotation_matrix){
      unsigned int r(G_.rows()), c(G_.columns()); // Normally c=4
      matrix_t res(r, c);
      res.partial(r, 3).replace(
          G_.partial(r, 3) * rotation_matrix.transpose());
      res.partial(r, c - 3, 0, 3).replace(G_.partial(r, c - 3, 0, 3));
      return res;
    }

    matrix_t G_rotated(const matrix_t &rotation_matrix) const {
      return rotate_G(G, rotation_matrix);
    }

    /**
     * Calculate C matrix, which is required to obtain DOP
     * C = G^t * W * G
     *
     * @return C matrix
     */
    matrix_t C() const {
      return (G.transpose() * W * G).inverse();
    }
    /**
     * Transform coordinate of matrix C, which will be used to calculate HDOP/VDOP
     * C' = (G * R^t)^t W * (G * R^t) = R * G^t * W * G * R^t = R * C * R^t,
     * where R is a rotation matrix, for example, ECEF to ENU.
     *
     * @param rotation_matrix 3 by 3 rotation matrix
     * @return transformed matrix C'
     * @see rotate_G
     */
    static matrix_t rotate_C(const matrix_t &C, const matrix_t &rotation_matrix){
      unsigned int n(C.rows()); // Normally n=4
      matrix_t res(n, n);
      res.partial(3, 3).replace( // upper left
          rotation_matrix * C.partial(3, 3) * rotation_matrix.transpose());
      res.partial(3, n - 3, 0, 3).replace( // upper right
          rotation_matrix * C.partial(3, n - 3, 0, 3));
      res.partial(n - 3, 3, 3, 0).replace( // lower left
          C.partial(n - 3, 3, 3, 0) * rotation_matrix.transpose());
      res.partial(n - 3, n - 3, 3, 3).replace( // lower right
          C.partial(n - 3, n - 3, 3, 3));
      return res;
    }
    /**
     * Solve x of linear equation (y = G x + v) to minimize sigma{v^t * v}
     * where v =~ N(0, sigma), and y and G are observation delta (=delta_r variable)
     * and a design matrix, respectively.
     * This yields x = (G^t * W * G)^{-1} * (G^t * W) y = S y.
     *
     * 4 by row(y) S matrix (=(G^t * W * G)^{-1} * (G^t * W)) will be used to calculate protection level
     * to investigate relationship between bias on each satellite and solution.
     * residual v = (I - P) = (I - G S), where P = G S, which is irrelevant to rotation,
     * because P = G R R^{t} S = G' S'.
     *
     * @param S (output) coefficient matrix to calculate solution, i.e., (G^t * W * G)^{-1} * (G^t * W)
     * @return x vector
     * @see rotate_S()
     */
    inline matrix_t least_square(matrix_t &S) const {
      matrix_t Gt_W(G.transpose() * W);
      S = (Gt_W * G).inverse() * Gt_W;
      return S * delta_r;
    }
    matrix_t least_square() const {
      matrix_t S;
      return least_square(S);
    }
    /**
     * Transform coordinate of coefficient matrix of solution S
     * x' = R x = R S y, where R is a rotation matrix, for example, ECEF to ENU.
     * Be careful, R is not ENU to ECEF in the example, which should be consistent to rotate_G().
     *
     * @param S coefficient matrix of solution
     * @param rotation_matrix 3 by 3 rotation matrix
     * @return transformed coefficient matrix S'
     * @see rotate_G
     */
    static matrix_t rotate_S(const matrix_t &S, const matrix_t &rotation_matrix){
      unsigned int r(S.rows()), c(S.columns()); // Normally r=4
      matrix_t res(r, c);
      res.partial(3, c).replace(rotation_matrix * S.partial(3, c));
      res.partial(r - 3, c, 3, 0).replace(S.partial(r - 3, c, 3, 0));
      return res;
    }
    /**
     * Calculate linear effect from bias on each range measurement to horizontal/vertical estimation.
     *
     * @param S coefficient matrix of solution
     * @param rotation_matrix 3 by 3 matrix, which makes S aligned to ENU or NED coordinates
     * @return slopes matrix (1st and 2nd columns correspond to horizontal and vertical components, respectively)
     * @see Eq.(5.26), (5.27) in @article{Mink, title={Performance of Receiver Autonomous Integrity Monitoring (RAIM) for Maritime Operations}, author={Mink, Michael}, pages={220} }
     * Note: returned values are not performed to be multiplied by sqrt(N-4)
     */
    matrix_t slope_HV(const matrix_t &S, const matrix_t &rotation_matrix = matrix_t::getI(3)) const {
      matrix_t S_ENU_or_NED(rotate_S(S, rotation_matrix));
      matrix_t res(G.rows(), 2); // 1st column = horizontal, 2nd column = vertical
      matrix_t P(G * S);
      for(unsigned int i(0), i_end(res.rows()); i < i_end; i++){
        if(W(i, i) <= 0){
          res(i, 0) = res(i, 1) = 0;
          continue;
        }
        float_t denom(std::sqrt((-P(i, i) + 1) * W(i, i)));
        res(i, 0) = std::sqrt(  // horizontal
            std::pow(S_ENU_or_NED(0, i), 2) + std::pow(S_ENU_or_NED(1, i), 2)) / denom;
        res(i, 1) = std::abs(S_ENU_or_NED(2, i)) / denom; // vertical
      }
      return res;
    }
    /**
     * Calculate weighted square sum of residual (WSSR) based on least square solution.
     * v^t W v (= (y - G x)^t W (y - G x) = y^t W (I-P) y)
     *
     * @param x solution
     * @return WSSR scalar
     */
    float_t wssr(const matrix_t &x = least_square()) const {
      matrix_t v(delta_r - G * x);
      return (v.transpose() * W * v)(0, 0);
    }
    /**
     * Calculate weighted square sum of residual (WSSR) based on least square solution
     * with solution coefficient matrix (S).
     * v^t W v (= (y - G x)^t W (y - G x) = y^t W (I-G*S) y)
     *
     * @param S coefficient matrix of solution
     * @param W_dash (output) pointer to store scale factor matrix of y^t y,
     * i.e., *W_dash = norm(t(I-G*S)) * W * norm(I-G*S)
     * @return WSSR scalar
     */
    float_t wssr_S(const matrix_t &S, matrix_t *W_dash = NULL) const {
      matrix_t rhs(matrix_t::getI(W.rows()) - (G * S));
      if(W_dash){
        *W_dash = W.copy();
        for(unsigned int i(0), i_end(rhs.rows()); i < i_end; ++i){
          (*W_dash)(i, i) *= (rhs.rowVector(i) * rhs.rowVector(i).transpose())(0, 0);
        }
      }
      return (delta_r.transpose() * W * rhs * delta_r)(0, 0);
    }
    typedef linear_solver_t<typename MatrixT::partial_offsetless_t> partial_t;
    partial_t partial(unsigned int size) const {
      if(size >= G.rows()){size = G.rows();}
      return partial_t(
          G.partial(size, G.columns()), W.partial(size, size), delta_r.partial(size, 1));
    }
    typedef linear_solver_t<typename MatrixT::circular_t> exclude_t;
    exclude_t exclude(const unsigned int &row) const {
      unsigned int size(G.rows()), offset((row + 1) % size);
      if(size >= 1){size--;}
      // generate matrices having circular view
      return exclude_t(
          G.circular(offset, 0, size, G.columns()),
          W.circular(offset, offset, size, size),
          delta_r.circular(offset, 0, size, 1));
    }
    template <class MatrixT2>
    void copy_G_W_row(const linear_solver_t<MatrixT2> &src,
        const unsigned int &i_src, const unsigned int &i_dst){
      unsigned int c_dst(G.columns()), c_src(src.G.columns()),
          c(c_dst > c_src ? c_src : c_dst); // Normally c=4
      for(unsigned int j(0); j < c; ++j){
        G(i_dst, j) = src.G(i_src, j);
      }
      W(i_dst, i_dst) = src.W(i_src, i_src);
    }
  };

  struct geometric_matrices_t : public linear_solver_t<matrix_t> {
    typedef linear_solver_t<matrix_t> super_t;
    geometric_matrices_t(
        const unsigned int &capacity,
        const unsigned int &estimate_system_differences = 0)
        : super_t(
          matrix_t(capacity, 4 + estimate_system_differences),
          matrix_t(capacity, capacity), matrix_t(capacity, 1)) {
      for(unsigned int i(0); i < capacity; ++i){
        super_t::G(i, 3) = 1;
      }
    }
  };

  struct measurement2_item_t {
    prn_t prn;
    const typename measurement_t::mapped_type *k_v_map;
    const GPS_Solver_Base<FloatT> *solver;
  };
  typedef std::vector<measurement2_item_t> measurement2_t;


  /**
   * Update position solution
   * This function will be called multiple times in a solution calculation iteration.
   * It may be overridden in a subclass for extraction of calculation source
   * such as design matrix.
   *
   * @param geomat residual, desgin and weight matrices
   * @param res (in,out) current solution to be updated
   * @return (bool) If solution will be treated as the final solution, true is returned; otherwise false.
   */
  virtual bool update_position_solution(
      const geometric_matrices_t &geomat,
      user_pvt_t &res) const {

    // Least square
    matrix_t delta_x(geomat.partial(res.used_satellites).least_square());

    xyz_t delta_user_position(delta_x.partial(3, 1));
    res.user_position.xyz += delta_user_position;
    res.user_position.llh = res.user_position.xyz.llh();

    const float_t &delta_receiver_error(delta_x(3, 0));
    res.receiver_error += delta_receiver_error;

    return (delta_x.partial(4, 1).norm2F() <= 1E-6); // equivalent to abs(x) <= 1E-3 [m]
  }

  struct user_pvt_opt_t {
    /**
     * how many iterations are required to perform coarse estimation before fine one;
     * If initial position and clock error are goodly guessed, this will be zero.
     */
    int warmup;
    int max_trial;
    bool estimate_velocity;
    user_pvt_opt_t(
        const bool &good_init = true,
        const bool &with_velocity = true)
        : warmup(good_init ? 0 : 5),
        max_trial(10),
        estimate_velocity(with_velocity) {}
  };

  /**
   * Calculate User position/velocity by using associated solvers.
   * This function can be overridden in a subclass.
   *
   * @param res (out) calculation results and matrices used for calculation
   * @param measurement PRN, pseudo-range, and pseudo-range rate information
   * associated with a specific solver corresponding to a satellite system
   * @param receiver_time receiver time at measurement
   * @param user_position_init initial solution of user position in XYZ meters and LLH
   * @param receiver_error_init initial solution of receiver clock error in meters
   * @param opt options for user PVT calculation
   * @see update_ephemeris(), register_ephemeris
   */
  virtual void user_pvt(
      user_pvt_t &res,
      const measurement2_t &measurement,
      const gps_time_t &receiver_time,
      const pos_t &user_position_init,
      const float_t &receiver_error_init,
      const user_pvt_opt_t &opt = user_pvt_opt_t()) const {

    res.receiver_time = receiver_time;

    // 1. Position calculation

    res.user_position = user_position_init;
    res.receiver_error = receiver_error_init;

    geometric_matrices_t geomat(measurement.size());
    typedef std::vector<std::pair<unsigned int, float_t> > index_obs_t;
    index_obs_t idx_rate_rel; // [(index of measurement, relative rate), ...]
    idx_rate_rel.reserve(measurement.size());

    // If initialization is not appropriate, more iteration will be performed.
    bool converged(false);
    for(int i_trial(-opt.warmup); i_trial < opt.max_trial; i_trial++){

      idx_rate_rel.clear();
      unsigned int i_row(0), i_measurement(0);
      res.used_satellite_mask.clear();

      gps_time_t time_arrival(
          receiver_time - (res.receiver_error / space_node_t::light_speed));

      const bool coarse_estimation(i_trial <= 0);
      for(typename measurement2_t::const_iterator it(measurement.begin()), it_end(measurement.end());
          it != it_end;
          ++it, ++i_measurement){

        static const xyz_t zero(0, 0, 0);
        relative_property_t prop(it->solver->relative_property(
            it->prn, *(it->k_v_map),
            res.receiver_error, time_arrival,
            res.user_position, zero));

        if(prop.weight <= 0){
          continue; // intentionally excluded satellite
        }else{
          res.used_satellite_mask.set(it->prn);
        }

        if(coarse_estimation){
          prop.weight = 1;
        }else{
          idx_rate_rel.push_back(std::make_pair(i_measurement, prop.rate_relative_neg));
        }

        geomat.delta_r(i_row, 0) = prop.range_residual;
        geomat.G(i_row, 0) = prop.los_neg[0];
        geomat.G(i_row, 1) = prop.los_neg[1];
        geomat.G(i_row, 2) = prop.los_neg[2];
        geomat.W(i_row, i_row) = prop.weight;

        ++i_row;
      }

      if((res.used_satellites = i_row) < 4){
        res.error_code = user_pvt_t::ERROR_INSUFFICIENT_SATELLITES;
        return;
      }

      try{
        converged = update_position_solution(geomat, res);
        if((!coarse_estimation) && converged){break;}
      }catch(const std::runtime_error &e){ // expect to detect matrix operation error
        res.error_code = user_pvt_t::ERROR_POSITION_LS;
        return;
      }
    }

    if(!converged){
      res.error_code = user_pvt_t::ERROR_POSITION_NOT_CONVERGED;
      return;
    }

    try{
      res.dop = typename user_pvt_t::dop_t(geomat.rotate_C(
          geomat.partial(res.used_satellites).C(), res.user_position.ecef2enu()));
    }catch(const std::runtime_error &e){ // expect to detect matrix operation error
      res.error_code = user_pvt_t::ERROR_DOP;
      return;
    }

    if(!opt.estimate_velocity){
      res.error_code = user_pvt_t::ERROR_VELOCITY_SKIPPED;
      return;
    }

    /* 2. Calculate velocity
     * Check consistency between range and rate for velocity calculation,
     * then, assign design and weight matrices
     */
    geometric_matrices_t geomat2(res.used_satellites);
    int i_range(0), i_rate(0);

    for(typename index_obs_t::const_iterator it(idx_rate_rel.begin()), it_end(idx_rate_rel.end());
        it != it_end;
        ++it, ++i_range){

      float_t rate;
      if(!(measurement[it->first].solver->rate(
          *(measurement[it->first].k_v_map), // const version of measurement[PRN]
          rate))){continue;}

      // Copy design matrix and set rate
      geomat2.copy_G_W_row(geomat, i_range, i_rate);
      geomat2.delta_r(i_rate, 0) = rate + it->second;

      ++i_rate;
    }

    if(i_rate < 4){
      res.error_code = user_pvt_t::ERROR_VELOCITY_INSUFFICIENT_SATELLITES;
      return;
    }

    try{
      // Least square
      matrix_t sol(geomat2.partial(i_rate).least_square());

      xyz_t vel_xyz(sol.partial(3, 1, 0, 0));
      res.user_velocity_enu = enu_t::relative_rel(
          vel_xyz, res.user_position.llh);
      res.receiver_error_rate = sol(3, 0);
    }catch(const std::runtime_error &e){ // expect to detect matrix operation error
      res.error_code = user_pvt_t::ERROR_VELOCITY_LS;
      return;
    }

    res.error_code = user_pvt_t::ERROR_NO;
  }

public:
  /**
   * Calculate satellite position
   *
   * @param prn satellite number
   * @param time GPS time
   * @param res object to which results are stored
   * @return If position is available, &res will be returned, otherwise NULL.
   */
  virtual xyz_t *satellite_position(
      const prn_t &prn,
      const gps_time_t &time,
      xyz_t &res) const {
    return NULL;
  }

  /**
   * Calculate User position/velocity with hint
   * This is multi-constellation version,
   * and reference implementation to be hidden by optimized one in sub class.
   *
   * @param res (out) calculation results and matrices used for calculation
   * @param measurement PRN, pseudo-range, and pseudo-range rate information
   * @param receiver_time receiver time at measurement
   * @param user_position_init initial solution of user position in XYZ meters and LLH
   * @param receiver_error_init initial solution of receiver clock error in meters
   * @param good_init if true, initial position and clock error are goodly guessed.
   * @param with_velocity if true, perform velocity estimation.
   * @see update_ephemeris(), register_ephemeris
   */
  virtual void user_pvt(
      user_pvt_t &res,
      const measurement_t &measurement,
      const gps_time_t &receiver_time,
      const pos_t &user_position_init,
      const float_t &receiver_error_init,
      const bool &good_init = true,
      const bool &with_velocity = true) const {

    measurement2_t measurement2;
    measurement2.reserve(measurement.size());
    for(typename measurement_t::const_iterator it(measurement.begin()), it_end(measurement.end());
        it != it_end; ++it){
      typename measurement2_t::value_type v = {
          it->first, &(it->second), &select(it->first)}; // prn, measurement, solver
      xyz_t sat;
      if(!v.solver->satellite_position(v.prn, receiver_time, sat)){
        // pre-check satellite availability; remove it when its position is unknown
        continue;
      }
      measurement2.push_back(v);
    }
    user_pvt(
        res,
        measurement2, receiver_time, user_position_init, receiver_error_init,
        user_pvt_opt_t(good_init, with_velocity));
  }

  template <class SolverT>
  struct solver_interface_t {
    const SolverT &solver;
    solver_interface_t(const SolverT &solver_) : solver(solver_) {}

    typedef typename SolverT::user_pvt_t pvt_t;

    pvt_t user_pvt(
        const measurement_t &measurement,
        const gps_time_t &receiver_time,
        const pos_t &user_position_init,
        const float_t &receiver_error_init,
        const bool &good_init = true,
        const bool &with_velocity = true) const {
      pvt_t res;
      solver.user_pvt(res,
          measurement, receiver_time,
          user_position_init, receiver_error_init,
          good_init, with_velocity);
      return res;
    }

    /**
     * Calculate User position/velocity with hint
     *
     * @param measurement PRN, pseudo-range, and pseudo-range rate information
     * @param receiver_time receiver time at measurement
     * @param user_position_init_xyz initial solution of user position in meters
     * @param receiver_error_init initial solution of receiver clock error in meters
     * @param good_init if true, initial position and clock error are goodly guessed.
     * @param with_velocity if true, perform velocity estimation.
     * @return calculation results and matrices used for calculation
     * @see update_ephemeris(), register_ephemeris
     */
    pvt_t user_pvt(
        const measurement_t &measurement,
        const gps_time_t &receiver_time,
        const xyz_t &user_position_init_xyz,
        const float_t &receiver_error_init,
        const bool &good_init = true,
        const bool &with_velocity = true) const {
      pos_t user_position_init = {user_position_init_xyz, user_position_init_xyz.llh()};
      return user_pvt(
          measurement, receiver_time,
          user_position_init, receiver_error_init,
          good_init, with_velocity);
    }

    /**
     * Calculate User position/velocity without hint
     *
     * @param measurement PRN, pseudo-range, and pseudo-range rate information
     * @param receiver_time receiver time at measurement
     * @return calculation results and matrices used for calculation
     */
    pvt_t user_pvt(
        const measurement_t &measurement,
        const gps_time_t &receiver_time) const {
      return user_pvt(measurement, receiver_time, xyz_t(), 0, false);
    }

    /**
     * Calculate User position/velocity with hint
     *
     * @param prn_range PRN, pseudo-range list
     * @param prn_rate PRN, pseudo-range rate list
     * @param receiver_time receiver time at measurement
     * @param user_position_init initial solution of user position in XYZ meters and LLH
     * @param receiver_error_init initial solution of receiver clock error in meters
     * @param good_init if true, initial position and clock error are goodly guessed.
     * @param with_velocity if true, perform velocity estimation.
     * @return calculation results and matrices used for calculation
     * @see update_ephemeris(), register_ephemeris
     */
    pvt_t user_pvt(
        const prn_obs_t &prn_range,
        const prn_obs_t &prn_rate,
        const gps_time_t &receiver_time,
        const pos_t &user_position_init,
        const float_t &receiver_error_init,
        const bool &good_init = true,
        const bool &with_velocity = true) const {
      measurement_t measurement;
      measurement_util_t::merge(measurement, prn_range, measurement_items_t::L1_PSEUDORANGE);
      measurement_util_t::merge(measurement, prn_rate, measurement_items_t::L1_RANGE_RATE);
      return user_pvt(
          measurement, receiver_time,
          user_position_init, receiver_error_init,
          good_init, with_velocity);
    }

    /**
     * Calculate User position/velocity with hint
     *
     * @param prn_range PRN, pseudo-range list
     * @param prn_rate PRN, pseudo-range rate list
     * @param receiver_time receiver time at measurement
     * @param user_position_init_xyz initial solution of user position in meters
     * @param receiver_error_init initial solution of receiver clock error in meters
     * @param good_init if true, initial position and clock error are goodly guessed.
     * @param with_velocity if true, perform velocity estimation.
     * @return calculation results and matrices used for calculation
     * @see update_ephemeris(), register_ephemeris
     */
    pvt_t user_pvt(
        const prn_obs_t &prn_range,
        const prn_obs_t &prn_rate,
        const gps_time_t &receiver_time,
        const xyz_t &user_position_init_xyz,
        const float_t &receiver_error_init,
        const bool &good_init = true,
        const bool &with_velocity = true) const {
      pos_t user_position_init = {user_position_init_xyz, user_position_init_xyz.llh()};
      return user_pvt(
          prn_range, prn_rate, receiver_time,
          user_position_init, receiver_error_init,
          good_init, with_velocity);
    }

    /**
     * Calculate User position/velocity without hint
     *
     * @param prn_range PRN, pseudo-range list
     * @param prn_rate PRN, pseudo-range rate list
     * @param receiver_time receiver time at measurement
     * @return calculation results and matrices used for calculation
     */
    pvt_t user_pvt(
        const prn_obs_t &prn_range,
        const prn_obs_t &prn_rate,
        const gps_time_t &receiver_time) const {
      return user_pvt(prn_range, prn_rate, receiver_time, xyz_t(), 0, false);
    }

    /**
     * Calculate User position with hint
     *
     * @param prn_range PRN, pseudo-range list
     * @param receiver_time receiver time at measurement
     * @param user_position_init initial solution of user position in meters
     * @param receiver_error_init initial solution of receiver clock error in meters
     * @param good_init if true, initial position and clock error are goodly guessed.
     * @return calculation results and matrices used for calculation
     */
    pvt_t user_position(
        const prn_obs_t &prn_range,
        const gps_time_t &receiver_time,
        const xyz_t &user_position_init,
        const float_t &receiver_error_init,
        const bool &good_init = true) const {

      return user_pvt(
          prn_range, prn_obs_t(), receiver_time,
          user_position_init, receiver_error_init,
          good_init, false);
    }

    /**
     * Calculate User position without hint
     *
     * @param prn_range PRN and pseudo range
     * @param receiver_time receiver time at measurement
     * @return calculation results and matrices used for calculation
     */
    pvt_t user_position(
        const prn_obs_t &prn_range,
        const gps_time_t &receiver_time) const {
      return user_position(prn_range, receiver_time, xyz_t(), 0, false);
    }
  };

  solver_interface_t<GPS_Solver_Base<FloatT> > solve() const {
    return solver_interface_t<GPS_Solver_Base<FloatT> >(*this);
  }

  static typename user_pvt_t::dop_t dop(const matrix_t &C, const pos_t &user_position) {
    return typename user_pvt_t::dop_t(geometric_matrices_t::rotate_C(C, user_position.ecef2enu()));
  }
};

template <class FloatT>
const typename GPS_Solver_Base<FloatT>::measurement_item_set_t
    GPS_Solver_Base<FloatT>::L1CA = {
#define make_entry(key) \
    GPS_Solver_Base<FloatT>::measurement_items_t::L1_ ## key
#define make_entry2(key) { \
    make_entry(key), \
    make_entry(key ## _SIGMA)}
      make_entry2(PSEUDORANGE),
      make_entry2(DOPPLER),
      make_entry2(CARRIER_PHASE),
      make_entry2(RANGE_RATE),
      make_entry(SIGNAL_STRENGTH_dBHz),
      make_entry(LOCK_SEC),
#undef make_entry2
#undef make_entry
    };

template <class FloatT>
const typename GPS_Solver_Base<FloatT>::range_error_t
    GPS_Solver_Base<FloatT>::range_error_t::not_corrected = {
      MASK_RECEIVER_CLOCK | MASK_SATELLITE_CLOCK | MASK_IONOSPHERIC | MASK_TROPOSPHERIC,
      {0},
    };

template <class FloatT>
const typename GPS_Solver_Base<FloatT>::no_correction_t
    GPS_Solver_Base<FloatT>::no_correction;

template <class FloatT, class PVT_BaseT = typename GPS_Solver_Base<FloatT>::user_pvt_t>
struct GPS_PVT_Debug : public PVT_BaseT {
  typename GPS_Solver_Base<FloatT>::matrix_t G, W, delta_r;
};

template <class FloatT, class SolverBaseT = GPS_Solver_Base<FloatT> >
struct GPS_Solver_Base_Debug : public SolverBaseT {
  typedef SolverBaseT base_t;
  typedef GPS_Solver_Base_Debug<FloatT, SolverBaseT> self_t;
  virtual ~GPS_Solver_Base_Debug() {}

#if defined(__GNUC__) && (__GNUC__ < 5)
#define inheritate_type(x) typedef typename base_t::x x;
#else
#define inheritate_type(x) using typename base_t::x;
#endif
  inheritate_type(float_t);
  inheritate_type(geometric_matrices_t);
#undef inheritate_type

  typedef GPS_PVT_Debug<float_t, typename base_t::user_pvt_t> user_pvt_t;

  typename base_t::template solver_interface_t<self_t> solve() const {
    return typename base_t::template solver_interface_t<self_t>(*this);
  }

protected:
  virtual bool update_position_solution(
      const geometric_matrices_t &geomat,
      typename GPS_Solver_Base<FloatT>::user_pvt_t &res) const {

    // Least square
    if(!base_t::update_position_solution(geomat, res)){
      return false;
    }
    static_cast<user_pvt_t &>(res).G = geomat.G;
    static_cast<user_pvt_t &>(res).W = geomat.W;
    static_cast<user_pvt_t &>(res).delta_r = geomat.delta_r;
    return true;
  }
};


#endif /* __GPS_SOLVER_BASE_H__ */