/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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 BOOST_TEST_MAIN
#define BOOST_TEST_MODULE davidson_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/bseoperator_btda.h"
#include "votca/xtp/davidsonsolver.h"
#include "votca/xtp/eigen.h"
#include "votca/xtp/matrixfreeoperator.h"

using namespace votca::xtp;
using namespace votca;

Eigen::MatrixXd symm_matrix(Index N, double eps) {
  Eigen::MatrixXd matrix;
  matrix = eps * Eigen::MatrixXd::Random(N, N);
  Eigen::MatrixXd tmat = matrix.transpose();
  matrix = matrix + tmat;
  return matrix;
}

Eigen::MatrixXd init_matrix(Index N, double eps) {
  Eigen::MatrixXd matrix = Eigen::MatrixXd::Zero(N, N);
  for (Index i = 0; i < N; i++) {
    for (Index j = i; j < N; j++) {
      if (i == j) {
        matrix(i, i) = std::sqrt(static_cast<double>(1 + i));
      } else {
        matrix(i, j) = eps / std::pow(static_cast<double>(j - i), 2);
        matrix(j, i) = eps / std::pow(static_cast<double>(j - i), 2);
      }
    }
  }
  return matrix;
}

BOOST_AUTO_TEST_SUITE(davidson_test)

BOOST_AUTO_TEST_CASE(davidson_full_matrix) {

  Index size = 100;
  Index neigen = 10;
  double eps = 0.01;
  Eigen::MatrixXd A = init_matrix(size, eps);
  Logger log;
  DavidsonSolver DS(log);
  DS.solve(A, neigen);
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);
  if (!check_eigenvalues) {
    std::cout << "ref" << std::endl;
    std::cout << es.eigenvalues().head(neigen).transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << DS.eigenvalues().transpose() << std::endl;
  }

  BOOST_CHECK_EQUAL(check_eigenvalues, 1);
}

BOOST_AUTO_TEST_CASE(davidson_full_matrix_large) {

  Index size = 400;
  Index neigen = 10;
  double eps = 0.01;
  Eigen::MatrixXd A = init_matrix(size, eps);
  Logger log;
  DavidsonSolver DS(log);
  DS.solve(A, neigen);
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);
  if (!check_eigenvalues) {
    std::cout << "ref" << std::endl;
    std::cout << es.eigenvalues().head(neigen).transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << DS.eigenvalues().transpose() << std::endl;
  }

  BOOST_CHECK_EQUAL(check_eigenvalues, 1);
}

BOOST_AUTO_TEST_CASE(davidson_full_matrix_fail) {

  Index size = 100;
  Index neigen = 10;
  double eps = 0.01;
  Eigen::MatrixXd A = init_matrix(size, eps);

  Logger log;
  DavidsonSolver DS(log);
  DS.set_iter_max(1);
  DS.solve(A, neigen);
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);

  BOOST_CHECK_EQUAL(check_eigenvalues, 0);
}

class TestOperator : public MatrixFreeOperator {
 public:
  TestOperator() = default;
  Eigen::RowVectorXd OperatorRow(Index index) const override;

 private:
};

//  get a col of the operator
Eigen::RowVectorXd TestOperator::OperatorRow(Index index) const {
  Index lsize = this->size();
  Eigen::RowVectorXd row_out = Eigen::RowVectorXd::Zero(lsize);
  for (Index j = 0; j < lsize; j++) {
    if (j == index) {
      row_out(j) = std::sqrt(static_cast<double>(index + 1));
    } else {
      row_out(j) = 0.01 / std::pow(static_cast<double>(j - index), 2);
    }
  }
  return row_out;
}

BOOST_AUTO_TEST_CASE(davidson_matrix_free) {

  Index size = 100;
  Index neigen = 10;

  // Create Operator
  TestOperator Aop;
  Aop.set_size(size);

  Logger log;
  DavidsonSolver DS(log);
  DS.set_tolerance("normal");
  DS.set_size_update("safe");
  DS.solve(Aop, neigen);

  Eigen::MatrixXd A = Aop.get_full_matrix();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);

  BOOST_CHECK_EQUAL(check_eigenvalues, 1);
  if (!check_eigenvalues) {
    std::cout << "ref" << std::endl;
    std::cout << es.eigenvalues().head(neigen).transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << DS.eigenvalues().transpose() << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(davidson_matrix_free_large) {

  Index size = 400;
  Index neigen = 10;

  // Create Operator
  TestOperator Aop;
  Aop.set_size(size);

  Logger log;
  DavidsonSolver DS(log);
  DS.set_tolerance("normal");
  DS.set_size_update("safe");
  DS.solve(Aop, neigen);
  std::cout << log << std::endl;
  Eigen::MatrixXd A = Aop.get_full_matrix();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);

  BOOST_CHECK_EQUAL(check_eigenvalues, 1);
  if (!check_eigenvalues) {
    std::cout << "ref" << std::endl;
    std::cout << es.eigenvalues().head(neigen).transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << DS.eigenvalues().transpose() << std::endl;
  }
}

class BlockOperator : public MatrixFreeOperator {
 public:
  BlockOperator() = default;
  Eigen::MatrixXd OperatorBlock(Index row, Index col) const override;

  bool useRow() const override { return false; }
  bool useBlock() const override { return true; }
  Index getBlocksize() const override { return size() / 10; }

 private:
};

//  get a block of the operator
Eigen::MatrixXd BlockOperator::OperatorBlock(Index row, Index col) const {
  Index blocksize = getBlocksize();
  Eigen::MatrixXd block = Eigen::MatrixXd::Zero(blocksize, blocksize);
  Index blocdisttodiagonal = std::abs(row - col) * blocksize;
  for (Index i_col = 0; i_col < blocksize; i_col++) {
    for (Index i_row = 0; i_row < blocksize; i_row++) {
      block(i_row, i_col) =
          0.01 / std::pow(static_cast<double>(std::abs(i_row - i_col) +
                                              blocdisttodiagonal),
                          2);
    }
  }
  if (blocdisttodiagonal == 0) {
    for (Index i = 0; i < blocksize; i++) {
      block(i, i) = std::sqrt(static_cast<double>(row * blocksize + i + 1));
    }
  }

  return block;
}

BOOST_AUTO_TEST_CASE(davidson_matrix_free_block) {

  Index size = 100;
  Index neigen = 10;

  // Create Operator
  BlockOperator Aop;
  Aop.set_size(size);

  Logger log;
  DavidsonSolver DS(log);
  DS.set_tolerance("normal");
  DS.set_size_update("safe");

  Eigen::MatrixXd A = Aop.get_full_matrix();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);
  DS.solve(Aop, neigen);
  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);

  BOOST_CHECK_EQUAL(check_eigenvalues, 1);
  if (!check_eigenvalues) {
    std::cout << "ref" << std::endl;
    std::cout << es.eigenvalues().head(neigen).transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << DS.eigenvalues().transpose() << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(davidson_matrix_free_block_large) {

  Index size = 400;
  Index neigen = 10;

  // Create Operator
  BlockOperator Aop;
  Aop.set_size(size);

  Logger log;
  DavidsonSolver DS(log);
  DS.set_tolerance("normal");
  DS.set_size_update("safe");

  Eigen::MatrixXd A = Aop.get_full_matrix();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);
  DS.solve(Aop, neigen);
  auto lambda = DS.eigenvalues();
  auto lambda_ref = es.eigenvalues().head(neigen);
  bool check_eigenvalues = lambda.isApprox(lambda_ref, 1E-6);

  BOOST_CHECK_EQUAL(check_eigenvalues, 1);
  if (!check_eigenvalues) {
    std::cout << "ref" << std::endl;
    std::cout << es.eigenvalues().head(neigen).transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << DS.eigenvalues().transpose() << std::endl;
  }
}

Eigen::ArrayXi index_eval(Eigen::VectorXd ev, Index neigen) {

  Index nev = ev.rows();
  Index npos = nev / 2;

  Eigen::ArrayXi idx = Eigen::ArrayXi::Zero(npos);
  Index nstored = 0;

  // get only positives
  for (Index i = 0; i < nev; i++) {
    if (ev(i) > 0) {
      idx(nstored) = int(i);
      nstored++;
    }
  }

  // sort the epos eigenvalues
  std::sort(idx.data(), idx.data() + idx.size(),
            [&](Index i1, Index i2) { return ev[i1] < ev[i2]; });
  return idx.head(neigen);
}

Eigen::MatrixXd extract_eigenvectors(const Eigen::MatrixXd &V,
                                     const Eigen::ArrayXi &idx) {
  Eigen::MatrixXd W = Eigen::MatrixXd::Zero(V.rows(), idx.size());
  for (Index i = 0; i < idx.size(); i++) {
    W.col(i) = V.col(idx(i));
  }
  return W;
}

class HermitianBlockOperator : public MatrixFreeOperator {
 public:
  HermitianBlockOperator() = default;

  void attach_matrix(const Eigen::MatrixXd &mat);
  Eigen::RowVectorXd OperatorRow(Index index) const override;
  void set_diag(Index diag);
  Eigen::VectorXd diag_el;

 private:
  Eigen::MatrixXd _mat;
};

void HermitianBlockOperator::attach_matrix(const Eigen::MatrixXd &mat) {
  _mat = mat;
}

//  get a col of the operator
Eigen::RowVectorXd HermitianBlockOperator::OperatorRow(Index index) const {
  return _mat.row(index);
}

BOOST_AUTO_TEST_CASE(davidson_hamiltonian_matrix_free) {

  Index size = 60;
  Index neigen = 5;
  Logger log;

  // Create Operator
  HermitianBlockOperator Rop;
  Rop.set_size(size);
  Eigen::MatrixXd rmat = init_matrix(size, 0.01);
  Rop.attach_matrix(rmat);

  HermitianBlockOperator Cop;
  Cop.set_size(size);
  Eigen::MatrixXd cmat = symm_matrix(size, 0.01);
  Cop.attach_matrix(cmat);

  // create Hamiltonian operator
  HamiltonianOperator<HermitianBlockOperator, HermitianBlockOperator> Hop(Rop,
                                                                          Cop);

  DavidsonSolver DS(log);
  DS.set_tolerance("normal");
  DS.set_size_update("max");
  DS.set_matrix_type("HAM");
  DS.solve(Hop, neigen);
  auto lambda = DS.eigenvalues().real();
  std::sort(lambda.data(), lambda.data() + lambda.size());
  Eigen::MatrixXd H = Hop.get_full_matrix();

  Eigen::EigenSolver<Eigen::MatrixXd> es(H);
  Eigen::ArrayXi idx = index_eval(es.eigenvalues().real(), neigen);
  Eigen::VectorXd lambda_ref = idx.unaryExpr(es.eigenvalues().real());

  bool check_eigenvalues = lambda.isApprox(lambda_ref.head(neigen), 1E-6);
  if (!check_eigenvalues) {
    std::cout << "Davidson not converged after " << DS.num_iterations()
              << " iterations" << std::endl;
    std::cout << "Reference eigenvalues" << std::endl;
    std::cout << lambda_ref.head(neigen) << std::endl;
    std::cout << "Davidson eigenvalues" << std::endl;
    std::cout << lambda << std::endl;
    std::cout << "Residue norms" << std::endl;
    std::cout << DS.residues() << std::endl;
  }
  BOOST_CHECK_EQUAL(check_eigenvalues, 1);

  Eigen::MatrixXd evect_dav = DS.eigenvectors().real();
  Eigen::MatrixXd evect = es.eigenvectors().real();
  Eigen::MatrixXd evect_ref = extract_eigenvectors(evect, idx);

  bool check_eigenvectors =
      evect_ref.cwiseAbs2().isApprox(evect_dav.cwiseAbs2(), 0.001);
  BOOST_CHECK_EQUAL(check_eigenvectors, 1);
}

BOOST_AUTO_TEST_CASE(davidson_hamiltonian_matrix_free_large) {

  Index size = 120;
  Index neigen = 5;
  Logger log;
  // log.setReportLevel(TLogLevel::logDEBUG);

  // Create Operator
  HermitianBlockOperator Rop;
  Rop.set_size(size);
  Eigen::MatrixXd rmat = init_matrix(size, 0.01);
  Rop.attach_matrix(rmat);

  HermitianBlockOperator Cop;
  Cop.set_size(size);
  Eigen::MatrixXd cmat = symm_matrix(size, 0.01);
  Cop.attach_matrix(cmat);

  // create Hamiltonian operator
  HamiltonianOperator<HermitianBlockOperator, HermitianBlockOperator> Hop(Rop,
                                                                          Cop);

  DavidsonSolver DS(log);
  DS.set_tolerance("normal");
  DS.set_size_update("safe");
  DS.set_max_search_space(50);
  DS.set_matrix_type("HAM");
  DS.solve(Hop, neigen);
  std::cout << log;
  auto lambda = DS.eigenvalues().real();
  std::sort(lambda.data(), lambda.data() + lambda.size());
  Eigen::MatrixXd H = Hop.get_full_matrix();

  Eigen::EigenSolver<Eigen::MatrixXd> es(H);
  Eigen::ArrayXi idx = index_eval(es.eigenvalues().real(), neigen);
  Eigen::VectorXd lambda_ref = idx.unaryExpr(es.eigenvalues().real());

  bool check_eigenvalues = lambda.isApprox(lambda_ref.head(neigen), 1E-6);
  if (!check_eigenvalues) {
    std::cout << "Davidson not converged after " << DS.num_iterations()
              << " iterations" << std::endl;
    std::cout << "Reference eigenvalues" << std::endl;
    std::cout << lambda_ref.head(neigen) << std::endl;
    std::cout << "Davidson eigenvalues" << std::endl;
    std::cout << lambda << std::endl;
    std::cout << "Residue norms" << std::endl;
    std::cout << DS.residues() << std::endl;
  }
  BOOST_CHECK_EQUAL(check_eigenvalues, 1);

  Eigen::MatrixXd evect_dav = DS.eigenvectors().real();
  Eigen::MatrixXd evect = es.eigenvectors().real();
  Eigen::MatrixXd evect_ref = extract_eigenvectors(evect, idx);

  bool check_eigenvectors =
      evect_ref.cwiseAbs2().isApprox(evect_dav.cwiseAbs2(), 0.001);
  BOOST_CHECK_EQUAL(check_eigenvectors, 1);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __TRIANGULATE_HPP
/**
 * Triangulate
 *
 * Takes a list of polylines and separates them into complex polygons such that
 * no two overlap.  This is equivalent to the 2D arrangement of the input line
 * segments.  Each complex polygon is represented as a list of triangles and a
 * list of poly-lines forming the boundary.  The input polygons can be
 * self-intersecting.
 *
 * The result is computed using the constrained Delaunay triangulation (CDT).
 * To reject the extra triangles created by the CDT, the arrangement of all the
 * input is computed.  Triangles not bounded by this arrangement are rejected.
 * To associate output polygons with input polylines, the arrangement of each
 * input polyline is computed.
 *
 * Input: Polylines as list of points, one polyline per line, separated by
 * whitespace, with an arbitrary ID as the first token, e.g.
 *     0 x1 y1 x2 y2 x3 y3
 *     1 x1 y1 x2 y2 x3 y3 x4 y4 x5 y5
 *     2 x1 y1 x2 y2 x3 y3 x4 y4
 * The id can be anything as long a it does not contain whitespace.  The input
 * may be terminated either with "END" on one line or by ending the file.  Any
 * text after "END" is ignored.
 *
 * Output: Flattened whitespace-separated list of vertices, triangles, and
 * lines separated by "|".  Each complex polygon is on its own line.  The first
 * group on each line is the IDs of the input polygons that contain it, or
 * nothing.
 * Triangles and lines are indices into the list of vertices e.g.
 *     0 | x0 y0 x1 y1 x2 y2 | v0 v1 v2 | v0 v1 v0 v2 v1 v2
 *     1 2 | x0 y0 x1 y1 x2 y2 x3 y3 | v0 v1 v2 v0 v2 v3 | v0 v1 v1 v2 v2 v3 v3 v0
 */

#include <stdexcept>
#include <iostream>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/intersections.h>
#include <CGAL/Polygon_2.h>

#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arr_polyline_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_naive_point_location.h>

#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>

#include <CGAL/squared_distance_2.h>

using std::cout;
using std::cerr;
using std::cin;
using std::string;
using std::endl;
using std::vector;

// Kernel
typedef CGAL::Exact_predicates_exact_constructions_kernel K;
typedef CGAL::Polygon_2<K> Polygon_2;

// Arrangement
typedef CGAL::Arr_segment_traits_2<K>                      Segment_traits_2;
typedef CGAL::Arr_polyline_traits_2<Segment_traits_2>      Traits_2;
typedef Traits_2::Point_2                                  Point_2;
typedef Traits_2::Curve_2                                  Polyline_2;
typedef CGAL::Arrangement_2<Traits_2>                      Arrangement_2;
typedef CGAL::Arr_naive_point_location<Arrangement_2>      Point_location_2;
typedef Arrangement_2::Face_const_handle                   Arr_face_handle_2;

// Triangulations
typedef CGAL::Triangulation_vertex_base_2<K>                     Vb;
typedef CGAL::Constrained_triangulation_face_base_2<K>           Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>              TDS;
typedef CGAL::Exact_intersections_tag                            Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<K, TDS, Itag> CDT_parent;
typedef CGAL::Constrained_triangulation_plus_2<CDT_parent>       CDT;

// Wrapper class to hold variables
class Triangulate {
	public:
		void load_polylines(std::istream &input);
		void build_triangulation();
		void build_out_triangle_components();
		void print_out_triangle_components();

	private:

		/// INPUT

		// storage for polygon points
		vector<vector<Point_2> > polyline_points;

		// poly-lines submitted by user
		vector<Polyline_2> polylines;

		// arrangement of each input polygon (to handle self-intersecting inputs)
		vector<std::pair<string, Arrangement_2> > poly_arr;

		// arrangement of all polylines
		Arrangement_2 arr;

		// constrained delaunay triangulation (CDT) of all input polyglines
		CDT cdt;

		// triangles in the CDT
		vector<CDT::Face_handle> tris;

		// map faces to their index in the list
		std::map<CDT::Face_handle, size_t> tri2index;

		// triangle midpoints
		vector<Point_2> tri_midpoints;

		// output polygons (connected component of triangles) as a list of
		// triangle indices
		vector<vector<size_t> > out_triangle_components;
};

inline bool
arrangement_contains(const Arrangement_2 &arr, const Point_2 &p) {
	Point_location_2 pl(arr);
	CGAL::Object obj = pl.locate(p);
	Arr_face_handle_2 f;
	return (CGAL::assign(f, obj) && !f->is_unbounded());
}

#define __TRIANGULATE_HPP
#endif /* __TRIANGULATE_HPP */


/*-------------------------------------------------------------
Copyright 2019 Wenxin Liu, Kartik Mohta, Giuseppe Loianno

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.
--------------------------------------------------------------*/

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/cache.h>
#include <nav_msgs/Odometry.h>
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Dense>
#include <tuple>

namespace sdd_vio {

class ImuIntegrationNodelet : public nodelet::Nodelet
{
   private:
    void onInit(void);

    bool has_received_first_vio_;
    bool new_odom_available_;  // if true, then a new odom has been received and IMU should re-integrate from that point.
    ros::Time vio_odom_timestamp_;  // The timestamp of new odom, same as image timestamp
    double vio_odom_seq_;

    /* the odom from vio, these are with respect to the initial IMU frame */
    Eigen::Vector3f p_vio_;
    Eigen::Isometry3f T_vio_;
    Eigen::Quaternionf q_vio_;
    Eigen::Vector3f v_vio_;

    /* the actual current state (still relative to the initial IMU frame) that needs to be published */
    Eigen::Vector3f p_;
    Eigen::Isometry3f T_;
    Eigen::Quaternionf q_;
    Eigen::Vector3f v_;

    /* fixed params */
    Eigen::Isometry3f T_world_imu_;  // transformation between world frame and imu body frame
    Eigen::Vector3f b_w_;  // gyro bias
    Eigen::Vector3f b_a_;  // accelerometer bias
    Eigen::Vector3f g_;  // gravity in initial IMU frame

    ros::Time last_timestamp_;

    /* publisher to publish high rate odometry */
    ros::Publisher pub_odom_;
    /* subscriber to tightly-coupled vio odometry low frequency output */
    message_filters::Subscriber<nav_msgs::Odometry> sub_odom_;
    /* subscriber to IMU messages */
    message_filters::Subscriber<sensor_msgs::Imu> sub_imu_;
    /* IMU cache linked to subscriber */
    message_filters::Cache<sensor_msgs::Imu>* imu_cache_;

    /* publish odom at IMU rate */
    void imu_callback(const sensor_msgs::Imu::ConstPtr& imu);
    /* set flag for incoming vio odometry to trigger IMU re-integration */
    void vio_callback(const nav_msgs::Odometry::ConstPtr& odom);
    /* integrate IMU from last image pose to current */
    std::tuple<Eigen::Vector3f, Eigen::Vector3f, Eigen::Quaternionf> imuIntegrate(const std::vector<sensor_msgs::Imu::ConstPtr> &imu_vector) const;

};

void ImuIntegrationNodelet::onInit(void)
{
  ros::NodeHandle nh = getPrivateNodeHandle();

  /* initialization */
  p_vio_.setZero(3);
  T_vio_.setIdentity();
  q_vio_.setIdentity();
  v_vio_.setZero(3);

  p_.setZero(3);
  T_.setIdentity();
  q_.setIdentity();
  v_.setZero(3);

  b_w_.setZero(3);  // imu biases
  b_a_.setZero(3);
  g_ << 0,0,9.8;  // gravity
  T_world_imu_.setIdentity();
  Eigen::Matrix3f R_world_imu;
  R_world_imu << 1,0,0,
                0,-1,0,
                0,0,-1;
  T_world_imu_.linear() = R_world_imu;

  //std::cout<<"isometry T_: "<<T_.matrix()<<"\n";
  //std::cout<<"isometry T_world_imu_: "<<T_world_imu_.matrix()<<"\n";

  last_timestamp_ = ros::Time::now();

  new_odom_available_ = false;
  has_received_first_vio_ = false;

  /* initialize publisher */
  pub_odom_ = nh.advertise<nav_msgs::Odometry>("odom", 100);
  /* initialize vio odom subscriber */
  sub_odom_.subscribe(nh, "odom_topic", 100, ros::TransportHints().tcpNoDelay());
  sub_odom_.registerCallback(boost::bind(&ImuIntegrationNodelet::vio_callback, this, _1));
  /* initialize imu subscriber and cache callback */
  sub_imu_.subscribe(nh, "imu_topic", 500, ros::TransportHints().tcpNoDelay());
  imu_cache_ = new message_filters::Cache<sensor_msgs::Imu>(sub_imu_,500);
  imu_cache_->registerCallback(boost::bind(&ImuIntegrationNodelet::imu_callback, this, _1));

}

/* upon receiving an odom message, save the timestamp and odom info, set flag */
void ImuIntegrationNodelet::vio_callback(const nav_msgs::Odometry::ConstPtr &odom)
{

    new_odom_available_ = true;
    has_received_first_vio_ = true;
    vio_odom_timestamp_ = odom->header.stamp;
    vio_odom_seq_ = odom->header.seq;

    p_vio_(0) = odom->pose.pose.position.x;
    p_vio_(1) = odom->pose.pose.position.y;
    p_vio_(2) = odom->pose.pose.position.z;

    q_vio_.x() = odom->pose.pose.orientation.x;
    q_vio_.y() = odom->pose.pose.orientation.y;
    q_vio_.z() = odom->pose.pose.orientation.z;
    q_vio_.w() = odom->pose.pose.orientation.w;

    T_vio_.linear() = q_vio_.toRotationMatrix();
    T_vio_.translation() = p_vio_;

    v_vio_(0) = odom->twist.twist.linear.x;
    v_vio_(1) = odom->twist.twist.linear.y;
    v_vio_(2) = odom->twist.twist.linear.z;

}

void ImuIntegrationNodelet::imu_callback(const sensor_msgs::Imu::ConstPtr &imu)
{

    ros::Time current_timestamp = imu->header.stamp;

    /* raw observation */
    Eigen::Vector3f w, a;
    w(0) = imu->angular_velocity.x;
    w(1) = imu->angular_velocity.y;
    w(2) = imu->angular_velocity.z;
    a(0) = imu->linear_acceleration.x;
    a(1) = imu->linear_acceleration.y;
    a(2) = imu->linear_acceleration.z;

    /* Calibration of IMU */
    static const int total_num = 300;
    static int data_i = 0;  // the number of data processed
    static float w1 = 0, w2 = 0, w3 = 0, a1 = 0, a2 = 0, a3 = 0;
    if (data_i < total_num)  // take first 300 data - for calibration on bias
    {
        ROS_INFO("ImuIntegration nodelet - Imu Seq: [%d]", imu->header.seq);
        w1 += w(0);
        w2 += w(1);
        w3 += w(2);
        a1 += a(0) + g_(0);
        a2 += a(1) + g_(1);
        a3 += a(2) + g_(2);

        if (data_i == total_num-1)  // the last date collected for calibration
        {
            Eigen::Vector3f w_b(w1/total_num, w2/total_num, w3/total_num), a_b(a1/total_num, a2/total_num, a3/total_num);
            b_w_ = w_b;
            //b_a_ = a_b;
            std::cout<<"calibrated gyro bias: \n"<<w_b<<"\n";
            std::cout<<"calibrated accel bias: \n"<<a_b<<"\n";
//            throw std::runtime_error("Calibration done!");
        }
        data_i++;
    }

    if (new_odom_available_)
    {
        //std::cout<<"Received new odom from VIO! Getting imu data vector from buffer: \n";
        //std::cout<<"vio_odom_timestamp: "<<vio_odom_timestamp_<<"\n";
        //std::cout<<"current_timestamp: "<<current_timestamp<<"\n";
        std::vector<sensor_msgs::Imu::ConstPtr> imu_vector = imu_cache_->getInterval(vio_odom_timestamp_, current_timestamp);
        if (imu_vector.size() == 0)
          ROS_WARN_STREAM("Fail to retrieve IMU data between current frame and last frame.");
        std::tie(p_, v_, q_) = imuIntegrate(imu_vector);
        new_odom_available_ = false;

    }

    if (data_i != 1 && has_received_first_vio_)  // if not the first IMU data
    {
      /* getting current odometry in initial IMU frame */
      Eigen::Vector3f w_mBias = w - b_w_;
      Eigen::Vector3f a_mBias = a - b_a_;
      float delta_t = (current_timestamp - last_timestamp_).toSec();

      /* increment T_pub by one IMU measurement */
      Eigen::AngleAxisf aa_inc(w_mBias.norm() * delta_t, w_mBias.normalized());
      p_ = p_ + v_*delta_t + 0.5*g_*delta_t*delta_t + 0.5*q_.toRotationMatrix()*(a_mBias*delta_t*delta_t);
      v_ = v_ + g_*delta_t + q_.toRotationMatrix()*(a_mBias*delta_t);
      q_ = q_ * aa_inc;

      T_.linear() = q_.toRotationMatrix();
      T_.translation() = p_;
    }

    last_timestamp_ = current_timestamp;

    /* transform to world frame */
    Eigen::Isometry3f T_pub = T_world_imu_ * T_ * T_world_imu_.inverse();
    Eigen::Vector3f v_pub = T_world_imu_ * v_;
    Eigen::Quaternionf q_pub(T_pub.rotation());
    Eigen::Vector3f p_pub = T_pub.translation();

    /* publish odometry */
    nav_msgs::Odometry odom;
    std_msgs::Header header_msg;
    header_msg.frame_id = "world";
    header_msg.stamp = imu->header.stamp;  // use original imu message's header
    header_msg.seq = imu->header.seq;
    /* prepare cam msg to publish */
    odom.header = header_msg;
    odom.pose.pose.position.x = p_pub(0);
    odom.pose.pose.position.y = p_pub(1);
    odom.pose.pose.position.z = p_pub(2);
    odom.pose.pose.orientation.x = q_pub.x();
    odom.pose.pose.orientation.y = q_pub.y();
    odom.pose.pose.orientation.z = q_pub.z();
    odom.pose.pose.orientation.w = q_pub.w();
    odom.twist.twist.linear.x = v_pub(0);
    odom.twist.twist.linear.y = v_pub(1);
    odom.twist.twist.linear.z = v_pub(2);
    pub_odom_.publish(odom);

}

std::tuple<Eigen::Vector3f, Eigen::Vector3f, Eigen::Quaternionf> ImuIntegrationNodelet::imuIntegrate(const std::vector<sensor_msgs::Imu::ConstPtr>& imu_vector) const
{

    int num_measurement = imu_vector.size();
    Eigen::Vector3f v_proc = v_vio_;  // states during integration process
    Eigen::Vector3f p_proc = p_vio_;
    Eigen::Quaternionf q_proc = q_vio_;

    // not to use the last element
    for (int i=0;i<num_measurement-1;i++)
    {
        //std::cout<<"measurement "<<i<<": \n";
        Eigen::Vector3f w(imu_vector[i]->angular_velocity.x,
                          imu_vector[i]->angular_velocity.y,
                          imu_vector[i]->angular_velocity.z);  // angular velocity
        Eigen::Vector3f a(imu_vector[i]->linear_acceleration.x,
                          imu_vector[i]->linear_acceleration.y,
                          imu_vector[i]->linear_acceleration.z);  // linear acceleration
        /* remove bias */
        w = w - b_w_;
        a = a - b_a_;
        /* obtain delta t */
        const float delta_t = (imu_vector[i+1]->header.stamp - imu_vector[i]->header.stamp).toSec();

        /* update state */
        const Eigen::AngleAxisf aa_inc(w.norm() * delta_t, w.normalized());
        const Eigen::Quaternionf q_inc(aa_inc);
        p_proc = p_proc + v_proc*delta_t + 0.5*g_*delta_t*delta_t + 0.5*q_proc.toRotationMatrix()*(a*delta_t*delta_t);
        v_proc = v_proc + g_*delta_t + q_proc.toRotationMatrix()*(a*delta_t);
        q_proc = q_proc * q_inc;
    }

    return std::make_tuple(p_proc, v_proc, q_proc);
}

}

PLUGINLIB_EXPORT_CLASS(sdd_vio::ImuIntegrationNodelet, nodelet::Nodelet);


//
// Copyright 2013 Krzysztof Czainski
// Copyright 2020 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>

#include <boost/core/lightweight_test.hpp>

#include <cmath>

namespace gil = boost::gil;

// FIXME: Remove when https://github.com/boostorg/core/issues/38 happens
#define BOOST_GIL_TEST_IS_CLOSE(a, b, epsilon) BOOST_TEST(std::abs((a) - (b)) < (epsilon))

template <class F, class I>
struct test_map_fn
{
    using point_t = gil::point<F>;
    using result_type = point_t;
    result_type operator()(gil::point<I> const &src) const
    {
        F x = static_cast<F>(src.x) - 0.5;
        F y = static_cast<F>(src.y) - 0.5;
        return {x, y};
    }
};

namespace boost { namespace gil {

// NOTE: I suggest this could be the default behavior:

template <typename T>
struct mapping_traits;

template <class F, class I>
struct mapping_traits<test_map_fn<F, I>>
{
    using result_type = typename test_map_fn<F, I>::result_type;
};

template <class F, class I>
inline point<F> transform(test_map_fn<F, I> const &mf, point<I> const &src)
{
    return mf(src);
}

}} // namespace boost::gil

void test_bilinear_sampler_test()
{
    // R G B
    // G W R
    // B R G
    gil::rgb8_image_t img(3, 3);
    gil::rgb8_view_t v = view(img);
    v(0, 0) = v(1, 2) = v(2, 1) = gil::rgb8_pixel_t(128, 0, 0);
    v(0, 1) = v(1, 0) = v(2, 2) = gil::rgb8_pixel_t(0, 128, 0);
    v(0, 2) = v(2, 0) = gil::rgb8_pixel_t(0, 0, 128);
    v(1, 1) = gil::rgb8_pixel_t(128, 128, 128);

    gil::rgb8_image_t dims(4, 4);
    gil::rgb8c_view_t dv = gil::const_view(dims);

    test_map_fn<double, gil::rgb8_image_t::coord_t> mf;

    gil::resample_pixels(gil::const_view(img), gil::view(dims), mf, gil::bilinear_sampler());

    BOOST_TEST(gil::rgb8_pixel_t(128, 0, 0) == dv(0, 0));
    BOOST_TEST(gil::rgb8_pixel_t(64, 64, 0) == dv(0, 1));
    BOOST_TEST(gil::rgb8_pixel_t(0, 64, 64) == dv(0, 2));
    BOOST_TEST(gil::rgb8_pixel_t(0, 0, 128) == dv(0, 3));

    BOOST_TEST(gil::rgb8_pixel_t(64, 64, 0) == dv(1, 0));
    BOOST_TEST(gil::rgb8_pixel_t(64, 96, 32) == dv(1, 1));
    BOOST_TEST(gil::rgb8_pixel_t(64, 64, 64) == dv(1, 2));
    BOOST_TEST(gil::rgb8_pixel_t(64, 0, 64) == dv(1, 3));

    BOOST_TEST(gil::rgb8_pixel_t(0, 64, 64) == dv(2, 0));
    BOOST_TEST(gil::rgb8_pixel_t(64, 64, 64) == dv(2, 1));
    BOOST_TEST(gil::rgb8_pixel_t(96, 64, 32) == dv(2, 2));
    BOOST_TEST(gil::rgb8_pixel_t(64, 64, 0) == dv(2, 3));

    BOOST_TEST(gil::rgb8_pixel_t(0, 0, 128) == dv(3, 0));
    BOOST_TEST(gil::rgb8_pixel_t(64, 0, 64) == dv(3, 1));
    BOOST_TEST(gil::rgb8_pixel_t(64, 64, 0) == dv(3, 2));
    BOOST_TEST(gil::rgb8_pixel_t(0, 128, 0) == dv(3, 3));
}

int main()
{
    test_bilinear_sampler_test();

    return ::boost::report_errors();
}


//
//  GPUTSDFVolume.hpp
//  A GPU Based TSDF
//
//  Created by Dave on 11/03/2016.
//  Copyright © 2016 Sindesso. All rights reserved.
//

#ifndef TSDFVolumeCPU_hpp
#define TSDFVolumeCPU_hpp

#include "Camera.hpp"

//#include <Eigen/Core>

#include <cstdint>
#include <iostream>
#include <string>
#include <cmath>

typedef struct {
    float m11, m21, m31, m41;
    float m12, m22, m32, m42;
    float m13, m23, m33, m43;
    float m14, m24, m34, m44;
} Mat44CPU;

typedef struct {
    float m11, m21, m31;
    float m12, m22, m32;
    float m13, m23, m33;
} Mat33CPU;

struct float3cpu {
    float x, y, z;
};

struct int3cpu {
    int x, y, z;
};

struct dim3cpu {
    unsigned int x, y, z;
};

struct uchar3cpu {
    unsigned char x, y, z;
};

inline float f3_dot(const float3cpu& f1, const float3cpu& f2) {
    return f1.x * f2.x + f1.y * f2.y + f1.z * f2.z;
}

/**
 * Subtract one float3 from another
 * @param f1 First float3
 * @param f2 Second float3
 * @return f1 - f2
 */
inline float3cpu f3_sub( const float3cpu& f1, const float3cpu& f2 ) {
    float3cpu f{f1.x-f2.x, f1.y-f2.y, f1.z-f2.z};
    return f;
}

/**
 * Add one float3 to another
 * @param f1 First float3
 * @param f2 Second float3
 * @return f1 + f2
 */
inline float3cpu f3_add( const float3cpu& f1, const float3cpu& f2 ) {
    float3cpu f{f2.x+f1.x, f2.y+f1.y, f2.z+f1.z};
    return f;
}

/**
 * Multiply a float3 by a scalar value
 * @param s The scalar
 * @param vec The float3
 */
inline float3cpu f3_mul_scalar( const float& scalar, const float3cpu& vec ) {
    float3cpu f{vec.x * scalar, vec.y * scalar, vec.z * scalar};
    return f;
}

/**
 * Perform per element division of f1 by v2
 * @param f1 the first float3 (numerators)
 * @param f2 The second float3 (denominators)
 * @return f1 ./ f2
 */
inline float3cpu f3_div_elem(const float3cpu& f1, const float3cpu& f2 ) {
    float3cpu f{f1.x/f2.x, f1.y/f2.y, f1.z/f2.z};
    return f;
}

/**
 * Perform per element division of f1 by v2
 * @param f1 the first float3 (numerators)
 * @param i2 The second float3 (denominators)
 * @return f1 ./ f2
 */
inline float3cpu f3_div_elem( const float3cpu& f, const dim3cpu& i ) {
    float3cpu ff{f.x/i.x, f.y/i.y, f.z/i.z};
    return ff;
}

/**
 * Normalise a float3
 * @param vec The float3 vector
 */
inline float3cpu f3_normalise(float3cpu vec) {
    float l = sqrt( vec.x*vec.x+vec.y*vec.y+vec.z*vec.z);
    vec.x /= l;
    vec.y /= l;
    vec.z /= l;
    return vec;
}

/**
 * Normalise a float3
 * @param vec The float3 vector
 */
inline float f3_norm( float3cpu vec ) {
    return sqrt(vec.x*vec.x+vec.y*vec.y+vec.z*vec.z);
}

/**
 * Perform a matrix multiplication
 * @param mat33 The matrix
 * @param vec3 The vector
 * @return an output vector
 */
inline float3cpu m3_f3_mul( const Mat33CPU& mat, const float3cpu& vec ) {
    float3cpu result;
    result.x = mat.m11 * vec.x + mat.m12 * vec.y + mat.m13 * vec.z;
    result.y = mat.m21 * vec.x + mat.m22 * vec.y + mat.m23 * vec.z;
    result.z = mat.m31 * vec.x + mat.m32 * vec.y + mat.m33 * vec.z;
    return result;
}

/**
 * Perform a matrix multiplication
 * @param mat33 The matrix
 * @param vec3 The vector
 * @return an output vector
 */
inline float3cpu m3_i3_mul( const Mat33CPU& mat, const int3cpu& vec ) {
    float3cpu result;
    result.x = mat.m11 * vec.x + mat.m12 * vec.y + mat.m13 * vec.z;
    result.y = mat.m21 * vec.x + mat.m22 * vec.y + mat.m23 * vec.z;
    result.z = mat.m31 * vec.x + mat.m32 * vec.y + mat.m33 * vec.z;
    return result;
}

/**
 * Project down into pixel coordinates
 * given the inv_pose and K matrices
 * plus a point in world coords
 */
inline int3cpu world_to_pixel( const Mat44CPU & inv_pose, const Mat33CPU& k, const float3cpu& point ) {
    float3cpu cam_point;
    cam_point.x = inv_pose.m11 * point.x + inv_pose.m12 * point.y + inv_pose.m13 * point.z + inv_pose.m14;
    cam_point.y = inv_pose.m21 * point.x + inv_pose.m22 * point.y + inv_pose.m23 * point.z + inv_pose.m24;
    cam_point.z = inv_pose.m31 * point.x + inv_pose.m32 * point.y + inv_pose.m33 * point.z + inv_pose.m34;
    float w = inv_pose.m41 * point.x + inv_pose.m42 * point.y + inv_pose.m43 * point.z + inv_pose.m44;
    cam_point.x /= w;
    cam_point.y /= w;
    cam_point.z /= w;

    float3cpu image_point {cam_point.x / cam_point.z, cam_point.y / cam_point.z, 1.0f};
    int3cpu pixel;
    pixel.x = static_cast<uint>( round( k.m11 * image_point.y + k.m12 * image_point.z + k.m13 ) );
    pixel.y = static_cast<uint>( round( k.m21 * image_point.y + k.m22 * image_point.z + k.m23 ) );
    pixel.z = static_cast<uint>( round( k.m31 * image_point.y + k.m32 * image_point.z + k.m33 ) ); // Should be 1

    return pixel;
}

class TSDFVolumeCPU {
public:
    struct DeformationNode {
        float3cpu  translation;
        float3cpu  rotation;
    };

    struct Float3 {
        float x;
        float y;
        float z;
        inline Float3( const float3cpu& rhs ) {
            x = rhs.x;
            y = rhs.y;
            z = rhs.z;
        }
        inline Float3( float fx=0.0f, float fy=0.0f, float fz=0.0f ) {
            x = fx;
            y = fy;
            z = fz;
        }
        inline operator float3cpu() const {return float3cpu{x, y, z}; }

        inline Float3 operator -( const Float3& rhs ) const {
            return float3cpu{x - rhs.x, y - rhs.y, z - rhs.z};
        }
        inline Float3 operator +( const Float3& rhs ) const {
            return float3cpu{x + rhs.x, y + rhs.y, z + rhs.z};
        }
        inline Float3 operator /( const float scalar ) const {
            return float3cpu{x / scalar, y / scalar, z / scalar};
        }
        inline Float3 operator *( const Float3& rhs ) const {
            return float3cpu{x * rhs.x, y * rhs.y, z * rhs.z};
        }
        inline float norm( ) const {
            return std::sqrt( x*x + y*y + z*z );
        }

    };

    struct Int3 {
        int16_t x;
        int16_t y;
        int16_t z;
    };

    struct  UInt3 {
        unsigned int x;
        unsigned int y;
        unsigned int z;

        inline UInt3( const dim3cpu& rhs ) : x{rhs.x}, y{rhs.y}, z{rhs.z} {};
        inline UInt3( uint32_t x, uint32_t y, uint32_t z ) : x{x}, y{y}, z{z} {};
        inline operator dim3cpu() const {return dim3cpu{x, y, z}; }
    };

    ~TSDFVolumeCPU();

    /**
    * Make a TSDFVolume with the given dimensions (voxels) and physcial dimensions
    * @param size The number of voxels in each X,Y and Z dimension
    * @param physical_size The size ( in mm ) of the space contained in the volume
    */
    TSDFVolumeCPU( const UInt3& size = UInt3{64, 64, 64},
                const Float3& physical_size = Float3 { 3000.0f, 3000.0f, 3000.0f} );

    /**
     * Make a TSDFVolume with the given dimensins and physical dimensions
     * @param volume_x X dimension in voxels
     * @param volume_y Y dimension in voxels
     * @param volume_z Z dimension in voxels
     * @param psize_x Physical size in X dimension in mm
     * @param psize_y Physical size in Y dimension in mm
     * @param psize_z Physical size in Z dimension in mm
     */
    TSDFVolumeCPU( uint16_t volume_x, uint16_t volume_y, uint16_t volume_z, float psize_x, float psize_y, float psize_z );

    /**
     * Load a TSDFVolume from the specified file. The volume must previously have been saved
     */
    TSDFVolumeCPU( const std::string& file_name );

    /**
     * Set the size of the volume. This will delete any existing values and resize the volume, clearing it when done.
     * Volume offset is maintained
     * @param volume_x X dimension in voxels
     * @param volume_y Y dimension in voxels
     * @param volume_z Z dimension in voxels
     * @param psize_x Physical size in X dimension in mm
     * @param psize_y Physical size in Y dimension in mm
     * @param psize_z Physical size in Z dimension in mm
     */
    void set_size( uint16_t volume_x, uint16_t volume_y, uint16_t volume_z, float psize_x, float psize_y, float psize_z);

    void set_truncation_distance(float d);

    /**
     * @return the size of this space in voxels.
     */
    inline UInt3 size( ) const { return (UInt3) m_size; }

    /**
     * @return the dimensions of each voxel in mm
     */
    inline Float3 voxel_size( ) const { return (Float3) m_voxel_size; }

    /**
     * @return the physical size of the volume in world coords (mm)
     */
    inline Float3 physical_size( ) const { return (Float3) m_physical_size; }

    /**
     * @return the truncation distance (mm)
     */
    inline float truncation_distance( ) const { return m_truncation_distance; }

    /**
     * Offset the TSDF volume in space by the given offset. By default, the bottom, left, front corner of
     * voxel (0,0,0) is at world coordinate (0,0,0). This moves that point to the new world coordinate by a
     * @param ox X offset in mm
     * @param oy Y offset in mm
     * @param oz Z offset in mm
     */
    inline void offset( float ox, float oy, float oz ) {
        m_offset.x = ox;
        m_offset.y = oy;
        m_offset.z = oz;
    }

    /**
     * @return the offset f the TSDF volume in space
     */
    inline Float3 offset( ) const { return (Float3) m_offset; }

    /**
     * Clear the voxel and weight data
     */
    void clear( );

#pragma mark - Data access

 /**
     *
     */
    inline size_t index( int x, int y, int z ) const {
        return x + (y * m_size.x) + (z * m_size.x * m_size.y);
    };

    /**
     * @return pointer to translation data
     */
    inline DeformationNode *  deformation() const {
        return m_deformation_nodes;
    }

    /**
     * Set the deformation data for this space
     * @param data Data in host memory space; Assumed to be vx*vy*vz DeformationNode
     */
    void set_deformation( DeformationNode *deformation);

    /**
     * Return pointer to distance data
     * @return Pointer to distance data
     */
    inline const float * distance_data() const {
        return m_distances;
    }

    /**
     * Set the distance data for the TSDF in one call
     * @param distance_data Pointer to enough floats to populate the TSFD
     */
    void set_distance_data(float * distance_data);

    /**
     * Return pointer to weight data
     * @return Pointer to weight data
     */
    inline const float * weight_data() const {
        return m_weights;
    }

    /**
     * Set the weight data for the TSDF in one call
     * @param weight_data Pointer to enough floats to populate the TSFD
     */
    void set_weight_data( const float * weight_data );

    /**
     * @return the global rotation of the TSDF deformation
     * as a vector of 3 Euler angles (X then Y then Z - also Tait-Bryan angles)
     */
    inline float3cpu global_rotation( ) const {
        return m_global_rotation;
    }

    /**
     * @return the global translation of the TSDF deformation
     */
    inline float3cpu global_translation( ) const {
        return m_global_translation;
    }

#pragma mark - Deform a set of points
    /**
     * Apply the volume's deformation field t the given set of points, modifying them in place
     */
    void deform_mesh( const int num_points, float3cpu * points ) const;

#pragma mark - Integrate new depth data
    /**
     * Integrate a range map into the TSDF
     * @param depth_map Pointer to width*height depth values where 0 is an invalid depth and positive values are expressed in mm
     * @param width The horiontal dimension of the depth_map
     * @param height The height of the depth_map
     * @param camera The camera from which the depth_map was taken
     */
    void integrate( const uint16_t * depth_map, uint32_t width, uint32_t height, const Camera & camera );

#pragma mark - Import/Export

    /**
     * Save the TSDF to file
     * @param The filename
     * @return true if the file saved OK otherwise false.
     */
    bool save_to_file( const std::string & file_name) const;

    /**
     * Load the given TSDF file
     * @param The filename
     * @return true if the file saved OK otherwise false.
     */
    bool load_from_file( const std::string & file_name);

#pragma mark - Rendering
    void raycast( uint16_t width, uint16_t height, const Camera& camera, Eigen::Matrix<float, 3, Eigen::Dynamic>& vertices, Eigen::Matrix<float, 3, Eigen::Dynamic>& normals ) const ;

private:
    /**
     * Deallocate storage for this TSDF
     */
    void deallocate( );

    // Size of voxel grid
    dim3cpu                    m_size;

    // Physical size of space represented in mm
    float3cpu                  m_physical_size;

    // Offset of physical grid in world coordinates
    float3cpu                  m_offset;

    // Size of a voxel
    float3cpu m_voxel_size;

    //  Truncation distance
    float m_truncation_distance;

    // Max weight for a voxel
    float m_max_weight;

    // Per grid point data
    float                   *m_distances;

    // Colour data, RGB as 3xuchar
    uchar3cpu                  *m_colours;

    //  Confidence weight for distance and colour
    float                   *m_weights;

    // Deformation field
    DeformationNode         *m_deformation_nodes;

    // Global translation
    float3cpu                  m_global_translation;

    // Global rotation
    float3cpu                  m_global_rotation;
};
#endif /* TSDFVolumeCPU_hpp */


/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

Copyright (c) 2019 Panda Team
*/
#ifndef _METRIC_DISTANCE_K_RANDOM_VOI_CPP
#define _METRIC_DISTANCE_K_RANDOM_VOI_CPP

#include <algorithm>
#include <cmath>
#include <iostream>
#include <random>
#include <unordered_set>
#include <vector>

#include <boost/functional/hash.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>

#include "../../space/tree.hpp"
#include "VOI.hpp"

namespace metric {

namespace {
    template <typename T>
    void add_noise(std::vector<std::vector<T>>& data)
    {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::normal_distribution<T> dis(0, 1);
        double c = 1e-10;
        for (auto& v : data) {
            std::transform(v.begin(), v.end(), v.begin(), [&gen, c, &dis](T e) {
                auto g = dis(gen);
                auto k = e + c * g;
                return k;
            });
        }
    }

    template <typename T1, typename T2>
    T1 log(T1 logbase, T2 x)
    {
        return std::log(x) / std::log(logbase);
    }

    template <typename T>
    void print_vec(const std::vector<T>& v)
    {
        std::cout << "[";
        for (auto d : v) {
            std::cout << d << ", ";
        }
        std::cout << "]";
    }

    template <typename Node_ptr, typename Distance>
    void print(const std::vector<std::pair<Node_ptr, Distance>>& data)
    {
        std::cout << "[";
        for (auto& v : data) {
            print_vec(v.first->data);
            std::cout << " dist=" << v.second << "]";
        }
        std::cout << "]" << std::endl;
    }
    template <typename T>
    void combine(
        const std::vector<std::vector<T>>& X, const std::vector<std::vector<T>>& Y, std::vector<std::vector<T>>& XY)
    {
        std::size_t N = X.size();
        std::size_t dx = X[0].size();
        std::size_t dy = Y[0].size();
        XY.resize(N);
        for (std::size_t i = 0; i < N; i++) {
            XY[i].resize(dx + dy);
            std::size_t k = 0;
            for (std::size_t j = 0; j < dx; j++, k++) {
                XY[i][k] = X[i][j];
            }
            for (std::size_t j = 0; j < dy; j++, k++) {
                XY[i][k] = Y[i][j];
            }
        }
    }
    template <typename T>
    std::vector<T> unique(const std::vector<T>& data)
    {
        std::unordered_set<std::size_t> hashes;
        std::vector<T> result;
        result.reserve(data.size());
        std::copy_if(data.begin(), data.end(), std::back_inserter(result),
            [&hashes](const T& i) { return hashes.insert(boost::hash_value(i)).second; });
        return result;
    }

}  // namespace

template <typename T, typename Metric, typename L>
typename std::enable_if<!std::is_integral<T>::value, T>::type entropy(
    std::vector<std::vector<T>> data, std::size_t k, L logbase, Metric metric)
{
    if (data.empty() || data[0].empty()) {
        return 0;
    }
    if (data.size() < k + 1)
        throw std::invalid_argument("number of points in dataset must be larger than k");

    T p = 1;
    T N = data.size();
    T d = data[0].size();
    T two = 2.0;  // this is in order to make types match the log template function
    T cb = d * log(logbase, two);

    if constexpr (!std::is_same<Metric, typename metric::Chebyshev<T>>::value) {
        if constexpr (std::is_same<Metric, typename metric::Euclidian<T>>::value) {
            p = 2;
        } else if constexpr (std::is_same<Metric, typename metric::P_norm<T>>::value) {
            p = metric.p;
        }
        cb = cb + d * log(logbase, std::tgamma(1 + 1 / p)) - log(logbase, std::tgamma(1 + d / p));
    }

    add_noise(data);
    metric::Tree<std::vector<T>, Metric> tree(data, -1, metric);
    T entropyEstimate = boost::math::digamma(N) - boost::math::digamma(k) + cb + d * log(logbase, two);
    for (std::size_t i = 0; i < N; i++) {
        auto res = tree.knn(data[i], k + 1);
        entropyEstimate += d / N * log(logbase, res.back().second);
    }
    return entropyEstimate;
}

// Kozachenko-Leonenko estimator based on https://hal.archives-ouvertes.fr/hal-00331300/document (Shannon diff. entropy,
// q = 1)

template <typename T, typename Metric = metric::Euclidian<T>, typename L = T>  // TODO check if L = T is correct
typename std::enable_if<!std::is_integral<T>::value, T>::type entropy_kl(
    std::vector<std::vector<T>> data, std::size_t k = 3, L logbase = 2, Metric metric = Metric())
{
    if (data.empty() || data[0].empty())
        return 0;
    if (data.size() < k + 1)
        throw std::invalid_argument("number of points in dataset must be larger than k");
    if constexpr (!std::is_same<Metric, typename metric::Euclidian<T>>::value)
        throw std::logic_error("entropy function is now implemented only for Euclidean distance");

    metric::Tree<std::vector<T>, Metric> tree(data, -1, metric);

    size_t N = data.size();
    size_t m = data[0].size();
    T two = 2.0;  // this is in order to make types match the log template function
    T sum = 0;
    auto Pi = boost::math::constants::pi<T>();
    T half_m = m / two;
    auto coeff = (N - 1) * exp(-boost::math::digamma(k + 1)) * std::pow(Pi, half_m) / boost::math::tgamma(half_m + 1);

    for (std::size_t i = 0; i < N; i++) {
        auto neighbors = tree.knn(data[i], k + 1);
        auto ro = neighbors.back().second;
        sum = sum + log(logbase, coeff * std::pow(ro, m));
    }

    return sum;
}

template <typename T>
std::pair<std::vector<double>, std::vector<std::vector<T>>> pluginEstimator(const std::vector<std::vector<T>>& Y)
{
    std::vector<std::vector<T>> uniqueVal = unique(Y);
    std::vector<double> counts(uniqueVal.size());
    for (std::size_t i = 0; i < counts.size(); i++) {
        for (std::size_t j = 0; j < Y.size(); j++) {
            if (Y[j] == uniqueVal[i])
                counts[i]++;
        }
    }
    std::size_t length = Y.size() * Y[0].size();
    std::transform(counts.begin(), counts.end(), counts.begin(), [&length](auto& i) { return i / length; });

    return std::make_pair(counts, uniqueVal);
}

template <typename T, typename Metric>
typename std::enable_if<!std::is_integral<T>::value, T>::type mutualInformation(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, int k, Metric metric, int version)
{
    T N = Xc.size();

    if (N < k + 1 || Yc.size() < k + 1)
        throw std::invalid_argument("number of points in dataset must be larger than k");

    auto X = Xc;
    auto Y = Yc;
    add_noise(X);
    add_noise(Y);
    std::vector<std::vector<T>> XY;
    combine(X, Y, XY);
    metric::Tree<std::vector<T>, Metric> tree(XY, -1, metric);
    auto entropyEstimate = boost::math::digamma(k) + boost::math::digamma(N);
    if (version == 2) {
        entropyEstimate -= 1 / static_cast<double>(k);
    }

    metric::Tree<std::vector<T>, Metric> xTree(X, -1, metric);

    for (std::size_t i = 0; i < N; i++) {
        auto res = tree.knn(XY[i], k + 1);
        auto neighbor = res.back().first;
        auto dist = res.back().second;
        std::size_t nx = 0;
        if (version == 1) {
            auto dist_eps = std::nextafter(
                dist, std::numeric_limits<decltype(dist)>::max());  // this is instead of replacing < with <= in Tree //
                // added by Max F in order to match Julia code logic
                // without updating Tree
            nx = xTree.rnn(X[i], dist_eps).size();  // we include points that lay on the sphere
        } else if (version == 2) {
            auto ex = metric(X[neighbor->ID], X[i]);
            auto ex_eps = std::nextafter(
                ex, std::numeric_limits<decltype(ex)>::max());  // this it to include the most distant point into the
                // sphere // added by Max F in order to match Julia code
                // logic without updating Tree
            auto rnn_set = xTree.rnn(X[i], ex_eps);
            nx = rnn_set.size();  // replaced ex by ex_eps by Max F
        } else {
            throw std::runtime_error("this version not allowed");
        }
        entropyEstimate -= 1.0 / N * boost::math::digamma(static_cast<double>(nx));
    }
    return entropyEstimate;
}

template <typename T>
typename std::enable_if<std::is_integral<T>::value, T>::type mutualInformation(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, T logbase)
{
    std::vector<std::vector<T>> XY;
    combine(Xc, Yc, XY);
    return entropy<T>(Xc, logbase)
        + entropy<T>(Yc,
            logbase)  // entropy overload for integers is not implemented yet
        - entropy<T>(XY, logbase);
}

template <typename T, typename Metric>
typename std::enable_if<!std::is_integral<T>::value, T>::type variationOfInformation(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, int k, T logbase)
{
    return entropy<T, Metric>(Xc, k, logbase, Metric()) + entropy<T, Metric>(Yc, k, logbase, Metric())
        - 2 * mutualInformation<T>(Xc, Yc, k);
}

template <typename T>
typename std::enable_if<!std::is_integral<T>::value, T>::type variationOfInformation_normalized(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, int k, T logbase)
{
    using Cheb = metric::Chebyshev<T>;
    auto mi = mutualInformation<T>(Xc, Yc, k);
    return 1 - (mi / (entropy<T, Cheb>(Xc, k, logbase, Cheb()) + entropy<T, Cheb>(Yc, k, logbase, Cheb()) - mi));
}

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    using Cheb = metric::Chebyshev<El>;
    return entropy<El, Cheb>(a, k, logbase, Cheb()) + entropy<El, Cheb>(b, k, logbase, Cheb())
        - 2 * mutualInformation<El>(a, b, k);
}

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI_normalized<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    using Cheb = metric::Chebyshev<El>;
    auto mi = mutualInformation<El>(a, b, this->k);
    return 1
        - (mi
            / (entropy<El, Cheb>(a, this->k, this->logbase, Cheb())
                + entropy<El, Cheb>(b, this->k, this->logbase, Cheb()) - mi));
}

// VOI based on Kozachenko-Leonenko entropy estimator

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI_kl<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    std::vector<std::vector<El>> ab;
    combine(a, b, ab);
    return 2 * entropy_kl(ab) - entropy_kl(a) - entropy_kl(b);
}

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI_normalized_kl<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    auto entropy_a = entropy_kl(a);
    auto entropy_b = entropy_kl(b);
    std::vector<std::vector<El>> ab;
    combine(a, b, ab);
    auto joint_entropy = entropy_kl(ab);
    auto mi = entropy_a + entropy_b - joint_entropy;
    return 1 - (mi / (entropy_a + entropy_b - mi));
}

}  // namespace metric
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_2PI_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_2PI_HPP_INCLUDED

#include <nt2/toolbox/trigonometric/functions/rem_2pi.hpp>
#include <nt2/toolbox/trigonometric/functions/scalar/impl/trigo/selection_tags.hpp>
#include <nt2/include/functions/simd/rem_pio2.hpp>
#include <nt2/include/functions/simd/rem_pio2_medium.hpp>
#include <nt2/include/functions/simd/round.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/constants/inv2pi.hpp>
#include <nt2/include/constants/pix2_1.hpp>
#include <nt2/include/constants/pix2_2.hpp>
#include <nt2/include/constants/pix2_3.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/fusion/tuple.hpp>

/////////////////////////////////////////////////////////////////////////////
// reference based Implementation
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_2pi_, boost::simd::tag::simd_,
                      (A0)(X),
                      ((simd_ < floating_<A0>,X > ))
                    )
  {
    typedef boost::fusion::tuple<A0,A0>        result_type;

    inline result_type operator()(A0 const& a0) const
      {
        result_type res;
        nt2::rem_2pi(a0,
                     boost::fusion::at_c<0>(res),
                     boost::fusion::at_c<1>(res)
                     );
        return res;
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION(  nt2::tag::rem_2pi_, boost::simd::tag::simd_,(A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0,A0 & xr,A0 & xc) const
    {
      typedef typename meta::as_integer<A0>::type i_type;
      A0 n = tofloat(rem_pio2(a0, xr, xc));
      xr = xr+n*Pio_2<A0>();
      xr =  if_else(gt(xr, Pi<A0>()), xr-Twopi<A0>(), xr);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_2pi_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_ <floating_<A0>,X  > ))
                             ((simd_ <floating_<A0>,X  > ))
                             ((simd_ <floating_<A0>,X  > ))
                             ((target_ <unspecified_<A1> >))
                 )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0, A0 & xr, A0& xc, A1 const&) const
    {
      typedef typename A1::type selector;
      rem2pi<selector, void>::rem(a0, xr, xc);
    }
  private:
    template < class T, class dummy = void> struct rem2pi
    {
      static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
      {
        BOOST_ASSERT_MSG(false, "wrong target for rem_2pi");
      }
    };
    template < class dummy> struct rem2pi < big_, dummy>
    {
      static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
      {
        nt2::rem_2pi(x, xr, xc);
      }
    };
    template < class dummy> struct rem2pi < very_small_, dummy > // |a0| <2*pi
    {
      static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
      {
        xr = if_else(gt(x, Pi<A0>()), x-Twopi<A0>(),
                     if_else(lt(x, -Pi<A0>()), x+Twopi<A0>(), x));
        xc = Zero<A0>();
      }
    };
    template < class dummy> struct rem2pi < small_, dummy >// |a0| <= 20*pi
    {
      static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
      {
        A0 xi =  nt2::round(x*Inv2pi<A0>());
        xr = x-xi*Pix2_1<A0>();
        xr -= xi*Pix2_2<A0>();
        xr -= xi*Pix2_3<A0>();
        xc = Zero<A0>();
       }
    };

    template < class dummy> struct rem2pi < medium_, dummy >
    {
      static inline result_type rem(A0 const& x, A0 & xr, A0& xc)
      {
        typedef typename meta::as_integer<A0>::type i_type;
        A0 n = tofloat(rem_pio2_medium(x, xr, xc));
        xr = xr+n*Pio_2<A0>();
        xr = if_else(gt(xr, Pi<A0>()), xr-Twopi<A0>(), xr);
      }
    };
  };

} }
#endif


#ifndef MATHTOOLBOX_LOG_DETERMINANT_HPP
#define MATHTOOLBOX_LOG_DETERMINANT_HPP

#include <Eigen/Cholesky>
#include <Eigen/Core>

namespace mathtoolbox
{
    double CalcLogDetOfSymmetricPositiveDefiniteMatrix(const Eigen::MatrixXd& matrix);
    double CalcLogDetOfSymmetricPositiveDefiniteMatrix(const Eigen::LLT<Eigen::MatrixXd>& matrix_llt);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_LOG_DETERMINANT_HPP


#ifndef TEST_UNIT_TORSTEN_FRIEBERG_KARLSSON_TEST_FIXTURE
#define TEST_UNIT_TORSTEN_FRIEBERG_KARLSSON_TEST_FIXTURE

#include <stan/math/rev/core/recover_memory.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <stan/math/torsten/test/unit/test_fixture_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

struct FribergKarlssonFunc {
   // parms contains both the PK and the PD parameters.
   // x contains both the PK and the PD states.
  template <typename T0, typename T1, typename T2>
  Eigen::Matrix<typename stan::return_type_t<T0, T1, T2>, -1, 1>
  operator()(const T0& t,
             const Eigen::Matrix<T1, -1, 1>& x,
             std::ostream* pstream__,
             const std::vector<T2>& parms,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i) const {
    using scalar = typename stan::return_type_t<T0, T1, T2>;

    // PK variables
    T2
      CL = parms[0],
      Q = parms[1],
      VC = parms[2],
      VP = parms[3],
      ka = parms[4],
      k10 = CL / VC,
      k12 = Q / VC,
      k21 = Q / VP;

    // PD variables
    T2
      MTT = parms[5],
      circ0 = parms[6],
      gamma = parms[7],
      alpha = parms[8],
      ktr = 4 / MTT;
    typename stan::return_type_t<T1, T2>
      prol = x[3] + circ0,
      transit1 = x[4] + circ0,
      transit2 = x[5] + circ0,
      transit3 = x[6] + circ0,
      circ = stan::math::fmax(stan::math::machine_precision(), x[7] + circ0);

    Eigen::Matrix<scalar, -1, 1> dxdt(8);
    dxdt[0] = -ka * x[0];
    dxdt[1] = ka * x[0] - (k10 + k12) * x[1] + k21 * x[2];
    dxdt[2] = k12 * x[1] - k21 * x[2];

    scalar conc = x[1] / VC;
    scalar Edrug = alpha * conc;

    dxdt[3] = ktr * prol * (((1 - Edrug) * pow((circ0 / circ), gamma)) - 1);
    dxdt[4] = ktr * (prol - transit1);
    dxdt[5] = ktr * (transit1 - transit2);
    dxdt[6] = ktr * (transit2 - transit3);
    dxdt[7] = ktr * (transit3 - circ);

    return dxdt;
  }
};

template<typename T>
struct test_fk : public TorstenPMXTest<test_fk<T> > {
  test_fk() {
    this -> ncmt = 8;

    this -> reset_events(10);

    this -> theta.resize(1);

    // CL , Q , Vc , Vp , ka , MTT , Circ0 , alpha , gamma
    this -> theta[0] = {10, 15, 35, 105, 2.0, 125, 5, 0.17, 3e-4};

    this -> biovar.resize(1);
    this -> biovar[0] = {1, 1, 1, 1, 1, 1, 1, 1};
    this -> tlag.resize(1);
    this -> tlag[0] = {0, 0, 0, 0, 0, 0, 0, 0};

    for(int i = 0; i < this -> nt; i++) {
      this -> time[i] = i * 1.25;
    }

    this -> amt[0] = 80000.0;
    this -> cmt[0] = 1;
    this -> evid[0] = 1;
    this -> ii[0] = 12;
    this -> addl[0] = 14;
  }
};

#endif


/**
 * @file Tools/Math/RotationMatrix.hpp
 * Delcaration of class RotationMatrix
 * @author Martin Kallnik
 * @author Thomas Kindler
 * @author Max Risler
 * @author Alexis Tsogias
 */

#pragma once

#include <Eigen/Dense>

using namespace Eigen;

/**
 * Representation for 3x3 RotationMatrices
 */
class RotationMatrix : public Matrix3f
{
public:
  RotationMatrix() : Matrix3f(Matrix3f::Identity()) {}
  RotationMatrix(const Matrix3f& other) : Matrix3f(other) {}
  RotationMatrix(const AngleAxisf& angleAxis) : Matrix3f(angleAxis.toRotationMatrix()) {}
  RotationMatrix(const Quaternionf& quat) : Matrix3f(quat.toRotationMatrix()) {}

  RotationMatrix& operator=(const Matrix3f& other)
  {
    Matrix3f::operator=(other);
    return *this;
  }

  RotationMatrix& operator=(const AngleAxisf& angleAxis)
  {
    Matrix3f::operator=(angleAxis.toRotationMatrix());
    return *this;
  }

  RotationMatrix& operator=(const Quaternionf& quat)
  {
    Matrix3f::operator=(quat.toRotationMatrix());
    return *this;
  }

  /**
   * Multiplication of this matrix by vector.
   * @param  vector  The vector this one is multiplied by
   * @return         A new vector containing the result
   */
  Vector3f operator*(const Vector3f& vector) const
  {
    return Matrix3f::operator*(vector);
  }

  /**
   * Multiplication of this rotation matrix by another rotation matrix.
   * @param  other  The other matrix this one is multiplied by
   * @return        A new matrix containing the result
   *                of the calculation.
   */
  RotationMatrix operator*(const RotationMatrix& other) const
  {
    return RotationMatrix(Base::operator*(other));
  }

  RotationMatrix& operator*=(const AngleAxisf& rot)
  {
    Matrix3f::operator*=(rot.toRotationMatrix());
    return *this;
  }

  RotationMatrix& operator*=(const Quaternionf& rot)
  {
    Matrix3f::operator*=(rot.toRotationMatrix());
    return *this;
  }

  RotationMatrix& operator*=(const RotationMatrix& rot)
  {
    Matrix3f::operator*=(rot);
    return *this;
  }

  /**
   * Invert the matrix.
   *
   * @note: Inverted rotation matrix is transposed matrix.
   */
  RotationMatrix& invert()
  {
    transposeInPlace();
    return *this;
  }

  RotationMatrix inverse() const
  {
    return RotationMatrix(transpose());
  }

  void normalize()
  {
    *this = Quaternionf(*this).normalized();
  }

  RotationMatrix normalized() const
  {
    return Quaternionf(*this).normalized();
  }

  /**
   * Converts the rotation matrix into an angleAxis.
   * @return The rotation matrix as angleAxis.
   */
  AngleAxisf getAngleAxis() const;

  /**
   * Converts the rotation matrix into an angleAxis in single vector format.
   * @return The rotation matrix as angleAxis.
   */
  // Vector3f getPackedAngleAxis() const;

  /**
   * Rotation around the x-axis.
   *
   * @param   angle  The angle this pose will be rotated by
   * @return  A reference to this object after the calculation.
   */
  RotationMatrix& rotateX(const float angle);

  /**
   * Rotation around the y-axis.
   *
   * @param   angle  The angle this pose will be rotated by
   * @return  A reference to this object after the calculation.
   */
  RotationMatrix& rotateY(const float angle);

  /**
   * Rotation around the z-axis.
   *
   * @param   angle  The angle this pose will be rotated by
   * @return  A reference to this object after the calculation.
   */
  RotationMatrix& rotateZ(const float angle);

  /**
   * Get the x-angle of a RotationMatrix.
   *
   * @return  The angle around the x-axis between the original
   *          and the rotated z-axis projected on the y-z-plane
   */
  float getXAngle() const;

  /**
   * Get the y-angle of a RotationMatrix.
   *
   * @return  The angle around the y-axis between the original
   *          and the rotated x-axis projected on the x-z-plane
   */
  float getYAngle() const;

  /**
   * Get the z-angle of a RotationMatrix.
   *
   * @return  The angle around the z-axis between the original
   *          and the rotated x-axis projected on the x-y-plane
   */
  float getZAngle() const;

  /**
   * Create and return a RotationMatrix, rotated around x-axis
   *
   * @param   angle
   * @return  rotated RotationMatrix
   */
  static RotationMatrix aroundX(const float angle);

  /**
   * Create and return a RotationMatrix, rotated around y-axis
   *
   * @param   angle
   * @return  rotated RotationMatrix
   */
  static RotationMatrix aroundY(const float angle);

  /**
   * Create and return a RotationMatrix, rotated around z-axis
   *
   * @param   angle
   * @return  rotated RotationMatrix
   */
  static RotationMatrix aroundZ(const float angle);

  /**
   * Creates a RotationMatrix rotated around z, y and x (in this order!).
   * Equivalent to fromRotationZ(z).rotateY(y).rotateX(x);
   */
  static RotationMatrix fromEulerAngles(const float x, const float y, const float z);

  /**
   * Creates a RotationMatrix rotatied around the z, y and x components of the Vector3 (in this order!).
   * Equivalent to fromRotationZ(rotation.z).rotateY(rotation.y).rotateX(rotation.x);
   */
  static RotationMatrix fromEulerAngles(const Vector3f rotation);

private:
  // The following is a hack in order to keep the kicks from the Kickengine working...
  friend class KickViewWidget;
  friend class KickEngineData;
  friend class StableKickEngineData;
  friend class RotationMatrix_getPackedAngleAxisFaulty_Test;

  Vector3f getPackedAngleAxisFaulty() const;

  static void reg();
};


#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/LU>
#include <limits>
#include "rose499/spline.hpp"

using namespace Eigen;

constexpr int Spline::PolyOrder;
constexpr int Spline::CoeffCount;

namespace
{
    constexpr int ResampleCount = 9;

    using ValueType = Spline::ValueType;
    using MatrixXT = SimulatorTypes::MatrixXT;
    using VectorXT = SimulatorTypes::VectorXT;
    using PolySpace = Array<ValueType, 1, Spline::CoeffCount>;

    PolySpace polydiff_power(PolySpace poly, int order)
    {
        PolySpace prevPoly = poly;
        PolySpace dpoly = PolySpace::Constant(0);
        PolySpace powerCoeff = PolySpace::Constant(1);
        for(auto i = 0; i < order; i++)
        {
            Array<ValueType, 1, Dynamic> power(1, Spline::CoeffCount - i);
            power.setLinSpaced(0, Spline::PolyOrder - i);
            powerCoeff.tail(Spline::CoeffCount - i) = powerCoeff.tail(Spline::CoeffCount - i) * power;
        }
        dpoly.tail(Spline::CoeffCount - order) = poly.head(Spline::CoeffCount - order);
        dpoly = dpoly * powerCoeff;
        return dpoly;
    }

    PolySpace polydiffshift(PolySpace poly)
    {
        PolySpace dpoly = PolySpace::Constant(0);
        dpoly.head(Spline::CoeffCount - 1) = poly.tail(Spline::CoeffCount - 1);
        dpoly.head(Spline::CoeffCount - 1) *= Array<ValueType, Spline::CoeffCount - 1, 1>::LinSpaced(1, Spline::PolyOrder);
        return dpoly;
    }

    /**
        Builds an equality matrix that only provides end point guarantees and
        C^2 continuity everywhere.
    **/
    void
    polyBuildSoftEqualityMatrix(    MatrixXT& Aeq,
                                    VectorXT& beq,
                                    Matrix<ValueType, 2, Dynamic> const & waypoints,
                                    double direction = std::numeric_limits<double>::infinity() )
    {
        constexpr size_t DirectionConstraintCount = 1;
        constexpr auto CoeffCount = Spline::CoeffCount;

        auto const waypointCount = waypoints.cols();
        auto const segmentCount = waypointCount - 1;

        bool const supportDirection = std::isfinite(direction);

        Aeq.resize( 4*(waypointCount-2)                                         // Differential Constraints
                        + 2*waypointCount                                       // Equality Constraints
                        + (supportDirection ? DirectionConstraintCount : 0)     // Direction Constraint
                    , 2*CoeffCount*segmentCount );
        beq.resize(Aeq.rows(), 1);

        Aeq.setZero();
        beq.setZero();

        PolySpace powers = PolySpace::LinSpaced(0, Spline::PolyOrder);
        auto row = 0;
        for(auto indSeg = 0; indSeg < segmentCount; ++indSeg)
        {
            auto col = 2*CoeffCount*indSeg;

            ValueType leftLambda = (indSeg * 1.0) / segmentCount;
            ValueType rightLambda = ((indSeg + 1.0) * 1.0) / segmentCount;

            PolySpace leftTerms = PolySpace::Constant(leftLambda);
            PolySpace rightTerms = PolySpace::Constant(rightLambda);
            leftTerms = pow(leftTerms, powers);
            rightTerms = pow(rightTerms, powers);

            PolySpace dLeftTerms = polydiff_power(leftTerms, 1);
            PolySpace ddLeftTerms = polydiff_power(leftTerms, 2);

            PolySpace dRightTerms = polydiff_power(rightTerms, 1);
            PolySpace ddRightTerms = polydiff_power(rightTerms, 2);

            if( indSeg < segmentCount - 1 )
            {
                // Right Equality (C0)
                Aeq.block<1, CoeffCount>(row + 0, col) = rightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -rightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = rightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -rightTerms;
                row += 2;

                // Right Differential Continuity (C1)
                Aeq.block<1, CoeffCount>(row + 0, col) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -dRightTerms;
                row += 2;

                // Right Differential Continuity (C2)
                Aeq.block<1, CoeffCount>(row + 0, col) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -ddRightTerms;
                row += 2;
            }
            else
            {
                // Hard Right Equality
                Aeq.block<1, CoeffCount>(row + 0, col) = rightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = rightTerms;
                beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg + 1);
                row += 2;
            }

            if( indSeg == 0 )
            {
                // Left Equality (C0)
                Aeq.block<1, CoeffCount>(row + 0, col) = leftTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = leftTerms;
                beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg);
                row += 2;

                if( supportDirection )
                {
                    while(direction > M_PI) direction -= 2*M_PI;
                    while(direction < -M_PI) direction += 2*M_PI;

                    if( std::abs(direction) <= std::atan2(1.0, 1.0)
                     || std::abs(direction - M_PI) <= std::atan2(1.0, 1.0)
                     || std::abs(direction + M_PI) <= std::atan2(1.0, 1.0) )
                    {
                        // Small tangent.
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col) = std::tan(direction)*dLeftTerms;
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col + CoeffCount) = -dLeftTerms;
                    }
                    else
                    {
                        // Large tangent
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col) = -dLeftTerms;
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col + CoeffCount) = dLeftTerms / std::tan(direction);
                    }
                }
            }
        }
    }

    /**
        Builds the equality matrix that is used in least squares to achieve
        C0, C1, and C2 equality at waypoints.

    **/
    void
    polyBuildEqualityMatrix( MatrixXT& Aeq,
                             VectorXT& beq,
                             Matrix<ValueType, 2, Dynamic> const & waypoints,
                             double direction = std::numeric_limits<double>::infinity() )
    {
        constexpr size_t DirectionConstraintCount = 2;
        constexpr auto CoeffCount = Spline::CoeffCount;

        auto const waypointCount = waypoints.cols();
        auto const segmentCount = waypointCount - 1;

        bool const supportDirection = std::isfinite(direction);

        Aeq.resize( 4*(segmentCount-1)                                          // Differential Constraints
                        + 4*(segmentCount)                                      // Equality Constraints
                        + (supportDirection ? DirectionConstraintCount : 0)     // Direction Constraint
                    , 2*CoeffCount*segmentCount );
        beq.resize(Aeq.rows(), 1);

        Aeq.setZero();
        beq.setZero();

        PolySpace powers = PolySpace::LinSpaced(0, Spline::PolyOrder);
        auto row = 0;
        for(auto indSeg = 0; indSeg < segmentCount; ++indSeg)
        {
            auto col = 2*CoeffCount*indSeg;

            ValueType leftLambda = (indSeg * 1.0) / segmentCount;
            ValueType rightLambda = ((indSeg + 1.0) * 1.0) / segmentCount;

            PolySpace leftTerms = PolySpace::Constant(leftLambda);
            PolySpace rightTerms = PolySpace::Constant(rightLambda);
            leftTerms = pow(leftTerms, powers);
            rightTerms = pow(rightTerms, powers);

            PolySpace dLeftTerms = polydiff_power(leftTerms, 1);
            PolySpace ddLeftTerms = polydiff_power(leftTerms, 2);

            PolySpace dRightTerms = polydiff_power(rightTerms, 1);
            PolySpace ddRightTerms = polydiff_power(rightTerms, 2);

            // Left Equality (C0)
            Aeq.block<1, CoeffCount>(row + 0, col) = leftTerms;
            Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = leftTerms;
            beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg);
            row += 2;

            // Right Equality (C0)
            Aeq.block<1, CoeffCount>(row + 0, col) = rightTerms;
            Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = rightTerms;
            beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg + 1);
            row += 2;

            if( (indSeg == 0) && supportDirection )
            {
                Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col) = dLeftTerms;
                Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(1, col + CoeffCount) = dLeftTerms;

                Matrix<ValueType, 2, 1> vec;
                vec(0) = std::cos(direction);
                vec(1) = std::sin(direction);

                beq.tail(DirectionConstraintCount) = vec;
            }

            if( indSeg < segmentCount - 1 )
            {
                // Right Differential Continuity (C1)
                Aeq.block<1, CoeffCount>(row + 0, col) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -dRightTerms;
                row += 2;

                // Right Differential Continuity (C2)
                Aeq.block<1, CoeffCount>(row + 0, col) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -ddRightTerms;
                row += 2;
            }
        }
    }

    void
    polyBuildLSQMatrix( MatrixXT& A,
                        VectorXT& b,
                        Matrix<ValueType, 2, Dynamic> const & waypoints )
    {
        constexpr auto CoeffCount = Spline::CoeffCount;
        constexpr auto IntermediateSampleCount = 9;
        const auto waypointCount = waypoints.cols();
        const auto segmentCount = waypointCount - 1;
        const auto sampleCount = segmentCount * IntermediateSampleCount;
        using SampleRow = Matrix<ValueType, 1, IntermediateSampleCount>;

        A.resize(2*sampleCount, 2*CoeffCount*segmentCount);
        b.resize(A.rows());

        A.setZero();
        b.setZero();

        PolySpace powers = PolySpace::LinSpaced(0, Spline::PolyOrder);
        size_t row = 0;
        size_t colX = 0, colY = CoeffCount;
        for(auto si = 0; si < segmentCount; ++si)
        {
            SampleRow sampleX = Matrix<ValueType, 1, IntermediateSampleCount+2>::LinSpaced(waypoints(0, si), waypoints(0, si+1)).middleCols(1, IntermediateSampleCount);
            SampleRow sampleY = Matrix<ValueType, 1, IntermediateSampleCount+2>::LinSpaced(waypoints(1, si), waypoints(1, si+1)).middleCols(1, IntermediateSampleCount);
            SampleRow sampleLambda = Matrix<ValueType, 1, IntermediateSampleCount+2>::LinSpaced(si * 1.0 / segmentCount, (si+1) * 1.0 / segmentCount).middleCols(1, IntermediateSampleCount);

            for(auto i = 0; i < sampleLambda.cols(); ++i)
            {
                PolySpace terms = PolySpace::Constant(sampleLambda(i));
                terms = pow(terms, powers);

                // Equality at the lambda
                A.block<1, CoeffCount>(row + 0, colX) = terms;
                A.block<1, CoeffCount>(row + 1, colY) = terms;
                b(row + 0) = sampleX(i);
                b(row + 1) = sampleY(i);

                row += 2;
            }

            colX += 2 * CoeffCount;
            colY += 2 * CoeffCount;
        }
    }

    /**
     * Splines a 5th order polynomial through the waypoints that ensures C2
     * conditions are met as well as a direction requirement at lambda=0
     *
     * The system is normally underdetermined so polyfit normally calculates the
     * right pseduo-inverse and performs a full pivot LU decomposition in order to
     * solve for the coefficients.
     *
     * Implements https://stanford.edu/class/ee103/lectures/constrained-least-squares/constrained-least-squares_slides.pdf
     */
    Matrix<ValueType, Dynamic, Spline::CoeffCount>
    polyfit(Matrix<ValueType, 2, Dynamic> waypoints, double direction = std::numeric_limits<double>::infinity() )
    {
        constexpr auto CoeffCount = Spline::CoeffCount;

        auto const waypointCount = waypoints.cols();
        auto const segmentCount = waypointCount - 1;

        MatrixXT Aeq, A;
        VectorXT beq, b;

        polyBuildSoftEqualityMatrix(Aeq, beq, waypoints, direction);
        polyBuildLSQMatrix(A, b, waypoints);

        // Moore-Penrose (Right) Pseudo-Inverse
        //c = Aeq.transpose() * (Aeq * Aeq.transpose()).fullPivLu().solve(beq);

        // Build full solution matrix F * (x, z)' = (2A'b, beq)'
        MatrixXT F( A.cols() + Aeq.rows(),
                    A.cols() + Aeq.rows() );
        VectorXT v( F.rows() );

        F.setZero();
        v.setZero();

        F.block(0, 0, A.cols(), A.cols()) = 2 * A.transpose() * A;
        F.block(0, A.cols(), Aeq.cols(), Aeq.rows()) = Aeq.transpose();
        F.block(A.cols(), 0, Aeq.rows(), Aeq.cols()) = Aeq;

        v.head(A.cols()) = 2 * A.transpose() * b;
        v.tail(beq.rows()) = beq;

        VectorXT d(F.rows(), 1);
        if( F.rows() > 100 )
        {
            SparseMatrix<ValueType, ColMajor> Fs;
            Fs = F.sparseView();

            SparseLU< SparseMatrix<ValueType, ColMajor> > solver;
            solver.analyzePattern(Fs);
            solver.factorize(Fs);
            d = solver.solve(v);
        }
        else
        {
            d = F.fullPivHouseholderQr().solve(v);
        }
        VectorXT c = d.head(Aeq.cols());
        /*
        MatrixXT c(Aeq.cols(), 1);

        // Moore-Penrose (Right) Pseudo-Inverse
        c = Aeq.transpose() * (Aeq * Aeq.transpose()).fullPivLu().solve(beq);
        */
        // Map the resulting coefficient vector into our matrix form of the polynomial
        // spline!
        Matrix<ValueType, Dynamic, CoeffCount> result(2 * segmentCount, CoeffCount);
        result = Map< Matrix<ValueType, Dynamic, CoeffCount, RowMajor> >(c.data(), result.rows(), result.cols());

        return result;
    }
}

Spline::Spline(Matrix<Spline::ValueType, 2, Dynamic> points)
  : mSplineCount(points.cols() - 1),
    mPoly(2 * mSplineCount, CoeffCount),
    mDPoly(2 * mSplineCount, CoeffCount),
    mDDPoly(2 * mSplineCount, CoeffCount)
{
    mPoly = ::polyfit(points);
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDPoly.row(i) = ::polydiffshift(mPoly.row(i));
    }
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDDPoly.row(i) = ::polydiffshift(mDPoly.row(i));
    }
    approximateSelf();
}

Spline::Spline(Eigen::Matrix<Spline::ValueType, 2, Eigen::Dynamic> points, double direction)
  : mSplineCount(points.cols() - 1),
    mPoly(2 * mSplineCount, CoeffCount),
    mDPoly(2 * mSplineCount, CoeffCount),
    mDDPoly(2 * mSplineCount, CoeffCount)
{
    mPoly = ::polyfit(points, direction);
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDPoly.row(i) = ::polydiffshift(mPoly.row(i));
    }
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDDPoly.row(i) = ::polydiffshift(mDPoly.row(i));
    }
    approximateSelf();
}

Spline::Spline()
  : mSplineCount(1),
    mPoly(2 * mSplineCount, CoeffCount),
    mDPoly(2 * mSplineCount, CoeffCount),
    mDDPoly(2 * mSplineCount, CoeffCount)
{
    mPoly.setZero();
    mDPoly.setZero();
    mDDPoly.setZero();
}

Matrix<Spline::ValueType, 2, 2> Spline::frame(ValueType parameter, uint32_t derivative) const
{
    // Min ops, 89 froats

    Matrix<ValueType, 2, 2> basis;
    Matrix<ValueType, 2, 1> tangent = (*this)(parameter, 1);
    ValueType speed = tangent.norm();

    if(derivative >= 0)
    {
        basis.col(0) = tangent / speed;

        // Rotate the first column by 90 degrees (right handed frame) to form the
        // full basis for R^2
        basis.col(1) = basis.col(0).reverse();
        basis(0, 1) = -basis(0, 1);
    }

    if(derivative >= 1)
    {
        Matrix<ValueType, 2, 1> accel = (*this)(parameter, 2);
        basis.col(0) = ( accel - accel.dot(basis.col(0)) * basis.col(0) ) / speed;
    }

    if(derivative >= 2)
        throw InvalidParameterException();

    // Rotate the first column by 90 degrees (right handed frame) to form the
    // full basis for R^2
    basis.col(1) = basis.col(0).reverse();
    basis(0, 1) = -basis(0, 1);
    return basis;
}

Spline::ValueType Spline::nearestPoint(Matrix<ValueType, 2, 1> point, ValueType lambdaStar) const
{
    auto error =
        [point, this](ValueType l) -> ValueType {
            return (point - this->operator()(l)).squaredNorm();
        };
    auto gradient =
        [point, this](ValueType l) -> ValueType {
            Matrix<ValueType, 2, 1> verr = point - this->operator()(l);
            return -2*verr.dot(this->operator()(l, 1));
        };

    ValueType lambda = lambdaStar;
    ValueType alpha = static_cast<ValueType>(1.0);
    alpha = std::min(alpha, alpha / speed(lambda));

    ValueType improvement = 1;
    int i = 500;
    while(std::abs(gradient(lambda)) > 1e-6 && (i > 0))
    {
        ValueType estimate = lambda - alpha * gradient(lambda);
        improvement = error(estimate) - error(lambda);
        if( improvement < 0 )
        {
            lambda = estimate;
            alpha *= 1.2;
        }
        else
            alpha *= 0.7;
        --i;
    }

    return std::max(std::min(lambda, static_cast<ValueType>(1.0)), static_cast<ValueType>(0.0));
}

int Spline::splineIndexUsed(ValueType parameter) const
{
    return std::max(0, std::min(mSplineCount - 1, (int)std::floor(mSplineCount * parameter)));
}

Spline::ValueType Spline::speed(ValueType parameter) const
{
    return (*this)(parameter, 1).norm();
}

Matrix<Spline::ValueType, 2, 1> Spline::operator() (ValueType parameter, uint32_t derivative) const
{
    using Vector = Matrix<Spline::ValueType, 2, 1>;
    using PolySpace = Array<Spline::ValueType, CoeffCount, 1>;

    // Identically zero derivative
    if( derivative > PolyOrder )
        return Vector::Zero();

    // Calculate x^0, x^1, ..., x^5
    PolySpace value = PolySpace::Constant(parameter);
    PolySpace powers = PolySpace::LinSpaced(0, PolyOrder);
    value = pow(value, powers);

    // Choose spline based on parameter value.
    int indSpline = splineIndexUsed(parameter);

    // And evaluate!
    switch(derivative)
    {
    case 0:
        return mPoly.block<2, CoeffCount>(2 * indSpline, 0) * value.matrix();

    case 1:
        return mDPoly.block<2, CoeffCount>(2 * indSpline, 0) * value.matrix();

    case 2:
        return mDDPoly.block<2, CoeffCount>(2 * indSpline, 0) * value.matrix();

    default:
        throw InvalidParameterException();
    };
}

void Spline::approximateSelf()
{
    Spline& self = *this;

    const double maxDelta = 1.0 / (2.0 * mSplineCount);
    double startLambda = 0;
    double endLambda = maxDelta;

    mApproximation.clear();
    do
    {
        Spline::Line currentSegment;
        Matrix<ValueType, 2, 1> start = self(startLambda);
        Matrix<ValueType, 2, 1> end = self(endLambda);

        double normedIntegral = 0.0;
        do
        {
            normedIntegral = 0.0;
            endLambda = startLambda / 2.0 + endLambda / 2.0;
            end = self(endLambda);

            Matrix<ValueType, 2, 1> midpoint = self(startLambda / 2.0 + endLambda / 2.0);

            // Integrate numerically
            for(int i = 0; (i < 1000) && (normedIntegral <= 0.1); ++i)
            {
                Matrix<ValueType, 3, 1> dError = Matrix<ValueType, 3, 1>::Constant(0);
                Matrix<ValueType, 3, 1> dLine = Matrix<ValueType, 3, 1>::Constant(0);
                dError.head(2) = self(startLambda + i * (endLambda - startLambda) / 1000)
                               - (start + i * (end - start) / 1000);
                dLine.head(2)  = (end - start) / 1000;
                normedIntegral += dError.cross(dLine).norm();
            }
        } while( normedIntegral > 0.1 );

        geom::set<0, 0>(currentSegment, start[0]);
        geom::set<0, 1>(currentSegment, start[1]);
        geom::set<1, 0>(currentSegment, end[0]);
        geom::set<1, 1>(currentSegment, end[1]);

        startLambda = endLambda;
        endLambda = std::min(endLambda + maxDelta, 1.0);

        mApproximation.push_back(currentSegment);
    } while( startLambda < 1.0 );

    mArcLength = 0;
    for(auto&& line : mApproximation)
    {
        mArcLength += geom::length(line);
    }
}

Matrix<Spline::ValueType, Dynamic, Spline::CoeffCount> Spline::poly() const { return mPoly; }
Matrix<Spline::ValueType, Dynamic, Spline::CoeffCount> Spline::dpoly() const{ return mDPoly; }
Matrix<Spline::ValueType, Dynamic, Spline::CoeffCount> Spline::ddpoly() const{ return mDDPoly; }
Spline::ApproximateSpline const & Spline::approximation() const { return mApproximation; }
Spline::ValueType Spline::arclength() const { return mArcLength; }