/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Ray casting and Point In Polygon test
// Check that a given vector does not cross a given (body or wake) panel
//
// References:
// http://geomalgorithms.com/a05-_intersect-1.html
// http://demonstrations.wolfram.com/AnEfficientTestForAPointToBeInAConvexPolygon/
// https://stackoverflow.com/questions/5188561/signed-angle-between-two-3d-vectors-with-same-origin-within-the-same-plane
//
// Inputs:
// - u0, u1, u2: vector joining center of reference cell to center of adjacent cell
// - w0, w1, w2: vector joining panel center to cell center
// - f0, f1, f2: center of reference cell coordinates
// - n0, n1, n2: panel unit normal
// - v00, v01, v02: panel vertex 1 coordinates
// - v10, v11, v12: panel vertex 2 coordinates
// - v20, v21, v22: panel vertex 3 coordinates
// - v30, v31, v32: panel vertex 4 coordinates
//
// Output:
// - 0 (vector crosses the panel) OR 1 (vector does not cross the panel)

#include <iostream>
#include <Eigen/Dense>
#include "cast_ray_pip.h"

#define TOL 1e-6

using namespace std;
using namespace Eigen;

int cast_ray_pip(double u0, double u1, double u2, double w0, double w1, double w2,
                 double f0, double f1, double f2, double n0, double n1, double n2,
                 double v00, double v01, double v02, double v10, double v11, double v12,
                 double v20, double v21, double v22, double v30, double v31, double v32) {

    double D, N, sI;
    Vector3d a, b, u, w, f, n, i, v0, v1, v2, v3;
    u << u0, u1, u2; // vector joining center of reference cell to center of adjacent cell
    w << w0, w1, w2; // vector joining panel center to cell center
    f << f0, f1, f2; // center of reference cell coordinates
    n << n0, n1, n2; // panel unit normal
    v0 << v00, v01, v02; // panel vertex 1 coordinates
    v1 << v10, v11, v12; // panel vertex 2 coordinates
    v2 << v20, v21, v22; // panel vertex 3 coordinates
    v3 << v30, v31, v32; // panel vertex 4 coordinates

    //// Ray casting

    D = n.dot(u);
    N = -n.dot(w);

    // A. Check if the vector is not parallel to the panel plane
    if (abs(D) < TOL) {
        return 1; // Vector is parallel to, or contained in, the panel plane
    }

    // B. Check if the vector is crossing the panel plane
    sI = N/D; // parameter for intersection
    if (sI < 0 || sI > 1) {
        return 1; // Vector does not reach the panel plane
    }
    // C. Compute the intersection of the vector and the plane
    else {
        i = f + sI * u; // Compute intersection
    }

    //// Point in polygon

    // D. Check that, for each pair of consecutive adjacent vertices, the angle between the lines joining the
    // intersection the vertices is between 0 and PI, measured counterclockwise

    // Vectors from intersection to vertices 1-4
    a = v0 - i;
    b = v3 - i;
    // If (a X b) vector has not the same orientation as panel normal, then angle is between PI and 2PI
    if ((a.cross(b)).dot(n) < -TOL)
        return 1;
    // Vectors from intersection to vertices 4-3
    a = v3 - i;
    b = v2 - i;
    if ((a.cross(b)).dot(n) < -TOL)
        return 1;
    // Vectors from intersection to vertices 3-2
    a = v2 - i;
    b = v1 - i;
    if ((a.cross(b)).dot(n) < -TOL)
        return 1;
    // Vectors from intersection to vertices 2-1
    a = v1 - i;
    b = v0 - i;
    if ((a.cross(b)).dot(n) < -TOL)
        return 1;

    // If program ran till there, then derivative intersects the panel and must be prevented!
    return 0;
}


#include "wave_filter.h"

// Standard libraries
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <ctime>

// Linear algebra math
#include <Eigen/Dense>

unsigned long long rdtsc(){
  unsigned int lo,hi;
  __asm__ __volatile__ ("rdtsc" : "=a" (lo), "=d" (hi));
  return ((unsigned long long)hi << 32) | lo;
}

WaveFilter::WaveFilter() : initialized_(false),
                           dT_(0.0) {};

WaveFilter::WaveFilter(double sigma, double omega0, double lambda, double gain, double dT)
{
  this->initialize(sigma, omega0, lambda, gain, dT);
}

WaveFilter::~WaveFilter()
{
}

void WaveFilter::initialize(double sigma, double omega0, double lambda, double gain, double dT)
{
  if (initialized_)
    return;

  A_ << 0, 1, -omega0*omega0, -2*lambda*omega0;
  B_ << 0, 2*lambda*omega0*sigma;
  x_ << 0,0;
  gain_ = gain;
  dT_ = dT;

  srand48_r(rdtsc(), &randBuffer);

  initialized_ = true;
}

double WaveFilter::updateFilter()
{
  if (!initialized_)
    return 0.0;

  x_ += (A_*x_ + B_*get_white_noise(&randBuffer))*dT_;

  return gain_*x_[1];
}

double get_white_noise(struct drand48_data *buf)
{
  double result;
  drand48_r(buf, &result);
  return 2*(result - 0.5);
}


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include <omp.h>

#include <random>
#include <map>
#include <string>
#include <iomanip>

#include <unistd.h>
#include <string>
#include <algorithm>
#include <random>

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

#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>
#include <boost/program_options.hpp>
#include <iterator>

using namespace std;
using namespace Eigen;
namespace po = boost::program_options;

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::LLT;
using Eigen::Lower;
using Eigen::Map;
using Eigen::Upper;
typedef Map<MatrixXd> MapMatd;

boost::random::mt19937 gen(time(0));

//distributions
double runif(double lower, double higher)
{
	boost::random::uniform_real_distribution<> dist(lower, higher);
	return dist(gen);
}

double rnorm(double mean, double sd)
{
	boost::random::normal_distribution<> dist(mean, sd);
	return dist(gen);
}

double rbeta(double alpha, double beta)
{

	boost::math::beta_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rinvchisq(double df, double scale)
{

	boost::math::inverse_chi_squared_distribution<> dist(df, scale);
	double q = quantile(dist, runif(0,1));

	return(q);
}
int rbernoulli(double p)
{
	std::bernoulli_distribution dist(p);
	return dist(gen);
}

//sampling functions
double sample_mu(int N, double Sigma2_e,const VectorXd& Y,const MatrixXd& X,const VectorXd& beta)
{
	double mean=((Y-X*beta).sum())/N;
	double sd=sqrt(Sigma2_e/N);
	double mu=rnorm(mean,sd);
	return(mu);
}

//sample variance of beta
double sample_sigma2_b(const VectorXd& beta,int NZ,double v0B,double s0B){
	double df=v0B+NZ;
	double scale=(beta.squaredNorm()*NZ+v0B*s0B)/(v0B+NZ);
	//cout<<NZ<<"\t"<<beta.squaredNorm()<<"\t"<<df<<"\t"<<scale<<"\t"<<endl;
	double psi2=rinvchisq(df, scale);
	return(psi2);
}

//sample error variance of Y
double sample_sigma2_e(int N,const VectorXd& epsilon,double v0E,double s0E){
	double sigma2=rinvchisq(v0E+N, (epsilon.squaredNorm()+v0E*s0E)/(v0E+N));
	return(sigma2);
}

//sample mixture weight
double sample_w(int M,int NZ){
	double w=rbeta(1+NZ,1+(M-NZ));
	return(w);
}

void ReadFromFile(std::vector<double> &x, const std::string &file_name)
{
	std::ifstream read_file(file_name);
	assert(read_file.is_open());

	std::copy(std::istream_iterator<double>(read_file), std::istream_iterator<double>(),
			std::back_inserter(x));

	read_file.close();
}

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

	po::options_description desc("Options");
	desc.add_options()
		("M", po::value<int>()->required(), "No. of simulated markers")
		("N", po::value<int>()->required(), "No. of simulated individuals")
		("iter", po::value<int>()->default_value(5000), "No. of Gibbs iterations")
		("pNZ", po::value<double>()->default_value(0.5), "Proportion nonzero")
		("input", po::value<std::string>()->default_value("none"),"Input filename")
		("out", po::value<std::string>()->default_value("BayesC_out"),"Output filename")
	;

	po::variables_map vm;
	po::store(po::parse_command_line(argc,argv,desc),vm);
	po::notify(vm);

	int M=vm["M"].as<int>();
	int N=vm["N"].as<int>();
	int iter=vm["iter"].as<int>();
	string input=vm["input"].as<string>();
	string output=vm["out"].as<string>();

	MatrixXd X(N,M);
	VectorXd Y(N);

	//beta coefficients
	VectorXd beta_true(M);
	beta_true.setZero();

	int i,j,k,l,m=0;

	//Was an input matrix given?

	if (input!="none"){ //Either read input tables for X and Y
		ifstream f1(input+".X");
		//f1 >> m >> n;
		for (int i = 0; i < N; i++)
		{
			for (int j = 0; j < M; j++)
			{
				f1 >> X(i,j);
				//cout<<X(i,j)<<endl;
			}
		}
		f1.close();
		cout<<"finished reading matrix X!"<<endl;

		std::vector<double> Y_in;
		ReadFromFile(Y_in, input+".Y");
		double* ptr_Y = &Y_in[0];
		Eigen::Map<Eigen::VectorXd> Y1(ptr_Y, Y_in.size());
		cout<<"finished reading vector Y!"<<endl;
		if(Y_in.size()!=N){cout<<"input Y vector size doesnt much the size indicated in the command line"<<endl;return 0;}
		Y=Y1;
		Y_in.clear();

	}else //or simulate
	{
		double pNZ=vm["pNZ"].as<double>();
		double sigmaY_true=1;
		double sigmab_true=1;
		int MT=pNZ*M;

		//Fill Genotype matrix
		for (i=0;i<N;i++){
			for (j=0;j<M;j++){
				X(i,j)=rnorm(0,1);
			}
		}
		for (i=0;i<MT;i++){
			beta_true[i]=rnorm(0,sigmab_true);
		}

		//error
		VectorXd error(N);
		for (i=0;i<N;i++){
			error[i]=rnorm(0,sigmaY_true);
		}

		//construct phenotypes
		Y=X*beta_true;
		Y+=error;
	}

	//normalize
	RowVectorXd mean = X.colwise().mean();
	RowVectorXd sd = ((X.rowwise() - mean).array().square().colwise().sum() / (X.rows() - 1)).sqrt();
	X = (X.rowwise() - mean).array().rowwise() / sd.array();

	double Emu=0;
	VectorXd vEmu(N);
	vEmu.setOnes();

	VectorXd Ebeta(M);
	Ebeta.setZero();
	VectorXd ny(M);
	ny.setZero();
	double Ew=0.5;
	//residual error
	VectorXd epsilon(N);

	epsilon=Y-X*Ebeta-vEmu*Emu;

	std::vector<int> markerI;
	for (int i=0; i<M; ++i) {
		markerI.push_back(i);
	}
	int marker=0;

	//non-zero variable NZ
	int NZ=0;

	double Sigma2_e=epsilon.squaredNorm()/(N*0.5);
	double Sigma2_b=rbeta(1,1);

	//Standard parameterization of hyperpriors for variances
	//double v0E=0.001,s0E=0.001,v0B=0.001,s0B=0.001;

	// Alternative parameterization of hyperpriors for variances
	double v0E=4,v0B=4;
	double s0B=((v0B-2)/v0B)*Sigma2_b;
	double s0E=((v0E-2)/v0E)*Sigma2_e;

	//pre-computed elements for calculations
	VectorXd el1(M);
	for (int i=0; i<M; ++i) {
		el1[i]=X.col(i).transpose()*X.col(i);
	}

	std::ofstream ofs;
	ofs.open(output+"_estimates.txt");
	for (int i=0; i<M; ++i) {
		ofs << "beta_" <<i<< ' ';
	}
	for (int i=0; i<M; ++i) {
		ofs << "incl_" <<i<< ' ';
	}
	ofs << "Ew" << " ";
	ofs << "Sigma2_b" << " ";
	ofs << "Sigma2_e" << " ";
	ofs << "\n";
	ofs.close();

	//begin GIBBS sampling iterations

	ofs.open (output+"_estimates.txt", std::ios_base::app);
	for (i=0;i<iter;i++){

		Emu=sample_mu(N,Sigma2_e,Y,X,Ebeta);

		//sample effects and probabilities jointly
		std::random_shuffle(markerI.begin(), markerI.end());
		for (j=0;j<M;j++){
			marker=markerI[j];

			epsilon=epsilon+X.col(marker)*Ebeta[marker];

			double Cj=el1[marker]+Sigma2_e/Sigma2_b; //adjusted variance
			double rj=X.col(marker).transpose()*epsilon; // mean

			double ratio=(((exp(-(pow(rj,2))/(2*Cj*Sigma2_e))*sqrt((Sigma2_b*Cj)/Sigma2_e))));
			ratio=Ew/(Ew+ratio*(1-Ew));
			ny[marker]=rbernoulli(ratio);

			if (ny[marker]==0){
				Ebeta[marker]=0;
			}
			else if (ny[marker]==1){
				Ebeta[marker]=rnorm(rj/Cj,Sigma2_e/Cj);
			}

			epsilon=epsilon-X.col(marker)*Ebeta[marker];

		}
		for (j=0;j<M;j++){
			ofs << Ebeta[j] << " ";
		}
		for (j=0;j<M;j++){
			ofs << ny[j] << " ";
		}
		NZ=ny.sum();
		//cout<<NZ<<endl;

		Ew=sample_w(M,NZ);
		epsilon=Y-X*Ebeta-vEmu*Emu;

		Sigma2_b=sample_sigma2_b(Ebeta,NZ,v0B,s0B);
		Sigma2_e=sample_sigma2_e(N,epsilon,v0E,s0E);

		ofs << Ew << " ";
		ofs << Sigma2_b << " ";
		ofs << Sigma2_e << " ";
		ofs << "\n";

	}
	ofs.close();

if (input=="none"){
	//write to files
	ofstream myfile1;
	myfile1.open (output+"_simulated_Y.txt");
	for (i=0;i<N;i++){
		myfile1 << Y[i] << ' ';
	}
	myfile1 << endl;
	myfile1.close();

	ofstream myfile2;
	myfile2.open (output+"_simulated_X.txt");
	for (i=0;i<N;i++){
		for (j=0;j<M;j++){
			myfile2<<X(i,j)<< ' ';
		}
		myfile2<<endl;
	}
	myfile2.close();

	ofstream myfile3;
	myfile3.open (output+"_simulated_betatrue.txt");
	myfile3 << beta_true << ' ';
	myfile3.close();
}

	return 0;
}


// Copyright (c) 2005-2009  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL$
// $Id$
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Sebastien Loriot, Sylvain Pion

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/CGAL_Ipelet_base.h>
#include <boost/format.hpp>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>

namespace CGAL_mesh_2{
  typedef CGAL::Exact_predicates_inexact_constructions_kernel       Kernel;
  typedef CGAL::Triangulation_vertex_base_2<Kernel>                 Vb;
  typedef CGAL::Delaunay_mesh_face_base_2<Kernel>                   Fb;
  typedef CGAL::Triangulation_data_structure_2<Vb, Fb>              Tds;
  typedef CGAL::Exact_predicates_tag                                     Itag;
  typedef CGAL::Constrained_Delaunay_triangulation_2<Kernel,Tds,Itag>    CDT;
  typedef CGAL::Delaunay_mesh_size_criteria_2<CDT>                  Criteria;
  typedef CGAL::Delaunay_mesher_2<CDT, Criteria>                    Mesher;

const std::string sublabel[] ={
  "Mesh_2", "Help"
};

const std::string helpmsg[] = {
  "Mesh a polygon using CGAL::Mesh_2; Use circle centers for seeds"
};

struct IpeletMesh2
  : CGAL::Ipelet_base<Kernel,2>
{
  IpeletMesh2()
    : CGAL::Ipelet_base<Kernel,2>("Mesh_2",sublabel, helpmsg) {}

  void protected_run(int);
};

void IpeletMesh2::protected_run(int fn)
{
  if (fn==1) {
    show_help();
    return;
  }

  std::list<Point_2> list_of_seeds;

  std::list<Point_2> pt_list;
  std::list<Segment_2> sg_list;
  std::list<Circle_2> cir_list;
  std::list<Polygon_2> pol_list;

  Iso_rectangle_2 bbox=
    read_active_objects(
      CGAL::dispatch_or_drop_output<Point_2,Polygon_2,Circle_2,Segment_2>(
        std::back_inserter(pt_list),
        std::back_inserter(pol_list),
        std::back_inserter(cir_list),
        std::back_inserter(sg_list)
      )
    );

  if (pt_list.empty() && sg_list.empty() && pol_list.empty()) {
    print_error_message("No mark selected");
    return;
  }

  CDT cdt;

  for (std::list<Point_2>::iterator it=pt_list.begin();it!=pt_list.end();++it)
    cdt.insert(*it);
  for (std::list<Segment_2>::iterator it=sg_list.begin();it!=sg_list.end();++it)
    cdt.insert_constraint(it->point(0),it->point(1));
  for (std::list<Polygon_2>::iterator it=pol_list.begin();it!=pol_list.end();++it)
    for(Polygon_2::Edge_const_iterator edge_it=it->edges_begin();edge_it!=it->edges_end();++edge_it)
      cdt.insert_constraint(edge_it->point(0),edge_it->point(1));
  for (std::list<Circle_2>::iterator it=cir_list.begin();it!=cir_list.end();++it)
    list_of_seeds.push_back(it->center());

  double alpha=0;

  int x=static_cast<int>( floor((bbox.max)().x()-(bbox.min)().x()) );
  int y=static_cast<int>( floor((bbox.max)().y()-(bbox.min)().y()) );

  int ret_val;
  boost::tie(ret_val,alpha)=request_value_from_user<double>((boost::format("Max edge length (BBox %1%x%2%)") % x % y).str() );
  if (ret_val == -1) return;

  if(alpha<0){
    print_error_message("Not a good value");
    return;
  }

  if (list_of_seeds.empty()){
    Mesher mesher(cdt);
    mesher.set_criteria(Criteria(0.125, alpha));
    mesher.refine_mesh();
  }
  else
    CGAL::refine_Delaunay_mesh_2(cdt,list_of_seeds.begin(), list_of_seeds.end(),
      Criteria(0.125, alpha));

  for (CDT::Finite_edges_iterator it=cdt.finite_edges_begin(); it!=cdt.finite_edges_end();++it)
    if (it->first->is_in_domain() || it->first->neighbor(it->second)->is_in_domain())
      draw_in_ipe(cdt.segment(*it));

  group_selected_objects_();
}

}

CGAL_IPELET(CGAL_mesh_2::IpeletMesh2)


///////////////////////////////////////////////////////////////////////////////
// Copyright Christopher Kormanyos 2013 - 2014.
// Copyright John Maddock 2013.
// 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)
//
// This work is based on an earlier work:
// "Algorithm 910: A Portable C++ Multiple-Precision System for Special-Function Calculations",
// in ACM TOMS, {VOL 37, ISSUE 4, (February 2011)} (C) ACM, 2011. http://doi.acm.org/10.1145/1916461.1916469
//

#include <algorithm>
#include <cstdint>
#include <deque>
#include <functional>
#include <iostream>
#include <limits>
#include <numeric>
#include <vector>
#include <boost/math/constants/constants.hpp>
#include <boost/noncopyable.hpp>

//#define USE_CPP_BIN_FLOAT
#define USE_CPP_DEC_FLOAT
//#define USE_MPFR

#if !defined(DIGIT_COUNT)
#define DIGIT_COUNT 100
#endif

#if !defined(BOOST_NO_CXX11_HDR_CHRONO)
  #include <chrono>
  #define STD_CHRONO std::chrono
#else
  #include <boost/chrono.hpp>
  #define STD_CHRONO boost::chrono
#endif

#if defined(USE_CPP_BIN_FLOAT)
  #include <boost/multiprecision/cpp_bin_float.hpp>
  typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<DIGIT_COUNT + 10> > mp_type;
#elif defined(USE_CPP_DEC_FLOAT)
  #include <boost/multiprecision/cpp_dec_float.hpp>
  typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<DIGIT_COUNT + 10> > mp_type;
#elif defined(USE_MPFR)
  #include <boost/multiprecision/mpfr.hpp>
  typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<DIGIT_COUNT + 10> > mp_type;
#else
  #error no multiprecision floating type is defined
#endif

template <class clock_type>
struct stopwatch
{
public:
  typedef typename clock_type::duration duration_type;

  stopwatch() : m_start(clock_type::now()) { }

  stopwatch(const stopwatch& other) : m_start(other.m_start) { }

  stopwatch& operator=(const stopwatch& other)
  {
    m_start = other.m_start;
    return *this;
  }

  ~stopwatch() { }

  duration_type elapsed() const
  {
    return (clock_type::now() - m_start);
  }

  void reset()
  {
    m_start = clock_type::now();
  }

private:
  typename clock_type::time_point m_start;
};

namespace my_math
{
  template<class T> T chebyshev_t(const std::int32_t n, const T& x);

  template<class T> T chebyshev_t(const std::uint32_t n, const T& x, std::vector<T>* vp);

  template<class T> bool isneg(const T& x) { return (x < T(0)); }

  template<class T> const T& zero() { static const T value_zero(0); return value_zero; }
  template<class T> const T& one () { static const T value_one (1); return value_one; }
  template<class T> const T& two () { static const T value_two (2); return value_two; }
}

namespace orthogonal_polynomial_series
{
  template<typename T> static inline T orthogonal_polynomial_template(const T& x, const std::uint32_t n, std::vector<T>* const vp = static_cast<std::vector<T>*>(0u))
  {
    // Compute the value of an orthogonal chebyshev polinomial.
    // Use stable upward recursion.

    if(vp != nullptr)
    {
      vp->clear();
      vp->reserve(static_cast<std::size_t>(n + 1u));
    }

    T y0 = my_math::one<T>();

    if(vp != nullptr) { vp->push_back(y0); }

    if(n == static_cast<std::uint32_t>(0u))
    {
      return y0;
    }

    T y1 = x;

    if(vp != nullptr) { vp->push_back(y1); }

    if(n == static_cast<std::uint32_t>(1u))
    {
      return y1;
    }

    T a = my_math::two <T>();
    T b = my_math::zero<T>();
    T c = my_math::one <T>();

    T yk;

    // Calculate higher orders using the recurrence relation.
    // The direction of stability is upward recursion.
    for(std::int32_t k = static_cast<std::int32_t>(2); k <= static_cast<std::int32_t>(n); ++k)
    {
      yk = (((a * x) + b) * y1) - (c * y0);

      y0 = y1;
      y1 = yk;

      if(vp != nullptr) { vp->push_back(yk); }
    }

    return yk;
  }
}

template<class T> T my_math::chebyshev_t(const std::int32_t n, const T& x)
{
  if(my_math::isneg(x))
  {
    const bool b_negate = ((n % static_cast<std::int32_t>(2)) != static_cast<std::int32_t>(0));

    const T y = chebyshev_t(n, -x);

    return (!b_negate ? y : -y);
  }

  if(n < static_cast<std::int32_t>(0))
  {
    const std::int32_t nn = static_cast<std::int32_t>(-n);

    return chebyshev_t(nn, x);
  }
  else
  {
    return orthogonal_polynomial_series::orthogonal_polynomial_template(x, static_cast<std::uint32_t>(n));
  }
}

template<class T> T my_math::chebyshev_t(const std::uint32_t n, const T& x, std::vector<T>* const vp) { return orthogonal_polynomial_series::orthogonal_polynomial_template(x, static_cast<std::int32_t>(n),  vp); }

namespace util
{
  template <class T> float digit_scale()
  {
    const int d = ((std::max)(std::numeric_limits<T>::digits10, 15));
    return static_cast<float>(d) / 300.0F;
  }
}

namespace examples
{
  namespace nr_006
  {
    template<typename T> class hypergeometric_pfq_base : private boost::noncopyable
    {
    public:
      virtual ~hypergeometric_pfq_base() { }

      virtual void ccoef() const = 0;

      virtual T series() const
      {
        using my_math::chebyshev_t;

        // Compute the Chebyshev coefficients.
        // Get the values of the shifted Chebyshev polynomials.
        std::vector<T> chebyshev_t_shifted_values;
        const T z_shifted = ((Z / W) * static_cast<std::int32_t>(2)) - static_cast<std::int32_t>(1);

        chebyshev_t(static_cast<std::uint32_t>(C.size()),
                    z_shifted,
                    &chebyshev_t_shifted_values);

        // Luke: C     ---------- COMPUTE SCALE FACTOR                       ----------
        // Luke: C
        // Luke: C     ---------- SCALE THE COEFFICIENTS                     ----------
        // Luke: C

        // The coefficient scaling is preformed after the Chebyshev summation,
        // and it is carried out with a single division operation.
        bool b_neg = false;

        const T scale = std::accumulate(C.begin(),
                                        C.end(),
                                        T(0),
                                        [&b_neg](T scale_sum, const T& ck) -> T
                                        {
                                          ((!b_neg) ? (scale_sum += ck) : (scale_sum -= ck));
                                          b_neg = (!b_neg);
                                          return scale_sum;
                                        });

        // Compute the result of the series expansion using unscaled coefficients.
        const T sum = std::inner_product(C.begin(),
                                         C.end(),
                                         chebyshev_t_shifted_values.begin(),
                                         T(0));

        // Return the properly scaled result.
        return sum / scale;
      }

    protected:
      const   T             Z;
      const   T             W;
      mutable std::deque<T> C;

      hypergeometric_pfq_base(const T& z,
                              const T& w) : Z(z),
                                            W(w),
                                            C(0u) { }

      virtual std::int32_t N() const { return static_cast<std::int32_t>(util::digit_scale<T>() * 500.0F); }
    };

    template<typename T> class ccoef4_hypergeometric_0f1 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef4_hypergeometric_0f1(const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              CP(c) { }

      virtual ~ccoef4_hypergeometric_0f1() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 80.
        const std::int32_t N1 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X1 = N2;

        T C1 = T(1) - CP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          const T DIVFAC = T(1) / X1;

          --X1;

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T term =   (A2 - ((A3 * DIVFAC) * X1))
                         + ((A2 * X1) * ((1 + (C1 + X1)) * Z1))
                         + ((A1 * X1) * ((DIVFAC - (C1 * Z1)) + (X1 * Z1)));

          hypergeometric_pfq_base<T>::C.push_front(term);

          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T CP;
    };

    template<typename T> class ccoef1_hypergeometric_1f0 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef1_hypergeometric_1f0(const T& a,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a) { }

      virtual ~ccoef1_hypergeometric_1f0() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 67.
        const std::int32_t N1 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X1 = N2;

        T V1 = T(1) - AP;

        // Here, we have corrected what appears to be an error in Luke's code.

        // Luke's original code listing has:
        //  AFAC = 2 + FOUR/W
        // But it appears as though the correct form is:
        //  AFAC = 2 - FOUR/W.

        const T AFAC = 2 - (T(4) / hypergeometric_pfq_base<T>::W);

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X1;

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T term = -(((X1 * AFAC) * A1) + ((X1 + V1) * A2)) / (X1 - V1);

          hypergeometric_pfq_base<T>::C.push_front(term);

          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;

      virtual std::int32_t N() const { return static_cast<std::int32_t>(util::digit_scale<T>() * 1600.0F); }
    };

    template<typename T> class ccoef3_hypergeometric_1f1 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef3_hypergeometric_1f1(const T& a,
                                const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a),
                                              CP(c) { }

      virtual ~ccoef3_hypergeometric_1f1() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 74.
        const std::int32_t N1 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X  = N1;
        std::int32_t X1 = N2;

        T XA  =  X + AP;
        T X3A = (X + 3) - AP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X;
          --X1;
          --XA;
          --X3A;

          const T X3A_over_X2 = X3A / static_cast<std::int32_t>(X + 2);

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T PART1 =  A1 * (((X + CP) * Z1) - X3A_over_X2);
          const T PART2 =  A2 * (Z1 * ((X + 3) - CP) + (XA / X1));
          const T PART3 =  A3 * X3A_over_X2;

          const T term = (((PART1 + PART2) + PART3) * X1) / XA;

          hypergeometric_pfq_base<T>::C.push_front(term);

          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;
      const T CP;
    };

    template<typename T> class ccoef6_hypergeometric_1f2 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef6_hypergeometric_1f2(const T& a,
                                const T& b,
                                const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a),
                                              BP(b),
                                              CP(c) { }

      virtual ~ccoef6_hypergeometric_1f2() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 85.
        const std::int32_t N1 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(1));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A4(0);
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X  = N1;
        T            PP = X + AP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X;
          --PP;

          const std::int32_t TWO_X    = static_cast<std::int32_t>(X * 2);
          const std::int32_t X_PLUS_1 = static_cast<std::int32_t>(X + 1);
          const std::int32_t X_PLUS_3 = static_cast<std::int32_t>(X + 3);
          const std::int32_t X_PLUS_4 = static_cast<std::int32_t>(X + 4);

          const T QQ = T(TWO_X + 3) / static_cast<std::int32_t>(TWO_X + static_cast<std::int32_t>(5));
          const T SS = (X + BP) * (X + CP);

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T PART1 =   A1 * (((PP - (QQ * (PP + 1))) * 2) + (SS * Z1));
          const T PART2 =  (A2 * (X + 2)) * ((((TWO_X + 1) * PP) / X_PLUS_1) - ((QQ * 4) * (PP + 1)) + (((TWO_X + 3) * (PP + 2)) / X_PLUS_3) + ((Z1 * 2) * (SS - (QQ * (X_PLUS_1 + BP)) * (X_PLUS_1 + CP))));
          const T PART3 =   A3 * ((((X_PLUS_3 - AP) - (QQ * (X_PLUS_4 - AP))) * 2) + (((QQ * Z1) * (X_PLUS_4 - BP)) * (X_PLUS_4 - CP)));
          const T PART4 = ((A4 * QQ) * (X_PLUS_4 - AP)) / X_PLUS_3;

          const T term = (((PART1 - PART2) + (PART3 - PART4)) * X_PLUS_1) / PP;

          hypergeometric_pfq_base<T>::C.push_front(term);

          A4 = A3;
          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;
      const T BP;
      const T CP;
    };

    template<typename T> class ccoef2_hypergeometric_2f1 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef2_hypergeometric_2f1(const T& a,
                                const T& b,
                                const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a),
                                              BP(b),
                                              CP(c) { }

      virtual ~ccoef2_hypergeometric_2f1() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 59.
        const std::int32_t N1 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X  = N1;
        std::int32_t X1 = N2;
        std::int32_t X3 = static_cast<std::int32_t>((X * 2) + 3);

        T X3A = (X + 3) - AP;
        T X3B = (X + 3) - BP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X;
          --X1;
          --X3A;
          --X3B;
          X3 -= 2;

          const std::int32_t X_PLUS_2 = static_cast<std::int32_t>(X + 2);

          const T XAB = T(1) / ((X + AP) * (X + BP));

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T PART1 = (A1 * X1) * (2 - (((AP + X1) * (BP + X1)) * ((T(X3) / X_PLUS_2) * XAB)) + ((CP + X) * (XAB * Z1)));
          const T PART2 = (A2 * XAB) * ((X3A * X3B) - (X3 * ((X3A + X3B) - 1)) + (((3 - CP) + X) * (X1 * Z1)));
          const T PART3 = (A3 * X1) * (X3A / X_PLUS_2) * (X3B * XAB);

          const T term = (PART1 + PART2) - PART3;

          hypergeometric_pfq_base<T>::C.push_front(term);

          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;
      const T BP;
      const T CP;

      virtual std::int32_t N() const { return static_cast<std::int32_t>(util::digit_scale<T>() * 1600.0F); }
    };

    template<class T> T luke_ccoef4_hypergeometric_0f1(const T& a, const T& x);
    template<class T> T luke_ccoef1_hypergeometric_1f0(const T& a, const T& x);
    template<class T> T luke_ccoef3_hypergeometric_1f1(const T& a, const T& b, const T& x);
    template<class T> T luke_ccoef6_hypergeometric_1f2(const T& a, const T& b, const T& c, const T& x);
    template<class T> T luke_ccoef2_hypergeometric_2f1(const T& a, const T& b, const T& c, const T& x);
  }
}

template<class T>
T examples::nr_006::luke_ccoef4_hypergeometric_0f1(const T& a, const T& x)
{
  const ccoef4_hypergeometric_0f1<T> hypergeometric_0f1_object(a, x, T(-20));

  hypergeometric_0f1_object.ccoef();

  return hypergeometric_0f1_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef1_hypergeometric_1f0(const T& a, const T& x)
{
  const ccoef1_hypergeometric_1f0<T> hypergeometric_1f0_object(a, x, T(-20));

  hypergeometric_1f0_object.ccoef();

  return hypergeometric_1f0_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef3_hypergeometric_1f1(const T& a, const T& b, const T& x)
{
  const ccoef3_hypergeometric_1f1<T> hypergeometric_1f1_object(a, b, x, T(-20));

  hypergeometric_1f1_object.ccoef();

  return hypergeometric_1f1_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef6_hypergeometric_1f2(const T& a, const T& b, const T& c, const T& x)
{
  const ccoef6_hypergeometric_1f2<T> hypergeometric_1f2_object(a, b, c, x, T(-20));

  hypergeometric_1f2_object.ccoef();

  return hypergeometric_1f2_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef2_hypergeometric_2f1(const T& a, const T& b, const T& c, const T& x)
{
  const ccoef2_hypergeometric_2f1<T> hypergeometric_2f1_object(a, b, c, x, T(-20));

  hypergeometric_2f1_object.ccoef();

  return hypergeometric_2f1_object.series();
}

int main()
{
  stopwatch<STD_CHRONO::high_resolution_clock> my_stopwatch;
  float total_time = 0.0F;

  std::vector<mp_type> hypergeometric_0f1_results(20U);
  std::vector<mp_type> hypergeometric_1f0_results(20U);
  std::vector<mp_type> hypergeometric_1f1_results(20U);
  std::vector<mp_type> hypergeometric_2f1_results(20U);
  std::vector<mp_type> hypergeometric_1f2_results(20U);

  const mp_type a(mp_type(3) / 7);
  const mp_type b(mp_type(2) / 3);
  const mp_type c(mp_type(1) / 4);

  std::int_least16_t i;

  std::cout << "test hypergeometric_0f1." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric0F1.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{}, {3/7}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_0f1_results.begin(),
                hypergeometric_0f1_results.end(),
                [&i, &a](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef4_hypergeometric_0f1(a, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric0F1.
  std::for_each(hypergeometric_0f1_results.begin(),
                hypergeometric_0f1_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_1f0." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric1F0.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7}, {}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_1f0_results.begin(),
                hypergeometric_1f0_results.end(),
                [&i, &a](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef1_hypergeometric_1f0(a, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric1F0.
  std::for_each(hypergeometric_1f0_results.begin(),
                hypergeometric_1f0_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_1f1." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric1F1.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7}, {2/3}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_1f1_results.begin(),
                hypergeometric_1f1_results.end(),
                [&i, &a, &b](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef3_hypergeometric_1f1(a, b, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric1F1.
  std::for_each(hypergeometric_1f1_results.begin(),
                hypergeometric_1f1_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_1f2." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric1F2.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7}, {2/3, 1/4}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_1f2_results.begin(),
                hypergeometric_1f2_results.end(),
                [&i, &a, &b, &c](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef6_hypergeometric_1f2(a, b, c, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric1F2.
  std::for_each(hypergeometric_1f2_results.begin(),
                hypergeometric_1f2_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_2f1." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric2F1.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7, 2/3}, {1/4}, -(i * EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_2f1_results.begin(),
                hypergeometric_2f1_results.end(),
                [&i, &a, &b, &c](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef2_hypergeometric_2f1(a, b, c, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric2F1.
  std::for_each(hypergeometric_2f1_results.begin(),
                hypergeometric_2f1_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "Total execution time = " << std::setprecision(3) << total_time << "s" << std::endl;
}


#include <dynamic_reconfigure/server.h>
#include <eigen_conversions/eigen_msg.h>
#include <Eigen/Geometry>
#include <deque>
#include "geometry_msgs/WrenchStamped.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "sub8_controller/GainConfig.h"
#include "sub8_msgs/Trajectory.h"
#include "sub8_msgs/Waypoint.h"

typedef std::deque<Eigen::Vector3d> ControllerDeque;

class PDController
{
  /*
    [1] One day, all the land will be boxplus
        http://arxiv.org/pdf/1107.1119.pdf
    TODO: Make this an abstract class
  */
public:
  PDController();

private:
  // ROS Miscellanea
  ros::NodeHandle nh;
  ros::Subscriber waypoint_sub;
  ros::Subscriber truth_sub;
  ros::Publisher wrench_pub;
  ros::Timer control_timer;
  dynamic_reconfigure::Server<sub8_controller::GainConfig> server;
  dynamic_reconfigure::Server<sub8_controller::GainConfig>::CallbackType reconfigure_callback;

  bool ready = false;  // Wait until we're ready
  bool got_state = false;
  bool got_target = false;
  double control_period = 0.02;

  // Pose-trackings
  struct PoseStruct
  {
    Eigen::Vector3d position;
    Eigen::Matrix3d orientation;
    Eigen::Vector3d linear_velocity;
    Eigen::Vector3d angular_velocity;
  };

  sub8_msgs::Trajectory::ConstPtr current_trajectory;
  PoseStruct target_state;
  PoseStruct current_state;

  // Control Parameters
  unsigned int waypoint_index = 0;
  unsigned int trans_history_length = 75;
  unsigned int angle_history_length = 75;
  ControllerDeque translation_error_history;
  ControllerDeque orientation_error_history;

  double waypoint_achievement_distance = 0.4;

  double kp_trans = 30;
  double kd_trans = 13;
  double ki_trans = 25;

  double kp_angle = 29;
  double kd_angle = 14;
  double ki_angle = 5;
  double sub_mass = 25;  // kg

  // Callbacks
  void gain_callback(sub8_controller::GainConfig &config, uint32_t level);

  void trajectory_callback(const sub8_msgs::Trajectory::ConstPtr &);
  void truth_callback(const nav_msgs::Odometry::ConstPtr &);
  geometry_msgs::WrenchStamped msg_from_wrench(Eigen::Vector3d &);
  geometry_msgs::WrenchStamped msg_from_wrench(Eigen::Vector3d &, Eigen::Vector3d);

  // Math
  void append_to_history(ControllerDeque &history, Eigen::Vector3d &datum, unsigned int length);
  Eigen::AngleAxis<double> rotation_difference(Eigen::Matrix3d &rotation_a, Eigen::Matrix3d &rotation_b);
  Eigen::Vector3d estimate_derivative(ControllerDeque &);
  Eigen::Vector3d estimate_integral(ControllerDeque &history);

  // Control
  void control_loop(const ros::TimerEvent &);
  void set_target(unsigned int waypoint_index);
};


//
// Created by leanne on 11/23/20.
//

#include "utility.h"
#include "VWAP.h"
#include <ctime>
#include <cmath>
#include <random>
#include <algorithm>
#include <cstdio>
#include <boost/timer/timer.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/variance.hpp>

using std::vector;
using std::ofstream;
using namespace boost::accumulators;
using timer = boost::timer::auto_cpu_timer;

void VWAPOption::computefirst2Moments(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::moment<1>,tag::variance(immediate) > > moment1;
    accumulator_set<double, stats<tag::moment<2>,tag::variance(immediate) > > moment2;

    //compute estimate for 1st moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment1(VWAP_T);
    }

    //compute estimate for 2nd moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment2(VWAP_T);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "E(VWAP) = " << moment<1>(moment1) << std::endl;
    cout << "E(VWAP^2) = " << moment<2>(moment2) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<1>(moment1) //E(VWAP)
         << ", "
         << sqrt(variance(moment1)) //StdErr(VWAP)
         << ", "
         << moment<2>(moment2) //E(VWAP)
         << ", "
         << sqrt(variance(moment2)) //StdErr(VWAP^2)
         << std::endl;
}

void VWAPOption::computeMoment3(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::moment<3> > > moment3;

    //compute estimate for 3rd moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment3(VWAP_T);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "E(VWAP^3) = " << moment<3>(moment3) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<3>(moment3) //E(VWAP^3)
         << std::endl;
}

void VWAPOption::computePrice(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::mean, tag::variance(immediate) > > price;

    //compute estimate for VWAP price
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        if((VWAP_T - K_ ) > 0)
        {
            price(VWAP_T - K_);
        }
        else
        {
            price(0.0);
        }
    }
    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "price = " << mean(price) << std::endl;
    cout << "stdDevPrice = " << sqrt(variance(price)) << std::endl;

    file << sigmaPrice_
         << ", "
         << mean(price) //option price
         << ", "
         << sqrt(variance(price)) << std::endl;

}
void VWAPOption::computeMoment3AndPrice(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::moment<3> > > moment3;
    accumulator_set<double, stats<tag::mean, tag::variance(immediate) > > price;

    //compute estimate for 3rd moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment3(VWAP_T);
    }

    //compute estimate for VWAP price
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        if((VWAP_T - K_ ) > 0)
            price(VWAP_T - K_);
        else
            price(0.0);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(2) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "E(VWAP^3) = " << moment<3>(moment3) << std::endl;
    cout << "price = " << mean(price) << std::endl;
    cout << "stdDevPrice = " << sqrt(variance(price)) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<3>(moment3) //E(VWAP^3)
         << ", "
         << mean(price) //option price
         << ", "
         << sqrt(variance(price)) << std::endl;

}

void VWAPOption::computeParameters(ofstream& file) {
    using std::cout;
    timer t;
    for (int i = 0; i < NPaths_; ++i) {

        double VWAP_T = simulateSinglePath();

        VWAPMoments_(VWAP_T);

        if((VWAP_T - K_ ) > 0)
            callOptionPayoffs_(VWAP_T - K_);
        else
            callOptionPayoffs_(0.0);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "Delta = " << delta_ << std::endl;
    cout << "E(VWAP) = " << moment<1>(VWAPMoments_) << std::endl;
    cout << "E(VWAP^2) = " << moment<2>(VWAPMoments_) << std::endl;
    cout << "E(VWAP^3) = " << moment<3>(VWAPMoments_) << std::endl;
    cout << "price = " << mean(callOptionPayoffs_) << std::endl;
    cout << "stdDev = " << sqrt(variance(callOptionPayoffs_)) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<1>(VWAPMoments_) //E(VWAP)
         << ", "
         << moment<2>(VWAPMoments_) //E(VWAP^2)
         << ", "
         << moment<3>(VWAPMoments_) //E(VWAP^3)
         << ", "
         << mean(callOptionPayoffs_) //option price
         << ", "
         << sqrt(variance(callOptionPayoffs_)) << std::endl;
}

double VWAPOption::GetNormDistrDouble(std::mt19937 &gen)
{
    double val = 1;
    std::normal_distribution<> nd;
    //   val = nd(gen);
    do { val = nd(gen);}
    while(val < 0.4 || val > 2.5);

    return val;
}

//return a realisation of VWAP_T
double VWAPOption::simulateSinglePath() {
    int seed = -107;
    std::mt19937 gen(seed);

    double sumStUt = S0_*X0_*X0_;
    double sumUt   = X0_*X0_;

    double Sti = S0_;
    double Xti = X0_;
    double Uti = 0.0;
    const double muX = a_*(1-exp(-lambda_ * delta_));
    const double sigmaX2 = pow(sigmaVol_,2)/(2*lambda_)*(1-exp(-2*lambda_*delta_));

    //simulate path over [0,T]
    for (int i = 1; i <= numIncrements_; i++) {
        Sti = Sti*exp((mu_-0.5*sigmaPrice_*sigmaPrice_)*delta_
                      + sigmaPrice_*sqrt(delta_))*GetNormDistrDouble(gen);

        Xti = exp(-lambda_*delta_)*Xti + muX + sqrt(sigmaX2)*GetNormDistrDouble(gen);
        Uti = pow(Xti,2);

        sumStUt+= Sti*Uti;
        sumUt += Uti;
    }

    return (sumStUt/sumUt);
}


#include <fstream>
#include <iostream>
#include <experimental/filesystem>
#include <stdio.h>
#include <random>
#include <string>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "../tasks.hh"
#include "../classes/BinnedTypedMatrix.hh"

namespace fs = std::experimental::filesystem;

using namespace EMC;

using namespace boost::numeric::ublas;

/// from  https://stackoverflow.com/questions/440133/how-do-i-create-a-random-alpha-numeric-string-in-c
std::string random_string(std::string::size_type length) {
	static auto& chrs = "0123456789"
			"abcdefghijklmnopqrstuvwxyz"
			"ABCDEFGHIJKLMNOPQRSTUVWXYZ";

	thread_local static std::mt19937 rg { std::random_device { }() };
	thread_local static std::uniform_int_distribution<std::string::size_type> pick(
			0, sizeof(chrs) - 2);

	std::string s;

	s.reserve(length);

	while (length--)
		s += chrs[pick(rg)];

	return s;
}

unsigned int modeCount = 5;

void shiftCenters(std::vector<double>& inputVec, unsigned int mode, double error) {
	switch(mode) {
	case 1: {
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) += error;
		}
		break;
	}
	case 2: {
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) += error;
			if (inputVec.at(i) < 0) {
				inputVec.at(i) = 0;
			}
		}
		break;
	}
	case 3: {
		double half = inputVec.size() / 2.0;
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) += error * (i-half) / inputVec.size();
			if (inputVec.at(i) < 0) {
				inputVec.at(i) = 0;
			}
		}
		break;
	}
	case 4: {
		double half = inputVec.size() / 2.0;
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) -= error * (i-half) / inputVec.size();
			if (inputVec.at(i) < 0) {
				inputVec.at(i) = 0;
			}
		}
		break;
	}
	case 0: {
		/// do nothing, mode 0 does not modify bin centers
		break;
	}
	default:
		std::cout << "this should not happen!!" << std::endl;
		break;
	}
}

int EMC::rebin_invert(bool forceOverwrite, std::string input, std::string output,
		int countRate, double minRow, double minCol, double minRowBin,
		double minColBin, double maxRow, double maxCol, double rowError, double colError) {

	if (!forceOverwrite && fs::exists(output)) {
		std::cout << "Output file already exists, use -f to force overwrite." << std::endl;
		return 1;
	}

    std::streambuf* orig_buf = std::cout.rdbuf();
    std::cout.rdbuf(NULL);

	std::string randRebinName = output + "_" + random_string(5) + "_tmp.rebinned.mat";
	rebin(true, input, randRebinName, countRate, minRow, minCol, minRowBin, minColBin, maxRow, maxCol);

	std::ifstream infile(randRebinName);
	BinnedTypedMatrix inputMatrix = BinnedTypedMatrix::readFromFile(infile);
	infile.close();

	fs::remove(randRebinName);

	BinnedTypedMatrix newMat(inputMatrix.columnIndex, inputMatrix.rowIndex, inputMatrix.matType);
	{
		typedef permutation_matrix<std::size_t> pmatrix;
		// create a working copy of the input
		matrix<ValueError> A(inputMatrix.m);
		// create a permutation matrix for the LU-factorization
		pmatrix pm(A.size1());
		// perform LU-factorization
		int res = lu_factorize(A,pm);
		if( res != 0 )
			return false;
		// create identity matrix of "inverse"
		newMat.m.assign(identity_matrix<ValueError>(A.size1()));
		// backsubstitute to get the inverse
		lu_substitute(A, pm, newMat.m);
	}

	std::vector<std::vector<std::vector<real>>> wiggleResults(newMat.rowCount, std::vector<std::vector<real>>(newMat.columnCount, std::vector<real>()));

	for (unsigned int i=0; i<modeCount; i++) {
		for (unsigned int j=0; j<modeCount; j++) {
			if (i==0 && j==0) {
				continue;
			}

		    std::cout.rdbuf(orig_buf);
			std::cout << "mode " << i << " " << j << std::endl;
			std::cout.flush();
		    std::cout.rdbuf(NULL);

			std::string randKey = random_string(5);

		    std::cout.rdbuf(orig_buf);
			std::cout << "shift, ";
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			std::string tmpShiftedMatrixName = output + "_" + randKey + "_tmp.shifted.mat";
			{ // shift matrix by mode
				std::ifstream infile(input);
				BinnedTypedMatrix inputMat = BinnedTypedMatrix::readFromFile(infile);

				double mult = 0.7071067812;

				if (i==0 || j==0)
					mult=1;

				shiftCenters(inputMat.rowIndex, i, mult*rowError);
				shiftCenters(inputMat.columnIndex, j, mult*colError);

				std::ofstream shiftedMatOutputName(tmpShiftedMatrixName);
				inputMat.writeToFile(shiftedMatOutputName);
			}

		    std::cout.rdbuf(orig_buf);
			std::cout << "rebin, ";
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			std::string tmpRebinnedMatName = output + "_" + randKey + "_tmp.rebin.mat";
			rebin(true, tmpShiftedMatrixName, tmpRebinnedMatName, countRate, minRow, minCol, minRowBin, minColBin, maxRow, maxCol);

		    std::cout.rdbuf(orig_buf);
			std::cout << "invert ";
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			std::string tmpInvertedMatName = output + "_" + randKey + "_tmp.inverted.mat";
			invert(true, tmpRebinnedMatName, tmpInvertedMatName);

			std::ifstream infile(tmpInvertedMatName);
			BinnedTypedMatrix rebinnedInvertedMatrix = BinnedTypedMatrix::readFromFile(infile);
			infile.close();

			fs::remove(tmpShiftedMatrixName);
			fs::remove(tmpRebinnedMatName);
			fs::remove(tmpInvertedMatName);

		    std::cout.rdbuf(orig_buf);
			std::cout << "and done!" << std::endl;
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			for (unsigned int rowPos = 0; rowPos < newMat.rowCount; rowPos++) {
				for (unsigned int colPos = 0; colPos < newMat.columnCount; colPos++) {
					wiggleResults.at(rowPos).at(colPos).push_back(rebinnedInvertedMatrix.m(rowPos, colPos).value);
				}
			}
		}
	}

	for (unsigned int rowPos = 0; rowPos < newMat.rowCount; rowPos++) {
		for (unsigned int colPos = 0; colPos < newMat.columnCount; colPos++) {
			newMat.m(rowPos, colPos).err_sq = 0;
			for (unsigned int i = 0; i<(modeCount*modeCount-1); i++) {

				// use this (quadratic mean)
				newMat.m(rowPos, colPos).err_sq += pow(wiggleResults.at(rowPos).at(colPos).at(i)-newMat.m(rowPos, colPos).value, 2) / (modeCount*modeCount-1);

				// or this (linear mean)
				/*newMat.m(rowPos, colPos).err_sq += std::fabs(wiggleResults.at(rowPos).at(colPos).at(i)-newMat.m(rowPos, colPos).value) / (maxMode*maxMode);
				newMat.m(rowPos, colPos).err_sq = newMat.m(rowPos, colPos).err_sq * newMat.m(rowPos, colPos).err_sq;*/
			}
		}
	}

    std::ofstream outputF(output);
    newMat.writeToFile(outputF);
    outputF.close();

    return 0;
}


#pragma once

#include <Eigen/Core>

namespace fluid {
namespace algorithm {
namespace _impl {

index incrementalMeanVariance(const Eigen::Ref<Eigen::ArrayXXd> data,
                              index                             lastSampleCount,
                              Eigen::Ref<Eigen::ArrayXd>        mean,
                              Eigen::Ref<Eigen::ArrayXd>        var)
{

  Eigen::VectorXd rowSums = data.isNaN().select(0, data).colwise().sum();
  index          newSampleCount = data.rows();
  Eigen::ArrayXd lastSum = mean * lastSampleCount;

  if (mean.cols() > 0)
  {
    index updatedSampleCount = lastSampleCount + newSampleCount;
    mean = ((mean * lastSampleCount) + rowSums.array()) / updatedSampleCount;

    if (var.rows() > 0)
    {

      Eigen::ArrayXXd tmp  = (data.transpose().colwise() - (rowSums.array() / newSampleCount)).transpose();
      Eigen::ArrayXd correction = tmp.colwise().sum();
      tmp = tmp.square();

      Eigen::ArrayXd newUnnormalisedVar = tmp.colwise().sum();
      newUnnormalisedVar -= correction.square() / newSampleCount;

      Eigen::ArrayXd lastUnormalisedVar = var * lastSampleCount;

      if(lastSampleCount > 0)
      {
          double lastCountOverNewCount = static_cast<double>(lastSampleCount) / newSampleCount;
          var =(
              lastUnormalisedVar
              + newUnnormalisedVar
              + lastCountOverNewCount
                / updatedSampleCount
                * (lastSum / lastCountOverNewCount - rowSums.array()).square()
                );
      }
      var /= updatedSampleCount;
    }
    return updatedSampleCount;
  }
  return lastSampleCount;
}

} // namespace _impl
} // namespace algorithm
} // namespace fluid


#include <iostream>
#include <string>
#include <vector>

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

// #include <opencv2/core/core.hpp> // needed for verbosity >= 3, DISVISUAL
// #include <opencv2/highgui/highgui.hpp> // needed for verbosity >= 3, DISVISUAL
// #include <opencv2/imgproc/imgproc.hpp> // needed for verbosity >= 3, DISVISUAL

#include <sys/time.h>    // timeof day
#include <stdio.h>

#include "oflow.h"
#include "patchgrid.h"
#include "refine_variational.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  OFClass::OFClass(const float ** im_ao_in, const float ** im_ao_dx_in, const float ** im_ao_dy_in, // expects #sc_f_in pointers to float arrays for images and gradients.
                                                                                // E.g. im_ao[sc_f_in] will be used as coarsest coarsest, im_ao[sc_l_in] as finest scale
                                                                                // im_ao[  (sc_l_in-1) : 0 ] can be left as nullptr pointers
                                                                                // IMPORTANT assumption: mod(width,2^sc_f_in)==0  AND mod(height,2^sc_f_in)==0,
                  const float ** im_bo_in, const float ** im_bo_dx_in, const float ** im_bo_dy_in,
                  const int imgpadding_in,
                  float * outflow,
                  const float * initflow,
                  const int width_in, const int height_in,
                  const int sc_f_in, const int sc_l_in,
                  const int max_iter_in, const int min_iter_in,
                  const float  dp_thresh_in,
                  const float  dr_thresh_in,
                  const float res_thresh_in,
                  const int p_samp_s_in,
                  const float patove_in,
                  const bool usefbcon_in,
                  const int costfct_in,
                  const int noc_in,
                  const int patnorm_in,
                  const bool usetvref_in,
                  const float tv_alpha_in,
                  const float tv_gamma_in,
                  const float tv_delta_in,
                  const int tv_innerit_in,
                  const int tv_solverit_in,
                  const float tv_sor_in,
                  const int verbosity_in)
  : im_ao(im_ao_in), im_ao_dx(im_ao_dx_in), im_ao_dy(im_ao_dy_in),
    im_bo(im_bo_in), im_bo_dx(im_bo_dx_in), im_bo_dy(im_bo_dy_in)
{

  #ifdef WITH_OPENMP
    if (verbosity_in>1)
      cout <<  "OPENMP is ON - used in pconst, pinit, potim";
    #ifdef USE_PARALLEL_ON_FLOWAGGR
    if (verbosity_in>1)
      cout << ", cflow ";
    #endif
    if (verbosity_in>1) cout << endl;
  #endif //DWITH_OPENMP

  // Parse optimization parameters
  #if (SELECTMODE==1)
  op.nop = 2;
  #else
  op.nop = 1;
  #endif
  op.p_samp_s = p_samp_s_in;  // patch has even border length, center pixel is at (p_samp_s/2, p_samp_s/2) (ZERO INDEXED!)
  op.outlierthresh = (float)op.p_samp_s/2;
  op.patove = patove_in;
  op.sc_f = sc_f_in;
  op.sc_l = sc_l_in;
  op.max_iter = max_iter_in;
  op.min_iter = min_iter_in;
  op.dp_thresh = dp_thresh_in*dp_thresh_in; // saves the square to compare with squared L2-norm (saves sqrt operation)
  op.dr_thresh = dr_thresh_in;
  op.res_thresh = res_thresh_in;
  op.steps = std::max(1,  (int)floor(op.p_samp_s*(1-op.patove)));
  op.novals = noc_in * (p_samp_s_in)*(p_samp_s_in);
  op.usefbcon = usefbcon_in;
  op.costfct = costfct_in;
  op.noc = noc_in;
  op.patnorm = patnorm_in;
  op.verbosity = verbosity_in;
  op.noscales = op.sc_f-op.sc_l+1;
  op.usetvref = usetvref_in;
  op.tv_alpha = tv_alpha_in;
  op.tv_gamma = tv_gamma_in;
  op.tv_delta = tv_delta_in;
  op.tv_innerit = tv_innerit_in;
  op.tv_solverit = tv_solverit_in;
  op.tv_sor = tv_sor_in;
  op.normoutlier_tmpbsq = (v4sf) {op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier};
  op.normoutlier_tmp2bsq = __builtin_ia32_mulps(op.normoutlier_tmpbsq, op.twos);
  op.normoutlier_tmp4bsq = __builtin_ia32_mulps(op.normoutlier_tmpbsq, op.fours);

  // Variables for algorithm timings
  struct timeval tv_start_all, tv_end_all, tv_start_all_global, tv_end_all_global;
  if (op.verbosity>0)
    gettimeofday(&tv_start_all_global, nullptr);

  // ... per each scale
  double tt_patconstr[op.noscales], tt_patinit[op.noscales], tt_patoptim[op.noscales], tt_compflow[op.noscales], tt_tvopt[op.noscales], tt_all[op.noscales];
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {
    tt_patconstr[sl-op.sc_l]=0;
    tt_patinit[sl-op.sc_l]=0;
    tt_patoptim[sl-op.sc_l]=0;
    tt_compflow[sl-op.sc_l]=0;
    tt_tvopt[sl-op.sc_l]=0;
    tt_all[sl-op.sc_l]=0;
  }

  if (op.verbosity>1) gettimeofday(&tv_start_all, nullptr);

  // Create grids on each scale
  vector<OFC::PatGridClass*> grid_fw(op.noscales);
  vector<OFC::PatGridClass*> grid_bw(op.noscales); // grid for backward OF computation, only needed if 'usefbcon' is set to 1.
  vector<float*> flow_fw(op.noscales);
  vector<float*> flow_bw(op.noscales);
  cpl.resize(op.noscales);
  cpr.resize(op.noscales);
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {
    int i = sl-op.sc_l;

    float sc_fct = pow(2,-sl); // scaling factor at current scale
    cpl[i].sc_fct = sc_fct;
    cpl[i].height = height_in * sc_fct;
    cpl[i].width = width_in * sc_fct;
    cpl[i].imgpadding = imgpadding_in;
    cpl[i].tmp_lb = -(float)op.p_samp_s/2;
    cpl[i].tmp_ubw = (float) (cpl[i].width +op.p_samp_s/2-2);
    cpl[i].tmp_ubh = (float) (cpl[i].height+op.p_samp_s/2-2);
    cpl[i].tmp_w = cpl[i].width + 2*imgpadding_in;
    cpl[i].tmp_h = cpl[i].height+ 2*imgpadding_in;
    cpl[i].curr_lv = sl;
    cpl[i].camlr = 0;

    cpr[i] = cpl[i];
    cpr[i].camlr = 1;

    flow_fw[i]   = new float[op.nop * cpl[i].width * cpl[i].height];
    grid_fw[i]   = new OFC::PatGridClass(&(cpl[i]), &(cpr[i]), &op);

    if (op.usefbcon) // for merging forward and backward flow
    {
      flow_bw[i] = new float[op.nop * cpr[i].width * cpr[i].height];
      grid_bw[i] = new OFC::PatGridClass(&(cpr[i]), &(cpl[i]), &op);

      // Make grids known to each other, necessary for AggregateFlowDense();
      grid_fw[i]->SetComplGrid( grid_bw[i] );
      grid_bw[i]->SetComplGrid( grid_fw[i] );
    }
  }

  // Timing, Grid memory allocation
  if (op.verbosity>1)
  {
    gettimeofday(&tv_end_all, nullptr);
    double tt_gridconst = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
    printf("TIME (Grid Memo. Alloc. ) (ms): %3g\n", tt_gridconst);
  }

  // *** Main loop; Operate over scales, coarse-to-fine
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {
    int ii = sl-op.sc_l;

    if (op.verbosity>1) gettimeofday(&tv_start_all, nullptr);

    // Initialize grid (Step 1 in Algorithm 1 of paper)
    grid_fw[ii]->  InitializeGrid(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]);
    grid_fw[ii]->  SetTargetImage(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]);
    if (op.usefbcon)
    {
      grid_bw[ii]->InitializeGrid(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]);
      grid_bw[ii]->SetTargetImage(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]);
    }

    // Timing, Grid construction
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_patconstr[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_patconstr[ii];
      gettimeofday(&tv_start_all, nullptr);
    }

    // Initialization from previous scale, or to zero at first iteration. (Step 2 in Algorithm 1 of paper)
    if (sl < op.sc_f)
    {
      grid_fw[ii]->InitializeFromCoarserOF(flow_fw[ii+1]); // initialize from flow at previous coarser scale

      // Initialize backward flow
      if (op.usefbcon)
        grid_bw[ii]->InitializeFromCoarserOF(flow_bw[ii+1]);
    }
    else if (sl == op.sc_f && initflow != nullptr) // initialization given input flow
    {
      grid_fw[ii]->InitializeFromCoarserOF(initflow); // initialize from flow at coarser scale
    }

    // Timing, Grid initialization
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_patinit[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_patinit[ii];
      gettimeofday(&tv_start_all, nullptr);
    }

    // Dense Inverse Search. (Step 3 in Algorithm 1 of paper)
    grid_fw[ii]->Optimize();
    if (op.usefbcon)
      grid_bw[ii]->Optimize();

//     if (op.verbosity==4) // needed for verbosity >= 3, DISVISUAL
//     {
//       grid_fw[ii]->OptimizeAndVisualize(pow(2, sl));
//       if (op.usefbcon)
//         grid_bw[ii]->Optimize();
//     }
//     else
//     {
//       grid_fw[ii]->Optimize();
//       if (op.usefbcon)
//         grid_bw[ii]->Optimize();
//     }

    // Timing, DIS
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_patoptim[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_patoptim[ii];

      gettimeofday(&tv_start_all, nullptr);
    }

    // Densification. (Step 4 in Algorithm 1 of paper)
    float *tmp_ptr = flow_fw[ii];
    if (sl == op.sc_l)
      tmp_ptr = outflow;

    grid_fw[ii]->AggregateFlowDense(tmp_ptr);

    if (op.usefbcon && sl > op.sc_l )  // skip at last scale, backward flow no longer needed
      grid_bw[ii]->AggregateFlowDense(flow_bw[ii]);

    // Timing, Densification
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_compflow[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_compflow[ii];

      gettimeofday(&tv_start_all, nullptr);
    }

    // Variational refinement, (Step 5 in Algorithm 1 of paper)
    if (op.usetvref)
    {
      OFC::VarRefClass varref_fw(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl],
                                im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]
                                ,&(cpl[ii]), &(cpr[ii]), &op, tmp_ptr);

      if (op.usefbcon  && sl > op.sc_l )    // skip at last scale, backward flow no longer needed
          OFC::VarRefClass varref_bw(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl],
                                    im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]
                                    ,&(cpr[ii]), &(cpl[ii]), &op, flow_bw[ii]);
    }

    // Timing, Variational Refinement
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_tvopt[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_tvopt[ii];
      printf("TIME (Sc: %i, #p:%6i, pconst, pinit, poptim, cflow, tvopt, total): %8.2f %8.2f %8.2f %8.2f %8.2f -> %8.2f ms.\n", sl, grid_fw[ii]->GetNoPatches(), tt_patconstr[ii], tt_patinit[ii], tt_patoptim[ii], tt_compflow[ii], tt_tvopt[ii], tt_all[ii]);
    }

//     if (op.verbosity==3) // Display displacement result of this scale // needed for verbosity >= 3, DISVISUAL
//     {
//       // Display Grid on current scale
//       float sc_fct_tmp = pow(2, sl); // upscale factor
//
//       cv::Mat src(cpl[ii].height+2*cpl[ii].imgpadding, cpl[ii].width+2*cpl[ii].imgpadding, CV_32FC1, (void*) im_ao[sl]);
//       cv::Mat img_ao_mat = src(cv::Rect(cpl[ii].imgpadding, cpl[ii].imgpadding, cpl[ii].width, cpl[ii].height));
//
//       cv::Mat outimg;
//       img_ao_mat.convertTo(outimg, CV_8UC1);
//       cv::cvtColor(outimg, outimg, CV_GRAY2RGB);
//       cv::resize(outimg, outimg, cv::Size(), sc_fct_tmp, sc_fct_tmp, cv::INTER_NEAREST);
//       for (int i = 0; i < grid_fw[ii]->GetNoPatches() ; ++i)
//         DisplayDrawPatchBoundary(outimg, grid_fw[ii]->GetRefPatchPos(i), sc_fct_tmp);
//
//       for (int i = 0; i < grid_fw[ii]->GetNoPatches(); ++i)
//       {
//         // Show displacement vector
//         const Eigen::Vector2f pt_ref = grid_fw[ii]->GetRefPatchPos(i);
//         const Eigen::Vector2f pt_ret = grid_fw[ii]->GetQuePatchPos(i);
//
//         Eigen::Vector2f pta, ptb;
//         cv::line(outimg, cv::Point( (pt_ref[0]+.5)*sc_fct_tmp, (pt_ref[1]+.5)*sc_fct_tmp ), cv::Point( (pt_ret[0]+.5)*sc_fct_tmp, (pt_ret[1]+.5)*sc_fct_tmp ), cv::Scalar(0,255,0),  2);
//       }
//       cv::namedWindow( "Img_ao", cv::WINDOW_AUTOSIZE );
//       cv::imshow( "Img_ao", outimg);
//
//       cv::waitKey(0);
//     }

  }

  // Clean up
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {

    delete[] flow_fw[sl-op.sc_l];
    delete grid_fw[sl-op.sc_l];

    if (op.usefbcon)
    {
      delete[] flow_bw[sl-op.sc_l];
      delete grid_bw[sl-op.sc_l];
    }
  }

  // Timing, total algorithm run-time
  if (op.verbosity>0)
  {
    gettimeofday(&tv_end_all_global, nullptr);
    double tt = (tv_end_all_global.tv_sec-tv_start_all_global.tv_sec)*1000.0f + (tv_end_all_global.tv_usec-tv_start_all_global.tv_usec)/1000.0f;
    printf("TIME (O.Flow Run-Time   ) (ms): %3g\n", tt);
  }

}

// // needed for verbosity >= 3, DISVISUAL
// void OFClass::DisplayDrawPatchBoundary(cv::Mat img, const Eigen::Vector2f pt, const float sc)
// {
//   cv::line(img, cv::Point( (pt[0]+.5)*sc, (pt[1]+.5)*sc ), cv::Point( (pt[0]+.5)*sc, (pt[1]+.5)*sc ), cv::Scalar(0,0,255),  4);
//
//   float lb = -op.p_samp_s/2;
//   float ub = op.p_samp_s/2-1;
//
//   cv::line(img, cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Scalar(0,0,255),  1);
//   cv::line(img, cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Scalar(0,0,255),  1);
//   cv::line(img, cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Scalar(0,0,255),  1);
//   cv::line(img, cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Scalar(0,0,255),  1);
// }

}


/**
 * @file mpc.cpp
 * @brief !Valgrind output
 *  Memcheck, a memory error detector
 *  Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
 *  Using Valgrind-3.15.0 and LibVEX; rerun with -h for copyright info
 *  Command: ./src/control_system/control_system_mpc_example
 *
 *  HEAP SUMMARY:
 *      in use at exit: 0 bytes in 0 blocks
 *    total heap usage: 19,575 allocs, 19,575 frees, 866,248 bytes allocated
 *
 *  All heap blocks were freed -- no leaks are possible
 *
 *  For lists of detected and suppressed errors, rerun with: -s
 *  ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)
 */

#include <mpc.hpp>
#include <memory>
#include <iostream>
#include <Eigen/Dense>
#include <math.h>

using namespace controller;

int main()
{
    unsigned int x = 3; // Number of states [position x, position y, theta].
    unsigned int u = 2; // Input dimension [translation rate, rotation rate].
    double dt = 0.025; // Timestamp.
    double saturation = 10;

    // Declare MAT for MPC computation.
    Eigen::MatrixXd A(x, x); // System dynamics matrix.
    Eigen::MatrixXd B(x, u); // Input matrix.
    Eigen::MatrixXd Q(x, x); // Weight on the systems state.
    Eigen::MatrixXd R(u, u); // Weight on control input.

    A <<
        cos(M_PI_4), -sin(M_PI_4), 0.0,
        sin(M_PI_4), cos(M_PI_4), 0.0,
        0.0, 0.0, 1.0;

    B <<
        cos(M_PI_4) * dt, 0.0,
        sin(M_PI_4) * dt, 0.0,
        0.0, dt;

    R <<
        100.0, 0.0,
        0.0, 100.0;

    Q <<
        1.0, 0.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0;

    // Initialize MPC and assign the MAT.
    auto ptr = std::unique_ptr<MPC>(new MPC(Q, R, saturation));

    // Get state error.
    Eigen::MatrixXd stateError(x, x);

    stateError <<
        0.0023, 0.0, 0.0,
        0.0, 0.001, 0.0,
        0.0, 0.0, 0.001;

    // Get cmd_vel.
    double numIteration = 100000;
    double tolarance = 1.E-5;

    Eigen::MatrixXd cmd_vel = ptr->computeDiscrete(A, B, stateError, numIteration, tolarance, dt);
    std::cout << "\ncmd_vel.x\tcmd_vel.orientation.z\n" << cmd_vel(0,0) << "\t\t" << cmd_vel(0,1) << std::endl;

    /** !Output.
     * @brief cmd_vel.x	    cmd_vel.orientation.z
     *        0.157304		0.0800126
     */

    return EXIT_SUCCESS;
}


// (C) Copyright David Gleich 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/core_numbers.hpp>
#include <boost/property_map/property_map.hpp>
#include <stdio.h>

using namespace boost;

const char* errstr = "";

int test_1()
{
    // core numbers of sample graph
    typedef adjacency_list< vecS, vecS, undirectedS > Graph;

    Graph G(21);
    add_edge(0, 1, G);
    add_edge(1, 2, G);
    add_edge(1, 3, G);
    add_edge(2, 3, G);
    add_edge(1, 4, G);
    add_edge(3, 4, G);
    add_edge(4, 5, G);
    add_edge(4, 6, G);
    add_edge(5, 6, G);
    add_edge(4, 7, G);
    add_edge(5, 7, G);
    add_edge(6, 7, G);
    add_edge(7, 8, G);
    add_edge(3, 9, G);
    add_edge(8, 9, G);
    add_edge(8, 10, G);
    add_edge(9, 10, G);
    add_edge(10, 11, G);
    add_edge(10, 12, G);
    add_edge(3, 13, G);
    add_edge(9, 13, G);
    add_edge(3, 14, G);
    add_edge(9, 14, G);
    add_edge(13, 14, G);
    add_edge(16, 17, G);
    add_edge(16, 18, G);
    add_edge(17, 19, G);
    add_edge(18, 19, G);
    add_edge(19, 20, G);

    std::vector< int > core_nums(num_vertices(G));
    core_numbers(
        G, make_iterator_property_map(core_nums.begin(), get(vertex_index, G)));

    for (size_t i = 0; i < num_vertices(G); ++i)
    {
        printf("vertex %3lu : %i\n", (unsigned long)i, core_nums[i]);
    }

    int correct[21]
        = { 1, 2, 2, 3, 3, 3, 3, 3, 2, 3, 2, 1, 1, 3, 3, 0, 2, 2, 2, 2, 1 };
    for (size_t i = 0; i < num_vertices(G); ++i)
    {
        if (core_nums[i] != correct[i])
        {
            return 1; // error!
        }
    }
    return 0;
}

int test_2()
{
    // core numbers of sample graph
    typedef adjacency_list< listS, vecS, undirectedS, no_property,
        property< edge_weight_t, int > >
        graph_t;
    int num_nodes = 3;
    typedef std::pair< int, int > Edge;

    Edge edge_array[] = { Edge(0, 1), Edge(0, 2), Edge(1, 2) };
    int weights[] = { -1, -2, -2 };
    int num_arcs = sizeof(edge_array) / sizeof(Edge);

    graph_t G(edge_array, edge_array + num_arcs, weights, num_nodes);

    std::vector< int > core_nums(num_vertices(G));
    weighted_core_numbers(
        G, make_iterator_property_map(core_nums.begin(), get(vertex_index, G)));

    for (size_t i = 0; i < num_vertices(G); ++i)
    {
        printf("vertex %3lu : %i\n", (unsigned long)i, core_nums[i]);
    }

    int correct[3] = { -1, -1, -4 };
    for (size_t i = 0; i < num_vertices(G); ++i)
    {
        if (core_nums[i] != correct[i])
        {
            return 1; // error!
        }
    }
    return 0;
}

int test_3()
{
    // core numbers of a directed graph, the core numbers of a directed
    // cycle are always one
    typedef adjacency_list< vecS, vecS, directedS > graph_t;
    int num_nodes = 5;
    typedef std::pair< int, int > Edge;

    Edge edge_array[]
        = { Edge(0, 1), Edge(1, 2), Edge(2, 3), Edge(3, 4), Edge(4, 0) };
    int num_arcs = sizeof(edge_array) / sizeof(Edge);

    graph_t G(edge_array, edge_array + num_arcs, num_nodes);

    std::vector< int > core_nums(num_vertices(G));
    core_numbers(
        G, make_iterator_property_map(core_nums.begin(), get(vertex_index, G)));

    for (size_t i = 0; i < num_vertices(G); ++i)
    {
        printf("vertex %3lu : %i\n", (unsigned long)i, core_nums[i]);
    }

    int correct[5] = { 1, 1, 1, 1, 1 };
    for (size_t i = 0; i < num_vertices(G); ++i)
    {
        if (core_nums[i] != correct[i])
        {
            return 1; // error!
        }
    }
    return 0;
}

int main(int, char**)
{
    int nfail = 0, ntotal = 0;
    int rval;

    const char* name;

    name = "core_numbers";
    rval = test_1();
    ntotal++;
    if (rval != 0)
    {
        nfail++;
        printf("%20s  %50s\n", name, errstr);
    }
    else
    {
        printf("%20s  success\n", name);
    }

    name = "weighted_core_numbers";
    rval = test_2();
    ntotal++;
    if (rval != 0)
    {
        nfail++;
        printf("%20s  %50s\n", name, errstr);
    }
    else
    {
        printf("%20s  success\n", name);
    }

    name = "directed_corenums";
    rval = test_3();
    ntotal++;
    if (rval != 0)
    {
        nfail++;
        printf("%20s  %50s\n", name, errstr);
    }
    else
    {
        printf("%20s  success\n", name);
    }

    printf("\n");
    printf("Total tests  : %3i\n", ntotal);
    printf("Total failed : %3i\n", nfail);

    return nfail != 0;
}


/*
    Copyright (c) 2017 Mobile Robots Laboratory at Poznan University of Technology:
    -Jan Wietrzykowski name.surname [at] put.poznan.pl

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

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

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

#ifndef INCLUDE_MISC_HPP_
#define INCLUDE_MISC_HPP_

#include <ostream>
#include <vector>

#include <Eigen/Eigen>

#include <pcl/common/common_headers.h>
#include <pcl/impl/point_types.hpp>

#include <opencv2/opencv.hpp>

#include "Types.hpp"

static constexpr float pi = 3.14159265359;

template<class T>
std::ostream& operator<<(std::ostream& out, const std::vector<T>& vec){
	out << "[";
	for(int v = 0; v < (int)vec.size(); ++v){
		out << vec[v];
		if(v < vec.size() - 1){
			out << ", ";
		}
	}
	out << "]";

	return out;
}

class Misc{
public:

	static cv::Mat projectTo3D(cv::Mat depth, cv::Mat cameraParams);

    static cv::Mat reprojectTo2D(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr points, cv::Mat cameraParams);

    static Eigen::Vector3d projectPointOnPlane(const Eigen::Vector3d &pt, const Eigen::Vector4d &plane);

    static Eigen::Vector3d projectPointOnPlane(const Eigen::Vector2d &pt, const Eigen::Vector4d &plane, cv::Mat cameraMatrix);

	static bool projectImagePointsOntoPlane(const vectorVector2d &pts,
										   vectorVector3d &pts3d,
										   const cv::Mat &cameraMatrix,
										   const Eigen::Vector4d &planeEq);

	static bool nextChoice(std::vector<int>& choice, int N);

	static Eigen::Quaterniond planeEqToQuat(const Eigen::Vector4d &planeEq);

	static void normalizeAndUnify(Eigen::Quaterniond& q);

	static void normalizeAndUnify(Eigen::Vector4d& q);

    static Eigen::Vector4d toNormalPlaneEquation(const Eigen::Vector4d &plane);

	static Eigen::Vector3d logMap(const Eigen::Quaterniond &quat);

	static Eigen::Quaterniond expMap(const Eigen::Vector3d &vec);

	static Eigen::Matrix4d matrixQ(const Eigen::Quaterniond &q);

	static Eigen::Matrix4d matrixW(const Eigen::Quaterniond &q);

	static Eigen::Matrix3d matrixK(const Eigen::Quaterniond &q);

	static bool checkIfAlignedWithNormals(const Eigen::Vector3d& testedNormal,
                                            pcl::PointCloud<pcl::Normal>::ConstPtr normals,
                                            bool& alignConsistent);

	static double transformLogDist(const Vector7d &trans1,
								   const Vector7d &trans2);

    static double rotLogDist(const Eigen::Vector4d &rot1,
							 const Eigen::Vector4d &rot2);

    static cv::Mat colorIds(cv::Mat ids);

	static cv::Mat colorIdsWithLabels(cv::Mat ids);

	static Eigen::Vector3d closestPointOnLine(const Eigen::Vector3d &pt,
									   const Eigen::Vector3d &p,
									   const Eigen::Vector3d &n);

    template<typename MatrixTypeOut, typename MatrixTypeIn>
    static MatrixTypeOut pseudoInverse(const MatrixTypeIn &a, double epsilon = std::numeric_limits<double>::epsilon())
    {
        Eigen::JacobiSVD< MatrixTypeIn > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs()(0);
//        return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();

        typename Eigen::JacobiSVD< MatrixTypeIn >::SingularValuesType singularValues_inv = svd.singularValues();
        for ( long i = 0; i < singularValues_inv.cols(); ++i) {
            if ( fabs(svd.singularValues()(i)) > tolerance ) {
                singularValues_inv(i) = 1.0 / svd.singularValues()(i);
            }
            else{
                singularValues_inv(i)=0;
            }
        }
        return (svd.matrixV() * singularValues_inv.asDiagonal());
    }
};

static constexpr uint8_t colors[][3] = {
		{0xFF, 0x00, 0x00}, //Red
		{0xFF, 0xFF, 0xFF}, //White
		{0x00, 0xFF, 0xFF}, //Cyan
		{0xC0, 0xC0, 0xC0}, //Silver
		{0x00, 0x00, 0xFF}, //Blue
		{0x80, 0x80, 0x80}, //Gray
		{0x00, 0x00, 0xA0}, //DarkBlue
		{0x00, 0x00, 0x00}, //Black
		{0xAD, 0xD8, 0xE6}, //LightBlue
		{0xFF, 0xA5, 0x00}, //Orange
		{0x80, 0x00, 0x80}, //Purple
		{0xA5, 0x2A, 0x2A}, //Brown
		{0xFF, 0xFF, 0x00}, //Yellow
		{0x80, 0x00, 0x00}, //Maroon
		{0x00, 0xFF, 0x00}, //Lime
		{0x00, 0x80, 0x00}, //Green
		{0xFF, 0x00, 0xFF}, //Magenta
		{0x80, 0x80, 0x00} //Olive
};

class Visualizer{
public:

//	static pcl::PointCloud<pcl::PointXYZRGBA>::Ptr makeColorPointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr
};

#endif /* INCLUDE_MISC_HPP_ */


#include <boost/test/unit_test.hpp>

#include "math/interval.h" // header to test

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_interval)

BOOST_AUTO_TEST_CASE(int_interval) {
  math::interval<int> iv;
  BOOST_CHECK(iv.get_min() == -std::numeric_limits<int>::max());
  BOOST_CHECK(iv.get_max() == std::numeric_limits<int>::max());

  iv = math::interval<int>(-1, 1);
  BOOST_CHECK(iv.get_min() == -1);
  BOOST_CHECK(iv.get_max() == 1);
  BOOST_CHECK(iv.get_length() == 2);

  iv.set_min(3);
  BOOST_CHECK(iv.get_min() == 3);
  BOOST_CHECK(iv.get_max() == 3);

  iv.set_max(2);
  BOOST_CHECK(iv.get_min() == 2);
  BOOST_CHECK(iv.get_max() == 2);

  BOOST_CHECK(math::interval<int>::get_epsilon() == 1);
}

BOOST_AUTO_TEST_CASE(size_t_interval) {
  math::interval<size_t> iv;
  BOOST_CHECK(iv.get_min() == std::numeric_limits<size_t>::min());
  BOOST_CHECK(iv.get_max() == std::numeric_limits<size_t>::max());

  BOOST_CHECK(math::interval<size_t>::get_epsilon() == 1);
}

BOOST_AUTO_TEST_CASE(double_interval) {
  math::interval<double> iv;
  BOOST_CHECK(iv.get_min() == -std::numeric_limits<double>::max());
  BOOST_CHECK(iv.get_max() == std::numeric_limits<double>::max());

  iv = math::interval<double>(-1.0, 1.0);
  BOOST_CHECK(iv.get_min() == -1.0);
  BOOST_CHECK(iv.get_max() == 1.0);
  BOOST_CHECK(iv.get_length() == 2.0);

  iv.set_min(3.0);
  BOOST_CHECK(iv.get_min() == 3.0);
  BOOST_CHECK(iv.get_max() == 3.0);

  iv.set_max(2.0);
  BOOST_CHECK(iv.get_min() == 2.0);
  BOOST_CHECK(iv.get_max() == 2.0);

  BOOST_CHECK(math::interval<double>::get_epsilon() == std::numeric_limits<double>::epsilon());
}

BOOST_AUTO_TEST_CASE(interval_overlap) {
  math::interval<int> iv1(1, 3), iv2(2, 5), iv3(3, 5), iv4(4, 5), iv5(5, 5), iv6(1, 5), iv7(0, 5);

  BOOST_CHECK(iv1.is_continuous(iv2) == true);
  BOOST_CHECK(iv2.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.overlaps(iv2) == true);
  BOOST_CHECK(iv2.overlaps(iv1) == true);

  BOOST_CHECK(iv1.is_continuous(iv3) == true);
  BOOST_CHECK(iv3.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.overlaps(iv3) == true);
  BOOST_CHECK(iv3.overlaps(iv1) == true);

  BOOST_CHECK(iv1.is_continuous(iv4) == true);
  BOOST_CHECK(iv4.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.overlaps(iv4) == false);
  BOOST_CHECK(iv4.overlaps(iv1) == false);

  BOOST_CHECK(iv1.is_continuous(iv5) == false);
  BOOST_CHECK(iv5.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.overlaps(iv5) == false);
  BOOST_CHECK(iv5.overlaps(iv1) == false);

  BOOST_CHECK(iv1.is_continuous(iv6) == false);
  BOOST_CHECK(iv6.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.overlaps(iv6) == true);
  BOOST_CHECK(iv6.overlaps(iv1) == true);

  BOOST_CHECK(iv1.is_continuous(iv7) == false);
  BOOST_CHECK(iv7.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.overlaps(iv7) == true);
  BOOST_CHECK(iv7.overlaps(iv1) == true);

  math::interval<int> iv8(-1, 2), iv9(-1, 1), iv10(-1, 0), iv11(-1, -1), iv12(-1, 3);

  BOOST_CHECK(iv8.is_continuous(iv1) == true);
  BOOST_CHECK(iv1.is_continuous(iv8) == false);
  BOOST_CHECK(iv1.overlaps(iv8) == true);
  BOOST_CHECK(iv8.overlaps(iv1) == true);

  BOOST_CHECK(iv9.is_continuous(iv1) == true);
  BOOST_CHECK(iv1.is_continuous(iv9) == false);
  BOOST_CHECK(iv1.overlaps(iv9) == true);
  BOOST_CHECK(iv9.overlaps(iv1) == true);

  BOOST_CHECK(iv10.is_continuous(iv1) == true);
  BOOST_CHECK(iv1.is_continuous(iv10) == false);
  BOOST_CHECK(iv1.overlaps(iv10) == false);
  BOOST_CHECK(iv10.overlaps(iv1) == false);

  BOOST_CHECK(iv11.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.is_continuous(iv11) == false);
  BOOST_CHECK(iv1.overlaps(iv11) == false);
  BOOST_CHECK(iv11.overlaps(iv1) == false);

  BOOST_CHECK(iv12.is_continuous(iv1) == false);
  BOOST_CHECK(iv1.is_continuous(iv12) == false);
  BOOST_CHECK(iv1.overlaps(iv12) == true);
  BOOST_CHECK(iv12.overlaps(iv1) == true);
}

BOOST_AUTO_TEST_CASE(interval_less_min_max) {
  math::interval<int> iv1(1, 3), iv2(1, 4), iv3(2, 3);

  BOOST_CHECK(less_min_max(iv1, iv1) == false);

  BOOST_CHECK(less_min_max(iv1, iv2) == true);
  BOOST_CHECK(less_min_max(iv2, iv1) == false);

  BOOST_CHECK(less_min_max(iv1, iv3) == true);
  BOOST_CHECK(less_min_max(iv3, iv1) == false);
}

BOOST_AUTO_TEST_CASE(interval_less_length) {
  math::interval<int> iv1(1, 3), iv2(1, 4), iv3(2, 4);

  BOOST_CHECK(less_length(iv1, iv1) == false);

  BOOST_CHECK(less_length(iv1, iv2) == true);
  BOOST_CHECK(less_length(iv2, iv1) == false);

  BOOST_CHECK(less_length(iv1, iv3) == false);
  BOOST_CHECK(less_length(iv3, iv1) == false);
}

BOOST_AUTO_TEST_CASE(interval_less_max) {
  math::interval<int> iv1(1, 3), iv2(1, 4), iv3(2, 4);

  BOOST_CHECK(less_max(iv1, iv1) == false);

  BOOST_CHECK(less_max(iv1, iv2) == true);
  BOOST_CHECK(less_max(iv2, iv1) == false);

  BOOST_CHECK(less_max(iv2, iv3) == false);
  BOOST_CHECK(less_max(iv3, iv2) == false);
}

BOOST_AUTO_TEST_CASE(interval_equal_min_max) {
  math::interval<int> iv1(1, 3), iv2(1, 4), iv3(2, 4);
  BOOST_CHECK(equal_min_max(iv1, iv1));
  BOOST_CHECK(equal_min_max(iv1, iv2) == false);
  BOOST_CHECK(equal_min_max(iv1, iv3) == false);
}

BOOST_AUTO_TEST_CASE(interval_equal_length) {
  math::interval<int> iv1(1, 3), iv2(1, 4), iv3(2, 4);
  BOOST_CHECK(equal_length(iv1, iv1));
  BOOST_CHECK(equal_length(iv1, iv2) == false);
  BOOST_CHECK(equal_length(iv1, iv3));
}

BOOST_AUTO_TEST_CASE(interval_merge) {
  math::interval<int> iv1(5, 6), iv2(4, 7), iv3(4, 4), iv4(3, 3), iv5(6, 7), iv6(7, 7), iv7(8, 8);
  BOOST_CHECK(math::interval<int>::merge(iv1, iv2).size() == 1);
  BOOST_CHECK(math::interval<int>::merge(iv1, iv3).size() == 1);
  BOOST_CHECK(math::interval<int>::merge(iv1, iv4).empty());
  BOOST_CHECK(math::interval<int>::merge(iv1, iv5).size() == 1);
  BOOST_CHECK(math::interval<int>::merge(iv1, iv6).size() == 1);
  BOOST_CHECK(math::interval<int>::merge(iv1, iv7).empty());
}

BOOST_AUTO_TEST_SUITE_END()