/*
 * PseudoMeasurements.hpp
 *
 *  Created on: 02.08.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_PSEUDOMEASUREMENTS_HPP_
#define ESTIMATORS_PSEUDOMEASUREMENTS_HPP_

#include <Eigen/Core>

#include <glog/logging.h>
#include <ceres/ceres.h>

#include <Eigen_Utils.hpp>
#include "BatchEstimator.hpp"

/**
 * Setup a basic pseudo measurement so that other parts of the programm know its  dimensions
 */
#define SETUP_PSEUDO_MEASUREMENT(MEASURE_DIM)  \
	static constexpr int measure_dim=MEASURE_DIM; \
	static constexpr int state_dim=estimator_type::MODEL_TYPE::outer_size; \
	template<typename T>										\
	using MEASURE_TYPE=Eigen::Matrix<T,measure_dim,1>;

/**
 * Setup a pseudo measurement which contains a manifold with different inner size
 */
#define SETUP_PSEUDO_MEASUREMENT_WITH_MANIFOLD(MEASURE_DIM,MANIFOLD_DIM)  \
	SETUP_PSEUDO_MEASUREMENT(MEASURE_DIM)   \
	static constexpr int manifold_dim=MANIFOLD_DIM;   \
	template<typename T>										\
	using MANIFOLD_TYPE=Eigen::Matrix<T,manifold_dim,1>;

/**
 * Adds the necesarry function so that the UKF accepts the knowledge (for knowledge where alignment is not required)
 */
#define ADD_UKF_FUNCTION_PASSER  \
template<typename T>		\
MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state,const ALIGNMENT_TYPE<T> &alignment, void * prior) const {		\
			return this->operator()(state,prior);     \
		}

namespace zavi
::estimator::pseudo_measurement {

	/**
	 * Struct to contain relevant data for a function as Prior Knowledge
	 */
	template<typename ukf_type>
	struct FunctionMeasurement {
		FunctionMeasurement(const zavi::eigen_util::FuncCaller &function,std::shared_ptr<ukf_type> estimator ):function(function),ukf(estimator) {}
		zavi::eigen_util::FuncCaller function;
		std::shared_ptr<ukf_type> ukf;
	};
	/**
	 * pseudo measurement to return the position
	 * @param state  the current estimated state
	 * @param prior a maybe usefull prior
	 * @return an expected position at state
	 */
	inline Eigen::Matrix<double, 3, 1> position_measurement(const Eigen::Matrix<double,9,1> &state, void *prior) {
		return state.block(0,0,3,1);
	}

	/**
	 * Functor to calculate the shortest distance between a function and a point
	 */
	template <typename functor>
	class DistanceFunctor {
	public:
		~DistanceFunctor() {}
		DistanceFunctor(functor & function):function(function),x(0),y(0),z(0) {};
		template<typename T>
		bool operator()(const T * const time,T* residual) const {
			T func[3];
			function(time,(T* )func);
			residual[0]=pow(x-func[0],2)+pow(y-func[1],2)+pow(z-func[2],2);

			return true;
		}

	public:
		/**
		 * Set the point to which to calculate the distance
		 * @param point  point with x y z coordinates
		 */
		void setPoint(const Eigen::Matrix<double,3,1> & point) {
			x=point(0,0);
			y=point(1,0);
			z=point(2,0);
		}

		functor function;     // evaluation function
	private:

		double x,y,z;// point coordinates
	};

	/**
	 * Struct to contain relevant data for a function as Prior Knowledge
	 */
	template <typename functor,typename cov_type,typename ukf_type>
	struct DistanceMeasurement {
		DistanceMeasurement(functor &function,const cov_type &cov, std::shared_ptr<ukf_type> estimator ):function(function),cov(cov),ukf(estimator) {}
		DistanceFunctor<functor> function;
		cov_type cov;
		std::shared_ptr<ukf_type> ukf;
	};
	/**
	 * pseudo measurement to return the distance to prior function
	 * @param state  the current estimated state
	 * @param prior a maybe usefull prior
	 * @return an expected distance to function at state hard constriaint so it is always zero
	 */
	template <typename functor,typename ukf_type>
	Eigen::Matrix<double, 3, 1> distance_measurement(const Eigen::Matrix<double,9,1> &state, void * prior) {
		DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> *>(prior);
		container->function.function.give_real=false;     // tell function that the solver is active
		// The variable to solve for with its initial value.
		static double initial_x = 0;
		double x = initial_x;
		container->function.setPoint(state.block(0,0,3,1));
		// Build the problem.
		// Set up the only cost function (also known as residual). This uses
		// auto-differentiation to obtain the derivative (jacobian).
		ceres::CostFunction* cost_function =
		new ceres::AutoDiffCostFunction<DistanceFunctor<functor>, 1, 1>(new DistanceFunctor<functor>(container->function));
		ceres::Problem problem;

		problem.AddResidualBlock(cost_function, NULL, &x);

		// Run the solver!
		ceres:: Solver::Options options;
		options.linear_solver_type = ceres::DENSE_QR;
		options.minimizer_progress_to_stdout = false;
		options.max_num_line_search_step_size_iterations=10;
		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		//std::cout << summary.BriefReport() << std::endl;
		initial_x=x;
		double result[3];
		container->function.function(&x,(double *)result);
		Eigen::Matrix<double,3,1> returner;
		returner(0,0)=result[0]-state(0,0);
		returner(1,0)=result[1]-state(1,0);
		returner(2,0)=result[2]-state(2,0);

		container->function.function.give_real=true;// tell function that the solver is finished
		return returner;

	}
	/**
	 * pseudo measurement to return the distance to prior function
	 * @param state  the current estimated state
	 * @param prior a maybe usefull prior
	 * @return an expected distance to function at state hard constriaint so it is always zero
	 */
	template <typename functor,typename ukf_type>
	Eigen::Matrix<double, 1, 1> distance_measurement_single(const Eigen::Matrix<double,9,1> &state, void * prior) {
		DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> *>(prior);
		container->function.function.give_real=false;     // tell function that the solver is active
		// The variable to solve for with its initial value.
		static double initial_x = 0;
		double x = initial_x;
		container->function.setPoint(state.block(0,0,3,1));
		// Build the problem.
		// Set up the only cost function (also known as residual). This uses
		// auto-differentiation to obtain the derivative (jacobian).
		ceres::CostFunction* cost_function =
		new ceres::AutoDiffCostFunction<DistanceFunctor<functor>, 1, 1>(new DistanceFunctor<functor>(container->function));
		ceres::Problem problem;

		problem.AddResidualBlock(cost_function, NULL, &x);

		// Run the solver!
		ceres:: Solver::Options options;
		options.linear_solver_type = ceres::DENSE_QR;
		options.minimizer_progress_to_stdout = false;
		options.max_num_line_search_step_size_iterations=10;
		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		//std::cout << summary.BriefReport() << std::endl;
		initial_x=x;
		double result[1];
		container->function(&x,(double *)result);
		Eigen::Matrix<double,1,1> returner;
		returner(0,0)=result[0];

		container->function.function.give_real=true;// tell function that the solver is finished
		return returner;

	}

	/**
	 *  a pseudo distance measurement with a given function as ground truth
	 * @param plug  the imu plugin
	 * @param estimator  the function measurement struct
	 * @param time  time current time stamp
	 */
	template<typename functor,typename ukf_type>
	void pseudoDistanceFunctionMeasurement(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> *>(estimator);
		//plugin::IMU_Plugin *imu = static_cast<plugin::IMU_Plugin*>(plug);
		Eigen::Matrix<double,3,1> zeros=Eigen::Matrix<double,3,1>::Zero();
		container->ukf->measurementStep(zeros,time-last_time,distance_measurement<functor>,container->cov,container);
		last_time=time;
	}

	/**
	 *  a pseudo distance measurement with a given function as ground truth
	 * @param plug  the imu plugin
	 * @param estimator  the function measurement struct
	 * @param time  time current time stamp
	 */
	template<typename functor,typename ukf_type>
	void pseudoDistanceFunctionMeasurementSingle(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> *>(estimator);
		//plugin::IMU_Plugin *imu = static_cast<plugin::IMU_Plugin*>(plug);
		Eigen::Matrix<double,1,1> zeros=Eigen::Matrix<double,1,1>::Zero();
		container->ukf->measurementStep(zeros,time-last_time,distance_measurement_single<functor>,container->cov,container);
		last_time=time;
	}

	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 * The Velocity in x must equal the norm of the velocity
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type>
	struct DrivingForwardConstrain {
		SETUP_PSEUDO_MEASUREMENT(3)
		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state,void * prior) const {
			estimator_type * estimator=static_cast<estimator_type*>(prior);
			MEASURE_TYPE<T> body_velocity=estimator->getBoxModel().getOrientation(state).transpose()*estimator->getBoxModel().getVelocity(state)-getTarget(state,prior);
			return body_velocity;

		}
		ADD_UKF_FUNCTION_PASSER
		template<typename T>
		MEASURE_TYPE<T> getTarget(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			estimator_type * estimator=static_cast<estimator_type*>(prior);
			MEASURE_TYPE<T> returner=MEASURE_TYPE<T>::Zero();
			T norm=estimator->getBoxModel().getVelocity(state).norm();
			if(norm !=0.)     // ceres jet infinity fix
			returner(0,0)=norm;
			return returner;

		}

		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.1;
		}
	};

	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type>
	struct NoSideUpDriftConstrain {
		SETUP_PSEUDO_MEASUREMENT(2)
		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			return ( (estimator_type::MODEL_TYPE::getOrientation(state).transpose()*estimator_type::MODEL_TYPE::getVelocity(state))) .template block<2,1>(1,0);

		}
		ADD_UKF_FUNCTION_PASSER
		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.1;
		}
	};

	template<typename estimator_type>
	struct OnlyRollConstrain {
		static constexpr int measure_dim=2;     // measurement dimension
		static constexpr int state_dim=estimator_type::MODEL_TYPE::outer_size;
		template<typename T>
		using MEASURE_TYPE=Eigen::Matrix<T,measure_dim,1>;
		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			return eigen_util::inverseEulerRodriguez<T>(estimator_type::MODEL_TYPE::getOrientation(state)).template block<2,1>(1,0);

		}

		ADD_UKF_FUNCTION_PASSER
		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.01;
		}
	};

	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type, typename functor>
	struct FunctionOrientationMeasurement {

		SETUP_PSEUDO_MEASUREMENT_WITH_MANIFOLD(3,3)

		eigen_util::FuncCaller function;
		FunctionOrientationMeasurement<estimator_type,functor>() :function(functor()) {

		}

		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			estimator_type* estimator= static_cast<estimator_type *>(prior);
			return eigen_util::boxMinusOrientation(estimator->getBoxModel().getOrientation(state),eigen_util::orientationFromFunctor(state(estimator->getBoxModel().lambda_start,0),function,1e-5));

		}
		ADD_UKF_FUNCTION_PASSER

		template<typename T>
		static MANIFOLD_TYPE<T> boxminus(const MEASURE_TYPE<T> &a , const MEASURE_TYPE<T> &b) {
			return eigen_util::wrapAngles(b-a);
		}
		template<typename T>
		static MEASURE_TYPE<T> boxplus(const MEASURE_TYPE<T> &state , const MANIFOLD_TYPE<T> &delta) {
			return eigen_util::wrapAngles(state+delta);
		}

		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.3;
		}
	};
	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type, typename functor>
	struct FunctionOrientationChangeMeasurement {
		SETUP_PSEUDO_MEASUREMENT(3)

		eigen_util::FuncCaller function;
		FunctionOrientationChangeMeasurement<estimator_type,functor>() :function(functor()) {

		}

		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			estimator_type* estimator= static_cast<estimator_type *>(prior);
			Eigen::Matrix<T,3,1> current_axis=estimator->getBoxModel().getOrientationChange(state);
			Eigen::Matrix<T,3,1> target_axis=eigen_util::boxMinusOrientation(eigen_util::orientationFromFunctor(state(estimator->getBoxModel().lambda_start,0)-1e-5,function,1e-5),eigen_util::orientationFromFunctor(state(estimator->getBoxModel().lambda_start,0)+1e-5,function,1e-5))/2e-5;
			//zavi::plot::liveStateDraw<6>(target_axis,"target_axis");
			/*if(current_axis.norm()!=0.)
			current_axis.normalize();*/
			/*if(target_axis.norm() !=0.)
			target_axis.normalize();*/
			return current_axis-target_axis*estimator->getBoxModel().getLambdaVelocity(state);

		}
		ADD_UKF_FUNCTION_PASSER

		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.5;
		}
	};

	/**
	 * pseudo ukf measurement
	 * @template measure_struct a struct with () operator to define the measure function
	 * @template ukf_type the type of the estimator
	 * @template measure_dim the size of the measurement
	 * @template measurement a measurement object containing the wanted result
	 * @template cov the covariance of the pseudo measurement
	 */
	template<template<typename,typename ...> class measure_struct,typename ukf_type,typename ... meas_args>
	void ukfMeasurement(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		static measure_struct<ukf_type,meas_args ...> measure_function=measure_struct<ukf_type,meas_args ...>();
		ukf_type *ukf=static_cast<ukf_type*>(estimator);
		ukf->measurementStep(measure_function.template getTarget<double>(),time-last_time,measure_function,measure_function.getNoise(),estimator);     // No Prior is given since the circle_prior does not need an object
		last_time=time;
	}
	/**
	 * pseudo ukf measurement for manifolds
	 * @template measure_struct a struct with () operator to define the measure function
	 * @template ukf_type the type of the estimator
	 * @template measure_dim the size of the measurement
	 * @template measurement a measurement object containing the wanted result
	 * @template cov the covariance of the pseudo measurement
	 */
	template<template<typename,typename ...> class measure_struct,typename ukf_type,typename ... meas_args>
	void ukfManifoldMeasurement(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		typedef measure_struct<ukf_type,meas_args ...> MEASURE_STRUCT;
		static MEASURE_STRUCT measure_function=MEASURE_STRUCT();
		ukf_type *ukf=static_cast<ukf_type*>(estimator);
		//measure_function.template boxplus<double>(Eigen::Vector3d(), Eigen::Vector3d());
		ukf->measurementStepManifold(measure_function.template getTarget<double>(),time-last_time,measure_function,measure_function.getNoise(),MEASURE_STRUCT::boxplus,MEASURE_STRUCT::boxminus,estimator);
		last_time=time;
	}

	/**
	 * A cost function comparing the 1 state with the required circle trajectory
	 */
	template<template <typename T> class STATE_TYPE_T,typename MEASUREMENT_FUNCTION, int measure_dim >
	struct MeasurementDiffCostFunction {
		template<typename T>
		using MEASUREMENT_TYPE_T= Eigen::Matrix<T,measure_dim,1>;
		static inline MEASUREMENT_FUNCTION measurement_function=MEASUREMENT_FUNCTION();
		MeasurementDiffCostFunction(void * prior):prior(prior) {}

		template<typename T>
		bool operator()(const T * const a,const T* const alignment, T* result)const {
			static Eigen::Matrix<double,measure_dim,measure_dim> stiffnes=measurement_function.getNoise().diagonal().cwiseInverse().cwiseSqrt().asDiagonal();
			static Eigen::Matrix<double,measure_dim,1> measurement=measurement_function.template getTarget<double>();
			(Eigen::Map<MEASUREMENT_TYPE_T<T> > (result))=stiffnes*(measurement_function.template operator()<T>(Eigen::Map<const STATE_TYPE_T<T> >(a),Eigen::Map<const ALIGNMENT_TYPE<T> >(alignment),prior)-measurement);
			return true;
		}

		template<typename T>
		bool operator()(const T * const a, T* result)const {
			static Eigen::Matrix<double,measure_dim,measure_dim> stiffnes=measurement_function.getNoise().diagonal().cwiseInverse().cwiseSqrt().asDiagonal();
			static Eigen::Matrix<double,measure_dim,1> measurement=measurement_function.template getTarget<double>();
			(Eigen::Map<MEASUREMENT_TYPE_T<T> > (result))=stiffnes*(measurement_function.template operator()<T>(Eigen::Map<const STATE_TYPE_T<T> >(a),prior)-measurement);
			return true;
		}
		void * prior;
	};
	/**
	 * Adds the circular constraint to the ceres problem
	 * @param problem  ceres problem
	 * @param states list of all states
	 * @param inputs list of all inputs
	 * @param time_diffs list of all time diffs
	 */

	template<template<typename> class MEASUREMENT_FUNCTION, typename ESTIMATOR_TYPE,bool with_alignment=false>
	struct MeasurementConstraint: public ESTIMATOR_TYPE::BatchConstrain {
		static constexpr int measure_dim=MEASUREMENT_FUNCTION<ESTIMATOR_TYPE>::measure_dim;
		virtual ~MeasurementConstraint() {};
		typedef MeasurementDiffCostFunction<ESTIMATOR_TYPE::template STATE_TYPE_T,MEASUREMENT_FUNCTION<ESTIMATOR_TYPE>,measure_dim > COST_TYPE;
		virtual void operator()(ceres::Problem &problem,double * states,double * alignment, int state_size,void* data,std::vector<double> &time_diffs, BatchEstimator<typename ESTIMATOR_TYPE::MODEL_TYPE> * estimator) {
			if(with_alignment) {
				ceres::CostFunction* cost_function=new ceres::AutoDiffCostFunction<COST_TYPE,measure_dim,ESTIMATOR_TYPE::MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(new COST_TYPE(estimator));
				for(int i=0; i < state_size;i++) {
					problem.AddResidualBlock(cost_function,NULL,&states[i*ESTIMATOR_TYPE::MODEL_TYPE::outer_size], alignment);
				}
			}
			else {
				ceres::CostFunction* cost_function=new ceres::AutoDiffCostFunction<COST_TYPE,measure_dim,ESTIMATOR_TYPE::MODEL_TYPE::outer_size>(new COST_TYPE(estimator));
				for(int i=0; i < state_size;i++) {
					problem.AddResidualBlock(cost_function,NULL,&states[i*ESTIMATOR_TYPE::MODEL_TYPE::outer_size]);
				}
			}
		}
	};

}
//zavi::estimator::pseudo_measurement

#endif /* ESTIMATORS_PSEUDOMEASUREMENTS_HPP_ */


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include <snark/math/angle.h>
#include "coordinates.h"

namespace snark { namespace spherical {

const double coordinates::epsilon = 1e-6;

bool coordinates::operator==( const coordinates& rhs ) const
{ return is_near( rhs, epsilon ); }

bool coordinates::is_near( const coordinates& c, double epsilon ) const
{
    double dist = std::abs( longitude - c.longitude );
    return comma::math::equal( latitude, c.latitude, epsilon ) && comma::math::equal( 0, std::min( dist, 2 * M_PI - dist ), epsilon );
}

/// limits longitude to [-PI, PI)
static double limit( const double longitude )
{
    // todo: this is just a quick patch, not a real fix; what if longitude equals e.g. -3*M_PI?
    // todo: seriously quick and dirty, to fix anti merridian crossing
    return longitude < -M_PI ? longitude + 2 * M_PI : longitude >= M_PI ? longitude - 2 * M_PI : longitude;
}

coordinates::coordinates( const double latitude, const double longitude ) : latitude( latitude ), longitude( limit( longitude ) )
{
}

coordinates::coordinates( const snark::bearing_elevation& be ) : latitude( be.elevation() ), longitude( limit( be.bearing() ) )
{
}

coordinates::coordinates( const Eigen::Vector3d& xyz )
{
    snark::range_bearing_elevation rbe( xyz );
    latitude = rbe.elevation();
    longitude = limit( rbe.bearing() );
}

Eigen::Vector3d to_navigation_frame( const coordinates& c, const Eigen::Vector3d& v )
{
    const Eigen::Matrix3d& r1 = Eigen::AngleAxis< double >( -c.longitude, Eigen::Vector3d( 0, 0, 1 ) ).toRotationMatrix();
    const Eigen::Matrix3d& r2 = Eigen::AngleAxis< double >( c.latitude + M_PI / 2, Eigen::Vector3d( 0, 1, 0 ) ).toRotationMatrix();
    return r2 * r1 * v;
}

coordinates& coordinates::operator+=( const coordinates& rhs )
{
    double d = latitude + rhs.latitude;
    static const double epsilon = 0.00005;
    if( comma::math::equal( d, M_PI / 2, epsilon ) ) { d = M_PI / 2; }
    else if( comma::math::equal( d, -M_PI / 2, epsilon ) ) { d = -M_PI / 2; }
    else if( d > M_PI / 2 || d < -M_PI / 2 ) { COMMA_THROW( comma::exception, "adding " << ( latitude * 180 / M_PI ) << " and " << ( rhs.latitude * 180 / M_PI ) << " gives invalid latitude of " << ( d * 180 / M_PI ) << " degress" ); }
    latitude = d;
    longitude += rhs.longitude;
    while( longitude < -M_PI ) { longitude += 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    while( this->longitude >= M_PI ) { longitude -= 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    return *this;
}

coordinates::operator std::string() const
{
    const std::pair< double, double >& c = to_degrees();
    return (boost::format("%.2f") % c.first ).str() + "," + (boost::format("%.2f") % c.second).str();
}

bool coordinates::is_near( const Eigen::Vector3d& c, double _epsilon ) const
{
    return ( to_cartesian() - c ).lpNorm<Eigen::Infinity>() < _epsilon;
}

coordinates coordinates::from_degrees(double latitude, double longitude)
{
    return coordinates( snark::math::radians( snark::math::degrees( latitude ) ).value,
                        snark::math::radians( snark::math::degrees( longitude ) ).value );
}

std::pair< double, double > coordinates::to_degrees() const
{
    return std::make_pair( snark::math::degrees( snark::math::radians( latitude ) ).value
                         , snark::math::degrees( snark::math::radians( longitude ) ).value );
}

} } // namespace snark { namespace spherical {


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_BLOCK_RIJNDAEL_HPP
#define CRYPTO3_BLOCK_RIJNDAEL_HPP

#include <boost/range/adaptor/sliced.hpp>

#include <nil/crypto3/block/detail/block_stream_processor.hpp>
#include <nil/crypto3/block/detail/cipher_modes.hpp>

#include <nil/crypto3/block/detail/rijndael/rijndael_policy.hpp>
#include <nil/crypto3/block/detail/rijndael/rijndael_impl.hpp>

#if defined(CRYPTO3_HAS_RIJNDAEL_NI)

#include <nil/crypto3/block/detail/rijndael/rijndael_ni_impl.hpp>

#elif defined(CRYPTO3_HAS_RIJNDAEL_SSSE3) || BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_SSSE3_VERSION

#include <nil/crypto3/block/detail/rijndael/rijndael_ssse3_impl.hpp>

#elif defined(CRYPTO3_HAS_RIJNDAEL_ARMV8)

#include <nil/crypto3/block/detail/rijndael/rijndael_armv8_impl.hpp>

#elif defined(CRYPTO3_HAS_RIJNDAEL_POWER8)

#include <nil/crypto3/block/detail/rijndael/rijndael_power8_impl.hpp>

#endif

#include <nil/crypto3/block/detail/utilities/cpuid/cpuid.hpp>

namespace nil {
    namespace crypto3 {
        namespace block {

            /*!
             * @brief Rijndael. AES competition winner.
             *
             * @ingroup block
             *
             * Generic Rijndael cipher implementation. Contains AES-standardized
             * cipher modifications with timing-attack and cache-line leaking
             * attack preventing mechanisms. Optimized for particular architecture
             * used.
             * AES-standartized version comes in three variants, AES-128, AES-192,
             * and AES-256.
             *
             * The standard 128-bit block cipher. Many modern platforms offer hardware
             * acceleration. However, on platforms without hardware support, AES
             * implementations typically are vulnerable to side channel attacks. For x86
             * systems with SSSE3 but without AES-NI, crypto3 has an implementation which avoids
             * known side channels.
             *
             * This implementation is intended to be based on table lookups which
             * are known to be vulnerable to timing and cache based side channel
             * attacks. Some countermeasures are used which may be helpful in some
             * situations:
             *
             * - Only a single 256-word T-table is used, with rotations applied.
             *   Most implementations use 4 T-tables which leaks much more
             *   information via cache usage.
             *
             * - The TE and TD tables are computed at runtime to avoid flush+reload
             *   attacks using clflush. As different processes will not share the
             *   same underlying table data, an attacker can't manipulate another
             *   processes cache lines via their shared reference to the library
             *   read only segment.
             *
             * - Each cache line of the lookup tables is accessed at the beginning
             *   of each call to encrypt or decrypt. (See the Z variable below)
             *
             * If available SSSE3 or AES-NI are used instead of this version, as both
             * are faster and immune to side channel attacks.
             *
             * Some AES cache timing papers for reference:
             *
             * [Software mitigations to hedge AES against cache-based software side channel
             * vulnerabilities](https://eprint.iacr.org/2006/052.pdf)
             *
             * [Cache Games - Bringing Access-Based Cache Attacks on AES to
             * Practice](http://www.ieee-security.org/TC/SP2011/PAPERS/2011/paper031.pdf)
             *
             * [Cache-Collision Timing Attacks Against AES. Bonneau,
             * Mironov](http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.88.4753)
             *
             * @tparam KeyBits Key length used in bits. Available values are: 128, 192, 256
             * @tparam BlockBits Block length used in bits. Available values are: 128, 192, 256
             */
            template<std::size_t KeyBits, std::size_t BlockBits>
            class rijndael {

                BOOST_STATIC_ASSERT(KeyBits >= 128 && KeyBits <= 256 && KeyBits % 32 == 0);
                BOOST_STATIC_ASSERT(BlockBits >= 128 && BlockBits <= 256 && BlockBits % 32 == 0);

                constexpr static const std::size_t version = KeyBits;
                typedef detail::rijndael_policy<KeyBits, BlockBits> policy_type;

                typedef
                    typename std::conditional<BlockBits == 128 && (KeyBits == 128 || KeyBits == 192 || KeyBits == 256),
#if defined(CRYPTO3_HAS_RIJNDAEL_NI)
                                              detail::rijndael_ni_impl<KeyBits, BlockBits, policy_type>,
#elif defined(CRYPTO3_HAS_RIJNDAEL_SSSE3) || BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_SSSE3_VERSION
                                              detail::rijndael_ssse3_impl<KeyBits, BlockBits, policy_type>,
#elif defined(CRYPTO3_HAS_RIJNDAEL_ARMV8)
                                              detail::rijndael_armv8_impl<KeyBits, BlockBits, policy_type>,
#elif defined(CRYPTO3_HAS_RIJNDAEL_POWER8)
                                              detail::rijndael_power8_impl<KeyBits, BlockBits, policy_type>,
#else
                                              detail::rijndael_impl<KeyBits, BlockBits, policy_type>,
#endif
                                              detail::rijndael_impl<KeyBits, BlockBits, policy_type>>::type impl_type;

                constexpr static const std::size_t key_schedule_words = policy_type::key_schedule_words;
                constexpr static const std::size_t key_schedule_bytes = policy_type::key_schedule_bytes;
                typedef typename policy_type::key_schedule_type key_schedule_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                constexpr static const std::size_t word_bytes = policy_type::word_bytes;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t key_bits = policy_type::key_bits;
                constexpr static const std::size_t key_words = policy_type::key_words;
                //                typedef typename policy_type::key_schedule_word_type key_schedule_word_type;
                typedef typename policy_type::key_type key_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::uint8_t rounds = policy_type::rounds;
                typedef typename policy_type::round_constants_type round_constants_type;

                template<class Mode, typename StateAccumulator, std::size_t ValueBits>
                struct stream_processor {
                    struct params_type {
                        constexpr static const std::size_t value_bits = ValueBits;
                        constexpr static const std::size_t length_bits = policy_type::word_bits * 2;
                    };

                    typedef block_stream_processor<Mode, StateAccumulator, params_type> type;
                };

                typedef typename stream_endian::little_octet_big_bit endian_type;

                rijndael(const key_type &key) : encryption_key({0}), decryption_key({0}) {
                    impl_type::schedule_key(key, encryption_key, decryption_key);
                }

                virtual ~rijndael() {
                    encryption_key.fill(0);
                    decryption_key.fill(0);
                }

                inline block_type encrypt(const block_type &plaintext) const {
                    return impl_type::encrypt_block(plaintext, encryption_key);
                }

                inline block_type decrypt(const block_type &plaintext) const {
                    return impl_type::decrypt_block(plaintext, decryption_key);
                }

            protected:
                key_schedule_type encryption_key, decryption_key;
            };
        }    // namespace block
    }        // namespace crypto3
}    // namespace nil

#endif


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program is distributed in the hope that it will be useful,            *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of             *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

#include "aslam/calibration/algorithms/marginalize.h"

#include <cmath>

#include <Eigen/Dense>

#include <aslam/backend/CompressedColumnMatrix.hpp>

#include "aslam/calibration/exceptions/InvalidOperationException.h"
#include "aslam/calibration/exceptions/OutOfBoundException.h"

namespace aslam {
namespace calibration {

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

double colNorm(cholmod_sparse* A, size_t j) {
    if (j >= A->ncol)
        throw OutOfBoundException<size_t>(j, "colNorm(): index must be lower than the number of columns", __FILE__,
                                          __LINE__);
    const std::ptrdiff_t* col_ptr = reinterpret_cast<const std::ptrdiff_t*>(A->p);
    const double* values = reinterpret_cast<const double*>(A->x);
    const std::ptrdiff_t p = col_ptr[j];
    const std::ptrdiff_t numElements = col_ptr[j + 1] - p;
    double norm = 0;
    for (std::ptrdiff_t i = 0; i < numElements; ++i) norm += values[p + i] * values[p + i];
    return std::sqrt(norm);
}

Eigen::MatrixXd marginalJacobian(cholmod_sparse* J_x, cholmod_sparse* J_thetat, cholmod_common* cholmod) {
    // compute the QR factorization of J_x
    SuiteSparseQR_factorization<double>* QR =
        SuiteSparseQR_factorize<double>(SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, J_x, cholmod);
    if (QR == NULL)
        throw InvalidOperationException(
            "marginalJacobian(): "
            "SuiteSparseQR_factorize failed");

    // compute the Jacobian of the reduced system
    cholmod_sparse* J_thetatQFull = SuiteSparseQR_qmult<double>(SPQR_XQ, QR, J_thetat, cholmod);
    if (J_thetatQFull == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "SuiteSparseQR_qmult failed");
    }
    std::ptrdiff_t* colIndices = new std::ptrdiff_t[J_x->ncol];
    for (size_t i = 0; i < J_x->ncol; ++i) colIndices[i] = i;
    cholmod_sparse* J_thetatQ = cholmod_l_submatrix(J_thetatQFull, NULL, -1, colIndices, J_x->ncol, 1, 0, cholmod);
    delete[] colIndices;
    if (J_thetatQ == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_submatrix failed");
    }
    cholmod_sparse* J_thetat2 = cholmod_l_aat(J_thetat, NULL, 0, 1, cholmod);
    if (J_thetat2 == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        cholmod_l_free_sparse(&J_thetatQ, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_aat failed");
    }
    cholmod_sparse* J_thetatQ2 = cholmod_l_aat(J_thetatQ, NULL, 0, 1, cholmod);
    if (J_thetatQ2 == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        cholmod_l_free_sparse(&J_thetatQ, cholmod);
        cholmod_l_free_sparse(&J_thetat2, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_aat failed");
    }
    double alpha[2];
    alpha[0] = 1;
    double beta[2];
    beta[0] = -1;
    cholmod_sparse* Omega = cholmod_l_add(J_thetat2, J_thetatQ2, alpha, beta, 1, 0, cholmod);
    if (Omega == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        cholmod_l_free_sparse(&J_thetatQ, cholmod);
        cholmod_l_free_sparse(&J_thetat2, cholmod);
        cholmod_l_free_sparse(&J_thetatQ2, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_add failed");
    }
    aslam::backend::CompressedColumnMatrix<std::ptrdiff_t> OmegaCCM;
    OmegaCCM.fromCholmodSparse(Omega);
    Eigen::MatrixXd OmegaDense(Omega->nrow, Omega->ncol);
    OmegaCCM.toDenseInto(OmegaDense);

    // clean allocated memory
    SuiteSparseQR_free(&QR, cholmod);
    cholmod_l_free_sparse(&J_thetatQFull, cholmod);
    cholmod_l_free_sparse(&J_thetatQ, cholmod);
    cholmod_l_free_sparse(&J_thetat2, cholmod);
    cholmod_l_free_sparse(&J_thetatQ2, cholmod);
    cholmod_l_free_sparse(&Omega, cholmod);

    return OmegaDense;
}

double marginalize(const aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>& Jt, size_t j, Eigen::MatrixXd& NS,
                   Eigen::MatrixXd& CS, Eigen::MatrixXd& Sigma, Eigen::MatrixXd& SigmaP, Eigen::MatrixXd& Omega,
                   double normTol, double epsTol) {
    // init cholmod
    cholmod_common cholmod;
    cholmod_l_start(&cholmod);

    // convert to cholmod_sparse
    cholmod_sparse JtCs;
    const_cast<aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>&>(Jt).getView(&JtCs);
    cholmod_sparse* J = cholmod_l_transpose(&JtCs, 1, &cholmod);
    if (J == NULL) {
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_transpose failed");
    }

    // extract the part corresponding to the state/landmarks/...
    std::ptrdiff_t* colIndices = new std::ptrdiff_t[j];
    for (size_t i = 0; i < j; ++i) colIndices[i] = i;
    cholmod_sparse* J_x = cholmod_l_submatrix(J, NULL, -1, colIndices, j, 1, 0, &cholmod);
    delete[] colIndices;
    if (J_x == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_submatrix failed");
    }

    // extract the part corresponding to the calibration parameters
    colIndices = new std::ptrdiff_t[J->ncol - j];
    for (size_t i = j; i < J->ncol; ++i) colIndices[i - j] = i;
    cholmod_sparse* J_theta = cholmod_l_submatrix(J, NULL, -1, colIndices, J->ncol - j, 1, 0, &cholmod);
    delete[] colIndices;
    if (J_theta == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_submatrix failed");
    }
    cholmod_sparse* J_thetat = cholmod_l_transpose(J_theta, 1, &cholmod);
    if (J_thetat == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_transpose failed");
    }

    // compute the marginal Jacobian
    try {
        Omega = marginalJacobian(J_x, J_thetat, &cholmod);
    } catch (const InvalidOperationException& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }

    // scale J_x
    cholmod_dense* G_x = cholmod_l_allocate_dense(J_x->ncol, 1, J_x->ncol, CHOLMOD_REAL, &cholmod);
    if (G_x == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_allocate_dense failed");
    }
    try {
        double* values = reinterpret_cast<double*>(G_x->x);
        for (size_t j = 0; j < J_x->ncol; ++j) {
            const double normCol = colNorm(J_x, j);
            if (normCol < normTol)
                values[j] = 0.0;
            else
                values[j] = 1.0 / normCol;
        }
    } catch (const OutOfBoundException<size_t>& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_x, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }
    if (cholmod_l_scale(G_x, CHOLMOD_COL, J_x, &cholmod) == 0) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_x, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_scale failed");
    }
    cholmod_l_free_dense(&G_x, &cholmod);

    // scale J_thetat
    cholmod_dense* G_theta = cholmod_l_allocate_dense(J_theta->ncol, 1, J_theta->ncol, CHOLMOD_REAL, &cholmod);
    if (G_theta == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_allocate_dense failed");
    }
    try {
        double* values = reinterpret_cast<double*>(G_theta->x);
        for (size_t j = 0; j < J_theta->ncol; ++j) {
            const double normCol = colNorm(J_theta, j);
            if (normCol < normTol)
                values[j] = 0.0;
            else
                values[j] = 1.0 / normCol;
        }
    } catch (const OutOfBoundException<size_t>& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_theta, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }
    if (cholmod_l_scale(G_theta, CHOLMOD_ROW, J_thetat, &cholmod) == 0) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_theta, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_scale failed");
    }
    cholmod_l_free_dense(&G_theta, &cholmod);

    // compute the scaled marginal Jacobian
    Eigen::MatrixXd OmegaScaled;
    try {
        OmegaScaled = marginalJacobian(J_x, J_thetat, &cholmod);
    } catch (const InvalidOperationException& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }

    // clean cholmod
    cholmod_l_free_sparse(&J, &cholmod);
    cholmod_l_free_sparse(&J_x, &cholmod);
    cholmod_l_free_sparse(&J_theta, &cholmod);
    cholmod_l_free_sparse(&J_thetat, &cholmod);
    cholmod_l_finish(&cholmod);

    // compute the thin SVD of OmegaScaled
    const Eigen::JacobiSVD<Eigen::MatrixXd> svdScaled(OmegaScaled, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // compute the numerical rank
    size_t nrank = OmegaScaled.cols();
    const Eigen::VectorXd& SScaled = svdScaled.singularValues();
    const double tol = OmegaScaled.rows() * SScaled(0) * epsTol;
    for (std::ptrdiff_t i = OmegaScaled.cols() - 1; i > 0; --i) {
        if (SScaled(i) > tol)
            break;
        else
            nrank--;
    }

    // compute the thin SVD of Omega
    const Eigen::JacobiSVD<Eigen::MatrixXd> svd(Omega, Eigen::ComputeThinU | Eigen::ComputeThinV);
    const Eigen::MatrixXd& V = svd.matrixV();

    // compute the numerical column space
    CS = V.block(0, 0, V.rows(), nrank);

    // compute the numerical null space
    NS = V.block(0, nrank, V.rows(), Omega.cols() - nrank);

    // compute the projected covariance matrix
    Eigen::MatrixXd invS(Eigen::MatrixXd::Zero(Omega.cols(), Omega.cols()));
    SigmaP = Eigen::MatrixXd::Zero(nrank, nrank);
    double svLogSum = 0;
    const Eigen::VectorXd& S = svd.singularValues();
    for (size_t i = 0; i < nrank; ++i) {
        SigmaP(i, i) = 1.0 / S(i);
        svLogSum = svLogSum + log2(S(i));
        invS(i, i) = SigmaP(i, i);
    }

    // compute the covariance matrix
    Sigma = V * invS * V.transpose();
    return svLogSum;
}

}  // namespace calibration
}  // namespace aslam


#ifndef STAN_MATH_PRIM_PROB_NEG_BINOMIAL_2_RNG_HPP
#define STAN_MATH_PRIM_PROB_NEG_BINOMIAL_2_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/constants.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a negative binomial random variate with the specified location and
 * precision parameters using the given random number generator.
 *
 * mu and phi can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_loc type of location parameter
 * @tparam T_prec type of precision parameter
 * @tparam RNG type of random number generator
 * @param mu (Sequence of) positive location parameter(s)
 * @param phi (Sequence of) positive precision parameter(s)
 * @param rng random number generator
 * @return (Sequence of) negative binomial random variate(s)
 * @throw std::domain_error if mu or phi are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_prec, class RNG>
inline typename VectorBuilder<true, int, T_loc, T_prec>::type
neg_binomial_2_rng(const T_loc& mu, const T_prec& phi, RNG& rng) {
  using boost::gamma_distribution;
  using boost::variate_generator;
  using boost::random::poisson_distribution;
  static const char* function = "neg_binomial_2_rng";
  check_positive_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Precision parameter", phi);
  check_consistent_sizes(function, "Location parameter", mu,
                         "Precision parameter", phi);

  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_prec> phi_vec(phi);
  size_t N = max_size(mu, phi);
  VectorBuilder<true, int, T_loc, T_prec> output(N);

  for (size_t n = 0; n < N; ++n) {
    double mu_div_phi = static_cast<double>(mu_vec[n]) / phi_vec[n];

    // gamma_rng params must be positive and finite
    check_positive_finite(function,
                          "Location parameter divided by the "
                          "precision parameter",
                          mu_div_phi);

    double rng_from_gamma = variate_generator<RNG&, gamma_distribution<> >(
        rng, gamma_distribution<>(phi_vec[n], mu_div_phi))();

    // same as the constraints for poisson_rng
    check_less(function, "Random number that came from gamma distribution",
               rng_from_gamma, POISSON_MAX_RATE);
    check_not_nan(function, "Random number that came from gamma distribution",
                  rng_from_gamma);
    check_nonnegative(function,
                      "Random number that came from gamma distribution",
                      rng_from_gamma);

    output[n] = variate_generator<RNG&, poisson_distribution<> >(
        rng, poisson_distribution<>(rng_from_gamma))();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/*
 * Copyright (c) 2015-2016, Luca Fulchir<luca@fulchir.it>, All rights reserved.
 *
 * This file is part of "libRaptorQ".
 *
 * libRaptorQ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * libRaptorQ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * and a copy of the GNU Lesser General Public License
 * along with libRaptorQ.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "RaptorQ/v1/common.hpp"
#include "RaptorQ/v1/degree.hpp"
#include "RaptorQ/v1/Rand.hpp"
#include "RaptorQ/v1/table2.hpp"
#include <cmath>
#include <Eigen/Core>
#include <vector>

namespace RaptorQ__v1 {
namespace Impl {

class RAPTORQ_LOCAL Tuple
{
    // d  1-30  LT-degree of encoded symbol
    // a  0-(W-1)
    // b  0-(W-1)
    // d1 PI-degree of encoded symbol (2 or 3)
    // a1 0-(P1-1)
    // b1 0-(P1-1)
public:
    Tuple() = default;
    Tuple (const Tuple&) = default;
    Tuple& operator= (const Tuple&) = default;
    Tuple (Tuple&&) = default;
    Tuple& operator= (Tuple&&) = default;
    ~Tuple() = default;

    uint16_t d, a, b, d1, a1, b1;   // great names. thanks rfc6330!
};

class RAPTORQ_API Parameters
{
public:
    explicit Parameters (const uint16_t symbols);
    Parameters() = delete;
    Parameters (const Parameters&) = default;
    Parameters& operator= (const Parameters&) = default;
    Parameters (Parameters&&) = default;
    Parameters& operator= (Parameters&&) = default;
    ~Parameters() {}

    uint16_t Deg (const uint32_t v) const;
    Tuple tuple (const uint32_t ISI) const;
    std::vector<uint16_t> get_idxs (const uint32_t ISI) const;

    uint16_t K_padded, S, H, W, L, P, P1, U, B; // RFC 6330, pg 22
    uint16_t J;
private:
    static bool is_prime (const uint16_t n);
};

inline Parameters::Parameters (const uint16_t symbols)
{
    uint16_t idx;
    for (idx = 0; idx < RaptorQ__v1::Impl::K_padded.size(); ++idx) {
        if (RaptorQ__v1::Impl::K_padded[idx] >= symbols) {
            K_padded = RaptorQ__v1::Impl::K_padded[idx];
            break;
        }
    }

    J = RaptorQ__v1::Impl::J_K_padded[idx];
    std::tie (S, H, W) = RaptorQ__v1::Impl::S_H_W [idx];

    L = K_padded + S + H;
    P = L - W;
    U = P - H;
    B = W - S;
    P1 = P + 1;         // first prime number bigger than P. turns out its
                        // always between 1 and 14 more numbers.
    while (!is_prime (P1))  // so this while will be really quick anyway
        ++P1;
}

inline bool Parameters::is_prime (const uint16_t n)
{
    // 1 as prime, don't care. Not in our scope anyway.
    // thank you stackexchange for the code
    if (n <= 3)
        return true;
    if (n % 2 == 0 || n % 3 == 0)
        return false;

    uint32_t i = 5;
    uint32_t w = 2;
    while (i * i <= n) {
        if (n % i == 0)
            return false;
        i += w;
        w = 6 - w;
    }
    return true;
}

inline uint16_t Parameters::Deg (const uint32_t v) const
{
    // rfc 6330, pg 27

    for (uint16_t d = 0; d < RaptorQ__v1::Impl::degree_distribution.size();++d){
        if (v < RaptorQ__v1::Impl::degree_distribution[d])
            return (d < (W - 2)) ? d : (W - 2);
    }
    return 0;   // never get here, but don't make the compiler complain
}

inline Tuple RaptorQ__v1::Impl::Parameters::tuple (const uint32_t ISI) const
{
    RaptorQ__v1::Impl::Tuple ret;

    // taken straight from RFC6330, pg 30
    // so thank them for the *beautiful* names
    // also, don't get confused with "B": this one is different,
    // and thus named "B1"

    size_t A = 53591 + J * 997;

    if (A % 2 == 0)
        ++A;
    size_t B1 = 10267 * (J + 1);
    uint32_t y = static_cast<uint32_t> (B1 + ISI * A);
    uint32_t v = rnd_get (y, 0, static_cast<uint32_t> (std::pow(2, 20)));
    ret.d = Deg (v);
    ret.a = 1 + static_cast<uint16_t> (rnd_get (y, 1, W - 1));
    ret.b = static_cast<uint16_t> (rnd_get (y, 2, W));
    if (ret.d < 4) {
        ret.d1 = 2 + static_cast<uint16_t> (rnd_get (ISI, 3, 2));
    } else {
        ret.d1 = 2;
    }
    ret.a1 = 1 + static_cast<uint16_t> (rnd_get (ISI, 4, P1 - 1));
    ret.b1 = static_cast<uint16_t> (rnd_get (ISI, 5, P1));

    return ret;
}

inline std::vector<uint16_t> Parameters::get_idxs (const uint32_t ISI) const
{
    // Needed to generate G_ENC: We need the ids of the symbols we would
    // use on a "Enc" call. So this is the "enc algorithm, but returns the
    // indexes instead of computing the result.
    // rfc6330, pg29

    std::vector<uint16_t> ret;
    Tuple t = tuple (ISI);

    ret.reserve (t.d + t.d1);
    ret.push_back (t.b);

    for (uint16_t j = 1; j < t.d; ++j) {
        t.b = (t.b + t.a) % W;
        ret.push_back (t.b);
    }
    while (t.b1 >= P)
        t.b1 = (t.b1 + t.a1) % P1;

    ret.push_back (W + t.b1);
    for (uint16_t j = 1; j < t.d1; ++j) {
        t.b1 = (t.b1 + t.a1) % P1;
        while (t.b1 >= P)
            t.b1 = (t.b1 + t.a1) % P1;
        ret.push_back (W + t.b1);
    }
    return ret;
}

}   // namespace Impl
}   // namespace RaptorQ


/*
 * Profile.cpp
 *
 *  Created on: Apr 16, 2011
 *      Author: tbabb
 */

#include <time.h>
#include <string>
#include <iostream>

#include <boost/iterator/counting_iterator.hpp>

#include <geomc/function/Dual.h>
#include <geomc/linalg/Vec.h>
#include <geomc/linalg/Matrix.h>
#include <geomc/random/RandomTools.h>
#include <geomc/random/MTRand.h>
#include <geomc/random/LCRand.h>
#include <geomc/function/Path.h>
#include <geomc/function/PerlinNoise.h>
#include <geomc/function/Raster.h>
#include <geomc/function/SphericalHarmonics.h>
#include <geomc/shape/BinLatticePartition.h>
#include <geomc/shape/Trace.h>
#include <geomc/shape/Oriented.h>
#include <geomc/shape/Intersect.h>
#include <geomc/shape/Frustum.h>

#include "RandomBattery.h"

#define NUM_PROFILE_CASES 1024;

using namespace geom;
using namespace std;

template <typename T, index_t N>
void fill_unit_vec_array(typename PointType<T,N>::point_t *dst, index_t n) {
    Sampler<T> rntools = Sampler<T>();

    for (index_t i = 0; i < n; i++) {
        dst[i] = rntools.template unit<N>();
    }
}

template <index_t N> void fill_unit_raw_vec_array(double dst[][N], index_t n) {
    Sampler<double> rntools = Sampler<double>();
    Vec<double,N> v;

    for (index_t i = 0; i < n; i++) {
        v = rntools.template unit<N>();
        for (index_t axis = 0; axis < N; axis++) {
            dst[i][axis] = v[axis];
        }
    }
}

template <typename T, index_t N> void fill_range_vec_array(Vec<T, N> *dst, index_t n, Vec<T,N> lo, Vec<T,N> hi) {
    Sampler<double> rntools = Sampler<double>();

    for (index_t i = 0; i < n; i++) {
        dst[i] = rntools.box(lo, hi);
    }
}

template <typename T, index_t N> void fill_range_vec_array(Vec<Dual<T>, N> *dst, index_t n, Vec<Dual<T>,N> lo, Vec<Dual<T>,N> hi) {
    Sampler< Dual<T> > rntools = Sampler< Dual<T> >();

    for (index_t i = 0; i < n; i++) {
        dst[i] = rntools.box(lo, hi);
    }
}

double profile_vec_cross(index_t iters) {
    const index_t n = NUM_PROFILE_CASES;
    Vec3d v;
    Vec3d *vecs_src = new Vec3d[n];
    Vec3d *vecs_dst = new Vec3d[n];

    fill_unit_vec_array<double,3>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].cross(vecs_src[(idx+1) % n]);
        idx += 1;
    }
    clock_t end = clock();

    delete [] vecs_src;
    delete [] vecs_dst;
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_add(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src_1 = new Vec<double,N>[n];
    Vec<double, N> *vecs_src_2 = new Vec<double,N>[n];
    Vec<double, N> *vecs_dst   = new Vec<double,N>[n];

    fill_unit_vec_array<double,N>(vecs_src_1, n);
    fill_unit_vec_array<double,N>(vecs_src_2, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx] = vecs_src_1[idx] + (vecs_src_2[idx]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();

    delete [] vecs_src_1;
    delete [] vecs_src_2;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_dot(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src_1 = new Vec<double,N>[n];
    Vec<double, N> *vecs_src_2 = new Vec<double,N>[n];
    double *vecs_dst = new double[n];

    fill_unit_vec_array<double,N>(vecs_src_1, n);
    fill_unit_vec_array<double,N>(vecs_src_2, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx] = vecs_src_1[idx].dot(vecs_src_2[idx]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();
    vecs_dst[0] += 1;

    delete [] vecs_src_1;
    delete [] vecs_src_2;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_norm(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src = new Vec<double,N>[n];
    double *vecs_dst = new double[n];

    fill_unit_vec_array<double,N>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].mag();
        idx += 1;
    }
    clock_t end = clock();
    vecs_dst[0] += 1;

    delete [] vecs_src;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_norm2(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src = new Vec<double,N>[n];
    double *vecs_dst = new double[n];

    fill_unit_vec_array<double,N>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].mag2();
        idx += 1;
    }
    clock_t end = clock();
    vecs_dst[0] += 1; //shenanigans to prevent compiler from optimizing to a null loop.

    delete [] vecs_src;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_hash(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src = new Vec<double,N>[n];
    index_t *vecs_dst= new index_t[n];

    fill_unit_vec_array<double,N>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].hashcode();
        idx += 1;
    }
    clock_t end = clock();
    vecs_dst[0] += 1;

    delete [] vecs_src;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

// XXX: TODO: HANDLE THIS

template <index_t N> double profile_raw_vec_add(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    double vecs_src1[n][N];
    double vecs_src2[n][N];
    double vecs_dst[n][N];

    fill_unit_raw_vec_array(vecs_src1, n);
    fill_unit_raw_vec_array(vecs_src2, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        for (index_t axis = 0; axis < N; axis++) {
            vecs_dst[idx][axis] = vecs_src1[idx][axis] + vecs_src2[idx][axis];
        }
        idx = (idx + 1) % n;
    }
    clock_t end = clock();
    vecs_dst[0][0] += 1;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_perlin(index_t iters) {
    typedef typename PointType<double,N>::point_t point_t;
    index_t n = NUM_PROFILE_CASES;
    point_t range = point_t(100000);
    point_t *vecs_src = new point_t[n];
    PerlinNoise<double,N> perlin;
    double *dest_vals = new double[n];

    fill_range_vec_array<double,N>(vecs_src, n, -range, range);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        dest_vals[idx] = perlin.eval(vecs_src[i % n]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();
    dest_vals[0] += 1;

    delete [] vecs_src;
    delete [] dest_vals;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_perlin_grad(index_t iters) {
    typedef Dual<T> dual;
    typedef typename PointType<dual,N>::point_t point_t;

    index_t n = NUM_PROFILE_CASES;
    point_t range = point_t(10000);
    point_t *vecs_src = new point_t[n];
    PerlinNoise<dual,N> perlin;
    dual *dest_vals = new dual[n];

    fill_range_vec_array<T,N>(vecs_src, n, -range, range);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        dest_vals[idx] = perlin.eval(vecs_src[i%n]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();

    delete [] vecs_src;
    delete [] dest_vals;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_perlin_hard_grad(index_t iters) {
    typedef typename PointType<T,N>::point_t point_t;

    index_t n = NUM_PROFILE_CASES;
    point_t range = point_t(10000);
    point_t *vecs_src = new point_t[n];
    PerlinNoise<T,N> perlin;
    point_t *dest_vals = new point_t[n];

    fill_range_vec_array<T,N>(vecs_src, n, -range, range);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        dest_vals[idx] = perlin.gradient(vecs_src[n]).second;
        idx = (idx + 1) % n;
    }
    clock_t end = clock();

    delete [] vecs_src;
    delete [] dest_vals;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T>
double profile_rayTriangleTest(index_t iters) {
    Sampler<T> rvs;
    static const index_t N = 3;
    index_t n = std::sqrt(iters);
    Ray<T,N>* rays = new Ray<T,N>[n];
    Vec<T,N>* pts  = new Vec<T,N>[n*N];

    // generate n random rays; pts.
    for (index_t i = 0; i < n; i++) {
        rays[i].origin    = rvs.template unit<N>(2.0);
        rays[i].direction = rvs.template unit<N>();
        for (index_t j = 0; j < N; ++j) {
            pts[i*N + j] = rvs.template unit<N>();
        }
    }
    index_t hits = 0;

    clock_t start = clock();
    for (index_t i = 0; i < n; i++) {
        Vec<T,N>* verts = pts + N * i;
        for (index_t j = 0; j < n; ++j) {
            hits += trace_tri(verts[0], verts[1], verts[2], rays[j], HIT_FRONT).hit;
        }
    }
    clock_t end = clock();

    delete [] rays;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N>
double profile_raySimplexTest(index_t iters) {
    Sampler<T> rvs;
    index_t n = std::sqrt(iters);
    Ray<T,N>* rays = new Ray<T,N>[n];
    Vec<T,N>* pts  = new Vec<T,N>[n*N];

    // generate n random rays; pts.
    for (index_t i = 0; i < n; i++) {
        rays[i].origin    = rvs.template unit<N>(2.0);
        rays[i].direction = rvs.template unit<N>();
        for (index_t j = 0; j < N; ++j) {
            pts[i*N + j] = rvs.template unit<N>();
        }
    }
    index_t hits = 0;
    Vec<T,N-1> srf_coords;
    T s;

    clock_t start = clock();
    for (index_t i = 0; i < n; i++) {
        Vec<T,N>* verts = pts + N * i;
        for (index_t j = 0; j < n; ++j) {
            hits += trace_simplex(verts, rays[j], &srf_coords, &s);
        }
    }
    clock_t end = clock();

    delete [] rays;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t Bands> double profile_sh(index_t iters) {
    SphericalHarmonics<T,Bands> sh;
    index_t n = std::min(iters, (index_t)1000000);
    Vec<T,3> *vs = new Vec<T,3>[n];
    fill_unit_vec_array<T,3>(vs, n);

    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        Vec<T,3> v = vs[i % n];
        sh.eval(v);
    }
    clock_t end = clock();

    delete [] vs;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t Bands> double profile_sh_buffer(index_t iters) {
    SphericalHarmonics<T,Bands> sh;
    SphericalHarmonics<T,Bands> buf;
    index_t n = std::min(iters, (index_t)1000000);
    Vec<T,3> *vs = new Vec<T,3>[n];
    fill_unit_vec_array<T,3>(vs, n);
    T k = 0;

    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        Vec<T,3> v = vs[i % n];
        spherical_harmonic_coeff(&buf, v.z, std::atan2(v.y, v.x));
        k = sh.dot(buf);
    }
    clock_t end = clock();

    delete [] vs;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t Bands>
void test_sh_methods(index_t iters) {
    SphericalHarmonics<T,Bands> sh;
    SphericalHarmonics<T,Bands> buf;
    index_t n = std::min(iters, (index_t)1000000);
    Vec<T,3> *vs = new Vec<T,3>[n];
    fill_unit_vec_array<T,3>(vs, n);

    std::fill(sh.coeffs.get(), sh.coeffs.get() + sh.size(), 1);

    T diff = 0;
    T a_avg = 0;
    T b_avg = 0;

    for (index_t i = 0; i < iters; i++) {
        Vec<T,3> v = vs[i % n];
        spherical_harmonic_coeff(&buf, v.z, std::atan2(v.y, v.x));
        T a = sh.dot(buf);
        T b = sh.eval(v);
        a_avg += a;
        b_avg += b;
        diff  += std::abs(a-b);
    }

    std::cout << "avg sh diff: " << diff / iters << std::endl;
    std::cout << "avg buf,std: " << a_avg << "," << b_avg << std::endl;

    delete [] vs;
}

template <typename T, index_t N> double profile_path(index_t iters) {
    Path<T,N> p;
    Sampler<T> rvs;
    int n_knots = 50;
    for (index_t i = 0; i < n_knots; i++) {
        p.knots.push_back(Ray<T,N>(rvs.template box<N>(), rvs.template solidball<N>()));
    }
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        p.eval(getRandom()->rand(0,n_knots));
    }
    clock_t end = clock();
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N, index_t Channels, Interpolation Interp>
double profile_raster(index_t iters) {
    static const index_t n = 128;
    Vec<index_t,N> dim(n);
    Raster<T,T,N,Channels> image(dim);
    Vec<T,N> *coords = new Vec<T,N>[n];
    Sampler<T> smp;

    // fill with random junk
    // also make some random coords to use
    for (index_t i = 0; i < n; i++) {
        coords[i] = smp.box(Vec<T,N>::zeros, (Vec<T,N>)dim);
        for (index_t j = 0; j < n; j++) {
            image.set(Vec<index_t,N>(i,j), smp.template unit<Channels>());
        }
    }

    // sample in a bunch of random places.
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        image.template sample<EDGE_CLAMP,Interp>(coords[i % n]);
    }
    clock_t end = clock();

    delete [] coords;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T> double profile_rotCtr(index_t iters) {
    Sampler<T> rvs;
    AffineTransform<T,3> xf;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        T angle = rvs.rng->template rand<T>(0, 2*M_PI);
        Vec<T,3> axis = rvs.template unit<3>();
        Vec<T,3> ctr  = rvs.box(Vec<T,3>(-10,-10,-10), Vec<T,3>(10,10,10));
        xf = rotation<T>(axis, ctr, angle);
    }
    clock_t end = clock();
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename M> double profile_mtxCopy(index_t iters) {
    index_t rows = 5;
    index_t cols = 5;
    M m(rows,cols);
    Random *rng = getRandom();
    typedef typename M::elem_t T;
    T *ary = new T[rows*cols+1];
    for (index_t i = 0; i < rows*cols+1; i++) {
        ary[i] = rng->rand(1.0);
    }
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        std::copy(m.begin(), m.end(), ary);
    }
    clock_t end = clock();

    delete [] ary;
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename M> double profile_mtxRegionCopy(index_t iters) {
    index_t rows = 5;
    index_t cols = 5;
    M m(rows,cols);
    Random *rng = getRandom();
    typedef typename M::elem_t T;
    T *ary = new T[rows*cols+1];
    for (index_t i = 0; i < rows*cols+1; i++) {
        ary[i] = rng->rand(1.0);
    }
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        MatrixRegion region(Vec<index_t,2>(1,1), Vec<index_t,2>(rows-1, cols-1));
        std::copy(m.region_begin(region), m.region_end(region), ary);
    }
    clock_t end = clock();

    delete [] ary;
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_mtxInverse(index_t iters) {
    SimpleMatrix<T,N,N> mtx[2];
    Random *rng = getRandom();
    for (T *p = mtx[0].begin(); p != mtx[0].end(); p++) {
        *p = rng->rand(1.0);
    }
    index_t x = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++, x = !x) {
        inv(&mtx[!x], mtx[x]);
    }
    clock_t end = clock();

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_mtxInverseLU(index_t iters) {
    SimpleMatrix<T,N,N> mtx[2];
    Random *rng = getRandom();
    for (T *p = mtx[0].begin(); p != mtx[0].end(); p++) {
        *p = rng->rand(1.0);
    }
    index_t x = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++, x = !x) {
        PLUDecomposition<T,N,N> plu = plu_decompose(mtx[x]);
        plu.inverse(&mtx[!x]);
    }
    clock_t end = clock();

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N>
void randomOrient(AffineTransform<T,N> *xf) {
    Sampler<T> smp;
    Vec<T,N> basis[N];
    for (index_t i = 0; i < N; i++) {
        basis[i] = smp.template unit<N>();
    }
    orthonormalize(basis,N);
    SimpleMatrix<T,N,N> rot(basis[0].begin());
    *xf = transformation(rot);
}

template <typename T>
void randomOrient(AffineTransform<T,3> *xf) {
    Sampler<T> smp;
    Quat<T> q = smp.template unit<4>();
    *xf = rotation(q);
}

template <typename T>
void randomOrient(AffineTransform<T,2> *xf) {
    T angle =  M_PI * getRandom()->rand<T>(-1,1);
    *xf = rotation(angle);
}

template <typename T, index_t N>
void randomBox(OrientedRect<T,N> *r) {
    Sampler<T> smp;

    randomOrient(&r->xf);
    r->xf *= scale(Vec<T,N>(getRandom()->rand<T>(2,8)));
    r->xf *= translation(smp.template unit<N>());
    Vec<T,N> b0 = smp.box(Vec<T,N>(-1), Vec<T,N>(1));
    Vec<T,N> b1 = smp.box(Vec<T,N>(-1), Vec<T,N>(1));
    r->shape = Rect<T,N>::spanning_corners(b0,b1);
}

template <typename T, index_t N>
void getCorners(const ViewFrustum<T,N>& f, Vec<T,N> p[1 << N]) {
    typedef PointType<T,N-1> Pt;
    const Frustum< Rect<T,N-1> >& f_i = f.shape;
    typename Pt::point_t extreme[2] = { f_i.base.lo, f_i.base.hi };
    Rect<T,1> h_clip = f_i.clipped_height();

    for (index_t k = 0; k < 2; k++) {
        // for lower and upper extents
        T h = k == 0 ? h_clip.lo : h_clip.hi;
        for (index_t c = 0; c < (1 << (N-1)); c++) {
            // for each corner of the base rect
            Vec<T,N> pt;
            for (index_t i = 0; i < N-1; i++) {
                // for each coordinate of the corner point
                pt[i] = Pt::iterator(extreme[(c & (1 << i)) != 0])[i];
            }
            pt[N-1] = 1;
            pt *= h;
            pt  = f.xf * pt;
            p[c + ((k > 0) ? (1 <<(N-1)) : 0)] = pt;
        }
    }
}

template <typename T, index_t N>
void getCorners(const OrientedRect<T,N>& r, Vec<T,N> p[1 << N]) {
    Vec<T,N> extreme[2] = { r.shape.lo, r.shape.hi };

    for (index_t i = 0; i < (1 << N); ++i) {
        Vec<T,N> v;
        for (index_t axis = 0; axis < N; ++axis) {
            v[axis] = extreme[((i >> axis) & 1)][axis];
        }
        p[i] = r.xf * v;
    }
}

template <typename T>
void randomFrustum(ViewFrustum<T,2>* f) {
    Sampler<T> smp;
    f->shape.height = Rect<T,1>::spanning_corners(
                        getRandom()->rand<T>(-5,5),
                        getRandom()->rand<T>(-5,5));
    f->shape.base = Rect<T,1>::spanning_corners(
                        getRandom()->rand<T>(-5),
                        getRandom()->rand<T>( 5));
    randomOrient(&f->xf);
    f->xf *= scale(Vec<T,2>(getRandom()->rand(2,8)));
    f->xf *= translation(smp.template unit<2>());
}

template <typename T, index_t N>
void randomFrustum(ViewFrustum<T,N>* f) {
    Sampler<T> smp;
    f->shape.height = Rect<T,1>::spanning_corners(
                        getRandom()->rand<T>(-5,5),
                        getRandom()->rand<T>(-5,5));
    Vec<T,N-1> b0 = smp.box(Vec<T,N-1>(-5), Vec<T,N-1>(5));
    Vec<T,N-1> b1 = smp.box(Vec<T,N-1>(-5), Vec<T,N-1>(5));
    f->shape.base = Rect<T,N-1>::spanning_corners(b0, b1);
    randomOrient(&f->xf);
    f->xf *= scale(Vec<T,N>(getRandom()->rand(2,8)));
    f->xf *= translation(smp.template unit<N>());
}

template <typename T, index_t N> double profile_gjkIntersect(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    const index_t n_corners = 1 << N;
    //Vec<T,N> *rects = new Vec<T,N>[n*n_corners];
    OrientedRect<T,N> *rects = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        //OrientedRect<T,N> r;
        randomBox(rects+i);
        //r.getCorners(rects + n_corners*i);
    }

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    clock_t start = clock();
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        //b = b ^ gjk_intersect(rects + n_corners*i0, n_corners, rects + n_corners*i1, n_corners, &d);
        b = b ^ gjk_intersect(rects[i0], rects[i1], &d);
    }
    clock_t end = clock();
    delete [] rects;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_OBB_intersect(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    OrientedRect<T,N> *boxes = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        randomBox(boxes + i);
    }

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    clock_t start = clock();
    bool SAT = false;
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        SAT = SAT ^ boxes[i0].intersects(boxes[i1]);
    }
    clock_t end = clock();

    delete [] boxes;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_OBB_bounds(index_t iters) {
    index_t n = std::min(iters, (index_t)100000);
    OrientedRect<T,N> *boxes = new OrientedRect<T,N>[n];
    for (index_t i = 0; i < n; i++) randomBox(boxes + i);

    clock_t s = 0;
    Vec<T,N> z;

    for (index_t k = 0; k < iters; k += n) {
        index_t nn = std::min(iters - k, n);

        Rect<T,N> r;

        clock_t start = clock();
        for (index_t j = 0; j < nn; j++) {
            r  = boxes[j].bounds();
            z += r.hi;
        }
        clock_t end = clock();

        s += end - start;
    }

    delete [] boxes;
    return s / (double)CLOCKS_PER_SEC;;
}

//#define EMIT_GJK_ERRS
//#define EMIT_GJK_ALL

// this is tautological for N > 3. OBBs use GJK directly!
template <typename T, index_t N> index_t test_gjkIntersect(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    OrientedRect<T,N>* boxes = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        randomBox(boxes + i);
    }
    const index_t n_corners = 1 << N;

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    index_t failures = 0;
    index_t positive = 0;
    index_t negative = 0;
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        Vec<T,N> b0[n_corners];
        Vec<T,N> b1[n_corners];
        getCorners(boxes[i0], b0);
        getCorners(boxes[i1], b1);
        bool print = false;
#ifdef EMIT_GJK_ALL
        print = true;
#endif
        bool gjk = gjk_intersect(b0, n_corners, b1, n_corners, &d); //, print);
        bool SAT = boxes[i0].intersects(boxes[i1]);
        if (gjk != SAT) {
#ifdef EMIT_GJK_ERRS
            gjk_intersect(b0, n_corners, b1, n_corners, &d); //, true);
            std::cout << ".\n";
#endif
            failures++;
        }
#ifdef EMIT_GJK_ALL
        std::cout << ".\n";
#endif
        if (SAT) positive++;
        else negative++;
    }

    delete [] boxes;

#if !defined(EMIT_GJK_ERRS) and !defined(EMIT_GJK_ALL)
    std::cout << "gjk " << N << "D failures: " << failures;
    std::cout << " (" << positive << " overlapped " << negative << " disjoint)" << std::endl;
#endif

    return failures;
}

template <typename T, index_t N> T test_gjkNearest(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    OrientedRect<T,N>* boxes = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        randomBox(boxes + i);
    }

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    T olap_sum = 0;
    T disj_sum = 0;
    index_t olap_n = 0;
    index_t disj_n = 0;
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        OrientedRect<T,N>& b0 = boxes[i0];
        OrientedRect<T,N>& b1 = boxes[i1];

        Vec<T,N> axi;
        bool olap = minimal_separation_axis(b0, b1, &axi);
        Vec<T,N> contact = b1.convexSupport(axi) + axi;
        T dist = (b0.convexSupport(-axi) - contact).mag();

        // tally error
        if (olap) {
            olap_sum += dist;
            olap_n++;
        } else {
            disj_sum += dist;
            disj_n++;
        }
    }

    delete [] boxes;

    std::cout << "gjk " << N << "D nearest axis:" << std::endl;
    std::cout << "  overlap mean error:  " << (olap_sum / olap_n) << std::endl;
    std::cout << "  disjoint mean error: " << (disj_sum / disj_n) << std::endl;
    return (olap_sum + disj_sum) / iters;
}

template <typename T, index_t N> index_t test_frustumSupport(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    const index_t n_corners = 1 << N;

    Sampler<T> smp;
    T err = 0;

    index_t failures = 0;
    for (index_t j = 0; j < iters/100; j++) {
        ViewFrustum<T,N> f;
        randomFrustum(&f);
        Vec<T,N> pts[n_corners];
        getCorners(f, pts);
        for (index_t k = 0; k < 100; k++) {
            Vec<T,N> d = smp.template unit<N>();
            Vec<T,N> p0 = f.convexSupport(d);
            Vec<T,N> p1 = pts[0];
            T dot0 = p1.dot(d);
            // brute force find support point
            for (index_t c = 1; c < n_corners; c++) {
                T dot1 = pts[c].dot(d);
                if (dot1 > dot0) {
                    dot0 = dot1;
                    p1 = pts[c];
                }
            }
            if (p0 != p1) {
                err += p1.dist(p0);
                failures++;
            }
        }
    }

    std::cout << "frustum support " << N << "D failures: " << failures;
    std::cout << " (" << (100 * failures / (double)iters) << "%)";
    std::cout << " average mismatch: " << (err / (double)iters) << std::endl;

    return failures;
}

template <typename T, index_t N> void test_mtxInverse(index_t iters) {
    SimpleMatrix<T,N,N> mtx[2];
    Random *rng = getRandom();
    for (T *p = mtx[0].begin(); p != mtx[0].end(); p++) {
        *p = rng->rand(1.0);
    }
    std::cout << mtx[0];
    index_t x = 0;
    for (index_t i = 0; i < iters; i++, x = !x) {
        inv(&mtx[!x], mtx[x]);
        std::cout << mtx[!x];
    }
}

template <typename M> void test_mtxRegionCopy(index_t rows, index_t cols) {
    M m(rows,cols);
    typedef typename M::elem_t T;
    const index_t subn = (rows-2)*(cols-2);
    T *v = new T[subn];

    for (index_t i = 0; i < subn; i++) {
        v[i] = 11*i;
    }

    std::fill(m.begin(), m.end(), 1);
    std::copy(v, v+subn, m.region_begin(1,1,rows-1, cols-1));
    cout << m;
}

template <typename T, index_t N> void test_OOBB() {
    OrientedRect<T,N> r1;
    OrientedRect<T,N> r2;
    cout << "box1 intersects box2: " << r1.intersects(r2) << endl;
}

template <typename T, index_t M, index_t N> void test_mtxArithmetic(index_t rows, index_t cols) {
    SimpleMatrix<T,M,N> a(rows,cols);
    SimpleMatrix<T,M,N> b(rows,cols);
    SimpleMatrix<T,M,N == 0 ? 0 : N+1> bogus(rows,cols+1);

    cout << "addTest:" << endl;
    cout << a + b << endl;
    cout << a * 3 << endl;
    cout << 3 * a << endl;
    //correctly does not compile:
    //cout << a + bogus << endl;
}

template <index_t N> void test_permuteMatrix() {
    PermutationMatrix<N> p;
    PermutationMatrix<N> p1;
    Vec<double,N> v;
    Random *rng = getRandom();
    for (index_t i = 0; i < N; i++) {
        v[i] = i;
    }
    for (index_t i = 0; i < N*2; i++) {
        p.swap_rows(rng->rand(N), rng->rand(N));
        p1.swap_rows(rng->rand(N), rng->rand(N));
    }
    SimpleMatrix<double,N,N> m;
    SimpleMatrix<double,N,N> minv;
    SimpleMatrix<double,N,N> m_p1;
    std::copy(p.begin(), p.end(),  m.begin());
    std::copy(p1.begin(), p1.end(),  m_p1.begin());
    inv(&minv, p);
    bool correct = (p * p1) == (m * m_p1);
    if (!correct) throw "permutation matrix failure";
    cout << "v: " << v << endl;
    cout << "P: " << endl << p << endl;
    cout << "P*v: " << (p * v) << endl;
    cout << "M: " << endl << m << endl;
    cout << "M*v: " << (m * v) << endl;
    cout << "P * P1: " << (p * p1) << endl;
    cout << "P * P1 correct? " << correct << endl;
    cout << "P^-1: " << endl << minv << endl;
}

template <index_t M, index_t N> void test_simpleMatrix() {
    SimpleMatrix<double, M, N> m;
    SimpleMatrix<double, DYNAMIC_DIM, DYNAMIC_DIM> q(M,N);
    m[0][1] = 5;
    q[0][1] = 8;
    cout << m << endl;
    cout << q << endl;
}

void profile(std::string name, double (*fnc)(index_t), index_t iterations) {
    double t = fnc(iterations);
    std::cout << name << " (" << iterations << " iters): " << t << " secs; " << iterations/t << " ops/sec"  << std::endl;
}

void test_matrix_asplode() {
    SimpleMatrix<double,4,4> mtx;
    SimpleMatrix<double,2,5> other;
    // correctly, won't compile:
    // mtx * other;
}

void test_dual() {
    typedef Dual<double> duald;
    cout << "sin(pi): " << sin(duald(PI,1)) << endl;
    cout << "cos(pi): " << cos(duald(PI,1)) << endl;
    cout << "tan(pi): " << tan(duald(PI,1)) << endl;
    cout << "2x + 1, @x=3: " << (duald(3,1) * 2 + 1) << endl;
    cout << "exp(1): " << exp(duald(1,1)) << endl;
    cout << "pow(x,x) @x=2: " << pow(duald(2,1), duald(2,1)) << endl;
    cout << "pow(x=2,2): " << pow(duald(2,1), 2) << endl;
    cout << endl;
}

using geom::operator<<;

#if defined(EMIT_GJK_ERRS) or defined(EMIT_GJK_ALL)
int main(int argc, char** argv) {
    test_gjkIntersect<double,3>(100000);
    return 0;
}
#else

int main(int argc, char** argv) {
    Vec2d a2d;
    Vec3d a3d;
    Vec4d a4d;

    //test_gjkNearest<double,3>(1000);

    index_t iters = 10000000;

    assert(&a2d.get(0) == &a2d.x && &a2d.get(1) == &a2d.y);
    assert(&a3d.get(0) == &a3d.x && &a3d.get(1) == &a3d.y && &a3d.get(2) == &a3d.z);
    assert(&a4d.get(0) == &a4d.x && &a4d.get(1) == &a4d.y && &a4d.get(2) == &a4d.z && &a4d.get(3) == &a4d.w);

    test_permuteMatrix<5>();
    test_simpleMatrix<4,3>();
    test_mtxArithmetic<double,4,3>(4,3);
    test_dual();
    test_OOBB<double,3>();
    test_gjkIntersect<double,2>(1000000);
    test_gjkIntersect<double,3>(1000000);
    // test_gjkNearest<double,2>(1000000);
    // test_gjkNearest<double,3>(1000);

#ifdef ENABLE_FRUSTUM
    test_frustumSupport<double,2>(1000000);
    test_frustumSupport<double,3>(1000000);
    test_frustumSupport<double,4>(1000000);
    std::cout << std::endl;
#endif

    profile("3d cross product", profile_vec_cross, iters);
    std::cout << std::endl;

    profile("2d add", profile_vec_add<2>, iters);
    profile("3d add", profile_vec_add<3>, iters);
    profile("4d add", profile_vec_add<4>, iters);
    profile("8d add", profile_vec_add<8>, iters);
    std::cout << std::endl;

    profile("2d raw add", profile_raw_vec_add<2>, iters);
    profile("3d raw add", profile_raw_vec_add<3>, iters);
    profile("4d raw add", profile_raw_vec_add<4>, iters);
    profile("8d raw add", profile_raw_vec_add<8>, iters);
    std::cout << std::endl;

    profile("2d norm", profile_vec_norm<2>, iters);
    profile("3d norm", profile_vec_norm<3>, iters);
    profile("4d norm", profile_vec_norm<4>, iters);
    profile("8d norm", profile_vec_norm<8>, iters);
    std::cout << std::endl;

    profile("2d norm2", profile_vec_norm2<2>, iters);
    profile("3d norm2", profile_vec_norm2<3>, iters);
    profile("4d norm2", profile_vec_norm2<4>, iters);
    profile("8d norm2", profile_vec_norm2<8>, iters);
    std::cout << std::endl;

    //profile("1d perlin", profile_perlin<1>, iters/100);
    profile("2d perlin", profile_perlin<2>, iters/100);
    profile("3d perlin", profile_perlin<3>, iters/100);
    profile("4d perlin", profile_perlin<4>, iters/100);
    profile("5d perlin", profile_perlin<5>, iters/100);
    std::cout << std::endl;

    //profile("1d perlin dual", profile_perlin_grad<double,1>, iters/100);
    profile("2d perlin dual", profile_perlin_grad<double,2>, iters/100);
    profile("3d perlin dual", profile_perlin_grad<double,3>, iters/100);
    profile("4d perlin dual", profile_perlin_grad<double,4>, iters/100);
    std::cout << std::endl;

    //profile("1d perlin grad", profile_perlin_hard_grad<double,1>, iters/100);
    profile("2d perlin grad", profile_perlin_hard_grad<double,2>, iters/100);
    profile("3d perlin grad", profile_perlin_hard_grad<double,3>, iters/100);
    profile("4d perlin grad", profile_perlin_hard_grad<double,4>, iters/100);
    std::cout << std::endl;

    profile("2d hash", profile_vec_hash<2>, iters);
    profile("3d hash", profile_vec_hash<3>, iters);
    profile("4d hash", profile_vec_hash<4>, iters);
    std::cout << std::endl;

    profile("2d dot", profile_vec_dot<2>, iters);
    profile("3d dot", profile_vec_dot<3>, iters);
    profile("4d dot", profile_vec_dot<4>, iters);
    profile("8d dot", profile_vec_dot<8>, iters);
    std::cout << std::endl;

    profile("ray3f-triangle hit",     profile_rayTriangleTest<float>,       iters/10);
    profile("ray3d-triangle hit",     profile_rayTriangleTest<double>,      iters/10);
    profile("ray3d-simplex hit",      profile_raySimplexTest<double,3>,     iters/10);
    profile("ray4d-simplex hit",      profile_raySimplexTest<double,4>,     iters/10);
    profile("ray5d-simplex hit",      profile_raySimplexTest<double,5>,     iters/10);

    std::cout << std::endl;

    profile("2d path", profile_path<double, 2>, iters);
    profile("3d path", profile_path<double, 3>, iters);
    profile("4d path", profile_path<double, 4>, iters);
    profile("2f path", profile_path<float, 2>,  iters);
    profile("3f path", profile_path<float, 3>,  iters);
    profile("4f path", profile_path<float, 4>,  iters);
    std::cout << std::endl;

    profile("OBB 2f bounds", profile_OBB_bounds<float,2>,  iters / 10);
    profile("OBB 3f bounds", profile_OBB_bounds<float,3>,  iters / 10);
    profile("OBB 4f bounds", profile_OBB_bounds<float,4>,  iters / 10);
    profile("OBB 2d bounds", profile_OBB_bounds<double,2>, iters / 10);
    profile("OBB 3d bounds", profile_OBB_bounds<double,3>, iters / 10);
    profile("OBB 4d bounds", profile_OBB_bounds<double,3>, iters / 10);
    std::cout << std::endl;

    profile("sh  3f band", profile_sh<float, 3>,  iters/10);
    profile("sh  8f band", profile_sh<float, 8>,  iters/50);
    profile("sh 16f band", profile_sh<float, 16>, iters/100);
    profile("sh  3d band", profile_sh<double, 3>,  iters/10);
    profile("sh  8d band", profile_sh<double, 8>,  iters/50);
    profile("sh 16d band", profile_sh<double, 16>, iters/100);
    std::cout << std::endl;

    profile("2f->3f linear img sample", profile_raster<float, 2, 3, INTERP_LINEAR>, iters/100);
    profile("3f->3f linear img sample", profile_raster<float, 3, 3, INTERP_LINEAR>, iters/100);
    profile("2f->3f cubic img sample",  profile_raster<float, 2, 3, INTERP_CUBIC>,  iters/100);
    profile("3f->3f cubic img sample",  profile_raster<float, 3, 3, INTERP_CUBIC>,  iters/100);
    std::cout << std::endl;

    profile("5x5 mtxf region copy",    profile_mtxRegionCopy<SimpleMatrix<float,0,0> >,    iters);
    profile("5x5 mtxd region copy",    profile_mtxRegionCopy<SimpleMatrix<double,0,0> >,   iters);
    profile("5x5 diagf region copy",   profile_mtxRegionCopy<DiagMatrix<float,5,5> >, iters);
    std::cout << std::endl;

    profile("5x5 mtxf copy",    profile_mtxCopy<SimpleMatrix<float,0,0> >, iters/100);
    profile("5x5 mtxd copy",    profile_mtxCopy<SimpleMatrix<double,0,0> >, iters/100);
    profile("5x5 diagf copy",   profile_mtxCopy<DiagMatrix<float,5,5> >, iters/100);   // xxx: memory error in here?!
    std::cout << std::endl;

    profile("2x2 mtxf inv", profile_mtxInverse<float,2>,  iters/10);
    profile("3x3 mtxf inv", profile_mtxInverse<float,3>,  iters/10);
    profile("4x4 mtxf inv", profile_mtxInverse<float,4>,  iters/10);
    profile("2x2 mtxd inv", profile_mtxInverse<double,2>, iters/10);
    profile("3x3 mtxd inv", profile_mtxInverse<double,3>, iters/10);
    profile("4x4 mtxd inv", profile_mtxInverse<double,4>, iters/10);
    profile("8x8 mtxd inv", profile_mtxInverse<double,8>, iters/10);
    std::cout << std::endl;

    profile("rotationd matrix", profile_rotCtr<double>, iters/10);
    profile("rotationf matrix", profile_rotCtr<float>, iters/10);
    std::cout << std::endl;

    profile("gjk float 2d",  profile_gjkIntersect<float,2>,  1000000);
    profile("gjk double 2d", profile_gjkIntersect<double,2>, 1000000);
    profile("gjk float 3d",  profile_gjkIntersect<float,3>,  1000000);
    profile("gjk double 3d", profile_gjkIntersect<double,3>, 1000000);
    profile("gjk float 4d",  profile_gjkIntersect<float,4>,  100000);
    profile("gjk double 4d", profile_gjkIntersect<double,4>, 100000);
    profile("gjk double 5d", profile_gjkIntersect<double,5>, 100000);
    std::cout << std::endl;

    profile("SAT 2D OBB double", profile_OBB_intersect<double,2>, 1000000);
    profile("SAT 3D OBB double", profile_OBB_intersect<double,3>, 1000000);
    std::cout << std::endl;

    std::cout << "sizeof vec2f: " << sizeof(Vec<float,2>) << std::endl;
    std::cout << "sizeof vec3f: " << sizeof(Vec<float,3>) << std::endl;
    std::cout << "sizeof vec4f: " << sizeof(Vec<float,4>) << std::endl;
    std::cout << "sizeof vec2d: " << sizeof(Vec<double,2>) << std::endl;
    std::cout << "sizeof vec3d: " << sizeof(Vec<double,3>) << std::endl;
    std::cout << "sizeof vec4d: " << sizeof(Vec<double,4>) << std::endl;

    try {
        test_matrix_asplode();
    } catch (GeomException &ex) {
        std::cout << ex.what() << std::endl << std::endl;
    }

    /*
    std::cout << "Mersenne Twister" << std::endl << "================================" << std::endl;
    RandomBattery bat = RandomBattery(new MTRand());
    bat.runAll(iters);
    std::cout << std::endl;

    std::cout << "Linear Congruential" << std::endl << "================================" << std::endl;
    bat = RandomBattery(new LCRand());
    bat.runAll(iters);
    std::cout << std::endl;
    */

    std::cout << "done." << std::endl;

    return 0;
}

#endif