#include "lin_reg_uni_hierarchy.h"

#include <Eigen/Dense>
#include <stan/math/prim/err.hpp>
#include <vector>

#include "src/utils/eigen_utils.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

double LinRegUniHierarchy::like_lpdf(
    const Eigen::RowVectorXd &datum,
    const Eigen::RowVectorXd &covariate /*= Eigen::RowVectorXd(0)*/) const {
  return stan::math::normal_lpdf(
      datum(0), state.regression_coeffs.dot(covariate), sqrt(state.var));
}

double LinRegUniHierarchy::marg_lpdf(
    const LinRegUni::Hyperparams &params, const Eigen::RowVectorXd &datum,
    const Eigen::RowVectorXd &covariate /*= Eigen::RowVectorXd(0)*/) const {
  double sig_n = sqrt(
      (1 + (covariate * params.var_scaling_inv * covariate.transpose())(0)) *
      params.scale / params.shape);
  return stan::math::student_t_lpdf(datum(0), 2 * params.shape,
                                    covariate.dot(params.mean), sig_n);
}

void LinRegUniHierarchy::initialize_state() {
  state.regression_coeffs = hypers->mean;
  state.var = hypers->scale / (hypers->shape + 1);
}

void LinRegUniHierarchy::initialize_hypers() {
  if (prior->has_fixed_values()) {
    // Set values
    hypers->mean = bayesmix::to_eigen(prior->fixed_values().mean());
    dim = hypers->mean.size();
    hypers->var_scaling =
        bayesmix::to_eigen(prior->fixed_values().var_scaling());
    hypers->var_scaling_inv = stan::math::inverse_spd(hypers->var_scaling);
    hypers->shape = prior->fixed_values().shape();
    hypers->scale = prior->fixed_values().scale();
    // Check validity
    if (dim != hypers->var_scaling.rows()) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    bayesmix::check_spd(hypers->var_scaling);
    if (hypers->shape <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    if (hypers->scale <= 0) {
      throw std::invalid_argument("Scale parameter must be > 0");
    }
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void LinRegUniHierarchy::update_hypers(
    const std::vector<bayesmix::AlgorithmState::ClusterState> &states) {
  auto &rng = bayesmix::Rng::Instance().get();
  if (prior->has_fixed_values()) {
    return;
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

LinRegUni::State LinRegUniHierarchy::draw(
    const LinRegUni::Hyperparams &params) {
  auto &rng = bayesmix::Rng::Instance().get();
  LinRegUni::State out;
  out.var = stan::math::inv_gamma_rng(params.shape, params.scale, rng);
  out.regression_coeffs = stan::math::multi_normal_prec_rng(
      params.mean, params.var_scaling / out.var, rng);
  return out;
}

void LinRegUniHierarchy::update_summary_statistics(
    const Eigen::RowVectorXd &datum, const Eigen::RowVectorXd &covariate,
    bool add) {
  if (add) {
    data_sum_squares += datum(0) * datum(0);
    covar_sum_squares += covariate.transpose() * covariate;
    mixed_prod += datum(0) * covariate.transpose();
  } else {
    data_sum_squares -= datum(0) * datum(0);
    covar_sum_squares -= covariate.transpose() * covariate;
    mixed_prod -= datum(0) * covariate.transpose();
  }
}

void LinRegUniHierarchy::clear_data() {
  mixed_prod = Eigen::VectorXd::Zero(dim);
  data_sum_squares = 0.0;
  covar_sum_squares = Eigen::MatrixXd::Zero(dim, dim);
  card = 0;
  cluster_data_idx = std::set<int>();
}

LinRegUni::Hyperparams LinRegUniHierarchy::get_posterior_parameters() {
  if (card == 0) {  // no update possible
    return *hypers;
  }
  // Compute posterior hyperparameters
  LinRegUni::Hyperparams post_params;
  post_params.var_scaling = covar_sum_squares + hypers->var_scaling;
  auto llt = post_params.var_scaling.llt();
  post_params.var_scaling_inv = llt.solve(Eigen::MatrixXd::Identity(dim, dim));
  post_params.mean =
      llt.solve(mixed_prod + hypers->var_scaling * hypers->mean);
  post_params.shape = hypers->shape + 0.5 * card;
  post_params.scale =
      hypers->scale +
      0.5 * (data_sum_squares +
             hypers->mean.transpose() * hypers->var_scaling * hypers->mean -
             post_params.mean.transpose() * post_params.var_scaling *
                 post_params.mean);
  return post_params;
}

void LinRegUniHierarchy::set_state_from_proto(
    const google::protobuf::Message &state_) {
  auto &statecast = google::protobuf::internal::down_cast<
      const bayesmix::AlgorithmState::ClusterState &>(state_);
  state.regression_coeffs =
      bayesmix::to_eigen(statecast.lin_reg_uni_ls_state().regression_coeffs());
  state.var = statecast.lin_reg_uni_ls_state().var();
  set_card(statecast.cardinality());
}

void LinRegUniHierarchy::write_state_to_proto(
    google::protobuf::Message *out) const {
  bayesmix::LinRegUniLSState state_;
  bayesmix::to_proto(state.regression_coeffs,
                     state_.mutable_regression_coeffs());
  state_.set_var(state.var);

  auto *out_cast = google::protobuf::internal::down_cast<
      bayesmix::AlgorithmState::ClusterState *>(out);
  out_cast->mutable_lin_reg_uni_ls_state()->CopyFrom(state_);
  out_cast->set_cardinality(card);
}

void LinRegUniHierarchy::write_hypers_to_proto(
    google::protobuf::Message *out) const {
  bayesmix::LinRegUniPrior hypers_;
  bayesmix::to_proto(hypers->mean,
                     hypers_.mutable_fixed_values()->mutable_mean());
  bayesmix::to_proto(hypers->var_scaling,
                     hypers_.mutable_fixed_values()->mutable_var_scaling());
  hypers_.mutable_fixed_values()->set_shape(hypers->shape);
  hypers_.mutable_fixed_values()->set_scale(hypers->scale);

  google::protobuf::internal::down_cast<bayesmix::LinRegUniPrior *>(out)
      ->mutable_fixed_values()
      ->CopyFrom(hypers_.fixed_values());
}


/*
 * odeint_rk4_lorenz_def_alg.cpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>

#include "rk_performance_test_case.hpp"

#include "lorenz.hpp"

typedef boost::array< double , 3 > state_type;
typedef boost::numeric::odeint::runge_kutta4_classic< state_type > rk4_odeint_type;

class odeint_wrapper
{
public:
    void reset_init_cond()
    {
        m_x[0] = 10.0 * rand() / RAND_MAX;
        m_x[1] = 10.0 * rand() / RAND_MAX;
        m_x[2] = 10.0 * rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        m_stepper.do_step( lorenz() , m_x , m_t , dt );
        //m_t += dt;
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
    rk4_odeint_type m_stepper;
};

int main()
{
    odeint_wrapper stepper;

    run( stepper );
}


#ifndef M_PI
#    define M_PI 3.14159265358979323846
#endif

#include <boost/shared_array.hpp>
#include <boost/range.hpp>

#include <smp/planner_utils/trajectory.hpp>
#include <smp/planner_utils/vertex_edge.hpp>
#include <smp/components/extenders/pos.h>

#include <smp/components/extenders/state_array_double.hpp>
#include <smp/components/extenders/input_array_double.hpp>
#include <smp/components/extenders/base.hpp>
#include <cmath>

#include <cstdlib>
/// Waypoint Position Controller WP=1
#define WP 1

using namespace smp;
using namespace std;

template< class typeparams, int NUM_DIMENSIONS >
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_insert_vertex (vertex_t *vertex_in) {

    return 1;
}

template< class typeparams, int NUM_DIMENSIONS >
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_insert_edge (edge_t *edge_in) {

    return 1;
}

template< class typeparams , int NUM_DIMENSIONS>
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_delete_vertex (vertex_t *vertex_in){

    return 1;
}

template< class typeparams, int NUM_DIMENSIONS >
int smp::extender_pos<typeparams,NUM_DIMENSIONS>
::ex_update_delete_edge (edge_t *edge_in) {

    return 1;
}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::set_angle_to_range(double alpha, double min)
{

    while (alpha >= min + 2.0 * M_PI) {
        alpha -= 2.0 * M_PI;
    }
    while (alpha < min) {
        alpha += 2.0 * M_PI;
    }
    return alpha;
}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::diff_angle_unwrap(double alpha1, double alpha2)
{
    double delta;

    // normalize angles alpha1 and alpha2
    alpha1 = set_angle_to_range(alpha1, 0);
    alpha2 = set_angle_to_range(alpha2, 0);

    // take difference and unwrap
    delta = alpha1 - alpha2;
    if (alpha1 > alpha2) {
        while (delta > M_PI) {
            delta -= 2.0 * M_PI;
        }
    } else if (alpha2 > alpha1) {
        while (delta < -M_PI) {
            delta += 2.0 * M_PI;
        }
    }
    return delta;
}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::normangle(double a, double mina){

    double ap,minap;
    ap=a;
    minap=mina;
    while (ap>= (minap+M_PI*2)){
        ap=ap-M_PI*2;
    }
    while(ap<minap){
        ap=ap+M_PI*2;

    }

    return ap;

}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::f(double rho){

    double Kv;
    /// New implementation  Kv= max atanh(rho)/rho
    Kv=1;
//    Kv=1;

    return tanh(rho*Kv);
    /// German Paper Version

    //  return (2*v0/M_PI)*atan((M_PI/(2*v0))*rho);

}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
::g(double t){
    /// German Paper Version
//        double a;
//        a=pow((t/T),4);
//        return a/(a+1);

    /// New implementation
    return 1;

}

template< class typeparams, int NUM_DIMENSIONS >
double* smp::extender_pos<typeparams,NUM_DIMENSIONS>
::posctrlstep (double x_c, double y_c, double t_c,
               double x_end, double y_end, double t_end, double ct, double b, int dir) {

    /** This function will generate a vector of double as output:

     *  [0] Vl velocity of the left wheel;
     *  [1] Vr velocity of the right wheel;
     *  [2] V translational Velocity;
     *  [3] W Angular Velocity.
     *  [4] EOT End Of Trajectory

    **/

    static double oldBeta,controllerType;

    double Krho,Kalpha,Kbeta,Kphi,Vmax,RhoThreshold1,RhoThreshold2,RhoEndCondition,PhiEndCondition;
    // [1 3 -1 -1]
    Krho    = 1.2;
    Kalpha  = 3;
    Kbeta   = -1;
    Kphi    = -1;
    Vmax    = Krho;
    //  RhoThreshold1   = 0.04;
    RhoThreshold1   = 0.005;

    RhoThreshold2   = 0.02;
    //  RhoThreshold2   = 0.2;
/// The RRT* edges' lenght is related to the RhoEndCondition
#if WP>0
    RhoEndCondition = 0.35;
#else
    RhoEndCondition = 0.35;
#endif

/// the PhiEndCondition has to be setted properly,
/// small Value, local minima can occur, tested --> greater than 35*M_PI/180

    //  PhiEndCondition = 1*M_PI/180; [NOT IN THE ASTOLFI PAPER :)]
    PhiEndCondition = 50*M_PI/180;

    if(ct==0){
        oldBeta=0;
        controllerType=1;
    }

    double dx,dy,rho,fRho,alpha,phi,beta,v,w,vl,vr,eot,vi,di;
    vi=0.1;
    // rho
    eot=1;
    dx=x_end-x_c;
    dy=y_end -y_c;
    rho=sqrt(dx*dx+dy*dy);
    fRho=rho;

    if(fRho>(Vmax/Krho)){
        fRho=Vmax/Krho;
    }

    //alpha

    alpha=atan2(dy,dx)-t_c;
    alpha=normangle(alpha,-M_PI);

    //direction

    if (dir==0){

        if(alpha>(M_PI/2)) {
            fRho=-fRho;
            alpha=alpha-M_PI;
        }else if(alpha<=-M_PI/2){
            fRho=-fRho;
            alpha=alpha+M_PI;
        }
    }
    else if(dir==-1){
        fRho=-fRho;
        alpha=alpha+M_PI;
        if(alpha>M_PI){
            alpha=alpha-2*M_PI;
        }
    }

    //phi

    phi=t_end-t_c;
    phi=normangle(phi, -M_PI);

    beta=normangle(phi-alpha, -M_PI);

    if ((abs(oldBeta-beta)>M_PI)){
        beta=oldBeta;
    }
    oldBeta=beta;

    //set speed

#if WP>0
    v=Krho*f(fRho)*g(ct+1);
    w=(Kalpha*alpha+Kbeta*beta)*g(ct+1);
#else
    v=Krho*fRho;
    w=Kalpha*alpha+Kbeta*beta;
#endif

#if WP>0
    if (rho<this->rho_endcondition && abs(phi)<this->phi_endcondition ){
#else
    if (rho<RhoEndCondition && abs(phi)<PhiEndCondition){

#endif

        eot=1;
        //    cout<<"eot evaluated... "<<eot<<endl;
    }
    else {
        eot=0;
        //   cout<<"eot evaluated... "<<eot<<endl;

    }

    if(eot){
#if WP>0
#else
        v=0.0;
#endif
        w=0.;
    }

    //Convert speed to wheel speed

    vl=v-w*b/2;

    if(abs(vl)>Vmax){

        if(vl<0){
            vl=Vmax*-1;}
        else{
            vl=Vmax;}
    }

    vr=v+w*b/2;

    if(abs(vr)>Vmax){
        if(vr<0){
            vr=Vmax*-1;}
        else{
            vr=Vmax;}
    }

    result[0]=vl;
    result[1]=vr;
    result[2]=v;
    result[3]=w;
    result[4]=eot;

    return result;

}

template< class typeparams, int NUM_DIMENSIONS >
double smp::extender_pos<typeparams,NUM_DIMENSIONS>
:: posctrl(state_t *state_ini, state_t *state_fin,int dir,double b, double dt ,
           list<state_t *> *list_states_out, list<input_t *> *list_inputs_out) {

    double sl,sr,oldSl,oldSr,t,eot,dSl,dSr,dSm,dSd,vl,vr,enc_l,enc_r;

    enc_l=0;
    enc_r=0;
    sl=0;
    sr=0;
    oldSl=0;
    oldSr=0;
    eot=0;
    t=0;
    vl=0;
    vr=0;

    double x,y,th;
    x=(*state_ini)[0];
    y=(*state_ini)[1];
    th=(*state_ini)[2];

    this->xi=x;
    this->yi=y;

    double vv,ww,ths;
    vv=0;
    ww=0;
    ths=0.1;

    double dist;
    dist=0;

    if(list_states_out){

        while(eot==0){
            // calculate distance for both wheels
            dSl=sl-oldSl;
            dSr=sr-oldSr;
            dSm=(dSl+dSr)/2;

            dSd=(dSr-dSl)/b;
            state_t *curr = new state_t;
            input_t *ve = new input_t;

            (*curr)[0]=x+dSm*cos(th+dSd/2);
            (*curr)[1]=y+dSm*sin(th+dSd/2);
            (*curr)[2]=normangle(th+dSd, -M_PI);

            intRes= posctrlstep ((*curr)[0],(*curr)[1],(*curr)[2],(*state_fin)[0], (*state_fin)[1],(*state_fin)[2], t,b,dir);
            //Save the velocity commands,eot
            vv=intRes[2];
            ww=intRes[3];
            (*ve)[0]=intRes[2];
            (*ve)[1]=intRes[3];
            eot=intRes[4];
            vl=intRes[0];
            vr=intRes[1];

            //Increase the timer
            t=t+dt;

            // keep track of previous wheel position
            oldSl=sl;
            oldSr=sr;

            // increase encoder values
            enc_l=enc_l+dt*vl;
            enc_r=enc_r+dt*vr;

            sl=enc_l;
            sr=enc_r;

            //
            float dxl,dyl;
            dxl=(*state_fin)[0]-(*curr)[0];
            dyl=(*state_fin)[1]-(*curr)[1];
            dist=sqrt(dxl*dxl+dyl*dyl);

            //save the state for the next sample
            x=(*curr)[0];
            y=(*curr)[1];
            th=(*curr)[2];

            if(eot==1){

            /// save the last state!!!
            state_t *save = new state_t;
            input_t *vesave = new input_t;
            (*vesave)[0]=intRes[2];
            (*vesave)[1]=intRes[3];
            dSl=sl-oldSl;
            dSr=sr-oldSr;
            dSm=(dSl+dSr)/2;
            dSd=(dSr-dSl)/b;
            (*save)[0]=x+dSm*cos(th+dSd/2);
            (*save)[1]=y+dSm*sin(th+dSd/2);
            (*save)[2]=normangle(th+dSd, -M_PI);
            // Add current values to the Trajectory
            list_states_out->push_back ((curr));
            list_inputs_out->push_back ((ve));

            list_states_out->push_back ((save));
            list_inputs_out->push_back ((vesave));

            }
            else

            {
            // Add current values to the Trajectory
            list_states_out->push_back ((curr));
            list_inputs_out->push_back ((ve));
            }

        }
    }
    return dist;

}

template< class typeparams, int NUM_DIMENSIONS >
smp::extender_pos<typeparams,NUM_DIMENSIONS>
::extender_pos () {
     result= (double*)malloc(sizeof(double)*5);
     intRes= (double*) malloc(sizeof(double)*5);
     this->dt_=0.01;
     this->rho_endcondition=0.15;
     this->L_axis=0.5;
     this->phi_endcondition = 20*M_PI/180;

}

template< class typeparams, int NUM_DIMENSIONS >
smp::extender_pos<typeparams,NUM_DIMENSIONS>
::~extender_pos () {

}

template< class typeparams, int NUM_DIMENSIONS>
int smp::extender_pos<typeparams, NUM_DIMENSIONS>
::extend (state_t *state_from_in, state_t *state_towards_in,
          int *exact_connection_out, trajectory_t *trajectory_out,
          list<state_t*> *intermediate_vertices_out) {

    int dir;
    dir=1;

    double b,dt,d,myEps;
    dt=this->dt_;
    T=0.31;
    /// Base
    // b=0.4;
    b=this->L_axis;
    /// myEps=0.1 for pursuit
    /// myEps=0.25 no pursuit
#if WP>0
    myEps=0.50;
#else
    myEps=0.5;
#endif
    //   cout<<"call posctrl /...."<<endl<<endl;
    intermediate_vertices_out->clear ();
    trajectory_out->clear ();
    d=posctrl(state_from_in, state_towards_in,dir, b, dt,&(trajectory_out->list_states),&(trajectory_out->list_inputs));

    if(d<myEps)
    {
        (*exact_connection_out)=1;
        return 1;

    }   else

    {

        (*exact_connection_out)=0;
        return 0;

    }

}


#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <math.h>
#include "pose.h"

#define DEBUG_ODOM false
#define DEBUG_IMU false
#define DEBUG_TF false
#define DEBUG_COV false

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;
MatrixXd Q = MatrixXd::Identity(12, 12); // For propogate, IMU noise
MatrixXd Rt = MatrixXd::Identity(6, 6);   // For update, Odometry noise

// ros::Time current_time, last_time;
ros::Time current_time;
ros::Time last_time;

/*  Mean and covariance matrixs
    ps: present state
    ba: state propogated
    ns: next state
	[x1] x(0)  x(1)  x(2) = position, x y z
	[x2] x(3)  x(4)  x(5) = orientation, roll pitch yaw, ZXY Euler
	[x3] x(6)  x(7)  x(8) = linear velocity, x y z
	[x4] x(9)  x(10) x(11)= gyroscope bias
	[x5] x(12) x(13) x(14)= accelerator bias   							*/
VectorXd mean_ps= MatrixXd::Zero(15, 1 );
VectorXd mean_ba= MatrixXd::Zero(15, 1 );
VectorXd mean_ns= MatrixXd::Zero(15, 1 );
MatrixXd cov_ps = MatrixXd::Identity(15, 15);
MatrixXd cov_ba = MatrixXd::Identity(15, 15);
MatrixXd cov_ns = MatrixXd::Identity(15, 15);
/* Observation Model	*/
VectorXd g_ut(6);

// Initially use a constant, later need to read from the environment
float g = 9.82;
bool IMU_UPDATED = false;
bool CAMERA_UPDATED = false;

/*  Process model, since IMU is an internal measurement
    the gyro bias and accelerator bias is part to the state
    From the process model, IMU measurement - bias = real state
    Nov. 16th, 2016 note:
    1. Everything IMU has an input, propogate once;
    2. Getting g ~ 9.8 m.s^-2 is tricky
    3. Use MATLAB to from the A, B, and U
    Optimal:
    4. The best way to handle timestamp difference is to store the propogated
       and origin data, and trace back to the most recent propogated value
       and repropogate after the odometry reading comes
*/
void imu_callback(const sensor_msgs::Imu::ConstPtr &msg)
{
	IMU_UPDATED = true;
	#if DEBUG_IMU
		// cout << "Before process, the mean_ps is: " << endl << mean_ps << endl;
		// cout << "The cov_ps is: " << endl << cov_ps << endl;
    	ROS_INFO("Imu Seq: [%d]", msg->header.seq);
    	ROS_INFO("Imu linear acceleration x: [%f], y: [%f], z: [%f]", \
		msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
    	ROS_INFO("Imu angular velocity x: [%f], y: [%f], z: [%f]", \
    	msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);
	#endif
    MatrixXd At = MatrixXd::Identity(15, 15);
    MatrixXd Ut = MatrixXd::Identity(15, 12);
    MatrixXd Ft = MatrixXd::Identity(15, 15);
    MatrixXd Vt = MatrixXd::Identity(15, 12);
    last_time = current_time;
	float dt = 0.0;
	if	(current_time < msg->header.stamp) { // Check for its first time
		current_time = msg->header.stamp;
    	dt = current_time.toSec() - last_time.toSec();
	}  else  {
		current_time = msg->header.stamp;
		dt = 0.0;
		return;
	}

	// double x11= mean_ps(0), x12= mean_ps(1), x13= mean_ps(2);
	double x21= mean_ps(3), x22= mean_ps(4), x23= mean_ps(5);
	double x31= mean_ps(6), x32= mean_ps(7), x33= mean_ps(8);
	double x41= mean_ps(9), x42= mean_ps(10),x43= mean_ps(11);
	double x51= mean_ps(12),x52= mean_ps(13),x53= mean_ps(14);
	// input u : acceleration, augular_velocity
	double am1= msg->linear_acceleration.x;
	double am2= msg->linear_acceleration.y;
	double am3= msg->linear_acceleration.z;
	double wm1= msg->angular_velocity.x;
	double wm2= msg->angular_velocity.y;
	double wm3= msg->angular_velocity.z;

	/**************************************************************************/
	/* updated to f(mu_t_1, u_t, 0), At(mu_t_1, u_t, 0), and Ut               */
	/* Generated by matlab ELEC6910P_Project2Phase2.m                         */
	/**************************************************************************/

	VectorXd f_t_1(15);
	f_t_1 <<
		x31,
		x32,
		x33,
		wm1*cos(x22) - 0*cos(x22) - x41*cos(x22) - 0*sin(x22) + wm3*sin(x22) - x43*sin(x22),
		-(0*cos(x21) - wm2*cos(x21) + x42*cos(x21) - 0*cos(x22)*sin(x21) + wm3*cos(x22)*sin(x21) - x43*cos(x22)*sin(x21) + 0*sin(x21)*sin(x22) - wm1*sin(x21)*sin(x22) + x41*sin(x21)*sin(x22))/cos(x21),
		-(0*cos(x22) - wm3*cos(x22) + x43*cos(x22) - 0*sin(x22) + wm1*sin(x22) - x41*sin(x22))/cos(x21),
		cos(x21)*sin(x23)*(0 - am2 + x52) - (cos(x23)*sin(x22) + cos(x22)*sin(x21)*sin(x23))*(0 - am3 + x53) - (cos(x22)*cos(x23) - sin(x21)*sin(x22)*sin(x23))*(0 - am1 + x51),
		- (cos(x22)*sin(x23) + cos(x23)*sin(x21)*sin(x22))*(0 - am1 + x51) - (sin(x22)*sin(x23) - cos(x22)*cos(x23)*sin(x21))*(0 - am3 + x53) - cos(x21)*cos(x23)*(0 - am2 + x52),
		cos(x21)*sin(x22)*(0 - am1 + x51) - cos(x21)*cos(x22)*(0 - am3 + x53) - sin(x21)*(0 - am2 + x52) - g,
		0,
		0,
		0,
		0,
		0,
		0;

	At <<
		0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
		0, 0, 0,                                                                                0,                                                 wm3*cos(x22) - 0*cos(x22) - x43*cos(x22) + 0*sin(x22) - wm1*sin(x22) + x41*sin(x22),                                                                                0, 0, 0, 0,                     -cos(x22),  0,                    -sin(x22),                                                0,                  0,                                                0,
		0, 0, 0,                              (0*cos(x22) - wm3*cos(x22) + x43*cos(x22) - 0*sin(x22) + wm1*sin(x22) - x41*sin(x22))/cos(x21)^2,                          -(sin(x21)*(0*cos(x22) - wm1*cos(x22) + x41*cos(x22) + 0*sin(x22) - wm3*sin(x22) + x43*sin(x22)))/cos(x21),                                                                                0, 0, 0, 0, -(sin(x21)*sin(x22))/cos(x21), -1, (cos(x22)*sin(x21))/cos(x21),                                                0,                  0,                                                0,
		0, 0, 0,                  -(sin(x21)*(0*cos(x22) - wm3*cos(x22) + x43*cos(x22) - 0*sin(x22) + wm1*sin(x22) - x41*sin(x22)))/cos(x21)^2,                                      (0*cos(x22) - wm1*cos(x22) + x41*cos(x22) + 0*sin(x22) - wm3*sin(x22) + x43*sin(x22))/cos(x21),                                                                                0, 0, 0, 0,             sin(x22)/cos(x21),  0,           -cos(x22)/cos(x21),                                                0,                  0,                                                0,
		0, 0, 0, cos(x21)*sin(x22)*sin(x23)*(0 - am1 + x51) - cos(x21)*cos(x22)*sin(x23)*(0 - am3 + x53) - sin(x21)*sin(x23)*(0 - am2 + x52), (cos(x23)*sin(x22) + cos(x22)*sin(x21)*sin(x23))*(0 - am1 + x51) - (cos(x22)*cos(x23) - sin(x21)*sin(x22)*sin(x23))*(0 - am3 + x53), (cos(x22)*sin(x23) + cos(x23)*sin(x21)*sin(x22))*(0 - am1 + x51) + (sin(x22)*sin(x23) - cos(x22)*cos(x23)*sin(x21))*(0 - am3 + x53) + cos(x21)*cos(x23)*(0 - am2 + x52), 0, 0, 0,                             0,  0,                            0,   sin(x21)*sin(x22)*sin(x23) - cos(x22)*cos(x23),  cos(x21)*sin(x23), - cos(x23)*sin(x22) - cos(x22)*sin(x21)*sin(x23),
		0, 0, 0, cos(x23)*sin(x21)*(0 - am2 + x52) - cos(x21)*cos(x23)*sin(x22)*(0 - am1 + x51) + cos(x21)*cos(x22)*cos(x23)*(0 - am3 + x53), (sin(x22)*sin(x23) - cos(x22)*cos(x23)*sin(x21))*(0 - am1 + x51) - (cos(x22)*sin(x23) + cos(x23)*sin(x21)*sin(x22))*(0 - am3 + x53), cos(x21)*sin(x23)*(0 - am2 + x52) - (cos(x23)*sin(x22) + cos(x22)*sin(x21)*sin(x23))*(0 - am3 + x53) - (cos(x22)*cos(x23) - sin(x21)*sin(x22)*sin(x23))*(0 - am1 + x51), 0, 0, 0,                             0,  0,                            0, - cos(x22)*sin(x23) - cos(x23)*sin(x21)*sin(x22), -cos(x21)*cos(x23),   cos(x22)*cos(x23)*sin(x21) - sin(x22)*sin(x23),
		0, 0, 0,                            cos(x22)*sin(x21)*(0 - am3 + x53) - cos(x21)*(0 - am2 + x52) - sin(x21)*sin(x22)*(0 - am1 + x51),                                                               cos(x21)*cos(x22)*(na1 - am1 + x51) + cos(x21)*sin(x22)*(0 - am3 + x53),                                                                                0, 0, 0, 0,                             0,  0,                            0,                                cos(x21)*sin(x22),          -sin(x21),                               -cos(x21)*cos(x22),
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

	Ut <<
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0,                     -cos(x22),  0,                    -sin(x22), 0, 0, 0, 0, 0, 0,
		0, 0, 0, -(sin(x21)*sin(x22))/cos(x21), -1, (cos(x22)*sin(x21))/cos(x21), 0, 0, 0, 0, 0, 0,
		0, 0, 0,             sin(x22)/cos(x21),  0,           -cos(x22)/cos(x21), 0, 0, 0, 0, 0, 0,
		   sin(x21)*sin(x22)*sin(x23) - cos(x22)*cos(x23),  cos(x21)*sin(x23), - cos(x23)*sin(x22) - cos(x22)*sin(x21)*sin(x23), 0, 0, 0, 0, 0, 0, 0, 0, 0,
		 - cos(x22)*sin(x23) - cos(x23)*sin(x21)*sin(x22), -cos(x21)*cos(x23),   cos(x22)*cos(x23)*sin(x21) - sin(x22)*sin(x23), 0, 0, 0, 0, 0, 0, 0, 0, 0,
		                                cos(x21)*sin(x22),          -sin(x21),                               -cos(x21)*cos(x22), 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
		0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;

	Ft = MatrixXd::Identity(15, 15) + dt * At;
    Vt = dt * Ut;

    // Calculate the propogagted mean and covariance
    mean_ba = mean_ps + dt * f_t_1;
    cov_ba = Ft * cov_ps * Ft.transpose() + Vt * Q * Vt.transpose();
	#if DEBUG_IMU
		// cout<< "After process, the mean_ba became" << endl << mean_ba << endl;
		// cout<< "The cov_ba became" << endl << cov_ba << endl;
	#endif
	#if DEBUG_COV
		ROS_INFO("Imu Seq: [%d]", msg->header.seq);
		cout<< "the mean_ba (0-5):" << endl << mean_ba.head(6) << endl;
		// cout<< "The cov_ba[6,6]  :" << endl << cov_ba.topLeftCorner(6, 6) << endl;
	#endif
}

// store singularity info
double phi_ppg;
double the_ppg;
double psi_ppg;
void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
	CAMERA_UPDATED = true;
    //your code for update
    //camera position in the IMU frame = (0, -0.04, -0.02)
    //camera orientaion in the IMU frame = Quaternion(0, 0, 1, 0); w x y z, respectively
    //			   RotationMatrix << -1, 0, 0,
    //							      0, 1, 0,
    //                                0, 0, -1;

	VectorXd zt(6); // Camera reading in x,y,z, ZXY Euler

	/*                     Transformation from TF                           */
	// Get the world frame in camera frame transformation from the msg
	tf::Transform camera_pose_cw;
	tf::poseMsgToTF(msg->pose.pose, camera_pose_cw);

	// Record the camera frame in the IMU frame from TA
	tf::Transform transform_ic;
	transform_ic.setOrigin( tf::Vector3(0, -0.04, -0.02) );
	transform_ic.setRotation( tf::Quaternion(0, 0, -1, 0) );
	tf::Transform camera_pose_iw = transform_ic * camera_pose_cw;
	tf::Transform camera_pose_wi = camera_pose_iw.inverse();

	geometry_msgs::Transform camera_pose_wi_geo;
	tf::transformTFToMsg(camera_pose_wi, camera_pose_wi_geo);

	/*                     Transformation from Eigen                       */
	geometry_msgs::Quaternion q_cw = msg->pose.pose.orientation;
	// camera to tag world
	Matrix3d R_cw = Quaterniond(q_cw.w, q_cw.x, q_cw.y, q_cw.z).toRotationMatrix();
	Vector3d T_cw;
	T_cw << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z;
//	MatrixXd H_cw(4,4);
//	H_cw.col(0) << R_cw.col(0), 0;
//	H_cw.col(1) << R_cw.col(1), 0;
//	H_cw.col(2) << R_cw.col(2), 0;
//	H_cw.col(3) << T_cw   , 1;

	// IMU to camera frame
	Matrix3d R_ic = Quaterniond(0, 0, 1, 0).toRotationMatrix();
    Vector3d T_ic = Vector3d(0, -0.04, -0.02);
//	MatrixXd H_ic(4,4);
//	H_ic << -1, 0,  0,  0,
//			 0, 1,  0, -0.04,
//			 0, 0, -1, -0.02,
//			 0, 0,  0,  1;

	// IMU to tag world
	// Matrix3d R_iw = R_ic * R_cw;
	// Vector3d T_iw = R_ic * T_cw + T_ic;

	// tag world to IMU
//	MatrixXd H_wi(4,4);
//	H_wi = H_ic.inverse() * H_cw.inverse();
	// Matrix3d R_wi = R_iw.inverse();
	// Vector3d T_wi = -R_wi*T_iw;
	Matrix3d R_wi;
	Vector3d T_wi;
	R_wi =  R_cw.transpose() *  R_ic.transpose();
	T_wi = -R_cw.transpose() * (R_ic.transpose() * T_ic + T_cw);
//	R_wi = H_wi.topLeftCorner(3, 3);
//	T_wi = H_wi.topRightCorner(3, 1);

	#if DEBUG_TF
		Quaterniond R_wi_q(R_wi);
		cout << "camera_pose_wi_geo transformation from TF is: " << endl;
		cout << camera_pose_wi_geo.translation.x << endl;
		cout << camera_pose_wi_geo.translation.y << endl;
		cout << camera_pose_wi_geo.translation.z << endl;
		cout << "quaternion from TF is: " << endl;
		cout << camera_pose_wi_geo.rotation.w << endl;
		cout << camera_pose_wi_geo.rotation.x << endl;
		cout << camera_pose_wi_geo.rotation.y << endl;
		cout << camera_pose_wi_geo.rotation.z << endl;
		cout << "camera_pose_wi_geo transformation from Eigen is: " << endl;
		cout << T_wi(0) << endl;
		cout << T_wi(1) << endl;
		cout << T_wi(2) << endl;
		cout << "quaternion from Eigen is: " << endl;
		cout << R_wi_q.w() << endl;
		cout << R_wi_q.x() << endl;
		cout << R_wi_q.y() << endl;
		cout << R_wi_q.z() << endl;
	#endif
	if (DEBUG_TF) {
		zt(0) = camera_pose_wi_geo.translation.x;
		zt(1) = camera_pose_wi_geo.translation.y;
		zt(2) = camera_pose_wi_geo.translation.z;
		// From quaternion to rotation matrix and then ZXY Euler
		Eigen::Quaterniond R_wi_quat;
		R_wi_quat.w() = camera_pose_wi_geo.rotation.w;
		R_wi_quat.x() = camera_pose_wi_geo.rotation.x;
		R_wi_quat.y() = camera_pose_wi_geo.rotation.y;
		R_wi_quat.z() = camera_pose_wi_geo.rotation.z;
		Matrix3d R_wi_tf = R_wi_quat.toRotationMatrix();
		zt(3) = asin(R_wi_tf(2, 1)); // roll
		zt(4) = atan2(-R_wi_tf(2, 0) / cos(zt(3)), R_wi_tf(2, 2) / cos(zt(3)) ); // pitch
		zt(5) = atan2(-R_wi_tf(0, 1) / cos(zt(3)), R_wi_tf(1, 1) / cos(zt(3)) ); // yaw
	}
	else {
		// Use the result from Eigen in rviz
		Vector3d rpy_wi = R_to_rpy(R_wi);
		zt << T_wi, rpy_wi;
//		zt.head(3) = T_wi;
//		zt(3) = asin(R_wi(2, 1)); // roll
//		zt(4) = atan2(-R_wi(2, 0) / cos(zt(3)), R_wi(2, 2)) / cos(zt(3)) ; // pitch
//		zt(5) = atan2(-R_wi(0, 1) / cos(zt(3)), R_wi(1, 1)) / cos(zt(3)) ; // yaw
	}

	if (msg->header.seq == 0) { // first time callback, initialize all messages
		mean_ps << zt, MatrixXd::Zero(9, 1);
		mean_ba << zt, MatrixXd::Zero(9, 1);
		mean_ns << zt, MatrixXd::Zero(9, 1);
		phi_ppg = mean_ba(3);
		the_ppg = mean_ba(4);
		psi_ppg = mean_ba(5);
	}

	// Check if the angle passes the singularity point for ZXY Euler angle

	double phi = zt(3);
	double the = zt(4);
	double psi = zt(5);
	if (phi_ppg - phi >  2 * M_PI) { zt(3) += 2 * M_PI; cout<<" phi changes up   2*pi: " << phi << endl; }
	if (phi_ppg - phi < -2 * M_PI) { zt(3) -= 2 * M_PI; cout<<" phi changes down 2*pi: " << phi << endl; }
	if (the_ppg - the >  2 * M_PI) { zt(4) += 2 * M_PI; cout<<" the changes up   2*pi: " << the << endl; }
	if (the_ppg - the < -2 * M_PI) { zt(4) -= 2 * M_PI; cout<<" the changes down 2*pi: " << the << endl; }
	if (psi_ppg - psi >  2 * M_PI) { zt(5) += 2 * M_PI; cout<<" psi changes up   2*pi: " << psi << endl; }
	if (psi_ppg - psi < -2 * M_PI) { zt(5) -= 2 * M_PI; cout<<" psi changes down 2*pi: " << psi << endl; }
	phi_ppg = phi;
	the_ppg = the;
	psi_ppg = psi;

    #if DEBUG_ODOM
        // cout<<" The x of camera: " << zt(0) <<endl;
        // cout<<" The y of camera: " << zt(1) <<endl;
        // cout<<" The z of camera: " << zt(2) <<endl;
        cout<<" mean_ba(3) : " << mean_ba(3) <<endl;
        cout<<" mean_ba(4) : " << mean_ba(4) <<endl;
        cout<<" mean_ba(5) : " << mean_ba(5) <<endl;
		cout<<" zt(3) : " << zt(3) <<endl;
        cout<<" zt(4) : " << zt(4) <<endl;
        cout<<" zt(5) : " << zt(5) <<endl;
        // cout<<" The pose of camera is in the coordinate frame: " <<  msg->header.frame_id <<endl;
        // cout<<" The twist of camera is in the child frame: " <<  msg->child_frame_id <<endl;
    #endif

    /*     Update, with C and W matrix										  */
    /*     Linear for this case, but still use  Extended Kalman Filter        */
    MatrixXd Kt = MatrixXd::Identity(15, 6); // Kalman
    MatrixXd Ct = MatrixXd::Identity(6, 15);
    // MatrixXd Wt = MatrixXd::Identity(6, 6);

    Kt = cov_ba * Ct.transpose() * ((Ct * cov_ba * Ct.transpose() + Rt).inverse());
	g_ut << mean_ba(0), mean_ba(1), mean_ba(2), mean_ba(3), mean_ba(4), mean_ba(5);
    mean_ns = mean_ba + Kt * (zt - g_ut);
    cov_ns  = cov_ba  - Kt * Ct * cov_ba;
    mean_ps = mean_ns;
    cov_ps  = cov_ns;

	// 2016.12.8 reconstruction
	Vector3d X_rpy;
	X_rpy(0) = mean_ns(3);
	X_rpy(1) = mean_ns(4);
	X_rpy(2) = mean_ns(5);
	Quaterniond Q_output;
	Q_output = rpy_to_R(X_rpy);

    nav_msgs::Odometry ekf_odom;
	if	(IMU_UPDATED) {
		ekf_odom.header.seq = msg->header.seq;
		ekf_odom.header.stamp = msg->header.stamp;
		ekf_odom.header.frame_id = "world";
		ekf_odom.pose.pose.position.x = mean_ns(0);
		ekf_odom.pose.pose.position.y = mean_ns(1);
		ekf_odom.pose.pose.position.z = mean_ns(2);
		ekf_odom.pose.pose.orientation.w = Q_output.w();
		ekf_odom.pose.pose.orientation.x = Q_output.x();
		ekf_odom.pose.pose.orientation.y = Q_output.y();
		ekf_odom.pose.pose.orientation.z = Q_output.z();
		ekf_odom.twist.twist.linear.x = mean_ns(6);
		ekf_odom.twist.twist.linear.y = mean_ns(7);
		ekf_odom.twist.twist.linear.z = mean_ns(8);
	}
	else {
		ekf_odom = *msg;
	}

    odom_pub.publish(ekf_odom);

	#if DEBUG_ODOM
		// cout<<" The Kalman gain is:" << endl << Kt << endl;
		// cout<<" The mean_ns is:" << endl << mean_ns << endl;
		cout<<" The cov_ns is:" << endl << cov_ns << endl;
		// cout<<" The g_ut is:" << endl << g_ut << endl;
		cout<<" The odometry before EKF:" << endl;
        ROS_INFO("Seq: [%d]", msg->header.seq);
		ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", camera_pose_wi_geo.translation.x,camera_pose_wi_geo.translation.y, camera_pose_wi_geo.translation.z);
		ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", camera_pose_wi_geo.rotation.x, camera_pose_wi_geo.rotation.y, camera_pose_wi_geo.rotation.z, camera_pose_wi_geo.rotation.w);
		cout<<" The odometry after EKF:" << endl;
        ROS_INFO("Seq: [%d]", ekf_odom.header.seq);
		ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", ekf_odom.pose.pose.position.x,ekf_odom.pose.pose.position.y, ekf_odom.pose.pose.position.z);
		ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", ekf_odom.pose.pose.orientation.x, ekf_odom.pose.pose.orientation.y, ekf_odom.pose.pose.orientation.z, ekf_odom.pose.pose.orientation.w);
		ROS_INFO("Vel-> Linear: [%f], Angular: [%f]", ekf_odom.twist.twist.linear.x,ekf_odom.twist.twist.angular.z);
    #endif
	#if DEBUG_COV
		ROS_INFO("Cam Seq: [%d]", ekf_odom.header.seq);
		cout<< "the mean_ns (0-5):" << endl << mean_ns.head(6) << endl;
		// cout<< "The cov_ns[6,6]  :" << endl << cov_ns.topLeftCorner(6, 6) << endl;
	#endif
	// cout<<" The end of Odometry callback" << endl << endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf");
    ros::NodeHandle n("~");
	ros::Time::init();
	current_time = ros::Time::now();
    ros::Subscriber s1 = n.subscribe("imu", 1000, imu_callback);
    ros::Subscriber s2 = n.subscribe("tag_odom", 1000, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>("ekf_odom", 100);
    // Q imu covariance matrix; Rt visual odomtry covariance matrix
    Q.topLeftCorner(6, 6) = 0.01 * Q.topLeftCorner(6, 6);
    Q.bottomRightCorner(6, 6) = 0.01 * Q.bottomRightCorner(6, 6);
    Rt.topLeftCorner(3, 3) = 0.5 * Rt.topLeftCorner(3, 3);
    Rt.bottomRightCorner(3, 3) = 0.5 * Rt.bottomRightCorner(3, 3);
    Rt.bottomRightCorner(1, 1) = 0.1 * Rt.bottomRightCorner(1, 1);

    ros::spin();
}


#include "joint_tracker/pdf/NonLinearRevoluteMeasurementPdf.h"

#include <Eigen/Geometry>
#include <lgsm/Lgsm>
#include <ros/ros.h>

#include "omip_common/OMIPUtils.h"

using namespace BFL;

#define REV_PDF_DIM 6
#define REV_CONDITIONAL_VAR_DIM 7
#define REV_NUM_CONDITIONAL_ARGS 2

NonLinearRevoluteMeasurementPdf::NonLinearRevoluteMeasurementPdf(const Gaussian& additiveNoise) :
    AnalyticConditionalGaussianAdditiveNoise(additiveNoise,REV_NUM_CONDITIONAL_ARGS),
    dfx(REV_PDF_DIM, REV_CONDITIONAL_VAR_DIM)
{

}

NonLinearRevoluteMeasurementPdf::~NonLinearRevoluteMeasurementPdf()
{

}

MatrixWrapper::ColumnVector NonLinearRevoluteMeasurementPdf::ExpectedValueGet() const
{
    ColumnVector state = ConditionalArgumentGet(0);

    double phi = state(1);
    double theta = state(2);
    double sp = sin(phi);
    double cp = cos(phi);
    double st = sin(theta);
    double ct = cos(theta);

    double px = state(3);
    double py = state(4);
    double pz = state(5);

    Eigen::Vector3d rev_joint_orientation = Eigen::Vector3d(cp * st, sp * st, ct);
    Eigen::Vector3d rev_joint_position = Eigen::Vector3d(px, py, pz);

    double rv = state(6);

    Eigen::Vector3d joint_translation = (-rv * rev_joint_orientation).cross(rev_joint_position);

    Eigen::Twistd joint_pose = Eigen::Twistd(rv * rev_joint_orientation.x(),
                                             rv * rev_joint_orientation.y(),
                                             rv * rev_joint_orientation.z(),
                                             joint_translation.x(),
                                             joint_translation.y(),
                                             joint_translation.z());

    ColumnVector expected_pose(REV_PDF_DIM);
    expected_pose(1) = joint_pose.vx();
    expected_pose(2) = joint_pose.vy();
    expected_pose(3) = joint_pose.vz();
    expected_pose(4) = joint_pose.rx();
    expected_pose(5) = joint_pose.ry();
    expected_pose(6) = joint_pose.rz();

    return expected_pose + this->AdditiveNoiseMuGet();
}

MatrixWrapper::Matrix NonLinearRevoluteMeasurementPdf::dfGet(unsigned int i) const
{
    if (i == 0) //derivative to the first conditional argument (x)
    {
        double phi = ConditionalArgumentGet(0)(1);
        double theta = ConditionalArgumentGet(0)(2);
        double sp = sin(phi);
        double cp = cos(phi);
        double st = sin(theta);
        double ct = cos(theta);

        double px = ConditionalArgumentGet(0)(3);
        double py = ConditionalArgumentGet(0)(4);
        double pz = ConditionalArgumentGet(0)(5);
        double rv = ConditionalArgumentGet(0)(6);

        dfx = 0;

        dfx(1, 1) = -rv * cp * st * pz;
        dfx(2, 1) = -rv * sp * st * pz;
        dfx(3, 1) = rv * st * (cp * px + sp * py);
        dfx(4, 1) = -rv * sp * st;
        dfx(5, 1) = rv * cp * st;

        dfx(1, 2) = -rv * (st * py + sp * ct * pz);
        dfx(2, 2) = rv * (cp * ct * pz + st * px);
        dfx(3, 2) = rv * ct * (sp * px - cp * py);
        dfx(4, 2) = rv * cp * ct;
        dfx(5, 2) = rv * sp * ct;
        dfx(6, 2) = -rv * st;

        dfx(2, 3) = -rv * ct;
        dfx(3, 3) = rv * sp * st;

        dfx(1, 4) = rv * ct;
        dfx(3, 4) = -rv * cp * st;

        dfx(1, 5) = -rv * sp * st;
        dfx(2, 5) = rv * cp * st;

        dfx(1, 6) = ct * py - sp * st * pz;
        dfx(2, 6) = cp * st * pz - ct * px;
        dfx(3, 6) = st * (sp * px - cp * py);
        dfx(4, 6) = cp * st;
        dfx(5, 6) = sp * st;
        dfx(6, 6) = ct;

        dfx(1, 7) = 0;
        dfx(2, 7) = 0;
        dfx(3, 7) = 0;
        dfx(4, 7) = 0;
        dfx(5, 7) = 0;
        dfx(6, 7) = 0;
        return dfx;
    }
    else
    {
        ROS_ERROR_STREAM_NAMED( "NonLinearRevoluteMeasurementPdf::dfGet",
                    "The derivative is not implemented for the " << i << "th conditional argument");
        exit(BFL_ERRMISUSE);
    }
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// Filename: matrix_free_2.cpp (part of MTL4)

#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	mtl::vampir_trace<9901> tracer;
	assert(int(size(v)) == m * n);
	Vector w(m * n);

	// Inner domain
	for (int i= 1; i < m-1; i++)
	    for (int j= 1, k= i * n + j; j < n-1; j++, k++)
		w[k]= 4 * v[k] - v[k-n] - v[k+n] - v[k-1] - v[k+1];

	// Upper border
	for (int j= 1; j < n-1; j++)
	    w[j]= 4 * v[j] - v[j+n] - v[j-1] - v[j+1];

	// Lower border
	for (int j= 1, k= (m-1) * n + j; j < n-1; j++, k++)
	    w[k]= 4 * v[k] - v[k-n] - v[k-1] - v[k+1];

	// Left border
	for (int i= 1, k= n; i < m-1; i++, k+= n)
	    w[k]= 4 * v[k] - v[k-n] - v[k+n] - v[k+1];

	// Right border
	for (int i= 1, k= n+n-1; i < m-1; i++, k+= n)
	    w[k]= 4 * v[k] - v[k-n] - v[k+n] - v[k-1];

	// Corners
	w[0]= 4 * v[0] - v[1] - v[n];
	w[n-1]= 4 * v[n-1] - v[n-2] - v[2*n - 1];
	w[(m-1)*n]= 4 * v[(m-1)*n] - v[(m-2)*n] - v[(m-1)*n+1];
	w[m*n-1]= 4 * v[m*n-1] - v[m*n-2] - v[m*n-n-1];

	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main(int, char**)
{
    using namespace std;

    mtl::compressed2D<double> B;
    laplacian_setup(B, 1000, 1000);

    mtl::dense_vector<double> v(1000000), w(1000000);
    iota(v);

    poisson2D_dirichlet A(1000, 1000);
    // cout << "A * v is " << A * v << endl;
    w= A * v;

    w= B * v;

     return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ARITHMETIC_NORMALIZE_HPP
#define BOOST_GEOMETRY_ARITHMETIC_NORMALIZE_HPP

#include <boost/geometry/core/coordinate_type.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Point>
inline typename coordinate_type<Point>::type vec_length_sqr(Point const& pt)
{
    return dot_product(pt, pt);
}

template <typename Point>
inline typename coordinate_type<Point>::type vec_length(Point const& pt)
{
    // NOTE: hypot() could be used instead of sqrt()
    return math::sqrt(dot_product(pt, pt));
}

template <typename Point>
inline bool vec_normalize(Point & pt, typename coordinate_type<Point>::type & len)
{
    typedef typename coordinate_type<Point>::type coord_t;

    coord_t const c0 = 0;
    len = vec_length(pt);

    if (math::equals(len, c0))
    {
        return false;
    }

    divide_value(pt, len);
    return true;
}

template <typename Point>
inline bool vec_normalize(Point & pt)
{
    typedef typename coordinate_type<Point>::type coord_t;
    coord_t len;
    return vec_normalize(pt, len);
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_NORMALIZE_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_CUPPEN_INCLUDE
#define MTL_MATRIX_CUPPEN_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/assert.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/iota.hpp>
#include <boost/numeric/mtl/operation/secular.hpp>
#include <boost/numeric/mtl/operation/sort.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/utility/domain.hpp>
#include <boost/numeric/mtl/matrix/permutation.hpp>

#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/itl/iteration/basic_iteration.hpp>
#include <boost/numeric/itl/krylov/fsm.hpp>

namespace mtl { namespace mat {

/// Eigenvalues of triangle matrix A with Cuppen's divide and conquer algorithm
/** Eigenvalues are returned in vector lambda. A is overwritten. **/
template <typename Matrix, typename Vector>
void inline cuppen_inplace(Matrix& A, Matrix& Q, Vector& lambda)
{
    using std::abs; using mtl::irange; using mtl::imax; using mtl::iall; using vec::iota;

    typedef typename Collection<Matrix>::value_type     value_type;
    typedef typename Collection<Matrix>::size_type      size_type;
    typedef vec::dense_vector<size_type, vec::parameters<> >   size_vector; // todo: with type trait

    size_type        nrows= num_rows(A);
    MTL_CRASH_IF(nrows != num_cols(A), "Matrix not square");
    const value_type zero= 0, one= 1;

    if (nrows == 1){
	lambda[0]= A[0][0];
	Q= one;
    } else {
	size_type     m= size_type(nrows/2);
	irange        till_m(m), from_m(m, imax);

	size_vector   perm(nrows);
	Matrix        T1(A[till_m][till_m]), T2(A[from_m][from_m]),                               // sub-matrices of A
	              Q0(nrows, nrows), Q1(Q0[till_m][till_m]), Q2(Q0[from_m][from_m]);           // Q0 and sub-matrices
	Vector        v(nrows, zero), diag(nrows), lambda1(diag[till_m]), lambda2(diag[from_m]);  // sub-vectors of diag

	//DIVIDE
	value_type    b= A[m-1][m];
	T1[m-1][m-1]-= abs(b);
	T2[0][0]-= abs(b);

	v[m-1]= b > zero ? one : -one;
	v[m]= one;

	cuppen_inplace(T1, Q1, lambda1);
	cuppen_inplace(T2, Q2, lambda2);

	Q0[till_m][from_m]= zero; Q0[from_m][till_m]= zero; // zero out non-diagonal blocks

	T1[m-1][m-1]+= abs(b);
	T2[0][0]+= abs(b);

	iota(perm);
	sort(diag, perm);

	// CONQUER, start with eq. (3.0.2) using rows (not columns)
	v[till_m]= b < zero ? Vector(-trans(Q1[m-1][iall])) : trans(Q1[m-1][iall]);
	v[from_m]= trans(Q2[0][iall]);

	// permutation on v
	mtl::mat::traits::permutation<>::type P= mtl::mat::permutation(perm);
	Vector v1(P * v);

	lambda= secular(v1, diag, abs(b));   // solve secular equation

	// std::cout << "lambda is " << lambda << "\ndiag is " << diag << '\n';
	//Lemma 3.0.2  ... calculate eigenvectors
	Matrix Q_tilde(nrows, nrows);
	for (size_type i = 0; i < nrows; i++) {
	    for (size_type j= 0; j < size(diag); ++j)
		MTL_THROW_IF (diag[j] == lambda[i],
			      logic_error("Can't compute eigenvector, probably due to double eigenvalue."));
	    Vector    li(nrows, lambda[i]), lambda_i(ele_quot(v1, diag - li));
	    Q_tilde[iall][i]= lambda_i / two_norm(lambda_i); // normalized eigenvector in Matrix Q
	}

	Q= Q0 * P * Q_tilde;

#if 0
	for (size_type i = 0; i < nrows; i++) {
	    // Vector    qi(Q[iall][i]); // Todo: find out valgrind complains about the memory of qi for reasons inexplicable
	    Vector qi(nrows);
	    for (size_type j= 0; j < nrows; j++)
		qi[j]= Q[j][i];

	    std::cout << "q[" << i << "] = " << qi << ", lambda[i] = " << lambda[i]
		      << ", diff = " << two_norm(Vector(A*qi - lambda[i]*qi)) << ", A*qi = " << Vector(A*qi) << ", li*qi = " << Vector(lambda[i]*qi) << '\n';
	    itl::basic_iteration<double>   iter(1.0, 20, 1e-5, 1e-5);
	    fsm(A, qi, lambda[i], 0.1, iter);
	    std::cout << "q[" << i << "] = " << qi << ", diff = " << two_norm(Vector(A*qi - lambda[i]*qi)) << '\n';
	    Q[iall][i]= qi;
	}
#endif
    }
}

/// Eigenvalues of triangle matrix A with Cuppen's divide and conquer algorithm
/** Eigenvalues are returned in vector lambda. A is copied. **/
// A not as reference to force copy
template <typename Matrix, typename Vector>
void inline cuppen(Matrix A, Matrix& Q, Vector& lambda)
{
    Q= 0.0;
    cuppen_inplace(A, Q, lambda);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_CUPPEN_INCLUDE


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

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

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

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

/*! \file spreadedoptionletvolatility2.hpp
    \brief Optionlet volaility with overlayed bilinearly interpolated spread surface
    \ingroup termstructures
*/

#pragma once

#include <ql/math/interpolations/interpolation2d.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/volatility/optionlet/optionletvolatilitystructure.hpp>

#include <boost/smart_ptr/shared_ptr.hpp>

namespace QuantExt {
using namespace QuantLib;

class SpreadedOptionletVolatility2 : public OptionletVolatilityStructure, public LazyObject {
public:
    SpreadedOptionletVolatility2(const Handle<OptionletVolatilityStructure>& baseVol,
                                 const std::vector<Date>& optionDates, const std::vector<Real>& strikes,
                                 const std::vector<std::vector<Handle<Quote>>>& volSpreads);
    BusinessDayConvention businessDayConvention() const override;
    Rate minStrike() const override;
    Rate maxStrike() const override;
    DayCounter dayCounter() const override;
    Date maxDate() const override;
    Time maxTime() const override;
    const Date& referenceDate() const override;
    Calendar calendar() const override;
    Natural settlementDays() const override;
    VolatilityType volatilityType() const override;
    Real displacement() const override;
    void update() override;
    void deepUpdate() override;

protected:
    boost::shared_ptr<SmileSection> smileSectionImpl(Time optionTime) const override;
    Volatility volatilityImpl(Time optionTime, Rate strike) const override;
    void performCalculations() const override;

private:
    Handle<OptionletVolatilityStructure> baseVol_;
    std::vector<Date> optionDates_;
    std::vector<Real> strikes_;
    std::vector<std::vector<Handle<Quote>>> volSpreads_;
    //
    mutable std::vector<Real> optionTimes_;
    mutable Matrix volSpreadValues_;
    mutable Interpolation2D volSpreadInterpolation_;
};
} // namespace QuantExt


#include <boost/math/constants/constants.hpp>
#include <catch2/catch.hpp>
#include <complex>
#include "ear/bs2051.hpp"
#include "ear/decorrelate.hpp"
#include "ear/layout.hpp"
#include "helper/vector_approx.hpp"
#include "kissfft/kissfft.hh"

const double PI = boost::math::constants::pi<double>();

using namespace ear;

// declare private decorrelate functions
namespace ear {
  std::vector<double> designDecorrelatorBasic(int decorrelatorId, int size);
  std::vector<long> genRandMt19937(int seed, int n);
};  // namespace ear

TEST_CASE("test_gen_rand_mt19937") {
  int seed = 5489;
  int i = 10000;
  long expected = 4123659995;
  REQUIRE(genRandMt19937(seed, i)[i - 1] == expected);
};

TEST_CASE("test_design_decorrelator") {
  double rand = genRandMt19937(0, 1)[0] / static_cast<double>(UINT32_MAX - 1);
  std::vector<std::complex<double>> expected = {
      std::complex<double>(1.0, 1.0),
      std::exp(std::complex<double>(0.0, 2.0 * rand * PI))};
  std::vector<double> filt = designDecorrelatorBasic(0, 512);
  REQUIRE(filt.size() == 512);
  kissfft<double> fft(256, false);
  std::vector<std::complex<double>> actual(512);
  fft.transform_real(&filt[0], &actual[0]);
  REQUIRE(actual[0].real() == Approx(expected[0].real()));
  REQUIRE(actual[0].imag() == Approx(expected[0].imag()));
  REQUIRE(actual[1].real() == Approx(expected[1].real()));
  REQUIRE(actual[1].imag() == Approx(expected[1].imag()));
};

TEST_CASE("test_design_decorrelators") {
  Layout layout = getLayout("4+5+0").withoutLfe();
  std::vector<std::vector<double>> filters =
      designDecorrelators<double>(layout);

  // M+030 should get the second filter
  boost::optional<int> index = layout.indexForName("M+030");
  auto rightFilter = filters[boost::get<int>(index)];
  REQUIRE_VECTOR_APPROX(rightFilter, designDecorrelatorBasic(1, 512));
}


#include "wave/optimization/ceres/odom_gp_reduced/point_to_line_gp.hpp"
#include <Eigen/QR>

namespace wave {

SE3PointToLineGPRed::SE3PointToLineGPRed(const double *const p,
                                         const double *const pA,
                                         const double *const pB,
                                         const Mat12 &hat,
                                         const Mat12 &candle,
                                         const Mat3 &CovZ,
                                         bool calculate_weight)
    : pt(p), ptA(pA), ptB(pB), hat(hat), candle(candle) {
    this->JP_T.setZero();
    this->JP_T.block<3, 3>(0, 3).setIdentity();

    this->diff[0] = this->ptB[0] - this->ptA[0];
    this->diff[1] = this->ptB[1] - this->ptA[1];
    this->diff[2] = this->ptB[2] - this->ptA[2];
    this->bottom = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

    if (this->bottom < 1e-10) {
        // The points defining the line are too close to each other
        throw std::out_of_range("Points defining line are too close!");
    }

    this->Jres_P(0, 0) = 1 - (diff[0] * diff[0] / bottom);
    this->Jres_P(0, 1) = -(diff[0] * diff[1] / bottom);
    this->Jres_P(0, 2) = -(diff[0] * diff[2] / bottom);
    this->Jres_P(1, 0) = -(diff[1] * diff[0] / bottom);
    this->Jres_P(1, 1) = 1 - (diff[1] * diff[1] / bottom);
    this->Jres_P(1, 2) = -(diff[1] * diff[2] / bottom);
    this->Jres_P(2, 0) = -(diff[2] * diff[0] / bottom);
    this->Jres_P(2, 1) = -(diff[2] * diff[1] / bottom);
    this->Jres_P(2, 2) = 1 - (diff[2] * diff[2] / bottom);

    Eigen::Vector3d unitdiff;
    double invlength = 1.0 / sqrt(this->bottom);
    if (this->diff[2] > 0) {
        unitdiff[0] = this->diff[0] * invlength;
        unitdiff[1] = this->diff[1] * invlength;
        unitdiff[2] = this->diff[2] * invlength;
    } else {
        unitdiff[0] = -this->diff[0] * invlength;
        unitdiff[1] = -this->diff[1] * invlength;
        unitdiff[2] = -this->diff[2] * invlength;
    }

    Eigen::Vector3d unitz;
    unitz << 0, 0, 1;

    auto v = unitdiff.cross(unitz);
    auto s = v.norm();
    auto c = unitz.dot(unitdiff);
    Mat3 skew;
    Transformation<>::skewSymmetric3(v, skew);
    this->rotation = Eigen::Matrix3d::Identity() + skew + skew * skew * ((1 - c) / (s * s));

    this->Jres_P = this->rotation * this->Jres_P;

    if (calculate_weight) {
        auto rotated = this->Jres_P * CovZ * this->Jres_P.transpose();
        this->weight_matrix = rotated.block<2, 2>(0, 0).inverse().sqrt();
    } else {
        this->weight_matrix.setIdentity();
    }
}

bool SE3PointToLineGPRed::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    Eigen::Map<const Mat34> tk_map(parameters[0], 3, 4);
    Eigen::Map<const Mat34> tkp1_map(parameters[1], 3, 4);

    Transformation<Eigen::Map<const Mat34>, true> Tk(tk_map);
    Transformation<Eigen::Map<const Mat34>, true> Tkp1(tkp1_map);

    Eigen::Map<const Vec6> vel_k(parameters[2], 6, 1);

    if (jacobians) {
        Transformation<Eigen::Map<const Eigen::Matrix<double, 3, 4>>, true>::interpolateAndJacobians(
          Tk,
          Tkp1,
          vel_k,
          vel_k,
          this->hat,
          this->candle,
          this->T_current,
          this->JT_Ti,
          this->JT_Tip1,
          this->JT_Wi,
          this->JT_Wip1);
    } else {
        Transformation<Eigen::Map<const Eigen::Matrix<double, 3, 4>>, true>::interpolate(
          Tk, Tkp1, vel_k, vel_k, this->hat, this->candle, this->T_current);
    }

    Eigen::Map<const Vec3> PT(this->pt, 3, 1);
    Vec3 point = this->T_current.transform(PT);

    double p_A[3] = {point(0) - this->ptA[0], point(1) - this->ptA[1], point(2) - this->ptA[2]};

    double scaling = ceres::DotProduct(p_A, diff);
    // point on line closest to point
    double p_Tl[3] = {this->ptA[0] + (scaling / bottom) * diff[0],
                      this->ptA[1] + (scaling / bottom) * diff[1],
                      this->ptA[2] + (scaling / bottom) * diff[2]};

    Eigen::Map<const Vec3> pt_Tl(p_Tl, 3, 1);
    Eigen::Map<Eigen::Vector2d> reduced(residuals, 2, 1);

    reduced = this->weight_matrix * (this->rotation * (point - pt_Tl)).block<2, 1>(0, 0);

    if (jacobians != nullptr) {
        this->JP_T(0, 1) = point(2);
        this->JP_T(0, 2) = -point(1);
        this->JP_T(1, 0) = -point(2);
        this->JP_T(1, 2) = point(0);
        this->JP_T(2, 0) = point(1);
        this->JP_T(2, 1) = -point(0);

        // Jres_P already has rotation incorporated during construction
        this->Jr_T = this->Jres_P * this->JP_T;

        if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tk(jacobians[0], 2, 12);
            Jr_Tk.block<2, 6>(0, 0) = this->weight_matrix * this->Jr_T.block<2, 6>(0, 0) * this->JT_Ti;
            Jr_Tk.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tkp1(jacobians[1], 2, 12);
            Jr_Tkp1.block<2, 6>(0, 0) = this->weight_matrix * this->Jr_T.block<2, 6>(0, 0) * this->JT_Tip1;
            Jr_Tkp1.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[2]) {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> jac_map(jacobians[2], 2, 6);
            jac_map = this->weight_matrix * this->Jr_T.block<2, 6>(0, 0) * (this->JT_Wi + this->JT_Wip1);
        }
    }

    return true;
}

}  // namespace wave


#include "segments.hpp"

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

#include <boost/foreach.hpp>

using namespace std;

int orientation(Point A, Point B, Point C)
{
    int val = (B[1] - A[1]) * (C[0] - B[0]) - (B[0] - A[0]) * (C[1] - B[1]);
    return val == 0 ? 0 : (val > 0 ? 1 : 2);
}

bool onSegment(Point P, Segment S) //P collinear with S
{
    if (P[0] <= std::max(S.A[0], S.B[0]) &&
        P[0] >= std::min(S.A[0], S.B[0]) &&
        P[1] <= std::max(S.A[1], S.B[1]) &&
        P[1] >= std::min(S.A[1], S.B[1]) )
    {
        return true;
    }
    return false;
}

int intersect(Segment sA, Segment sB)
{
    Point PA = sA.A, QA = sA.B, PB = sB.B, QB = sB.B;
    int o1 = orientation(PA, QA, PB);
    int o2 = orientation(PA, QA, QB);
    int o3 = orientation(PB, QB, PA);
    int o4 = orientation(PB, QB, QA);

    if (o1 != o2 && o3 != o4)
        return 1;

    if (o1 == 0 && onSegment(PB, sA)) return 1;
    if (o2 == 0 && onSegment(QB, sA)) return 1;
    if (o3 == 0 && onSegment(PA, sB)) return 1;
    if (o4 == 0 && onSegment(QA, sB)) return 1;

    return 0;

}

bool isInPolygon(Polygon polygon, Point P)
{
    Point Q = P;
    Q[0] = INF;
    int nInter = 0;
    Segment halfLine;
    halfLine.A = P;
    halfLine.B = Q;

    BOOST_FOREACH(Segment seg, polygon.seg)
    {
        if (orientation(seg.A, P, seg.B) == 0)
            return onSegment(P, seg);
        nInter += intersect(seg, halfLine);
    }

    return nInter&1;
}


#ifndef TRIUMF_BNMR_NUCLEI_HPP
#define TRIUMF_BNMR_NUCLEI_HPP

#include <boost/math/constants/constants.hpp>

#include <triumf/nmr/utilities.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// β-NMR probe nuclei and their properties
namespace nuclei {

// lithium-8
template <typename T = double> struct lithium_8 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 1e-3 * 838.40; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return 1.65350; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0326; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

// beryllium-11
template <typename T = double> struct beryllium_11 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 13.76; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 1.0 / 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return -1.6816; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

// boron-12
template <typename T = double> struct boron_12 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 1e-3 * 20.20; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 1.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return 1.003; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0132; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

// magnesium-31
template <typename T = double> struct magnesium_31 {
  // radioactive half-life (s)
  static inline constexpr T half_life() { return 1e-3 * 236.0; }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 1.0 / 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return -0.88340; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return 0.0; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

} // namespace nuclei

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_NUCLEI_HPP


#include <cstddef>
#include <cassert>
#include <immintrin.h> // AVX2

#include <benchmark/benchmark.h>
#include <boost/numeric/ublas/vector.hpp>

namespace
{
  const std::size_t kVectorSize = 100000000;

  double doNaiveDotProduct(const std::vector<double>& v1, const std::vector<double>& v2)
  {
    assert((v1.size() == v2.size()) && "Dot product can only be done on vectors with the same size");

    double dotProduct = 0.0;

    for (std::size_t i = 0; i < v1.size(); ++i)
    {
      dotProduct += (v1[i] * v2[i]);
    }

    return dotProduct;
  }

  double doBoostDotProduct(const boost::numeric::ublas::vector<double>& v1, const boost::numeric::ublas::vector<double>& v2)
  {
    return inner_prod(v1, v2);
  }

  double doSimdDotProduct(const std::vector<double>& v1, const std::vector<double>& v2)
  {
    assert((v1.size() == v2.size()) && "Dot product can only be done on vectors with the same size");

    const std::size_t numPacks = v1.size() / 4; // 4 doubles per 256-bit register
    const std::size_t lastPackSize = v1.size() % 4;

    __m256d accumulated = _mm256_setzero_pd();
    __m256d a;
    __m256d b;

    for (std::size_t i = 0; i < numPacks; ++i)
    {
      a.m256d_f64[0] = v1[i * 4];
      a.m256d_f64[1] = v1[i * 4 + 1];
      a.m256d_f64[2] = v1[i * 4 + 2];
      a.m256d_f64[3] = v1[i * 4 + 3];

      b.m256d_f64[0] = v2[i * 4];
      b.m256d_f64[1] = v2[i * 4 + 1];
      b.m256d_f64[2] = v2[i * 4 + 2];
      b.m256d_f64[3] = v2[i * 4 + 3];

      accumulated = _mm256_fmaddsub_pd(a, b, accumulated);
    }

    const std::size_t lastPackOffset = numPacks * 4;

    for (std::size_t i = 0; i < 4; ++i)
    {
      a.m256d_f64[i] = (i < lastPackSize) ? v1[lastPackOffset + i] : 0.0;
      b.m256d_f64[i] = (i < lastPackSize) ? v2[lastPackOffset + i] : 0.0;
    }

    accumulated = _mm256_fmaddsub_pd(a, b, accumulated);

    __m128d accumulatedHigh = _mm256_extractf128_pd(accumulated, 1);
    __m128d sum = _mm_add_pd(accumulatedHigh, _mm256_castpd256_pd128(accumulated));

    return sum.m128d_f64[0] + sum.m128d_f64[1];
  }
}

static void naiveDotProduct(benchmark::State& state)
{
  std::vector<double> v1(kVectorSize);
  std::vector<double> v2(kVectorSize);

  while (state.KeepRunning())
  {
    doNaiveDotProduct(v1, v2);
  }
}
BENCHMARK(naiveDotProduct);

static void boostDotProduct(benchmark::State& state)
{
  boost::numeric::ublas::vector<double> v1(kVectorSize);
  boost::numeric::ublas::vector<double> v2(kVectorSize);

  while (state.KeepRunning())
  {
    doBoostDotProduct(v1, v2);
  }
}
BENCHMARK(boostDotProduct);

static void simdDotProduct(benchmark::State& state)
{
  std::vector<double> v1(kVectorSize);
  std::vector<double> v2(kVectorSize);

  while (state.KeepRunning())
  {
    doSimdDotProduct(v1, v2);
  }
}
BENCHMARK(simdDotProduct);

BENCHMARK_MAIN();


/**
 * @file tests/det_test.cpp
 * @author Parikshit Ram (pram@cc.gatech.edu)
 *
 * Unit tests for the functions of the class DTree and the utility functions
 * using this class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

// This trick does not work on Windows.  We will have to comment out the tests
// that depend on it.
#ifndef _WIN32
  #define protected public
  #define private public
#endif

#include <mlpack/methods/det/dtree.hpp>
#include <mlpack/methods/det/dt_utils.hpp>

#ifndef _WIN32
  #undef protected
  #undef private
#endif

using namespace mlpack;
using namespace mlpack::det;
using namespace std;

BOOST_AUTO_TEST_SUITE(DETTest);

// Tests for the private functions.  We cannot perform these if we are on
// Windows because we cannot make private functions accessible using the macro
// trick above.
#ifndef _WIN32
BOOST_AUTO_TEST_CASE(TestGetMaxMinVals)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<arma::mat> tree(testData);

  BOOST_REQUIRE_EQUAL(tree.MaxVals()[0], 7);
  BOOST_REQUIRE_EQUAL(tree.MinVals()[0], 3);
  BOOST_REQUIRE_EQUAL(tree.MaxVals()[1], 7);
  BOOST_REQUIRE_EQUAL(tree.MinVals()[1], 0);
  BOOST_REQUIRE_EQUAL(tree.MaxVals()[2], 8);
  BOOST_REQUIRE_EQUAL(tree.MinVals()[2], 1);
}

BOOST_AUTO_TEST_CASE(TestComputeNodeError)
{
  arma::vec maxVals("7 7 8");
  arma::vec minVals("3 0 1");

  DTree<arma::mat> testDTree(maxVals, minVals, 5);
  double trueNodeError = -log(4.0) - log(7.0) - log(7.0);

  BOOST_REQUIRE_CLOSE((double) testDTree.logNegError, trueNodeError, 1e-10);

  testDTree.start = 3;
  testDTree.end = 5;

  double nodeError = testDTree.LogNegativeError(5);
  trueNodeError = 2 * log(2.0 / 5.0) - log(4.0) - log(7.0) - log(7.0);
  BOOST_REQUIRE_CLOSE(nodeError, trueNodeError, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestWithinRange)
{
  arma::vec maxVals("7 7 8");
  arma::vec minVals("3 0 1");

  DTree<arma::mat> testDTree(maxVals, minVals, 5);

  arma::vec testQuery(3);
  testQuery << 4.5 << 2.5 << 2;

  BOOST_REQUIRE_EQUAL(testDTree.WithinRange(testQuery), true);

  testQuery << 8.5 << 2.5 << 2;

  BOOST_REQUIRE_EQUAL(testDTree.WithinRange(testQuery), false);
}

BOOST_AUTO_TEST_CASE(TestFindSplit)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<arma::mat> testDTree(testData);

  size_t obDim;
  double obLeftError, obRightError, obSplit;

  size_t trueDim = 2;
  double trueSplit = 5.5;
  double trueLeftError = 2 * log(2.0 / 5.0) - (log(7.0) + log(4.0) + log(4.5));
  double trueRightError = 2 * log(3.0 / 5.0) - (log(7.0) + log(4.0) + log(2.5));

  testDTree.logVolume = log(7.0) + log(4.0) + log(7.0);
  BOOST_REQUIRE(testDTree.FindSplit(
      testData, obDim, obSplit, obLeftError, obRightError, 1));

  BOOST_REQUIRE(trueDim == obDim);
  BOOST_REQUIRE_CLOSE(trueSplit, obSplit, 1e-10);

  BOOST_REQUIRE_CLOSE(trueLeftError, obLeftError, 1e-10);
  BOOST_REQUIRE_CLOSE(trueRightError, obRightError, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestSplitData)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<arma::mat> testDTree(testData);

  arma::Col<size_t> oTest(5);
  oTest << 1 << 2 << 3 << 4 << 5;

  size_t splitDim = 2;
  double trueSplitVal = 5.5;

  size_t splitInd = testDTree.SplitData(
      testData, splitDim, trueSplitVal, oTest);

  BOOST_REQUIRE_EQUAL(splitInd, 2); // 2 points on left side.

  BOOST_REQUIRE_EQUAL(oTest[0], 1);
  BOOST_REQUIRE_EQUAL(oTest[1], 4);
  BOOST_REQUIRE_EQUAL(oTest[2], 3);
  BOOST_REQUIRE_EQUAL(oTest[3], 2);
  BOOST_REQUIRE_EQUAL(oTest[4], 5);
}

BOOST_AUTO_TEST_CASE(TestSparseFindSplit)
{
  arma::mat realData(4, 7);

  realData << .0 << 4 << 5 << 7 << 0 << 5 << 0 << arma::endr
           << .0 << 5 << 0 << 0 << 1 << 7 << 1 << arma::endr
           << .0 << 5 << 6 << 7 << 1 << 0 << 8 << arma::endr
           << -1 << 2 << 5 << 0 << 0 << 0 << 0 << arma::endr;

  arma::sp_mat testData(realData);

  DTree<arma::sp_mat> testDTree(testData);

  size_t obDim;
  double obLeftError, obRightError, obSplit;

  size_t trueDim = 1;
  double trueSplit = .5;
  double trueLeftError = 2 * log(3.0 / 7.0) -
      (log(7.0) + log(0.5) + log(8.0) + log(6.0));
  double trueRightError = 2 * log(4.0 / 7.0) -
      (log(7.0) + log(6.5) + log(8.0) + log(6.0));

  testDTree.logVolume = log(7.0) + log(7.0) + log(8.0) + log(6.0);
  BOOST_REQUIRE(testDTree.FindSplit(
      testData, obDim, obSplit, obLeftError, obRightError, 1));

  BOOST_REQUIRE(trueDim == obDim);
  BOOST_REQUIRE_CLOSE(trueSplit, obSplit, 1e-10);

  BOOST_REQUIRE_CLOSE(trueLeftError, obLeftError, 1e-10);
  BOOST_REQUIRE_CLOSE(trueRightError, obRightError, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestSparseSplitData)
{
  arma::mat realData(4, 7);

  realData << .0 << 4 << 5 << 7 << 0 << 5 << 0 << arma::endr
           << .0 << 5 << 0 << 0 << 1 << 7 << 1 << arma::endr
           << .0 << 5 << 6 << 7 << 1 << 0 << 8 << arma::endr
           << -1 << 2 << 5 << 0 << 0 << 0 << 0 << arma::endr;

  arma::sp_mat testData(realData);

  DTree<arma::sp_mat> testDTree(testData);

  arma::Col<size_t> oTest(7);
  oTest << 1 << 2 << 3 << 4 << 5 << 6 << 7;

  size_t splitDim = 1;
  double trueSplitVal = .5;

  size_t splitInd = testDTree.SplitData(
      testData, splitDim, trueSplitVal, oTest);

  BOOST_REQUIRE_EQUAL(splitInd, 3); // 2 points on left side.

  BOOST_REQUIRE_EQUAL(oTest[0], 1);
  BOOST_REQUIRE_EQUAL(oTest[1], 4);
  BOOST_REQUIRE_EQUAL(oTest[2], 3);
  BOOST_REQUIRE_EQUAL(oTest[3], 2);
  BOOST_REQUIRE_EQUAL(oTest[4], 5);
  BOOST_REQUIRE_EQUAL(oTest[5], 6);
  BOOST_REQUIRE_EQUAL(oTest[6], 7);
}

#endif

// Tests for the public functions.

BOOST_AUTO_TEST_CASE(TestGrow)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  double rootError, lError, rError, rlError, rrError;

  rootError = -log(4.0) - log(7.0) - log(7.0);

  lError = 2 * log(2.0 / 5.0) - (log(7.0) + log(4.0) + log(4.5));
  rError =  2 * log(3.0 / 5.0) - (log(7.0) + log(4.0) + log(2.5));

  rlError = 2 * log(1.0 / 5.0) - (log(0.5) + log(4.0) + log(2.5));
  rrError = 2 * log(2.0 / 5.0) - (log(6.5) + log(4.0) + log(2.5));

  DTree<arma::mat> testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);

  BOOST_REQUIRE_EQUAL(oTest[0], 0);
  BOOST_REQUIRE_EQUAL(oTest[1], 3);
  BOOST_REQUIRE_EQUAL(oTest[2], 1);
  BOOST_REQUIRE_EQUAL(oTest[3], 2);
  BOOST_REQUIRE_EQUAL(oTest[4], 4);

  // Test the structure of the tree.
  BOOST_REQUIRE(testDTree.Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree.Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Right()->Left() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Right()->Right() == NULL);

  BOOST_REQUIRE(testDTree.SubtreeLeaves() == 3);

  BOOST_REQUIRE(testDTree.SplitDim() == 2);
  BOOST_REQUIRE_CLOSE(testDTree.SplitValue(), 5.5, 1e-5);
  BOOST_REQUIRE(testDTree.Right()->SplitDim() == 1);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->SplitValue(), 0.5, 1e-5);

  // Test node errors for every node (these are private functions).
#ifndef _WIN32
  BOOST_REQUIRE_CLOSE(testDTree.logNegError, rootError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Left()->logNegError, lError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->logNegError, rError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->Left()->logNegError, rlError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->Right()->logNegError, rrError, 1e-10);
#endif

  // Test alpha.
  double rootAlpha, rAlpha;
  rootAlpha = std::log(-((std::exp(rootError) - (std::exp(lError) +
      std::exp(rlError) + std::exp(rrError))) / 2));
  rAlpha = std::log(-(std::exp(rError) - (std::exp(rlError) +
      std::exp(rrError))));

  BOOST_REQUIRE_CLOSE(alpha, min(rootAlpha, rAlpha), 1e-10);
}

BOOST_AUTO_TEST_CASE(TestPruneAndUpdate)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;
  DTree<arma::mat> testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);
  alpha = testDTree.PruneAndUpdate(alpha, testData.n_cols, false);

  BOOST_REQUIRE_CLOSE(alpha, numeric_limits<double>::max(), 1e-10);
  BOOST_REQUIRE(testDTree.SubtreeLeaves() == 1);

  double rootError = -log(4.0) - log(7.0) - log(7.0);

  BOOST_REQUIRE_CLOSE(testDTree.LogNegError(), rootError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.SubtreeLeavesLogNegError(), rootError, 1e-10);
  BOOST_REQUIRE(testDTree.Left() == NULL);
  BOOST_REQUIRE(testDTree.Right() == NULL);
}

BOOST_AUTO_TEST_CASE(TestComputeValue)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::vec q1(3), q2(3), q3(3), q4(3);

  q1 << 4 << 2 << 2;
  q2 << 5 << 0.25 << 6;
  q3 << 5 << 3 << 7;
  q4 << 2 << 3 << 3;

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::mat> testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);

  double d1 = (2.0 / 5.0) / exp(log(4.0) + log(7.0) + log(4.5));
  double d2 = (1.0 / 5.0) / exp(log(4.0) + log(0.5) + log(2.5));
  double d3 = (2.0 / 5.0) / exp(log(4.0) + log(6.5) + log(2.5));

  BOOST_REQUIRE_CLOSE(d1, testDTree.ComputeValue(q1), 1e-10);
  BOOST_REQUIRE_CLOSE(d2, testDTree.ComputeValue(q2), 1e-10);
  BOOST_REQUIRE_CLOSE(d3, testDTree.ComputeValue(q3), 1e-10);
  BOOST_REQUIRE_CLOSE(0.0, testDTree.ComputeValue(q4), 1e-10);

  alpha = testDTree.PruneAndUpdate(alpha, testData.n_cols, false);

  double d = 1.0 / exp(log(4.0) + log(7.0) + log(7.0));

  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q1), 1e-10);
  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q2), 1e-10);
  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q3), 1e-10);
  BOOST_REQUIRE_CLOSE(0.0, testDTree.ComputeValue(q4), 1e-10);
}

BOOST_AUTO_TEST_CASE(TestVariableImportance)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  double rootError, lError, rError, rlError, rrError;

  rootError = -1.0 * exp(-log(4.0) - log(7.0) - log(7.0));

  lError = -1.0 * exp(2 * log(2.0 / 5.0) - (log(7.0) + log(4.0) + log(4.5)));
  rError =  -1.0 * exp(2 * log(3.0 / 5.0) - (log(7.0) + log(4.0) + log(2.5)));

  rlError = -1.0 * exp(2 * log(1.0 / 5.0) - (log(0.5) + log(4.0) + log(2.5)));
  rrError = -1.0 * exp(2 * log(2.0 / 5.0) - (log(6.5) + log(4.0) + log(2.5)));

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::mat> testDTree(testData);
  testDTree.Grow(testData, oTest, false, 2, 1);

  arma::vec imps;

  testDTree.ComputeVariableImportance(imps);

  BOOST_REQUIRE_CLOSE((double) 0.0, imps[0], 1e-10);
  BOOST_REQUIRE_CLOSE((double) (rError - (rlError + rrError)), imps[1], 1e-10);
  BOOST_REQUIRE_CLOSE((double) (rootError - (lError + rError)), imps[2], 1e-10);
}

BOOST_AUTO_TEST_CASE(TestSparsePruneAndUpdate)
{
  arma::mat realData(3, 5);

  realData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::sp_mat testData(realData);

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::sp_mat> testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);
  alpha = testDTree.PruneAndUpdate(alpha, testData.n_cols, false);

  BOOST_REQUIRE_CLOSE(alpha, numeric_limits<double>::max(), 1e-10);
  BOOST_REQUIRE(testDTree.SubtreeLeaves() == 1);

  double rootError = -log(4.0) - log(7.0) - log(7.0);

  BOOST_REQUIRE_CLOSE(testDTree.LogNegError(), rootError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.SubtreeLeavesLogNegError(), rootError, 1e-10);
  BOOST_REQUIRE(testDTree.Left() == NULL);
  BOOST_REQUIRE(testDTree.Right() == NULL);
}

BOOST_AUTO_TEST_CASE(TestSparseComputeValue)
{
  arma::mat realData(3, 5);

  realData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::vec q1d(3), q2d(3), q3d(3), q4d(3);

  q1d << 4 << 2 << 2;
  q2d << 5 << 0.25 << 6;
  q3d << 5 << 3 << 7;
  q4d << 2 << 3 << 3;

  arma::sp_mat testData(realData);
  arma::sp_vec q1(q1d), q2(q2d), q3(q3d), q4(q4d);

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::sp_mat> testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);

  double d1 = (2.0 / 5.0) / exp(log(4.0) + log(7.0) + log(4.5));
  double d2 = (1.0 / 5.0) / exp(log(4.0) + log(0.5) + log(2.5));
  double d3 = (2.0 / 5.0) / exp(log(4.0) + log(6.5) + log(2.5));

  BOOST_REQUIRE_CLOSE(d1, testDTree.ComputeValue(q1), 1e-10);
  BOOST_REQUIRE_CLOSE(d2, testDTree.ComputeValue(q2), 1e-10);
  BOOST_REQUIRE_CLOSE(d3, testDTree.ComputeValue(q3), 1e-10);
  BOOST_REQUIRE_CLOSE(0.0, testDTree.ComputeValue(q4), 1e-10);

  alpha = testDTree.PruneAndUpdate(alpha, testData.n_cols, false);

  double d = 1.0 / exp(log(4.0) + log(7.0) + log(7.0));

  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q1), 1e-10);
  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q2), 1e-10);
  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q3), 1e-10);
  BOOST_REQUIRE_CLOSE(0.0, testDTree.ComputeValue(q4), 1e-10);
}

/**
 * These are not yet implemented.
 *
BOOST_AUTO_TEST_CASE(TestTagTree)
{
  MatType testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
            << 5 << 0 << 1 << 7 << 1 << arma::endr
            << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<>* testDTree = new DTree<>(&testData);

  delete testDTree;
}

BOOST_AUTO_TEST_CASE(TestFindBucket)
{
  MatType testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
            << 5 << 0 << 1 << 7 << 1 << arma::endr
            << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<>* testDTree = new DTree<>(&testData);

  delete testDTree;
}

// Test functions in dt_utils.hpp

BOOST_AUTO_TEST_CASE(TestTrainer)
{

}

BOOST_AUTO_TEST_CASE(TestPrintVariableImportance)
{

}

BOOST_AUTO_TEST_CASE(TestPrintLeafMembership)
{

}
*/

// Test the copy constructor and the copy operator.
BOOST_AUTO_TEST_CASE(CopyConstructorAndOperatorTest)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  // Construct another DTree for testing the children.
  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::mat> *testDTree = new DTree<arma::mat>(testData);
  testDTree->Grow(testData, oTest, false, 2, 1);

  DTree<arma::mat> testDTree2(*testDTree);
  DTree<arma::mat> testDTree3 = *testDTree;

  double maxVals0 = testDTree->MaxVals()[0];
  double maxVals1 = testDTree->MaxVals()[1];
  double maxVals2 = testDTree->MaxVals()[2];
  double minVals0 = testDTree->MinVals()[0];
  double minVals1 = testDTree->MinVals()[1];
  double minVals2 = testDTree->MinVals()[2];

  double maxValsL0 = testDTree->Left()->MaxVals()[0];
  double maxValsL1 = testDTree->Left()->MaxVals()[1];
  double maxValsL2 = testDTree->Left()->MaxVals()[2];
  double minValsL0 = testDTree->Left()->MinVals()[0];
  double minValsL1 = testDTree->Left()->MinVals()[1];
  double minValsL2 = testDTree->Left()->MinVals()[2];

  double maxValsR0 = testDTree->Right()->MaxVals()[0];
  double maxValsR1 = testDTree->Right()->MaxVals()[1];
  double maxValsR2 = testDTree->Right()->MaxVals()[2];
  double minValsR0 = testDTree->Right()->MinVals()[0];
  double minValsR1 = testDTree->Right()->MinVals()[1];
  double minValsR2 = testDTree->Right()->MinVals()[2];

  // Delete the original tree.
  delete testDTree;

  // Test the data of copied tree (using copy constructor).
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[0], maxVals0);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[0], minVals0);
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[1], maxVals1);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[1], minVals1);
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[2], maxVals2);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[2], minVals2);

  // Test the data of the copied tree (using the copy operator).
  BOOST_REQUIRE_EQUAL(testDTree3.MaxVals()[0], maxVals0);
  BOOST_REQUIRE_EQUAL(testDTree3.MinVals()[0], minVals0);
  BOOST_REQUIRE_EQUAL(testDTree3.MaxVals()[1], maxVals1);
  BOOST_REQUIRE_EQUAL(testDTree3.MinVals()[1], minVals1);
  BOOST_REQUIRE_EQUAL(testDTree3.MaxVals()[2], maxVals2);
  BOOST_REQUIRE_EQUAL(testDTree3.MinVals()[2], minVals2);

  // Test the structure of the tree copied using the copy constructor.
  BOOST_REQUIRE(testDTree2.Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Right()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Right()->Right() == NULL);

  // Test the structure of the tree copied using the copy operator.
  BOOST_REQUIRE(testDTree3.Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree3.Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree3.Right()->Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree3.Right()->Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree3.Right()->Right()->Left() == NULL);
  BOOST_REQUIRE(testDTree3.Right()->Right()->Right() == NULL);

  // Test the data of the tree copied using the copy constructor.
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[0], maxValsL0);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[1], maxValsL1);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[2], maxValsL2);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[0], minValsL0);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[1], minValsL1);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[2], minValsL2);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[0], maxValsR0);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[1], maxValsR1);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[2], maxValsR2);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[0], minValsR0);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[1], minValsR1);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[2], minValsR2);
  BOOST_REQUIRE(testDTree2.SplitDim() == 2);
  BOOST_REQUIRE_CLOSE(testDTree2.SplitValue(), 5.5, 1e-5);
  BOOST_REQUIRE(testDTree2.Right()->SplitDim() == 1);
  BOOST_REQUIRE_CLOSE(testDTree2.Right()->SplitValue(), 0.5, 1e-5);

  // Test the data of the tree copied using the copy operator.
  BOOST_REQUIRE_EQUAL(testDTree3.Left()->MaxVals()[0], maxValsL0);
  BOOST_REQUIRE_EQUAL(testDTree3.Left()->MaxVals()[1], maxValsL1);
  BOOST_REQUIRE_EQUAL(testDTree3.Left()->MaxVals()[2], maxValsL2);
  BOOST_REQUIRE_EQUAL(testDTree3.Left()->MinVals()[0], minValsL0);
  BOOST_REQUIRE_EQUAL(testDTree3.Left()->MinVals()[1], minValsL1);
  BOOST_REQUIRE_EQUAL(testDTree3.Left()->MinVals()[2], minValsL2);
  BOOST_REQUIRE_EQUAL(testDTree3.Right()->MaxVals()[0], maxValsR0);
  BOOST_REQUIRE_EQUAL(testDTree3.Right()->MaxVals()[1], maxValsR1);
  BOOST_REQUIRE_EQUAL(testDTree3.Right()->MaxVals()[2], maxValsR2);
  BOOST_REQUIRE_EQUAL(testDTree3.Right()->MinVals()[0], minValsR0);
  BOOST_REQUIRE_EQUAL(testDTree3.Right()->MinVals()[1], minValsR1);
  BOOST_REQUIRE_EQUAL(testDTree3.Right()->MinVals()[2], minValsR2);
  BOOST_REQUIRE(testDTree3.SplitDim() == 2);
  BOOST_REQUIRE_CLOSE(testDTree3.SplitValue(), 5.5, 1e-5);
  BOOST_REQUIRE(testDTree3.Right()->SplitDim() == 1);
  BOOST_REQUIRE_CLOSE(testDTree3.Right()->SplitValue(), 0.5, 1e-5);
}

// Test the move constructor.
BOOST_AUTO_TEST_CASE(MoveConstructorTest)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  // Construct another DTree for testing the children.
  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::mat> *testDTree = new DTree<arma::mat>(testData);
  testDTree->Grow(testData, oTest, false, 2, 1);

  double maxVals0 = testDTree->MaxVals()[0];
  double maxVals1 = testDTree->MaxVals()[1];
  double maxVals2 = testDTree->MaxVals()[2];
  double minVals0 = testDTree->MinVals()[0];
  double minVals1 = testDTree->MinVals()[1];
  double minVals2 = testDTree->MinVals()[2];

  double maxValsL0 = testDTree->Left()->MaxVals()[0];
  double maxValsL1 = testDTree->Left()->MaxVals()[1];
  double maxValsL2 = testDTree->Left()->MaxVals()[2];
  double minValsL0 = testDTree->Left()->MinVals()[0];
  double minValsL1 = testDTree->Left()->MinVals()[1];
  double minValsL2 = testDTree->Left()->MinVals()[2];

  double maxValsR0 = testDTree->Right()->MaxVals()[0];
  double maxValsR1 = testDTree->Right()->MaxVals()[1];
  double maxValsR2 = testDTree->Right()->MaxVals()[2];
  double minValsR0 = testDTree->Right()->MinVals()[0];
  double minValsR1 = testDTree->Right()->MinVals()[1];
  double minValsR2 = testDTree->Right()->MinVals()[2];

  // Construct a new tree using the move constructor.
  DTree<arma::mat> testDTree2(std::move(*testDTree));

  // Check default values of the original tree.
  BOOST_REQUIRE_EQUAL(testDTree->LogNegError(), -DBL_MAX);
  BOOST_REQUIRE(testDTree->Left() == (DTree<arma::mat>*) NULL);
  BOOST_REQUIRE(testDTree->Right() == (DTree<arma::mat>*) NULL);

  // Delete the original tree.
  delete testDTree;

  // Test the data of the moved tree.
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[0], maxVals0);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[0], minVals0);
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[1], maxVals1);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[1], minVals1);
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[2], maxVals2);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[2], minVals2);

  // Test the structure of the moved tree.
  BOOST_REQUIRE(testDTree2.Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Right()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Right()->Right() == NULL);

  // Test the data of the moved tree.
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[0], maxValsL0);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[1], maxValsL1);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[2], maxValsL2);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[0], minValsL0);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[1], minValsL1);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[2], minValsL2);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[0], maxValsR0);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[1], maxValsR1);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[2], maxValsR2);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[0], minValsR0);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[1], minValsR1);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[2], minValsR2);
  BOOST_REQUIRE(testDTree2.SplitDim() == 2);
  BOOST_REQUIRE_CLOSE(testDTree2.SplitValue(), 5.5, 1e-5);
  BOOST_REQUIRE(testDTree2.Right()->SplitDim() == 1);
  BOOST_REQUIRE_CLOSE(testDTree2.Right()->SplitValue(), 0.5, 1e-5);
}

// Test the move operator.
BOOST_AUTO_TEST_CASE(MoveOperatorTest)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  // Construct another DTree for testing the children.
  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree<arma::mat> *testDTree = new DTree<arma::mat>(testData);
  testDTree->Grow(testData, oTest, false, 2, 1);

  double maxVals0 = testDTree->MaxVals()[0];
  double maxVals1 = testDTree->MaxVals()[1];
  double maxVals2 = testDTree->MaxVals()[2];
  double minVals0 = testDTree->MinVals()[0];
  double minVals1 = testDTree->MinVals()[1];
  double minVals2 = testDTree->MinVals()[2];

  double maxValsL0 = testDTree->Left()->MaxVals()[0];
  double maxValsL1 = testDTree->Left()->MaxVals()[1];
  double maxValsL2 = testDTree->Left()->MaxVals()[2];
  double minValsL0 = testDTree->Left()->MinVals()[0];
  double minValsL1 = testDTree->Left()->MinVals()[1];
  double minValsL2 = testDTree->Left()->MinVals()[2];

  double maxValsR0 = testDTree->Right()->MaxVals()[0];
  double maxValsR1 = testDTree->Right()->MaxVals()[1];
  double maxValsR2 = testDTree->Right()->MaxVals()[2];
  double minValsR0 = testDTree->Right()->MinVals()[0];
  double minValsR1 = testDTree->Right()->MinVals()[1];
  double minValsR2 = testDTree->Right()->MinVals()[2];

  // Construct a new tree using the move constructor.
  DTree<arma::mat> testDTree2 = std::move(*testDTree);

  // Check default values of the original tree.
  BOOST_REQUIRE_EQUAL(testDTree->LogNegError(), -DBL_MAX);
  BOOST_REQUIRE(testDTree->Left() == (DTree<arma::mat>*) NULL);
  BOOST_REQUIRE(testDTree->Right() == (DTree<arma::mat>*) NULL);

  // Delete the original tree.
  delete testDTree;

  // Test the data of the moved tree.
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[0], maxVals0);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[0], minVals0);
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[1], maxVals1);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[1], minVals1);
  BOOST_REQUIRE_EQUAL(testDTree2.MaxVals()[2], maxVals2);
  BOOST_REQUIRE_EQUAL(testDTree2.MinVals()[2], minVals2);

  // Test the structure of the moved tree.
  BOOST_REQUIRE(testDTree2.Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Right()->Left() == NULL);
  BOOST_REQUIRE(testDTree2.Right()->Right()->Right() == NULL);

  // Test the data of moved tree.
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[0], maxValsL0);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[1], maxValsL1);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MaxVals()[2], maxValsL2);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[0], minValsL0);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[1], minValsL1);
  BOOST_REQUIRE_EQUAL(testDTree2.Left()->MinVals()[2], minValsL2);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[0], maxValsR0);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[1], maxValsR1);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MaxVals()[2], maxValsR2);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[0], minValsR0);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[1], minValsR1);
  BOOST_REQUIRE_EQUAL(testDTree2.Right()->MinVals()[2], minValsR2);
  BOOST_REQUIRE(testDTree2.SplitDim() == 2);
  BOOST_REQUIRE_CLOSE(testDTree2.SplitValue(), 5.5, 1e-5);
  BOOST_REQUIRE(testDTree2.Right()->SplitDim() == 1);
  BOOST_REQUIRE_CLOSE(testDTree2.Right()->SplitValue(), 0.5, 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


#include <Eigen/Sparse>
// system includes ------------------------------------------------------------
#include <algorithm>
#include <boost/program_options.hpp>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

#include <cmath>
#include <random>

// own includes ---------------------------------------------------------------
#include "spectral/basis/spectral_basis_dimension_accessor.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"

#include "spectral/rotate_basis.hpp"

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

const int dim = 2;

int main(int argc, char *argv[])
{
  int K;
  double beta;
  bool sorted;
  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("K", po::value<int>(&K)->default_value(10))
      ("sorted", po::value<bool>(&sorted)->default_value(true))
      ("beta", po::value<double>(&beta)->default_value(2));
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 1;
  }

  // ------------------------------------------------------------
  typedef SpectralBasisFactoryKS::basis_type basis_type;
  basis_type trial_basis;
  SpectralBasisFactoryKS::create(trial_basis, K, K, beta, sorted);
  // sort
  typedef typename basis_type::elem_t elem_t;

  // write to disk
  SpectralBasisFactoryKS::write_basis_descriptor(trial_basis, "spectral_basis.desc");
  basis_type test_basis;
  SpectralBasisFactoryKS::create_test(test_basis, K, K, beta, sorted);
  SpectralBasisFactoryKS::write_basis_descriptor(test_basis, "spectral_basis_test.desc");

  // for (int i = 0; i < K; ++i) {
  //   // loop over all elements in basis
  //   for (auto  it = trial_basis.begin(); it < trial_basis.end(); ++it) {

  //   }
  // }

  RotateBasis<basis_type> rotate_basis(trial_basis);

  rotate_basis.init();

  unsigned int L = 100;
  unsigned int N = trial_basis.n_dofs();

  Eigen::VectorXd x(L * N);
  Eigen::VectorXd y(L * N);
  Eigen::VectorXd y2(L * N);

  std::random_device rd;

  // Choose a random mean between 1 and 6
  // std::default_random_engine e1(rd());
  std::default_random_engine e1(1);
  std::uniform_real_distribution<double> uniform_dist(-10, 10);

  for (unsigned int i = 0; i < L * N; i++) {
    x[i] = uniform_dist(e1);
  }

  double dphi = 2;

  rotate_basis.apply(y.data(), x.data(), -dphi, L);
  rotate_basis.apply(y2.data(), y.data(), dphi, L);

  std::cout << "error: " << (y2 - x).norm() << std::endl;

  //  Eigen::VectorXd diff = (y2-x);

  // cout << diff
  //      << endl;

  return 0;
}