/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers
 Copyright (C) 2015 Roland Lichters

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/models/betaeta.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/time/schedule.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/integrals/segmentintegral.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

BetaEta::BetaEta(const Handle<YieldTermStructure> &termStructure,
                 const std::vector<Date> &volstepdates,
                 const std::vector<Real> &volatilities, const Real reversion,
                 const Real beta, const Real eta)
    : TermStructureConsistentModel(termStructure), CalibratedModel(4),
      reversion_(arguments_[0]), sigma_(arguments_[1]), pBeta_(arguments_[2]),
      pEta_(arguments_[3]), volstepdates_(volstepdates) {
    QL_REQUIRE(!termStructure.empty(),
               "no yield term structure given (empty handle)");
    // integrator and fall back
    integrator_ = boost::make_shared<GaussLobattoIntegral>(100000, 1E-8, 1E-8);
    integrator2_ = boost::make_shared<SegmentIntegral>(1000);
    volatilities_.resize(volatilities.size());
    for (Size i = 0; i < volatilities.size(); ++i)
        volatilities_[i] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(volatilities[i]));
    reversions_.resize(1);
    reversions_[0] = Handle<Quote>(boost::make_shared<SimpleQuote>(reversion));
    beta_ = Handle<Quote>(boost::make_shared<SimpleQuote>(beta));
    eta_ = Handle<Quote>(boost::make_shared<SimpleQuote>(eta));
    initialize();
}

BetaEta::BetaEta(const Handle<YieldTermStructure> &termStructure,
                 const std::vector<Date> &volstepdates,
                 const std::vector<Real> &volatilities,
                 const std::vector<Real> &reversions, const Real beta,
                 const Real eta)
    : TermStructureConsistentModel(termStructure), CalibratedModel(4),
      reversion_(arguments_[0]), sigma_(arguments_[1]), pBeta_(arguments_[2]),
      pEta_(arguments_[3]), volstepdates_(volstepdates) {
    QL_REQUIRE(!termStructure.empty(),
               "no yield term structure given (empty handle)");
    volatilities_.resize(volatilities.size());
    for (Size i = 0; i < volatilities.size(); ++i)
        volatilities_[i] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(volatilities[i]));
    reversions_.resize(reversions.size());
    for (Size i = 0; i < reversions_.size(); ++i)
        reversions_[i] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(reversions[i]));
    beta_ = Handle<Quote>(boost::make_shared<SimpleQuote>(beta));
    eta_ = Handle<Quote>(boost::make_shared<SimpleQuote>(eta));
    initialize();
}

BetaEta::BetaEta(const Handle<YieldTermStructure> &termStructure,
                 const std::vector<Date> &volstepdates,
                 const std::vector<Handle<Quote> > &volatilities,
                 const Handle<Quote> reversion, const Handle<Quote> beta,
                 const Handle<Quote> eta)
    : TermStructureConsistentModel(termStructure), CalibratedModel(4),
      reversion_(arguments_[0]), sigma_(arguments_[1]), pBeta_(arguments_[2]),
      pEta_(arguments_[3]), volatilities_(volatilities),
      reversions_(std::vector<Handle<Quote> >(1, reversion)),
      volstepdates_(volstepdates) {

    QL_REQUIRE(!termStructure.empty(),
               "no yield term structure given (empty handle)");
    initialize();
}

BetaEta::BetaEta(const Handle<YieldTermStructure> &termStructure,
                 const std::vector<Date> &volstepdates,
                 const std::vector<Handle<Quote> > &volatilities,
                 const std::vector<Handle<Quote> > &reversions,
                 const Handle<Quote> beta, const Handle<Quote> eta)
    : TermStructureConsistentModel(termStructure), CalibratedModel(4),
      reversion_(arguments_[0]), sigma_(arguments_[1]), pBeta_(arguments_[2]),
      pEta_(arguments_[3]), volatilities_(volatilities),
      reversions_(reversions), volstepdates_(volstepdates) {

    QL_REQUIRE(!termStructure.empty(),
               "no yield term structure given (empty handle)");
    initialize();
}

void BetaEta::updateTimes() const {
    volsteptimes_.clear();
    int j = 0;
    for (std::vector<Date>::const_iterator i = volstepdates_.begin();
         i != volstepdates_.end(); ++i, ++j) {
        volsteptimes_.push_back(termStructure()->timeFromReference(*i));
        volsteptimesArray_[j] = volsteptimes_[j];
        if (j == 0)
            QL_REQUIRE(volsteptimes_[0] > 0.0, "volsteptimes must be positive ("
                                                   << volsteptimes_[0] << ")");
        else
            QL_REQUIRE(volsteptimes_[j] > volsteptimes_[j - 1],
                       "volsteptimes must be strictly increasing ("
                           << volsteptimes_[j - 1] << "@" << (j - 1) << ", "
                           << volsteptimes_[j] << "@" << j << ")");
    }
}

void BetaEta::updateReversion() {
    for (Size i = 0; i < reversion_.size(); i++) {
        reversion_.setParam(i, reversions_[i]->value());
    }
    update();
}

void BetaEta::updateVolatility() {
    for (Size i = 0; i < sigma_.size(); i++) {
        sigma_.setParam(i, volatilities_[i]->value());
    }
    update();
}

void BetaEta::updateBeta() {
    pBeta_.setParam(0, beta_->value());
    betaLink_ = beta_->value();
    update();
}

void BetaEta::updateEta() {
    pEta_.setParam(0, eta_->value());
    etaLink_ = eta_->value();
    update();
}

void BetaEta::initialize() {
    useTabulation(true);
    volsteptimesArray_ = Array(volstepdates_.size());
    updateTimes();
    QL_REQUIRE(volatilities_.size() == volsteptimes_.size() + 1,
               "there must be n+1 volatilities ("
                   << volatilities_.size() << ") for n volatility step times ("
                   << volsteptimes_.size() << ")");
    sigma_ = PiecewiseConstantParameter(volsteptimes_, NoConstraint());

    QL_REQUIRE(reversions_.size() == 1 ||
                   reversions_.size() == volsteptimes_.size() + 1,
               "there must be 1 or n+1 reversions ("
                   << reversions_.size() << ") for n volatility step times ("
                   << volsteptimes_.size() << ")");
    if (reversions_.size() == 1) {
        reversion_ = ConstantParameter(reversions_[0]->value(), NoConstraint());
    } else {
        reversion_ = PiecewiseConstantParameter(volsteptimes_, NoConstraint());
    }

    pBeta_ = ConstantParameter(beta_->value(), NoConstraint());
    pEta_ = ConstantParameter(eta_->value(), NoConstraint());
    betaLink_ = beta_->value();
    etaLink_ = eta_->value();

    for (Size i = 0; i < sigma_.size(); i++) {
        sigma_.setParam(i, volatilities_[i]->value());
    }
    for (Size i = 0; i < reversion_.size(); i++) {
        reversion_.setParam(i, reversions_[i]->value());
    }

    registerWith(termStructure());

    volatilityObserver_ = boost::make_shared<VolatilityObserver>(this);
    reversionObserver_ = boost::make_shared<ReversionObserver>(this);
    betaObserver_ = boost::make_shared<BetaObserver>(this);
    etaObserver_ = boost::make_shared<EtaObserver>(this);

    for (Size i = 0; i < reversions_.size(); ++i)
        reversionObserver_->registerWith(reversions_[i]);
    for (Size i = 0; i < volatilities_.size(); ++i)
        volatilityObserver_->registerWith(volatilities_[i]);

    betaObserver_->registerWith(beta_);
    etaObserver_->registerWith(eta_);

    core_ = boost::make_shared<BetaEtaCore>(volsteptimesArray_, sigma_.params(),
                                            reversion_.params(), betaLink_,
                                            etaLink_);
}

const Real BetaEta::numeraire(const Time t, const Real x,
                              const Handle<YieldTermStructure> &yts) const {
    Real d =
        yts.empty() ? this->termStructure()->discount(t) : yts->discount(t);
    Real result =
        std::exp(core_->lambda(t) * x + core_->M(0, 0, t, useTabulation_)) / d;
    return result;
}

const Real BetaEta::zerobond(const Time T, const Time t, const Real x,
                             const Handle<YieldTermStructure> &yts) const {
    Real d = yts.empty()
                 ? this->termStructure()->discount(T) /
                       this->termStructure()->discount(t)
                 : yts->discount(T) / yts->discount(t);

    Real result = d * std::exp(-(core_->lambda(T) - core_->lambda(t)) * x -
                               (core_->M(0, 0, T, useTabulation_) -
                                core_->M(0, 0, t, useTabulation_)) +
                               core_->M(t, x, T, useTabulation_));
    return result;
}

const Real BetaEta::forwardRate(const Date &fixing, const Date &referenceDate,
                                const Real x,
                                boost::shared_ptr<IborIndex> iborIdx) const {

    QL_REQUIRE(iborIdx != NULL, "no ibor index given");

    calculate();

    if (fixing <= (evaluationDate_ + (enforcesTodaysHistoricFixings_ ? 0 : -1)))
        return iborIdx->fixing(fixing);

    Handle<YieldTermStructure> yts =
        iborIdx->forwardingTermStructure(); // might be empty, then use
                                            // model curve

    Date valueDate = iborIdx->valueDate(fixing);
    Date endDate = iborIdx->fixingCalendar().advance(
        valueDate, iborIdx->tenor(), iborIdx->businessDayConvention(),
        iborIdx->endOfMonth());
    // FIXME Here we should use the calculation date calendar ?
    Real dcf = iborIdx->dayCounter().yearFraction(valueDate, endDate);

    return (zerobond(valueDate, referenceDate, x, yts) -
            zerobond(endDate, referenceDate, x, yts)) /
           (dcf * zerobond(endDate, referenceDate, x, yts));
}

const Real BetaEta::swapRate(const Date &fixing, const Period &tenor,
                             const Date &referenceDate, const Real x,
                             boost::shared_ptr<SwapIndex> swapIdx) const {

    QL_REQUIRE(swapIdx != NULL, "no swap index given");

    calculate();

    if (fixing <= (evaluationDate_ + (enforcesTodaysHistoricFixings_ ? 0 : -1)))
        return swapIdx->fixing(fixing);

    Handle<YieldTermStructure> ytsf =
        swapIdx->iborIndex()->forwardingTermStructure();
    Handle<YieldTermStructure> ytsd =
        swapIdx->discountingTermStructure(); // either might be empty, then
                                             // use model curve

    Schedule sched, floatSched;

    boost::shared_ptr<VanillaSwap> underlying =
        underlyingSwap(swapIdx, fixing, tenor);

    sched = underlying->fixedSchedule();

    boost::shared_ptr<OvernightIndexedSwapIndex> oisIdx =
        boost::dynamic_pointer_cast<OvernightIndexedSwapIndex>(swapIdx);
    if (oisIdx != NULL) {
        floatSched = sched;
    } else {
        floatSched = underlying->floatingSchedule();
    }

    // should be fine for overnightindexed swap indices as well
    Real annuity = swapAnnuity(fixing, tenor, referenceDate, x, swapIdx);
    Rate floatleg = 0.0;
    if (ytsf.empty() && ytsd.empty()) { // simple 100-formula can be used
                                        // only in one curve setup
        floatleg =
            (zerobond(sched.dates().front(), referenceDate, x,
                      Handle<YieldTermStructure>()) -
             zerobond(sched.calendar().adjust(sched.dates().back(),
                                              underlying->paymentConvention()),
                      referenceDate, x, Handle<YieldTermStructure>()));
    } else {
        for (Size i = 1; i < floatSched.size(); i++) {
            floatleg +=
                (zerobond(floatSched[i - 1], referenceDate, x, ytsf) /
                     zerobond(floatSched[i], referenceDate, x, ytsf) -
                 1.0) *
                zerobond(floatSched.calendar().adjust(
                             floatSched[i], underlying->paymentConvention()),
                         referenceDate, x, ytsd);
        }
    }
    return floatleg / annuity;
}

const Real BetaEta::swapAnnuity(const Date &fixing, const Period &tenor,
                                const Date &referenceDate, const Real x,
                                boost::shared_ptr<SwapIndex> swapIdx) const {

    QL_REQUIRE(swapIdx != NULL, "no swap index given");

    calculate();

    Handle<YieldTermStructure> ytsd =
        swapIdx->discountingTermStructure(); // might be empty, then use
                                             // model curve

    boost::shared_ptr<VanillaSwap> underlying =
        underlyingSwap(swapIdx, fixing, tenor);

    Schedule sched = underlying->fixedSchedule();

    Real annuity = 0.0;
    for (unsigned int j = 1; j < sched.size(); j++) {
        annuity += zerobond(sched.calendar().adjust(
                                sched.date(j), underlying->paymentConvention()),
                            referenceDate, x, ytsd) *
                   swapIdx->dayCounter().yearFraction(sched.date(j - 1),
                                                      sched.date(j));
    }
    return annuity;
}

const Disposable<Array> BetaEta::xGrid(const Real stdDevs, const int gridPoints,
                                       const Real T, const Real t,
                                       const Real x) const {

    Array result(2 * gridPoints + 1, 0.0);

    // approximate standard deviation for x
    Real s = std::sqrt(core_->tau(t, T));

    Real h = stdDevs * s / (static_cast<Real>(gridPoints));

    // ensure that only grid points greater or equal the barrier are generated
    // do this by scaling the points left from x linearly
    std::vector<Real> hx, hy;
    Real leftX = x - h * static_cast<Real>(gridPoints);
    hx.push_back(leftX);
    hx.push_back(x);
    hy.push_back(std::max(leftX, -1.0 / beta_->value()));
    hy.push_back(x);

    LinearInterpolation l(hx.begin(), hx.end(), hy.begin());

    for (int j = -gridPoints; j <= gridPoints; ++j) {
        Real tmp = x + h * (static_cast<Real>(j));
        result[j + gridPoints] =
            (j < 0 && gridPoints > 0 && h > 0.0) ? l(tmp) : tmp;
    }

    return result;
}

} // namespace QuantLib


#include <boost/concept/assert.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/circulator.h>
#include <CGAL/Circulator/Circulator_concepts.h>

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_3 Point_3;
typedef CGAL::Surface_mesh<Point_3> SM;
typedef SM::Halfedge_around_target_circulator Halfedge_around_source_circulator;
typedef SM::Halfedge_around_target_circulator Halfedge_around_target_circulator;
typedef SM::Halfedge_around_target_circulator Vertex_around_target_circulator;
typedef SM::Halfedge_around_target_circulator Face_around_target_circulator;
typedef SM::Halfedge_around_target_circulator Halfedge_around_face_circulator;
typedef SM::Halfedge_around_target_circulator Vertex_around_face_circulator;
typedef SM::Halfedge_around_target_circulator Face_around_face_circulator;

template <typename Circ>
void test()
{
  BOOST_CONCEPT_ASSERT((CGAL::Concepts::BidirectionalCirculator<Circ>));
  Circ circ;
  if(circ){}
  if(circ == NULL){}
}

int main()
{
  test<Halfedge_around_source_circulator>();
  test<Halfedge_around_target_circulator>();
  test<Vertex_around_target_circulator>();
  test<Face_around_target_circulator>();

  test<Halfedge_around_face_circulator>();
  test<Vertex_around_face_circulator>();
  test<Face_around_face_circulator>();

  return 0;
}


/// \file   nuslam.cpp
/// \brief  A package for Extended Kalman Filter Slam implementation.

#include "nuslam/nuslam.hpp"
#include "rigid2d/diff_drive.hpp"
#include "rigid2d/rigid2d.hpp"

#include <armadillo>
#include <cmath>
#include <iostream>
#include <sstream>
#include <utility>
#include <vector>

namespace nuslam
{
  /********** Measurement struct member functions **********/

  /// Create zero-measurement
  Measurement::Measurement()
  {
    r = 0.0;
    phi = 0.0;
  }

  /// Create a measurement with r,phi inputs
  Measurement::Measurement(double x_, double y_, int id_)
  {
    r = std::sqrt(pow(x_, 2) + pow(y_, 2));
    phi = rigid2d::normalize_angle(atan2(y_, x_));
    id = id_;
  }

  /// Create a measurement vector
  arma::mat Measurement::compute_z()
  {
    arma::mat z = arma::mat(2, 1);
    z(0, 0) = r;
    z(1, 0) = rigid2d::normalize_angle(phi);

    return z;
  }

  /********** EKF class member functions **********/

  /// Initialize the combined state vector.
  /// Start with a guess for the robot state (0, 0, 0) and zero e map state.
  EKF::EKF()
  {
    q_t.fill(0.0);

    xi_predict.fill(0.0);
    cov_predict.fill(0.0);

    Q_mat.fill(0.0);
    Q_mat(0, 0) = 1e-3;
    Q_mat(1, 1) = 1e-3;
    Q_mat(2, 2) = 1e-3;

    R_mat.fill(0.0);
    R_mat(0, 0) = 1e-2;
    R_mat(1, 1) = 1e-2;
  }

  /// Check if the measured landmark exists in the landmark dictionary.
  bool EKF::check_landmarks(const int landmark_id)
  {
    if (id2landmark.count(landmark_id) == 1)
    {
      return false;
    }
    return true;
  }

  /// Update the landmarks matrix and landmark covariance.
  void EKF::add_new_measurement(const Measurement &meas)
  {
    if (check_landmarks(meas.id))
    {
      update_landmark(meas);
    }
  }

  /// Update the landmarks matrix and landmark covariance.
  void EKF::update_landmark(const Measurement &meas)
  {

    // Update id2landmark dictionary with new measurement
    id2landmark.insert(std::make_pair(meas.id, xi_predict.n_rows - 3));

    // Update covariance matrix
    cov_predict = update_matrix_size(cov_predict);
    cov_predict(cov_predict.n_rows - 1, cov_predict.n_cols - 1) =
        10000.0; // Infinity
    cov_predict(cov_predict.n_rows - 2, cov_predict.n_cols - 2) =
        10000.0; // Infinity

    // Update process noise for the robot motion model Q (expanding to fill the
    // whole state)
    Q_mat = update_matrix_size(Q_mat);

    // Update map state matrix
    double m_x =
        xi_predict(1, 0) +
        meas.r * cos(rigid2d::normalize_angle(meas.phi + xi_predict(0, 0)));
    double m_y =
        xi_predict(2, 0) +
        meas.r * sin(rigid2d::normalize_angle(meas.phi + xi_predict(0, 0)));

    arma::mat m_addition = arma::mat(2, 1);
    m_addition(0, 0) = m_x;
    m_addition(1, 0) = m_y;

    xi_predict = std::move(arma::join_cols(xi_predict, m_addition));
    // m_t = std::move(arma::join_cols(m_t, m_addition));
  }

  /// Get the state transition ξ_t (q_t, m_t) and the map’s movement with respect
  /// to the state ξ.
  arma::mat EKF::get_new_state(const Twist2D &twist)
  {
    arma::mat T_wbp = arma::mat(3, 1);
    arma::mat w_t = (arma::mat(3, 1)).fill(0.0);

    // If the rotational velocity is zero (twist.thetadot = 0)
    if (rigid2d::almost_equal(twist.thetadot, 0.0, 1.0e-6))
    {
      T_wbp(0, 0) = 0.0;
      T_wbp(1, 0) = twist.xdot * cos(xi_predict(0, 0));
      T_wbp(2, 0) = twist.xdot * sin(xi_predict(0, 0));
    }

    // If the rotational velocity is not zero (twist.thetadot != 0)
    else
    {
      double dx_dtheta = twist.xdot / twist.thetadot;
      T_wbp(0, 0) = twist.thetadot;
      T_wbp(1, 0) = -dx_dtheta * sin(xi_predict(0, 0)) +
                    dx_dtheta * sin(xi_predict(0, 0) + twist.thetadot);
      T_wbp(2, 0) = dx_dtheta * cos(xi_predict(0, 0)) -
                    dx_dtheta * cos(xi_predict(0, 0) + twist.thetadot);
    }

    // Return the current state
    xi_predict(0, 0) =
        rigid2d::normalize_angle(xi_predict(0, 0) + T_wbp(0, 0) + w_t(0, 0));
    xi_predict(1, 0) = xi_predict(1, 0) + T_wbp(1, 0) + w_t(1, 0);
    xi_predict(2, 0) = xi_predict(2, 0) + T_wbp(2, 0) + w_t(2, 0);

    // arma::mat q_t_new = q_t + T_wbp + w_t;
    // q_t_new(0, 0) = rigid2d::normalize_angle(q_t_new(0, 0));
    return xi_predict;
  }

  /// Get derivative of g with respect to the state ξ.
  arma::mat EKF::get_transition(const Twist2D &twist)
  {
    arma::mat g = (arma::mat(xi_predict.n_rows, xi_predict.n_rows)).fill(0.0);
    arma::mat A_t = (arma::mat(xi_predict.n_rows, xi_predict.n_rows)).fill(0.0);
    arma::mat I = eye(size(g));

    // If the rotational velocity is zero (twist.thetadot = 0)
    if (rigid2d::almost_equal(twist.thetadot, 0.0, 1.0e-6))
    {
      g(0, 0) = 0.0;
      g(1, 0) = -twist.xdot * sin(xi_predict(0, 0));
      g(2, 0) = twist.xdot * cos(xi_predict(0, 0));
    }

    // If the rotational velocity is not zero (twist.thetadot != 0)
    else
    {
      double dx_dtheta = twist.xdot / twist.thetadot;
      g(0, 0) = 0.0;
      g(1, 0) = -dx_dtheta * cos(xi_predict(0, 0)) +
                dx_dtheta * cos(xi_predict(0, 0) + twist.thetadot);
      g(2, 0) = -dx_dtheta * sin(xi_predict(0, 0)) +
                dx_dtheta * sin(xi_predict(0, 0) + twist.thetadot);
    }

    // Return the current state
    return (I + g);
  }

  /// Predict the next step - finds the estimated state and covariance.
  void EKF::predict(const Twist2D &twist)
  {
    xi_predict = get_new_state(twist);
    arma::mat A_t = get_transition(twist);
    cov_predict = A_t * cov_predict * A_t.t() + Q_mat;
  }

  /// Compute the measurement h for range and bearing to landmark.
  arma::mat EKF::get_h(int index)
  {
    arma::mat h = arma::mat(2, 1);
    h(0, 0) = std::sqrt(pow(xi_predict(index + 3, 0) - xi_predict(1, 0), 2) +
                        pow(xi_predict(index + 4, 0) - xi_predict(2, 0), 2));
    h(1, 0) = rigid2d::normalize_angle(
        atan2(xi_predict(index + 4, 0) - xi_predict(2, 0),
              xi_predict(index + 3, 0) - xi_predict(1, 0)) -
        xi_predict(0, 0));

    return h;
  }

  /// Compute the derivative of h with respect to the state.
  arma::mat EKF::get_H(int index)
  {
    int i = index + 3;
    double del_x = xi_predict(i, 0) - xi_predict(1, 0);
    double del_y = xi_predict(i + 1, 0) - xi_predict(2, 0);
    double d = pow(del_x, 2) + pow(del_y, 2);

    arma::mat H = arma::mat(2, cov_predict.n_rows).fill(0.0);
    H(0, 1) = -del_x / sqrt(d);
    H(0, 2) = -del_y / sqrt(d);
    H(1, 0) = -1;
    H(1, 1) = del_y / d;
    H(1, 2) = -del_x / d;

    H(0, i) = del_x / sqrt(d);
    H(0, i + 1) = del_y / sqrt(d);
    H(1, i) = -del_y / d;
    H(1, i + 1) = del_x / d;

    return H;
  }

  /// Update the next step.
  void EKF::update(std::vector<Measurement> meas)
  {
    arma::mat I = eye(size(cov_predict));
    arma::mat z = arma::mat(2, 1);
    arma::mat z_del = arma::mat(2, 1);

    for (auto &m : meas)
    {
      int m_row = id2landmark.find(m.id)->second;

      // Compute the theoretical measurement, given the current state estimate
      auto z_theory = get_h(m_row);

      // Compute the Kalman gain from the linearized measurement model
      auto H_i = get_H(m_row);
      arma::mat H_i_t = H_i.t();
      arma::mat inv_mat = arma::inv(H_i * cov_predict * H_i_t + R_mat);
      arma::mat K = cov_predict * H_i_t * inv_mat;

      // Compute the posterior state update
      // arma::mat z = m.compute_z();
      z(0, 0) = m.r;
      z(1, 0) = rigid2d::normalize_angle(m.phi);
      // z_del = z - z_theory;
      z_del = z - z_theory;
      z_del(1, 0) = rigid2d::normalize_angle(z_del(1, 0));

      xi_predict = xi_predict + K * z_del;
      xi_predict(0, 0) = rigid2d::normalize_angle(xi_predict(0, 0));

      // Compute the posterior covariance
      cov_predict = (I - K * H_i) * cov_predict;
    }
  }

  /// Add a new zero row and column to a matrix (used when adding new landmark).
  arma::mat EKF::update_matrix_size(arma::mat mat)
  {
    arma::mat mat_addition_col = (arma::mat(mat.n_rows, 2)).fill(0.0);
    arma::mat mat_addition_row = (arma::mat(2, mat.n_cols + 2)).fill(0.0);

    arma::mat mat_update = std::move(arma::join_rows(mat, mat_addition_col));
    mat_update = std::move(arma::join_cols(mat_update, mat_addition_row));

    return mat_update;
  }

  /// Run the Extended Kalman Filter algorithm.
  void EKF::run_ekf(const Twist2D &twist, const std::vector<Measurement> &meas)
  {
    // For every measurement
    for (auto &m : meas)
    {
      add_new_measurement(m);
    }

    // Predict and update
    predict(twist);
    update(meas);

    // Update the combined state vector and the covariance
    // xi = xi_predict;
    // cov = cov_predict;

    // Update robot state and map state
    q_t = xi_predict.submat(0, 0, 2, 0);
    q_t(0, 0) = rigid2d::normalize_angle(q_t(0, 0));
    m_t = xi_predict.submat(3, 0, xi_predict.n_rows - 1, 0);
  }

  /// Output the robot state
  arma::mat EKF::output_state()
  {
    q_t(0, 0) = rigid2d::normalize_angle(q_t(0, 0));
    return q_t;
  }

  /// Output the map state
  arma::mat EKF::output_map_state() { return m_t; }
} // namespace nuslam


//Header for this file
#include "programframe.h"
//C System-Headers
//
//C++ System headers
//
//Boost Headers
#include <boost/lexical_cast.hpp>//lexical cast
//Project Specific Headers
#include "../../ModeCharacterization/modecharacterization.h"

namespace etig {

ProgramFrame::ProgramFrame(QObject *parent) : ProgramCore( parent ) {

}

void ProgramFrame::Prequel() {
    PrequelReflection();
    mxg_n5183b->SetPower( signal_generator_power_dBm );

    emit OutputToNA();
}

void ProgramFrame::SetBackground() {

    MoveToBGSubtractionLength();

    std::vector< double > background = hp8757_c->TakeDataMultiple();
    data_list blank_data = power_to_data_list( background, na_min_freq, na_max_freq );

    mode_tracker.SetBackground( blank_data );

    MoveToStartLength();

}
template < typename T >
uint GetRebinSize( T f_res, T time_int ) {
    T delta_f = static_cast<T>( 1 )/( time_int );
    T f_axion = f_res*1e6 / ( static_cast<T>( 1e6 ) * static_cast<T>( 2 ) );

    return static_cast< uint >( std::floor( f_axion / delta_f ) );
}

void ProgramFrame::EstablishBinSize() {

    MoveToEndLength();

    auto network_analyzer_scan = hp8757_c->TakeDataMultiple();

    emit UpdateNA( network_analyzer_scan, nwa_span_MHz*4.0 );

    auto formatted_na_scan = power_to_data_list( network_analyzer_scan, na_min_freq, na_max_freq );

    double peak_position = 0;

    for( uint i = 0; i < 10; i ++ ) {

        try {
            peak_position = FindMinimaPeak( formatted_na_scan );
            break;
        } catch (const mode_track_failure& e) {
            Jitter();
        }

    }

    MoveToStartLength();

    rebin_size = GetRebinSize<double>( peak_position, 1.0 );
}

double ProgramFrame::FindMinimaPeak(data_list formatted_points ) {
    return mode_tracker.GetPeaksBiLat( formatted_points, 1 );
}

std::string ProgramFrame::BuildHeader() {

    std::string header = "";

    header += "length_of_tune;" + boost::lexical_cast<std::string>( length_of_tune ) + "\n";
    header += "revs_per_iterations;" + boost::lexical_cast<std::string>( revs_per_iterations ) + "\n";
    header += "start_length;" + boost::lexical_cast<std::string>( start_length ) + "\n";
    header += "nwa_span_MHz;" + boost::lexical_cast<std::string>( nwa_span_MHz ) + "\n";
    header += "nwa_points;" + boost::lexical_cast<std::string>( nwa_points ) + "\n";
    header += "nwa_power_dBm;" + boost::lexical_cast<std::string>( nwa_power_dBm ) + "\n";
    header += "freq_window_MHz;" + boost::lexical_cast<std::string>( freq_window_MHz ) + "\n";
    header += "digitizer_rate_MHz;" + boost::lexical_cast<std::string>( digitizer_rate_MHz ) + "\n";

    header += "num_averages;" + boost::lexical_cast<std::string>( num_averages ) + "\n";
    header += "Q;" + boost::lexical_cast<std::string>( quality_factor ) + "\n";
    header += "hwhm;" + boost::lexical_cast<std::string>( hwhm ) + "\n";
    header += "center_frequency;" + boost::lexical_cast<std::string>( center_frequency ) + "\n";

    double current_length = arduino->GetCavityLength();
    header += "cavity_length;" + boost::lexical_cast<std::string>( current_length ) + "\n";
    header += "rebin_size;" + boost::lexical_cast<std::string>( rebin_size ) + "\n";

    return header;
}

double ProgramFrame::DeriveCavityLength() {

    return start_length + ( iteration*length_of_tune /number_of_iterations );
}

double ProgramFrame::RecenterPeak( std::vector< data_triple<double> > to_check ) {

    try {
        return mode_tracker.GetMaxPeak( to_check );
    } catch (const daq_failure& e) {
        throw daq_failure( "Could not find a local maxima in provided data set." );
    }

}

void ProgramFrame::ShiftFrequencyWindow( double center_frequency ) {

    double nyquist_frequency = digitizer_rate_MHz/2.0;
    double shift_frequency = center_frequency - nyquist_frequency/2.0;

    mxg_n5183b->SetFrequency( shift_frequency );

    emit LOFrequency( shift_frequency );
}

double ProgramFrame::CheckPeak( double possible_mode_position ) {

    if( possible_mode_position <= 0.0 ) {
        throw daq_failure( "Mode position to less than or equal to 0 MHz" );
    }

    // Since we identified the position of our mode using reflection measurements
    // we need to switch to transmission to find the 'real' position of the mode
    xdl_35_5tp->SwitchToTransmission();
    emit ToTransmission();

    hp8757_c->SetFrequencyWindow( possible_mode_position, nwa_span_MHz );

    std::vector< double > initial_window = hp8757_c->TakeDataSingle();

    emit UpdateNA( initial_window, nwa_span_MHz );
//    na_view->UpdateSignal( initial_window, nwa_span_MHz );

    double min_freq_initial = possible_mode_position - nwa_span_MHz/2.0;
    double max_freq_initial = possible_mode_position + nwa_span_MHz/2.0;

    double new_mode_position = 0;

    try {
        new_mode_position = RecenterPeak( power_to_data_list( initial_window, min_freq_initial, max_freq_initial ) );
    } catch (const daq_failure& e) {
        throw daq_failure( "Could not find a local maxima in provided data set." );
    }

    hp8757_c->SetFrequencyWindow( new_mode_position, nwa_span_MHz );

    std::vector< double > final_window = hp8757_c->TakeDataSingle();

    double min_freq_final = new_mode_position - nwa_span_MHz/2;
    double max_freq_final = new_mode_position + nwa_span_MHz/2;

    data_list mode_window = power_to_data_list( final_window, min_freq_final, max_freq_final );
    ModeTraits mode_parameters( mode_window );

    center_frequency = mode_parameters.f0();
    quality_factor = mode_parameters.Q();
    hwhm = center_frequency/quality_factor;

//    na_view->UpdateSignal( final_window, single_scan_window );

    emit UpdateNA( final_window, nwa_span_MHz );

    //Return to reflection measurements
    xdl_35_5tp->SwitchToReflection();
    emit ToReflection();

    return new_mode_position;

}

template < typename T >
std::vector< T > ProgramFrame::data_list_to_power( std::vector< data_triple <T> > data ) {

    std::vector< T > power_list;
    power_list.reserve( data.size() );

    for( const auto& triple : data ) {
        power_list.push_back( triple.power_dBm );
    }

    return power_list;
}

std::vector< data_triple<double> > ProgramFrame::power_to_data_list ( std::vector< float > power_list,
        float min_freq,
        float max_freq ) {

    uint number_of_points = power_list.size();

    double cavity_length = arduino->GetCavityLength();

    std::vector< data_triple< double > > processed;
    processed.reserve( number_of_points );
    double num_points_f = static_cast<double>( number_of_points );

    for( uint i = 0; i < number_of_points ; i++ ) {

        double i_f = static_cast<double>( i );

        double min_freq_d = static_cast< double >( min_freq );
        double max_freq_d = static_cast< double >( max_freq );
        double frequency = ( i_f/num_points_f )*( max_freq_d - min_freq_d ) + min_freq_d;
        double power = static_cast<double>( power_list.at(i) );

        processed.push_back( data_triple< double >( cavity_length, frequency, power ) );

    }

    return processed;

}

std::vector< data_triple<double> > ProgramFrame::power_to_data_list ( std::vector< double > power_list,
        double min_freq,
        double max_freq ) {

    uint number_of_points = power_list.size();
//    double max_freq = center_frequency + nwa_span_MHz / 2.0;
//    double min_freq = center_frequency - nwa_span_MHz / 2.0;

    double cavity_length = arduino->GetCavityLength();

    std::vector< data_triple<double> > processed;
    processed.reserve( number_of_points );

    for( uint i = 0; i < number_of_points ; i++ ) {

        double i_f = static_cast<double>(i);
        double num_points_f = static_cast<double>(number_of_points);

        double frequency = ( i_f/num_points_f )*( max_freq - min_freq ) + min_freq;
        double power = power_list.at(i);

        processed.push_back( data_triple< double >( cavity_length, frequency, power ) );

    }

    return processed;

}

}


//#include "physics\cutting\virtual_scalpel.h"
//
//#include <Eigen/Geometry>
//
//namespace sbs {
//namespace physics {
//namespace cutting {
//
//virtual_scalpel_h::virtual_scalpel_h(
//    common::line_segment_t const& l,
//    common::shared_vertex_surface_mesh_t const& render_model,
//    double rotation_speed,
//    double eps)
//    : l1_(l),
//      l2_(l),
//      swept_surface_(),
//      previous_translation_{0., 0., 0.},
//      translation_{0., 0., 0.},
//      rotation_speed_(rotation_speed),
//      previous_pitch_angle_(0.),
//      pitch_angle_(0.),
//      previous_yaw_angle_(0.),
//      yaw_angle_(0.),
//      pitch_axis_(),
//      yaw_axis_(),
//      eps_(eps),
//      render_model_(render_model),
//      last_valid_swept_surface_(),
//      state_(state_t::in_transition)
//{
//}
//
//double virtual_scalpel_h::rotation_speed() const
//{
//    return rotation_speed_;
//}
//
//void virtual_scalpel_h::set_rotation_speed(double speed)
//{
//    rotation_speed_ = speed;
//}
//
//void virtual_scalpel_h::rotate(double dx, double dy)
//{
//    yaw_angle_   = rotation_speed_ * dx;
//    pitch_angle_ = rotation_speed_ * dy;
//
//    Eigen::AngleAxisd const yaw(yaw_angle_, yaw_axis_);
//    Eigen::AngleAxisd const pitch(pitch_angle_, pitch_axis_);
//    Eigen::Matrix3d const rotation = pitch.toRotationMatrix() * yaw.toRotationMatrix();
//
//    // update render model
//    Eigen::Vector3d geometric_mean = /*render_model_.vertices().rowwise().mean()*/ l2_.p;
//    render_model_.vertices().colwise() -= geometric_mean;
//    render_model_.vertices() = rotation * render_model_.vertices();
//    render_model_.vertices().colwise() += geometric_mean;
//
//    // update physical model
//    geometric_mean = l2_.p;
//    l2_.q          = rotation * (l2_.q - geometric_mean) + geometric_mean;
//
//    double constexpr minimum_edge_length = 1e-10;
//    bool const surface_can_be_swept      = (l2_.p - l1_.p).norm() > minimum_edge_length ||
//                                      (l2_.q - l1_.q).norm() > minimum_edge_length;
//
//    if (surface_can_be_swept && state_ == state_t::in_transition)
//    {
//        state_ = state_t::sweeping;
//    }
//}
//
//void virtual_scalpel_h::set_translation(Eigen::Vector3d const& t)
//{
//    Eigen::Vector3d const displacement = t - translation_;
//    translation_                       = t;
//
//    // update render model
//    render_model_.vertices().colwise() += displacement;
//    // update physical model
//    l2_.p += displacement;
//    l2_.q += displacement;
//
//    if (can_surface_be_swept() && state_ == state_t::in_transition)
//    {
//        state_ = state_t::sweeping;
//    }
//}
//
//void virtual_scalpel_h::set_pitch_axis(Eigen::Vector3d const& pitch_axis)
//{
//    pitch_axis_ = pitch_axis;
//}
//
//void virtual_scalpel_h::set_yaw_axis(Eigen::Vector3d const& yaw_axis)
//{
//    yaw_axis_ = yaw_axis;
//}
//
//void virtual_scalpel_h::step()
//{
//    if (state_ == state_t::in_transition)
//        return;
//
//    compute_swept_surface();
//
//    l1_ = l2_;
//
//    state_ = state_t::in_transition;
//}
//
//common::shared_vertex_surface_mesh_t virtual_scalpel_h::get_scalpel_render_model() const
//{
//    common::shared_vertex_surface_mesh_t render_model{render_model_};
//    render_model.set_color({0.f, 0.f, 0.f});
//    Eigen::Vector3f const red{1.f, 0.f, 0.f};
//    Eigen::Vector3f const blue{0.f, 0.f, 1.f};
//    render_model.colors().col(0) = red;
//    render_model.colors().col(1) = red;
//    render_model.colors().col(2) = blue;
//    render_model.colors().col(3) = blue;
//
//    return render_model;
//}
//
//common::shared_vertex_surface_mesh_t const& virtual_scalpel_h::get_render_swept_surface() const
//{
//    return last_valid_swept_surface_;
//}
//
//common::shared_vertex_surface_mesh_t const& virtual_scalpel_h::get_swept_surface() const
//{
//    return swept_surface_;
//}
//
//common::line_segment_t const& virtual_scalpel_h::get_cutting_edge() const
//{
//    return l2_;
//}
//
//bool virtual_scalpel_h::can_surface_be_swept() const
//{
//    double constexpr minimum_edge_length = 1e-10;
//    bool const surface_can_be_swept      = (l2_.p - l1_.p).norm() > minimum_edge_length ||
//                                      (l2_.q - l1_.q).norm() > minimum_edge_length;
//    return surface_can_be_swept;
//}
//
//void virtual_scalpel_h::compute_swept_surface()
//{
//    auto const& p1 = l1_.p;
//    auto const& p2 = l1_.q;
//    auto const& p3 = l2_.p;
//    auto const& p4 = l2_.q;
//
//    swept_surface_.vertices().resize(3, 4);
//    swept_surface_.normals().resize(3, 4);
//    swept_surface_.triangles().resize(3, 2);
//
//    swept_surface_.vertices().col(0) = p1;
//    swept_surface_.vertices().col(1) = p2;
//    swept_surface_.vertices().col(2) = p3;
//    swept_surface_.vertices().col(3) = p4;
//
//    swept_surface_.triangles().col(0) =
//        common::shared_vertex_surface_mesh_t::triangle_type{0, 1, 2};
//    swept_surface_.triangles().col(1) =
//        common::shared_vertex_surface_mesh_t::triangle_type{2, 1, 3};
//
//    common::normal_t const n1 = (p2 - p1).cross(p3 - p1).normalized();
//    common::normal_t const n2 = (p1 - p2).cross(p3 - p2).normalized();
//
//    swept_surface_.normals().col(0) = n1;
//    swept_surface_.normals().col(1) = ((n1 + n2) / 2.).normalized();
//    swept_surface_.normals().col(2) = ((n1 + n2) / 2.).normalized();
//    swept_surface_.normals().col(3) = n2;
//
//    Eigen::Vector3f const red{1.f, 0.f, 0.f};
//    Eigen::Vector3f const blue{0.f, 0.f, 1.f};
//    swept_surface_.set_color(red);
//    swept_surface_.colors().col(2) = blue;
//    swept_surface_.colors().col(3) = blue;
//
//    last_valid_swept_surface_ = swept_surface_;
//}
//
//} // namespace cutting
//} // namespace physics
//} // namespace sbs


#pragma once

#include <memory>

// ros
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Dense>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/sensors/rgb_camera.hpp"

// Noisy params 1 - medium lookahead
/*
#define NOISE_STD_ 10.0
#define MAX_DEPTH_IN_PCL_ 7.5
*/
// Noisy params 2 - short lookahead
#define NOISE_STD_ 10.0
#define MAX_DEPTH_IN_PCL_ 6.9

// Noise-free params
/*
#define NOISE_STD_ 0.0
#define MAX_DEPTH_IN_PCL_ 20
*/

using namespace flightlib;

namespace flightros {

class DepthConverter {
 public:
  DepthConverter(const ros::NodeHandle& nh,
                 const ros::NodeHandle& pnh);
  ~DepthConverter();
  void depthCallback(const sensor_msgs::ImageConstPtr& depth_img);
  Eigen::Matrix3f K;
  Eigen::Matrix3f invK;

 private:
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;

  ros::Subscriber sub_depth;
  ros::Publisher cloud_pub;
  image_transport::Publisher noisy_pub;
};
}


// Volumetric3D_cylinder.cpp
// created by Kuangdai on 19-Oct-2016
// a cylinder-shaped heterogeneity

#include "Volumetric3D_cylinder.h"
#include "Parameters.h"
#include "Geodesy.h"
#include <boost/algorithm/string.hpp>
#include <sstream>

void Volumetric3D_cylinder::initialize(const std::vector<std::string> &params) {
    // need at least 10 parameters to make a cylinder
    if (params.size() < 10) {
        throw std::runtime_error("Volumetric3D_cylinder::initialize || "
            "Not enough parameters for a cylinder-shaped heterogeneity. Need 10 at least.");
    }

    const std::string source = "Volumetric3D_cylinder::initialize";

    // property name
    bool found = false;
    for (int i = 0; i < Volumetric3D::MaterialPropertyString.size(); i++) {
        if (boost::iequals(params[0], Volumetric3D::MaterialPropertyString[i])) {
            mMaterialProp = Volumetric3D::MaterialProperty(i);
            found = true;
            break;
        }
    }
    if (!found) {
        throw std::runtime_error("Volumetric3D_cylinder::initialize || "
            "Unknown material property, name = " + params[0]);
    }

    // reference type
    found = false;
    for (int i = 0; i < Volumetric3D::MaterialRefTypeString.size(); i++) {
        if (boost::iequals(params[1], Volumetric3D::MaterialRefTypeString[i]) ||
            boost::iequals(params[1], Volumetric3D::MaterialRefTypeStringShort[i])) {
            mReferenceType = Volumetric3D::MaterialRefType(i);
            found = true;
            break;
        }
    }
    if (!found) {
        throw std::runtime_error("Volumetric3D_cylinder::initialize || "
            "Unknown material reference type, type = " + params[1]);
    }

    // value inside
    Parameters::castValue(mValueInside, params[2], source);

    // radius
    Parameters::castValue(mRadius, params[3], source); mRadius *= 1e3;

    // location
    Parameters::castValue(mD1, params[4], source); mD1 *= 1e3;
    Parameters::castValue(mLat1, params[5], source);
    Parameters::castValue(mLon1, params[6], source);

    Parameters::castValue(mD2, params[7], source); mD2 *= 1e3;
    Parameters::castValue(mLat2, params[8], source);
    Parameters::castValue(mLon2, params[9], source);

    // optional
    try {
        int ipar = 10;
        Parameters::castValue(mSourceCentered, params.at(ipar++), source);
        Parameters::castValue(mFluid, params.at(ipar++), source);
        Parameters::castValue(mHWHM_lateral, params.at(ipar++), source); mHWHM_lateral *= 1e3;
        Parameters::castValue(mHWHM_top_bot, params.at(ipar++), source); mHWHM_top_bot *= 1e3;
    } catch (std::out_of_range) {
        // nothing
    }

    // compute xyz of endpoints and length
    RDCol3 rtpPoint1, rtpPoint2;
    if (mSourceCentered) {
        RDCol3 rtpPoint1Src, rtpPoint2Src;
        rtpPoint1Src(0) = Geodesy::getROuter() - mD1;
        rtpPoint1Src(1) = mLat1 * degree;
        rtpPoint1Src(2) = mLon1 * degree;
        rtpPoint1 = Geodesy::rotateSrc2Glob(rtpPoint1Src, mSrcLat, mSrcLon, mSrcDep);
        rtpPoint2Src(0) = Geodesy::getROuter() - mD2;
        rtpPoint2Src(1) = mLat2 * degree;
        rtpPoint2Src(2) = mLon2 * degree;
        rtpPoint2 = Geodesy::rotateSrc2Glob(rtpPoint2Src, mSrcLat, mSrcLon, mSrcDep);
    } else {
        rtpPoint1(0) = Geodesy::getROuter() - mD1;
        rtpPoint1(1) = Geodesy::lat2Theta_d(mLat1, mD1);
        rtpPoint1(2) = Geodesy::lon2Phi(mLon1);
        rtpPoint2(0) = Geodesy::getROuter() - mD2;
        rtpPoint2(1) = Geodesy::lat2Theta_d(mLat2, mD2);
        rtpPoint2(2) = Geodesy::lon2Phi(mLon2);
    }
    mXyzPoint1 = Geodesy::toCartesian(rtpPoint1);
    mXyzPoint2 = Geodesy::toCartesian(rtpPoint2);
    mLength = (mXyzPoint1 - mXyzPoint2).norm();

    // use 20% of radius for lateral HWHM if not specified
    if (mHWHM_lateral < 0.) {
        mHWHM_lateral = mRadius * .2;
    }

    // use 10% of cylinder length for top-bot HWHM if not specified
    if (mHWHM_top_bot < 0.) {
        mHWHM_top_bot = mLength * .1;
    }

    // for Absolute models
    if (mReferenceType == Volumetric3D::MaterialRefType::Absolute) {
        // decay is not allowed
        mHWHM_lateral = mHWHM_top_bot = 0.;
        // convert to SI
        mValueInside *= MaterialPropertyAbsSI[mMaterialProp];
    }
}

bool Volumetric3D_cylinder::get3dProperties(double r, double theta, double phi, double rElemCenter,
    std::vector<MaterialProperty> &properties,
    std::vector<MaterialRefType> &refTypes,
    std::vector<double> &values) const {

    // header
    properties = std::vector<MaterialProperty>(1, mMaterialProp);
    refTypes = std::vector<MaterialRefType>(1, mReferenceType);
    values = std::vector<double>(1, 0.);

    // distance from point to axis
    RDCol3 rtpTarget;
    rtpTarget(0) = r;
    rtpTarget(1) = theta;
    rtpTarget(2) = phi;
    const RDCol3 &xyzTarget = Geodesy::toCartesian(rtpTarget);
    const RDCol3 &xyzDiff1 = xyzTarget - mXyzPoint1;
    const RDCol3 &xyzDiff2 = xyzTarget - mXyzPoint2;
    double distToLine = xyzDiff1.cross(xyzDiff2).norm() / mLength;

    // outside range
    if (distToLine > mRadius + 4. * mHWHM_lateral) {
        return false;
    }

    // compute Gaussian lateral
    double distToSurf = distToLine - mRadius;
    if (distToSurf < 0.) {
        distToSurf = 0.;
    }
    double stddev = mHWHM_lateral / sqrt(2. * log(2.));
    double gaussian_lateral = mValueInside * exp(-distToSurf * distToSurf / (stddev * stddev * 2.));

    // distance from point to ends
    double d1 = xyzDiff1.norm();
    double d2 = xyzDiff2.norm();
    double dmax = std::max(d1, d2);
    double distToTopBot = sqrt(dmax * dmax - distToLine * distToLine) - mLength;

    // outside range
    if (distToTopBot > 4. * mHWHM_top_bot) {
        return false;
    }

    double gaussian_topbot = gaussian_lateral;
    if (distToTopBot > 0.) {
        // compute Gaussian top-bottom
        stddev = mHWHM_top_bot / sqrt(2. * log(2.));
        gaussian_topbot = gaussian_lateral * exp(-distToTopBot * distToTopBot / (stddev * stddev * 2.));
    }

    // set perturbations
    values[0] = gaussian_topbot;
    return true;
}

std::string Volumetric3D_cylinder::verbose() const {
    std::stringstream ss;
    ss << "\n======================= 3D Volumetric ======================" << std::endl;
    ss << "  Model Name               =   cylinder" << std::endl;
    ss << "  Material Property        =   " << MaterialPropertyString[mMaterialProp] << std::endl;
    ss << "  Reference Type           =   " << MaterialRefTypeString[mReferenceType] << std::endl;
    if (mReferenceType == Volumetric3D::MaterialRefType::Absolute) {
        ss << "  Value Inside             =   " << mValueInside / MaterialPropertyAbsSI[mMaterialProp] << std::endl;
    } else {
        ss << "  Value Inside             =   " << mValueInside << std::endl;
    }
    ss << "  Cylinder Radius / km     =   " << mRadius / 1e3 << std::endl;
    ss << "  Depth_1 / km             =   " << mD1 / 1e3 << std::endl;
    ss << "  Lat_1 or Theta_1 / deg   =   " << mLat1 << std::endl;
    ss << "  Lon_1 or Phi_1 / deg     =   " << mLon1 << std::endl;
    ss << "  Depth_2 / km             =   " << mD2 / 1e3 << std::endl;
    ss << "  Lat_2 or Theta_2 / deg   =   " << mLat2 << std::endl;
    ss << "  Lon_2 or Phi_2 / deg     =   " << mLon2 << std::endl;
    ss << "  Source-centered          =   " << (mSourceCentered ? "YES" : "NO") << std::endl;
    ss << "  HWHM lateral / km        =   " << mHWHM_lateral / 1e3 << std::endl;
    ss << "  HWHM top-bot / km        =   " << mHWHM_top_bot / 1e3 << std::endl;
    ss << "======================= 3D Volumetric ======================\n" << std::endl;
    return ss.str();
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>

#include "equirectangular_camera_colinearity_tait_bryan_wc_jacobian.h"
#include "equirectangular_camera_coplanarity_tait_bryan_wc_jacobian.h"
#include "equirectangular_camera_coplanarity_rodrigues_wc_jacobian.h"
#include "equirectangular_camera_coplanarity_quaternion_wc_jacobian.h"
#include "quaternion_constraint_jacobian.h"
#include "structures.h"
#include "transformations.h"
#include "cauchy.h"

struct KeyPoint{
	double u;
	double v;
	int index_to_tie_point;
};

struct Camera{
	Eigen::Affine3d pose;
	std::vector<KeyPoint> key_points;
};

std::vector<Eigen::Vector3d> tie_points;
std::vector<Camera> cameras;
int cols = 4096;
int rows = 2048;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = -215, rotate_y = 273;
float translate_z = -68.0;
float translate_x = 4, translate_y = 30.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	for(size_t i = 0 ; i < 100; i++){
		Eigen::Vector3d p;
		p.x() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 100.0;
		p.y() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 200.0;
		p.z() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 100.0;
		tie_points.push_back(p);
	}

	for(int i = -50 ; i < 50; i+=20){
		Camera c;
		c.pose = Eigen::Affine3d::Identity();
		c.pose(0,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		c.pose(1,3) = i;
		c.pose(2,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		cameras.push_back(c);
	}

	for(size_t i = 0 ; i < cameras.size(); i++){
		for(size_t j = 0; j < tie_points.size(); j++){
			KeyPoint kp;
			kp.index_to_tie_point = j;

			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
			equrectangular_camera_colinearity_tait_bryan_wc(kp.u, kp.v, rows, cols, M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, tie_points[j].x(), tie_points[j].y(), tie_points[j].z());

			cameras[i].key_points.push_back(kp);
		}
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("equirectangular_camera_coplanarity_ba");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glColor3f(1,0,0);
	glPointSize(5);
	glBegin(GL_POINTS);
	for(size_t i = 0 ; i < tie_points.size(); i++){
		glVertex3f(tie_points[i].x(), tie_points[i].y(), tie_points[i].z());
	}
	glEnd();

	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'c':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose pose;
				pose.om = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.fi = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.ka = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;

				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose);
				cameras[i].pose = cameras[i].pose * m;
			}
			break;
		}
		case 'p':{
			for(size_t i = 0; i < tie_points.size(); i++){
				tie_points[i].x() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
				tie_points[i].y() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
				tie_points[i].z() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points.size(); k++){
						if(i != j){
							TaitBryanPose pose_1 = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
							TaitBryanPose pose_2 = pose_tait_bryan_from_affine_matrix(cameras[j].pose);

							double px_1 = pose_1.px;
							double py_1 = pose_1.py;
							double pz_1 = pose_1.pz;
							double om_1 = pose_1.om;
							double fi_1 = pose_1.fi;
							double ka_1 = pose_1.ka;

							double px_2 = pose_2.px;
							double py_2 = pose_2.py;
							double pz_2 = pose_2.pz;
							double om_2 = pose_2.om;
							double fi_2 = pose_2.fi;
							double ka_2 = pose_2.ka;

							double u_1 = (cameras[i].key_points[k].u);
							double v_1 = (cameras[i].key_points[k].v);
							double u_2 = (cameras[j].key_points[k].u);
							double v_2 = (cameras[j].key_points[k].v);

							double delta;
							observation_equation_equirectangular_camera_coplanarity_tait_bryan_wc(delta, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, om_1, fi_1, ka_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, om_2, fi_2, ka_2);

							Eigen::Matrix<double, 1, 12, Eigen::RowMajor> jacobian;
							observation_equation_equirectangular_camera_coplanarity_tait_bryan_wc_jacobian(jacobian, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, om_1, fi_1, ka_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, om_2, fi_2, ka_2);

							int ir = tripletListB.size();
							int ic_1 = i * 3;
							int ic_2 = j * 3;

							tripletListA.emplace_back(ir     , ic_1 + 0, -jacobian(0,3));
							tripletListA.emplace_back(ir     , ic_1 + 1, -jacobian(0,4));
							tripletListA.emplace_back(ir     , ic_1 + 2, -jacobian(0,5));

							tripletListA.emplace_back(ir     , ic_2 + 0, -jacobian(0,9));
							tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,10));
							tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,11));

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta, 1));

							tripletListB.emplace_back(ir    , 0,  delta);
						}
					}
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 3);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 3, cameras.size() * 3);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 3, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 3){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points.size(); k++){
						if(i != j){
							RodriguesPose pose_1 = pose_rodrigues_from_affine_matrix(cameras[i].pose);
							RodriguesPose pose_2 = pose_rodrigues_from_affine_matrix(cameras[j].pose);

							double px_1 = pose_1.px;
							double py_1 = pose_1.py;
							double pz_1 = pose_1.pz;
							double sx_1 = pose_1.sx;
							double sy_1 = pose_1.sy;
							double sz_1 = pose_1.sz;

							double px_2 = pose_2.px;
							double py_2 = pose_2.py;
							double pz_2 = pose_2.pz;
							double sx_2 = pose_2.sx;
							double sy_2 = pose_2.sy;
							double sz_2 = pose_2.sz;

							double u_1 = (cameras[i].key_points[k].u);
							double v_1 = (cameras[i].key_points[k].v);
							double u_2 = (cameras[j].key_points[k].u);
							double v_2 = (cameras[j].key_points[k].v);

							double delta;
							observation_equation_equirectangular_camera_coplanarity_rodrigues_wc(delta, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, sx_1, sy_1, sz_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, sx_2, sy_2, sz_2);

							Eigen::Matrix<double, 1, 12, Eigen::RowMajor> jacobian;
							observation_equation_equirectangular_camera_coplanarity_rodrigues_wc_jacobian(jacobian, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, sx_1, sy_1, sz_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, sx_2, sy_2, sz_2);

							int ir = tripletListB.size();
							int ic_1 = i * 3;
							int ic_2 = j * 3;

							tripletListA.emplace_back(ir, ic_1 + 0, -jacobian(0,3));
							tripletListA.emplace_back(ir, ic_1 + 1, -jacobian(0,4));
							tripletListA.emplace_back(ir, ic_1 + 2, -jacobian(0,5));

							tripletListA.emplace_back(ir, ic_2 + 0, -jacobian(0,9));
							tripletListA.emplace_back(ir, ic_2 + 1, -jacobian(0,10));
							tripletListA.emplace_back(ir, ic_2 + 2, -jacobian(0,11));

							tripletListP.emplace_back(ir, ir,  cauchy(delta, 1));

							tripletListB.emplace_back(ir, 0, delta);
						}
					}
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 3);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 3, cameras.size() * 3);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 3, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 3){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_rodrigues(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points.size(); k++){
						if(i != j){
							QuaternionPose pose_1 = pose_quaternion_from_affine_matrix(cameras[i].pose);
							QuaternionPose pose_2 = pose_quaternion_from_affine_matrix(cameras[j].pose);

							double px_1 = pose_1.px;
							double py_1 = pose_1.py;
							double pz_1 = pose_1.pz;
							double q0_1 = pose_1.q0;
							double q1_1 = pose_1.q1;
							double q2_1 = pose_1.q2;
							double q3_1 = pose_1.q3;

							double px_2 = pose_2.px;
							double py_2 = pose_2.py;
							double pz_2 = pose_2.pz;
							double q0_2 = pose_2.q0;
							double q1_2 = pose_2.q1;
							double q2_2 = pose_2.q2;
							double q3_2 = pose_2.q3;

							double u_1 = (cameras[i].key_points[k].u);
							double v_1 = (cameras[i].key_points[k].v);
							double u_2 = (cameras[j].key_points[k].u);
							double v_2 = (cameras[j].key_points[k].v);

							double delta;
							observation_equation_equirectangular_camera_coplanarity_quaternion_wc(delta, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, q0_1, q1_1, q2_1, q3_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, q0_2, q1_2, q2_2, q3_2);

							Eigen::Matrix<double, 1, 14, Eigen::RowMajor> jacobian;
							observation_equation_equirectangular_camera_coplanarity_quaternion_wc_jacobian(jacobian, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, q0_1, q1_1, q2_1, q3_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, q0_2, q1_2, q2_2, q3_2);

							int ir = tripletListB.size();
							int ic_1 = i * 4;
							int ic_2 = j * 4;

							tripletListA.emplace_back(ir, ic_1 + 0, -jacobian(0,3));
							tripletListA.emplace_back(ir, ic_1 + 1, -jacobian(0,4));
							tripletListA.emplace_back(ir, ic_1 + 2, -jacobian(0,5));
							tripletListA.emplace_back(ir, ic_1 + 3, -jacobian(0,6));

							tripletListA.emplace_back(ir, ic_2 + 0, -jacobian(0,10));
							tripletListA.emplace_back(ir, ic_2 + 1, -jacobian(0,11));
							tripletListA.emplace_back(ir, ic_2 + 2, -jacobian(0,12));
							tripletListA.emplace_back(ir, ic_2 + 3, -jacobian(0,13));

							tripletListP.emplace_back(ir, ir, cauchy(delta, 1));

							tripletListB.emplace_back(ir, 0,  delta);
						}
					}
				}
			}

			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 4;
				int ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 0 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 1 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 2 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 4);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 4, cameras.size() * 4);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 4, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 4){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_quaternion(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodriguez)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
	std::cout << "c: add noise to cameras" << std::endl;
	std::cout << "p: add noise to tie points" << std::endl;
}


//============================================================================
// Name        : cFEM-Nonlinear.cpp
// Author      : Ted Kwan
// Version     :
// Copyright   : Your copyright notice
// Description : Nonlinear Finite Element Method in C++
//============================================================================

#include <iostream>
#include "Mesh.h"
#include "MeshMG.h"
#include <vector>
#include <armadillo>
#include "FiniteElemNL.h"

using namespace std;
using namespace arma;

/**
* This is a testing program for the nonlinear finite element method
* in CPP.
*/
int main() {
	vector<double> meshvec(5);
	meshvec[0]=0;
	meshvec[1]=1.0;
	meshvec[2]=0;
	meshvec[3]=1.0;
	meshvec[4]=0.25;
	//Mesh msh(meshvec,"blahdiddyblah");
	clock_t start;
	start=clock();
	FiniteElemNL femp(meshvec,"blah man yo",3);
	double duration=clock()-start;
	cout << "This took: " << (duration/(double)(CLOCKS_PER_SEC))<< endl;
	//vec u=femp.u;
	//u.print("u: ");
//	MeshMG mshmg(meshvec);
//	mshmg.uniformrefine();
	//vector<sp_mat> stiffmass=mshmg.assembleMatrix();
	//sp_mat stiffness=stiffmass[0];
	//	mass.set_size(stiffmass[1].n_rows,stiffmass[1].n_cols);
	//sp_mat mass=stiffmass[1];
//	sp_mat stiffness=mshmg.stiffs.back();
//	stiffness.print("Stiffness: ");
	//cout << "!!!Hello World!!!" << endl; // prints !!!Hello World!!!
	return 0;
}


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_BUFFERED_SMALL_CROSSOVER_EVENT_GENERATOR_HPP_
#define CLOTHO_BUFFERED_SMALL_CROSSOVER_EVENT_GENERATOR_HPP_

#include <boost/property_tree/ptree.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <cstring>
#include <iostream>

#include "clotho/recombination/recombination_rate_parameter.hpp"
#include "clotho/recombination/sequence_bias_parameter.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#ifdef DEBUGGING
#define ASSERT_VALID_RANGE( x, min, max ) assert( min <= x && x < max );
#else
#define ASSERT_VALID_RANGE( x, min, max )
#endif

namespace clotho {
namespace genetics {

template < class RNG, class PositionType >
class buffered_small_crossover_event_generator {
public:
    typedef RNG             random_engine_type;
    typedef PositionType    position_type;
    typedef PositionType    event_type;
    typedef unsigned int    bin_type;

    typedef typename position_distribution_helper< PositionType >::type position_distribution_type;

    typedef crossover_event_distribution_helper< double > event_distribution_helper_type;

    typedef boost::random::bernoulli_distribution< double > sequence_bias_distribution_type;

    typedef typename event_distribution_helper_type::IntType    IntType;
    typedef typename event_distribution_helper_type::type       event_distribution_type;

    typedef position_type *                         position_vector;
    typedef event_type *                            event_vector;
    typedef bin_type *                              bin_vector;

    static const unsigned int BUFFER_MAX = 64;

    buffered_small_crossover_event_generator( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rand( rng )
        , m_pos( NULL )
        , m_pos_size(0)
        , m_events_size(0)
        , m_base_seq(0)
    {
        recombination_rate_parameter< double > rho( config);

        m_event_dist.param( typename event_distribution_type::param_type( rho.m_rho ) );

        sequence_bias_parameter< double > bias( config );
        m_seq_bias.param( typename sequence_bias_distribution_type::param_type( bias.m_bias ) );
    }

    void update( position_vector pos, size_t N ) {
        m_pos = pos;
        m_pos_size = N;
    }

/**
 *
 * Copies a subsequence of the parent position vector into local memory
 *
 */
    void update_buffer( unsigned int offset ) {
        memcpy( m_pos_buffer, m_pos + offset, BUFFER_MAX * sizeof( position_type ) );
    }

/**
 *
 * Generates an ordered list of crossover events
 *
 * @return Number of events generated
 */
    size_t generate() {
        // clear the counts
        IntType N = m_event_dist( *m_rand );    // generate the maximum number of events
        resize( N );

        setBaseSequence( m_seq_bias( *m_rand ) );

        position_type accum = 0.0;

        size_t i = 0;
        position_type e = (position_type)(N);
        while( e > (position_type) 0 ) {
            position_type p = m_pos_dist( *m_rand );

            while( p == 0.0 ) { p = m_pos_dist( *m_rand ); }

            accum += log( p ) / e;

            p = (1.0 - exp( accum ));

            assert( i == 0 || m_events[ i - 1 ] < p);

            m_events[ i++ ] = p;
            e -= 1.0;
        }

        return N;
    }

/**
 *
 * test whether the genetic position at the given buffer relative index
 * is in a crossover region
 *
 */
    inline bool operator()( const size_t index ) const {
        assert( index < BUFFER_MAX );
#ifdef DEBUGGING
        if( index >= m_pos_size ) {
            std::cerr << "Crossover: " << index << " < " << m_pos_size << std::endl;
            assert(false);
        }
#endif

        position_type p = m_pos_buffer[ index ];
        return eval( p );
    }

    inline bool eval( position_type p ) const {

        bool res = false;
        if( m_events_size == 1 ) {
            res = ( m_events[0] < p );
        } else {
            size_t lo = 0;
            while( lo < m_events_size && m_events[lo] < p ){ ++lo; }
            res = ((lo & 1) ? true : false); // % 2
        }

        return res;
    }

    void setBaseSequence( bool is_base ) {
        m_base_seq = ((is_base) ? 0 : 1);
    }

    size_t getBaseSequence() const {
        return m_base_seq;
    }

    virtual ~buffered_small_crossover_event_generator() { }

protected:

    void resize( size_t e ) {
        assert( e < BUFFER_MAX );
        m_events_size = e;
    }

    random_engine_type          * m_rand;
    event_distribution_type     m_event_dist;
    position_distribution_type  m_pos_dist;

    position_type       m_pos_buffer[ BUFFER_MAX ];

    position_vector     m_pos;
    size_t              m_pos_size;

    event_type          m_events[ BUFFER_MAX ];
    size_t              m_events_size;

    sequence_bias_distribution_type m_seq_bias;
    size_t              m_base_seq;
};

#undef ASSERT_VALID_RANGE

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_BUFFERED_SMALL_CROSSOVER_EVENT_GENERATOR_HPP_


#include "Scheduler_node.hpp"

#include <Eigen/Dense>
#include <exception>
#include <iostream>
#include <map>
#include <node.h>
#include <pthread.h>
#include <queue>
#include <stdio.h>
#include <uv.h>
#include <v8.h>
#include <memory>
#include <fstream>

#include "../../Algorithms/ProximalGradientDescent.hpp"
#include "../../Algorithms/IterativeUpdate.hpp"
#include "../../Algorithms/Algorithm.hpp"
#include "../../Algorithms/AlgorithmOptions.hpp"
#include "../../Algorithms/HypoTestPlaceHolder.h"
#include "../../Models/ModelOptions.hpp"
#include "../Job.hpp"
#include "../Scheduler.hpp"
#include "../../JSON/JsonCoder.hpp"
#include "../../IO/FileIO.hpp"
#include "../../IO/PlinkReader.h"
#include "../../IO/MongoInterface.hpp"

using namespace Eigen;
using namespace std;
using namespace v8;

void imputation(const FunctionCallbackInfo<Value>& args) {
    bool result = false;
    Isolate* isolate = args.GetIsolate();
    if (ArgsHaveJobID(args,0)) {
        const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
        result = Scheduler::Instance().imputation(job_id);
    }
    args.GetReturnValue().Set(Boolean::New(isolate, result));
}

void setMetaData(const FunctionCallbackInfo<Value>& args) {
    bool result = false;
    Isolate* isolate = args.GetIsolate();
    if (ArgsHaveJobID(args,0)) {
        const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
        string filename(*v8::String::Utf8Value(args[1]->ToString()));
        string marker_ids_path(*v8::String::Utf8Value(args[2]->ToString()));
//		cout << "DEBUG-ywt: " << marker_ids_path << endl;

        /* Get the markerName file extension type */
        vector<string> marker_ids;
        string ext_name = marker_ids_path.substr(marker_ids_path.length() - 3, 3);
        if (ext_name.compare("csv") == 0) // csv file
        {
            ifstream in(marker_ids_path);
            std::string line;
            while (std::getline(in, line))
                marker_ids.push_back(line);
        }
        else if (ext_name.compare("bim") == 0 || ext_name.compare("bed") == 0 || ext_name.compare("fam") == 0) // Plink file
        {
            marker_ids_path.erase(marker_ids_path.length() - 4, 4); // cut the extension
            PlinkReader::getInstance().getXname(marker_ids_path, marker_ids);
        }
        result = Scheduler::Instance().setMetaData(job_id,filename,marker_ids);
    }
    args.GetReturnValue().Set(Boolean::New(isolate, result));
}

// Sets the X matrix of a given model from Plink file.
// Arguments: job_id, plink filepath
void setX(const FunctionCallbackInfo<Value>& args) {
	bool result = false;
	Isolate* isolate = args.GetIsolate();

	if (ArgsHaveJobID(args, 0)) {
		const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
		string path_name(*v8::String::Utf8Value(args[1]->ToString()));
		MatrixXf mat;

		/* Get the extension type */
		string ext_name = path_name.substr(path_name.length() - 3, 3);
		if (ext_name.compare("csv") == 0) // csv file
			mat = FileIO::getInstance().readMatrixFile(path_name);
		else if (ext_name.compare("bim") == 0 || ext_name.compare("bed") == 0 || ext_name.compare("fam") == 0) // Plink file
		{
			path_name.erase(path_name.length() - 4, 4); // cut the extension
			PlinkReader::getInstance().getX(path_name, mat);
		}
		result = Scheduler::Instance().setX(job_id, mat);
	}
	args.GetReturnValue().Set(Boolean::New(isolate, result));
}

// Sets the Y matrix of a given model from Plink file.
// Arguments: job_id, plink filepath
void setY(const FunctionCallbackInfo<Value>& args) {
	bool result = false;
	Isolate* isolate = args.GetIsolate();

	if (ArgsHaveJobID(args, 0)) {
		const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
		string path_name(*v8::String::Utf8Value(args[1]->ToString()));
		MatrixXf mat;

		/* Get the extension type */
		string ext_name = path_name.substr(path_name.length() - 3, 3);
		if (ext_name.compare("csv") == 0) // csv file
			mat = FileIO::getInstance().readMatrixFile(path_name);
		else if (ext_name.compare("bim") == 0 || ext_name.compare("bed") == 0 || ext_name.compare("fam") == 0) // Plink file
		{
			path_name.erase(path_name.length() - 4, 4); // cut the extension
			PlinkReader::getInstance().getY(path_name, mat);
		}

		result = Scheduler::Instance().setY(job_id, mat);
	}
	args.GetReturnValue().Set(Boolean::New(isolate, result));
}

// Arguments: job_id, string of attribute name, matrix to use
void setModelAttributeMatrix(const FunctionCallbackInfo<Value>& args) {
	bool result = false;
	Isolate* isolate = args.GetIsolate();
	if (ArgsHaveJobID(args, 0)) {
		const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
		const string attribute_name(*v8::String::Utf8Value(args[1]->ToString()));
		Local<v8::Array> ar = Local<v8::Array>::Cast(args[2]);
		MatrixXf* mat = v8toEigen(ar);
		result = Scheduler::Instance().setModelAttributeMatrix(job_id, attribute_name, mat);
	}
	args.GetReturnValue().Set(Boolean::New(isolate, result));
}

// Creates a new job, but does not run it. Synchronous.
// Arguments: JSON to be converted to JobOptions_t
void newJob(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();
	Handle<Object> options_v8 = Handle<Object>::Cast(args[0]);
	const JobOptions_t& options = JobOptions_t(isolate, options_v8);
	try {
		const job_id_t id = Scheduler::Instance().newJob(options);
		if (id == 0) {
			isolate->ThrowException(Exception::Error(
				String::NewFromUtf8(isolate, "Could not add another job")));
			return;
		}

		Local<Integer> retval = Integer::New(isolate, id);
		args.GetReturnValue().Set(retval);
	} catch (const exception& e) {
		isolate->ThrowException(Exception::Error(
			String::NewFromUtf8(isolate, e.what())));
		return;
	}
}

// Maybe begins the training of an algorithm, given the job number.
// Asynchronous.
// Arguments: job_id_t job_id, function callback
void startJob(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();
	// Inspect arguments.
	try {
		if (!ArgsHaveJobID(args, 0)) {
			args.GetReturnValue().Set(Boolean::New(isolate, false));
			return;
		}

		const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
		shared_ptr<Job_t> job = Scheduler::Instance().getJob(job_id);
		job->exception = nullptr;
		job->callback.Reset(isolate, Local<Function>::Cast(args[1]));
		job->job_id = job_id;
		bool result = Scheduler::Instance().startJob(job_id, trainAlgorithmComplete);
		usleep(1);	// let the execution thread start -- necessary?
		if (job->exception) {
			rethrow_exception(job->exception);
		}
		args.GetReturnValue().Set(Boolean::New(isolate, result));
	} catch (const exception& e) {
		isolate->ThrowException(Exception::Error(
			String::NewFromUtf8(isolate, e.what())));
		args.GetReturnValue().Set(Boolean::New(isolate, false));
	}
}

// Checks the status of an algorithm, given the algorithm's job number.
// Currently synchronous.
// Arguments: job_id_t job_id
void checkJob(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();

	// Check argument types.
	if (!ArgsHaveJobID(args, 0)) {
		args.GetReturnValue().Set(Boolean::New(isolate, false));
		return;
	}

	const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
	const float progress = Scheduler::Instance().checkJobProgress(job_id);
	Local<Number> retval = Number::New(isolate, progress);
	args.GetReturnValue().Set(retval);
}

// Gets the results of a job, given the job's id.
// Synchronous.
// Arguments: job_id_t job_id
// Returns: MatrixXf of results, empty on error.
void getJobResult(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();
	try {
		// Check argument types.
		if (!ArgsHaveJobID(args, 0)) {
			args.GetReturnValue().Set(Boolean::New(isolate, false));
			return;
		}

		const job_id_t job_id = (unsigned int)Local<Number>::Cast(args[0])->Value();
		const MatrixXf& result = Scheduler::Instance().getJobResult(job_id);
		Local<v8::Array> obj = v8::Array::New(isolate);
		obj->Set(0, v8::String::NewFromUtf8(isolate, JsonCoder::getInstance().encodeMatrix(result).c_str()));
		args.GetReturnValue().Set(obj);
	}
	catch (const exception& e) {
		isolate->ThrowException(Exception::Error(
			String::NewFromUtf8(isolate, e.what())));
	}
}

// Cancels a potentially running Algorithm.
// Synchronous.
// Arguments: job_id_t job_id
// Returns: boolean representing success.
void cancelJob(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();

	// Check argument types.
	if (!ArgsHaveJobID(args, 0)) {
		args.GetReturnValue().Set(Boolean::New(isolate, false));
		return;
	}
	try {
		const job_id_t job_id = (unsigned int)Local<Integer>::Cast(args[0])->Value();
		const bool success = Scheduler::Instance().cancelJob(job_id);
		Handle<Boolean> retval = Boolean::New(isolate, success);
		args.GetReturnValue().Set(retval);
	} catch(const exception& e) {
		isolate->ThrowException(Exception::Error(
			String::NewFromUtf8(isolate, e.what())));
	}
}

// Arguments: job_id
void deleteJob(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();

	if (!ArgsHaveJobID(args, 0)) {
		args.GetReturnValue().Set(Boolean::New(isolate, false));
		return;
	}

	const job_id_t job_id = (unsigned int)Local<Integer>::Cast(args[0])->Value();
	const bool success = Scheduler::Instance().deleteJob(job_id);
	Handle<Boolean> retval = Boolean::New(isolate, success);
	args.GetReturnValue().Set(retval);
}

/////////////////////////////////
// Helper Functions
/////////////////////////////////

// Handles packaging of algorithm results to return to the frontend.
// Called by libuv in event loop when async training completes.
void trainAlgorithmComplete(uv_work_t* req, int status) {
	// Runs in event loop when algorithm completes.
	Isolate* isolate = Isolate::GetCurrent();
	HandleScope handleScope(isolate);
	Job_t* job = static_cast<Job_t*>(req->data);
    Local<Boolean> retval;
	try {
		// Pack up the data to be returned to JS
		const MatrixXf& result = Scheduler::Instance().getJobResult(job->job_id);

        int success = MongoInterface::getInstance().storeResults(result,job->filename,job->marker_ids);
        retval = Boolean::New(isolate, success);
		if (status < 0) { // libuv error
			throw runtime_error("Libuv error (check server)");
		}
		if (job->exception) {
			rethrow_exception(job->exception);
		}
	} catch(const exception& e) {
		// If the job failed, the second entry in the array is the exception text.
		// Is this really a good way to return data? It is different than the result from getJobResult()
		std::cout << e.what() << std::endl;
	}

	Handle<Value> argv[] = { retval };
	// execute the callback
	Local<Function>::New(isolate, job->callback)->Call(
		isolate->GetCurrentContext()->Global(), 1, argv);
	job->callback.Reset();
}

MatrixXf* v8toEigen(Local<v8::Array>& ar) {
	const unsigned int rows = ar->Length();
	Local<v8::Array> props = Local<v8::Object>::Cast(ar->Get(0))->GetPropertyNames();
	const unsigned int cols = props->Length();
	Eigen::MatrixXf* mat = new Eigen::MatrixXf(rows, cols);

	for (unsigned int i=0; i<rows; i++) {
		for (unsigned int j=0; j<cols; j++) {
			(*mat)(i,j) = (float)Local<v8::Object>::Cast(ar->Get(i))->Get(props->Get(j))->NumberValue();
		}
	}
	return mat;
}

// Checks that the argument in the supplied position is a number type.
// Doesn't check that it is a valid job ID.
bool ArgsHaveJobID(const FunctionCallbackInfo<Value>& args, const int position) {
	Isolate* isolate = args.GetIsolate();
	if (args.Length() < 1) {
		isolate->ThrowException(Exception::TypeError(
			String::NewFromUtf8(isolate, "Must supply a job id.")));
		args.GetReturnValue().Set(Boolean::New(isolate, false));
		return false;
	}

	if (!(args[0]->IsNumber())) {
		isolate->ThrowException(Exception::TypeError(
			String::NewFromUtf8(isolate, "Job id must be a number.")));
		args.GetReturnValue().Set(Boolean::New(isolate, false));
		return false;
	}

	return true;
}

/* Deprecated - only interacting with jobs now
// Creates a new algorithm, but does not run it. Currently synchronous.
// Arguments: JSON to be converted to AlgorithmOptions_t
void newAlgorithm(const FunctionCallbackInfo<Value>& args) {
	Isolate* isolate = args.GetIsolate();

	if (args.Length() < 1) {
		isolate->ThrowException(Exception::TypeError(
			String::NewFromUtf8(isolate, "Wrong number of arguments")));
		return;
	}

	Handle<Object> options_v8 = Handle<Object>::Cast(args[0]);
	const AlgorithmOptions_t& options = AlgorithmOptions_t(isolate, options_v8);

	const int id = Scheduler::Instance().newAlgorithm(options);
	if (id < 0) {
		isolate->ThrowException(Exception::Error(
			String::NewFromUtf8(isolate, "Could not add another algorithm.")));
		return;
	}

	Local<Integer> retval = Integer::New(isolate, id);
	args.GetReturnValue().Set(retval);
}

// Creates a new model, but does not run it. Synchronous.
// Arguments: JSON to be converted to ModelOptions_t
void newModel(const FunctionCallbackInfo<Value>& args) {
	Isolate* isolate = args.GetIsolate();
	Handle<Object> options_v8 = Handle<Object>::Cast(args[0]);
	const ModelOptions_t& options = ModelOptions_t(isolate, options_v8);
	const int id = Scheduler::Instance().newModel(options);
	if (id < 0) {
		isolate->ThrowException(Exception::Error(
			String::NewFromUtf8(isolate, "Could not add another model")));
		return;
	}

	Local<Integer> retval = Integer::New(isolate, id);
	args.GetReturnValue().Set(retval);
}

// Arguments: int alg_id
// Returns: boolean for success
void deleteAlgorithm(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();
	const int algorithm_id = (int)Local<Integer>::Cast(args[0])->Value();
	const bool success = Scheduler::Instance().deleteAlgorithm(algorithm_id);
	Handle<Boolean> retval = Boolean::New(isolate, success);
	args.GetReturnValue().Set(retval);
}

// Arguments: int model_id
void deleteModel(const v8::FunctionCallbackInfo<v8::Value>& args) {
	Isolate* isolate = args.GetIsolate();
	const int model_id = (int)Local<Integer>::Cast(args[0])->Value();
	const bool success = Scheduler::Instance().deleteModel(model_id);
	Handle<Boolean> retval = Boolean::New(isolate, success);
	args.GetReturnValue().Set(retval);
}

*/


/*
Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

using namespace std;
using namespace Eigen;
using namespace KDL;

int main (int argc, char **argv)
{

	ros::init(argc, argv, "Grasp_quality");	// ROS node
	ros::NodeHandle nh;

	int n_c = 1; //number of contacts
	int n_q = 19; //number of joints
	int n_z = 1; // number of synergie
	int synergy = 1;

	double Kx = 1;
	double Ky = 1;
	double Kz = 1;

	double f_i_max = 20;
	double mu_friction = 1;
	double PCR;

	Eigen::VectorXd f(6*n_c); // contact force
	Eigen::VectorXd H_f(3*n_c); // contact force with type model of contact
	Eigen::VectorXd f_y(3*n_c); // controllable contact force

	Eigen::VectorXd w(6);

	Eigen::MatrixXd G(6, 6*n_c);
	Eigen::MatrixXd G_H_t(6,3*n_c); // Grasp Matrix
	Eigen::MatrixXd J(6*n_c, n_q);
	Eigen::MatrixXd H_J(3*n_c,n_q); // Hand Jacobian
	Eigen::MatrixXd G_r_k(3*n_c,6); // weighted right pseudoinverse of G

	Eigen::MatrixXd K(3*n_c,3*n_c); // grasp stiffness matrix
	Eigen::MatrixXd Kis(3,3);
	Eigen::MatrixXd Ks(3*n_c,3*n_c); // contact stiffness matrix
	Eigen::MatrixXd Kp(n_q,n_q); // joint stiffness matrix

	Eigen::MatrixXd S(n_q,n_z); // Synergie matriz for underactuacted hand

	Eigen::VectorXd d_f(3*n_c);

	// calculation Ks(Cj)
	int s = 0; // step for component
	for( int i = 0 ; i < n_c ; i++ )
	{
		Kis(0,0) = 0;
		Kis(1,1) = 0;
		Kis(2,2) = 0;

		if( H_f(i+s+3) >= 0 )
			if(  sqrt( H_f(i+s+0)*H_f(i+s+0) + H_f(i+s+1)*H_f(i+s+1)) <= mu_friction * H_f(i+s+2) )
			{
				Kis(0,0) =  Kx;
				Kis(1,1) =  Ky;
				Kis(2,2) =  Kz;
			}
			else
				Ks(2,2) = Kz;

		Ks.block<3,3>(s,s) = Kis;
		s += 3;
	}

	// calculation the grasp stiffness matrix K(Cj)
	Eigen::MatrixXd K_(3*n_c,3*n_c);
	K_ = Ks.inverse() + H_J * Kp.inverse() * H_J.transpose() ;
	K  = K_.inverse();

	// evaluation the constraint N(K(Cj)*Gt) = 0
	// it must be satisfied to immobilize the object
	Eigen::MatrixXd Kcj_Gt = K * G_H_t.transpose();
	FullPivLU<MatrixXd> lu_(Kcj_Gt);
	Eigen::MatrixXd Null_Kcj_Gt = lu_.kernel();

	bool Matrix_is_Zero = true;

	for (int i = 0 ; i < Null_Kcj_Gt.rows() ; i++ )
		for ( int j = 0 ; j < Null_Kcj_Gt.cols() ; j++)
			if( Null_Kcj_Gt(i,j) != 0 )
			{	Matrix_is_Zero = false; break; }

	if( !Matrix_is_Zero ) // condition-constrain of PGR is not satisfy
		return 0;

	Eigen::MatrixXd H_(3,6);

	H_ << 1, 0, 0, 0, 0, 0,
		  0, 1, 0, 0, 0, 0,
		  0, 0, 1, 0, 0, 0;

	Eigen::MatrixXd H(3*n_c,6*n_c);

	int i = 0;
	int j = 0;
	for(int n = 0 ;  n < n_c ; n++)
	{
		H.block<3,6>(i,j) = H_;
		i += 3;
		j += 6;
	}

	//define the sinergie matrix
	for(int j = 0 ; j < n_q ; j++)
		for(int i = 0; i < n_z ; i++)
			S(j,i) = 1;

	// apply the kind of the point id contact model
	G_H_t = G * H.transpose();
	H_J = H * J ;
	H_f = H * f ;

	// calculatin of external wrench
	w = - G_H_t * H_f;

	//calculation weighted right pseudoinverse
	Eigen::MatrixXd G_inv;
	Eigen::MatrixXd G_;
	G_ = G_H_t * K * G_H_t.transpose();
	G_inv = G_.inverse();
	G_r_k = K * G.transpose() * G_inv;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////////////////

	//calculation of a basis for the subspace of the controllable internal forces
	// with null of GHt
	FullPivLU<MatrixXd> lu(G_H_t);
	Eigen::MatrixXd E_ = lu.kernel();
	Eigen::VectorXd y_(E_.cols());

	for(int i = 0; i < y_.size(); i++)
		y_(i)=1;

	f_y = - G_r_k * w + E_* y_ ; //calculation of the controllable contact forces

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////777777777777777777777777777777777777
	///////////////////////////////////////////////////////////////////////////////////////

	//calculation of a basis for the subspace of the controllable internal force
	// with synergy
	Eigen::MatrixXd I = MatrixXd::Identity(3*n_c, 3*n_c);
	Eigen::MatrixXd F(3*n_c,n_q);
	Eigen::MatrixXd E;
	Eigen::VectorXd y(1);

	y(0) = synergy;

	F = ( I - G_r_k * G ) * K * H_J; // and maps independently controlled joint reference displacements δqr’s into active internal forces

	E = F * S ;

	f_y = - G_r_k * w + E * y ; // calculation of the controllable contact forces

	// search the minimum value of d(f_y) vector
	Eigen::VectorXd f_i(3);
	Eigen::VectorXd n_i(3);
	Eigen::VectorXd f_i_n(1);
	int step = 0;
	double f_i_ ;

	for(int i = 0 ; i < (f_y.size()/3)  ; i++)
	{
		f_i(0) = f_y(step+0);
		f_i(1) = f_y(step+1);
		f_i(2) = f_y(step+2);

		f_i_ = f_i(0)+f_i(1)+f_i(2);
		f_i_n = f_i * n_i.transpose();

		d_f(step+0)  =  f_i_n(0);
		d_f(step+1)  =  mu_friction*f_i_n(0) - (sin(acos(f_i_n(0)/f_i_)));
		d_f(step+2)  =  f_i_max - f_i.norm();

		step += 3;
	}

	double d_min = d_f(0);

	for(int i = 0 ; i < d_f.size() ; i++)
		if( d_min > d_f(i) )
			d_min = d_f(i);

	Eigen::VectorXd Singular ;
	JacobiSVD<MatrixXd> svd(G_r_k, ComputeThinU | ComputeThinV);
    Singular = svd.singularValues();

    double sigma_max = Singular[0];

    PCR = d_min / sigma_max;

	cout << " YEAH ENJOY " << endl;
	cout << "   fine   " << endl;

	ros::spin();
	return 0;
}