#pragma once

#include <fstream>
#include <mutex>

#include <basalt/utils/ba_utils.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <basalt/linearization/landmark_block.hpp>
#include <basalt/utils/sophus_utils.hpp>

namespace basalt {

template <typename Scalar, int POSE_SIZE>
class LandmarkBlockAbsDynamic : public LandmarkBlock<Scalar> {
 public:
  using Options = typename LandmarkBlock<Scalar>::Options;
  using State = typename LandmarkBlock<Scalar>::State;

  inline bool isNumericalFailure() const override {
    return state == State::NumericalFailure;
  }

  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;

  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;

  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
  using RowMatX =
      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

  virtual inline void allocateLandmark(
      Keypoint<Scalar>& lm,
      const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
                                         RelPoseLin<Scalar>>& relative_pose_lin,
      const Calibration<Scalar>& calib, const AbsOrderMap& aom,
      const Options& options,
      const std::map<TimeCamId, size_t>* rel_order = nullptr) override {
    // some of the logic assumes the members are at their initial values
    BASALT_ASSERT(state == State::Uninitialized);

    UNUSED(rel_order);

    lm_ptr = &lm;
    options_ = &options;
    calib_ = &calib;

    // TODO: consider for VIO that we have a lot of 0 columns if we just use aom
    // --> add option to use different AOM with reduced size and/or just
    // involved poses --> when accumulating results, check which case we have;
    // if both aom are identical, we don't have to do block-wise operations.
    aom_ = &aom;

    pose_lin_vec.clear();
    pose_lin_vec.reserve(lm.obs.size());
    pose_tcid_vec.clear();
    pose_tcid_vec.reserve(lm.obs.size());

    // LMBs without host frame should not be created
    BASALT_ASSERT(aom.abs_order_map.count(lm.host_kf_id.frame_id) > 0);

    for (const auto& [tcid_t, pos] : lm.obs) {
      size_t i = pose_lin_vec.size();

      auto it = relative_pose_lin.find(std::make_pair(lm.host_kf_id, tcid_t));
      BASALT_ASSERT(it != relative_pose_lin.end());

      if (aom.abs_order_map.count(tcid_t.frame_id) > 0) {
        pose_lin_vec.push_back(&it->second);
      } else {
        // Observation droped for marginalization
        pose_lin_vec.push_back(nullptr);
      }
      pose_tcid_vec.push_back(&it->first);

      res_idx_by_abs_pose_[it->first.first.frame_id].insert(i);   // host
      res_idx_by_abs_pose_[it->first.second.frame_id].insert(i);  // target
    }

    // number of pose-jacobian columns is determined by oam
    padding_idx = aom_->total_size;

    num_rows = pose_lin_vec.size() * 2 + 3;  // residuals and lm damping

    size_t pad = padding_idx % 4;
    if (pad != 0) {
      padding_size = 4 - pad;
    }

    lm_idx = padding_idx + padding_size;
    res_idx = lm_idx + 3;
    num_cols = res_idx + 1;

    // number of columns should now be multiple of 4 for good memory alignment
    // TODO: test extending this to 8 --> 32byte alignment for float?
    BASALT_ASSERT(num_cols % 4 == 0);

    storage.resize(num_rows, num_cols);

    damping_rotations.clear();
    damping_rotations.reserve(6);

    state = State::Allocated;
  }

  // may set state to NumericalFailure --> linearization at this state is
  // unusable. Numeric check is only performed for residuals that were
  // considered to be used (valid), which depends on
  // use_valid_projections_only setting.
  virtual inline Scalar linearizeLandmark() override {
    BASALT_ASSERT(state == State::Allocated ||
                  state == State::NumericalFailure ||
                  state == State::Linearized || state == State::Marginalized);

    // storage.setZero(num_rows, num_cols);
    storage.setZero();
    damping_rotations.clear();
    damping_rotations.reserve(6);

    bool numerically_valid = true;

    Scalar error_sum = 0;

    size_t i = 0;
    for (const auto& [tcid_t, obs] : lm_ptr->obs) {
      std::visit(
          [&, obs = obs](const auto& cam) {
            // TODO: The pose_lin_vec[i] == nullptr is intended to deal with
            // dropped measurements during marginalization. However, dropped
            // measurements should only occur for the remaining frames, not for
            // the marginalized frames. Maybe these are observations bewtween
            // two marginalized frames, if more than one is marginalized at the
            // same time? But those we would not have to drop... Double check if
            // and when this happens and possibly resolve by fixing handling
            // here, or else updating the measurements in lmdb before calling
            // linearization. Otherwise, check where else we need a `if
            // (pose_lin_vec[i])` check or `pose_lin_vec[i] != nullptr` assert
            // in this class.

            if (pose_lin_vec[i]) {
              size_t obs_idx = i * 2;
              size_t abs_h_idx =
                  aom_->abs_order_map.at(pose_tcid_vec[i]->first.frame_id)
                      .first;
              size_t abs_t_idx =
                  aom_->abs_order_map.at(pose_tcid_vec[i]->second.frame_id)
                      .first;

              Vec2 res;
              Eigen::Matrix<Scalar, 2, POSE_SIZE> d_res_d_xi;
              Eigen::Matrix<Scalar, 2, 3> d_res_d_p;

              using CamT = std::decay_t<decltype(cam)>;
              bool valid = linearizePoint<Scalar, CamT>(
                  obs, *lm_ptr, pose_lin_vec[i]->T_t_h, cam, res, &d_res_d_xi,
                  &d_res_d_p);

              if (!options_->use_valid_projections_only || valid) {
                numerically_valid = numerically_valid &&
                                    d_res_d_xi.array().isFinite().all() &&
                                    d_res_d_p.array().isFinite().all();

                const Scalar res_squared = res.squaredNorm();
                const auto [weighted_error, weight] =
                    compute_error_weight(res_squared);
                const Scalar sqrt_weight =
                    std::sqrt(weight) / options_->obs_std_dev;

                error_sum += weighted_error /
                             (options_->obs_std_dev * options_->obs_std_dev);

                storage.template block<2, 3>(obs_idx, lm_idx) =
                    sqrt_weight * d_res_d_p;
                storage.template block<2, 1>(obs_idx, res_idx) =
                    sqrt_weight * res;

                d_res_d_xi *= sqrt_weight;
                storage.template block<2, 6>(obs_idx, abs_h_idx) +=
                    d_res_d_xi * pose_lin_vec[i]->d_rel_d_h;
                storage.template block<2, 6>(obs_idx, abs_t_idx) +=
                    d_res_d_xi * pose_lin_vec[i]->d_rel_d_t;
              }
            }

            i++;
          },
          calib_->intrinsics[tcid_t.cam_id].variant);
    }

    if (numerically_valid) {
      state = State::Linearized;
    } else {
      state = State::NumericalFailure;
    }

    return error_sum;
  }

  virtual inline void performQR() override {
    BASALT_ASSERT(state == State::Linearized);

    // Since we use dense matrices Householder QR might be better:
    // https://mathoverflow.net/questions/227543/why-householder-reflection-is-better-than-givens-rotation-in-dense-linear-algebr

    if (options_->use_householder) {
      performQRHouseholder();
    } else {
      performQRGivens();
    }

    state = State::Marginalized;
  }

  // Sets damping and maintains upper triangular matrix for landmarks.
  virtual inline void setLandmarkDamping(Scalar lambda) override {
    BASALT_ASSERT(state == State::Marginalized);
    BASALT_ASSERT(lambda >= 0);

    if (hasLandmarkDamping()) {
      BASALT_ASSERT(damping_rotations.size() == 6);

      // undo dampening
      for (int n = 2; n >= 0; n--) {
        for (int m = n; m >= 0; m--) {
          storage.applyOnTheLeft(num_rows - 3 + n - m, n,
                                 damping_rotations.back().adjoint());
          damping_rotations.pop_back();
        }
      }
    }

    if (lambda == 0) {
      storage.template block<3, 3>(num_rows - 3, lm_idx).diagonal().setZero();
    } else {
      BASALT_ASSERT(Jl_col_scale.array().isFinite().all());

      storage.template block<3, 3>(num_rows - 3, lm_idx)
          .diagonal()
          .setConstant(sqrt(lambda));

      BASALT_ASSERT(damping_rotations.empty());

      // apply dampening and remember rotations to undo
      for (int n = 0; n < 3; n++) {
        for (int m = 0; m <= n; m++) {
          damping_rotations.emplace_back();
          damping_rotations.back().makeGivens(
              storage(n, lm_idx + n),
              storage(num_rows - 3 + n - m, lm_idx + n));
          storage.applyOnTheLeft(num_rows - 3 + n - m, n,
                                 damping_rotations.back());
        }
      }
    }
  }

  // lambda < 0 means computing exact model cost change
  virtual inline void backSubstitute(const VecX& pose_inc,
                                     Scalar& l_diff) override {
    BASALT_ASSERT(state == State::Marginalized);

    // For now we include all columns in LMB
    BASALT_ASSERT(pose_inc.size() == signed_cast(padding_idx));

    const auto Q1Jl = storage.template block<3, 3>(0, lm_idx)
                          .template triangularView<Eigen::Upper>();

    const auto Q1Jr = storage.col(res_idx).template head<3>();
    const auto Q1Jp = storage.topLeftCorner(3, padding_idx);

    Vec3 inc = -Q1Jl.solve(Q1Jr + Q1Jp * pose_inc);

    // We want to compute the model cost change. The model function is
    //
    //     L(inc) = F(x) + inc^T J^T r + 0.5 inc^T J^T J inc
    //
    // and thus the expected decrease in cost for the computed increment is
    //
    //     l_diff = L(0) - L(inc)
    //            = - inc^T J^T r - 0.5 inc^T J^T J inc
    //            = - inc^T J^T (r + 0.5 J inc)
    //            = - (J inc)^T (r + 0.5 (J inc)).
    //
    // Here we have J = [Jp, Jl] under the orthogonal projection Q = [Q1, Q2],
    // i.e. the linearized system (model cost) is
    //
    //    L(inc) = 0.5 || J inc + r ||^2 = 0.5 || Q^T J inc + Q^T r ||^2
    //
    // and below we thus compute
    //
    //    l_diff = - (Q^T J inc)^T (Q^T r + 0.5 (Q^T J inc)).
    //
    // We have
    //             | Q1^T |            | Q1^T Jp   Q1^T Jl |
    //    Q^T J =  |      | [Jp, Jl] = |                   |
    //             | Q2^T |            | Q2^T Jp      0    |.
    //
    // Note that Q2 is the nullspace of Jl, and Q1^T Jl == R. So with inc =
    // [incp^T, incl^T]^T we have
    //
    //                | Q1^T Jp incp + Q1^T Jl incl |
    //    Q^T J inc = |                             |
    //                | Q2^T Jp incp                |
    //

    // undo damping before we compute the model cost difference
    setLandmarkDamping(0);

    // compute "Q^T J incp"
    VecX QJinc = storage.topLeftCorner(num_rows - 3, padding_idx) * pose_inc;

    // add "Q1^T Jl incl" to the first 3 rows
    QJinc.template head<3>() += Q1Jl * inc;

    auto Qr = storage.col(res_idx).head(num_rows - 3);
    l_diff -= QJinc.transpose() * (Scalar(0.5) * QJinc + Qr);

    // TODO: detect and handle case like ceres, allowing a few iterations but
    // stopping eventually
    if (!inc.array().isFinite().all() ||
        !lm_ptr->direction.array().isFinite().all() ||
        !std::isfinite(lm_ptr->inv_dist)) {
      std::cerr << "Numerical failure in backsubstitution\n";
    }

    // Note: scale only after computing model cost change
    inc.array() *= Jl_col_scale.array();

    lm_ptr->direction += inc.template head<2>();
    lm_ptr->inv_dist = std::max(Scalar(0), lm_ptr->inv_dist + inc[2]);
  }

  virtual inline size_t numReducedCams() const override {
    BASALT_LOG_FATAL("check what we mean by numReducedCams for absolute poses");
    return pose_lin_vec.size();
  }

  inline void addQ2JpTQ2Jp_mult_x(VecX& res,
                                  const VecX& x_pose) const override {
    UNUSED(res);
    UNUSED(x_pose);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual inline void addQ2JpTQ2r(VecX& res) const override {
    UNUSED(res);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual inline void addJp_diag2(VecX& res) const override {
    BASALT_ASSERT(state == State::Linearized);

    for (const auto& [frame_id, idx_set] : res_idx_by_abs_pose_) {
      const int pose_idx = aom_->abs_order_map.at(frame_id).first;
      for (const int i : idx_set) {
        const auto block = storage.block(2 * i, pose_idx, 2, POSE_SIZE);

        res.template segment<POSE_SIZE>(pose_idx) +=
            block.colwise().squaredNorm();
      }
    }
  }

  virtual inline void addQ2JpTQ2Jp_blockdiag(
      BlockDiagonalAccumulator<Scalar>& accu) const override {
    UNUSED(accu);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual inline void scaleJl_cols() override {
    BASALT_ASSERT(state == State::Linearized);

    // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
    // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
    Jl_col_scale =
        (options_->jacobi_scaling_eps +
         storage.block(0, lm_idx, num_rows - 3, 3).colwise().norm().array())
            .inverse();

    storage.block(0, lm_idx, num_rows - 3, 3) *= Jl_col_scale.asDiagonal();
  }

  virtual inline void scaleJp_cols(const VecX& jacobian_scaling) override {
    BASALT_ASSERT(state == State::Marginalized);

    // we assume we apply scaling before damping (we exclude the last 3 rows)
    BASALT_ASSERT(!hasLandmarkDamping());

    storage.topLeftCorner(num_rows - 3, padding_idx) *=
        jacobian_scaling.asDiagonal();
  }

  inline bool hasLandmarkDamping() const { return !damping_rotations.empty(); }

  virtual inline void printStorage(const std::string& filename) const override {
    std::ofstream f(filename);

    Eigen::IOFormat CleanFmt(4, 0, " ", "\n", "", "");

    f << "Storage (state: " << state
      << ", damping: " << (hasLandmarkDamping() ? "yes" : "no")
      << " Jl_col_scale: " << Jl_col_scale.transpose() << "):\n"
      << storage.format(CleanFmt) << std::endl;

    f.close();
  }
#if 0
  virtual inline void stage2(
      Scalar lambda, const VecX* jacobian_scaling, VecX* precond_diagonal2,
      BlockDiagonalAccumulator<Scalar>* precond_block_diagonal,
      VecX& bref) override {
    // 1. scale jacobian
    if (jacobian_scaling) {
      scaleJp_cols(*jacobian_scaling);
    }

    // 2. dampen landmarks
    setLandmarkDamping(lambda);

    // 3a. compute diagonal preconditioner (SCHUR_JACOBI_DIAGONAL)
    if (precond_diagonal2) {
      addQ2Jp_diag2(*precond_diagonal2);
    }

    // 3b. compute block diagonal preconditioner (SCHUR_JACOBI)
    if (precond_block_diagonal) {
      addQ2JpTQ2Jp_blockdiag(*precond_block_diagonal);
    }

    // 4. compute rhs of reduced camera normal equations
    addQ2JpTQ2r(bref);
  }
#endif

  inline State getState() const override { return state; }

  virtual inline size_t numQ2rows() const override { return num_rows - 3; }

 protected:
  inline void performQRGivens() {
    // Based on "Matrix Computations 4th Edition by Golub and Van Loan"
    // See page 252, Algorithm 5.2.4 for how these two loops work
    Eigen::JacobiRotation<Scalar> gr;
    for (size_t n = 0; n < 3; n++) {
      for (size_t m = num_rows - 4; m > n; m--) {
        gr.makeGivens(storage(m - 1, lm_idx + n), storage(m, lm_idx + n));
        storage.applyOnTheLeft(m, m - 1, gr);
      }
    }
  }

  inline void performQRHouseholder() {
    VecX tempVector1(num_cols);
    VecX tempVector2(num_rows - 3);

    for (size_t k = 0; k < 3; ++k) {
      size_t remainingRows = num_rows - k - 3;

      Scalar beta;
      Scalar tau;
      storage.col(lm_idx + k)
          .segment(k, remainingRows)
          .makeHouseholder(tempVector2, tau, beta);

      storage.block(k, 0, remainingRows, num_cols)
          .applyHouseholderOnTheLeft(tempVector2, tau, tempVector1.data());
    }
  }

  inline std::tuple<Scalar, Scalar> compute_error_weight(
      Scalar res_squared) const {
    // Note: Definition of cost is 0.5 ||r(x)||^2 to be in line with ceres

    if (options_->huber_parameter > 0) {
      // use huber norm
      const Scalar huber_weight =
          res_squared <= options_->huber_parameter * options_->huber_parameter
              ? Scalar(1)
              : options_->huber_parameter / std::sqrt(res_squared);
      const Scalar error =
          Scalar(0.5) * (2 - huber_weight) * huber_weight * res_squared;
      return {error, huber_weight};
    } else {
      // use squared norm
      return {Scalar(0.5) * res_squared, Scalar(1)};
    }
  }

  void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
                          size_t start_idx) const override {
    Q2r.segment(start_idx, num_rows - 3) =
        storage.col(res_idx).tail(num_rows - 3);

    BASALT_ASSERT(Q2Jp.cols() == signed_cast(padding_idx));

    Q2Jp.block(start_idx, 0, num_rows - 3, padding_idx) =
        storage.block(3, 0, num_rows - 3, padding_idx);
  }

  void get_dense_Q2Jp_Q2r_rel(
      MatX& Q2Jp, VecX& Q2r, size_t start_idx,
      const std::map<TimeCamId, size_t>& rel_order) const override {
    UNUSED(Q2Jp);
    UNUSED(Q2r);
    UNUSED(start_idx);
    UNUSED(rel_order);
    BASALT_LOG_FATAL("not implemented");
  }

  void add_dense_H_b(DenseAccumulator<Scalar>& accum) const override {
    UNUSED(accum);
    BASALT_LOG_FATAL("not implemented");
  }

  void add_dense_H_b(MatX& H, VecX& b) const override {
    const auto r = storage.col(res_idx).tail(num_rows - 3);
    const auto J = storage.block(3, 0, num_rows - 3, padding_idx);

    H.noalias() += J.transpose() * J;
    b.noalias() += J.transpose() * r;
  }

  void add_dense_H_b_rel(
      MatX& H_rel, VecX& b_rel,
      const std::map<TimeCamId, size_t>& rel_order) const override {
    UNUSED(H_rel);
    UNUSED(b_rel);
    UNUSED(rel_order);
    BASALT_LOG_FATAL("not implemented");
  }

  const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
      const override {
    BASALT_LOG_FATAL("not implemented");
  }

  Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
      const std::map<TimeCamId, size_t>& rel_order) const override {
    UNUSED(rel_order);
    BASALT_LOG_FATAL("not implemented");
  }

  void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const override {
    UNUSED(H_rel);
    UNUSED(b_rel);
    BASALT_LOG_FATAL("not implemented");
  }

  virtual TimeCamId getHostKf() const override { return lm_ptr->host_kf_id; }

 private:
  // Dense storage for pose Jacobians, padding, landmark Jacobians and
  // residuals [J_p | pad | J_l | res]
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
      storage;

  Vec3 Jl_col_scale = Vec3::Ones();
  std::vector<Eigen::JacobiRotation<Scalar>> damping_rotations;

  std::vector<const RelPoseLin<Scalar>*> pose_lin_vec;
  std::vector<const std::pair<TimeCamId, TimeCamId>*> pose_tcid_vec;
  size_t padding_idx = 0;
  size_t padding_size = 0;
  size_t lm_idx = 0;
  size_t res_idx = 0;

  size_t num_cols = 0;
  size_t num_rows = 0;

  const Options* options_ = nullptr;

  State state = State::Uninitialized;

  Keypoint<Scalar>* lm_ptr = nullptr;
  const Calibration<Scalar>* calib_ = nullptr;
  const AbsOrderMap* aom_ = nullptr;

  std::map<int64_t, std::set<int>> res_idx_by_abs_pose_;
};

}  // namespace basalt


// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ASTRONOMY_COORDINATE_BASE_COORDINATE_SYSTEM_HPP
#define BOOST_ASTRONOMY_COORDINATE_BASE_COORDINATE_SYSTEM_HPP

#include <tuple>
#include <cmath>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/static_assert.hpp>

#include <boost/astronomy/coordinate/representation.hpp>
#include <boost/astronomy/coordinate/differential.hpp>
#include <boost/astronomy/detail/is_base_template_of.hpp>
#include <boost/astronomy/coordinate/arithmetic.hpp>

namespace boost { namespace astronomy { namespace coordinate {

typedef boost::geometry::degree degree;
typedef boost::geometry::radian radian;

namespace bu = boost::units;
namespace bg = boost::geometry;

template <typename Representation, typename Differential>
struct base_frame
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
        <boost::astronomy::coordinate::base_representation, Representation>::value),
        "First template argument is expected to be a representation class");
    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
        <boost::astronomy::coordinate::base_differential, Differential>::value),
        "Second template argument is expected to be a differential class");
    ///@endcond

protected:
    Representation data;
    Differential motion;

public:
    typedef Representation representation;
    typedef Differential differential;

    //!returns 2d propermotion
    std::tuple
    <
        typename Differential::quantity1,
        typename Differential::quantity2
    > get_proper_motion() const
    {
        return std::make_tuple
        (
            Differential::quantity1::from_value(bg::get<0>(motion.get_differential())),
            Differential::quantity2::from_value(bg::get<1>(motion.get_differential()))
        );
    }

    //!returns radial velocity (line of sight velocity) of the object
    typename Differential::quantity3 get_radial_velocity() const
    {
        return Differential::quantity3::from_value(bg::get<2>(motion.get_differential()));
    }

    /*!returns differential data in form of boost::geometry::model::point
    having components (pm_lat, pm_lon, radial_velocity) including cos(lat) component*/
    Differential get_differential() const
    {
        return motion;
    }

    //!set differential for the motion of the object
    void set_differential(Differential const& other)
    {
        this->motion = other;
    }

    /*!returns coordinate data in form of boost::geometry::model::point
    having components (lat, lon, distance)*/
    Representation get_data() const
    {
        return data;
    }

    //!returns data in specified subclass of base_representation
    //template <typename ReturnType>
    //ReturnType represent_as() const
    //{
    //    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
    //        <boost::astronomy::coordinate::base_representation, ReturnType>::value),
    //        "return type is expected to be a representation class");

    //    return ReturnType(data);
    //}

    //!angular separation between two coordinates in radians
    bu::quantity<bu::si::plane_angle> get_angular_separation(base_frame const& other) const
    {
        return bu::quantity<bu::si::plane_angle>::from_value(
            std::acos((dot(this->data, other.get_data()) /
                (magnitude(this->data) * magnitude(other.get_data()))).value())
        );
    }

};
}}} //namespace boost::astronomy::coordinate
#endif  // !BOOST_ASTRONOMY_COORDINATE_BASE_COORDINATE_SYSTEM_HPP


/*! \file hydrodynamics_2d.hpp
  \brief Various manipulations of hydrodynamic variables
  \author Almog Yalinewich
*/

#ifndef HYDRODYNAMICS_2D_HPP
#define HYDRODYNAMICS_2D_HPP 1

#include "spatial_distribution2d.hpp"
#include "../common/equation_of_state.hpp"
#include "point_motion.hpp"
#include "spatial_reconstruction.hpp"
#include "SourceTerm.hpp"
#include "../../misc/utils.hpp"
#include "../../misc/lazy_list.hpp"
#include "../common/hydrodynamics.hpp"
#include "../../misc/universal_error.hpp"
#include "../../tessellation/ConvexHull.hpp"
#include "../../tessellation/polygon_overlap_area.hpp"
#include <boost/scoped_ptr.hpp>
#include "../../tessellation/EdgeLengthCorrect.hpp"
#include "physical_geometry.hpp"
#include "time_step_function.hpp"
#include "cache_data.hpp"
#include "../common/riemann_solver.hpp"

/*! \brief Rotates primitive variables to align with edge
  \param n Normal directions
  \param p Parallel direction
  \param cell Primitive variables
  \return Rotated cell
 */
Primitive RotatePrimitive(const Vector2D& n,
	const Vector2D& p,
	const Primitive& cell);

/*! \brief Rotates flux from the edge frame back to the lab frame
  \param c Flux
  \param n Normal direction
  \param p Parallel direction
  \return Rotated flux
 */
Conserved RotateFluxBack(const Conserved& c,
	const Vector2D& n,
	const Vector2D& p);

/*! \brief Given an edge and an index of one neighbor, returns the index of another neighbor
  \param edge Voronoi edge
  \param index Index of one neighbor
  \return Index of the other neighbor
 */
int get_other_index(const Edge& edge,
	const int index);

/*! \brief Initialize computational cells
  \param density Density distribution
  \param pressure Pressure distribution
  \param xvelocity Distribution of the x component of the velocity
  \param yvelocity Distribution of the y component of the velocity
  \param eos Equation of state
  \param tess Tessellation
  \param CMvalue Determines whether to evaluate the spatial distributions in the mesh generating points or the center of mass
  \return List of primitive variables
*/
vector<Primitive> InitialiseCells
(SpatialDistribution const& density,
	SpatialDistribution const& pressure,
	SpatialDistribution const& xvelocity,
	SpatialDistribution const& yvelocity,
	EquationOfState const& eos,
	Tessellation const& tess, bool CMvalue = true);

/*! \brief Calculates the intensive conserved variables
  \param cells Hydrodynamical cells
  \return List of conserved variables
*/
vector<Conserved> CalcConservedIntensive
(vector<Primitive> const& cells);

/*! \brief Calculates the extensive conserved variables
  \param cons_int Conserved intensive variables
  \param tess Tessellation
  \param pg Physical geometry
  \return List of conserved variables
*/
vector<Conserved> CalcConservedExtensive
(const vector<Conserved>& cons_int,
	const Tessellation& tess,
	const PhysicalGeometry& pg);

/*! \brief Calculates the time step for a cell
  \param cell Computational cell
  \param width Cell width
  \param face_velocites Velocities of the edges of the cell
  \return Time step
*/
double TimeStepForCell(Primitive const& cell,
	double width, vector<Vector2D> const& face_velocites);

/*! \brief Move mesh points
  \param pointvelocity Velocities of all mesh points
  \param dt Time step
  \param tessellation Tessellation
  \param oldpoints Possible input for the location of the old points
  \param reorder Should we reorder the points
  \return The indeces of the hilbert order of the points
*/
vector<int> MoveMeshPoints(vector<Vector2D> const& pointvelocity,
	double dt, Tessellation& tessellation, bool reorder,
	vector<Vector2D> oldpoints = vector<Vector2D>());
/*! \brief Move mesh points
  \param pointvelocity Velocities of all mesh points
  \param dt Time step
  \param tessellation The local tessellation
  \param vproc The processor tessellation
  \param oldpoints Possible input for the location of the old points
  \param reorder Should we reorder the points
  \return The indeces of the hilbert order of the points
*/
vector<int> MoveMeshPoints(vector<Vector2D> const& pointvelocity,
	double dt, Tessellation& tessellation, Tessellation const& vproc, bool reorder,
	vector<Vector2D> oldpoints = vector<Vector2D>());

/*! \brief Calculates the intensive conserved variables
  \param tess Tessellation
  \param extensive Extensive conserved variables
  \param pg Physical geometry
  \return List of intensive conserved variables
 */
vector<Conserved> calc_conserved_intensive
(const Tessellation& tess,
	const vector<Conserved>& extensive,
	const PhysicalGeometry& pg);

/*! \brief Updates the intensive conserved variables
  \param tessellation Tessellation
  \param conservedextensive Extensive conserved variables
  \param conservedintensive Intensive conserved variables
*/
void UpdateConservedIntensive(Tessellation const& tessellation,
	vector<Conserved> const& conservedextensive,
	vector<Conserved>& conservedintensive);

/*! \brief Calculates the flux in the bulk of the fluid
  \param normaldir A unit vector normal to the interface
  \param paraldir A unit vector parallel to the interface
  \param left Primitive variables on the left side of the interface
  \param right Primitive variables on the right side of the interface
  \param edge_velocity Velocity of the interface
  \param rs Riemann solver
  \return Flux
 */
Conserved FluxInBulk(Vector2D const& normaldir,
	Vector2D const& paraldir,
	Primitive const& left,
	Primitive const& right,
	Vector2D const& edge_velocity,
	RiemannSolver const& rs);

/*! \brief Adds force contribution to the extensive conserved variables
  \param tess Tessellation
  \param pg Physical geometry
  \param cd Cache data
  \param cells Computational cells
  \param fluxes Fluxes
  \param point_velocities Velocities of the mesh generating points
  \param source Source term
  \param t Time
  \param dt Time step
  \param extensives Extensive variables
  \param tracerstickernames The names of the tracers and stickers
 */
void ExternalForceContribution
(const Tessellation& tess,
	const PhysicalGeometry& pg,
	const CacheData& cd,
	const vector<ComputationalCell>& cells,
	const vector<Extensive>& fluxes,
	const vector<Vector2D>& point_velocities,
	const SourceTerm& source,
	double t,
	double dt,
	vector<Extensive>& extensives,
	TracerStickerNames const& tracerstickernames);

/*! \brief Returns the position of all mesh generating points
  \param tess Tessellation
  \return Position of all mesh generating points
 */
vector<Vector2D> get_all_mesh_points
(Tessellation const& tess);

/*! \brief Changes the energy and sound speed so they would satisfy the equation of state
  \param vp Primitive variables
  \param eos Equation of state
  \return Primitive variable with corrected sound speed and energy
 */
vector<Primitive> make_eos_consistent
(vector<Primitive> const& vp,
	EquationOfState const& eos);

/*! \brief Returns the energies due to external potentials
  \param tess Tessellation
  \param g TBA
  \return TBA
  \todo Add documentation
 */
vector<double> GetForceEnergy(Tessellation const& tess,
	vector<double> const& g);

/*! \brief Calculates extensive tracers
  \param intensive_tracer Intensive tracers
  \param tess Tessellation
  \param cells List of primitive variables
  \param pg Physical geometry
  \return List of extensive tracers
 */
vector<vector<double> > calc_extensive_tracer
(const vector<vector<double> > & intensive_tracer,
	const Tessellation& tess,
	const vector<Primitive>& cells,
	const PhysicalGeometry& pg);

/*! \brief Calculates the extensive tracer
  \param tracer Intensive tracer
  \param tess Tessellation
  \param cells Fluid elements
  \param result List of extensive tracers
 */
void MakeTracerExtensive
(vector<vector<double> > const&tracer,
	Tessellation const& tess, vector<Primitive> const& cells,
	vector<vector<double> > &result);

/*! \brief Makes a list of points to remove
  \param tess Tessellation
  \param point TBA
  \param R Radius?
  \param PointToRemove output
  \param Inner TBA
  \todo Add documentation
 */
void GetPointToRemove(Tessellation const& tess, Vector2D const& point,
	double R, vector<int> & PointToRemove, int Inner);

/*! \brief Applies a correction to the extensive variables due to the change in volume during time step.
  \details This method calculates the change in extensive by calculating the volume swept by an edge and multiplying it by the intensive variables of the respective cell.
  \param extensive Extensive variables
  \param intensive Intensive variables
  \param tessold Old tessellation
  \param tessnew New tessellation
  \param facevelocity Face velocity
  \param dt Time step
  \param pointvelocity Velocities of mesh generating points
 */
void FixAdvection(vector<Conserved>& extensive,
	vector<Conserved> const& intensive,
	Tessellation const& tessold,
	Tessellation const& tessnew,
	vector<Vector2D> const& facevelocity,
	double dt, vector<Vector2D> const& pointvelocity);

/*! \brief Determines the time step
  \param hydro_time_step Time step derived from hydrodynamics
  \param external_dt Time step suggested by user
  \param current_time Current simulation time
  \param end_time Termination time
  \return Time step
 */
double determine_time_step(double hydro_time_step,
	double external_dt,
	double current_time,
	double end_time);

#endif // HYDRODYNAMICS_2D_HPP


/*
 * @file pppBayesTree.cpp
 * @brief Iterative GPS Range/Phase Estimator with collected data
 * @author Ryan Watson & Jason Gross
 */

#include "nonlinear/ISAM2.h"
#include "nonlinear/NonlinearFactorGraph.h"
#include "inference/Symbol.h"

#include "slam/PriorFactor.h"
#include "slam/BetweenFactor.h"
#include "pppbayestree/gnssNavigation/GnssData.h"
#include "pppbayestree/gnssNavigation/GnssTools.h"
#include "pppbayestree/gnssNavigation/PhaseFactor.h"
#include "pppbayestree/gnssNavigation/nonBiasStates.h"
#include "pppbayestree/configReader/ConfDataReader.hpp"

#include "pppbayestree/gnssNavigation/PseudorangeFactor.h"

// GPSTK
#include "pppbayestree/gpstk/MJD.hpp"
#include "pppbayestree/gpstk/PowerSum.hpp"
#include "pppbayestree/gpstk/Decimate.hpp"
#include "pppbayestree/gpstk/SolidTides.hpp"
#include "pppbayestree/gpstk/PoleTides.hpp"
#include "pppbayestree/gpstk/TropModel.hpp"
#include "pppbayestree/gpstk/BasicModel.hpp"
#include "pppbayestree/gpstk/CommonTime.hpp"
#include "pppbayestree/gpstk/PCSmoother.hpp"
#include "pppbayestree/gpstk/OceanLoading.hpp"
#include "pppbayestree/gpstk/CodeSmoother.hpp"
#include "pppbayestree/gpstk/SimpleFilter.hpp"
#include "pppbayestree/gpstk/MWCSDetector.hpp"
#include "pppbayestree/gpstk/SatArcMarker.hpp"
#include "pppbayestree/gpstk/DCBDataReader.hpp"
#include "pppbayestree/gpstk/ComputeWindUp.hpp"
#include "pppbayestree/gpstk/Rinex3NavData.hpp"
#include "pppbayestree/gpstk/GNSSconstants.hpp"
#include "pppbayestree/gpstk/ComputeLinear.hpp"
#include "pppbayestree/gpstk/GPSWeekSecond.hpp"
#include "pppbayestree/gpstk/LICSDetector2.hpp"
#include "pppbayestree/gpstk/DataStructures.hpp"
#include "pppbayestree/gpstk/RinexObsStream.hpp"
#include "pppbayestree/gpstk/Rinex3ObsStream.hpp"
#include "pppbayestree/gpstk/Rinex3NavStream.hpp"
#include "pppbayestree/gpstk/ComputeTropModel.hpp"
#include "pppbayestree/gpstk/SP3EphemerisStore.hpp"
#include "pppbayestree/gpstk/ComputeSatPCenter.hpp"
#include "pppbayestree/gpstk/EclipsedSatFilter.hpp"
#include "pppbayestree/gpstk/GPSEphemerisStore.hpp"
#include "pppbayestree/gpstk/CorrectCodeBiases.hpp"
#include "pppbayestree/gpstk/ComputeSatPCenter.hpp"
#include "pppbayestree/gpstk/RequireObservables.hpp"
#include "pppbayestree/gpstk/CorrectObservables.hpp"
#include "pppbayestree/gpstk/LinearCombinations.hpp"
#include "pppbayestree/gpstk/GravitationalDelay.hpp"
#include "pppbayestree/gpstk/PhaseCodeAlignment.hpp"
#include "pppbayestree/slam/dataset.h"

// BOOST
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

// STD
#include <chrono>
#include <iomanip>
#include <fstream>
#include <iostream>
#include <unistd.h>
#include <algorithm>
#include <Eigen/Core>

using namespace std;
using namespace gpstk;
using namespace boost;
using namespace std::chrono;
using namespace minisam;
namespace po = boost::program_options;

int main(int argc, char* argv[])
{
    // define std out print color
    vector<int> prn_vec;
    vector<rnxData> data;
    const string red("\033[0;31m");
    const string green("\033[0;32m");
    string confFile, gnssFile, station, sp3File, p1p2File, p1c1File, antennaModel;
    string rnx_file, nav_file, sp3_file, out_file, antexFile;
    double xn, yn, zn, xp, yp, range, phase, rho, minElev, weightFactor;
    double antennaOffSetH, antennaOffSetE, antennaOffSetN;
    int startKey(0), currKey, startEpoch(0), svn, doy;
    int nThreads(-1), phase_break, break_count(0), nextKey, dec_int, itsBelowThree=0, count=0;
    bool printECEF, printENU, printAmb, printUpdateRate, first_ob(true), usingP1(false);

    FILE *fprealtime=fopen("examples_tuning/gpsdata/pppgpsposbackcount.txt","w+");

    cout.precision(12);

    /*po::options_description desc("Available options");
    desc.add_options()
            ("help,h", "Print help message")
            ("confFile,c", po::value<string>(&confFile)->default_value(""),
            "Input config file" )
            ("out", po::value<string>(&out_file)->default_value(""),
            "output file.")
            ("usingP1", "Are you using P1 instead of C1?");
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
    po::notify(vm);*/

    ConfDataReader confReader;
    confReader.open("examples_tuning/gpsdata/phastball.conf");
    ISAM2Data isam2data;

    /*if (confFile.empty() ) {
            cout << red << "\n\n Currently, you need to provide a conf file \n"
                 << "\n\n"  << green << desc << endl;
    }*/

    while ( (station = confReader.getEachSection()) != "" )
    {
        // Fetch nominal station location [m]
        xn = confReader.fetchListValueAsDouble("nominalECEF",station);
        yn = confReader.fetchListValueAsDouble("nominalECEF",station);
        zn = confReader.fetchListValueAsDouble("nominalECEF",station);
        // day of year ( used for Niell Trop model)
        doy = confReader.getValueAsInt("DOY", station);
        // Elevation cut-off
        minElev = confReader.getValueAsDouble("minElev", station);
        // Code/carrier ratio
        weightFactor = confReader.getValueAsDouble("weightFactor", station);
        // Data file names
        gnssFile = confReader("rnxFile", station);
        sp3File = confReader("sp3File", station);
        p1p2File = confReader("p1p2", station);
        p1c1File = confReader("p1c1", station);
        // Print statements
        printENU = confReader.getValueAsBoolean("printENU", station);
        printAmb = confReader.getValueAsBoolean("printAmb", station);
        printECEF = confReader.getValueAsBoolean("printECEF", station);
        printUpdateRate = confReader.getValueAsBoolean("printUpdateRate", station);
    }

    //usingP1 = (vm.count("usingP1")>0);

    minivector nomXYZ(xn, yn, zn);
    minivector prop_xyz = nomXYZ;

    /*#ifdef USE_TBB
    std::auto_ptr<tbb::task_scheduler_init> init;
    if(nThreads > 0) {
            init.reset(new tbb::task_scheduler_init(nThreads));
    }
    else
            cout << green << " \n\n Using threads for all processors" << endl;
    #else
    */
    if(nThreads > 0)
    {
        cout << red <<" \n\n not compiled with TBB, so threading is"
             << " disabled and the --threads option cannot be used."
             << endl;
        exit(1);
    }
    //#endif

    //ISAM2DoglegParams doglegParams;
    ISAM2Params parameters;
    //parameters.optimizationParamsDogleg=new ISAM2DoglegParams;
    parameters.optimizationParamsGaussNewton=new ISAM2GaussNewtonParams;
    parameters.relinearizeThresholdDouble = 0.1;
    parameters.relinearizeSkip = 10;
    ISAM2 isam(parameters);

    double output_time = 0.0;
    double rw = 2.5;
    double rangeWeight = pow(rw,2);
    double phaseWeight = pow(rw*1/weightFactor,2);

    string value;

    minivector prior_nonBias(5,0.0);
    //prior_nonBias<<0.0, 0.0, 0.0, 0.0, 0.0;

    double bias_state(0.0);
    minivector phase_arc(34,0.0);
    //phase_arc.setZero();
    minivector bias_counter(34,0.0);
   // bias_counter.setZero();

    for (int i=1; i<34; i++)
    {
        bias_counter.data[i] = bias_counter.data[i-1] + 10000;
    }

    minivector* initEst5=new minivector(5);
    minivector_set_zero(initEst5);
   // initEst5<<0.0, 0.0, 0.0, 0.0, 0.0;

    minivector*  between_nonBias_State=new minivector(5);
    minivector_set_zero(between_nonBias_State);
    //between_nonBias_State<<0.0, 0.0, 0.0, 0.0, 0.0;

    //Values initial_values;
    std::map<int,minimatrix*> initial_values;
   // std::map<int,Pose3> initial_valuesP;
    //Values result;
   // std::map<int,Eigen::VectorXd> result;

    minivector BiasinitNoise(5);
    BiasinitNoise.data[0]=10.0;BiasinitNoise.data[1]=10.0;BiasinitNoise.data[2]=10.0;
    BiasinitNoise.data[3]=3e8;BiasinitNoise.data[4]=1e-1;
    //BiasinitNoise<<10.0, 10.0, 10.0, 3e8, 1e-1;
    minivector_cwisesqrt(&BiasinitNoise);
   // GaussianNoiseModel* nonBias_InitNoise=new GaussianNoiseModel(BiasinitNoise.cwiseSqrt());
    GaussianNoiseModel* nonBias_InitNoise=new GaussianNoiseModel(BiasinitNoise);

    minivector nonBiasProcessNoise(5);
    nonBiasProcessNoise.data[0]=0.1;nonBiasProcessNoise.data[1]=0.1;nonBiasProcessNoise.data[2]=0.1;
    nonBiasProcessNoise.data[3]=3e6;nonBiasProcessNoise.data[4]=3e-5;
    minivector_cwisesqrt(&nonBiasProcessNoise);

     GaussianNoiseModel* nonBias_ProcessNoise=new GaussianNoiseModel(nonBiasProcessNoise);

    //0.1, 0.1, 0.1, 3e6, 3e-5;
    minivector init_Noise(1,100);//init_Noise[0]=100;
    //init_Noise<<100;
    minivector_cwisesqrt(&init_Noise);

     GaussianNoiseModel* initNoise=new GaussianNoiseModel(init_Noise);

    string obs_path = findExampleDataFile(gnssFile,"examples_tuning/gpsdata");
    string p1p2_path = findExampleDataFile(p1p2File,"examples_tuning/gpsdata");
    string p1c1_path = findExampleDataFile(p1c1File,"examples_tuning/gpsdata");

    if ( obs_path.empty() )
    {
        // cout << " Must pass in obs file !!! " << desc << endl;
        exit(1);
    }
    if ( sp3File.empty() )
    {
        //cout << " Must pass in ephemeris file !!! " << desc << endl;
        exit(1);
    }
    // Declare a "SP3EphemerisStore" object to handle precise ephemeris
    SP3EphemerisStore SP3EphList;

    size_t pos = 0;
    string path, token;
    string delimiter = " ";
    while ((pos = sp3File.find(delimiter)) != string::npos)
    {
        path = findExampleDataFile(sp3File.substr(0,pos),"examples_tuning/gpsdata");
        SP3EphList.loadFile(path);
        sp3File.erase(0, pos + delimiter.length());
    }
    path = findExampleDataFile(sp3File,"examples_tuning/gpsdata");
    SP3EphList.loadFile(path);

    // Set flags to reject satellites with bad or absent positional
    // values or clocks
    SP3EphList.rejectBadPositions(true);
    SP3EphList.rejectBadClocks(true);

    // Create the input observation file stream
    Rinex3ObsStream rin(obs_path);

    // station nominal position
    Position nominalPos(xn, yn, zn);

    CorrectCodeBiases corrCode;
    corrCode.setDCBFile(p1p2_path, p1c1_path);

    if (!usingP1)
    {
        corrCode.setUsingC1(true);
    }

    // This is the GNSS data structure that will hold all the
    // GNSS-related information
    gnssRinex gRin;

    RequireObservables requireObs;
    requireObs.addRequiredType(TypeID::L1);
    requireObs.addRequiredType(TypeID::L2);

    SimpleFilter pObsFilter;
    pObsFilter.setFilteredType(TypeID::C1);

    if ( usingP1 )
    {
        requireObs.addRequiredType(TypeID::P1);
        pObsFilter.addFilteredType(TypeID::P1);
        requireObs.addRequiredType(TypeID::P2);
        pObsFilter.addFilteredType(TypeID::P2);
    }
    else
    {
        requireObs.addRequiredType(TypeID::C1);
        pObsFilter.addFilteredType(TypeID::C1);
        requireObs.addRequiredType(TypeID::P2);
        pObsFilter.addFilteredType(TypeID::P2);
    }

    // Declare a couple of basic modelers
    BasicModel basic(nominalPos, SP3EphList);
    basic.setMinElev(minElev);

    // Object to correct for SP3 Sat Phase-center offset
    ComputeSatPCenter svPcenter(SP3EphList, nominalPos);

    // Objects to mark cycle slips
    MWCSDetector markCSMW;  // Checks Merbourne-Wubbena cycle slip

    // object def several linear combinations
    LinearCombinations comb;

    // Object to compute linear combinations for cycle slip detection
    ComputeLinear linear1;
    if ( usingP1 )
    {
        linear1.addLinear(comb.pdeltaCombination);
        linear1.addLinear(comb.mwubbenaCombination);
    }
    else
    {
        linear1.addLinear(comb.pdeltaCombWithC1);
        linear1.addLinear(comb.mwubbenaCombWithC1);
    }
    linear1.addLinear(comb.ldeltaCombination);
    linear1.addLinear(comb.liCombination);

    ComputeLinear linear2;

    // Read if we should use C1 instead of P1
    if ( usingP1 )
    {
        linear2.addLinear(comb.pcCombination);
    }
    else
    {
        linear2.addLinear(comb.pcCombWithC1);
    }
    linear2.addLinear(comb.lcCombination);

    LICSDetector2 markCSLI2;       // Checks LI cycle slips

    // Object to keep track of satellite arcs
    SatArcMarker markArc;
    markArc.setDeleteUnstableSats(true);

    // Objects to compute gravitational delay effects
    GravitationalDelay grDelay(nominalPos);

    // Object to remove eclipsed satellites
    EclipsedSatFilter eclipsedSV;

    //Object to compute wind-up effect
    ComputeWindUp windup( SP3EphList, nominalPos );

    // Object to compute prefit-residuals
    ComputeLinear linear3(comb.pcPrefit);
    linear3.addLinear(comb.lcPrefit);

    TypeIDSet tset;
    tset.insert(TypeID::prefitC);
    tset.insert(TypeID::prefitL);

    // Declare a NeillTropModel object, setting the defaults
    NeillTropModel neillTM( nominalPos.getAltitude(),
                            nominalPos.getGeodeticLatitude(),
                            doy);

    // Objects to compute the tropospheric data
    ComputeTropModel computeTropo(neillTM);

    // initialize factor graph
    //NonlinearFactorGraph *graph = new NonlinearFactorGraph();
    NonlinearFactorGraph graph;

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();
    // Loop over all data epochs
    while(rin >> gRin)
    {
        TimeSystem sys;
        sys.fromString("GPS");
        CommonTime time(gRin.header.epoch);
        time.setTimeSystem(sys);
        GPSWeekSecond gpstime( time );

        // update nominal ECEF with propogated pos.
        NeillTropModel neillTM( nominalPos.getAltitude(),
                                nominalPos.getGeodeticLatitude(),
                                doy);
        try
        {
            gRin >> requireObs // Check if required observations are present
                 >> pObsFilter // Filter out spurious data
                 >> linear1 // Compute linear combinations to detect CS
                 >> markCSLI2 // Mark cycle slips
                 >> markArc // Keep track of satellite arcs
                 >> basic // Compute the basic components of model
                 >> eclipsedSV // Remove satellites in eclipse
                 >> grDelay // Compute gravitational delay
                 >> svPcenter // Computer delta for sat. phase center
                 >> corrCode // Correct for differential code biases
                 >> windup // phase windup correction
                 >> computeTropo // neill trop function
                 >> linear2  // Compute ionosphere-free combinations
                 >> linear3;   // Compute prefit residuals
        }
        catch(Exception& e)
        {
            continue;
        }
        catch(...)
        {
            cerr << "Unknown exception at epoch: " << time << endl;
            continue;
        }

        // Iterate through the GNSS Data Structure
        satTypeValueMap::const_iterator it;
        typeValueMap::const_iterator itObs;
        if ( itsBelowThree > 0 )
        {
            itsBelowThree = 0;
            continue;
        }

        if (gRin.body.size() == 0)
        {
            continue;
        }

        // Loop over all observed sats at current epoch
        for (it = gRin.body.begin(); it!= gRin.body.end(); it++)
        {

            start = std::chrono::steady_clock::now();
            svn = ((*it).first).id;
            double satX, satY, satZ;
            satX = (*it).second.getValue(TypeID::satX);
            satY = (*it).second.getValue(TypeID::satY);
            satZ = (*it).second.getValue(TypeID::satZ);
            minivector satXYZ(satX,satY,satZ);
            double range, rangeRes;
            range = (*it).second.getValue(TypeID::PC);
            rangeRes = (*it).second.getValue(TypeID::prefitC);
            double phase, phaseRes;
            phase = (*it).second.getValue(TypeID::LC);
            phaseRes = (*it).second.getValue(TypeID::prefitL);
            int phase_break;
            phase_break = (*it).second.getValue(TypeID::satArc);

            if (first_ob)
            {
                startKey = count;
                first_ob=false;
                PriorFactor* npn=new PriorFactor(Symbol('X',count).key(), initEst5,  nonBias_InitNoise);

                graph.push_back(npn);
                initial_values.insert(std::make_pair(Symbol('X',count).key(), new minivector(initEst5)));

            }

            if (phase_arc.data[svn]!=phase_break)
            {
                bias_state = phase - range;
                if (count > startKey)
                {
                    bias_counter.data[svn] = bias_counter.data[svn] +1;
                }
                minivector *biasb=new minivector(1);
                biasb->data[0]=bias_state;
               // biasb<<bias_state;

                //GaussianNoiseModel* initNoise=new GaussianNoiseModel(init_Noise.cwiseSqrt());

                PriorFactor* nphb=new PriorFactor(Symbol('B',bias_counter.data[svn]).key(),
                biasb,initNoise);
                graph.push_back(nphb);
                initial_values.insert(std::make_pair(Symbol('B',bias_counter.data[svn]).key(), new minivector(biasb)));
                phase_arc.data[svn] = phase_break;
            }
            // Generate pseudorange factor
            minivector gpsRangeFactorvec(1,sqrt(elDepWeight(satXYZ, nomXYZ, rangeWeight)));

            //gpsRangeFactorvec<<elDepWeight(satXYZ, nomXYZ, rangeWeight);
            //GaussianNoiseModel* ngpsrfn=new GaussianNoiseModel(gpsRangeFactorvec.cwiseSqrt());
            GaussianNoiseModel* ngpsrfn=new GaussianNoiseModel(gpsRangeFactorvec);
            PseudorangeFactor* ngpsrf=new PseudorangeFactor(Symbol('X',count).key(), rangeRes,
             new minivector(satXYZ), new minivector(nomXYZ),ngpsrfn);

            graph.push_back(ngpsrf);
            gpsRangeFactorvec.data[0]=sqrt(elDepWeight(satXYZ, nomXYZ, phaseWeight));
            GaussianNoiseModel* ngpspfn2=new GaussianNoiseModel(gpsRangeFactorvec);
            //GaussianNoiseModel* ngpspfn2=new GaussianNoiseModel(gpsRangeFactorvec.cwiseSqrt());
            PhaseFactor* ngpspf=new PhaseFactor(Symbol('X',count).key(),
             Symbol('B',bias_counter.data[svn]).key(),phaseRes, new minivector(satXYZ),
             new minivector( nomXYZ),ngpspfn2);
            graph.push_back(ngpspf);

            prn_vec.push_back(svn);
        }
        if (count > startKey )
        {
           // GaussianNoiseModel* nonBias_ProcessNoise=
           // new GaussianNoiseModel(nonBiasProcessNoise.cwiseSqrt());

            BetweenFactor* nbfnbs=new BetweenFactor(Symbol('X',count).key(),
            Symbol('X',count-1).key(), new minivector(between_nonBias_State), nonBias_ProcessNoise);
            graph.push_back(nbfnbs);
        }

        isam.update(graph, initial_values,isam2data);
        //result = isam.calculateEstimate(isam2data);
        isam.calculateEstimate(isam2data);

        end = std::chrono::steady_clock::now();

        //prior_nonBias =result.at(Symbol('X',count).key());
        //prior_nonBias =isam2data.result.at(Symbol('X',count).key());
        minimatrix_memcpy(&prior_nonBias,isam2data.resulttheta_.at(Symbol('X',count).key()));

        minivector delta_xyz(prior_nonBias.data[0], prior_nonBias.data[1], prior_nonBias.data[2]);
        Position deltaPos(prior_nonBias.data[0], prior_nonBias.data[1], prior_nonBias.data[2]);
        //prop_xyz = nomXYZ - delta_xyz;
        minivector_sub(&prop_xyz,nomXYZ,delta_xyz);
        nominalPos -= deltaPos;

        if (printECEF)
        {
            cout << "xyz " << gpstime.week << " " << gpstime.sow << " " << prop_xyz.x() <<
            " " << prop_xyz.y() << " " << prop_xyz.data[2] << endl;
        }

        if (printENU)
        {
            minivector enu = xyz2enu(prop_xyz, nomXYZ);
            cout << "enu " << gpstime.week << " " << gpstime.sow << " " <<
            enu.x() << " " << enu.y() << " " << enu.data[2] << endl;
            fprintf(fprealtime,"%d %.15f %.15f %.15f %.15f %d\n",
                    gpstime.week,gpstime.sow,enu.x(),enu.y(),enu.data[2],
                    isam.lastBacksubVariableCount);
        }

        if (printAmb)
        {
            for (int k=0; k<prn_vec.size(); k++)
            {
                cout << "amb. " << gpstime.week << " " << gpstime.sow << " ";
                cout << prn_vec[k] << " ";
                //cout << result.at(Symbol('B',bias_counter[prn_vec[k]]).key()) << endl;
                minimatrix_print(isam2data.resulttheta_.at(Symbol('B',
                bias_counter.data[prn_vec[k]]).key()));
                cout << endl;
            }
        }

        if (printUpdateRate)
        {
            cout << "Elapsed time "
                 << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count()
                 << " µs" << endl;
        }

        output_time = output_time +1;
        graph.resize(0);
        initial_values.clear();
        prn_vec.clear();
        count++;
        initial_values.insert(std::make_pair(Symbol('X',count).key(),
        new minivector(prior_nonBias)));
       if(count>20000)
        {
         isam.clearall();
    isam2data.clearvalues();
    isam2data.clearfactors();
    delete parameters.optimizationParamsGaussNewton;
    delete between_nonBias_State;
    delete nonBias_InitNoise;
    delete initNoise;
     delete nonBias_ProcessNoise;
     for(auto& dlt:initial_values)
    {
       if(dlt.second!=NULL)
       delete dlt.second;
    }
    return 0;
        }
    }
   // ofstream bs("examples_tuning/data/gnsstree.dot");
    //isam.saveGraph(bs);

    isam.clearall();
    isam2data.clearvalues();
    isam2data.clearfactors();
    delete parameters.optimizationParamsGaussNewton;
    delete between_nonBias_State;
    delete nonBias_InitNoise;
    delete initNoise;
    delete nonBias_ProcessNoise;
    for(auto& dlt:initial_values)
    {
       if(dlt.second!=NULL)
       delete dlt.second;
    }

    return 0;
}
/* ************************************************************************* */


#pragma once
/** @file KDTree
 * @brief A binary tree constructed by splitting each box by a median.
 */

#include <vector>
#include <algorithm>
#include <type_traits>
#include <iterator>

#include <iostream>
#include <iomanip>

#include <boost/range.hpp>
#include <boost/iterator/permutation_iterator.hpp>

#include "fmmtl/tree/util/CountedProxyIterator.hpp"

#include "fmmtl/util/Logger.hpp"
#include "fmmtl/numeric/Vec.hpp"
#include "fmmtl/tree/BoundingBox.hpp"

#include "fmmtl/numeric/bits.hpp"

namespace fmmtl {
using boost::has_range_iterator;

//! Class for tree structure
template <unsigned DIM>
class KDTree {
  // Predeclarations
  struct Box;
  struct Body;
  struct BoxData;

  // The type of this tree
  typedef KDTree<DIM> tree_type;

 public:
  //! The type of indices and integers in this tree
  typedef unsigned size_type;

  //! The spacial point type used for centers and extents
  typedef Vec<DIM,double> point_type;

  //! Public type declarations
  typedef Box           box_type;
  typedef Body          body_type;
  using box_iterator  = CountedProxyIterator<Box,  const KDTree, size_type>;
  using body_iterator = CountedProxyIterator<Body, const KDTree, size_type>;

 private:
  // Tree representation

  // Permutation: permute_[i] is the current idx of originally ith point
  std::vector<size_type> permute_;
  // Vector of data describing a box
  std::vector<BoxData> box_data_;

  struct BoxData {
    // Index of the first body in this box
    size_type body_begin_;
    // Index of one-past-last body in this box
    size_type body_end_;

    // Bounding Box of this box
    BoundingBox<point_type> bounding_box_;

    BoxData(size_type bb, size_type be,
            const point_type& min, const point_type& max)
        : body_begin_(bb), body_end_(be),
          bounding_box_(min,max) {
    }
  };

  struct Body {
    /** Construct an invalid Body */
    Body() {}
    //! The original order this body was seen
    size_type number() const {
      return tree_->permute_[idx_];
    }
    //! The current order of this body
    size_type index() const {
      return idx_;
    }
   private:
    size_type idx_;
    tree_type* tree_;
    friend body_iterator;
    Body(size_type idx, const tree_type* tree)
        : idx_(idx), tree_(const_cast<tree_type*>(tree)) {
      FMMTL_ASSERT(idx_ < tree_->size());
    }
    friend class KDTree;
  };

  // A tree-aligned box
  struct Box {
    typedef typename tree_type::box_iterator  box_iterator;
    typedef typename tree_type::body_iterator body_iterator;

    //! Construct an invalid Box
    Box() {}
    //! The index of this box
    size_type index() const {
      return idx_;
    }
    //! The level of this box (root level is 0)
    size_type level() const {
      return std::log2(idx_+1);  // XXX: Slow? do this with bits
    }

    //! The dimension of each side of this box
    point_type extents() const {
      return data().bounding_box_.max() - data().bounding_box_.min();
    }
    //! The squared radius of this box
    double radius_sq() const {
      return norm_2_sq(extents()) / 4.0;
    }
    //! The center of this box
    point_type center() const {
      return (data().bounding_box_.max() + data().bounding_box_.min()) / 2.0;
    }

    //! The parent box of this box
    Box parent() const {
      FMMTL_ASSERT(!(*this == tree_->root()));
      return Box((idx_-1)/2, tree_);
    }

    //! True if this box is a leaf and has no children
    bool is_leaf() const {
      return 2*idx_+1 >= tree_->box_data_.size();
    }
    //! The begin iterator to the child boxes contained in this box
    box_iterator child_begin() const {
      FMMTL_ASSERT(!is_leaf());
      return box_iterator(2*idx_+1, tree_);
    }
    //! The end iterator to the child boxes contained in this box
    box_iterator child_end() const {
      FMMTL_ASSERT(!is_leaf());
      return box_iterator(2*idx_+3, tree_);
    }
    //! The number of children this box has
    static constexpr size_type num_children() {
      return 2;
    }

    //! The begin iterator to the bodies contained in this box
    body_iterator body_begin() const {
      return body_iterator(data().body_begin_, tree_);
    }
    //! The end iterator to the bodies contained in this box
    body_iterator body_end() const {
      return body_iterator(data().body_end_, tree_);
    }
    //! The number of bodies this box contains
    size_type num_bodies() const {
      return std::distance(body_begin(), body_end());
    }

    //! Equality comparison operator
    bool operator==(const Box& b) const {
      FMMTL_ASSERT(tree_ == b.tree_);
      return idx_ == b.idx_;
    }
    //! Comparison operator for std:: containers and algorithms
    bool operator<(const Box& b) const {
      FMMTL_ASSERT(tree_ == b.tree_);
      return idx_ < b.idx_;
    }

    //! Write a Box to an output stream
    inline friend std::ostream& operator<<(std::ostream& s,
                                           const box_type& b) {
      size_type num_bodies = b.num_bodies();
      size_type first_body = b.body_begin()->index();
      size_type last_body = first_body + num_bodies - 1;
      size_type parent_idx = b.index()==0 ? 0 : b.parent().index();

      return s << "Box " << b.index()
               << " (L" << b.level() << ", P" << parent_idx
               << ", " << num_bodies << (num_bodies == 1 ? " body" : " bodies")
               << " " << first_body << "-" << last_body
               << "): " << b.center() << " - " << b.extents();
    }
   private:
    size_type idx_;
    tree_type* tree_;
    friend box_iterator;
    Box(size_type idx, const tree_type* tree)
        : idx_(idx), tree_(const_cast<tree_type*>(tree)) {
      FMMTL_ASSERT(idx_ < tree_->boxes());
    }
    inline BoxData& data() const {
      return tree_->box_data_[idx_];
    }
    friend class KDTree;
  };

 public:

  /** Construct a tree encompassing a bounding box
   * and insert a range of points */
  template <typename Range>
  KDTree(const Range& rng, size_type n_crit = 256,
         typename std::enable_if<has_range_iterator<Range>::value>::type* = 0)
      : KDTree(rng.begin(), rng.end(), n_crit) {
  }

  /** Construct an tree encompassing a bounding box
   * and insert a range of points */
  template <typename PointIter>
  KDTree(PointIter first, PointIter last, size_type n_crit = 256) {
    insert(first, last, n_crit);
  }

  /** Return the Bounding Box that this KDTree encompasses */
  BoundingBox<point_type> bounding_box() const {
    return box_data_[0].bounding_box_;
  }

  /** Return the center of this KDTree */
  point_type center() const {
    return root().center();
  }

  /** The number of bodies contained in this tree */
  inline size_type size() const {
    return permute_.size();
  }
  /** The number of bodies contained in this tree */
  inline size_type bodies() const {
    return size();
  }

  /** The number of boxes contained in this tree */
  inline size_type boxes() const {
    return box_data_.size();
  }

  /** The number of boxes contained in level L of this tree */
  inline size_type boxes(size_type L) const {
    return (1 << L);
  }

  /** The maximum level of any box in this tree */
  inline size_type levels() const {
    return std::log2(boxes());  // XXX: Slow?
  }

  /** Returns true if the box is contained in this tree, false otherwise */
  inline bool contains(const box_type& box) const {
    return this == box.tree_;
  }
  /** Returns true if the body is contained in this tree, false otherwise */
  inline bool contains(const body_type& body) const {
    return this == body.tree_;
  }

  /** Return the root box of this tree */
  box_type root() const {
    return Box(0, this);
  }
  /** Return a box given its index */
  box_type box(const size_type idx) const {
    FMMTL_ASSERT(idx < box_data_.size());
    return Box(idx, this);
  }
  /** Return a body given its index */
  body_type body(const size_type idx) const {
    FMMTL_ASSERT(idx < size());
    return Body(idx, this);
  }
  /** Return an iterator to the first body in this tree */
  body_iterator body_begin() const {
    return body_iterator(0, this);
  }
  /** Return an iterator one past the last body in this tree */
  body_iterator body_end() const {
    return body_iterator(bodies(), this);
  }
  /** Return an iterator to the first box in this tree */
  box_iterator box_begin() const {
    return box_iterator(0, this);
  }
  /** Return an iterator one past the last box in this tree */
  box_iterator box_end() const {
    return box_iterator(boxes(), this);
  }
  /** Return an iterator to the first box at level L in this tree
   * @pre L < levels()
   */
  box_iterator box_begin(size_type L) const {
    FMMTL_ASSERT(L < levels());
    return box_iterator((1 << L) - 1, this);
  }
  /** Return an iterator one past the last box at level L in this tree
   * @pre L < levels()
   */
  box_iterator box_end(size_type L) const {
    FMMTL_ASSERT(L < levels());
    return box_iterator((1 << (L+1)) - 1, this);
  }

  template <typename RandomAccessIter>
  struct body_permuted_iterator {
    typedef typename std::vector<size_type>::const_iterator permute_iter;
    typedef boost::permutation_iterator<RandomAccessIter, permute_iter> type;
  };

  /** Tranform (permute) an iterator so its traversal follows the same order as
   * the bodies contained in this tree
   */
  template <typename RandomAccessIter>
  typename body_permuted_iterator<RandomAccessIter>::type
  body_permute(RandomAccessIter it, const body_iterator& bi) const {
    return boost::make_permutation_iterator(it, permute_.cbegin() + bi.index());
  }

  /** Tranform (permute) an iterator so its traversal follows the same order as
   * the bodies contained in this tree
   *
   * Specialized for bi = body_begin().
   */
  template <typename RandomAccessIter>
  typename body_permuted_iterator<RandomAccessIter>::type
  body_permute(RandomAccessIter it) const {
    return body_permute(it, body_begin());
  }

  /** Write an KDTree to an output stream */
  inline friend std::ostream& operator<<(std::ostream& s,
                                         const tree_type& t) {
    struct {
      inline std::ostream& print(std::ostream& s,
                                 const box_type& box) {
        s << std::string(2*box.level(), ' ') << box;
        if (!box.is_leaf())
          for (auto ci = box.child_begin(); ci != box.child_end(); ++ci)
            print(s << "\n", *ci);
        return s;
      }
    } recursive_box;

    return recursive_box.print(s, t.root());
  }

 private:
  //! TODO: Make dynamic and public?
  template <typename PointIter>
  void insert(PointIter p_first, PointIter p_last, size_type NCRIT) {
    FMMTL_LOG("KDTree Insert");

    FMMTL_ASSERT(p_first != p_last);

    // Create a point-idx pair vector
    typedef typename std::iterator_traits<PointIter>::value_type point_i_type;

    // XXX: Generalize?
    static_assert(std::is_same<point_i_type, point_type>::value,
                  "PointIter value_type must be point_type");

    typedef std::pair<point_type, unsigned> point_t;

    std::vector<point_t> point;
    // If iterators are random access, we can reserve space efficiently
    // Compile-time predicate!
    if (std::is_same<typename std::iterator_traits<PointIter>::iterator_category,
                     std::random_access_iterator_tag>::value)
      point.reserve(std::distance(p_first, p_last));

    unsigned idx = 0;
    BoundingBox<point_i_type> root_bb(*p_first);
    for (PointIter pi = p_first; pi != p_last; ++pi, ++idx) {
      point.emplace_back(*pi, idx);
      root_bb |= point.back().first;
    }

    permute_.reserve(point.size());

    // The number of leaf boxes that will be created
    // (Smallest power of two greater than or equal to ceil(N/NCRIT))
    unsigned leaves = ceil_pow_2((point.size() + NCRIT - 1) / NCRIT);
    unsigned levels = std::log2(leaves);

    // Reserve the number of boxes that will be added
    box_data_.reserve(2*leaves - 1);
    // Push the root box which contains all points
    box_data_.emplace_back(0, size_type(point.size()),
                           root_bb.min(), root_bb.max());

    // For every box that is created
    unsigned end_k = (1 << levels) - 1;
    for (unsigned k = 0; k < end_k; ++k) {

      // Get the bounding box of the current box
      auto& bb = box_data_[k].bounding_box_;

      // Make a comparator for the largest dimension
      const unsigned dim = max_dim(bb.max() - bb.min());
      auto comp = [=] (const point_t& a, const point_t& b) {
        return a.first[dim] < b.first[dim];
      };

      // Partition the points on the median of this dimension
      auto p_begin = point.begin() + box_data_[k].body_begin_;
      auto p_end   = point.begin() + box_data_[k].body_end_;
      auto p_mid   = p_begin + (p_end - p_begin + 1) / 2;
      std::nth_element(p_begin, p_mid, p_end, comp);

      // Record the child boxes
      unsigned mid = p_mid - point.begin();
      point_type split = bb.max();
      split[dim] = (*p_mid).first[dim];
      box_data_.emplace_back(box_data_[k].body_begin_, mid, bb.min(), split);
      split = bb.min();
      split[dim] = (*p_mid).first[dim];
      box_data_.emplace_back(mid, box_data_[k].body_end_, split, bb.max());
    }

    // Assert no re-allocation
    FMMTL_ASSERT(box_data_.size() <= 2*leaves-1);

    // Extract the permutation idx
    for (auto&& p : point)
      permute_.push_back(p.second);
  }

  static unsigned max_dim(const point_type& p) {
    return std::max_element(p.begin(), p.end()) - p.begin();
  }

  // Just making sure for now
  KDTree(const KDTree&) {};
  void operator=(const KDTree&) {};
};

} // end namespace fmmtl


#ifndef BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP
#define BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/util/math.hpp>
#include <cstdio>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/aasincos.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace chamb
    {

            static const double THIRD = 0.333333333333333333;
            static const double TOL = 1e-9;

            // specific for 'chamb'
            struct VECT { double r, Az; };
            struct XY { double x, y; };

            struct par_chamb
            {
                struct { /* control point data */
                double phi, lam;
                double cosphi, sinphi;
                VECT v;
                XY    p;
                double Az;
                } c[3];
                XY p;
                double beta_0, beta_1, beta_2;
            };

                static VECT /* distance and azimuth from point 1 to point 2 */
            vect(double dphi, double c1, double s1, double c2, double s2, double dlam) {
                VECT v;
                double cdl, dp, dl;

                cdl = cos(dlam);
                if (fabs(dphi) > 1. || fabs(dlam) > 1.)
                    v.r = aacos(s1 * s2 + c1 * c2 * cdl);
                else { /* more accurate for smaller distances */
                    dp = sin(.5 * dphi);
                    dl = sin(.5 * dlam);
                    v.r = 2. * aasin(sqrt(dp * dp + c1 * c2 * dl * dl));
                }
                if (fabs(v.r) > TOL)
                    v.Az = atan2(c2 * sin(dlam), c1 * s2 - s1 * c2 * cdl);
                else
                    v.r = v.Az = 0.;
                return v;
            }
                static double /* law of cosines */
            lc(double b,double c,double a) {
                return aacos(.5 * (b * b + c * c - a * a) / (b * c));
            }

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_chamb_spheroid : public base_t_f<base_chamb_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_chamb m_proj_parm;

                inline base_chamb_spheroid(const Parameters& par)
                    : base_t_f<base_chamb_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double sinphi, cosphi, a;
                    VECT v[3];
                    int i, j;

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    for (i = 0; i < 3; ++i) { /* dist/azimiths from control */
                        v[i] = vect(lp_lat - this->m_proj_parm.c[i].phi, this->m_proj_parm.c[i].cosphi, this->m_proj_parm.c[i].sinphi,
                            cosphi, sinphi, lp_lon - this->m_proj_parm.c[i].lam);
                        if ( ! v[i].r)
                            break;
                        v[i].Az = adjlon(v[i].Az - this->m_proj_parm.c[i].v.Az);
                    }
                    if (i < 3) /* current point at control point */
                        { xy_x = this->m_proj_parm.c[i].p.x; xy_y = this->m_proj_parm.c[i].p.y; }
                    else { /* point mean of intersepts */
                        { xy_x = this->m_proj_parm.p.x; xy_y = this->m_proj_parm.p.y; }
                        for (i = 0; i < 3; ++i) {
                            j = i == 2 ? 0 : i + 1;
                            a = lc(this->m_proj_parm.c[i].v.r, v[i].r, v[j].r);
                            if (v[i].Az < 0.)
                                a = -a;
                            if (! i) { /* coord comp unique to each arc */
                                xy_x += v[i].r * cos(a);
                                xy_y -= v[i].r * sin(a);
                            } else if (i == 1) {
                                a = this->m_proj_parm.beta_1 - a;
                                xy_x -= v[i].r * cos(a);
                                xy_y -= v[i].r * sin(a);
                            } else {
                                a = this->m_proj_parm.beta_2 - a;
                                xy_x += v[i].r * cos(a);
                                xy_y += v[i].r * sin(a);
                            }
                        }
                        xy_x *= THIRD; /* mean of arc intercepts */
                        xy_y *= THIRD;
                    }
                }

                static inline std::string get_name()
                {
                    return "chamb_spheroid";
                }

            };

            // Chamberlin Trimetric
            template <typename Parameters>
            void setup_chamb(Parameters& par, par_chamb& proj_parm)
            {
                int i, j;
                char line[10];

                for (i = 0; i < 3; ++i) { /* get control point locations */
                    (void)sprintf(line, "rlat_%d", i+1);
                    proj_parm.c[i].phi = pj_param(par.params, line).f;
                    (void)sprintf(line, "rlon_%d", i+1);
                    proj_parm.c[i].lam = pj_param(par.params, line).f;
                    proj_parm.c[i].lam = adjlon(proj_parm.c[i].lam - par.lam0);
                    proj_parm.c[i].cosphi = cos(proj_parm.c[i].phi);
                    proj_parm.c[i].sinphi = sin(proj_parm.c[i].phi);
                }
                for (i = 0; i < 3; ++i) { /* inter ctl pt. distances and azimuths */
                    j = i == 2 ? 0 : i + 1;
                    proj_parm.c[i].v = vect(proj_parm.c[j].phi - proj_parm.c[i].phi, proj_parm.c[i].cosphi, proj_parm.c[i].sinphi,
                        proj_parm.c[j].cosphi, proj_parm.c[j].sinphi, proj_parm.c[j].lam - proj_parm.c[i].lam);
                    if (! proj_parm.c[i].v.r) throw proj_exception(-25);
                    /* co-linearity problem ignored for now */
                }
                proj_parm.beta_0 = lc(proj_parm.c[0].v.r, proj_parm.c[2].v.r, proj_parm.c[1].v.r);
                proj_parm.beta_1 = lc(proj_parm.c[0].v.r, proj_parm.c[1].v.r, proj_parm.c[2].v.r);
                proj_parm.beta_2 = geometry::math::pi<double>() - proj_parm.beta_0;
                proj_parm.p.y = 2. * (proj_parm.c[0].p.y = proj_parm.c[1].p.y = proj_parm.c[2].v.r * sin(proj_parm.beta_0));
                proj_parm.c[2].p.y = 0.;
                proj_parm.c[0].p.x = - (proj_parm.c[1].p.x = 0.5 * proj_parm.c[0].v.r);
                proj_parm.p.x = proj_parm.c[2].p.x = proj_parm.c[0].p.x + proj_parm.c[2].v.r * cos(proj_parm.beta_0);
                par.es = 0.;
            }

        }} // namespace detail::chamb
    #endif // doxygen

    /*!
        \brief Chamberlin Trimetric projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Projection parameters
         - lat_1: Latitude of control point 1 (degrees)
         - lon_1: Longitude of control point 1 (degrees)
         - lat_2: Latitude of control point 2 (degrees)
         - lon_2: Longitude of control point 2 (degrees)
         - lat_3: Latitude of control point 3 (degrees)
         - lon_3: Longitude of control point 3 (degrees)
        \par Example
        \image html ex_chamb.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct chamb_spheroid : public detail::chamb::base_chamb_spheroid<Geographic, Cartesian, Parameters>
    {
        inline chamb_spheroid(const Parameters& par) : detail::chamb::base_chamb_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::chamb::setup_chamb(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class chamb_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_f<chamb_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void chamb_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("chamb", new chamb_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP


#ifndef DCM_PLANNER_H
#define DCM_PLANNER_H

#include <PnC/PlannerSet/DCMPlanner/Footstep.hpp>
#include <Utils/Math/minjerk_vec.hpp>
#include <Utils/Math/hermite_quaternion_curve.hpp>
#include <Utils/IO/IOUtilities.hpp>
#include <Eigen/Dense>
#include <cmath>
#include <string>
#include <iostream>
#include <map>

class DCMPlanner{
public:
  DCMPlanner();
  ~DCMPlanner();

  static int const DCM_RL_SWING_VRP_TYPE;
  static int const DCM_LL_SWING_VRP_TYPE;
  static int const DCM_MIDSTEP_TRANSFER_VRP_TYPE;
  static int const DCM_TRANSFER_VRP_TYPE;
  static int const DCM_END_VRP_TYPE;

  std::vector<int> rvrp_type_list; // List of type of virtual repelant point

  std::vector<Footstep> footstep_list; // Footstep list to generate pattern

  std::vector<Eigen::Vector3d> rvrp_list; // List of virtual repelant points.
  std::vector<Eigen::Vector3d> dcm_ini_list; // List of initial DCM states
  std::vector<Eigen::Vector3d> dcm_eos_list; // List of end-of-step DCM states

  // Initial DCM states
  Eigen::Vector3d ini_dcm_pos;
  Eigen::Vector3d ini_dcm_vel;

  // map containing the rvrp index that have a corresponding footstep swing
  std::map<int, int> rvrp_index_to_footstep_index;
  // Vector containing hermite quaternion curve objects for the pelvis orientation.
  // one curve per RVRP.
  std::vector<HermiteQuaternionCurve> pelvis_ori_quat_curves;

  // Initial and Boundary Conditions for the continuous DS trajectory
  std::vector<Eigen::Vector3d> dcm_ini_DS_list;
  std::vector<Eigen::Vector3d> dcm_vel_ini_DS_list;
  std::vector<Eigen::Vector3d> dcm_acc_ini_DS_list;
  std::vector<Eigen::Vector3d> dcm_end_DS_list;
  std::vector<Eigen::Vector3d> dcm_vel_end_DS_list;
  std::vector<Eigen::Vector3d> dcm_acc_end_DS_list;
  std::vector<Eigen::MatrixXd> dcm_P; // polynomial matrices for polynomial interpolation
  std::vector<MinJerkCurveVec> dcm_minjerk; // minjerk curves for interpolation

  // DCM walking parameters
  double t_transfer = 0.1; //0.125 ; //0.1; // exponential interpolation transfer time during initial transfer or same step transfer
  double t_ds = 0.05; // double support polynomial transfer time
  double t_ss = 0.3; // single support exponential interpolation  time
  double percentage_settle = 0.99;//0.999; // percent to converge at the end of the trajectory
  double alpha_ds = 0.5; // value between 0.0 and 1.0 for double support DCM interpolation

  void paramInitialization(const YAML::Node& node);
  void setRobotMass(double mass);
  void setCoMHeight(double z_vrp_in); // Sets the desired CoM Height
  void setInitialTime(double t_start_in); // Sets the initial offset time.
  void setInitialOri(const Eigen::Quaterniond initial_ori_in);

  double getInitialTime(); // Returns t_start;

  // DCM trajectory calculation
  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        initial_footstance  - a footstep object describing the stance leg.
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & initial_footstance, bool clear_list=false);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        initial_footstance  - a footstep object describing the stance leg.
  //        initial_rvrp        - an initial virtual repelant point (eg: average of the stance feet's rvrp).
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & initial_footstance, const Eigen::Vector3d & initial_rvrp);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & left_footstance, const Footstep & right_footstance);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  //        initial_com            - the initial location of the com
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & left_footstance, const Footstep & right_footstance, const Eigen::Vector3d & initial_com);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  //        initial_dcm            - the initial starting position of the dcm
  //        initial_dcm_vel        - the initial starting velocity of the dcm
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list,
                                 const Footstep & left_footstance, const Footstep & right_footstance,
                                 const Eigen::Vector3d & initial_dcm, const Eigen::Vector3d & initial_dcm_vel);

  // Compute: DCM, DCM vel, CoM, CoM Vel, given time, t.
  // t is a global time.
  void get_ref_dcm(const double t, Eigen::Vector3d & dcm_out);
  void get_ref_dcm_vel(const double t, Eigen::Vector3d & dcm_vel_out);
  void get_ref_com(const double t, Eigen::Vector3d & com_out);
  void get_ref_com_vel(const double t, Eigen::Vector3d & com_vel_out);
  void get_ref_r_vrp(const double t, Eigen::Vector3d & r_vrvp_out);
  void get_ref_reaction_force(const double t, Eigen::Vector3d & f_out);

  // Global reference quat, ang vel and ang acc
  void get_ref_ori_ang_vel_acc(const double t, Eigen::Quaterniond & quat_out,
                                               Eigen::Vector3d & ang_vel_out,
                                               Eigen::Vector3d & ang_acc_out);

  // computes the CoM velocity given the current CoM position and DCM velocity state.
  void get_com_vel(const Eigen::Vector3d & com_pos, const Eigen::Vector3d & dcm, Eigen::Vector3d & com_vel_out);
  // computes the CoM reaction force / leg reaction force given mass, CoM position and the r_vrp
  void get_reaction_force(const double mass, const Eigen::Vector3d & com_pos, const Eigen::Vector3d & r_vrp, Eigen::Vector3d fr_out);
  // computes the current r_vrp given
  //   -the DCM dynamics time constant b,
  //  - the current dcm
  //  - and the current dcm_vel
  void get_r_vrp(const double b_in, const Eigen::Vector3d & dcm, const Eigen::Vector3d & dcm_vel, Eigen::Vector3d & r_vrp_out);

  // prints the boundary conditions of the DCM
  void printBoundaryConditions();

  // Helper Functions
  // Returns the exponential step index the current time falls in.
  int which_step_index(const double t);
  // Returns the polynomial step index to use given the input time from t_start.
  int which_step_index_to_use(const double t);

  // returns the starting and ending time of the step_index from t_start.
  double get_t_step_start(const int step_index);
  double get_t_step_end(const int step_index);

  // returns the double support starting and ending time of the step_index from t_start.
  double get_double_support_t_start(const int step_index);
  double get_double_support_t_end(const int step_index);

  // if the step_index is a swing type, returns true an populates the swing start time and end
  bool get_t_swing_start_end(const int step_index, double & swing_start_time, double & swing_end_time);

  // returns the polynomial duration for the given step index
  double get_polynomial_duration(const int step_index);

  // Get the initial and end of double support transition times
  double get_eoDS_transition_time();
  double get_iniDS_transition_time();

  // Returns the type of VRP. Clamps the index to valid values.
  int get_r_vrp_type(const int step_index);

  // Returns t_end
  double get_total_trajectory_time();

  // Returns t_settle;
  double get_settle_time(){
    double t_settle = -b*log(1.0 - percentage_settle);
    return t_settle;
  }

private:
  // DCM parameters:
  double robot_mass = 50; // kg
  double gravity = 9.81;
  double z_vrp = 0.75; // desired VRP height / CoM height
  double b = std::sqrt(z_vrp/gravity); // time constant of DCM dynamics

  double t_start = 0.0; // the starting time for the DCM Walking reference

   // Outputs the average r_vrp location given two footstances
  void get_average_rvrp(const Footstep & footstance_1, const Footstep & footstance_2, Eigen::Vector3d & average_rvrp);

    // computes all the dcm states. Computation properly populates the dcm_ini_list and dcm_eos_list
  void computeDCM_states();

  // Sums up the total trajectory time and stores the result in t_end
  void compute_total_trajectory_time();
  double t_end = 0.0;

  // computes the reference com trajectories by integration
  void compute_reference_com();
  double dt_local = 1e-3; // discretization factor used
  // containers for the integrated reference CoM position and velocites
  std::vector<Eigen::Vector3d> ref_com_pos;
  std::vector<Eigen::Vector3d> ref_com_vel;

  // computes the reference pelvis orientation.
  void compute_reference_pelvis_ori();
  Footstep initial_leftfoot_stance;
  Footstep initial_rightfoot_stance;
  Eigen::Quaterniond initial_ori;

  // input: r_vrp_d_i - the desired virtual repelant point for the i-th step.
  //        t_step    - the time interval to use for backwards integration
  //        dcm_eos_i - the DCM state at the end of the i-th step.
  // computes the step i's initial DCM state and the end-of-step i-1's dcm state.
  // The computation is stored in the dcm_ini_list and dcm_eos_list.
  Eigen::Vector3d computeDCM_ini_i(const Eigen::Vector3d & r_vrp_d_i, const double & t_step, const Eigen::Vector3d & dcm_eos_i);

  // Computes the double support DCM boundary conditions for continuous double support trajectories
  // step_index the boundary condition for the step index
  // t_DS_{ini, end} the interpolation duration away from a Virtual repelant point.
  Eigen::Vector3d computeDCM_iniDS_i(const int & step_index, const double t_DS_ini);
  Eigen::Vector3d computeDCM_eoDS_i(const int & step_index, const double t_DS_end);
  Eigen::Vector3d computeDCMvel_iniDS_i(const int & step_index, const double t_DS_ini);
  Eigen::Vector3d computeDCMvel_eoDS_i(const int & step_index, const double t_DS_end);
  Eigen::Vector3d computeDCMacc_iniDS_i(const int & step_index, const double t_DS_ini);
  Eigen::Vector3d computeDCMacc_eoDS_i(const int & step_index, const double t_DS_end);

  // Returns the DCM exponential interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_exp(const int & step_index, const double & t);

  // Returns the DCM velocity exponential interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_vel_exp(const int & step_index, const double & t);

  // Returns the DCM acceleration exponential interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_acc_exp(const int & step_index, const double & t);

  // Returns the DCM double support polynomial interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_poly(const int & step_index, const double & t);

  // Returns the DCM double support velocity polynomial interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_vel_poly(const int & step_index, const double & t);

  // Returns the DCM double support min jerk interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_minjerk(const int & step_index, const double & t);

  // Returns the DCM double support velocity min jerk interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_vel_minjerk(const int & step_index, const double & t);

  // Get the t_step for step i.
  double get_t_step(const int & step_i);

  // returns matrix P \in R^{4x3}
  // P = mat_coeff * [dcm_ini^T; \dot{dcm}_ini^T; dcm_end^T; \dot{dcm}_end]
  // where mat_coeff \in R^{4x4} and ^T denotes a vector transpose transpose.
  // Ts is the duration in seconds
  Eigen::MatrixXd polynomialMatrix(const double Ts,
                                   const Eigen::Vector3d & dcm_ini, const Eigen::Vector3d & dcm_vel_ini,
                                   const Eigen::Vector3d & dcm_end, const Eigen::Vector3d & dcm_vel_end);

  int clampINT(int input, int low_bound, int upper_bound);
  double clampDOUBLE(double input, double low_bound, double upper_bound);

};

#endif