#pragma once
#include <algorithm>    // std::sort
#include <boost/bimap.hpp>
#include <itkImage.h>
#include "ImageUtils.h"
#include "Log.h"
template<class ImageType>
class SegmentationMapper{
public:

    typedef typename ImageType::Pointer ImagePointerType;
    typedef typename ImageType::ConstPointer ConstImagePointerType;
    typedef boost::bimap<int,int> MapType;
    typedef MapType::value_type MapValueType;
    typedef typename ImageUtils<ImageType>::ImageIteratorType ImageIteratorType;
    typedef typename ImageUtils<ImageType>::ConstImageIteratorType ConstImageIteratorType;
private:
    MapType m_map;
    int m_nLabels;
public:
    ImagePointerType FindMapAndApplyMap(ImagePointerType input){
        return FindMapAndApplyMap((ConstImagePointerType)input);
    }
    ImagePointerType FindMapAndApplyMap(ConstImagePointerType input){

        ImagePointerType result=ImageUtils<ImageType>::duplicateConst(input);

        ConstImageIteratorType inputIt(input,input->GetLargestPossibleRegion());
        ImageIteratorType resultIt(result,input->GetLargestPossibleRegion());

        m_map=MapType();
        m_nLabels=0;
        std::vector<int> labelList(0);
        LOGV(2)<<"Mapping segmentation labels to a continuous discrete range... "<<std::endl;
        //build mmap
        for (inputIt.GoToBegin();!inputIt.IsAtEnd();++ inputIt){
            int l=inputIt.Get();
            if (m_map.left.find(l)==m_map.left.end()){
                m_map.insert(MapValueType(l,m_nLabels));
                labelList.push_back(l);
                ++m_nLabels;
            }
        }
        //order preserving mapping!
        std::sort(labelList.begin(),labelList.end());
        m_map=MapType();
        for (int n=0;n<m_nLabels;++n){
            m_map.insert(MapValueType(labelList[n],n));
            LOGV(5)<<"Mapping label "<<labelList[n]<<" to "<<n<<std::endl;

        }

        for (resultIt.GoToBegin();!resultIt.IsAtEnd();++ resultIt){
            int l=resultIt.Get();
            if (m_map.left.find(l)==m_map.left.end()){
                LOGV(4)<<"could not find map for label "<<l<<std::endl;
                resultIt.Set(0);
            }else{
                resultIt.Set(m_map.left.find(l)->second);
            }
        }
        LOGV(2)<<"Found "<<m_nLabels<<" segmentation labels."<<std::endl;
        return result;
    }
    void FindMap(ImagePointerType input){

        ImagePointerType result=ImageUtils<ImageType>::duplicate(input);

        ConstImageIteratorType inputIt(input,input->GetLargestPossibleRegion());

        m_map=MapType();
        m_nLabels=0;
        std::vector<int> labelList(0);
        LOGV(2)<<"Mapping segmentation labels to a continuous discrete range... "<<std::endl;
        //build mmap
        for (inputIt.GoToBegin();!inputIt.IsAtEnd();++ inputIt){
            int l=inputIt.Get();
            if (m_map.left.find(l)==m_map.left.end()){
                m_map.insert(MapValueType(l,m_nLabels));
                labelList.push_back(l);
                ++m_nLabels;
            }
        }
        //order preserving mapping!
        std::sort(labelList.begin(),labelList.end());
        m_map=MapType();
        for (int n=0;n<m_nLabels;++n){
            m_map.insert(MapValueType(labelList[n],n));
            LOGV(5)<<"Mapping label "<<labelList[n]<<" to "<<n<<std::endl;

        }

        LOGV(2)<<"Found "<<m_nLabels<<" segmentation labels."<<std::endl;
    }
    ImagePointerType ApplyMap(ImagePointerType input){
        return ApplyMap((ConstImagePointerType)input);
    }
    ImagePointerType ApplyMap(ConstImagePointerType input){
        if (m_map.size() ==0){
            return FindMapAndApplyMap(input);
        }
        ImagePointerType result=ImageUtils<ImageType>::duplicateConst(input);
        ImageIteratorType resultIt(result,input->GetLargestPossibleRegion());
        for (resultIt.GoToBegin();!resultIt.IsAtEnd();++ resultIt){
            int l=resultIt.Get();
             if (m_map.left.find(l)==m_map.left.end()){
                LOG<<"could not find map for label "<<l<<std::endl;
                resultIt.Set(0);
            }else{
                resultIt.Set(m_map.left.find(l)->second);
            }        }
        return result;
    }

    int GetInverseMappedLabel(int l){
         if (m_map.right.find(l)==m_map.right.end()){
                LOG<<"could not find map for label "<<l<<std::endl;
         }
        return m_map.right.find(l)->second;
    }

    ImagePointerType MapInverse(ImagePointerType input){
        ImagePointerType result=ImageUtils<ImageType>::duplicate(input);
        ImageIteratorType resultIt(result,input->GetLargestPossibleRegion());
        for (resultIt.GoToBegin();!resultIt.IsAtEnd();++ resultIt){
            int l=resultIt.Get();
            if (m_map.right.find(l)==m_map.right.end()){
                LOGV(3)<<"could not find map for label "<<l<<std::endl;
                resultIt.Set(0);
            }else{
                resultIt.Set(m_map.right.find(l)->second);
            }
        }
           return result;

    }
    int getNumberOfLabels(){return m_nLabels;}

};//class SegmentationMapper


#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs = boost::simd;

int main()
{
    bs::pack<float, 4> p{1.f, 2.f, 3.f, 4.f};
    std::cout << p + 10 * p << "\n";

    return 0;
}


#include <algorithm>
#include <limits>
#include <unordered_map>
#include <cassert>

#include <boost/lambda/lambda.hpp>

#include "MaximumLikelihood.h"
#include "Likelihood.h"
#include "FunctionMinimization.h"
#include "CompositeMinimization.h"

using namespace boost::lambda;

namespace Grante {

MaximumLikelihood::MaximumLikelihood(FactorGraphModel* fg_model)
	: ParameterEstimationMethod(fg_model), opt_method(LBFGSMethod) {
}

MaximumLikelihood::~MaximumLikelihood() {
}

void MaximumLikelihood::SetOptimizationMethod(
	MLEOptimizationMethod opt_method) {
	this->opt_method = opt_method;
}

MaximumLikelihood::MLEProblem* MaximumLikelihood::GetLearnProblem() {
	return (new MLEProblem(this));
}

double MaximumLikelihood::Train(double conv_tol, unsigned int max_iter) {
	// Minimize the 1/N negative log-likelihood objective
	MLEProblem mle_prob(this);
	std::vector<double> x_opt;

	FunctionMinimization::CheckDerivative(mle_prob, 1.0e-2, 4, 1.0e-8, 1.0e-2);

	// Use rank-200 approximation to inverse Hessian.  Contrary to popular
	// belief ("ten to twenty vectors are enough for L-BFGS"), for likelihood
	// objectives of discrete models this really makes a big difference
	// because evaluating the objective is rather expensive.
	PrintProblemStatistics();
	double obj = std::numeric_limits<double>::signaling_NaN();
	switch (opt_method) {
	case (LBFGSMethod):
		obj = FunctionMinimization::LimitedMemoryBFGSMinimize(
			mle_prob, x_opt, conv_tol, max_iter, true, 200);
		break;
	case (SimpleGradientMethod):
		obj = FunctionMinimization::GradientMethodMinimize(mle_prob,
			x_opt, conv_tol, max_iter, true);
		break;
	case (BarzilaiBorweinMethod):
		obj = FunctionMinimization::BarzilaiBorweinMinimize(
			mle_prob, x_opt, conv_tol, max_iter, true);
		break;
	case (FISTAMethod):
		obj = CompositeMinimization::FISTAMinimize(
			mle_prob, x_opt, conv_tol, max_iter, true);
		break;
	default:
		assert(0);
		break;
	}

	// Save optimal weights into model
	mle_prob.LinearToFactorWeights(x_opt);

	return (obj);
}

// Function minimization part
MaximumLikelihood::MLEProblem::MLEProblem(MaximumLikelihood* mle_base)
	: mle_base(mle_base) {
	// Compute dimension once and setup factor type orders
	dim = 0;
	const std::vector<FactorType*>& factor_types =
		mle_base->fg_model->FactorTypes();
	for (std::vector<FactorType*>::const_iterator fti = factor_types.begin();
		fti != factor_types.end(); ++fti) {
		dim += (*fti)->WeightDimension();
		parameter_order.push_back((*fti)->Name());
	}
}

MaximumLikelihood::MLEProblem::~MLEProblem() {
}

double MaximumLikelihood::MLEProblem::EvalF(const std::vector<double>& x,
	std::vector<double>& grad) {
	assert(x.size() == dim);
	assert(grad.empty() || grad.size() == dim);

	// 1. Convert x into factor parameters
	LinearToFactorWeights(x);

	// 2. Setup parameter gradient
	std::unordered_map<std::string, std::vector<double> >
		parameter_gradient;
	SetupParameterGradient(parameter_gradient);

	// 3. Compute likelihood related parameter gradient:
	//    \sum_{n=1}^N \nabla_w -log p(x_n;w)
	double nll = EvaluateLikelihoodGradient(parameter_gradient);

	// Scale by 1/N to have: 1/N (\sum_{n=1}^N \nabla_w - log p(x_n;w))
	double scale = 1.0 / static_cast<double>(mle_base->training_data.size());
	for (std::vector<std::string>::const_iterator
		ft_name = parameter_order.begin();
		ft_name != parameter_order.end(); ++ft_name) {
		std::transform(parameter_gradient[*ft_name].begin(),
			parameter_gradient[*ft_name].end(),
			parameter_gradient[*ft_name].begin(), scale * _1);
	}
	nll *= scale;

	// 4. Convert gradient into linear form
	if (grad.empty() == false)
		AddParameterGradient(parameter_gradient, grad);

	return (nll);
}

double MaximumLikelihood::MLEProblem::EvalG(const std::vector<double>& x,
	std::vector<double>& subgrad) {
	assert(x.size() == dim);
	assert(subgrad.empty() || subgrad.size() == dim);
	LinearToFactorWeights(x);

	// Scaling factor
	double scale = 1.0 / static_cast<double>(mle_base->training_data.size());

	// 1. Setup parameter gradient
	std::unordered_map<std::string, std::vector<double> >
		parameter_gradient;
	SetupParameterGradient(parameter_gradient);

	// 2. Add regularizer
	double nll = 0.0;
	for (std::multimap<std::string, Prior*>::const_iterator
		prior = mle_base->priors.begin();
		prior != mle_base->priors.end(); ++prior)
	{
		FactorType* ft = mle_base->fg_model->FindFactorType(prior->first);
		nll += prior->second->EvaluateNegLogP(ft->Weights(),
			parameter_gradient[prior->first], scale);
	}

	// 2. Convert gradient into linear form
	if (subgrad.empty() == false)
		AddParameterGradient(parameter_gradient, subgrad);

	return (nll);
}

void MaximumLikelihood::MLEProblem::EvalGProximalOperator(
	const std::vector<double>& u, double L,
	std::vector<double>& wprox) const {
	assert(u.size() == dim);
	assert(wprox.size() == dim);

	// Hot-path: no prior, simply copy u
	wprox = u;
	if (mle_base->priors.empty())
		return;

	// Compute scaling factor used to adjust L for all proximal problems
	double scale = 1.0 / static_cast<double>(mle_base->training_data.size());
	L /= scale;

	// Setup vectors of appropriate size
	std::unordered_map<std::string, std::vector<double> >
		parameter_gradient;
	SetupParameterGradient(parameter_gradient);

	// Proximal problem decomposes over priors, solve it separately
	size_t base_idx = 0;
	for (std::vector<std::string>::const_iterator
		ft_name = parameter_order.begin();
		ft_name != parameter_order.end(); ++ft_name) {
		// Find prior
		std::multimap<std::string, Prior*>::const_iterator pri =
			mle_base->priors.find(*ft_name);
		if (pri == mle_base->priors.end())
			continue;	// skip, no prior for this type

		// Obtain suitably sized vector
		std::unordered_map<std::string,
			std::vector<double> >::const_iterator pgi =
				parameter_gradient.find(*ft_name);
		assert(pgi != parameter_gradient.end());

		// Create a copy of the subvector of u
		std::vector<double> u_pgi(u.begin() + base_idx,
			u.begin() + base_idx + pgi->second.size());
		std::vector<double> wprox_pgi(u_pgi.size(), 0.0);

		pri->second->EvaluateProximalOperator(u_pgi, L, wprox_pgi);
		std::copy(wprox_pgi.begin(), wprox_pgi.end(),
			wprox.begin() + base_idx);
		base_idx += pgi->second.size();
	}
}

double MaximumLikelihood::MLEProblem::EvaluateLikelihoodGradient(
	std::unordered_map<std::string, std::vector<double> >&
		parameter_gradient) {
	// For each sample: run forward map, run inference, compute gradient
	Likelihood lh(mle_base->fg_model);
	double nll = 0.0;
	int training_sample_count =
		static_cast<int>(mle_base->training_data.size());

	#pragma omp parallel for schedule(dynamic)
	for (int n = 0; n < training_sample_count; ++n) {
		// Get sample
		FactorGraph* ts_fg = mle_base->training_data[n].first;
		const FactorGraphObservation* ts_obs =
			mle_base->training_data[n].second;

		// Compute forward map: parameters (changed) to energies
		ts_fg->ForwardMap();

		// Compute marginals
		InferenceMethod* ts_inf = mle_base->inference_methods[n];
		ts_inf->ClearInferenceResult();
		ts_inf->PerformInference();
		const std::vector<std::vector<double> >& marginals =
			ts_inf->Marginals();
		double log_z = ts_inf->LogPartitionFunction();

		#pragma omp critical
		{
			// Compute likelihood and gradient
			nll += lh.ComputeNegLogLikelihood(ts_fg, ts_obs, marginals,
				log_z, parameter_gradient);
		}

		// Get rid of marginals
		ts_inf->ClearInferenceResult();
		ts_fg->EnergiesRelease();
	}
	return (nll);
}

void MaximumLikelihood::MLEProblem::SetupParameterGradient(
	std::unordered_map<std::string, std::vector<double> >&
		parameter_gradient) const {
	for (std::vector<std::string>::const_iterator
		ft_name = parameter_order.begin();
		ft_name != parameter_order.end(); ++ft_name) {
		FactorType* ft = mle_base->fg_model->FindFactorType(*ft_name);

		// Initialize to zero
		parameter_gradient[*ft_name] =
			std::vector<double>(ft->WeightDimension(), 0.0);
	}
}

void MaximumLikelihood::MLEProblem::AddParameterGradient(
	const std::unordered_map<std::string, std::vector<double> >&
		parameter_gradient, std::vector<double>& grad) const {
	size_t base_idx = 0;
	for (std::vector<std::string>::const_iterator
		ft_name = parameter_order.begin();
		ft_name != parameter_order.end(); ++ft_name) {
		std::unordered_map<std::string,
			std::vector<double> >::const_iterator pgi =
				parameter_gradient.find(*ft_name);
		assert(pgi != parameter_gradient.end());
		std::transform(pgi->second.begin(), pgi->second.end(),
			grad.begin() + base_idx, grad.begin() + base_idx,
			[](double pge, double ge) -> double { return (pge + ge); });
		base_idx += pgi->second.size();
	}
}

void MaximumLikelihood::MLEProblem::LinearToFactorWeights(
	const std::vector<double>& x) {
	unsigned int base_idx = 0;
	for (std::vector<std::string>::const_iterator
		ft_name = parameter_order.begin();
		ft_name != parameter_order.end(); ++ft_name) {
		// Get factor type
		FactorType* ft = mle_base->fg_model->FindFactorType(*ft_name);
		unsigned int ft_w_len = ft->WeightDimension();
		std::copy(x.begin() + base_idx, x.begin() + base_idx + ft_w_len,
			ft->Weights().begin());
		base_idx += ft_w_len;
	}
	assert(base_idx == x.size());
}

unsigned int MaximumLikelihood::MLEProblem::Dimensions() const {
	return (dim);
}

void MaximumLikelihood::MLEProblem::ProvideStartingPoint(
	std::vector<double>& x0) const {
	assert(x0.size() == dim);

	// Initial parameters are user-provided
	unsigned int base_idx = 0;
	for (std::vector<std::string>::const_iterator
		ft_name = parameter_order.begin();
		ft_name != parameter_order.end(); ++ft_name) {
		// Get factor type
		FactorType* ft = mle_base->fg_model->FindFactorType(*ft_name);
		unsigned int ft_w_len = ft->WeightDimension();
		std::copy(ft->Weights().begin(), ft->Weights().end(),
			x0.begin() + base_idx);
		base_idx += ft_w_len;
	}
	assert(base_idx == x0.size());
}

}


#include "metrics.h"

#include <boost/format.hpp>

std::vector<Eigen::Vector3d> sample_points_on_sphere(const int v_vertical_number)
{
	std::vector<Eigen::Vector3d> test_vecs;
	double theta_interval = M_PI / v_vertical_number;
	for (int i = 0; i < v_vertical_number; ++i)
	{
		double theta = i * theta_interval;
		int m = static_cast<int>(std::max(std::floor(v_vertical_number * std::sin(theta)), 1.));
		double fi_interval = 2 * M_PI / m;
		for (int j = 0; j < m; ++j)
		{
			double fi = fi_interval * j;
			test_vecs.emplace_back(std::sin(theta) * std::sin(fi), std::cos(theta), std::sin(theta) * std::cos(fi));
		}
	}
	return test_vecs;
}

std::vector<std::vector<bool>> compute_visibility(const std::vector<Viewpoint>& v_viewpoints,
	const SurfaceMesh& v_mesh, const PointSet3& point_set, const double v_fov_degree_h, const double v_fov_degree_v, const double v_dmax, Tree* v_tree)
{
	std::vector<std::vector<bool>> point_view_visibility;
	point_view_visibility.resize(point_set.size(), std::vector<bool>(v_viewpoints.size(), false));

	// Build AABB tree
	if (v_tree == nullptr)
	{
		v_tree = new Tree(CGAL::faces(v_mesh).first, CGAL::faces(v_mesh).second, v_mesh);
		v_tree->build();
	}

#pragma omp parallel for
	for (int i_point = 0; i_point < point_set.size(); ++i_point)
	{
		const Point3& point = point_set.point(i_point);

		//#pragma omp parallel for
		for (int j = 0; j < v_viewpoints.size(); j++)
		{
			if (intersectiontools::is_visible(v_viewpoints[j].pos_mesh, v_viewpoints[j].direction, cgaltools::cgal_point_2_eigen(point), *v_tree, v_fov_degree_h, v_fov_degree_v, v_dmax))
				point_view_visibility[i_point][j] = true;
		}
	}
	return point_view_visibility;
}

std::vector<std::vector<bool>> compute_visibility(
	const std::vector<Viewpoint>& v_viewpoints, const SurfaceMesh& v_mesh,
	const PointSet3& point_set,
	const Eigen::Matrix3d& v_intrinsic_matrix,
	const double v_dmax, Tree* v_tree)
{
	std::vector<std::vector<bool>> point_view_visibility;
	point_view_visibility.resize(point_set.size(), std::vector<bool>(v_viewpoints.size(), false));

	// Build AABB tree
	if (v_tree == nullptr)
	{
		v_tree = new Tree(CGAL::faces(v_mesh).first, CGAL::faces(v_mesh).second, v_mesh);
		v_tree->build();
	}

#pragma omp parallel for
	for (int i_point = 0; i_point < point_set.size(); ++i_point)
	{
		const Point3& point = point_set.point(i_point);

		//#pragma omp parallel for
		for (int j = 0; j < v_viewpoints.size(); j++)
		{

			if (intersectiontools::is_visible(
				v_intrinsic_matrix,
				MapConverter::get_pos_pack_from_direction_vector(v_viewpoints[j].pos_mesh, v_viewpoints[j].direction).camera_matrix, v_viewpoints[j].pos_mesh,
				cgaltools::cgal_point_2_eigen(point), *v_tree, v_dmax)
				) {
#pragma omp critical
					{
						point_view_visibility[i_point][j] = true;
					}
			}
		}
	}
	return point_view_visibility;
}

std::vector<std::vector<bool>> compute_visibility(
	const std::vector<Viewpoint>& v_viewpoints, const SurfaceMesh& v_mesh,
	const PointSet3& point_set,
	const Eigen::Matrix3d& v_intrinsic_matrix,
	const double v_dmax, const RTCScene& v_embree_scene)
{
	std::vector<std::vector<bool>> point_view_visibility;
	point_view_visibility.resize(point_set.size(), std::vector<bool>(v_viewpoints.size(), false));

#pragma omp parallel for
	for (int i_point = 0; i_point < point_set.size(); ++i_point)
	{
		const Point3& point = point_set.point(i_point);

		//#pragma omp parallel for
		for (int j = 0; j < v_viewpoints.size(); j++)
		{

			if (intersectiontools::is_visible(
				v_intrinsic_matrix,
				MapConverter::get_pos_pack_from_direction_vector(v_viewpoints[j].pos_mesh, v_viewpoints[j].direction).camera_matrix, v_viewpoints[j].pos_mesh,
				cgaltools::cgal_point_2_eigen(point), v_embree_scene, v_dmax, v_mesh)
				) {
#pragma omp critical
					{
						point_view_visibility[i_point][j] = true;
					}
			}
		}
	}
	return point_view_visibility;
}

std::vector<std::vector<int>> compute_visibilityIndex(
	const std::vector<Viewpoint>& v_viewpoints, const SurfaceMesh& v_mesh,
	const PointSet3& point_set,
	const Eigen::Matrix3d& v_intrinsic_matrix,
	const double v_dmax, Tree* v_tree)
{
	std::vector<std::vector<int>> point_view_visibility(point_set.size());

	// Build AABB tree
	if (v_tree == nullptr)
	{
		v_tree = new Tree(CGAL::faces(v_mesh).first, CGAL::faces(v_mesh).second, v_mesh);
		v_tree->build();
	}

#pragma omp parallel for
	for (int i_point = 0; i_point < point_set.size(); ++i_point)
	{
		const Point3& point = point_set.point(i_point);

		for (int j = 0; j < v_viewpoints.size(); j++)
		{

			if (intersectiontools::is_visible(
				v_intrinsic_matrix,
				MapConverter::get_pos_pack_from_direction_vector(v_viewpoints[j].pos_mesh, v_viewpoints[j].direction).camera_matrix, v_viewpoints[j].pos_mesh,
				cgaltools::cgal_point_2_eigen(point), *v_tree, v_dmax)
				) {
#pragma omp critical
					{
						point_view_visibility.at(i_point).push_back(j);
					}
			}
		}
	}
	return point_view_visibility;
}

std::vector<std::vector<int>> compute_visibilityIndex(
	const std::vector<Viewpoint>& v_viewpoints, const SurfaceMesh& v_mesh,
	const PointSet3& point_set,
	const Eigen::Matrix3d& v_intrinsic_matrix,
	const double v_dmax, const RTCScene& v_embree_scene)
{
	std::vector<std::vector<int>> point_view_visibility(point_set.size());

#pragma omp parallel for
	for (int i_point = 0; i_point < point_set.size(); ++i_point)
	{
		const Point3& point = point_set.point(i_point);

		for (int j = 0; j < v_viewpoints.size(); j++)
		{

			if (intersectiontools::is_visible(
				v_intrinsic_matrix,
				MapConverter::get_pos_pack_from_direction_vector(v_viewpoints[j].pos_mesh, v_viewpoints[j].direction).camera_matrix, v_viewpoints[j].pos_mesh,
				cgaltools::cgal_point_2_eigen(point), v_embree_scene, v_dmax, v_mesh)
				) {
#pragma omp critical
					{
						point_view_visibility.at(i_point).push_back(j);
					}
			}
		}
	}
	return point_view_visibility;
}

std::array<double, 5> calculate_point_reconstructability(const Viewpoint& v_view1, const Viewpoint& v_view2,
	const Eigen::Vector3d& v_point, const Eigen::Vector3d& v_normal, const double dmax)
{
	int k1 = 32;
	double alpha1 = 3.1415926f / 16;
	int k3 = 8;
	double alpha3 = 3.1415926f / 4;

	Eigen::Vector3d view_to_point1 = v_point - v_view1.pos_mesh;
	Eigen::Vector3d view_to_point2 = v_point - v_view2.pos_mesh;

	double alpha = std::acos(std::clamp(static_cast<double>(view_to_point1.normalized().dot(view_to_point2.normalized())), -1., 1.));
	double omega1 = 1. / (1 + std::exp(-k1 * (alpha - alpha1)));
	double omega2 = 1 - std::min(std::max(view_to_point1.norm(), view_to_point2.norm()) / dmax, 1.);
	double omega3 = 1. - 1. / (1 + std::exp(-k3 * (alpha - alpha3)));
	double Theta1 = (-view_to_point1).normalized().dot(v_normal);
	double Theta2 = (-view_to_point2).normalized().dot(v_normal);
	double cosTheta = std::min(Theta1, Theta2);
	double value = omega1 * omega2 * omega3 * cosTheta;
	return std::array<double, 5>{omega1, omega2, omega3, cosTheta, value > 0 ? value : 0};
}
double biviewsREC(const Eigen::Vector3d& v_view1, const Eigen::Vector3d& v_view2,
	const Eigen::Vector3d& v_point, const Eigen::Vector3d& v_normal, const double dmax)
{

	const Eigen::Vector3d view_to_point1 = v_point - v_view1;
	const Eigen::Vector3d view_to_point2 = v_point - v_view2;

	const double alpha = std::acos(std::clamp(static_cast<double>(view_to_point1.normalized().dot(view_to_point2.normalized())), -1., 1.));
	const double omega2 = 1 - std::min(std::max(view_to_point1.norm(), view_to_point2.norm()) / dmax, 1.);
	const double cosTheta = std::min((-view_to_point1).normalized().dot(v_normal), (-view_to_point2).normalized().dot(v_normal));
	const double value = (1. / (1 + std::exp(-32 * (alpha - M_PI / 16.)))) * omega2 * (1. - 1. / (1 + std::exp(-8 * (alpha - M_PI / 4.)))) * cosTheta;
	return value > 0 ? value : 0;
}

std::vector<std::array<double, 5>> reconstructability_hueristic(std::vector<Viewpoint> trajectory,
	const PointSet3& point_set,
	const SurfaceMesh& v_mesh,
	std::vector<std::vector<bool>>& point_view_visibility,
	const double dmax, const double v_fov_degree_h, const double v_fov_degree_v,
	Tree* v_tree)
{
	if (!point_set.has_normal_map())
	{
		std::cout << "Point set do not have normals" << std::endl;
		throw;
	}
	std::vector<std::array<double, 5>> reconstructabilities(point_set.size());

	if (point_view_visibility.size() == 0)
	{
		point_view_visibility = compute_visibility(trajectory, v_mesh, point_set, v_fov_degree_h, v_fov_degree_v, dmax, v_tree);
	}
	else if (point_view_visibility.size() != point_set.size())
	{
		std::cout << "Point set size does not match visibility array size" << std::endl;
		throw;
	}

	// Build AABB tree
	if (v_tree == nullptr)
	{
		v_tree = new Tree(CGAL::faces(v_mesh).first, CGAL::faces(v_mesh).second, v_mesh);
		v_tree->build();
	}

	int log_step = 0;
	//#pragma omp parallel for
	for (int i_point = 0; i_point < point_set.size(); ++i_point)
	{
		const Point3& point = point_set.point(i_point);
		const Vector3& normal = point_set.normal(i_point) / std::sqrt(point_set.normal(i_point).squared_length());

		std::vector<std::array<double, 5>> point_recon;
#pragma omp parallel for
		for (int id_view1 = 0; id_view1 < trajectory.size(); id_view1++)
		{
			if (!point_view_visibility[i_point][id_view1])
				continue;
			for (int id_view2 = id_view1 + 1; id_view2 < trajectory.size(); id_view2++)
			{
				const Eigen::Vector3d point_eigen(point.x(), point.y(), point.z());
				Eigen::Vector3d normal_eigen(normal.x(), normal.y(), normal.z());
				if (!point_view_visibility[i_point][id_view2])
					continue;
#pragma omp critical
				{
					point_recon.push_back(calculate_point_reconstructability(trajectory[id_view1], trajectory[id_view2], point_eigen, normal_eigen, dmax));
				}
			}
		}

		//std::cout<<"Reconstructability Smith18 "<< std::endl;
		//  int log_step=0;
		//for (int id_view1 = 0; id_view1 < trajectory.size(); id_view1++)
		//{
		//	for (int id_view2 = id_view1+1; id_view2 < trajectory.size(); id_view2++)
		//		std::cout<<(boost::format("%3d, ")%std::get<4>(point_recon[log_step++])).str();
		//	std::cout<<std::endl;
		//}

		std::array<double, 5> total_recon = std::accumulate(point_recon.begin(), point_recon.end(), std::array<double, 5>{0., 0., 0., 0., 0.}, [](std::array<double, 5> sum, auto item)
			{
				sum[0] += std::get<0>(item);
				sum[1] += std::get<1>(item);
				sum[2] += std::get<2>(item);
				sum[3] += std::get<3>(item);
				sum[4] += std::get<4>(item);
				return sum;
			});

		//double num_view_pair = trajectory.size()* (trajectory.size()-1) / 2;
		double num_view_pair = 1;
		reconstructabilities[i_point] = std::array<double, 5>{
			total_recon[0] / num_view_pair,
				total_recon[1] / num_view_pair,
				total_recon[2] / num_view_pair,
				total_recon[3] / num_view_pair,
				(total_recon[4] > 0 ? total_recon[4] : 0) / num_view_pair};

#pragma omp critical
		{
			if (log_step != 0 && log_step % (point_set.size() / 10) == 0)
				LOG(INFO) << log_step << " / " << point_set.size();
			log_step += 1;
		}
	}

	return reconstructabilities;
}

double normal_distribution(const double v_x, const double v_exponential, double v_sigma)
{
	return std::exp(-std::pow(v_x, v_exponential) / 2 / v_sigma / v_sigma);
}

struct Default_lalala_parameters
{
	double alpha_weights_sigma1 = 5. / 90.;
	double alpha_weights_exponential1 = 2.;
	double alpha_weights_sigma2 = 30. / 90.;
	double alpha_weights_exponential2 = 2.;

	double gsd_weights_sigma = 1.5;
	double gsd_weights_exponential = 4.;

	double scale_weights_sigma = .25;
	double scale_weights_exponential = 2.;

	double angle_to_normal_weights_sigma = 30. / 90.;
	double angle_to_normal_weights_exponential = 2.;

	double distortion_weights_sigma = 1.;
	double distortion_weights_exponential = 2.;
};

std::vector<double> compute_reconstructability_lalala_item(const Viewpoint& v_view1, const Viewpoint& v_view2,
	const Eigen::Vector3d& v_point, const Eigen::Vector3d& v_normal, const double v_max_distance,
	const double v_fov_degree_h, const double v_fov_degree_v)
{
	Default_lalala_parameters default_lalala_parameters;
	// Base ability for triangulation and feature matching
	// Both in Furukawa et al.[2010] and Smith et al.[2018]
	Eigen::Vector3d view_to_point1 = v_point - v_view1.pos_mesh;
	Eigen::Vector3d view_to_point2 = v_point - v_view2.pos_mesh;
	double alpha_in_degree = std::acos(std::clamp(view_to_point1.normalized().dot(view_to_point2.normalized()), -1., 1.)) / M_PI * 180.;
	double base_score; // alpha ~ [0,180] -> base_score ~ [0,1]
	if (alpha_in_degree < 20.)
		base_score = normal_distribution(alpha_in_degree / 90., default_lalala_parameters.alpha_weights_exponential1, default_lalala_parameters.alpha_weights_sigma1); // Furukawaet al.[2010]
	else
		base_score = normal_distribution(alpha_in_degree / 90., default_lalala_parameters.alpha_weights_exponential2, default_lalala_parameters.alpha_weights_sigma2); // Furukawaet al.[2010] Modified the parameter

	double distance1 = view_to_point1.norm();
	double distance2 = view_to_point2.norm();
	// GSD
	// distance / max_distance = 1 -> GSD_score = 0.7f
	// distance / max_distance = 1.5 -> GSD_score = 0.15f
	// distance / max_distance = 2 -> GSD_score = 0.01f
	double GSD_score = normal_distribution(distance1 / v_max_distance, default_lalala_parameters.gsd_weights_exponential, default_lalala_parameters.gsd_weights_sigma) *
		normal_distribution(distance2 / v_max_distance, default_lalala_parameters.gsd_weights_exponential, default_lalala_parameters.gsd_weights_sigma);

	// Scale factor for feature matching
	// min(distance1,distance2) / max(distance1,distance2)
	// 0.2 -> scale_score = 0.9
	// 0.5 -> scale_score = 0.6
	// 0.75 -> scale_score = 0.3
	// 1 -> scale_score = 0.15
	double scale_score = normal_distribution(1 - std::min(distance1, distance2) / std::max(distance1, distance2),
		default_lalala_parameters.scale_weights_exponential, default_lalala_parameters.scale_weights_sigma);

	// Direction to point normal
	double angle_to_normal1 = std::acos(std::clamp(static_cast<double>(-(view_to_point1.normalized()).dot(v_normal.normalized())), -1., 1.)) / M_PI * 180.;
	double angle_to_normal2 = std::acos(std::clamp(static_cast<double>(-(view_to_point2.normalized()).dot(v_normal.normalized())), -1., 1.)) / M_PI * 180.;
	double normal_score = normal_distribution(angle_to_normal1 / 90.,
		default_lalala_parameters.angle_to_normal_weights_exponential, default_lalala_parameters.angle_to_normal_weights_sigma) * normal_distribution(angle_to_normal2 / 90.,
			default_lalala_parameters.angle_to_normal_weights_exponential, default_lalala_parameters.angle_to_normal_weights_sigma);

	// Direction of lens distortion
	// 0.1 (fov/2) -> disortion_score = 1
	// 0.5 (fov/2) -> disortion_score = 0.9
	// 0.75 (fov/2) -> disortion_score = 0.5
	// 0.9 (fov/2) -> disortion_score = 0.2
	double angle_to_view_direction1 = std::acos(std::clamp(static_cast<double>(view_to_point1.normalized().dot(v_view1.direction.normalized())), -1., 1.)) / (v_fov_degree_v / 180. * M_PI);
	double angle_to_view_direction2 = std::acos(std::clamp(static_cast<double>(view_to_point2.normalized().dot(v_view2.direction.normalized())), -1., 1.)) / (v_fov_degree_v / 180. * M_PI);
	double disortion_score = normal_distribution(angle_to_view_direction1,
		default_lalala_parameters.distortion_weights_exponential, default_lalala_parameters.distortion_weights_sigma) * normal_distribution(angle_to_view_direction2,
			default_lalala_parameters.distortion_weights_exponential, default_lalala_parameters.distortion_weights_sigma);

	double value = base_score * GSD_score * scale_score * normal_score * disortion_score;
	return std::vector<double>{base_score, GSD_score, scale_score, normal_score, disortion_score, value};
}

void find_min_max_path(std::vector<Eigen::MatrixXd>& v_distance_matrix, const int max_depth)
{
	const int num_views = v_distance_matrix[0].rows();
	for (int depth = 1; depth < max_depth; ++depth)
	{
		for (int i_start = 0; i_start < num_views - 1; ++i_start)
		{
			for (int i_target = i_start + 1; i_target < num_views; ++i_target)
			{
				for (int i_neighbour = 0; i_neighbour < num_views; i_neighbour++)
				{
					if (i_neighbour == i_start || i_neighbour == i_target)
						continue;
					v_distance_matrix[depth](i_start, i_target) = std::max(v_distance_matrix[depth](i_start, i_target), std::max(v_distance_matrix[depth - 1](i_start, i_target),
						std::min(v_distance_matrix[depth - 1](i_start, i_neighbour), v_distance_matrix[depth - 1](i_neighbour, i_target))));
					v_distance_matrix[depth](i_target, i_start) = v_distance_matrix[depth](i_start, i_target);
				}
			}
		}
	}
	return;
}

double compute_reconstructability_lalala(const std::vector<Viewpoint>& v_viewpoints, const Point3& v_point,
	const Vector3& v_normal, const double v_suggest_distance, const double v_fov_degree_h, const double v_fov_degree_v, const Eigen::Matrix3d& v_intrinsic_matrix,
	const int v_index, const int v_redundancy_density, const bool v_with_minmax, const bool v_log_txt)
{
	const Eigen::Vector3d point = cgaltools::cgal_point_2_eigen(v_point);
	double centre_theta = std::acos(std::clamp((double)v_normal.z(), -1., 1.)); // Notations in phisics, theta is responsible for z axis
	double centre_phi = std::atan2(v_normal.y(), v_normal.x());
	Eigen::Isometry3d rotation;
	rotation.setIdentity();
	rotation.rotate(Eigen::AngleAxisd(centre_phi, Eigen::Vector3d(0., 0., 1.)));
	rotation.rotate(Eigen::AngleAxisd(centre_theta, Eigen::Vector3d(0., 1., 0.)));
	double sqrt_2_2 = std::sqrt(2.) / 2;

	// Reduce redundancy
	std::vector<Viewpoint> viewpoints_after_reduce_redundancy;
	if (v_redundancy_density > 0)
	{
		std::vector<Eigen::Vector3d> supplementary_view = sample_points_on_sphere(v_redundancy_density);

		// Rotate the supplementary view
		for (int i = supplementary_view.size() - 1; i >= 0; --i)
		{
			if (supplementary_view[i].z() < 0)
				supplementary_view.erase(supplementary_view.begin() + i);
			supplementary_view[i] = rotation * supplementary_view[i];
		}
		std::vector<std::vector<int>> exist_distance(supplementary_view.size());

		// Calculate the nearest view for each supplementary view
#pragma omp parallel for
		for (int i_planned_view = 0; i_planned_view < v_viewpoints.size(); ++i_planned_view)
		{
			int nearest_supplementary_id = -1; double nearest_distance = -99999.;
			for (int i_supplementary_view = 0; i_supplementary_view < supplementary_view.size(); ++i_supplementary_view)
			{
				double distance = v_viewpoints[i_planned_view].direction.dot(-supplementary_view[i_supplementary_view]);
				if (distance > nearest_distance)
				{
					nearest_distance = distance;
					nearest_supplementary_id = i_supplementary_view;
				}
			}
#pragma omp critical
			{
				exist_distance[nearest_supplementary_id].push_back(i_planned_view);
			}
		}

		// Refine
		const double DISTANCE_STEP = v_suggest_distance / 2;
#pragma omp parallel for
		for (int i_supplementary_view = 0; i_supplementary_view < supplementary_view.size(); ++i_supplementary_view)
		{
			std::sort(exist_distance[i_supplementary_view].begin(), exist_distance[i_supplementary_view].end(),
				[&v_viewpoints](const int& item1, const int& item2) {return (v_viewpoints[item1].pos_mesh - v_viewpoints[item2].pos_mesh).norm(); });

			double min_distance = -DISTANCE_STEP;
			for (int i_planned_view = 0; i_planned_view < exist_distance[i_supplementary_view].size(); i_planned_view++)
			{
				if ((v_viewpoints[exist_distance[i_supplementary_view][i_planned_view]].pos_mesh - point).norm() > min_distance + DISTANCE_STEP)
				{
#pragma omp critical
					{
						viewpoints_after_reduce_redundancy.push_back(v_viewpoints[exist_distance[i_supplementary_view][i_planned_view]]);
					}
					min_distance = (v_viewpoints[exist_distance[i_supplementary_view][i_planned_view]].pos_mesh - point).norm();
				}
			}
		}
	}
	else
		viewpoints_after_reduce_redundancy = v_viewpoints;

	// Calculate base reconstructability
	std::vector<std::vector<std::vector<double>>> reconstructability_result(viewpoints_after_reduce_redundancy.size(), std::vector<std::vector<double>>(viewpoints_after_reduce_redundancy.size()));
#pragma omp parallel for
	for (int i_view1 = 0; i_view1 < viewpoints_after_reduce_redundancy.size(); ++i_view1)
	{
		for (int i_view2 = i_view1 + 1; i_view2 < viewpoints_after_reduce_redundancy.size(); ++i_view2)
		{
			auto reconstructability_base = compute_reconstructability_lalala_item(
				viewpoints_after_reduce_redundancy[i_view1], viewpoints_after_reduce_redundancy[i_view2], cgaltools::cgal_point_2_eigen(v_point),
				cgaltools::cgal_vector_2_eigen(v_normal), v_suggest_distance, v_fov_degree_h, v_fov_degree_v);
			reconstructability_result[i_view1][i_view2] = reconstructability_base;
			reconstructability_result[i_view2][i_view1] = reconstructability_base;
		}
	}

	// Find minmax path
	double total_reconstructability = 0.;
	if (v_with_minmax)
	{
		const int MAX_DEPTH = 5;
		std::vector<Eigen::MatrixXd> reconstructability_matrix(MAX_DEPTH, Eigen::MatrixXd(viewpoints_after_reduce_redundancy.size(), viewpoints_after_reduce_redundancy.size()));
		for (auto& item : reconstructability_matrix)
			item.fill(0.);
		for (int i_view1 = 0; i_view1 < viewpoints_after_reduce_redundancy.size(); ++i_view1)
			for (int i_view2 = 0; i_view2 < viewpoints_after_reduce_redundancy.size(); ++i_view2)
				if (i_view1 != i_view2)
					reconstructability_matrix[0](i_view1, i_view2) = reconstructability_result[i_view1][i_view2][5];
		find_min_max_path(reconstructability_matrix, MAX_DEPTH);
		for (int log_depth = 0; log_depth < MAX_DEPTH; ++log_depth)
		{
			//std::cout<<"Depth "<< log_depth<<std::endl;
			for (int i_start = 0; i_start < viewpoints_after_reduce_redundancy.size(); ++i_start)
			{
				for (int i_target = i_start + 1; i_target < viewpoints_after_reduce_redundancy.size(); ++i_target)
				{
					//std::cout<<(boost::format("%3d, ") % reconstructability_matrix[log_depth](i_start,i_target)).str();
					//total_reconstructability+=reconstructability_matrix[log_depth](i_start,i_target);
				}
				//std::cout<<std::endl;
			}
		}
		for (int i_start = 0; i_start < viewpoints_after_reduce_redundancy.size(); ++i_start)
			for (int i_target = i_start + 1; i_target < viewpoints_after_reduce_redundancy.size(); ++i_target)
				total_reconstructability += reconstructability_matrix[MAX_DEPTH - 1](i_start, i_target);
	}
	else
	{
		for (int i_start = 0; i_start < viewpoints_after_reduce_redundancy.size(); ++i_start)
			for (int i_target = i_start + 1; i_target < viewpoints_after_reduce_redundancy.size(); ++i_target)
				total_reconstructability += reconstructability_result[i_start][i_target][5];
	}

	// Log
	if (v_log_txt) {
		int resolution_x = static_cast<int>(v_intrinsic_matrix(0, 2) * 2);
		int resolution_y = static_cast<int>(v_intrinsic_matrix(1, 2) * 2);

		std::string out_str = "";
		out_str += std::to_string(viewpoints_after_reduce_redundancy.size()) + "\n";
		out_str += std::to_string(total_reconstructability) + "\n";
		for (int i_view1 = 0; i_view1 < viewpoints_after_reduce_redundancy.size(); ++i_view1)
		{
			auto pos_pack = MapConverter::get_pos_pack_from_direction_vector(viewpoints_after_reduce_redundancy[i_view1].pos_mesh, viewpoints_after_reduce_redundancy[i_view1].direction);
			auto pixel_position = v_intrinsic_matrix * pos_pack.camera_matrix * cgaltools::cgal_point_2_eigen(v_point);
			pixel_position /= pixel_position.z();

			Eigen::Vector3d view_to_point1 = cgaltools::cgal_point_2_eigen(v_point) - viewpoints_after_reduce_redundancy[i_view1].pos_mesh;
			double angle_to_normal1 = std::acos(std::clamp(static_cast<double>(-(view_to_point1.normalized()).dot(cgaltools::cgal_vector_2_eigen(v_normal).normalized())), -1., 1.)) / M_PI * 180.;
			double angle_to_view_direction1 = std::acos(std::clamp(static_cast<double>(view_to_point1.normalized().dot(viewpoints_after_reduce_redundancy[i_view1].direction.normalized())), -1., 1.));
			out_str += (boost::format("%s,%d,%d,%d,%d,%d,%d,%d,%d\n")
				% viewpoints_after_reduce_redundancy[i_view1].img_name
				% view_to_point1.x() % view_to_point1.y() % view_to_point1.z()
				% (view_to_point1.norm() / v_suggest_distance)
				% (angle_to_normal1 / 90.)
				% (angle_to_view_direction1 / (v_fov_degree_v / 180. * M_PI))
				% (pixel_position.x() / resolution_x) % (pixel_position.y() / resolution_y)
				).str();
		}
		for (int i_view1 = 0; i_view1 < viewpoints_after_reduce_redundancy.size(); ++i_view1)
		{
			for (int i_view2 = i_view1 + 1; i_view2 < viewpoints_after_reduce_redundancy.size(); ++i_view2)
			{
				Eigen::Vector3d view_to_point1 = cgaltools::cgal_point_2_eigen(v_point) - viewpoints_after_reduce_redundancy[i_view1].pos_mesh;
				Eigen::Vector3d view_to_point2 = cgaltools::cgal_point_2_eigen(v_point) - viewpoints_after_reduce_redundancy[i_view2].pos_mesh;
				double alpha = std::acos(std::clamp(static_cast<double>(view_to_point1.normalized().dot(view_to_point2.normalized())), -1., 1.)) / M_PI * 180.;
				out_str += (boost::format("%d,%d\n") %
					(alpha / 90.) %
					(1 - std::min(view_to_point1.norm(), view_to_point2.norm()) / std::max(view_to_point1.norm(), view_to_point2.norm()))
					).str();
			}
		}
		std::ofstream log_file("log/" + std::to_string(v_index) + ".txt", std::ofstream::binary | std::ios::out);
		log_file << out_str;
		log_file.close();
	}
	return total_reconstructability;
}

std::vector<double> compute_reconstructability_point_set_lalala(const std::vector<Viewpoint>& v_viewpoints,
	const PointSet3& v_point_set, const std::vector<std::vector<bool>>& point_view_visibility,
	const double v_max_distance, const double v_fov_degree_h, const double v_fov_degree_v, const Eigen::Matrix3d& v_intrinsic_matrix,
	const int v_redundancy_density, const bool v_with_minmax, const bool v_log_txt)
{
	std::vector<double> reconstructability(v_point_set.size(), 0.);
	int log_step = 0;
#pragma omp parallel for schedule(dynamic,1)
	for (int i_point = 0; i_point < v_point_set.size(); ++i_point)
	{
		std::vector<Viewpoint> viewpoints_visible_to_current_point;
		for (int i_view = 0; i_view < v_viewpoints.size(); ++i_view)
		{
			if (point_view_visibility[i_point][i_view])
				viewpoints_visible_to_current_point.push_back(v_viewpoints[i_view]);
		}
		if (viewpoints_visible_to_current_point.size() <= 1)
			reconstructability[i_point] = 0.;
		else
			reconstructability[i_point] = compute_reconstructability_lalala(viewpoints_visible_to_current_point, v_point_set.point(i_point), v_point_set.normal(i_point),
				v_max_distance, v_fov_degree_h, v_fov_degree_v, v_intrinsic_matrix, i_point, v_redundancy_density, v_with_minmax, v_log_txt);
#pragma omp critical
		{
			if (log_step != 0 && log_step % (v_point_set.size() / 10) == 0)
				LOG(INFO) << log_step << " / " << v_point_set.size();
			log_step += 1;
		}
	}
	return reconstructability;
}


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#include "random.hh"
#include "typedef.hh"

#include "external_begin.hh"
    #include <iostream>
    #include <ostream>
    #include <boost/format.hpp>
#include "external_end.hh"

using namespace std;
using namespace boost;
using namespace nuwen;
using namespace nuwen::random;

int main() {
    twister t;

    cout << "Here are 10 random 8, 16, 32, and 64-bit values:" << endl;

    for (int i = 0; i < 10; ++i) {
        const uc_t  uc  = t.random_uc();
        const us_t  us  = t.random_us();
        const ul_t  ul  = t.random_ul();
        const ull_t ull = t.random_ull();

        cout << format("0x%02X 0x%04X 0x%08X 0x%016X") % static_cast<us_t>(uc) % us % ul % ull << endl;
    }

    cout << endl << "And here are 10 random floats and doubles in [0, 1]:" << endl;

    for (int i = 0; i < 10; ++i) {
        const float  f = t.random_float_0_1();
        const double d = t.random_double_0_1();

        cout << format("%1% %2%") % f % d << endl;
    }
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/boolean/include/functions/ifnotsub.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/true.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/include/constants/allbits.hpp>

NT2_TEST_CASE_TPL ( ifnotsub_real__3_0,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::ifnotsub;
  using boost::simd::tag::ifnotsub_;
  using boost::simd::logical;
  typedef typename boost::dispatch::meta::call<ifnotsub_(logical<T>,T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(ifnotsub(logical<T>(T(0)),T(1),T(2)), T(-1));
  NT2_TEST_EQUAL(ifnotsub(boost::simd::True<logical<T> >(),T(1),T(2)), T(1));
  NT2_TEST_EQUAL(ifnotsub(boost::simd::False<logical<T> >(),boost::simd::Inf<T>(),boost::simd::Inf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(ifnotsub(boost::simd::False<logical<T> >(),boost::simd::Minf<T>(),boost::simd::Minf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(ifnotsub(boost::simd::False<logical<T> >(),boost::simd::Nan<T>(),boost::simd::Nan<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(ifnotsub(boost::simd::False<logical<T> >(),boost::simd::Zero<T>(),boost::simd::One<T>()), boost::simd::Mone<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( ifnotsub_integer__3_0,  BOOST_SIMD_INTEGRAL_TYPES)
{

  using boost::simd::ifnotsub;
  using boost::simd::tag::ifnotsub_;
  using boost::simd::logical;
  typedef typename boost::dispatch::meta::call<ifnotsub_(logical<T>,T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(ifnotsub(logical<T>(T(-1)),T(1),T(2)), T(1));
  NT2_TEST_EQUAL(ifnotsub(logical<T>(T(0)),T(2),T(1)), T(1));
  NT2_TEST_EQUAL(ifnotsub(logical<T>(boost::simd::Zero<T>()),boost::simd::Zero<T>(),boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
} // end of test for integer_


/*!
 * @file
 * @brief
 * @copyright alphya 2020-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_CHECK_SEMANTICS_HPP
#define NYARUGA_UTIL_CHECK_SEMANTICS_HPP

#pragma once

// ランダムな値を入れた配列と T -> bool な関数を使って、セマンティクスが
// 満たされているかをテストする
// 例えば、ある型の集合が群の構造を持つか確かめたいとき、Concept で制限できるのは演算(+)が定義
// されているかどうかといった程度で、結合則が満たされているかを確かめることができない
// そのような場合に結合則をテストするような関数を使ってテストするためのヘルパ関数がここに定義されている

#include <utility>
#include <concepts>
#include <array>
#include <boost/hana/functional/overload.hpp>

namespace nyaruga {

namespace util {

namespace detail {

template <typename ArrayLike, typename Fn, std::size_t... I>
constexpr bool check_semantics_impl(ArrayLike&& a, Fn&& f, std::index_sequence<I...>)
{
    return ( f(a[I]) && ... );
}

} // namespace detail

namespace check_semantics_ { // protection from unintended ADL

static inline constexpr auto check_semantics = boost::hana::overload(
[]<std::size_t N>(auto&& a, auto&& f) requires requires() { { f(a[0]) } -> std::same_as<bool>; }
{
    return detail::check_semantics_impl(std::forward<decltype(a)>(a), std::forward<decltype(f)>(f), std::make_index_sequence<N>{});
},

[]<std::size_t N, typename T, template <class, std::size_t> class Array>
(const Array<T, N>& a, auto&& f) requires requires() { { f(a[0]) } -> std::same_as<bool>; }
{
    return detail::check_semantics_impl(std::forward<decltype(a)>(a), std::forward<decltype(f)>(f), std::make_index_sequence<N>{});
},

[]<std::size_t N, typename T, template <class, std::size_t> class Array>
(T (&a)[N], auto&& f) requires requires() { { f(a[0]) } -> std::same_as<bool>; }
{
    return detail::check_semantics_impl(std::forward<decltype(a)>(a), std::forward<decltype(f)>(f), std::make_index_sequence<N>{});
},

[]<std::size_t N, typename T, template <class, std::size_t> class Array>
(T (&&a)[N], auto&& f) requires requires() { { f(a[0]) } -> std::same_as<bool>; }
{
    return detail::check_semantics_impl(std::forward<decltype(a)>(a), std::forward<decltype(f)>(f), std::make_index_sequence<N>{});
}

);

} // namespace check_semantics_

using namespace check_semantics_;

/* 使用方法

// 値をランダムに生成する（これは、コンパイル時乱数の配列を生成する <random_array> を使うとよい）
constexpr std::array<int, 5> a = {2, 4, 5, 2, 5};

// 何かの semantics を調査して、bool を返す関数を用意する（一引数関数とする）
auto f = [](int a){ return a == a; };

// 配列のそれぞれの値に対して、semantics を満たすかどうかを確かめる
static_assert(check_semantics(a, f), "");

*/

} // nyaruga :: util

#endif // NYARUGA_UTIL_CHECK_SEMANTICS_HPP


//=======================================================================
// Copyright 2001 University of Notre Dame.
// Author: Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

/*
   Sample output:

   filtered edge set: (A,B) (C,D) (D,B)
   filtered out-edges:
   A --> B
   B -->
   C --> D
   D --> B
   E -->
 */

#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_utility.hpp>

template <typename EdgeWeightMap>
struct positive_edge_weight {
  positive_edge_weight() { }
  positive_edge_weight(EdgeWeightMap weight) : m_weight(weight) { }
  template <typename Edge>
  bool operator()(const Edge& e) const {
    return 0 < boost::get(m_weight, e);
  }
  EdgeWeightMap m_weight;
};

int main()
{
  using namespace boost;

  typedef adjacency_list<multisetS, vecS, directedS,
    no_property, property<edge_weight_t, int> > Graph;
  typedef property_map<Graph, edge_weight_t>::type EdgeWeightMap;

  enum { A, B, C, D, E, N };
  const char* name = "ABCDE";
  Graph g(N);
  add_edge(A, B, 2, g);
  add_edge(A, C, 0, g);
  add_edge(C, D, 1, g);
  add_edge(C, D, 0, g);
  add_edge(C, D, 3, g);
  add_edge(C, E, 0, g);
  add_edge(D, B, 3, g);
  add_edge(E, C, 0, g);

  EdgeWeightMap weight = get(edge_weight, g);

  std::cout << "unfiltered edge_range(C,D)\n";
  graph_traits<Graph>::out_edge_iterator f, l;
  for (boost::tie(f, l) = edge_range(C, D, g); f != l; ++f)
    std::cout << name[source(*f, g)] << " --" << weight[*f]
              << "-> " <<  name[target(*f, g)] << "\n";

  positive_edge_weight<EdgeWeightMap> filter(weight);
  typedef filtered_graph<Graph, positive_edge_weight<EdgeWeightMap> > FGraph;
  FGraph fg(g, filter);

  std::cout << "filtered edge_range(C,D)\n";
  graph_traits<FGraph>::out_edge_iterator first, last;
  for (boost::tie(first, last) = edge_range(C, D, fg); first != last; ++first)
    std::cout << name[source(*first, fg)] << " --" << weight[*first]
              << "-> " <<  name[target(*first, fg)] << "\n";

  return 0;
}


#include <boost/proto/debug.hpp>
#include <boost/test/unit_test.hpp>
#include <string>

#include <metaSMT/frontend/Logic.hpp>
#include <metaSMT/frontend/QF_BV.hpp>
#include <metaSMT/frontend/QF_UF.hpp>
#include <metaSMT/API/SymbolTable.hpp>
#include <metaSMT/API/Simplify.hpp>

using namespace std;
using namespace metaSMT;
using namespace metaSMT::solver;
using namespace metaSMT::logic;
using namespace metaSMT::logic::QF_BV;
using namespace metaSMT::logic::QF_UF;
namespace proto = boost::proto;
using boost::dynamic_bitset;

BOOST_FIXTURE_TEST_SUITE(simplify_t, Solver_Fixture )

struct Lookup {
  std::map<unsigned, std::string> map_;

  std::string operator()(unsigned id) {
    return map_[id];
  }

  template < typename VarType >
  void insert(VarType var, std::string const &name) {
    map_.insert(std::make_pair(boost::proto::value(var).id, name));
  }
}; // Lookup

BOOST_AUTO_TEST_CASE( predicate_t ) {
  Lookup lookup;

  predicate a = new_variable();

  lookup.insert(a, "a");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, True ) );
  Expr.push_back( evaluate(ctx, False ) );
  Expr.push_back( evaluate(ctx, a ) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( not_t ) {
  Lookup lookup;

  predicate a = new_variable();

  lookup.insert(a, "a");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, Not(True)) );
  Expr.push_back( evaluate(ctx, Not(False)) );
  Expr.push_back( evaluate(ctx, Not(a)) );
  Expr.push_back( evaluate(ctx, Not(Not(a))) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( and_t ) {
  Lookup lookup;

  predicate a = new_variable();
  predicate b = new_variable();
  predicate c = new_variable();
  predicate d = new_variable();

  lookup.insert(a, "a");
  lookup.insert(b, "b");
  lookup.insert(c, "c");
  lookup.insert(d, "d");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, And(a,a)) );
  Expr.push_back( evaluate(ctx, And(a,b)) );
  Expr.push_back( evaluate(ctx, And(And(a,a), And(a,a))) );
  Expr.push_back( evaluate(ctx, And(And(a,b), And(a,b))) );
  Expr.push_back( evaluate(ctx, And(a, And(b, And(c,a)))) );
  Expr.push_back( evaluate(ctx, And(And(a,b), And(c,d))) );
  Expr.push_back( evaluate(ctx, And(True,And(And(a,b),And(True,d)))) );
  Expr.push_back( evaluate(ctx, And(True,And(And(a,b),And(False,d)))) );
  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( ite_t ) {
  Lookup lookup;

  predicate a = new_variable();
  predicate b = new_variable();
  predicate c = new_variable();
  predicate d = new_variable();

  lookup.insert(a, "a");
  lookup.insert(b, "b");
  lookup.insert(c, "c");
  lookup.insert(d, "d");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, Ite(True, a, b)) );
  Expr.push_back( evaluate(ctx, Ite(False, a, b)) );
  Expr.push_back( evaluate(ctx, Ite(c, a, a)) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( equal_t ) {
  Lookup lookup;

  predicate a = new_variable();
  predicate b = new_variable();
  predicate c = new_variable();
  predicate d = new_variable();

  lookup.insert(a, "a");
  lookup.insert(b, "b");
  lookup.insert(c, "c");
  lookup.insert(d, "d");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, equal(True, a)) );
  Expr.push_back( evaluate(ctx, equal(False, a)) );
  Expr.push_back( evaluate(ctx, equal(a, True)) );
  Expr.push_back( evaluate(ctx, equal(a, False)) );
  Expr.push_back( evaluate(ctx, equal(a, a)) );
  Expr.push_back( evaluate(ctx, equal(a, b)) );
  Expr.push_back( evaluate(ctx, equal(And(a,a), And(a,a))) );
  Expr.push_back( evaluate(ctx, equal(And(a,b), And(a,b))) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( nequal_t ) {
  Lookup lookup;

  predicate a = new_variable();
  predicate b = new_variable();
  predicate c = new_variable();
  predicate d = new_variable();

  lookup.insert(a, "a");
  lookup.insert(b, "b");
  lookup.insert(c, "c");
  lookup.insert(d, "d");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, nequal(True, a)) );
  Expr.push_back( evaluate(ctx, nequal(False, a)) );
  Expr.push_back( evaluate(ctx, nequal(a, True)) );
  Expr.push_back( evaluate(ctx, nequal(a, False)) );
  Expr.push_back( evaluate(ctx, nequal(a, a)) );
  Expr.push_back( evaluate(ctx, nequal(a, b)) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( implies_t ) {
  Lookup lookup;

  predicate a = new_variable();
  predicate b = new_variable();
  predicate c = new_variable();
  predicate d = new_variable();

  lookup.insert(a, "a");
  lookup.insert(b, "b");
  lookup.insert(c, "c");
  lookup.insert(d, "d");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, implies(True, a)) );
  Expr.push_back( evaluate(ctx, implies(False, a)) );
  Expr.push_back( evaluate(ctx, implies(a, True)) );
  Expr.push_back( evaluate(ctx, implies(a, False)) );
  Expr.push_back( evaluate(ctx, implies(a, a)) );
  Expr.push_back( evaluate(ctx, implies(a, b)) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( bvnot_t ) {
  Lookup lookup;

  bitvector a = new_bitvector();

  lookup.insert(a, "a");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, bvnot(bit0)) );
  Expr.push_back( evaluate(ctx, bvnot(bit1)) );
  Expr.push_back( evaluate(ctx, bvnot(bvnot(bit0))) );
  Expr.push_back( evaluate(ctx, bvnot(bvnot(bit1))) );
  Expr.push_back( evaluate(ctx, bvnot(a)) );
  Expr.push_back( evaluate(ctx, bvnot(bvnot(a))) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( bv_constants ) {
  Lookup lookup;

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, bit0) );
  Expr.push_back( evaluate(ctx, bit1) );
  Expr.push_back( evaluate(ctx, bvuint(0,1)) );
  Expr.push_back( evaluate(ctx, bvuint(1,1)) );

  Expr.push_back( evaluate(ctx, bvuint(2,2)) );
  Expr.push_back( evaluate(ctx, bvuint(3,2)) );
  Expr.push_back( evaluate(ctx, bvsint(0,1)) );
  Expr.push_back( evaluate(ctx, bvsint(1,1)) );

  Expr.push_back( evaluate(ctx, bvsint(2,2)) );
  Expr.push_back( evaluate(ctx, bvsint(3,2)) );
  Expr.push_back( evaluate(ctx, bvbin("0")) );
  Expr.push_back( evaluate(ctx, bvbin("1")) );

  Expr.push_back( evaluate(ctx, bvbin("10")) );
  Expr.push_back( evaluate(ctx, bvbin("11")) );
  Expr.push_back( evaluate(ctx, bvhex("0")) );
  Expr.push_back( evaluate(ctx, bvhex("1")) );
  Expr.push_back( evaluate(ctx, bvhex("2")) );
  Expr.push_back( evaluate(ctx, bvhex("3")) );
  Expr.push_back( evaluate(ctx, bvhex("A")) );
  Expr.push_back( evaluate(ctx, bvhex("F")) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( bv_constants_overflow ) {
  Lookup lookup;

  std::list<ContextType::result_type> Expr;

  std::string const zero8_str = "0000" "0000";
  std::string const zero16_str = zero8_str + zero8_str;
  std::string const zero32_str = zero16_str + zero16_str;
  std::string const zero64_str = zero32_str + zero32_str;

  Expr.push_back( evaluate(ctx, bvadd(bvhex( zero8_str), bvhex( zero8_str))) );
  Expr.push_back( evaluate(ctx, bvadd(bvhex(zero16_str), bvhex(zero16_str))) );

  Expr.push_back( evaluate(ctx, bvadd(bvbin(zero32_str), bvbin(zero32_str))) );
  Expr.push_back( evaluate(ctx, bvadd(bvbin(zero64_str), bvbin(zero64_str))) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( bv_arithmetic ) {
  Lookup lookup;

  unsigned const w = 8;
  bitvector x1 = new_bitvector(w);
  bitvector x2 = new_bitvector(w);
  bitvector x3 = new_bitvector(w);
  bitvector x4 = new_bitvector(w);
  lookup.insert(x1, "x1");
  lookup.insert(x2, "x2");
  lookup.insert(x3, "x3");
  lookup.insert(x4, "x4");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, bvadd(bit0, bit0)) );
  Expr.push_back( evaluate(ctx, bvadd(bit0, bit1)) );
  Expr.push_back( evaluate(ctx, bvadd(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvadd(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvadd(x1, x2)) );

  Expr.push_back( evaluate(ctx, bvsub(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvsub(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvsub(x1, x2)) );

  Expr.push_back( evaluate(ctx, bvmul(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvmul(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvmul(x1, x2)) );

  Expr.push_back( evaluate(ctx, bvudiv(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvudiv(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvudiv(x1, x2)) );

  Expr.push_back( evaluate(ctx, bvsdiv(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvsdiv(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvsdiv(x1, x2)) );

  Expr.push_back( evaluate(ctx, bvurem(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvurem(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvurem(x1, x2)) );

  Expr.push_back( evaluate(ctx, bvsrem(x1, bvuint(0,w))) );
  Expr.push_back( evaluate(ctx, bvsrem(bvuint(0,w), x1)) );
  Expr.push_back( evaluate(ctx, bvsrem(x1, x2)) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( bv_comparison ) {
  Lookup lookup;

  unsigned const w = 8;
  bitvector x1 = new_bitvector(w);
  bitvector x2 = new_bitvector(w);
  lookup.insert(x1, "x1");
  lookup.insert(x2, "x2");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, And(bvslt(x1,x2), bvsgt(x1,x2))) );
  Expr.push_back( evaluate(ctx, Or(bvslt(x1,x2), bvsgt(x1,x2))) );
  Expr.push_back( evaluate(ctx, And(bvsle(x1,x2), bvsge(x1,x2))) );
  Expr.push_back( evaluate(ctx, Or(bvsle(x1,x2), bvsge(x1,x2))) );

  Expr.push_back( evaluate(ctx, And(bvult(x1,x2), bvugt(x1,x2))) );
  Expr.push_back( evaluate(ctx, Or(bvult(x1,x2), bvugt(x1,x2))) );
  Expr.push_back( evaluate(ctx, And(bvule(x1,x2), bvuge(x1,x2))) );
  Expr.push_back( evaluate(ctx, Or(bvule(x1,x2), bvuge(x1,x2))) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_CASE( extend_expression_t ) {
  Lookup lookup;

  bitvector x = new_bitvector(8);
  bitvector y = new_bitvector(32);
  lookup.insert(x, "x");
  lookup.insert(y, "y");
  set_symbol_table( ctx, lookup);

  std::list<ContextType::result_type> Expr;

  Expr.push_back( evaluate(ctx, equal(y, sign_extend(24, x))) );
  Expr.push_back( evaluate(ctx, equal(x,  sign_extend(0, x))) );
  Expr.push_back( evaluate(ctx, equal(y, zero_extend(24, x))) );
  Expr.push_back( evaluate(ctx, equal(x,  zero_extend(0, x))) );

  for (std::list<ContextType::result_type>::const_iterator
	 it = Expr.begin(), ie = Expr.end();
       it != ie; ++it) {
    assumption(ctx, nequal(*it, simplify(ctx, *it)));
    BOOST_REQUIRE( !solve(ctx) );
  }
}

BOOST_AUTO_TEST_SUITE_END() //Solver

//  vim: ft=cpp:ts=2:sw=2:expandtab


#ifndef BOOST_METAPARSE_GETTING_STARTED_4_1_HPP
#define BOOST_METAPARSE_GETTING_STARTED_4_1_HPP

// Automatically generated header file

// Definitions before section 4
#include "4.hpp"

// Definitions of section 4
#include <boost/metaparse/lit_c.hpp>

#include <boost/metaparse/sequence.hpp>

using exp_parser4 = build_parser<sequence<token<int_>, token<lit_c<'+'>>, token<int_>>>;

// query:
//    exp_parser4::apply<BOOST_METAPARSE_STRING("11 + 2")>::type

#ifdef __METASHELL
#include <metashell/formatter.hpp>
#endif

// query:
//    exp_parser4::apply<BOOST_METAPARSE_STRING("11 + 2")>::type

#endif


#pragma once

#include <cereal/cereal.hpp>
#include <Eigen/Core>

namespace cereal {

template <typename Archive, typename T, int _Rows, int _Cols, int _Opts,
          int _MaxRows, int _MaxCols>
inline std::enable_if_t<
  traits::is_output_serializable< BinaryData<T>, Archive>::value and
  std::is_arithmetic<T>::value and not std::is_same<T, bool>::value
> CEREAL_SAVE_FUNCTION_NAME(
    Archive &ar,
    const Eigen::Matrix<T,_Rows,_Cols,_Opts,_MaxRows,_MaxCols>& mat
) {

  //ar( _Rows, _Cols, _Opts, _MaxRows, _MaxCols );
  int32_t rows = mat.rows();
  int32_t cols = mat.cols();
  ar( rows, cols );
  ar( binary_data( mat.data(), static_cast<std::size_t>(rows * cols * sizeof(T)) ));

}

template <typename Archive, typename T, int _Rows, int _Cols, int _Opts,
          int _MaxRows, int _MaxCols>
inline std::enable_if_t<
  traits::is_input_serializable< BinaryData<T>, Archive>::value and
  std::is_arithmetic<T>::value and not std::is_same<T, bool>::value
> CEREAL_LOAD_FUNCTION_NAME(
    Archive &ar,
    Eigen::Matrix<T,_Rows,_Cols,_Opts,_MaxRows,_MaxCols>& mat
) {

  //ar( Rows, Cols, Opts, MaxRows, MaxCols );

  int32_t rows;
  int32_t cols;
  ar( rows, cols );

  mat.resize( rows, cols );

  ar( binary_data( mat.data(), static_cast<std::size_t>(rows * cols * sizeof(T)) ));

}

}


#include <Eigen/Core>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <aslam/common/entrypoint.h>
#include <aslam/common/memory.h>
#include <aslam/cameras/distortion.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/cameras/distortion-null.h>
#include <aslam/cameras/distortion-radtan.h>
#include <aslam/cameras/distortion-equidistant.h>
#include <aslam/common/numdiff-jacobian-tester.h>

///////////////////////////////////////////////
// Types to test
///////////////////////////////////////////////
using testing::Types;
typedef Types<aslam::RadTanDistortion,
              aslam::FisheyeDistortion,
              aslam::EquidistantDistortion> ImplementationsNoNull;

typedef Types<aslam::RadTanDistortion,
              aslam::FisheyeDistortion,
              aslam::EquidistantDistortion,
              aslam::NullDistortion> Implementations;

///////////////////////////////////////////////
// Test fixture
///////////////////////////////////////////////
template <class DistortionType>
class TestDistortions : public testing::Test {
 protected:
  TestDistortions() : distortion_(DistortionType::createTestDistortion()) {};
  virtual ~TestDistortions() {};
  typename DistortionType::Ptr distortion_;
};

template <class DistortionType>
class TestDistortionsNotNull : public TestDistortions<DistortionType> { };

TYPED_TEST_CASE(TestDistortions, Implementations);
TYPED_TEST_CASE(TestDistortionsNotNull, ImplementationsNoNull);

///////////////////////////////////////////////
// Test cases
///////////////////////////////////////////////
TYPED_TEST(TestDistortions, DistortAndUndistortUsingInternalParametersRandom) {
  const size_t kNumSamples = 1e5;
  for (int i = 0; i < kNumSamples; ++i) {
    Eigen::Vector2d keypoint = 5 * Eigen::Vector2d::Random();
    Eigen::Vector2d keypoint2 = keypoint;

    this->distortion_->distort(&keypoint2);
    this->distortion_->undistort(&keypoint2);

    EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint2, keypoint, 1e-5));
  }
}

TYPED_TEST(TestDistortions, DistortAndUndistortUsingInternalParameters) {
  // Box on the normalized image plane corresponds to
  // a pinhole camera with a resolution of 2000x2000, f=200 and c=1000.
  static constexpr double box_side = 5.0;
  static constexpr double step_size = 0.1;

  for (double u = -box_side / 2.0; u < box_side / 2.0; u += step_size) {
    for (double v = -box_side / 2.0; v < box_side / 2.0; v += step_size) {
      Eigen::Vector2d keypoint(u, v);
      Eigen::Vector2d keypoint2 = keypoint;
      this->distortion_->distort(&keypoint2);
      this->distortion_->undistort(&keypoint2);

      EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint2, keypoint, 1e-5));
    }
  }
}

TYPED_TEST(TestDistortions, DistortAndUndistortUsingExternalParameters) {
  // Set new parameters.
  Eigen::VectorXd dist_coeff = this->distortion_->getParameters();
  this->distortion_->setParameters(dist_coeff / 2.0);

  // Box on the normalized image plane corresponds to
  // a pinhole camera with a resolution of 2000x2000, f=200 and c=1000.
  static constexpr double box_side = 5.0;
  static constexpr double step_size = 0.1;

  for (double u = -box_side / 2.0; u < box_side / 2.0; u += step_size) {
    for (double v = -box_side / 2.0; v < box_side / 2.0; v += step_size) {
      Eigen::Vector2d keypoint(u, v);
      Eigen::Vector2d keypoint2 = keypoint;

      this->distortion_->distortUsingExternalCoefficients(&dist_coeff, &keypoint2, nullptr);
      this->distortion_->undistortUsingExternalCoefficients(dist_coeff, &keypoint2);

      EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint2, keypoint, 1e-5));
    }
  }
}

TYPED_TEST(TestDistortions, DistortAndUndistortImageCenter) {
  Eigen::Vector2d keypoint(0.0, 0.0);

  Eigen::Vector2d keypoint2 = keypoint;
  this->distortion_->undistort(&keypoint2);
  this->distortion_->distort(&keypoint2);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint2, keypoint, 1e-12));

  keypoint2 = keypoint;
  this->distortion_->distort(&keypoint2);
  this->distortion_->undistort(&keypoint2);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint2, keypoint, 1e-12));
}

/// Wrapper that brings the distortion function to the form needed by the differentiator.
struct Point3dJacobianFunctor : public aslam::common::NumDiffFunctor<2, 2> {

  Point3dJacobianFunctor(const aslam::Distortion& distortion, const Eigen::VectorXd& dist_coeffs)
      : distortion_(distortion),
        dist_coeffs_(dist_coeffs){};

  virtual ~Point3dJacobianFunctor() {};

  virtual bool functional(const typename NumDiffFunctor::InputType& x,
                          typename NumDiffFunctor::ValueType& fvec,
                          typename NumDiffFunctor::JacobianType* Jout) const {
    Eigen::Vector2d out_keypoint = x;

    // Get function value and Jacobian
    if(Jout)
      distortion_.distortUsingExternalCoefficients(&dist_coeffs_, &out_keypoint, Jout);
    else
      distortion_.distortUsingExternalCoefficients(&dist_coeffs_, &out_keypoint, nullptr);

    fvec = out_keypoint;
    return true;
  };

  const aslam::Distortion& distortion_;
  const Eigen::VectorXd dist_coeffs_;
};

TYPED_TEST(TestDistortions, JacobianWrtKeypoint) {
  Eigen::Vector2d keypoint(0.3, -0.2);
  Eigen::VectorXd dist_coeffs = this->distortion_->getParameters();

  TEST_JACOBIAN_FINITE_DIFFERENCE(Point3dJacobianFunctor, keypoint, 1e-5, 1e-4,
                                  *(this->distortion_), dist_coeffs);
}

/// Wrapper that brings the distortion function to the form needed by the differentiator.
template<int numDistortion>
struct DistortionJacobianFunctor : public aslam::common::NumDiffFunctor<2, numDistortion> {

  DistortionJacobianFunctor(const aslam::Distortion& distortion, const Eigen::Vector2d& keypoint)
      : distortion_(distortion),
        keypoint_(keypoint) {};

  virtual ~DistortionJacobianFunctor() {};

  virtual bool functional(
      const typename aslam::common::NumDiffFunctor<2, numDistortion>::InputType& x,
      typename aslam::common::NumDiffFunctor<2, numDistortion>::ValueType& fvec,
      typename aslam::common::NumDiffFunctor<2, numDistortion>::JacobianType* Jout) const {

    Eigen::Vector2d out_keypoint = keypoint_;
    Eigen::Matrix<double, 2, Eigen::Dynamic> JoutDynamic;
    JoutDynamic.setZero();
    Eigen::Matrix<double, Eigen::Dynamic, 1> xDynamic = x;

    // Get value
    distortion_.distortUsingExternalCoefficients(&xDynamic, &out_keypoint, nullptr);

    fvec = out_keypoint;

    // Get Jacobian wrt distortion coeffs.
    if(Jout) {
      distortion_.distortParameterJacobian(&xDynamic, keypoint_, &JoutDynamic);
      (*Jout) = JoutDynamic;
    }

    return true;
  };

  const aslam::Distortion& distortion_;
  const Eigen::Vector2d keypoint_;
};

TYPED_TEST(TestDistortionsNotNull, JacobianWrtDistortion) {
  Eigen::Vector2d keypoint(0.3, -0.2);
  Eigen::VectorXd dist_coeffs = this->distortion_->getParameters();

  TEST_JACOBIAN_FINITE_DIFFERENCE(DistortionJacobianFunctor<TypeParam::parameterCount()>,
                                  dist_coeffs, 1e-5, 1e-4, *(this->distortion_), keypoint);
}

///////
// Test parameters
///////
TEST(TestParameter, testEquidistantDistortionParameters) {
  Eigen::Vector3d invalid1 = Eigen::Vector3d::Zero();
  EXPECT_FALSE(aslam::EquidistantDistortion::areParametersValid(invalid1));

  Eigen::Matrix<double, 5, 1> invalid2 = Eigen::Matrix<double, 5, 1>::Zero();
  EXPECT_FALSE(aslam::EquidistantDistortion::areParametersValid(invalid2));

  Eigen::Vector4d valid = Eigen::Vector4d::Zero();
  EXPECT_TRUE(aslam::EquidistantDistortion::areParametersValid(valid));
}

namespace aslam {
TEST(TestParameter, testFisheyeDistortionParameters) {
  Eigen::Matrix<double, 0, 1> invalid1 = Eigen::Matrix<double, 0, 1>::Zero();
  EXPECT_FALSE(FisheyeDistortion::areParametersValid(invalid1));

  Eigen::Vector2d invalid2 = Eigen::Vector2d::Zero();
  EXPECT_FALSE(FisheyeDistortion::areParametersValid(invalid2));

  /// todo(mbuerki): enable once https://github.com/ethz-asl/aslam_cv2/issues/245 is resolved.
  //Eigen::Matrix<double, 1, 1> invalid3 = Eigen::Matrix<double, 1, 1>::Zero();
  //EXPECT_FALSE(aslam::FisheyeDistortion::areParametersValid(invalid3));

  const double min_w = FisheyeDistortion::getMinValidW();
  const double max_w = FisheyeDistortion::getMaxValidW();
  double invalid_w_5 = min_w - 1e-6;
  double invalid_w_6 = max_w + 1e-6;

  Eigen::Matrix<double, 1, 1> invalid5;
  invalid5 << invalid_w_5;
  EXPECT_FALSE(FisheyeDistortion::areParametersValid(invalid5));

  invalid5(0) = invalid_w_6;
  EXPECT_FALSE(FisheyeDistortion::areParametersValid(invalid5));

  Eigen::Matrix<double, 1, 1> valid;
  valid << min_w;
  EXPECT_TRUE(FisheyeDistortion::areParametersValid(valid));

  valid(0) =  max_w;
  EXPECT_TRUE(FisheyeDistortion::areParametersValid(valid));

  valid(0) = (min_w + max_w) / 2.0;
  Eigen::Matrix<double, 1, 1> valid1;
  valid1 << min_w;
  EXPECT_TRUE(FisheyeDistortion::areParametersValid(valid));
}
}  // namespace aslam

TEST(TestParameter, testRadTanParameters) {
  Eigen::Vector3d invalid1 = Eigen::Vector3d::Zero();
  EXPECT_FALSE(aslam::RadTanDistortion::areParametersValid(invalid1));

  Eigen::Matrix<double, 5, 1> invalid2 = Eigen::Matrix<double, 5, 1>::Zero();
  EXPECT_FALSE(aslam::RadTanDistortion::areParametersValid(invalid2));

  Eigen::Vector4d valid = Eigen::Vector4d::Zero();
  EXPECT_TRUE(aslam::RadTanDistortion::areParametersValid(valid));
}

ASLAM_UNITTEST_ENTRYPOINT


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Vivek Shah (University of Copenhagen)
 *
 * 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 dbindex 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.
 *
 * hash_test.cc
 *
 *  Created on: May 2, 2016
 *  Author: Vivek Shah <bonii @ di.ku.dk>
 */

#include <gtest/gtest.h>
#include <vector>
#include <cstdint>
#include <memory>
#include <utility>
#include <random>
#include <algorithm>
#include <limits>
#include <cmath>
#include <functional>
#include <cmath>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/ref.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include "../src/hash_functions/mod_hash.h"
#include "../src/hash_functions/mult_shift_hash.h"
#include "../src/hash_functions/murmur_hash_32.h"
#include "../src/hash_functions/tabulation_hash.h"

namespace dbindex {
    namespace test {
        using hash_value_t = std::uint32_t;
        using input_key_t = std::uint64_t;
        static constexpr int MOD_HASH_MOD_VAL = 100;
        static constexpr int MAX_KEY_LEN_VAL = 64;
        static constexpr int NUM_EQUALITY_INPUTS = 20000;
        static constexpr int NUM_EQUALITY_ASSERTS = 10;
        static constexpr int NUM_SPREAD_ITERATIONS = 10;
        static constexpr int NUM_SPREAD_INPUTS = 20000;
        static constexpr int NUM_SPREAD_BUCKETS = 100;
        static constexpr float ACCEPTABLE_DEV_MEAN_RATIO = .1;

        class hash_function_test: public ::testing::Test {
        public:
            void test_spread_of_hash_function(
                    dbindex::abstract_hash<hash_value_t>& hash_fn,
                    std::function<input_key_t()>& input_generator) {
                static_assert(NUM_SPREAD_BUCKETS < NUM_SPREAD_INPUTS, "Buckets must be less than inputs");
                std::vector<std::uint64_t> histogram(NUM_SPREAD_BUCKETS);
                int failed_iterations = 0;
                for (int iteration = 0; iteration < NUM_SPREAD_ITERATIONS;
                        iteration++) {
                    std::fill(histogram.begin(), histogram.end(), 0);
                    for (int i = 0; i < NUM_SPREAD_INPUTS; i++) {
                        auto input_val = input_generator();
                        auto input_val_string = std::string(
                                reinterpret_cast<char*>(&input_val),
                                sizeof(input_val));
                        auto hash_val = hash_fn.get_hash(input_val_string);
                        hash_value_t bucket = hash_val
                                % (hash_value_t) NUM_SPREAD_BUCKETS;
                        histogram[bucket]++;
                    }
                    //Check spread
                    boost::accumulators::accumulator_set<double,
                            boost::accumulators::features<
                                    boost::accumulators::tag::mean,
                                    boost::accumulators::tag::variance>> acc;
                    std::for_each(histogram.begin(), histogram.end(),
                            [&](std::uint64_t& val) {acc(val);});
                    auto mean = boost::accumulators::extract::mean(acc);
                    auto std_dev = std::sqrt(
                            boost::accumulators::extract::variance(acc));
                    if ((float) std_dev / mean > ACCEPTABLE_DEV_MEAN_RATIO) {
                        failed_iterations++;
                    }
                    //Should do a Chi-Squared test
                }

                EXPECT_TRUE(failed_iterations < NUM_SPREAD_ITERATIONS / 2);
            }

            void test_equality_of_hash_function(
                    dbindex::abstract_hash<hash_value_t>& hash_fn,
                    std::function<input_key_t()>& input_generator) {
                std::vector<std::pair<input_key_t, hash_value_t>> inputs(
                        NUM_EQUALITY_INPUTS);
                for (int iteration = 0; iteration < NUM_EQUALITY_ASSERTS;
                        iteration++) {
                    for (int i = 0; i < NUM_EQUALITY_INPUTS; i++) {
                        if (iteration == 0) {
                            //Compute and store
                            auto input_val = input_generator();
                            inputs[i] =
                                    std::make_pair(input_val,
                                            hash_fn.get_hash(
                                                    std::string(
                                                            reinterpret_cast<char*>(&input_val),
                                                            sizeof(input_val))));
                        } else {
                            auto input_val = inputs[i].first;
                            EXPECT_TRUE(
                                    inputs[i].second
                                            == hash_fn.get_hash(
                                                    std::string(
                                                            reinterpret_cast<char*>(&input_val),
                                                            sizeof(input_val))));
                        }
                    }

                }
            }
        };

        TEST_F(hash_function_test, equality_mod_hash) {
            std::default_random_engine generator;
            std::uniform_int_distribution<input_key_t> distribution(
                    std::numeric_limits<input_key_t>::min(),
                    std::numeric_limits<input_key_t>::max());
            std::function<input_key_t()> input_gen_fn { std::bind(distribution,
                    generator) };

            dbindex::mod_hash<hash_value_t, MOD_HASH_MOD_VAL> mod_hash;
            test_equality_of_hash_function(mod_hash, input_gen_fn);

        }

        TEST_F(hash_function_test, equality_mult_shift_hash) {
            std::default_random_engine generator;
            std::uniform_int_distribution<input_key_t> distribution(
                    std::numeric_limits<input_key_t>::min(),
                    std::numeric_limits<input_key_t>::max());
            std::function<input_key_t()> input_gen_fn { std::bind(distribution,
                    generator) };

            dbindex::mult_shift_hash<hash_value_t> mult_shift_hash;
            test_equality_of_hash_function(mult_shift_hash, input_gen_fn);
        }

        /*        TEST_F(hash_function_test, equality_murmur_hash_32) {
         std::default_random_engine generator;
         std::uniform_int_distribution<input_key_t> distribution(
         std::numeric_limits<input_key_t>::min(),
         std::numeric_limits<input_key_t>::max());
         std::function<input_key_t()> input_gen_fn {std::bind(distribution,generator)};

         dbindex::murmur_hash_32<hash_value_t> murmur_hash_32;
         test_equality_of_hash_function(murmur_hash_32, input_gen_fn);
         }
         */
        TEST_F(hash_function_test, equality_tabulation_hash) {
            std::default_random_engine generator;
            std::uniform_int_distribution<input_key_t> distribution(
                    std::numeric_limits<input_key_t>::min(),
                    std::numeric_limits<input_key_t>::max());
            std::function<input_key_t()> input_gen_fn { std::bind(distribution,
                    generator) };
            dbindex::tabulation_hash<hash_value_t, MAX_KEY_LEN_VAL> tabulation_hash;
            test_equality_of_hash_function(tabulation_hash, input_gen_fn);
        }

        TEST_F(hash_function_test, spread_mod_hash) {
            std::default_random_engine generator;
            std::uniform_int_distribution<input_key_t> distribution(
                    std::numeric_limits<input_key_t>::min(),
                    std::numeric_limits<input_key_t>::max());
            std::function<input_key_t()> input_gen_fn { std::bind(distribution,
                    generator) };

            dbindex::mod_hash<hash_value_t, MOD_HASH_MOD_VAL> mod_hash;
            test_spread_of_hash_function(mod_hash, input_gen_fn);
        }

        TEST_F(hash_function_test, spread_mult_shift_hash) {
            std::default_random_engine generator;
            std::uniform_int_distribution<input_key_t> distribution(
                    std::numeric_limits<input_key_t>::min(),
                    std::numeric_limits<input_key_t>::max());
            std::function<input_key_t()> input_gen_fn { std::bind(distribution,
                    generator) };

            dbindex::mult_shift_hash<hash_value_t> mult_shift_hash;
            test_spread_of_hash_function(mult_shift_hash, input_gen_fn);
        }
        /*
         TEST_F(hash_function_test, spread_murmur_hash_32) {
         std::default_random_engine generator;
         std::uniform_int_distribution<input_key_t> distribution(
         std::numeric_limits<input_key_t>::min(),
         std::numeric_limits<input_key_t>::max());
         std::function<input_key_t()> input_gen_fn {std::bind(distribution,generator)};

         dbindex::murmur_hash_32<hash_value_t> murmur_hash_32;
         test_spread_of_hash_function(murmur_hash_32, input_gen_fn);
         }
         */
        TEST_F(hash_function_test, spread_tabulation_hash) {
            std::default_random_engine generator;
            std::uniform_int_distribution<input_key_t> distribution(
                    std::numeric_limits<input_key_t>::min(),
                    std::numeric_limits<input_key_t>::max());
            std::function<input_key_t()> input_gen_fn { std::bind(distribution,
                    generator) };
            dbindex::tabulation_hash<hash_value_t, MAX_KEY_LEN_VAL> tabulation_hash;
            test_spread_of_hash_function(tabulation_hash, input_gen_fn);
        }

    }
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

/*!@file
 * This file includes an acceptance tests for the trimmed version of the grassmann pca
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
// data stored into a matrix
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

// boost chrono
#include <boost/chrono/include.hpp>

#include <fstream>
#include <sstream>

#include <include/grassmann_pca_with_trimming.hpp>
#include <include/private/boost_ublas_row_iterator.hpp>

std::string filename_data;
std::string filename_basis_vectors;
std::string filename_expected_result;

struct ConfigurationFiles
{

  ConfigurationFiles()
  {
    int argc  = boost::unit_test::framework::master_test_suite().argc;
    char**argv= boost::unit_test::framework::master_test_suite().argv;

    // from the command line, we expect the file that should be read.
    for(int i = 0; i < argc - 1; i++)
    {
      if(std::string(argv[i]) == "--data")
      {
        if(!filename_data.empty())
        {
          std::cerr << "Test initialisation error: data given several times" << std::endl;
        }
        filename_data = argv[i+1];
      }
      if(std::string(argv[i]) == "--basis_vectors")
      {
        if(!filename_basis_vectors.empty())
        {
          std::cerr << "Test initialisation error: basis_vectors given several times" << std::endl;
        }
        filename_basis_vectors = argv[i+1];
      }
      if(std::string(argv[i]) == "--expected_result")
      {
        if(!filename_expected_result.empty())
        {
          std::cerr << "Test initialisation error: expected_result given several times" << std::endl;
        }
        filename_expected_result = argv[i+1];
      }
    }
  }
};

BOOST_GLOBAL_FIXTURE( ConfigurationFiles );

template <class T>
std::vector<T>* readLines(std::istream& str)
{
  std::vector<T>* result = new std::vector<T>();
  std::string line;
  std::getline(str,line);

  std::stringstream lineStream(line);
  std::string cell;

  while(std::getline(lineStream, cell, '\t'))
  {
    std::stringstream current_stream(cell);
    T current_element;
    current_stream >> current_element;
    assert(!current_stream.fail());
    result->push_back(current_element);
  }
  return result;
}

template <class matrix_t>
bool read_matrix(const std::string &filename, matrix_t &mat_data)
{
  std::vector< std::vector<double>* > read_vectors;
  std::ifstream ff(filename.c_str());
  while(!ff.eof())
  {
    std::vector<double>* v = readLines<double>(ff);
    if(v->size())
    {
      read_vectors.push_back(v);
      if((read_vectors.size() % 1000) == 0)
      {
        std::cout << ".";
        std::cout.flush();
      }
    }
  }

  mat_data.resize(read_vectors.size(), read_vectors[0]->size());
  for(size_t i = 0; i < read_vectors.size(); i++)
  {
    std::copy(read_vectors[i]->begin(), read_vectors[i]->end(), boost::numeric::ublas::row(mat_data, i).begin());
    delete read_vectors[i];
  }
  read_vectors.clear();

  return true;
}

BOOST_AUTO_TEST_CASE(convergence_rate_tests_several_workers)
{

  using namespace grassmann_averages_pca;
  using namespace grassmann_averages_pca::details::ublas_helpers;
  namespace ub = boost::numeric::ublas;
  typedef boost::chrono::steady_clock clock_type;
  typedef boost::numeric::ublas::matrix<double> matrix_t;

  BOOST_REQUIRE(!filename_data.empty());

  typedef ub::vector<double> data_t;
  typedef grassmann_pca_with_trimming< data_t > grassmann_pca_t;
  grassmann_pca_t instance(.1);
  typedef row_iter<const matrix_t> const_row_iter_t;

  // reading the data, one vector per line
  std::cout << "Reading data" << std::endl;
  matrix_t mat_data;
  BOOST_REQUIRE(read_matrix(filename_data, mat_data));

  std::cout << std::endl << "Reading init basis vectors" << std::endl;
  // reading the init vectors, one vector per column
  std::vector<data_t> v_basisvectors_init;
  matrix_t mat_vectors;
  BOOST_REQUIRE(read_matrix(filename_basis_vectors, mat_vectors));
  for(int i = 0; i < mat_vectors.size2(); i++)
  {
    v_basisvectors_init.push_back(ub::column(mat_vectors, i));
  }

  std::cout << std::endl << "Reading expected result" << std::endl;
  std::vector<data_t> v_known_result;
  matrix_t mat_result;
  BOOST_REQUIRE(read_matrix(filename_expected_result, mat_result));
  for(int i = 0; i < mat_vectors.size2(); i++)
  {
    v_known_result.push_back(ub::column(mat_result, i));
  }

  const size_t nb_elements = mat_data.size1();
  const size_t dimensions = mat_data.size2();
  std::cout << std::endl << "Data: dimensions = " << dimensions << " #elements = " << nb_elements << std::endl;

  const size_t max_dimensions = mat_vectors.size2();

  std::vector<data_t> basis_vectors(max_dimensions);
  const int max_iterations = 1000;

  BOOST_CHECK(instance.set_nb_processors(7));
  BOOST_CHECK(instance.set_nb_steps_pca(0));

  clock_type::duration elapsed;

  clock_type::time_point start = clock_type::now();
  BOOST_CHECK(instance.batch_process(
    max_iterations,
    max_dimensions,
    const_row_iter_t(mat_data, 0),
    const_row_iter_t(mat_data, mat_data.size1()),
    basis_vectors.begin(),
    &v_basisvectors_init));
  elapsed = clock_type::now() - start;

  std::cout << "processing " << nb_elements << " elements "
    << "in " << boost::chrono::duration_cast<boost::chrono::microseconds>(elapsed) << std::endl;

  // testing the output sizes
  BOOST_REQUIRE_EQUAL(basis_vectors.size(), max_dimensions);
  for(int i = 0; i < max_dimensions; i++)
  {
    BOOST_TEST_CHECKPOINT("testing basis vector size for vector " << i);
    BOOST_REQUIRE_EQUAL(basis_vectors[i].size(), dimensions);
  }

  // testing orthogonality of all basis vectors
  for(int i = 0; i < max_dimensions - 1; i++)
  {
    for(int j = i + 1; j < max_dimensions; j++)
    {
      BOOST_CHECK_LE(ub::inner_prod(basis_vectors[i], basis_vectors[j]), 1E-6);
    }
  }

  // testing unitarity
  for(int i = 0; i < max_dimensions; i++)
  {
    BOOST_CHECK_CLOSE(ub::inner_prod(basis_vectors[i], basis_vectors[i]), 1, 1E-6);
  }

  // testing for coherence with matlab
  for(int i = 0; i < max_dimensions; i++)
  {
    details::norm_infinity norm;
    BOOST_CHECK_LE(norm(basis_vectors[i] - v_known_result[i]), 1E-2);
  }

}


#include "opencv2/opencv.hpp"

#include <boost/date_time/posix_time/posix_time.hpp>
namespace ptime = boost::posix_time;

#include "boost/filesystem.hpp"
namespace fs  = boost::filesystem;

#include <boost/format.hpp>
typedef boost::format fmt;

#include "Pipeline.hpp"
#include "Associativity.hpp"
#include "Viewer.hpp"
#include "util.hpp"

float _intrinsic_array[9]= {524,0,316.7,0,524,238.5,0,0,1};
float _distcoeff_array[4] = {0.2402,-0.6861,-0.0015,0.0003};

Pipeline::Pipeline(std::string _folder_path)
:
	cameraMatrix(cv::Mat(3, 3, CV_32F, _intrinsic_array)),
	distCoeffs(cv::Mat(1, 4, CV_32F, _distcoeff_array))
{
	int n = _folder_path.size();
	if (_folder_path[n - 1] == '/') _folder_path.resize(n - 1);

	folder_path = _folder_path;
}

void Pipeline::run(const bool save_clouds, const bool show_clouds)
{
	Logger _log("Pipeline");

	/**
	 * Stage 0: Load images from file
	 */
	Images images;
	load_images(folder_path, images);

	/**
	 * Stage 1: Detect features in loaded images
	 */
	CamFrames cam_Frames;
	DescriptorsVec descriptors_vec;
	extract_features(images, cam_Frames, descriptors_vec);

	// Free Image.gray
	for (int i = 0; i < images.size(); i++)
		const_cast<cv::Mat&>(images[i].gray).release();

	/**
	 * Stage 2: Calculate descriptors and find image pairs through matching
	 */
	ImagePairs image_pairs;
	find_matching_pairs(images, cam_Frames, descriptors_vec, image_pairs);

	// Free some memory
	DescriptorsVec().swap(descriptors_vec);

	/**
	 * State 3: Compute pairwise R and t
	 */
	register_camera(image_pairs, cam_Frames);

	/**
	 * Stage 4: Construct associativity matrix and spanning tree
	 */
	Associativity assocMat(cam_Frames.size());
	for (int p = 0; p < image_pairs.size(); p++)
	{
		ImagePair* pair = &image_pairs[p];
		int i = pair->pair_index.first,
		    j = pair->pair_index.second;

		if (pair -> R.empty()) continue;

		assocMat(i, j) = pair;
		assocMat(j, i) = pair;

		assert(assocMat(i, j) == assocMat(j, i));
		assert(assocMat(i, j)->pair_index.first  == i &&
		       assocMat(i, j)->pair_index.second == j);
	}

	Associativity tree;
	const int camera_num = build_spanning_tree(image_pairs, assocMat, tree);

	/**
	 * Stage 5: Compute global Rs and ts
	 */
	CameraPoses gCameraPoses;
	glo_cam_poses(images, gCameraPoses, image_pairs, tree);

	/**
	 * Stage 6: Find and cluster depth points from local camera frame to global camera frame
	 */
	PointClusters pointClusters;
	PointMap pointMap;
	find_clusters(assocMat, gCameraPoses, cam_Frames, pointClusters, pointMap);

	// Free some memory
	ImagePairs().swap(image_pairs);

	/**
	 * Stage 7: get center of mass from clusters
	 */
	PointCloud pointCloud(pointClusters.size());
	find_CoM(pointClusters, pointCloud);

	// Free pointClusters
	for (int i = 0; i < pointClusters.size(); i++)
		PointCluster().swap(pointClusters[i]);
	PointClusters().swap(pointClusters);

	// Save cloud before BA
	Viewer viewer("Before BA");
	auto cloud  = viewer.createPointCloud(images, gCameraPoses, cameraMatrix);
	int  n      = cloud->points.size();
	auto time   = ptime::second_clock::local_time();
	auto tstamp = ptime::to_iso_string(time);
	auto folder = fs::path(folder_path).filename().string();
	auto fname  = (fmt("%s_%s_%d_noBA.pcd") % folder % tstamp % n).str();

	if (save_clouds)
		viewer.saveCloud(cloud, fname);
	if (show_clouds)
		viewer.showCloudPoints(cloud, false);

	/**
	 * State 8: Bundle Adjustment
	 */
	bundle_adjustment(pointMap, cam_Frames, false, gCameraPoses, pointCloud);
	_log.tok();

	/**
	 * Show calculated point cloud
	 */
	Viewer viewer_ba("After BA no Depth");
	cloud = viewer_ba.createPointCloud(images, gCameraPoses, cameraMatrix);
	n     = cloud->points.size();
	fname = (fmt("%s_%s_%d_BA_noD.pcd") % folder % tstamp % n).str();

	if (save_clouds)
		viewer_ba.saveCloud(cloud, fname);
	if (show_clouds)
		viewer_ba.showCloudPoints(cloud,false);

	bundle_adjustment(pointMap, cam_Frames, true, gCameraPoses, pointCloud);

	// Free some memory
	PointMap().swap(pointMap);
	CamFrames().swap(cam_Frames);
	PointCloud().swap(pointCloud);

	/**
	 * Show calculated point cloud
	 */
	Viewer viewer_baD("After BA with Depth");
	cloud = viewer_baD.createPointCloud(images, gCameraPoses, cameraMatrix);
	n     = cloud->points.size();
	fname = (fmt("%s_%s_%d_BA_D.pcd") % folder % tstamp % n).str();

	// Free some memory
	Images().swap(images);
	CameraPoses().swap(gCameraPoses);

	if (save_clouds)
		viewer_baD.saveCloud(cloud, fname);
	if (show_clouds)
		viewer_baD.showCloudPoints(cloud);

}