/*
Copyright(c) 2014 Akihiro Nishimura

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

#include "ctr.h"
#include "SigUtil/lib/calculation/binary_operation.hpp"
#include "SigUtil/lib/tools/convergence.hpp"
#include "SigUtil/lib/functional/filter.hpp"
#include "SigUtil/lib/functional/list_deal.hpp"
#include <Eigen/Dense>

#include "SigUtil/lib/tools/time_watch.hpp"

namespace sigtm
{
//using namespace boost::numeric;

const double projection_z = 1.0;

static double safe_log(double x)
{
	return x > 0 ? std::log(x) : log_lower_limit;
};

template <class C>
static auto row_(C&& src, uint i) ->decltype(ublas::row(src, i))
{
	return ublas::row(src, i);
}
static auto row_(EigenMatrix& src, uint i) ->decltype(src.row(i))
{
	return src.row(i);
}
static auto row_(EigenMatrix const& src, uint i) ->decltype(src.row(i))
{
	return src.row(i);
}

template <class C>
static auto at_(C&& src, uint row, uint col) ->decltype(src(row, col))
{
	return src(row, col);
}
static auto at_(EigenMatrix& src, uint row, uint col) ->decltype(src.coeffRef(col, row))
{
	return src.coeffRef(col, row);
}
static auto at_(EigenMatrix const& src, uint row, uint col) ->decltype(src.coeffRef(col, row))
{
	return src.coeffRef(col, row);
}

template <class V>
void normalize_dist_v(V&& vec)
{
	double sum = vec.sum();
	vec /= sum;
}
template <class F, class V>
auto map_v(F&& func, V&& vec)
{
	using RT = decltype(sig::impl::eval(std::forward<F>(func), std::forward<V>(vec)(0)));

	EigenVector result(vec.size());

	for (uint i = 0, size = vec.size(); i < size; ++i){
		result[i] = std::forward<F>(func)(std::forward<V>(vec)(i));
	}

	return result;
}

template <class F, class M>
auto map_m(F&& func, M&& mat)
{
	using RT = decltype(sig::impl::eval(std::forward<F>(func), std::forward<M>(mat)(0, 0)));

	const uint col_size = mat.cols();
	const uint row_size = mat.rows();

	EigenMatrix result(col_size, row_size);

	for (uint i = 0; i < col_size; ++i){
		for (uint j = 0; j < row_size; ++j){
			result(i, j) = std::forward<F>(func)(std::forward<M>(mat)(i, j));
		}
	}

	return result;
}

template <class V>
bool is_feasible(V const& x)
{
	double val;
	double sum = 0;
	for (uint i = 0, size = x.size()-1; i < size; ++i) {
		val = x[i];
		if (val < 0 || val >1) return false;
		sum += val;
		if (sum > 1) return false;
	}
	return true;
}

// project x on to simplex (using // http://www.cs.berkeley.edu/~jduchi/projects/DuchiShSiCh08.pdf)
template <class V1, class V2>
void simplex_projection(
	V1 const& x,
	V2& x_proj,
	double z)
{
	x_proj = x;
	std::sort(x_proj.data(), x_proj.data() + x_proj.size());
	double cumsum = -z, u;
	int j = 0;

	for (int i = x.size() - 1; i >= 0; --i) {
		u = x_proj[i];
		cumsum += u;
		if (u > cumsum / (j + 1)) j++;
		else break;
	}
	double theta = cumsum / j;
	for (int i = 0, size = x.size(); i < size; ++i) {
		u = x[i] - theta;
		if (u <= 0) u = 0.0;
		x_proj[i] = u;
	}
	normalize_dist_v(x_proj); // fix the normaliztion issue due to numerical errors
}

template <class V1, class V2, class V3>
auto df_simplex(
	V1 const&gamma,
	V2 const& v,
	double lambda,
	V3 const& opt_x)
{
	EigenVector g = -lambda * (opt_x - v);
	EigenVector y = gamma;

	//sig::for_each_v([](double& v1, double v2){ v1 /= v2; }, y, opt_x);
	for(uint i = 0, size = y.size(); i < size; ++i){
		y[i] /= opt_x[i];
	}

	g += y;

	//sig::for_each_v([](double& v1){ v1 *= -1; }, g);
	g.array() *= -1;

	return g;
}

template <class V1, class V2, class V3>
double f_simplex(
	V1 const& gamma,
	V2 const& v,
	double lambda,
	V3 const& opt_x)
{
	auto y = map_v([&](double x){ return safe_log(x); }, opt_x);
	auto z = v - opt_x;

	//double f = ublas::inner_prod(y, gamma);
	double f = y.dot(gamma);

	//double val = ublas::inner_prod(z, z);
	double val = z.dot(z);

	f -= 0.5 * lambda * val;

	return -f;
}

// projection gradient algorithm
template <class V1, class V2, class V3>
void optimize_simplex(
	V1 const& gamma,
	V2 const& v,
	double lambda,
	V3& opt_x)
{
	size_t size = sig::min(gamma.size(), v.size());
	EigenVector x_bar(size);
	EigenVector opt_x_old = opt_x;

	double f_old = f_simplex(gamma, v, lambda, opt_x);

	auto g = df_simplex(gamma, v, lambda, opt_x);

	normalize_dist_v(g);
	//double ab_sum = sig::sum(g);
	//if (ab_sum > 1.0) g *= (1.0 / ab_sum); // rescale the gradient

	opt_x -= g;

	simplex_projection(opt_x, x_bar, projection_z);

	x_bar -= opt_x_old;

	//double r = 0.5 * ublas::inner_prod(g, x_bar);
	double r = 0.5 * g.dot(x_bar);

	const double beta = 0.5;
	double t = beta;
	for (uint iter = 0; iter < 100; ++iter) {
		opt_x = opt_x_old;
		opt_x += t * x_bar;

		double f_new = f_simplex(gamma, v, lambda, opt_x);

		if (f_new > f_old + r * t) t = t * beta;
		else break;
	}

	if (!is_feasible(opt_x))  printf("sth is wrong, not feasible. you've got to check it ...\n");
}

void CTR::init()
{
	sig::SimpleRandom<double> randf(0, 1, FixedRandom);

	beta_ = MatrixKV_(V_, K_); //SIG_INIT_MATRIX(double, K, V, 0);

	if (hparam_->beta_.empty()){
		for(TopicId k = 0; k < K_; ++k){
			auto& beta_v = row_(beta_, k);
			for (uint v = 0; v < V_; ++v) beta_v[v] = randf() + hparam_->beta_smooth_;
			normalize_dist_v(beta_v);
		}
	}
	else{
		std::cout << "beta loading" << std::endl;
		//beta_ = sig::to_matrix_ublas(hparam_->beta_);
		for (uint k = 0; k < K_; ++k){
			for (uint v = 0; v < V_; ++v) beta_(v, k) = hparam_->beta_[k][v];
		}
	}

	theta_ = MatrixIK_::Zero(K_, I_); // SIG_INIT_MATRIX(double, I, K, 0);

	if (hparam_->theta_opt_){
		if (hparam_->theta_.empty()){
			for (ItemId i = 0; i < I_; ++i){
				auto& theta_v = row_(theta_, i);
				for (uint k = 0; k < K_; ++k) theta_v[k] = randf() + hparam_->alpha_smooth_;
				normalize_dist_v(theta_v);
			}
		}
		else{
			std::cout << "theta loading" << std::endl;
			//theta_ = sig::to_matrix_ublas(hparam_->theta_);
			for (uint i = 0; i < I_; ++i){
				for (uint k = 0; k < K_; ++k) beta_(k, i) = hparam_->beta_[i][k];

			}
		}
	}

	user_factor_ = MatrixUK_::Zero(U_, K_);
	item_factor_ = MatrixIK_::Zero(I_, K_);

	if (hparam_->theta_opt_){
		for (ItemId i = 0; i < I_; ++i){
			auto& if_v = row_(item_factor_, i);
			for (uint k = 0; k < K_; ++k) if_v[k] = randf();
		}
	}
	else{
		item_factor_ = theta_;
	}
}

void CTR::printUFactor() const
{
	std::cout << "user_factor" << std::endl;
	for (uint u = 0; u<U_; ++u){
		for (uint k = 0; k<K_; ++k) std::cout << user_factor_(u, k) << ", ";
		std::cout << std::endl;
	}
}
void CTR::printIFactor() const
{
	std::cout << "item_factor" << std::endl;
	for (uint i = 0; i<I_; ++i){
		for (uint k = 0; k<K_; ++k) std::cout << item_factor_(i, k) << ", ";
		std::cout << std::endl;
	}
}

/*
	std::cout << "estimate rating" << std::endl;
	for (uint u = 0; u<ratings_->userSize(); ++u){
		for (uint i = 0; i<ratings_->itemSize(); ++i) std::cout << estimate(u, i) << ", ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/

void CTR::saveTmp() const
{
	/*
	sprintf(name, "%s/%04d-U.dat", directory, iter);
      FILE * file_U = fopen(name, "w");
      mtx_fprintf(file_U, user_factor_);
      fclose(file_U);

      sprintf(name, "%s/%04d-V.dat", directory, iter);
      FILE * file_V = fopen(name, "w");
      mtx_fprintf(file_V, item_factor_);
      fclose(file_V);

      if (hparam_->ctr_run) {
        sprintf(name, "%s/%04d-theta.dat", directory, iter);
        FILE * file_theta = fopen(name, "w");
        mtx_fprintf(file_theta, m_theta);
        fclose(file_theta);

        sprintf(name, "%s/%04d-beta.dat", directory, iter);
        FILE * file_beta = fopen(name, "w");
        mtx_fprintf(file_beta, m_beta);
        fclose(file_beta);
	}
	*/
}

void CTR::save() const
{
}

void CTR::load()
{

}

double CTR::docInference(ItemId id,	bool update_word_ss)
{
	double pseudo_count = 1.0;
	double likelihood = 0;
	auto const& theta_v = row_(theta_, id);
	auto log_theta_v = map_v([&](double x){ return safe_log(x); }, theta_v);

	for (auto tid : item_tokens_[id]){
		WordId w = tokens_[tid].word_id;
		auto& phi_v = row_(phi_, tid);

		for (TopicId k = 0; k < K_; ++k){
			phi_v[k] = theta_v[k] * at_(beta_, k, w);
		}
		normalize_dist_v(phi_v);

		for (TopicId k = 0; k < K_; ++k){
			double const& p = phi_v[k];
			if (p > 0){
				likelihood += p * (log_theta_v[k] + log_beta_(k, w) - std::log(p));
			}
		}
	}

	if (pseudo_count > 0) {
		//likelihood += pseudo_count * std::accumulate(std::begin(log_theta_v), std::end(log_theta_v), 0.0);
		likelihood += pseudo_count * log_theta_v.sum();
	}

	// smoothing with small pseudo counts
	//sig::for_each_v([&](double& v){ v = pseudo_count; }, gamma_);
	gamma_.array() += pseudo_count;

	for (auto tid : item_tokens_[id]){
		for (TopicId k = 0; k < K_; ++k) {
			//double x = doc->m_counts[tid] * phi_(tid, k);	// doc_word_ct only
			double const& x = at_(phi_, tid, k);
			gamma_[k] += x;

			if (update_word_ss){
				at_(word_ss_, k, tokens_[tid].word_id) -= x;
			}
		}
	}

	return likelihood;
}

void CTR::updateU()
{
	double delta_ab = hparam_->a_ - hparam_->b_;
	MatrixKK_ XX = MatrixKK_::Zero(K_, K_);

	// calculate VCV^T in equation(8)
	for (uint i = 0; i < I_; i ++){
		if (std::begin(item_ratings_[i]) != std::end(item_ratings_[i])){
			auto const& vec_v = row_(item_factor_, i);

			//XX += outer_prod(vec_v, vec_v);
			XX += vec_v.transpose() * vec_v;
		}
    }

	// negative item weight
	XX *= hparam_->b_;

	//sig::for_diagonal([&](double& v){ v += hparam_->lambda_u_; }, XX);
	XX.diagonal().array() += hparam_->lambda_u_;

	for (uint j = 0; j < U_; ++j){
		auto const& ratings = user_ratings_[j];

		if (std::begin(ratings) != std::end(ratings)){
			EigenMatrix A = XX;
			VectorK_ x(K_, 0);

			for (auto rating : ratings){
				auto const& vec_v = row_(item_factor_, rating->item_id_);

				A += delta_ab * vec_v.transpose() * vec_v;
				x += hparam_->a_ * vec_v;
			}

			auto vec_u = row_(user_factor_, j);
			//vec_u = *sig::matrix_vector_solve(std::move(A), std::move(x));	// update vector u
			auto slv = A.fullPivLu().solve(x);
			for (uint k = 0; k < K_; ++k) vec_u.coeffRef(k) = slv.coeff(k);

			// update the likelihood
			//auto result = inner_prod(vec_u, vec_u);
			auto result = vec_u.dot(vec_u);
			likelihood_ += -0.5 * hparam_->lambda_u_ * result;
		}
	}
}

void CTR::updateV()
{
	double delta_ab = hparam_->a_ - hparam_->b_;
	MatrixKK_ XX = MatrixKK_::Zero(K_, K_);

	for (uint j = 0; j < U_; ++j){
		if (std::begin(user_ratings_[j]) != std::end(user_ratings_[j])){
			auto const& vec_u = row_(user_factor_, j);
			//XX += outer_prod(vec_u, vec_u);
			XX += vec_u.transpose() * vec_u;
		}
	}
	XX.array() *= hparam_->b_;

	for (uint i = 0; i < I_; ++i){
		auto& vec_v = row_(item_factor_, i);
		auto const& theta_v = row_(theta_, i);
		auto const& ratings = item_ratings_[i];

		if (std::begin(ratings) != std::end(ratings)){
			EigenMatrix A = XX;
			VectorK_ x(K_, 0);

			for (auto rating : ratings){
				auto const& vec_u = row_(user_factor_, rating->user_id_);

				//A += delta_ab * outer_prod(vec_u, vec_u);
				A += delta_ab * vec_u.transpose() * vec_u;
				x += hparam_->a_ * vec_u;
			}

			//sig::for_each_v([&](double& x, double t){ x += hparam_->lambda_v_ * t; }, xx, theta_v);
			x += hparam_->lambda_v_ * theta_v;	// adding the topic vector

			EigenMatrix B = A;		// save for computing likelihood

			//sig::for_diagonal([&](double& v){ v += hparam_->lambda_v_; }, A);
			A.diagonal().array() += hparam_->lambda_v_;

			//vec_v = *sig::matrix_vector_solve(A, std::move(x));	// update vector v
			vec_v = A.colPivHouseholderQr().solve(x);

			// update the likelihood for the relevant part
			likelihood_ += -0.5 * item_ratings_[i].size() * hparam_->a_;

			for (auto rating : ratings){
				auto const& vec_u = row_(user_factor_, rating->user_id_);
				//auto result = inner_prod(vec_u, vec_v);
				auto result = vec_u.dot(vec_u);

				likelihood_ += hparam_->a_ * result;
			}
			//likelihood_ += -0.5 * ublas::inner_prod(vec_v, ublas::prod(B, vec_v));
			likelihood_ += -0.5 * vec_v.dot(B * vec_v.transpose());

			// likelihood part of theta, even when theta=0, which is a special case
			EigenVector x2 = vec_v;

			//sig::for_each_v([](double& v1, double v2){ v1 -= v2; }, x2, theta_v);
			x2 -= theta_v;

			//auto result = inner_prod(x2, x2);
			auto result = x2.dot(x2);
			likelihood_ += -0.5 * hparam_->lambda_v_ * result;

			if (hparam_->theta_opt_){
				likelihood_ += docInference(i, true);
				optimize_simplex(gamma_, vec_v, hparam_->lambda_v_, row_(theta_, i));
			}
		}
		else{
			// m=0, this article has never been rated
			if (hparam_->theta_opt_) {
				docInference(i, false);
				normalize_dist_v(gamma_);
				row_(theta_, i) = gamma_;
			}
		}
	}
}

void CTR::updateBeta()
{
	beta_ = word_ss_;

	for (TopicId k = 0; k < K_; ++k){
		auto& beta_v = row_(beta_, k);

		normalize_dist_v(beta_v);
		row_(log_beta_, k) = map_v([&](double x){ return safe_log(x); }, beta_v);
	}
}

auto CTR::recommend_impl(Id id, bool for_user) const->std::vector<EstValueType>
{
	std::vector<EstValueType> result;

	if (for_user){
		uint i = 0;
		for (auto e : user_ratings_[id]){
			for (uint ed = e->item_id_; i < ed; ++i){
				result.push_back(std::make_pair(i, estimate(id, i)));
			}
			i = e->item_id_ + 1;
		}
		for (; i < I_; ++i){
			result.push_back(std::make_pair(i, estimate(id, i)));
		}
	}
	else{
		uint u = 0;
		for (auto e : item_ratings_[id]){
			for (uint ed = e->item_id_; u < ed; ++u){
				result.push_back(std::make_pair(u, estimate(u, id)));
			}
			u = e->user_id_ + 1;
		}
		for (; u < U_; ++u){
			result.push_back(std::make_pair(u, estimate(u, id)));
		}
	}

	sig::sort(result, [](std::pair<Id, double> const& v1, std::pair<Id, double> const& v2){ return v1.second > v2.second; });

	return result;
}

void CTR::train(uint max_iter, uint min_iter, uint save_lag)
{
	uint iter = 0;
	double likelihood_old;
	sig::ManageConvergenceSimple conv(conv_epsilon_);

	auto info_print = [](uint iter, double likelihood, double converge){
		std::cout << "iter=" << iter << ", likelihood=" << likelihood << ", converge=" << converge << std::endl;
		return true;
	};

	if (max_iter < min_iter) std::swap(max_iter, min_iter);

	if (hparam_->theta_opt_){
		gamma_ = VectorK_::Zero(K_);
		log_beta_ = map_m([&](double x){ return safe_log(x); }, beta_);
		word_ss_ = MatrixKV_(V_, K_); // SIG_INIT_MATRIX(double, K, V, 0);
		phi_ = MatrixKV_(K_, T_);  //SIG_INIT_MATRIX(double, T, K, 0);
	}

	 while ((!conv.is_convergence() && iter < max_iter) || iter < min_iter)
	 {
		likelihood_old = likelihood_;
		likelihood_ = 0.0;

		//printUFactor();
		//printIFactor();

		//sig::TimeWatch tw;
		updateU();
		//tw.save();
		//std::cout << tw.get_total_time() << std::endl;

		//if (hparam_->lda_regression_) break; // one iteration is enough for lda-regression

		updateV();

		// update beta if needed
		if (hparam_->theta_opt_) updateBeta();

		if(likelihood_ < likelihood_old) std::cout << "likelihood is decreasing!" << std::endl;

		// save intermediate results
		if (iter % save_lag == 0) {
			saveTmp();
		}

		++iter;
		conv.update( sig::abs_delta(likelihood_, likelihood_old) / likelihood_old);

		info_print(iter, likelihood_, conv.get_value());
	 }

	 std::cout << "train finished" << std::endl;
}

auto CTR::recommend(Id id, bool for_user, sig::Maybe<uint> top_n, sig::Maybe<double> threshold) const->std::vector<EstValueType>
{
	auto result = recommend_impl(id, for_user);

	if (top_n) result = sig::take(*top_n, std::move(result));
	if (threshold) sig::filter([&](std::pair<Id, double> const& e){ return e.second > *threshold; }, std::move(result));

	return result;
}

inline double CTR::estimate(UserId u_id, ItemId i_id) const
{
	//return inner_prod(row_(user_factor_, u_id), row_(item_factor_, i_id));
	return row_(user_factor_, u_id).dot(row_(item_factor_, i_id));
}

/*
void c_ctr::learn_map_estimate(
	const c_data* users,
	const c_data* items,
	const c_corpus* c,
	const ctr_hyperparameter* param,
	const char* directory)
{
  // init model parameters
  printf("\ninitializing the model ...\n");
  init_model(hparam_->ctr_run);

  // filename
  char name[500];

  // start time
  time_t start, current;
  time(&start);
  int elapsed = 0;

  int iter = 0;
  double likelihood = -exp(50), likelihood_old;
  double converge = 1.0;

  /// create the state log file
  sprintf(name, "%s/state.log", directory);
  FILE* file = fopen(name, "w");
  fprintf(file, "iter time likelihood converge\n");

  int i, j, m, n, l, k;
  int* item_ids;
  int* user_ids;

  double result;

  /// confidence parameters
  double a_minus_b = hparam_->a - hparam_->b;

  update();

  save();

  // free memory
  gsl_matrix_free(XX);
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_vector_free(x);

  if (hparam_->ctr_run && hparam_->theta_opt) {
    gsl_matrix_free(phi);
    gsl_matrix_free(log_beta);
    gsl_matrix_free(word_ss);
    gsl_vector_free(gamma);
  }
}
*/

}	// sigtm


/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * 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 Willow Garage, Inc. 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 OWNER 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.
 */

/*
 * Author: Stuart Glaser
 */

#include <boost/shared_ptr.hpp>

#include "robot_mechanism_controllers/joint_spline_trajectory_controller.h"
#include <sstream>
#include "angles/angles.h"
#include "pluginlib/class_list_macros.h"

PLUGINLIB_EXPORT_CLASS( controller::JointSplineTrajectoryController, pr2_controller_interface::Controller)

namespace controller {

// These functions are pulled from the spline_smoother package.
// They've been moved here to avoid depending on packages that aren't
// mature yet.

static inline void generatePowers(int n, double x, double* powers)
{
  powers[0] = 1.0;
  for (int i=1; i<=n; i++)
  {
    powers[i] = powers[i-1]*x;
  }
}

static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
    double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
{
  coefficients.resize(6);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.5*end_acc;
    coefficients[3] = 0.0;
    coefficients[4] = 0.0;
    coefficients[5] = 0.0;
  }
  else
  {
    double T[6];
    generatePowers(5, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = 0.5*start_acc;
    coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
                       12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
    coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
                       16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
    coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
                       6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
  }
}

/**
 * \brief Samples a quintic spline segment at a particular time
 */
static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
    double& position, double& velocity, double& acceleration)
{
  // create powers of time:
  double t[6];
  generatePowers(5, time, t);

  position = t[0]*coefficients[0] +
      t[1]*coefficients[1] +
      t[2]*coefficients[2] +
      t[3]*coefficients[3] +
      t[4]*coefficients[4] +
      t[5]*coefficients[5];

  velocity = t[0]*coefficients[1] +
      2.0*t[1]*coefficients[2] +
      3.0*t[2]*coefficients[3] +
      4.0*t[3]*coefficients[4] +
      5.0*t[4]*coefficients[5];

  acceleration = 2.0*t[0]*coefficients[2] +
      6.0*t[1]*coefficients[3] +
      12.0*t[2]*coefficients[4] +
      20.0*t[3]*coefficients[5];
}

static void getCubicSplineCoefficients(double start_pos, double start_vel,
    double end_pos, double end_vel, double time, std::vector<double>& coefficients)
{
  coefficients.resize(4);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.0;
    coefficients[3] = 0.0;
  }
  else
  {
    double T[4];
    generatePowers(3, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
    coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
  }
}

JointSplineTrajectoryController::JointSplineTrajectoryController()
  : loop_count_(0), robot_(NULL)
{
}

JointSplineTrajectoryController::~JointSplineTrajectoryController()
{
  sub_command_.shutdown();
  serve_query_state_.shutdown();
}

bool JointSplineTrajectoryController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
{
  using namespace XmlRpc;
  node_ = n;
  robot_ = robot;

  // Gets all of the joints
  XmlRpc::XmlRpcValue joint_names;
  if (!node_.getParam("joints", joint_names))
  {
    ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  for (int i = 0; i < joint_names.size(); ++i)
  {
    XmlRpcValue &name_value = joint_names[i];
    if (name_value.getType() != XmlRpcValue::TypeString)
    {
      ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
                node_.getNamespace().c_str());
      return false;
    }

    pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
    if (!j) {
      ROS_ERROR("Joint not found: %s. (namespace: %s)",
                ((std::string)name_value).c_str(), node_.getNamespace().c_str());
      return false;
    }
    joints_.push_back(j);
  }

  // Ensures that all the joints are calibrated.
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    if (!joints_[i]->calibrated_)
    {
      ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
                joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
      return false;
    }
  }

  // Sets up pid controllers for all of the joints
  std::string gains_ns;
  if (!node_.getParam("gains", gains_ns))
    gains_ns = node_.getNamespace() + "/gains";
  pids_.resize(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
    if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name)))
      return false;

  // Creates a dummy trajectory
  boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &traj = *traj_ptr;
  traj[0].start_time = robot_->getTime().toSec();
  traj[0].duration = 0.0;
  traj[0].splines.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    traj[0].splines[j].coef[0] = 0.0;
  current_trajectory_box_.set(traj_ptr);

  sub_command_ = node_.subscribe("command", 1, &JointSplineTrajectoryController::commandCB, this);
  serve_query_state_ = node_.advertiseService(
    "query_state", &JointSplineTrajectoryController::queryStateService, this);

  q.resize(joints_.size());
  qd.resize(joints_.size());
  qdd.resize(joints_.size());

  controller_state_publisher_.reset(
    new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
    (node_, "state", 1));
  controller_state_publisher_->lock();
  for (size_t j = 0; j < joints_.size(); ++j)
    controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
  controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
  controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
  controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
  controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.error.positions.resize(joints_.size());
  controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
  controller_state_publisher_->unlock();

  return true;
}

void JointSplineTrajectoryController::starting()
{
  last_time_ = robot_->getTime();

  for (size_t i = 0; i < pids_.size(); ++i)
    pids_[i].reset();

  // Creates a "hold current position" trajectory.
  boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &hold = *hold_ptr;
  hold[0].start_time = last_time_.toSec() - 0.001;
  hold[0].duration = 0.0;
  hold[0].splines.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    hold[0].splines[j].coef[0] = joints_[j]->position_;

  current_trajectory_box_.set(hold_ptr);
}

void JointSplineTrajectoryController::update()
{
  // Checks if all the joints are calibrated.

  ros::Time time = robot_->getTime();
  ros::Duration dt = time - last_time_;
  last_time_ = time;

  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.get(traj_ptr);
  if (!traj_ptr)
    ROS_FATAL("The current trajectory can never be null");

  // Only because this is what the code originally looked like.
  const SpecifiedTrajectory &traj = *traj_ptr;

  // Determines which segment of the trajectory to use.  (Not particularly realtime friendly).
  int seg = -1;
  while (seg + 1 < (int)traj.size() &&
         traj[seg+1].start_time < time.toSec())
  {
    ++seg;
  }

  if (seg == -1)
  {
    if (traj.size() == 0)
      ROS_ERROR("No segments in the trajectory");
    else
      ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
    return;
  }

  // ------ Trajectory Sampling

  for (size_t i = 0; i < q.size(); ++i)
  {
    sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
                               time.toSec() - traj[seg].start_time,
                               q[i], qd[i], qdd[i]);
  }

  // ------ Trajectory Following

  std::vector<double> error(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    error[i] = q[i] - joints_[i]->position_;
    joints_[i]->commanded_effort_ += pids_[i].computeCommand(error[i],
          joints_[i]->velocity_ - qd[i], dt);
  }

  // ------ State publishing

  if (loop_count_ % 10 == 0)
  {
    if (controller_state_publisher_ && controller_state_publisher_->trylock())
    {
      controller_state_publisher_->msg_.header.stamp = time;
      for (size_t j = 0; j < joints_.size(); ++j)
      {
        controller_state_publisher_->msg_.desired.positions[j] = q[j];
        controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
        controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
        controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
        controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
        controller_state_publisher_->msg_.error.positions[j] = error[j];
        controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
      }
      controller_state_publisher_->unlockAndPublish();
    }
  }

  ++loop_count_;
}

void JointSplineTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
  ros::Time time = last_time_;
  ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
            time.toSec(), msg->header.stamp.toSec());

  boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
  SpecifiedTrajectory &new_traj = *new_traj_ptr;

  // ------ If requested, performs a stop

  if (msg->points.empty())
  {
    starting();
    return;
  }

  // ------ Correlates the joints we're commanding to the joints in the message

  std::vector<int> lookup(joints_.size(), -1);  // Maps from an index in joints_ to an index in the msg
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    for (size_t k = 0; k < msg->joint_names.size(); ++k)
    {
      if (msg->joint_names[k] == joints_[j]->joint_->name)
      {
        lookup[j] = k;
        break;
      }
    }

    if (lookup[j] == -1)
    {
      ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
      return;
    }
  }

  // ------ Grabs the trajectory that we're currently following.

  boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
  current_trajectory_box_.get(prev_traj_ptr);
  if (!prev_traj_ptr)
  {
    ROS_FATAL("The current trajectory can never be null");
    return;
  }
  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;

  // ------ Copies over the segments from the previous trajectory that are still useful.

  // Useful segments are still relevant after the current time.
  int first_useful = -1;
  while (first_useful + 1 < (int)prev_traj.size() &&
         prev_traj[first_useful + 1].start_time <= time.toSec())
  {
    ++first_useful;
  }

  // Useful segments are not going to be completely overwritten by the message's splines.
  int last_useful = -1;
  double msg_start_time;
  if (msg->points.size() > 0)
    msg_start_time = (msg->header.stamp + msg->points[0].time_from_start).toSec();
  else
    msg_start_time = std::max(time.toSec(), msg->header.stamp.toSec());

  while (last_useful + 1 < (int)prev_traj.size() &&
         prev_traj[last_useful + 1].start_time < msg_start_time)
  {
    ++last_useful;
  }

  if (last_useful < first_useful)
    first_useful = last_useful;

  // Copies over the old segments that were determined to be useful.
  for (int i = std::max(first_useful,0); i <= last_useful; ++i)
  {
    new_traj.push_back(prev_traj[i]);
  }

  // We always save the last segment so that we know where to stop if
  // there are no new segments.
  if (new_traj.size() == 0)
    new_traj.push_back(prev_traj[prev_traj.size() - 1]);

  // ------ Determines when and where the new segments start

  // Finds the end conditions of the final segment
  Segment &last = new_traj[new_traj.size() - 1];
  std::vector<double> prev_positions(joints_.size());
  std::vector<double> prev_velocities(joints_.size());
  std::vector<double> prev_accelerations(joints_.size());

  ROS_DEBUG("Initial conditions for new set of splines:");
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
                               msg->header.stamp.toSec() - last.start_time,
                               prev_positions[i], prev_velocities[i], prev_accelerations[i]);
    ROS_DEBUG("    %.2lf, %.2lf, %.2lf  (%s)", prev_positions[i], prev_velocities[i],
              prev_accelerations[i], joints_[i]->joint_->name.c_str());
  }

  // ------ Tacks on the new segments

  std::vector<double> positions;
  std::vector<double> velocities;
  std::vector<double> accelerations;

  std::vector<double> durations(msg->points.size());
  durations[0] = msg->points[0].time_from_start.toSec();
  for (size_t i = 1; i < msg->points.size(); ++i)
    durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();

  // Checks if we should wrap
  std::vector<double> wrap(joints_.size(), 0.0);
  assert(!msg->points[0].positions.empty());
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
    {
      double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]);
      wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j];
    }
  }

  for (size_t i = 0; i < msg->points.size(); ++i)
  {
    Segment seg;

    seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
    seg.duration = durations[i];
    seg.splines.resize(joints_.size());

    // Checks that the incoming segment has the right number of elements.

    if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
      return;
    }
    if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
      return;
    }
    if (msg->points[i].positions.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
      return;
    }

    // Re-orders the joints in the command to match the interal joint order.

    accelerations.resize(msg->points[i].accelerations.size());
    velocities.resize(msg->points[i].velocities.size());
    positions.resize(msg->points[i].positions.size());
    for (size_t j = 0; j < joints_.size(); ++j)
    {
      if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
      if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
      if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
    }

    // Converts the boundary conditions to splines.

    for (size_t j = 0; j < joints_.size(); ++j)
    {
      if (prev_accelerations.size() > 0 && accelerations.size() > 0)
      {
        getQuinticSplineCoefficients(
          prev_positions[j], prev_velocities[j], prev_accelerations[j],
          positions[j], velocities[j], accelerations[j],
          durations[i],
          seg.splines[j].coef);
      }
      else if (prev_velocities.size() > 0 && velocities.size() > 0)
      {
        getCubicSplineCoefficients(
          prev_positions[j], prev_velocities[j],
          positions[j], velocities[j],
          durations[i],
          seg.splines[j].coef);
        seg.splines[j].coef.resize(6, 0.0);
      }
      else
      {
        seg.splines[j].coef[0] = prev_positions[j];
        if (durations[i] == 0.0)
          seg.splines[j].coef[1] = 0.0;
        else
          seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
        seg.splines[j].coef[2] = 0.0;
        seg.splines[j].coef[3] = 0.0;
        seg.splines[j].coef[4] = 0.0;
        seg.splines[j].coef[5] = 0.0;
      }
    }

    // Pushes the splines onto the end of the new trajectory.

    new_traj.push_back(seg);

    // Computes the starting conditions for the next segment

    prev_positions = positions;
    prev_velocities = velocities;
    prev_accelerations = accelerations;
  }

  // ------ Commits the new trajectory

  if (!new_traj_ptr)
  {
    ROS_ERROR("The new trajectory was null!");
    return;
  }

  current_trajectory_box_.set(new_traj_ptr);
  ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
#if 0
  for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
  {
    ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
    for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
    {
      ROS_DEBUG("    %.2lf  %.2lf  %.2lf  %.2lf , %.2lf  %.2lf(%s)",
                new_traj[i].splines[j].coef[0],
                new_traj[i].splines[j].coef[1],
                new_traj[i].splines[j].coef[2],
                new_traj[i].splines[j].coef[3],
                new_traj[i].splines[j].coef[4],
                new_traj[i].splines[j].coef[5],
                joints_[j]->joint_->name_.c_str());
    }
  }
#endif
}

bool JointSplineTrajectoryController::queryStateService(
  pr2_controllers_msgs::QueryTrajectoryState::Request &req,
  pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
{
  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.get(traj_ptr);
  if (!traj_ptr)
  {
    ROS_FATAL("The current trajectory can never be null");
    return false;
  }
  const SpecifiedTrajectory &traj = *traj_ptr;

  // Determines which segment of the trajectory to use
  int seg = -1;
  while (seg + 1 < (int)traj.size() &&
         traj[seg+1].start_time < req.time.toSec())
  {
    ++seg;
  }
  if (seg == -1)
    return false;

  for (size_t i = 0; i < q.size(); ++i)
  {
  }

  resp.name.resize(joints_.size());
  resp.position.resize(joints_.size());
  resp.velocity.resize(joints_.size());
  resp.acceleration.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    resp.name[j] = joints_[j]->joint_->name;
    sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
                               req.time.toSec() - traj[seg].start_time,
                               resp.position[j], resp.velocity[j], resp.acceleration[j]);
  }

  return true;
}

void JointSplineTrajectoryController::sampleSplineWithTimeBounds(
  const std::vector<double>& coefficients, double duration, double time,
  double& position, double& velocity, double& acceleration)
{
  if (time < 0)
  {
    double _;
    sampleQuinticSpline(coefficients, 0.0, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else if (time > duration)
  {
    double _;
    sampleQuinticSpline(coefficients, duration, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else
  {
    sampleQuinticSpline(coefficients, time,
                        position, velocity, acceleration);
  }
}

}


/**
 * @file fusion.hpp
 * @brief The main header file for the Code Base
 *
 */

#ifndef _FUSION_HPP_
#define _FUSION_HPP_

//INITIAL STATE ENTER BEFORE FLIGHT
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

#define x_x_0 0 //initial position in x direction
#define x_y_0 0 //initial position in y direction
#define x_z_0 0 //initial position in z direction
#define v_x_0 0 // initial velocity in the x direction
#define v_y_0 0 // initial velocity in the y direction
#define v_z_0 0 // initial velocity in the z direction
#define a_x_0 0 // initial acceleration in x direction
#define a_y_0 0 // initial acceleration in y direction
#define a_z_0 0 // initial acceleration in z direction
#define theta_x_0 0 // initial attitude in the x direction
#define theta_y_0 0 // initial attitude in the y direction
#define theta_z_0 0 // initial attitude in the z direction
#define omega_x_0 0 // initial angular velocity in the x direction
#define omega_y_0 0 // initial angular velocity in the y direction
#define omega_z_0 0 // initial angular velocity in the z direction
#define alpha_x_0 0 // initial angular acceleration in the x direction
#define alpha_y_0 0 // initial angular acceleration in the y direction
#define alpha_z_0 0 // initial angular acceleration in the z direction
#define mag_x_0 0 //  nitial expected magnetic field reading in the x direction
#define mag_y_0 0 // initial expected magnetic field reading in the y direction
#define mag_z_0 0 // initial expected magnetic field reading in the z direction

#define p_x_x_0 0.1 //initial position variance in x direction
#define p_x_y_0 0.1 //initial position variance in y direction
#define p_x_z_0 0.1 //initial position variance in z direction
#define p_v_x_0 0.1 // initial velocity variance in the x direction
#define p_v_y_0 0.1 // initial velocity variance in the y direction
#define p_v_z_0 0.1 // initial velocity variance in the z direction
#define p_a_x_0 0.01 // initial acceleration variance in x direction
#define p_a_y_0 0.01 // initial acceleration variance in y direction
#define p_a_z_0 0.01 // initial acceleration variance in z direction
#define p_theta_x_0 0.1 // initial attitude variance in the x direction
#define p_theta_y_0 0.1 // initial attitude variance in the y direction
#define p_theta_z_0 0.1 // initial attitude variance in the z direction
#define p_omega_x_0 0.1 // initial angular velocity variance in the x direction
#define p_omega_y_0 0.1 // initial angular velocity variance in the y direction
#define p_omega_z_0 0.1 // initial angular velocity variance in the z direction
#define p_alpha_x_0 0.01 // initial angular acceleration variance in the x direction
#define p_alpha_y_0 0.01 // initial angular acceleration variance in the y direction
#define p_alpha_z_0 0.01 // initial angular acceleration variance in the z direction
#define p_mag_x_0 0 // initial expected magnetic field reading variance in the x direction
#define p_mag_y_0 0 // initial expected magnetic field reading variance in the y direction
#define p_mag_z_0 0 // initial expected magnetic field reading variance in the z direction

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

#include "Arduino.h"
#include "Eigen.h"
#include <Eigen/Eigen>
#include <Adafruit_BNO055.h>
#include "Thread.h"
#include <ThreadController.h>

//! IMU Struct
/*!
*   This structs holds the BNO055 sample at a point in time to be stored and processed.
*/
struct IMUdata {
    float GYRO[3] = {0,0,0};             // Gyro in [DPS]
    float LINEAR_ACCEL[3] = {0,0,0};     // Accelerometer in [m/s^2]
    float GRAVITY_ACCEL[3] = {0,0,0};    // Accelerometer in [mg]
    float MAG[3] = {0,0,0};              // Magnetometer in [uT]
    float Quat[4] = {0,0,0,0};           // {w, x, y, z}
    float Temp = 0;
    uint32_t t = 0;

    float phi, theta, psi;                // Roll or phi (X), pitch or theta (Y), and yaw or psi (Z)
    float q_w, q_x, q_y, q_z;             // Quaternion
};

//! Main State Estimation Class
/*!
*
*/
class State
{
    private:
        float accel_error = 0.0;

        float gyro_error = 0.0;
        IMUdata *data;

    public:
        State(IMUdata*);
        ~State();

        void dataAq(IMUdata *data);

        void predict();
        void processCovarianceMatrix();
        void calculateKalmanGain();
        void stateDetermination();
        void updatePreviousState();
        void updateProcessCovarianceMatrix();
        void updateDynamics();
        // Eigen::MatrixXd dcmBodyToEarth(double theta, double phi, double psi);

        template<typename T>
        void print_mtxd(const T& X);

        float calcAccelSystematicError();
        float calcGyroSystematicError();

        float eulerAngle(float GYRO, float gyro_sen, float gyro_samp); // Function to compute Euler angle
        void quaternion(float phi, float theta, float psi, float &q_w, float &q_x, float &q_y, float &q_z); // Function to computer Quaternion

    protected:
};

//! BNO055 IMU Class
/*!
*   Class to manage the Adafruit BNO055 Absolute Orientation IMU Fusion breakout board
*/
class DigitalIMU {
    private:
        Adafruit_BNO055 board;
        sensors_event_t event;
        imu::Quaternion quat;
        imu::Vector<3> accel;
    public:
        DigitalIMU();
        DigitalIMU(int32_t sensorID, uint8_t address);
        bool begin();
        void sample(IMUdata* data, State* ptr);
};

namespace constants
{
    extern int interval_IMU;
    extern double dt;
    extern double baseAccel_error;
    extern double baseGyro_error;
    extern float gyro_sen;           // = 900 rad/sec
};

/* All lower case variables are vectors || all uppercase are Matrices*/
namespace matrices
{
    // BOTH DISCARDED AFTER FIRST RUN
    extern Eigen::VectorXd x_0; // Initial State Vector
    extern Eigen::MatrixXd P_0; // Initial Process Covariance Matrix

    // VECTORS
    extern Eigen::VectorXd y; /* Observation Vector */
    extern Eigen::VectorXd x_m; /* Direct Observation Vector */
    extern Eigen::VectorXd x_k; /* Final State Vector */
    extern Eigen::VectorXd x_kp; /* State Prediction */
    extern Eigen::VectorXd x_k_1; /* Previous State Vector (K-1) */
    extern Eigen::VectorXd w_k; /* Predicted State Noise */
    extern Eigen::VectorXd z_k; /* Measurement Noise */

    // MATRICES
    extern Eigen::MatrixXd C; /* Observation Transition Matrix*/
    extern Eigen::MatrixXd A; /* State Transition Matrix */
    extern Eigen::MatrixXd P_kp; /* Predicted Process Covariance */
    extern Eigen::MatrixXd P_k_1; /* Previous Predicted Process Covariance */
    extern Eigen::MatrixXd R; /* Sensor Covariance Matrix */
    extern Eigen::MatrixXd H; /* Observation model mapping matrix*/
    extern Eigen::MatrixXd K; /* Kalman Gain */

    // Identity Matrix
    extern Eigen::MatrixXd I; /* 21x21 Identity Matrix */
 };

#endif


/* boost random/binomial_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2010
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP_INCLUDED
#define BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP_INCLUDED

#include <boost/config/no_tr1/cmath.hpp>
#include <cstdlib>
#include <iosfwd>

#include <boost/random/detail/config.hpp>
#include <boost/random/uniform_01.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

namespace detail {

template<class RealType>
struct binomial_table {
    static const RealType table[10];
};

template<class RealType>
const RealType binomial_table<RealType>::table[10] = {
    0.08106146679532726,
    0.04134069595540929,
    0.02767792568499834,
    0.02079067210376509,
    0.01664469118982119,
    0.01387612882307075,
    0.01189670994589177,
    0.01041126526197209,
    0.009255462182712733,
    0.008330563433362871
};

}

/**
 * The binomial distribution is an integer valued distribution with
 * two parameters, @c t and @c p.  The values of the distribution
 * are within the range [0,t].
 *
 * The distribution function is
 * \f$\displaystyle P(k) = {t \choose k}p^k(1-p)^{t-k}\f$.
 *
 * The algorithm used is the BTRD algorithm described in
 *
 *  @blockquote
 *  "The generation of binomial random variates", Wolfgang Hormann,
 *  Journal of Statistical Computation and Simulation, Volume 46,
 *  Issue 1 & 2 April 1993 , pages 101 - 110
 *  @endblockquote
 */
template<class IntType = int, class RealType = double>
class binomial_distribution {
public:
    typedef IntType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef binomial_distribution distribution_type;
        /**
         * Construct a param_type object.  @c t and @c p
         * are the parameters of the distribution.
         *
         * Requires: t >=0 && 0 <= p <= 1
         */
        explicit param_type(IntType t_arg = 1, RealType p_arg = RealType (0.5))
          : _t(t_arg), _p(p_arg)
        {}
        /** Returns the @c t parameter of the distribution. */
        IntType t() const { return _t; }
        /** Returns the @c p parameter of the distribution. */
        RealType p() const { return _p; }
#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
        /** Writes the parameters of the distribution to a @c std::ostream. */
        template<class CharT, class Traits>
        friend std::basic_ostream<CharT,Traits>&
        operator<<(std::basic_ostream<CharT,Traits>& os,
                   const param_type& parm)
        {
            os << parm._p << " " << parm._t;
            return os;
        }

        /** Reads the parameters of the distribution from a @c std::istream. */
        template<class CharT, class Traits>
        friend std::basic_istream<CharT,Traits>&
        operator>>(std::basic_istream<CharT,Traits>& is, param_type& parm)
        {
            is >> parm._p >> std::ws >> parm._t;
            return is;
        }
#endif
        /** Returns true if the parameters have the same values. */
        friend bool operator==(const param_type& lhs, const param_type& rhs)
        {
            return lhs._t == rhs._t && lhs._p == rhs._p;
        }
        /** Returns true if the parameters have different values. */
        friend bool operator!=(const param_type& lhs, const param_type& rhs)
        {
            return !(lhs == rhs);
        }
    private:
        IntType _t;
        RealType _p;
    };

    /**
     * Construct a @c binomial_distribution object. @c t and @c p
     * are the parameters of the distribution.
     *
     * Requires: t >=0 && 0 <= p <= 1
     */
    explicit binomial_distribution(IntType t_arg = 1,
                                   RealType p_arg = RealType(0.5))
      : _t(t_arg), _p(p_arg)
    {
        init();
    }

    /**
     * Construct an @c binomial_distribution object from the
     * parameters.
     */
    explicit binomial_distribution(const param_type& parm)
      : _t(parm.t()), _p(parm.p())
    {
        init();
    }

    /**
     * Returns a random variate distributed according to the
     * binomial distribution.
     */
    template<class URNG>
    IntType operator()(URNG& urng) const
    {
        if(use_inversion()) {
            if(0.5 < _p) {
                return _t - invert(_t, 1-_p, urng);
            } else {
                return invert(_t, _p, urng);
            }
        } else if(0.5 < _p) {
            return _t - generate(urng);
        } else {
            return generate(urng);
        }
    }

    /**
     * Returns a random variate distributed according to the
     * binomial distribution with parameters specified by @c param.
     */
    template<class URNG>
    IntType operator()(URNG& urng, const param_type& parm) const
    {
        return binomial_distribution(parm)(urng);
    }

    /** Returns the @c t parameter of the distribution. */
    IntType t() const { return _t; }
    /** Returns the @c p parameter of the distribution. */
    RealType p() const { return _p; }

    /** Returns the smallest value that the distribution can produce. */
    IntType min BOOST_PREVENT_MACRO_SUBSTITUTION() const { return 0; }
    /** Returns the largest value that the distribution can produce. */
    IntType max BOOST_PREVENT_MACRO_SUBSTITUTION() const { return _t; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_t, _p); }
    /** Sets parameters of the distribution. */
    void param(const param_type& parm)
    {
        _t = parm.t();
        _p = parm.p();
        init();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
    /** Writes the parameters of the distribution to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const binomial_distribution& bd)
    {
        os << bd.param();
        return os;
    }

    /** Reads the parameters of the distribution from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is, binomial_distribution& bd)
    {
        bd.read(is);
        return is;
    }
#endif

    /** Returns true if the two distributions will produce the same
        sequence of values, given equal generators. */
    friend bool operator==(const binomial_distribution& lhs,
                           const binomial_distribution& rhs)
    {
        return lhs._t == rhs._t && lhs._p == rhs._p;
    }
    /** Returns true if the two distributions could produce different
        sequences of values, given equal generators. */
    friend bool operator!=(const binomial_distribution& lhs,
                           const binomial_distribution& rhs)
    {
        return !(lhs == rhs);
    }

private:

    /// @cond show_private

    template<class CharT, class Traits>
    void read(std::basic_istream<CharT, Traits>& is) {
        param_type parm;
        if(is >> parm) {
            param(parm);
        }
    }

    bool use_inversion() const
    {
        // BTRD is safe when np >= 10
        return m < 11;
    }

    // computes the correction factor for the Stirling approximation
    // for log(k!)
    static RealType fc(IntType k)
    {
        if(k < 10) return detail::binomial_table<RealType>::table[k];
        else {
            RealType ikp1 = RealType(1) / (k + 1);
            return (RealType(1)/12
                 - (RealType(1)/360
                 - (RealType(1)/1260)*(ikp1*ikp1))*(ikp1*ikp1))*ikp1;
        }
    }

    void init()
    {
        using std::sqrt;
        using std::pow;

        RealType p = (0.5 < _p)? (1 - _p) : _p;
        IntType t = _t;

        m = static_cast<IntType>((t+1)*p);

        if(use_inversion()) {
            _u.q_n = pow((1 - p), static_cast<RealType>(t));
        } else {
            _u.btrd.r = p/(1-p);
            _u.btrd.nr = (t+1)*_u.btrd.r;
            _u.btrd.npq = t*p*(1-p);
            RealType sqrt_npq = sqrt(_u.btrd.npq);
            _u.btrd.b = 1.15 + 2.53 * sqrt_npq;
            _u.btrd.a = -0.0873 + 0.0248*_u.btrd.b + 0.01*p;
            _u.btrd.c = t*p + 0.5;
            _u.btrd.alpha = (2.83 + 5.1/_u.btrd.b) * sqrt_npq;
            _u.btrd.v_r = 0.92 - 4.2/_u.btrd.b;
            _u.btrd.u_rv_r = 0.86*_u.btrd.v_r;
        }
    }

    template<class URNG>
    result_type generate(URNG& urng) const
    {
        using std::floor;
        using std::abs;
        using std::log;

        while(true) {
            RealType u;
            RealType v = uniform_01<RealType>()(urng);
            if(v <= _u.btrd.u_rv_r) {
                u = v/_u.btrd.v_r - 0.43;
                return static_cast<IntType>(floor(
                    (2*_u.btrd.a/(0.5 - abs(u)) + _u.btrd.b)*u + _u.btrd.c));
            }

            if(v >= _u.btrd.v_r) {
                u = uniform_01<RealType>()(urng) - 0.5;
            } else {
                u = v/_u.btrd.v_r - 0.93;
                u = ((u < 0)? -0.5 : 0.5) - u;
                v = uniform_01<RealType>()(urng) * _u.btrd.v_r;
            }

            RealType us = 0.5 - abs(u);
            IntType k = static_cast<IntType>(floor((2*_u.btrd.a/us + _u.btrd.b)*u + _u.btrd.c));
            if(k < 0 || k > _t) continue;
            v = v*_u.btrd.alpha/(_u.btrd.a/(us*us) + _u.btrd.b);
            RealType km = abs(k - m);
            if(km <= 15) {
                RealType f = 1;
                if(m < k) {
                    IntType i = m;
                    do {
                        ++i;
                        f = f*(_u.btrd.nr/i - _u.btrd.r);
                    } while(i != k);
                } else if(m > k) {
                    IntType i = k;
                    do {
                        ++i;
                        v = v*(_u.btrd.nr/i - _u.btrd.r);
                    } while(i != m);
                }
                if(v <= f) return k;
                else continue;
            } else {
                // final acceptance/rejection
                v = log(v);
                RealType rho =
                    (km/_u.btrd.npq)*(((km/3. + 0.625)*km + 1./6)/_u.btrd.npq + 0.5);
                RealType t = -km*km/(2*_u.btrd.npq);
                if(v < t - rho) return k;
                if(v > t + rho) continue;

                IntType nm = _t - m + 1;
                RealType h = (m + 0.5)*log((m + 1)/(_u.btrd.r*nm))
                           + fc(m) + fc(_t - m);

                IntType nk = _t - k + 1;
                if(v <= h + (_t+1)*log(static_cast<RealType>(nm)/nk)
                          + (k + 0.5)*log(nk*_u.btrd.r/(k+1))
                          - fc(k)
                          - fc(_t - k))
                {
                    return k;
                } else {
                    continue;
                }
            }
        }
    }

    template<class URNG>
    IntType invert(IntType t, RealType p, URNG& urng) const
    {
        RealType q = 1 - p;
        RealType s = p / q;
        RealType a = (t + 1) * s;
        RealType r = _u.q_n;
        RealType u = uniform_01<RealType>()(urng);
        IntType x = 0;
        while(u > r) {
            u = u - r;
            ++x;
            RealType r1 = ((a/x) - s) * r;
            // If r gets too small then the round-off error
            // becomes a problem.  At this point, p(i) is
            // decreasing exponentially, so if we just call
            // it 0, it's close enough.  Note that the
            // minimum value of q_n is about 1e-7, so we
            // may need to be a little careful to make sure that
            // we don't terminate the first time through the loop
            // for float.  (Hence the test that r is decreasing)
            if(r1 < std::numeric_limits<RealType>::epsilon() && r1 < r) {
                break;
            }
            r = r1;
        }
        return x;
    }

    // parameters
    IntType _t;
    RealType _p;

    // common data
    IntType m;

    union {
        // for btrd
        struct {
            RealType r;
            RealType nr;
            RealType npq;
            RealType b;
            RealType a;
            RealType c;
            RealType alpha;
            RealType v_r;
            RealType u_rv_r;
        } btrd;
        // for inversion
        RealType q_n;
    } _u;

    /// @endcond
};

}

// backwards compatibility
using random::binomial_distribution;

}

#include <boost/random/detail/enable_warnings.hpp>

#endif


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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 NETKET_ADADELTA_HPP
#define NETKET_ADADELTA_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class AdaDelta : public AbstractOptimizer {
  int npar_;

  double rho_;
  double epscut_;

  Eigen::VectorXd Eg2_;
  Eigen::VectorXd Edx2_;

  const Complex I_;

 public:
  // Json constructor
  explicit AdaDelta(double rho = 0.95, double epscut = 1.0e-7)
      : rho_(rho), epscut_(epscut), I_(0, 1) {
    npar_ = -1;

    PrintParameters();
  }

  // Json constructor
  explicit AdaDelta(const json &pars) : I_(0, 1) {
    npar_ = -1;

    from_json(pars);
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Adadelta optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Rho = " << rho_ << std::endl;
    InfoMessage() << "Epscut = " << epscut_ << std::endl;
  }

  void Init(const Eigen::VectorXd &pars) override {
    npar_ = pars.size();
    Eg2_.setZero(npar_);
    Edx2_.setZero(npar_);
  }

  void Init(const Eigen::VectorXcd &pars) override {
    npar_ = 2 * pars.size();
    Eg2_.setZero(npar_);
    Edx2_.setZero(npar_);
  }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) override {
    assert(npar_ > 0);

    Eg2_ = rho_ * Eg2_ + (1. - rho_) * grad.cwiseAbs2();

    Eigen::VectorXd Dx(npar_);

    for (int i = 0; i < npar_; i++) {
      Dx(i) = -std::sqrt(Edx2_(i) + epscut_) * grad(i);
      Dx(i) /= std::sqrt(Eg2_(i) + epscut_);
      pars(i) += Dx(i);
    }

    Edx2_ = rho_ * Edx2_ + (1. - rho_) * Dx.cwiseAbs2();
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXd &pars) override {
    Update(Eigen::VectorXd(grad.real()), pars);
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXcd &pars) override {
    assert(npar_ == 2 * pars.size());

    Eigen::VectorXd Dx(npar_);

    for (int i = 0; i < pars.size(); i++) {
      Eg2_(2 * i) =
          rho_ * Eg2_(2 * i) + (1. - rho_) * std::pow(grad(i).real(), 2);
      Eg2_(2 * i + 1) =
          rho_ * Eg2_(2 * i + 1) + (1. - rho_) * std::pow(grad(i).imag(), 2);

      Dx(2 * i) = -std::sqrt(Edx2_(2 * i) + epscut_) * grad(i).real();
      Dx(2 * i + 1) = -std::sqrt(Edx2_(2 * i + 1) + epscut_) * grad(i).imag();
      Dx(2 * i) /= std::sqrt(Eg2_(2 * i) + epscut_);
      Dx(2 * i + 1) /= std::sqrt(Eg2_(2 * i + 1) + epscut_);

      pars(i) += Dx(2 * i);
      pars(i) += I_ * Dx(2 * i + 1);

      Edx2_(2 * i) = rho_ * Edx2_(2 * i) + (1. - rho_) * std::pow(Dx(2 * i), 2);
      Edx2_(2 * i + 1) =
          rho_ * Edx2_(2 * i + 1) + (1. - rho_) * std::pow(Dx(2 * i + 1), 2);
    }
  }

  void Reset() override {
    Eg2_ = Eigen::VectorXd::Zero(npar_);
    Edx2_ = Eigen::VectorXd::Zero(npar_);
  }

  void from_json(const json &pars) {
    // DEPRECATED (to remove for v2.0.0)
    std::string section = "Optimizer";
    if (!FieldExists(pars, section)) {
      section = "Learning";
    }
    rho_ = FieldOrDefaultVal(pars[section], "Rho", 0.95);
    epscut_ = FieldOrDefaultVal(pars[section], "Epscut", 1.0e-7);
  }
};

}  // namespace netket

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file flatextrapolation.hpp
    \brief flat interpolation decorator
    \ingroup math
*/

#ifndef quantext_flat_extrapolation_hpp
#define quantext_flat_extrapolation_hpp

#include <ql/math/interpolation.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Flat extrapolation given a base interpolation
/*! \ingroup math
 */
class FlatExtrapolation : public Interpolation {
private:
    class FlatExtrapolationImpl : public Interpolation::Impl {

    public:
        FlatExtrapolationImpl(const boost::shared_ptr<Interpolation>& i) : i_(i) {}
        void update() { i_->update(); }
        Real xMin() const { return i_->xMin(); }
        Real xMax() const { return i_->xMax(); }
        std::vector<Real> xValues() const { QL_FAIL("not implemented"); }
        std::vector<Real> yValues() const { QL_FAIL("not implemented"); }
        bool isInRange(Real x) const { return i_->isInRange(x); }
        Real value(Real x) const {
            Real tmp = std::max(std::min(x, i_->xMax()), i_->xMin());
            return i_->operator()(tmp);
        }
        Real primitive(Real x) const {
            if (x >= i_->xMin() && x <= i_->xMax()) {
                return i_->primitive(x);
            }
            if (x < i_->xMin()) {
                return i_->primitive(i_->xMin()) - (i_->xMin() - x);
            } else {
                return i_->primitive(i_->xMax()) + (x - i_->xMax());
            }
        }
        Real derivative(Real x) const {
            if (x > i_->xMin() && x < i_->xMax()) {
                return i_->derivative(x);
            } else {
                // that is the left derivative for xmin and
                // the right derivative for xmax
                return 0.0;
            }
        }
        Real secondDerivative(Real x) const {
            if (x > i_->xMin() && x < i_->xMax()) {
                return i_->secondDerivative(x);
            } else {
                // that is the left derivative for xmin and
                // the right derivative for xmax
                return 0.0;
            }
        }

    private:
        const boost::shared_ptr<Interpolation> i_;
    };

public:
    FlatExtrapolation(const boost::shared_ptr<Interpolation>& i) {
        impl_ = boost::make_shared<FlatExtrapolationImpl>(i);
        impl_->update();
    }
};

//! %Linear-interpolation and flat extrapolation factory and traits
class LinearFlat {
public:
    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return FlatExtrapolation(boost::make_shared<LinearInterpolation>(xBegin, xEnd, yBegin));
    }
    static const bool global = false;
    static const Size requiredPoints = 2;
};

//! %Linear-interpolation and flat extrapolation factory and traits
class LogLinearFlat {
public:
    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return FlatExtrapolation(boost::make_shared<LogLinearInterpolation>(xBegin, xEnd, yBegin));
    }
    static const bool global = false;
    static const Size requiredPoints = 2;
};

//! Hermite interpolation and flat extrapolation factory and traits
class HermiteFlat {
public:
     template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
         return FlatExtrapolation(boost::make_shared<Parabolic>(xBegin, xEnd, yBegin));
     }
     static const bool global = false;
     static const Size requiredPoints = 2;
};

//! Cubic interpolation and flat extrapolation factory and traits
class CubicFlat {
public:
    CubicFlat(
        QuantLib::CubicInterpolation::DerivativeApprox da = QuantLib::CubicInterpolation::Kruger,
        bool monotonic = false,
        QuantLib::CubicInterpolation::BoundaryCondition leftCondition = QuantLib::CubicInterpolation::SecondDerivative,
        QuantLib::Real leftConditionValue = 0.0,
        QuantLib::CubicInterpolation::BoundaryCondition rightCondition = QuantLib::CubicInterpolation::SecondDerivative,
        QuantLib::Real rightConditionValue = 0.0)
        : da_(da), monotonic_(monotonic), leftType_(leftCondition), rightType_(rightCondition),
          leftValue_(leftConditionValue), rightValue_(rightConditionValue) {}

    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return FlatExtrapolation(boost::make_shared<CubicInterpolation>(
            xBegin, xEnd, yBegin, da_, monotonic_, leftType_, leftValue_, rightType_, rightValue_));
    }

    static const bool global = true;
    static const Size requiredPoints = 2;

private:
    QuantLib::CubicInterpolation::DerivativeApprox da_;
    bool monotonic_;
    QuantLib::CubicInterpolation::BoundaryCondition leftType_;
    QuantLib::CubicInterpolation::BoundaryCondition rightType_;
    QuantLib::Real leftValue_;
    QuantLib::Real rightValue_;
};

} // namespace QuantExt

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
//
// 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.
//---------------------------------------------------------------------------//

#define PROVING_KEY_FILE "provkey.bin"
#define VERIFICATION_KEY_FILE "verifkey.bin"
#define BIG_PROOF_FILE "big_proof.bin"
#define PROOF_FILE "proof.bin"
#define PRIMARY_INPUT_FILE "primary_input.bin"

#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "detail/r1cs_examples.hpp"
#include "detail/sha256_component.hpp"

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/crypto3/zk/components/component.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>
// #include <nil/crypto3/zk/snark/components/basic_components.hpp>

// #include <nil/crypto3/zk/snark/relations/constraint_satisfaction_problems/r1cs.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::zk::components;
using namespace nil::crypto3::algebra;

#include <iostream>
#include <string>
#include "picosha2.hpp"

using namespace std;

typedef algebra::curves::bls12<381> curve_type;
typedef typename curve_type::scalar_field_type field_type;
typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

using std::string;
using std::cout;
using std::endl;

using curve_type = curves::bls12<381>;
using field_type = typename curve_type::scalar_field_type;

// must be done here, after definition of curve_type and field_type
#include "utils.hpp"

void build_circuit ( blueprint<field_type> &bp, std::vector<uint32_t> &passphrase32 ){

  blueprint_variable<field_type> public_pubkey0;
  public_pubkey0.allocate( bp );

  blueprint_variable<field_type> public_pubkey1;
  public_pubkey1.allocate( bp );

  blueprint_variable<field_type> public_pubkey2;
  public_pubkey2.allocate( bp );

  blueprint_variable<field_type> public_pubkey3;
  public_pubkey3.allocate( bp );

  blueprint_variable<field_type> secret_pubkey0;
  secret_pubkey0.allocate( bp );

  blueprint_variable<field_type> secret_pubkey1;
  secret_pubkey1.allocate( bp );

  blueprint_variable<field_type> secret_pubkey2;
  secret_pubkey2.allocate( bp );

  blueprint_variable<field_type> secret_pubkey3;
  secret_pubkey3.allocate( bp );

  blueprint_variable<field_type> secret_hash0;
  secret_hash0.allocate( bp );

  blueprint_variable<field_type> secret_hash1;
  secret_hash1.allocate( bp );

  blueprint_variable<field_type> secret_hash2;
  secret_hash2.allocate( bp );

  blueprint_variable<field_type> secret_hash3;
  secret_hash3.allocate( bp );

  bp.set_input_sizes(4);

  // This sets up the blueprint variables
  // so that the first one (out) represents the public
  // input and the rest is private input

  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey0 , 1, public_pubkey0));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey1 , 1, public_pubkey1));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey2 , 1, public_pubkey2));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey3 , 1, public_pubkey3));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[0] , 1, secret_hash0));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[1] , 1, secret_hash1));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[2] , 1, secret_hash2));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[3] , 1, secret_hash3));
}

void prepare_circuit ( std::string passphrase ){

  using curve_type = curves::bls12<381>;
  using field_type = typename curve_type::scalar_field_type;

  std::vector<uint32_t> passphrase32(4);
  uint32s_of_passphrase( passphrase32, passphrase );

  blueprint<field_type> bp;
  build_circuit( bp, passphrase32 );

  const r1cs_constraint_system<field_type> constraint_system =
    bp.get_constraint_system();
  const typename r1cs_gg_ppzksnark<curve_type>::keypair_type keypair =
    generate<r1cs_gg_ppzksnark<curve_type>>(constraint_system);

  save_proving_key( PROVING_KEY_FILE, keypair.first ) ;
  save_verification_key( VERIFICATION_KEY_FILE , keypair.second );

}

void use_circuit( std::string passphrase, std::string pubkey ){

  std::vector<uint32_t> passphrase32(4);
  std::vector<uint32_t> pubkey32(4);

  if( pubkey.size() != 64 ){
    cerr << "Wrong size for pubkey, should be 64 hexa chars" << endl ;
    exit(2) ;
  }

  uint32s_of_passphrase( passphrase32, passphrase );
  uint32s_of_pubkey( pubkey32, pubkey );

  // public
  snark::r1cs_variable_assignment<field_type> primary_assignment;
  for( int i = 0; i < 4; i++){
    primary_assignment.push_back( pubkey32 [i] );
  }
  typename scheme_type::primary_input_type primary_input
    ( primary_assignment.begin(), primary_assignment.begin() + 4 );

  // private
  snark::r1cs_variable_assignment<field_type> auxiliary_assignment;
  for( int i = 0; i < 4; i++){
    auxiliary_assignment.push_back( pubkey32 [i] );
  }
  for( int i = 0; i < 4; i++){
    auxiliary_assignment.push_back( passphrase32 [i] );
  }

  typename scheme_type::auxiliary_input_type auxiliary_input
    ( auxiliary_assignment.begin(), auxiliary_assignment.begin() + 8 );

  typename scheme_type::proving_key_type pk =
    load_proving_key( PROVING_KEY_FILE );

  const typename r1cs_gg_ppzksnark<curve_type>::proof_type proof =
    prove<r1cs_gg_ppzksnark<curve_type>>( pk,  primary_input, auxiliary_input );

  const typename r1cs_gg_ppzksnark<curve_type>::verification_key_type vk =
    load_verification_key( VERIFICATION_KEY_FILE );

  bool verified =
    verify<r1cs_gg_ppzksnark<curve_type>>(vk, primary_input, proof);

  if( !verified ){
    cerr << "Invalid proof" << endl;
    exit(2) ;
  }

  save_primary_input( PRIMARY_INPUT_FILE, primary_input );
  save_big_proof( BIG_PROOF_FILE, proof, primary_input, vk );
  save_proof( PROOF_FILE, proof );

}

void print_usage(std::ostream &cout, std::string command )
{
    cout << command << " SUBCOMMAND [ARGUMENTS]" << endl ;
    cout << "Available subcommands:" << endl ;
    cout << "  * prepare PASSPHRASE : output circuit for PASSPHRASE to 'provkey.bin' and 'verifkey.bin'" << endl ;
    cout << "  * prove PASSPHRASE PUBKEY : generate 'proof.bin', 'verifkey.bin', 'variables.bin' and 'big_proof.bin'" << endl ;
    cout << "Arguments:" << endl ;
    cout << "  PASSPHRASE: an ASCII string that you have to memorize" << endl ;
    cout << "  PUBKEY: the Hexadecimal representation of your pubkey (without 0x)" << endl ;

}

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

  if( argc == 1 ){
    print_usage ( cout, argv[0] ) ;
    return 0 ;
  }

  std::string subcommand = argv[1] ;
  if( subcommand == "prepare" ){
    cerr << "prepare" << endl ;
    if( argc != 3 ){
      cerr << "Bad number of arguments:" << endl ;
      print_usage ( cerr, argv[0] ) ;
      return 1;
    }

    prepare_circuit( argv[2] );
    return 0 ;
  } else
  if( subcommand == "prove" ){
    cerr << "prove" << endl ;
    if( argc != 4 ){
      cerr << "Bad number of arguments:" << endl ;
      print_usage ( cerr, argv[0] ) ;
      return 1;
    }
    use_circuit( argv[2], argv[3] );
    return 0 ;
  } else {
    cerr << "Unknown subcommand: " << subcommand << endl;
    return 2 ;
  }

}

/*
*/


#ifndef GRAPHS_UTILS_HPP
#define GRAPHS_UTILS_HPP

#include <utility>
#include <vector>
#include <fstream>
#include <filesystem>
#include <numeric>

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>

#include <osmium/osm/node_ref.hpp>
#include <osmium/osm/way.hpp>

namespace graphs {
using Distance = double;
using Angle = long double;
using Location = std::pair<Angle, Angle>;
using Locations = std::vector<Location>;

template<typename T>
bool serialize(const std::string& filename, T&& data) {
    std::ofstream binary { filename, std::ios::out | std::ios::binary | std::ios::app };
    boost::archive::binary_oarchive archive { binary, boost::archive::no_header };
    archive << data;
    binary.close();
    return true;
}

template<typename T>
bool deserialize(const std::string& filename, T&& data) {
    if (!std::filesystem::exists(filename)) { return false; }
    std::ifstream binary { filename, std::ios::binary };
    boost::archive::binary_iarchive archive { binary, boost::archive::no_header };
    archive >> data;
    binary.close();
    return true;
}

/**
 * Factory method for Position.
 */
inline auto make_pos(const osmium::NodeRef& node) -> Location {
    return { node.lat(), node.lon() };
}

/**
 * Determines the great-circle distance between two points given their longitudes and latitudes.
 *
 * @param x, y OSM nodes with corresponding coordinates.
 * @return Distance between nodes.
 */
inline auto haversine(const Location& x, const Location& y) -> Distance {
    const auto[lat_1, lon_1] = x;
    const auto[lat_2, lon_2] = y;
    constexpr long double R = 6'371'000;

    // Convert to radians
    const auto phi1 = lat_1 * M_PI / 180;
    const auto phi2 = lat_2 * M_PI / 180;
    const auto d_phi = (lat_2 - lat_1) * M_PI / 180;
    const auto d_lambda = (lon_2 - lon_1) * M_PI / 180;

    // Square of half the chord length between the objects
    const auto a = std::pow(std::sin(d_phi / 2), 2) +
                   std::cos(phi1) * std::cos(phi2) * std::pow(std::sin(d_lambda / 2), 2);
    // Angular distance in radians
    const auto c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
    return R * c;
}

/**
 * Determines the geographical center of a building consisting of ambient nodes.
 *
 * @param nodes List of nodes of an OSM way.
 * @return Geocenter described by a pair of latitude and longitude respectively.
 */
inline auto barycenter(const osmium::WayNodeList& nodes) -> Location {
    const auto lat = std::accumulate(nodes.cbegin(), nodes.cend(), static_cast<long double>(0),
                                     [](auto lhs, const auto& node) { return lhs + node.lat(); });
    const auto lon = std::accumulate(nodes.cbegin(), nodes.cend(), static_cast<long double>(0),
                                     [](auto lhs, const auto& node) { return lhs + node.lon(); });
    const auto num = nodes.size();

    return { lat / num, lon / num };
}

inline auto barycenter(const Locations& locations) -> Location {
    const auto
        lat = std::accumulate(locations.cbegin(), locations.cend(), static_cast<long double>(0),
                              [](auto lhs, const auto& node) { return lhs + node.first; });
    const auto
        lon = std::accumulate(locations.cbegin(), locations.cend(), static_cast<long double>(0),
                              [](auto lhs, const auto& node) { return lhs + node.second; });
    const auto num = locations.size();

    return { lat / num, lon / num };
}
} // namespace graph

#endif // GRAPHS_UTILS_HPP


// chapter 3

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

typedef mpl::vector_c<int, 1, 0, 0, 0> mass;
typedef mpl::vector_c<int, 0, 1, 0, 0> length;
typedef mpl::vector_c<int, 0, 0, 1, 0> time;
typedef mpl::vector_c<int, 0, 1, -2, 0> accel;
typedef mpl::vector_c<int, 1, 1, -2, 0> force;
typedef mpl::vector_c<int, 0, 0, 0, 0> scalar;

template <class T, class Dimension>
struct Quantity
{
	Quantity(T x) : m_value(x) {}
	template <class OtherDimension>
	Quantity( Quantity<T, OtherDimension> &rhs) : m_value(rhs.value())
	{
		BOOST_STATIC_ASSERT((
			mpl::equal< Dimension, OtherDimension >::type::value
			));
	}

	T m_value;
	T value() const { return m_value; }

	template<class OtherDimension>
	Quantity<T, Dimension>& operator=(Quantity<T, OtherDimension> &rhs)
	{
		BOOST_STATIC_ASSERT((
			mpl::equal< Dimension, OtherDimension >::type::value
			));

		if (this != &rhs)
		{
			m_value = rhs.value();
		}
		return *this;
	}

	Quantity<T, Dimension> operator+(const Quantity<T, Dimension> &rhs)
	{
		return Quantity<T, Dimension>(value() + rhs.value());
	}
	Quantity<T, Dimension> operator-(const Quantity<T, Dimension> &rhs)
	{
		return Quantity<T, Dimension>(value() - rhs.value());
	}

	struct minus_f
	{
		template <class T1, class T2>
		struct apply : mpl::minus<T1,T2> {};
	};

	template <class OtherDimension>
	Quantity<
		T,
		typename mpl::transform<Dimension, OtherDimension, mpl::plus<_1,_2> >::type
	>
		operator*(const Quantity<T, OtherDimension> &rhs)
	{
		typedef typename mpl::transform<Dimension, OtherDimension, mpl::plus<_1,_2> >::type dim;
		return Quantity<T, dim>(value() * rhs.value());
	}

	template <class OtherDimension>
	Quantity<T, typename mpl::transform<Dimension, OtherDimension, minus_f>::type >
		operator/(const Quantity<T, OtherDimension> &rhs)
	{
		typedef typename mpl::transform<Dimension, OtherDimension, minus_f>::type dim;
		return Quantity<T, dim>(value() / rhs.value());
	}

};

void main()
{
	Quantity<float, mass> m(3.f);
	Quantity<float, mass> m2(4.f);
	Quantity<float, length> l(5.f);
	Quantity<float, accel> a(5.f);
	Quantity<float, mass> m3 = m + m2;
	Quantity<float, mass> m4 = m - m2;
	Quantity<float, force> f = m * a;

	m4 = f/a;

	BOOST_STATIC_ASSERT((
		mpl::plus<
		mpl::int_<2>
		,mpl::int_<3>
		,mpl::int_<4>
		>::type::value == 9
		));

}


/**
 * @file  CableTensionFactor.cpp
 * @brief Cable tension factor: relates cable tension, two mounting points, and
 * resultant forces
 * @author Frank Dellaert
 * @author Gerry Chen
 */

#include "CableTensionFactor.h"

#include <gtdynamics/utils/DynamicsSymbol.h>

#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

#include <boost/optional.hpp>
#include <iostream>
#include <string>

using namespace gtsam;

namespace gtdynamics {

/******************************************************************************/
Vector6 CableTensionFactor::computeWrench(
    double t, const Pose3 &wTx, boost::optional<Matrix &> H_t,
    boost::optional<Matrix &> H_wTx) const {
  // Jacobians: cable direction
  Matrix33 dir_H_wPb;
  Matrix36 wPb_H_wTx;
  // Jacobians: force to wrench conversion
  Matrix31 wf_H_t;
  Matrix33 wf_H_dir;
  Matrix33 xf_H_wf;
  Matrix33 xf_H_wRx;
  Matrix63 H_xf;
  Matrix33 xm_H_xf;  // = H_xf.topRows<3>(); TODO(gerry): pointer?

  // cable direction
  Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0);
  Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : 0);
  // force->wrench
  Vector3 wf = -t * dir;
  if (H_t) wf_H_t = -dir;
  if (H_wTx) wf_H_dir = -t * I_3x3;
  Vector3 xf = wTx.rotation().unrotate(wf,  // force in the EE frame
                                       H_wTx ? &xf_H_wRx : 0,
                                       (H_t || H_wTx) ? &xf_H_wf : 0);
  Vector3 xm = cross(xPb_, xf,     // moment in the EE frame
                     boost::none,  //
                     (H_t || H_wTx) ? &xm_H_xf : 0);

  Vector6 F = (Vector6() << xm, xf).finished();
  if (H_t || H_wTx) H_xf << xm_H_xf, I_3x3;
  if (H_t) *H_t = H_xf * xf_H_wf * wf_H_t;
  if (H_wTx) {
    *(H_wTx) = H_xf * xf_H_wf * wf_H_dir * dir_H_wPb * wPb_H_wTx;
    H_wTx->leftCols<3>() += H_xf * xf_H_wRx;
  }
  return F;
}

/******************************************************************************/
Vector6 CableTensionFactor::computeWrenchUsingAdjoint(
    double t, const Pose3 &wTx, boost::optional<Matrix &> H_t,
    boost::optional<Matrix &> H_wTx) const {
  // Jacobians: cable direction
  Matrix33 dir_H_wPb;
  Matrix36 wPb_H_wTx;
  // Jacobians: force to wrench conversion
  Matrix61 wF_H_t;
  Matrix63 wF_H_dir;

  // cable direction
  Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0);
  Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : 0);
  // force->wrench
  Pose3 bTx = Pose3(wTx.rotation().inverse(), xPb_).inverse();
  Vector6 wF = (Vector6() << 0, 0, 0, -t * dir).finished();
  if (H_t) wF_H_t = (Matrix61() << 0, 0, 0, -dir).finished();
  if (H_wTx) wF_H_dir = (Matrix63() << Z_3x3, -t * I_3x3).finished();
  Vector6 F = bTx.AdjointMap().transpose() * wF;
  // TODO(gerry): find jacobian of adjoint map
  return F;
}

}  // namespace gtdynamics


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Battin, R.H. An Introduction to the Mathematics and Methods of Astrodynamics,
 *          AIAA Education Series, 1999.
 *      Izzo, D. lambert_problem.h, keptoolbox.
 *
 *    Notes
 *      This code is an implementation of the method developed by Dario Izzo from ESA/ACT and
 *      publicly available at: http://keptoolbox.sourceforge.net/.
 *      After verification and validation, it was proven that this algorithm is faster and more
 *      robust than the implemented Lancaster & Blanchard and Gooding method. Notably, this method
 *      does not suffer from the near-pi singularity (pi-transfers are by nature singular).
 *
 */

#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterIzzo.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"

#include <Eigen/Geometry>

namespace tudat
{
namespace mission_segments
{

//! Execute Lambert targeting solver.
void LambertTargeterIzzo::execute( )
{
    // Call Izzo's Lambert targeting routine.
    solveLambertProblemIzzo( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                             timeOfFlight, gravitationalParameter, cartesianVelocityAtDeparture,
                             cartesianVelocityAtArrival, isRetrograde_, convergenceTolerance_,
                             maximumNumberOfIterations_ );
}

//! Get radial velocity at departure.
double LambertTargeterIzzo::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterIzzo::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterIzzo::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterIzzo::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival
            = angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterIzzo::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat