#include <utility/io/read_many.hpp>

#include <burst/iterator/merge_iterator.hpp>
#include <burst/range/merge.hpp>

#include <boost/assert.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/iterator_range.hpp>

#include <iostream>
#include <numeric>
#include <vector>

template <typename Container>
void test_on_the_fly_merge (const Container & values)
{
    using nested_container_type = typename Container::value_type;

    std::vector<boost::iterator_range<typename nested_container_type::const_iterator>> ranges;
    boost::for_each(values,
        [& ranges] (const nested_container_type & values)
        {
            ranges.push_back(boost::make_iterator_range(values));
        });

    clock_t merge_time = clock();
    auto merged_range = burst::merge(boost::make_iterator_range(ranges));
    auto distance = static_cast<std::size_t>(std::distance(merged_range.begin(), merged_range.end()));
    merge_time = clock() - merge_time;

    BOOST_VERIFY
    (
        distance ==
            std::accumulate(values.begin(), values.end(), 0ul,
                [] (std::size_t current_value, const nested_container_type & vector)
                {
                    return current_value + vector.size();
                })
    );

    std::cout << "Слияние на лету:" << std::endl;
    std::cout << "\t" << static_cast<double>(merge_time) / CLOCKS_PER_SEC << std::endl;
    std::cout << std::endl;
}

template <typename Container>
void test_merge_by_sorting (const Container & values)
{
    clock_t merge_time = clock();

    typename Container::value_type flat_values;
    for (const auto & row: values)
    {
        flat_values.insert(flat_values.end(), row.begin(), row.end());
    }

    std::sort(flat_values.begin(), flat_values.end());

    merge_time = clock() - merge_time;

    std::cout << "Слияние сортировкой:" << std::endl;
    std::cout << "\t" << static_cast<double>(merge_time) / CLOCKS_PER_SEC << std::endl;
    std::cout << std::endl;
}

template <typename Container>
void test_std_merge (const Container & values)
{
    typename Container::value_type merged;

    clock_t merge_time = clock();
    for (const auto & row: values)
    {
        typename Container::value_type result;
        result.reserve(merged.size() + row.size());
        std::merge(merged.begin(), merged.end(), row.begin(), row.end(), std::back_inserter(result));
        std::swap(result, merged);
    }
    merge_time = clock() - merge_time;

    std::cout << "std::merge:" << std::endl;
    std::cout << "\t" << static_cast<double>(merge_time) / CLOCKS_PER_SEC << std::endl;
    std::cout << std::endl;
}

int main ()
{
    std::vector<std::vector<std::int64_t>> values;
    utility::read_many(std::cin, values);

    test_merge_by_sorting(values);
    test_std_merge(values);
    test_on_the_fly_merge(values);
}


#include "MeshCuboidNonLinearSolver.h"

#include <bitset>

#include <Eigen/Eigenvalues>

MeshCuboidNonLinearSolver::MeshCuboidNonLinearSolver(
	const std::vector<MeshCuboid *>& _cuboids,
	const std::vector<MeshCuboidReflectionSymmetryGroup *>& _reflection_symmetry_groups,
	const std::vector<MeshCuboidRotationSymmetryGroup *>& _rotation_symmetry_groups,
	const Real _squared_neighbor_distance,
	const unsigned int _min_num_symmetry_point_pairs,
	const Real _symmetry_energy_term_weight)
	: cuboids_(_cuboids)
	, reflection_symmetry_groups_(_reflection_symmetry_groups)
	, rotation_symmetry_groups_(_rotation_symmetry_groups)
	, squared_neighbor_distance_(_squared_neighbor_distance)
	, min_num_symmetric_point_pairs_(_min_num_symmetry_point_pairs)
	, symmetry_energy_term_weight_(_symmetry_energy_term_weight)
	, num_cuboid_corner_variables_(MeshCuboidAttributes::k_num_attributes)
	, num_cuboid_axis_variables_(3 * 3)
	, num_reflection_symmetry_group_variables_(MeshCuboidReflectionSymmetryGroup::num_axis_parameters())
	, num_rotation_symmetry_group_variables_(MeshCuboidRotationSymmetryGroup::num_axis_parameters())
{
	num_cuboids_ = _cuboids.size();
	//assert(num_cuboids_ > 0);

	num_reflection_symmetry_groups_ = _reflection_symmetry_groups.size();
	//assert(num_reflection_symmetry_groups_ > 0);
	num_rotation_symmetry_groups_ = _rotation_symmetry_groups.size();
	//assert(num_rotation_symmetry_groups_ > 0);

	cuboid_corner_variable_start_index_ = 0;
	cuboid_axis_variable_start_index_ = cuboid_corner_variable_start_index_
		+ num_total_cuboid_corner_variables();
	reflection_symmetry_group_variable_start_index_ = cuboid_axis_variable_start_index_
		+ num_total_cuboid_axis_variables();
	rotation_symmetry_group_variable_start_index_ = reflection_symmetry_group_variable_start_index_
		+ num_total_reflection_symmetry_group_variables();
}

MeshCuboidNonLinearSolver::~MeshCuboidNonLinearSolver()
{

}

unsigned int MeshCuboidNonLinearSolver::num_total_cuboid_corner_variables() const
{
	return num_cuboid_corner_variables_ * num_cuboids_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_cuboid_axis_variables() const
{
	return num_cuboid_axis_variables_ * num_cuboids_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_reflection_symmetry_group_variables() const
{
	return num_reflection_symmetry_groups_ * num_reflection_symmetry_group_variables_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_rotation_symmetry_group_variables() const
{
	return num_rotation_symmetry_groups_ * num_rotation_symmetry_group_variables_;
}

unsigned int MeshCuboidNonLinearSolver::num_total_variables() const
{
	return num_total_cuboid_corner_variables()
		+ num_total_cuboid_axis_variables()
		+ num_total_reflection_symmetry_group_variables()
		+ num_total_rotation_symmetry_group_variables();
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_vector_variable(
	const std::pair<Index, Index>& _index_size_pair)
{
	assert(_index_size_pair.second > 0);
	NLPVectorExpression variable(_index_size_pair.second);
	for (unsigned int i = 0; i < _index_size_pair.second; ++i)
		variable[i] = NLPExpression(1.0, _index_size_pair.first + i);
	return variable;
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_cuboid_corner_variable_index_size(
	unsigned int _cuboid_index, unsigned int _corner_index) const
{
	const unsigned int dimension = 3;
	assert(_corner_index < MeshCuboid::k_num_corners);

	unsigned int index = cuboid_corner_variable_start_index_
		+ num_cuboid_corner_variables_ * _cuboid_index
		+ dimension * _corner_index;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_cuboid_axis_variable_index_size(
	unsigned int _cuboid_index, unsigned int _axis_index) const
{
	const unsigned int dimension = 3;
	assert(_axis_index < dimension);

	unsigned int index = cuboid_axis_variable_start_index_
		+ num_cuboid_axis_variables_ * _cuboid_index
		+ dimension * _axis_index;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_reflection_symmetry_group_variable_n_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_reflection_symmetry_groups_);

	unsigned int index = reflection_symmetry_group_variable_start_index_
		+ num_reflection_symmetry_group_variables_ * _symmetry_group_index
		+ 0;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_reflection_symmetry_group_variable_t_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_reflection_symmetry_groups_);

	unsigned int index = reflection_symmetry_group_variable_start_index_
		+ num_reflection_symmetry_group_variables_ * _symmetry_group_index
		+ dimension;
	return std::make_pair(index, 1);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_rotation_symmetry_group_variable_n_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_rotation_symmetry_groups_);

	unsigned int index = rotation_symmetry_group_variable_start_index_
		+ num_rotation_symmetry_group_variables_ * _symmetry_group_index
		+ 0;
	return std::make_pair(index, dimension);
}

std::pair<Index, Index> MeshCuboidNonLinearSolver::get_rotation_symmetry_group_variable_t_index_size(
	unsigned int _symmetry_group_index) const
{
	const unsigned int dimension = 3;
	assert(_symmetry_group_index < num_rotation_symmetry_groups_);

	unsigned int index = rotation_symmetry_group_variable_start_index_
		+ num_rotation_symmetry_group_variables_ * _symmetry_group_index
		+ dimension;
	return std::make_pair(index, dimension);
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_cuboid_corner_variable(
	unsigned int _cuboid_index, unsigned int _corner_index) const
{
	return create_vector_variable(get_cuboid_corner_variable_index_size(
		_cuboid_index, _corner_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_cuboid_axis_variable(
	unsigned int _cuboid_index, unsigned int _axis_index) const
{
	return create_vector_variable(get_cuboid_axis_variable_index_size(
		_cuboid_index, _axis_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_reflection_symmetry_group_variable_n(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_reflection_symmetry_group_variable_n_index_size(
		_symmetry_group_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_reflection_symmetry_group_variable_t(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_reflection_symmetry_group_variable_t_index_size(
		_symmetry_group_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_rotation_symmetry_group_variable_n(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_rotation_symmetry_group_variable_n_index_size(
		_symmetry_group_index));
}

NLPVectorExpression MeshCuboidNonLinearSolver::create_rotation_symmetry_group_variable_t(
	unsigned int _symmetry_group_index) const
{
	return create_vector_variable(get_rotation_symmetry_group_variable_t_index_size(
		_symmetry_group_index));
}

void MeshCuboidNonLinearSolver::create_energy_functions(
	const Eigen::MatrixXd &_cuboid_quadratic_term,
	const Eigen::VectorXd &_cuboid_linear_term,
	const double _cuboid_constant_term,
	std::vector<NLPFunction *> &_functions)
{
	if (_cuboid_quadratic_term.rows() != num_total_cuboid_corner_variables()
		|| _cuboid_quadratic_term.cols() != num_total_cuboid_corner_variables()
		|| _cuboid_linear_term.rows() != num_total_cuboid_corner_variables())
	{
		std::cerr << "Error: " << std::endl;
		assert(false);
	}

	_functions.clear();

	Eigen::MatrixXd quadratic_term = Eigen::MatrixXd::Zero(num_total_variables(), num_total_variables());
	Eigen::VectorXd linear_term = Eigen::VectorXd::Zero(num_total_variables());
	double constant_term = _cuboid_constant_term;

	quadratic_term.block(0, 0,
		num_total_cuboid_corner_variables(), num_total_cuboid_corner_variables()) = _cuboid_quadratic_term;
	linear_term.segment(0, num_total_cuboid_corner_variables()) = _cuboid_linear_term;

	NLPFunction *function_1 = new NLPEigenQuadFunction(quadratic_term, linear_term, constant_term);
	_functions.push_back(function_1);

	//
	NLPFunction *fuction_2 = create_reflection_symmetry_group_energy_function();
	assert(fuction_2);
	_functions.push_back(fuction_2);

	NLPFunction *fuction_3 = create_rotation_symmetry_group_energy_function();
	assert(fuction_3);
	_functions.push_back(fuction_3);
	//
}

NLPFunction *MeshCuboidNonLinearSolver::create_reflection_symmetry_group_energy_function()
{
	Eigen::MatrixXd quadratic_term = Eigen::MatrixXd::Zero(num_total_variables(), num_total_variables());
	Eigen::VectorXd linear_term = Eigen::VectorXd::Zero(num_total_variables());
	double constant_term = 0;

	std::vector<ANNpointArray> cuboid_ann_points;
	std::vector<ANNkd_tree *> cuboid_ann_kd_tree;

	create_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		create_reflection_symmetry_group_energy_function(symmetry_group_index,
			cuboid_ann_points, cuboid_ann_kd_tree,
			quadratic_term, linear_term, constant_term);
	}

	delete_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	NLPFunction *function = new NLPEigenQuadFunction(quadratic_term, linear_term, constant_term);
	return function;
}

void MeshCuboidNonLinearSolver::create_reflection_symmetry_group_energy_function(
	const unsigned int _symmetry_group_index,
	const std::vector<ANNpointArray>& _cuboid_ann_points,
	const std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree,
	Eigen::MatrixXd& _quadratic_term,
	Eigen::VectorXd& _linear_term,
	double &_constant_term)
{
	const MeshCuboidSymmetryGroup* symmetry_group = reflection_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	std::list<MeshCuboidSymmetryGroup::WeightedPointPair> sample_point_pairs;
	symmetry_group->get_symmetric_sample_point_pairs(cuboids_,
		_cuboid_ann_points, _cuboid_ann_kd_tree, squared_neighbor_distance_,
		sample_point_pairs);

	if (sample_point_pairs.size() < min_num_symmetric_point_pairs_)
		return;

	// Eq (1):
	// min {(I - nn^T)(x - y)}^2. Let d = (x - y).
	// Since (I - nn^T)^2 = (I - nn^T),
	// => min d^T(I - nn^T)d = d^Td - d^Tnn^Td = d^Td - n^Tdd^Tn.

	// Eq (2):
	// min {n^T(x + y) - 2t}^2.

	Eigen::Matrix3d A1 = Eigen::Matrix3d::Zero();
	Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3 + 1, 3 + 1);
	Real c = 0;

	Real sum_weight = 0.0;
	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		assert((*it).weight_ >= 0);
		sum_weight += (*it).weight_;
	}

	if (sum_weight == 0)
		return;

	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		Real weight = (*it).weight_ / sum_weight;

		Eigen::Vector3d sum_p, diff_p;
		for (int i = 0; i < 3; ++i)
		{
			sum_p[i] = (*it).p1_[i] + (*it).p2_[i];
			diff_p[i] = (*it).p1_[i] - (*it).p2_[i];
		}

		A1 += (weight * (-1) * diff_p * diff_p.transpose());
		c += (weight * diff_p.transpose() * diff_p);

		Eigen::VectorXd b(3 + 1);
		b << sum_p, -2;
		A2 += (weight * b * b.transpose());
	}

	std::pair<Index, Index> index_size_pair;
	index_size_pair = get_reflection_symmetry_group_variable_n_index_size(_symmetry_group_index);

	_quadratic_term.block<3, 3>(index_size_pair.first, index_size_pair.first) +=
		symmetry_energy_term_weight_ * A1;

	// NOTE:
	// Variables 'n' and 't' are adjacent in the variable list.
	_quadratic_term.block<3 + 1, 3 + 1>(index_size_pair.first, index_size_pair.first) +=
		symmetry_energy_term_weight_ * A2;

	_constant_term += symmetry_energy_term_weight_ * c;
}

NLPFunction *MeshCuboidNonLinearSolver::create_rotation_symmetry_group_energy_function()
{
	NLPExpression expression;

	std::vector<ANNpointArray> cuboid_ann_points;
	std::vector<ANNkd_tree *> cuboid_ann_kd_tree;

	create_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		// Minimize \| t \|_2^2.
		NLPVectorExpression t_variable = create_rotation_symmetry_group_variable_t(symmetry_group_index);
		expression += NLPVectorExpression::dot_product(t_variable, t_variable);

		create_rotation_symmetry_group_energy_function(symmetry_group_index,
			cuboid_ann_points, cuboid_ann_kd_tree, expression);
	}

	delete_cuboid_sample_point_ann_trees(cuboid_ann_points, cuboid_ann_kd_tree);

	expression *= symmetry_energy_term_weight_;

	NLPFunction *function = new NLPSparseFunction(num_total_variables(), expression);
	return function;
}

void MeshCuboidNonLinearSolver::create_rotation_symmetry_group_energy_function(
	const unsigned int _symmetry_group_index,
	const std::vector<ANNpointArray>& _cuboid_ann_points,
	const std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree,
	NLPExpression &_expression)
{
	const MeshCuboidRotationSymmetryGroup* symmetry_group = rotation_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	NLPVectorExpression n_variable = create_rotation_symmetry_group_variable_n(_symmetry_group_index);
	NLPVectorExpression t_variable = create_rotation_symmetry_group_variable_t(_symmetry_group_index);

	std::list<MeshCuboidSymmetryGroup::WeightedPointPair> sample_point_pairs;
	symmetry_group->get_symmetric_sample_point_pairs(cuboids_,
		_cuboid_ann_points, _cuboid_ann_kd_tree, squared_neighbor_distance_,
		sample_point_pairs);

	if (sample_point_pairs.size() < min_num_symmetric_point_pairs_)
		return;

	Real sum_weight = 0.0;
	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		assert((*it).weight_ >= 0);
		sum_weight += (*it).weight_;
	}

	for (std::list<MeshCuboidSymmetryGroup::WeightedPointPair>::iterator it = sample_point_pairs.begin();
		it != sample_point_pairs.end(); ++it)
	{
		Real weight = (*it).weight_ / sum_weight;

		MyMesh::Normal n; MyMesh::Point t;
		symmetry_group->get_rotation_axis(n, t);

		int symmetry_order = static_cast<int>(std::round((*it).angle_ / symmetry_group->get_rotation_angle()));
		MyMesh::Point symmetry_point_1 = symmetry_group->get_symmetric_point((*it).p1_, symmetry_order);

		Eigen::Vector3d x_vec, y_vec;
		for (int i = 0; i < 3; ++i)
		{
			x_vec[i] = (*it).p1_[i];
			y_vec[i] = (*it).p2_[i];
		}

		// Debug.
		//Eigen::Vector3d n_vec, t_vec, symm_x_vec;
		//for (int i = 0; i < 3; ++i)
		//{
		//	n_vec[i] = n[i];
		//	t_vec[i] = t[i];
		//	symm_x_vec[i] = symmetry_point_1[i];
		//}

		//Eigen::AngleAxisd axis_rotation((*it).angle_, n_vec);
		//Eigen::VectorXd transf_x_vec = axis_rotation.toRotationMatrix() * (x_vec - t_vec) + t_vec;

		//Eigen::VectorXd diff_1 = transf_x_vec - symm_x_vec;
		//if (diff_1.norm() > 1.0E-8)
		//{
		//	std::cout << "distance = " << diff_1.norm() << std::endl;
		//	std::cout << "angle = " << (*it).angle_ << std::endl;
		//	std::cout << "symmetry_order = " << symmetry_order << std::endl;
		//	std::cout << symm_x_vec.transpose() << std::endl;
		//	std::cout << transf_x_vec.transpose() << std::endl;
		//	system("pause");
		//}

		//Eigen::VectorXd diff_2 = transf_x_vec - y_vec;
		//if (diff_2.norm() > neighbor_distance_)
		//{
		//	std::cout << "distance = " << diff_2.norm() << std::endl;
		//	std::cout << "neighbor_distance = " << neighbor_distance_ << std::endl;
		//	std::cout << "weight = " << (neighbor_distance_ - sqrt((*it).weight_)) << std::endl;
		//	std::cout << symm_x_vec.transpose() << std::endl;
		//	std::cout << transf_x_vec.transpose() << std::endl;
		//	system("pause");
		//}

		Real sin_angle = std::sin((*it).angle_);
		Real cos_angle = std::cos((*it).angle_);

		NLPVectorExpression x_minus_t = (x_vec - t_variable);
		NLPVectorExpression y_minus_t = (y_vec - t_variable);

		NLPVectorExpression term_1 = x_minus_t * cos_angle;
		NLPVectorExpression term_2 = NLPVectorExpression::cross_product(n_variable, x_minus_t) * sin_angle;
		NLPVectorExpression term_3 = n_variable * NLPVectorExpression::dot_product(n_variable, x_minus_t) * (1 - cos_angle);
		NLPVectorExpression all_terms = term_1 + term_2 + term_3 - y_minus_t;
		NLPExpression squared_all_terms = NLPVectorExpression::dot_product(all_terms, all_terms);

		// Debug.
		//Number *x = new Number[num_total_variables()];
		//std::pair<Index, Index> n_index_size = get_rotation_symmetry_group_variable_n_index_size(_symmetry_group_index);
		//std::pair<Index, Index> t_index_size = get_rotation_symmetry_group_variable_t_index_size(_symmetry_group_index);
		//for (Index i = 0; i < n_index_size.second; ++i)
		//	x[n_index_size.first + i] = n[i];
		//for (Index i = 0; i < t_index_size.second; ++i)
		//	x[t_index_size.first + i] = t[i];
		//std::cout << "eval = " << squared_all_terms.eval(x) << std::endl;

		//Eigen::Vector3d term_1_vec = cos_angle * (x_vec - t_vec);
		//Eigen::Vector3d term_2_vec = sin_angle * n_vec.cross(x_vec - t_vec);
		//Eigen::Vector3d term_3_vec = (1 - cos_angle) * n_vec.dot(x_vec - t_vec) * n_vec;
		//Eigen::Vector3d term_4_vec = -(y_vec - t_vec);
		//Eigen::Vector3d all_terms_vec = term_1_vec + term_2_vec + term_3_vec + term_4_vec;
		//std::cout << "eval = " << all_terms_vec.squaredNorm() << std::endl;

		//delete[] x;
		//system("pause");
		//

		_expression += squared_all_terms;
	}
}

void MeshCuboidNonLinearSolver::add_constraints(NLPFormulation &_formulation)
{
	add_cuboid_constraints(_formulation);
	add_reflection_symmetry_group_constraints(_formulation);
	add_rotation_symmetry_group_constraints(_formulation);
}

void MeshCuboidNonLinearSolver::add_cuboid_constraints(NLPFormulation &_formulation)
{
	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		add_cuboid_constraints(cuboid_index, _formulation);
	}
}

void MeshCuboidNonLinearSolver::add_reflection_symmetry_group_constraints(NLPFormulation &_formulation)
{
	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		add_reflection_symmetry_group_constraints(symmetry_group_index, _formulation);
	}
}

void MeshCuboidNonLinearSolver::add_rotation_symmetry_group_constraints(NLPFormulation &_formulation)
{
	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		add_rotation_symmetry_group_constraints(symmetry_group_index, _formulation);
	}
}

void MeshCuboidNonLinearSolver::add_cuboid_constraints(
	const unsigned int _cuboid_index,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = 3;

	for (unsigned int axis_index = 0; axis_index < dimension; ++axis_index)
	{
		NLPVectorExpression axis_variable_1 = create_cuboid_axis_variable(_cuboid_index, axis_index);

		// Unit vector constraint.
		_formulation.add_constraint(NLPVectorExpression::dot_product(
			axis_variable_1, axis_variable_1), 1, 1);

		// Orthogonality constraint.
		if (dimension >= 2)
		{
			NLPVectorExpression axis_variable_2 = create_cuboid_axis_variable(_cuboid_index,
				(axis_index + 1) % dimension);
			assert(axis_variable_1.dimension() == axis_variable_2.dimension());

			// NOTE:
			// Relaxing equality constraint.
			// The equality constraints cause "too few degrees" errors.
			_formulation.add_constraint(NLPVectorExpression::dot_product(
				axis_variable_1, axis_variable_2), -1.0E-12, 1.0E-12);
		}
	}

	for (int axis_index = 0; axis_index < dimension; ++axis_index)
	{
		// NOTE:
		// Implemented only for 3 dimension.
		assert(dimension == 3);

		for (int axis_1 = 0; axis_1 < 2; ++axis_1)
		{
			for (int axis_2 = 0; axis_2 < 2; ++axis_2)
			{
				std::bitset<3> bits;
				bits[(axis_index + 1) % 3] = axis_1;
				bits[(axis_index + 2) % 3] = axis_2;

				bits[axis_index] = false;
				int corner_index_1 = bits.to_ulong();

				bits[axis_index] = true;
				int corner_index_2 = bits.to_ulong();

				NLPVectorExpression corner_variable_1 = create_cuboid_corner_variable(_cuboid_index, corner_index_1);
				NLPVectorExpression corner_variable_2 = create_cuboid_corner_variable(_cuboid_index, corner_index_2);

				// A cuboid edge is orthogonal with two axes.
				for (int other_axis_index = 0; other_axis_index < dimension; ++other_axis_index)
				{
					if (other_axis_index == axis_index) continue;
					NLPVectorExpression other_axis_variable = create_cuboid_axis_variable(_cuboid_index, other_axis_index);

					// n^T(x - y) = 0.
					NLPExpression expression = NLPVectorExpression::dot_product(other_axis_variable,
						corner_variable_1 - corner_variable_2);

					// NOTE:
					// Relaxing equality constraint.
					// The equality constraints cause "too few degrees" errors.
					_formulation.add_constraint(expression, -1.0E-12, 1.0E-12);
				}
			}
		}
	}
}

void MeshCuboidNonLinearSolver::add_reflection_symmetry_group_constraints(
	const unsigned int _symmetry_group_index,
	NLPFormulation &_formulation)
{
	const MeshCuboidReflectionSymmetryGroup *symmetry_group = reflection_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	NLPVectorExpression n_variable = create_reflection_symmetry_group_variable_n(_symmetry_group_index);

	std::vector< unsigned int > single_cuboid_indices;
	symmetry_group->get_single_cuboid_indices(cuboids_, single_cuboid_indices);

	for (std::vector< unsigned int >::const_iterator it = single_cuboid_indices.begin();
		it != single_cuboid_indices.end(); ++it)
	{
		unsigned int cuboid_index = (*it);

		add_single_cuboid_reflection_constraints(
			cuboid_index,
			_symmetry_group_index,
			symmetry_group->get_aligned_global_axis_index(),
			_formulation);
	}

	std::vector< std::pair<unsigned int, unsigned int> > pair_cuboid_indices;
	symmetry_group->get_pair_cuboid_indices(cuboids_, pair_cuboid_indices);

	for (std::vector< std::pair<unsigned int, unsigned int> >::const_iterator it = pair_cuboid_indices.begin();
		it != pair_cuboid_indices.end(); ++it)
	{
		add_pair_cuboid_reflection_constraints(
			(*it).first, (*it).second,
			_symmetry_group_index,
			symmetry_group->get_aligned_global_axis_index(),
			_formulation);
	}

	// Unit vector constraint.
	NLPExpression expression = NLPVectorExpression::dot_product(
		n_variable, n_variable);
	_formulation.add_constraint(expression, 1, 1);
}

void MeshCuboidNonLinearSolver::add_reflection_constraints(
	const NLPVectorExpression& _x_variable,
	const NLPVectorExpression& _y_variable,
	const NLPVectorExpression& _n_variable,
	const NLPVectorExpression& _t_variable,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = _x_variable.dimension();
	assert(_y_variable.dimension() == dimension);
	assert(_n_variable.dimension() == dimension);
	assert(_t_variable.dimension() == 1);

	// n^T(x + y) - 2t = 0.
	NLPExpression expression_1 = NLPVectorExpression::dot_product(_n_variable,
		_x_variable + _y_variable)
		+ (_t_variable[0] * -2);

	// NOTE:
	// Relaxing equality constraint.
	// The equality constraints cause "too few degrees" errors.
	_formulation.add_constraint(expression_1, -1.0E-12, 1.0E-12);

	// n^T(x - y).
	NLPExpression temp = NLPVectorExpression::dot_product(_n_variable,
		_x_variable - _y_variable);

	// (I - nn^T)(x - y) = (x - y) - n(n^T(x - y)) = 0.
	NLPVectorExpression expression_2 = (_x_variable - _y_variable) - (_n_variable * temp);

	// NOTE:
	// Relaxing equality constraint.
	// The equality constraints cause "too few degrees" errors.
	_formulation.add_constraint(expression_2, -1.0E-12, 1.0E-12);
}

void MeshCuboidNonLinearSolver::add_single_cuboid_reflection_constraints(
	const unsigned int _cuboid_index,
	const unsigned int _symmetry_group_index,
	const unsigned int _reflection_axis_index,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = 3;
	assert(_reflection_axis_index < dimension);

	bool *is_corner_visited = new bool[MeshCuboid::k_num_corners];
	memset(is_corner_visited, false, MeshCuboid::k_num_corners * sizeof(bool));

	for (unsigned int corner_index_1 = 0; corner_index_1 < MeshCuboid::k_num_corners; ++corner_index_1)
	{
		if (is_corner_visited[corner_index_1])
			continue;

		std::bitset<3> bits(corner_index_1);
		bits[_reflection_axis_index].flip();
		unsigned int corner_index_2 = bits.to_ulong();

		assert(!is_corner_visited[corner_index_2]);

		add_reflection_constraints(
			create_cuboid_corner_variable(_cuboid_index, corner_index_1),
			create_cuboid_corner_variable(_cuboid_index, corner_index_2),
			create_reflection_symmetry_group_variable_n(_symmetry_group_index),
			create_reflection_symmetry_group_variable_t(_symmetry_group_index),
			_formulation);

		is_corner_visited[corner_index_1] = is_corner_visited[corner_index_2] = true;
	}

	delete[] is_corner_visited;
}

void MeshCuboidNonLinearSolver::add_pair_cuboid_reflection_constraints(
	const unsigned int _cuboid_index_1,
	const unsigned int _cuboid_index_2,
	const unsigned int _symmetry_group_index,
	const unsigned int _reflection_axis_index,
	NLPFormulation &_formulation)
{
	const unsigned int dimension = 3;
	assert(_reflection_axis_index < dimension);

	for (unsigned int corner_index_1 = 0; corner_index_1 < MeshCuboid::k_num_corners; ++corner_index_1)
	{
		std::bitset<3> bits(corner_index_1);
		bits[_reflection_axis_index].flip();
		unsigned int corner_index_2 = bits.to_ulong();

		add_reflection_constraints(
			create_cuboid_corner_variable(_cuboid_index_1, corner_index_1),
			create_cuboid_corner_variable(_cuboid_index_2, corner_index_2),
			create_reflection_symmetry_group_variable_n(_symmetry_group_index),
			create_reflection_symmetry_group_variable_t(_symmetry_group_index),
			_formulation);
	}
}

void MeshCuboidNonLinearSolver::add_rotation_symmetry_group_constraints(
	const unsigned int _symmetry_group_index,
	NLPFormulation &_formulation)
{
	const MeshCuboidRotationSymmetryGroup *symmetry_group = rotation_symmetry_groups_[_symmetry_group_index];
	assert(symmetry_group);

	const unsigned int dimension = 3;
	NLPVectorExpression n_variable = create_rotation_symmetry_group_variable_n(_symmetry_group_index);
	NLPVectorExpression t_variable = create_rotation_symmetry_group_variable_t(_symmetry_group_index);

	std::vector< unsigned int > single_cuboid_indices;
	symmetry_group->get_single_cuboid_indices(cuboids_, single_cuboid_indices);

	for (std::vector< unsigned int >::const_iterator it = single_cuboid_indices.begin();
		it != single_cuboid_indices.end(); ++it)
	{
		unsigned int cuboid_index = (*it);

		NLPVectorExpression axis_variable = create_cuboid_axis_variable(cuboid_index,
			symmetry_group->get_aligned_global_axis_index());

		// The symmetry axis should be the same direction with the cuboid axis.
		NLPVectorExpression expression_1 = n_variable - axis_variable;
		_formulation.add_constraint(NLPVectorExpression::dot_product(expression_1, expression_1), 0, 0);

		NLPVectorExpression cuboid_center_variable(dimension);
		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			cuboid_center_variable += create_cuboid_corner_variable(cuboid_index, corner_index);
		cuboid_center_variable *= (1.0 / MeshCuboid::k_num_corners);

		//  cross(n, (c - t)) = 0.
		NLPVectorExpression expression_2 = NLPVectorExpression::cross_product(
			n_variable, cuboid_center_variable - t_variable);
		_formulation.add_constraint(NLPVectorExpression::dot_product(expression_2, expression_2), 0, 0);
	}

	// Ignore pairs of cuboids.

	// Unit vector constraint.
	NLPExpression expression = NLPVectorExpression::dot_product(
		n_variable, n_variable);
	_formulation.add_constraint(expression, 1, 1);
}

void MeshCuboidNonLinearSolver::fix_cuboid(
	const unsigned int _cuboid_index,
	NLPFormulation &_formulation)
{
	const MeshCuboid* cuboid = cuboids_[_cuboid_index];
	assert(cuboid);

	const unsigned int dimension = 3;

	for (unsigned int corner_index = 0; corner_index < dimension; ++corner_index)
	{
		NLPVectorExpression axis_variable = create_cuboid_axis_variable(_cuboid_index, corner_index);
		MyMesh::Normal axis = cuboid->get_bbox_axis(corner_index);
		Eigen::Vector3d axis_vec;
		for (unsigned int i = 0; i < 3; ++i)
			axis_vec[i] = axis[i];

		axis_variable -= axis_vec;
		_formulation.add_constraint(NLPVectorExpression::dot_product(axis_variable, axis_variable), 0, 0);
	}

	for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
	{
		NLPVectorExpression corner_variable = create_cuboid_corner_variable(_cuboid_index, corner_index);
		MyMesh::Normal corner = cuboid->get_bbox_corner(corner_index);
		Eigen::Vector3d corner_vec;
		for (unsigned int i = 0; i < 3; ++i)
			corner_vec[i] = corner[i];

		corner_variable -= corner_vec;
		_formulation.add_constraint(NLPVectorExpression::dot_product(corner_variable, corner_variable), 0, 0);
	}
}

bool MeshCuboidNonLinearSolver::compute_initial_values(const Eigen::VectorXd &_input,
	Eigen::VectorXd &_output)
{
	if (_input.rows() != num_total_cuboid_corner_variables())
	{
		std::cerr << "Error: " << std::endl;
		return false;
	}

	_output = Eigen::VectorXd::Zero(num_total_variables());
	_output.segment(0, num_total_cuboid_corner_variables()) = _input;

	compute_cuboid_axis_values(_output);
	compute_reflection_symmetry_group_values(_output);
	compute_rotation_symmetry_group_values(_output);

	// DEBUG.
	for (int i = 0; i < num_total_variables(); ++i)
	{
		if (isnan(_output[i]) || isinf(_output[i]))
		{
			assert(false);
		}
	}

	return true;
}

void MeshCuboidNonLinearSolver::compute_cuboid_axis_values(Eigen::VectorXd &_values)
{
	assert(_values.rows() >= num_total_variables());

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		const MeshCuboid* cuboid = cuboids_[cuboid_index];
		assert(cuboid);
		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			const MyMesh::Normal &axis = cuboid->get_bbox_axis(axis_index);

			std::pair<Index, Index> index_size_pair = get_cuboid_axis_variable_index_size(cuboid_index, axis_index);
			assert(index_size_pair.second == 3);
			for (unsigned int i = 0; i < index_size_pair.second; ++i)
				_values[index_size_pair.first + i] = axis[i];
		}
	}
}

void MeshCuboidNonLinearSolver::compute_reflection_symmetry_group_values(Eigen::VectorXd &_values)
{
	assert(_values.rows() >= num_total_variables());

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		const MeshCuboidReflectionSymmetryGroup* symmetry_group = reflection_symmetry_groups_[symmetry_group_index];
		assert(symmetry_group);

		MyMesh::Normal n; double t;
		symmetry_group->get_reflection_plane(n, t);

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_reflection_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == 3);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			_values[index_size_pair.first + i] = n[i];

		index_size_pair = get_reflection_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == 1);
		_values[index_size_pair.first] = t;
	}
}

void MeshCuboidNonLinearSolver::compute_rotation_symmetry_group_values(Eigen::VectorXd &_values)
{
	assert(_values.rows() >= num_total_variables());

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		const MeshCuboidRotationSymmetryGroup* symmetry_group = rotation_symmetry_groups_[symmetry_group_index];
		assert(symmetry_group);

		MyMesh::Normal n; MyMesh::Point t;
		symmetry_group->get_rotation_axis(n, t);

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_rotation_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == 3);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			_values[index_size_pair.first + i] = n[i];

		index_size_pair = get_rotation_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == 3);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			_values[index_size_pair.first + i] = t[i];
	}
}

void MeshCuboidNonLinearSolver::optimize(
	const Eigen::MatrixXd& _cuboid_quadratic_term,
	const Eigen::VectorXd& _cuboid_linear_term,
	const double _cuboid_constant_term,
	Eigen::VectorXd* _init_values_vec,
	const std::vector<unsigned int> *_fixed_cuboid_indices)
{
	// Update rotation angle.
	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		MeshCuboidRotationSymmetryGroup* symmetry_group = rotation_symmetry_groups_[symmetry_group_index];
		assert(symmetry_group);
		symmetry_group->compute_rotation_angle(cuboids_);
	}

	std::vector<NLPFunction *> functions;
	create_energy_functions(_cuboid_quadratic_term, _cuboid_linear_term, _cuboid_constant_term, functions);
	NLPFormulation formulation(functions);
	add_constraints(formulation);

	if (_fixed_cuboid_indices)
	{
		for (std::vector<unsigned int>::const_iterator it = (*_fixed_cuboid_indices).begin();
			it != (*_fixed_cuboid_indices).end(); ++it)
		{
			assert((*it) < cuboids_.size());
			fix_cuboid((*it), formulation);
		}
	}

	if (_init_values_vec)
	{
		Eigen::VectorXd all_init_values_vec;
		bool ret = compute_initial_values(*_init_values_vec, all_init_values_vec);
		assert(ret);
		formulation.set_values(all_init_values_vec.data());

		std::vector< Number > all_init_values(all_init_values_vec.rows());
		for (int i = 0; i < all_init_values_vec.rows(); ++i)
			all_init_values[i] = all_init_values_vec[i];

		std::cout << "Initial error = ";
		for (int i = 0; i < functions.size(); ++i)
			std::cout << functions[i]->eval(&all_init_values[0]) << ", ";
		std::cout << std::endl;
	}

	// ---- //
	// Create a new instance of your nlp
	//  (use a SmartPtr, not raw)
	SmartPtr<TNLP> mynlp = new IPOPTSolver(&formulation);
	//SmartPtr<TNLP> mynlp = new HS071_NLP();

	// Create a new instance of IpoptApplication
	//  (use a SmartPtr, not raw)
	// We are using the factory, since this allows us to compile this
	// example with an Ipopt Windows DLL
	SmartPtr<IpoptApplication> app = IpoptApplicationFactory();
	//app->RethrowNonIpoptException(true);

	// Change some options
	// Note: The following choices are only examples, they might not be
	//       suitable for your optimization problem.
	app->Options()->SetNumericValue("tol", 1e-8);
	app->Options()->SetStringValue("mu_strategy", "adaptive");
	app->Options()->SetIntegerValue("print_level", 0);
	app->Options()->SetStringValue("fixed_variable_treatment", "relax_bounds");
	// app->Options()->SetStringValue("output_file", "ipopt.out");
	// The following overwrites the default name (ipopt.opt) of the
	// options file
	// app->Options()->SetStringValue("option_file_name", "ipopt.opt");

	// Initialize the IpoptApplication and process the options
	ApplicationReturnStatus status;
	status = app->Initialize();
	if (status != Solve_Succeeded) {
		std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
		assert(false);
	}

	// Ask Ipopt to solve the problem
	status = app->OptimizeTNLP(mynlp);

	if (status == Solve_Succeeded) {
		//std::cout << std::endl << std::endl << "*** The problem solved!" << std::endl;
	}
	else {
		std::cout << std::endl << std::endl << "*** The problem FAILED!"
			<< " (" << static_cast<int>(status) << ")" << std::endl;
		//assert(false);
	}

	// As the SmartPtrs go out of scope, the reference count
	// will be decremented and the objects will automatically
	// be deleted.
	// ---- //

	// DEBUG.
	//formulation.print_constraint_evaluations();

	std::vector< Number > output;
	formulation.get_values(output);
	assert(output.size() == num_total_variables());
	//std::cout << "final = " << formulation.eval_function(&(output[0])) << ")" << std::endl;

	std::cout << "Final error = ";
	for (int i = 0; i < functions.size(); ++i)
		std::cout << functions[i]->eval(&output[0]) << ", ";
	std::cout << std::endl;

	// Update symmetry groups.
	update(output);
}

void MeshCuboidNonLinearSolver::update(const std::vector< Number >& _values)
{
	update_cuboids(_values);
	update_reflection_symmetry_groups(_values);
	update_rotation_symmetry_groups(_values);
}

void MeshCuboidNonLinearSolver::update_cuboids(const std::vector< Number >& _values)
{
	const unsigned int dimension = 3;

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		MeshCuboid *cuboid = cuboids_[cuboid_index];
		assert(cuboid);

		MyMesh::Point new_bbox_center(0.0);
		std::array<MyMesh::Point, MeshCuboid::k_num_corners> new_bbox_corners;
		std::array<MyMesh::Normal, dimension> new_bbox_axes;

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			std::pair<Index, Index> index_size_pair;
			index_size_pair = get_cuboid_corner_variable_index_size(cuboid_index, corner_index);
			assert(index_size_pair.second == dimension);
			for (unsigned int i = 0; i < index_size_pair.second; ++i)
				new_bbox_corners[corner_index][i] = _values[index_size_pair.first + i];

			new_bbox_center += new_bbox_corners[corner_index];
		}
		new_bbox_center = new_bbox_center / MeshCuboid::k_num_corners;

		for (unsigned int axis_index = 0; axis_index < dimension; ++axis_index)
		{
			std::pair<Index, Index> index_size_pair;
			index_size_pair = get_cuboid_axis_variable_index_size(cuboid_index, axis_index);
			assert(index_size_pair.second == dimension);
			for (unsigned int i = 0; i < index_size_pair.second; ++i)
				new_bbox_axes[axis_index][i] = _values[index_size_pair.first + i];

			new_bbox_axes[axis_index].normalize();

			// Flip axes.
			MyMesh::Point minus_axis_direction(0.0), plus_axis_direction(0.0);

			for (int axis_1 = 0; axis_1 < 2; ++axis_1)
			{
				for (int axis_2 = 0; axis_2 < 2; ++axis_2)
				{
					std::bitset<3> bits;
					bits[(axis_index + 1) % 3] = axis_1;
					bits[(axis_index + 2) % 3] = axis_2;

					bits[axis_index] = false;
					int minus_corner_index = bits.to_ulong();
					minus_axis_direction += new_bbox_corners[minus_corner_index];

					bits[axis_index] = true;
					int plus_corner_index = bits.to_ulong();
					plus_axis_direction += new_bbox_corners[plus_corner_index];
				}
			}

			// Compare the axis with the direction from the center to the average of corner points on +/- side.
			minus_axis_direction = (minus_axis_direction / 4.0 - new_bbox_center).normalized();
			plus_axis_direction = (plus_axis_direction / 4.0 - new_bbox_center).normalized();

			//if (dot(new_bbox_axes[axis_index], minus_axis_direction)
			//	> dot(new_bbox_axes[axis_index], plus_axis_direction))
			if (dot(new_bbox_axes[axis_index], cuboid->get_bbox_axis(axis_index)) < 0)
			{
				// Flip.
				new_bbox_axes[axis_index] = -new_bbox_axes[axis_index];
			}
		}

		cuboid->set_bbox_center(new_bbox_center);
		cuboid->set_bbox_corners(new_bbox_corners);
		cuboid->set_bbox_axes(new_bbox_axes, false);

		// Update cuboid surface points.
		for (unsigned int point_index = 0; point_index < cuboid->num_cuboid_surface_points();
			++point_index)
		{
			MeshCuboidSurfacePoint *cuboid_surface_point = cuboid->get_cuboid_surface_point(point_index);

			MyMesh::Point new_point(0.0);
			for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
				new_point += cuboid_surface_point->corner_weights_[corner_index] * new_bbox_corners[corner_index];

			cuboid_surface_point->point_ = new_point;
		}
	}
}

void MeshCuboidNonLinearSolver::update_reflection_symmetry_groups(const std::vector< Number >& _values)
{
	const unsigned int dimension = 3;

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_reflection_symmetry_groups_;
		++symmetry_group_index)
	{
		MyMesh::Normal n; double t;

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_reflection_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == dimension);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			n[i] = _values[index_size_pair.first + i];

		index_size_pair = get_reflection_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == 1);
		t = _values[index_size_pair.first];

		reflection_symmetry_groups_[symmetry_group_index]->set_reflection_plane(n, t);
	}
}

void MeshCuboidNonLinearSolver::update_rotation_symmetry_groups(const std::vector< Number >& _values)
{
	const unsigned int dimension = 3;

	for (unsigned int symmetry_group_index = 0; symmetry_group_index < num_rotation_symmetry_groups_;
		++symmetry_group_index)
	{
		MyMesh::Normal n; MyMesh::Point t;

		std::pair<Index, Index> index_size_pair;
		index_size_pair = get_rotation_symmetry_group_variable_n_index_size(symmetry_group_index);
		assert(index_size_pair.second == dimension);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			n[i] = _values[index_size_pair.first + i];

		index_size_pair = get_rotation_symmetry_group_variable_t_index_size(symmetry_group_index);
		assert(index_size_pair.second == dimension);
		for (unsigned int i = 0; i < index_size_pair.second; ++i)
			t[i] = _values[index_size_pair.first + i];

		rotation_symmetry_groups_[symmetry_group_index]->set_rotation_axis(n, t);
	}
}

void MeshCuboidNonLinearSolver::create_cuboid_sample_point_ann_trees(
	std::vector<ANNpointArray>& _cuboid_ann_points,
	std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree) const
{
	delete_cuboid_sample_point_ann_trees(_cuboid_ann_points, _cuboid_ann_kd_tree);
	_cuboid_ann_points.resize(num_cuboids_);
	_cuboid_ann_kd_tree.resize(num_cuboids_);

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		_cuboid_ann_kd_tree[cuboid_index] = NULL;
		_cuboid_ann_points[cuboid_index] = NULL;

		MeshCuboid *cuboid = cuboids_[cuboid_index];
		unsigned int num_cuboid_sample_points = cuboid->num_sample_points();
		if (num_cuboid_sample_points == 0)
			continue;

		Eigen::MatrixXd cuboid_sample_points(3, num_cuboid_sample_points);

		for (unsigned int point_index = 0; point_index < num_cuboid_sample_points; ++point_index)
		{
			for (unsigned int i = 0; i < 3; ++i)
				cuboid_sample_points.col(point_index)(i) =
				cuboid->get_sample_point(point_index)->point_[i];
		}

		_cuboid_ann_kd_tree[cuboid_index] = ICP::create_kd_tree(cuboid_sample_points,
			_cuboid_ann_points[cuboid_index]);
		assert(_cuboid_ann_points[cuboid_index]);
		assert(_cuboid_ann_kd_tree[cuboid_index]);
	}
}

void MeshCuboidNonLinearSolver::delete_cuboid_sample_point_ann_trees(
	std::vector<ANNpointArray>& _cuboid_ann_points,
	std::vector<ANNkd_tree *>& _cuboid_ann_kd_tree) const
{
	if (_cuboid_ann_points.empty() && _cuboid_ann_kd_tree.empty())
		return;

	assert(_cuboid_ann_points.size() == num_cuboids_);
	assert(_cuboid_ann_kd_tree.size() == num_cuboids_);

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids_; ++cuboid_index)
	{
		if (_cuboid_ann_points[cuboid_index]) annDeallocPts(_cuboid_ann_points[cuboid_index]);
		if (_cuboid_ann_kd_tree[cuboid_index]) delete _cuboid_ann_kd_tree[cuboid_index];
	}

	_cuboid_ann_points.clear();
	_cuboid_ann_kd_tree.clear();
}


#include <vector>
#include <crypto++/tiger.h>
#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include <bithorded/lib/treestore.hpp>
#include <bithorded/lib/hashtree.hpp>
#include <bithorded/store/hashstore.hpp>

using namespace std;
namespace fs = boost::filesystem;

using namespace bithorded::store;

typedef HashTree< HashStore > TigerTree;

class TestArray : public bithorded::IDataArray {
	std::vector<byte> _storage;
public:
	TestArray(size_t size)
	{
		_storage.resize(size);
	}

	virtual ssize_t write ( uint64_t offset, const void* src, size_t size ) {
		std::copy(static_cast<const byte *>(src), static_cast<const byte *>(src)+size, _storage.begin()+offset);
		return size;
	}
	virtual uint64_t size() const {
		return _storage.size();
	}
	virtual ssize_t read ( uint64_t offset, size_t size, byte* buf ) const {
		auto start = _storage.begin();
		std::advance(start, offset);
		auto end(start);
		std::advance(end, size);

		std::copy(start, end, buf);
		return size;
	}
	virtual string describe() {
		ostringstream oss;
		oss << "(Vector of size: " << _storage.size() << ")";
		return oss.str();
	}
};

BOOST_AUTO_TEST_CASE( hashstore_exceptions )
{
	BOOST_CHECK_THROW( new HashStore(boost::make_shared<TestArray>(0)), ios_base::failure );
	BOOST_CHECK_THROW( new HashStore(boost::make_shared<TestArray>(1)), ios_base::failure );
}

BOOST_AUTO_TEST_CASE( assetmeta_random_sequence )
{
	const uint LEAVES = 7;

	auto array = boost::make_shared<TestArray>(treesize(LEAVES)*sizeof(TigerBaseNode));
	{
		HashStore store(array);
		TigerTree tree(store, 0);

		const auto unit_size = TigerTree::Hasher::ATOMSIZE;
		byte block[unit_size];
		bzero(block, sizeof(block));

		auto root = tree.getRoot();

		BOOST_CHECK_EQUAL( root->state, TigerBaseNode::State::EMPTY);

		tree.setData(0*unit_size, block, sizeof(block));
		tree.setData(1*unit_size, block, sizeof(block));

		tree.setData(6*unit_size, block, sizeof(block));

		tree.setData(4*unit_size, block, sizeof(block));
		tree.setData(5*unit_size, block, sizeof(block));
		tree.setData(3*unit_size, block, sizeof(block));

		BOOST_CHECK_EQUAL( root->state, TigerBaseNode::State::EMPTY);

		tree.setData(2*unit_size, block, sizeof(block));

		BOOST_CHECK_EQUAL( root->state, TigerBaseNode::State::SET );
		BOOST_CHECK_EQUAL( root->base32Digest(), "FPSZ35773WS4WGBVXM255KWNETQZXMTEJGFMLTA" );
	}

	{
		HashStore store(array);
		TigerTree tree(store, 0);
		auto root = tree.getRoot();

		BOOST_CHECK_EQUAL( root->state, TigerBaseNode::State::SET );
		BOOST_CHECK_EQUAL( root->base32Digest(), "FPSZ35773WS4WGBVXM255KWNETQZXMTEJGFMLTA" );
	}
}

BOOST_AUTO_TEST_CASE( wild_index_jump )
{
	const uint LEAVES = 16*1024;

	auto array = boost::make_shared<TestArray>(treesize(LEAVES)*sizeof(TigerBaseNode));
	HashStore store(array);

	auto first = store[0];
	first->state = 99;
	for (uint i = 1; i < LEAVES; i++) {
		auto current = store[i];
		current->state = 0;
	}
	BOOST_CHECK_EQUAL( first->state, 99 );
}


//
//  eigen.hpp
//  AxiSEM3D
//
//  Created by Kuangdai Leng on 1/7/19.
//  Copyright © 2019 Kuangdai Leng. All rights reserved.
//

//  Eigen3 header

#ifndef eigen_hpp
#define eigen_hpp

///////////////// preprocessors /////////////////
// no auto resizing
#define EIGEN_NO_AUTOMATIC_RESIZING

// no dynamic allocation once time loop begins
#define EIGEN_RUNTIME_NO_MALLOC

// disable static alignment to enhance code portability
#define EIGEN_MAX_STATIC_ALIGN_BYTES 0

// initialize matrix with NAN
#define EIGEN_INITIALIZE_MATRICES_BY_NAN

///////////////// include /////////////////
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Weverything"
#include <Eigen/Dense>
#pragma clang diagnostic pop

#endif /* eigen_hpp */


#include <complex>
#include <exception>
#include <fstream>
#include <sstream>
#include <numeric>
#include <iostream>
#include <string>
#include <vector>
#include <range/v3/all.hpp>
#include <cstdint>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/interval_set.hpp>
namespace icl = boost::icl;
using gint = std::int64_t;

int main(int argc, char **argv)
{
  if (argc > 1) {
    gint l, h;
    icl::interval_set<gint> ips;
    std::ifstream ifs(argv[1]);
    while (ifs >> l >> h) {
      icl::interval<gint>::type p(l, h);
      ips.add(p);
    }
    icl::interval_set<gint> ipsb = ips;
    auto j = ips.begin();
    ++j;
    for (auto i = ips.begin(); i != ips.end() && (j != ips.end()); ++i, ++j) {
      if (j->lower() - i->upper() == 1) {
        icl::interval<gint>::type p(i->upper(), j->lower());
        ipsb.add(p);
      }
    }
    gint count = 0;
    for (auto i = ipsb.begin(); i != ipsb.end(); ++i) {
      count += (i->upper() - i->lower() + 1);
    }
    std::cout << std::numeric_limits<unsigned int>::max() - count + 1 << std::endl;
  }

  return 0;
}


//			Copyright Gavin Band 2008 - 2012.
// 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)

#define EIGEN_RUNTIME_NO_MALLOC
#include <iostream>
#include <numeric>
#include <iomanip>
#include <vector>
#include <utility>
#include <algorithm>
#include <boost/iterator/counting_iterator.hpp>
#include "metro/regression/MultinomialRegressionLogLikelihood.hpp"
#include "metro/intersect_ranges.hpp"
#include "metro/summation.hpp"
#include "genfile/Error.hpp"
#include "genfile/string_utils/string_utils.hpp"

// #define DEBUG_LOGLIKELIHOOD 1

namespace metro {
	namespace regression {

		namespace {
			template< typename T >
			std::ostream& print_matrix( std::string const& name, T const& matrix, std::ostream& out ) {
				return out << name << " = (" << matrix.rows() << "x" << matrix.cols() << ")\n"
					<< matrix << "\n" ;
			}
		}

		MultinomialRegressionLogLikelihood::UniquePtr MultinomialRegressionLogLikelihood::create(
			regression::Design::UniquePtr design
		) {
			return MultinomialRegressionLogLikelihood::UniquePtr(
				new MultinomialRegressionLogLikelihood( design )
			) ;
		}

		MultinomialRegressionLogLikelihood::MultinomialRegressionLogLikelihood(
			regression::Design::UniquePtr design
		):
			m_design( design ),
			m_number_of_samples( m_design->outcome().rows() ),
			m_parameter_vector( Vector::Zero( m_design->matrix().cols() * ( m_design->outcome().cols() - 1 ) )),
			m_parameter_matrix( Matrix::Zero( m_design->matrix().cols(), m_design->outcome().cols() - 1 ) ),
			m_parameter_identity( compute_parameter_identity() ),
			m_numberOfDerivativesComputed( 0 )
		{
			assert( m_design.get() != 0 ) ;
		}

		std::string MultinomialRegressionLogLikelihood::get_summary() const {
			using genfile::string_utils::to_string ;
			return "MultinomialRegressionLogLikelihood( " + to_string( m_number_of_samples ) + " samples, " + to_string( m_design->outcome().cols() ) + " outcomes )" ;
		}

		void MultinomialRegressionLogLikelihood::set_predictor_levels(
			Matrix const& levels,
			Matrix const& probabilities,
			std::vector< metro::SampleRange > const& included_samples
		) {
			m_design->set_predictors( levels, probabilities, included_samples ) ;

#if DEBUG_LOGLIKELIHOOD
			int const print_N = std::min( m_design->outcome().rows(), Vector::Index( 10 ) ) ;
			int const number_of_outcomes = m_design->outcome().cols() ;
			std::cerr << "MultinomialRegressionLogLikelihood::number of outcomes = " << number_of_outcomes << "\n" ;
			std::cerr << "MultinomialRegressionLogLikelihood::set_predictor_levels(): Included samples:" ;
			for( std::size_t i = 0; i < included_samples.size(); ++i ) {
				std::cerr << " " << included_samples[i].begin() << "-" << included_samples[i].end() ;
			}
			std::cerr << "\n" ;
			Matrix const& design_matrix = m_design->set_predictor_level(0).matrix() ;
			std::cerr << "MultinomialRegressionLogLikelihood::design matrix = (" << design_matrix.rows() << "x" << design_matrix.cols() << ")\n"
				<< design_matrix.block( 0, 0, print_N, design_matrix.cols() ) << "\n" ;
			std::cerr << "MultinomialRegressionLogLikelihood::predictor levels = (" << levels.rows() << "x" << levels.cols() << ")\n"
				<< levels << "\n" ;
#endif
			compute_psi( m_design->outcome(), levels.rows(), included_samples, &m_psi ) ;
			compute_rearranger( levels.rows(), &m_outcome_wise_to_predictor_wise_rearranger ) ;
			m_numberOfDerivativesComputed = 0 ;

#if DEBUG_LOGLIKELIHOOD
			std::cerr << "MultinomialRegressionLogLikelihood:" ;
			std::cerr << "outcome =\n" << m_design->outcome().head( print_N ) << "\n" ;
			print_matrix( "psi", m_psi.block( 0, 0, print_N, m_psi.cols() ), std::cerr ) ;
#endif
		}

		void MultinomialRegressionLogLikelihood::compute_psi(
			Matrix const& outcome,
			int const number_of_predictor_levels,
			std::vector< metro::SampleRange > const& included_samples,
			Matrix* result
		) const {
			// psi is N x LM, thought of as M blocks of NxL.
			// For each sample, we put a row of L 1's in the columns corresponding to the outcome for that sample.
			assert( result != 0 ) ;

			int const number_of_outcomes = outcome.cols() ;
			result->setZero( m_number_of_samples, number_of_outcomes * number_of_predictor_levels ) ;
			for( int i = 0; i < included_samples.size(); ++i ) {
				int const start_row = included_samples[i].begin() ;
				int const end_row = included_samples[i].end() ;
				for( int sample = start_row; sample < end_row; ++sample ) {
					// look for the 1 identifying the outcome for this sample
					Matrix::Index outcome_i = 0 ;
					result->block( sample, outcome_i * number_of_predictor_levels, 1, number_of_predictor_levels ).setConstant( 1.0 ) ;
				}
			}
		}

		void MultinomialRegressionLogLikelihood::compute_rearranger( int const number_of_predictor_levels, PermutationMatrix* result ) const {
			// rearranger is MLxML.	 It is a square matrix which when multiplied on the right turns a matrix like
			// f1(x1) f2(x1) ... fM(x1) f1(x2)...etc (L blocks of NxM)
			// into
			// f1(x1) f1(x2) ... f1(xl) f2(x1)...etc (M blocks of NxL).
			int const number_of_outcomes = m_design->outcome().cols() ;
			result->setIdentity(
				number_of_predictor_levels * number_of_outcomes
			) ;
			int column = 0 ;
			for( int j = 0; j < number_of_outcomes; ++j ) {
				for( int level_i = 0; level_i < number_of_predictor_levels; ++level_i, ++column ) {
//					result->indices().data()[( level_i * number_of_outcomes + j )] = column ;
					result->indices().data()[ column ] = ( level_i * number_of_outcomes ) + j ;
				}
			}
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "Rearranger2:\n" << result->toDenseMatrix() << "\n" ;
#endif
		}

		MultinomialRegressionLogLikelihood::Vector const& MultinomialRegressionLogLikelihood::parameters() const {
			return m_parameter_vector ;
		}

		void MultinomialRegressionLogLikelihood::evaluate_at(
			Vector const& parameters,
			int const numberOfDerivatives
		) {
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "MultinomialRegressionLogLikelihood::evaluate_at(): parameters = " << parameters.transpose() << "\n" ;
#endif
			int const number_of_outcomes = m_design->outcome().cols() ;
			assert( parameters.size() == m_design->matrix().cols() * ( number_of_outcomes - 1 ) ) ;

			m_parameter_vector = parameters ;
			m_numberOfDerivativesComputed = 0 ;
			m_parameter_matrix.resize( m_design->matrix().cols(), number_of_outcomes - 1 ) ;
			for( int i = 0; i < m_parameter_vector.size(); ++i ) {
				m_parameter_matrix( m_parameter_identity( i, 1 ), m_parameter_identity( i, 0 ) - 1 ) = m_parameter_vector(i) ;
			}

			evaluate( numberOfDerivatives ) ;
		}

		void MultinomialRegressionLogLikelihood::evaluate( int const numberOfDerivatives ) {
			if( m_numberOfDerivativesComputed <= numberOfDerivatives ) {
				evaluate_impl( numberOfDerivatives ) ;
			}
		}

		void MultinomialRegressionLogLikelihood::set_parameter_naming_scheme( MultinomialRegressionLogLikelihood::GetParameterName get_parameter_name ) {
			assert( get_parameter_name ) ;
			m_get_parameter_name = get_parameter_name ;
		}

		std::string MultinomialRegressionLogLikelihood::get_parameter_name( std::size_t i ) const {
			IntegerMatrix const identity = identify_parameters() ;
			return m_get_parameter_name( m_design->get_predictor_name( identity(i,1)), identity(i,0) ) ;
		}

		LogLikelihood::IntegerMatrix MultinomialRegressionLogLikelihood::identify_parameters() const {
			return m_parameter_identity ;
		}

		int MultinomialRegressionLogLikelihood::number_of_parameters() const {
			return m_parameter_identity.rows() ;
		}

		int MultinomialRegressionLogLikelihood::number_of_outcomes() const {
			return m_design->outcome().cols() ;
		}

		LogLikelihood::IntegerMatrix MultinomialRegressionLogLikelihood::compute_parameter_identity() const {
			// each parameter corresponds to a column of the design matrix and a non-baseline phenotype level.
			IntegerMatrix result = IntegerMatrix::Zero( m_parameter_vector.size(), 2 ) ;
			int const D = m_design->matrix().cols() ;
			int const number_of_outcomes = m_design->outcome().cols() ;
			for( int j = 0; j < D; ++j ) {
				for( int i = 1; i < number_of_outcomes; ++i ) {
					result( (i-1)*D + j, 0 ) = i ;
					result( (i-1)*D + j, 1 ) = j ;
				}
			}
			return result ;
		}

		void MultinomialRegressionLogLikelihood::evaluate_impl(
			int const numberOfDerivatives
		) {
			std::vector< metro::SampleRange > const& included_samples = m_design->nonmissing_samples() ;
			int const number_of_outcomes = m_design->outcome().cols() ;
			assert( numberOfDerivatives < 3 ) ;

#if DEBUG_LOGLIKELIHOOD
			std::cerr << "parameters = " << m_parameter_vector.transpose() << ".\n" ;
			print_matrix( "parameter matrix", m_parameter_matrix, std::cerr ) ;
#endif
			compute_F( m_parameter_matrix, &m_F ) ;
			compute_A_and_function_value( m_F, m_design->get_predictor_level_probabilities(), included_samples, &m_A, &m_value_of_function ) ;
#if DEBUG_LOGLIKELIHOOD
			int const print_N = std::min( m_design->outcome().rows(), Vector::Index( 10 ) ) ;
			print_matrix( "F", m_F.block( 0, 0, print_N, m_F.cols() ), std::cerr ) ;
			print_matrix( "A", m_A.block( 0, 0, print_N, m_A.cols() ), std::cerr ) ;
			print_matrix( "psi", m_psi.block( 0, 0, print_N, m_psi.cols() ), std::cerr ) ;
			std::cerr << "function value = \n" << m_value_of_function << "\n" ;
#endif
			++m_numberOfDerivativesComputed ;
			if( numberOfDerivatives > 0 ) {
				compute_B( m_A, m_F, m_psi, &m_B ) ;
				compute_value_of_first_derivative( m_B, included_samples, &m_value_of_first_derivative, &m_first_derivative_terms ) ;
				++m_numberOfDerivativesComputed ;
#if DEBUG_LOGLIKELIHOOD
				print_matrix( "B", m_B.block( 0, 0, print_N, m_B.cols() ), std::cerr ) ;
				print_matrix( "C", m_C.block( 0, 0, print_N, m_C.cols() ), std::cerr ) ;
				std::cerr << "derivative = \n" << m_value_of_first_derivative << "\n" ;
#endif
				if( numberOfDerivatives > 1 ) {
					compute_C( m_A, m_F, m_psi, included_samples, &m_C ) ;
					compute_value_of_second_derivative( m_B, m_C, included_samples, &m_value_of_second_derivative ) ;
					++m_numberOfDerivativesComputed ;
#if DEBUG_LOGLIKELIHOOD
					print_matrix( "C", m_C.block( 0, 0, print_N, m_C.cols() ), std::cerr ) ;

					std::cerr << "2nd derivative = \n"
						<< m_value_of_second_derivative << ".\n" ;
#endif
				}
			}
		}

		void MultinomialRegressionLogLikelihood::compute_F( Matrix const& parameters, Matrix* result ) const {
			// F is NxLM, though of as (M+1) blocks of L.
			// We compute it as L blocks of (M+1), since this leads to simpler code, and then rearrange columns.
			int const number_of_levels = m_design->get_number_of_predictor_levels() ;
			int const number_of_outcomes = m_design->outcome().cols() ;
			result->setZero( m_number_of_samples, number_of_levels *  number_of_outcomes ) ;
			Vector sums = Vector::Zero( m_number_of_samples ) ;
			for( int level_i = 0; level_i < number_of_levels; ++level_i ) {
				Matrix const& design_matrix = m_design->set_predictor_level( level_i ).matrix() ;
				int const block_first_column = level_i * number_of_outcomes ;
				MatrixBlock block = result->block( 0, block_first_column, m_number_of_samples, number_of_outcomes ) ;
				// numerator for baseline outcome is 1
				block.col(0).setConstant( 1.0 ) ;
				// compute numerator for non-baseline outcomes
				MatrixBlock alternative_outcome_block = result->block( 0, block_first_column + 1, m_number_of_samples, number_of_outcomes - 1 ) ;
				alternative_outcome_block = ( design_matrix * parameters ).array().exp() ;
				// Normalise all outcome probabilities
				sums = alternative_outcome_block.rowwise().sum() ;
				for( int col = 0; col < block.cols(); ++col ) {
					block.col( col ).array() /= ( Vector::Constant( m_number_of_samples, 1.0 ) + sums ).array() ;
				}
			}
			// We've computed F in the order
			// f1(x1) f2(x1) ... fM(x1) f1(x2) ...
			// We now re-arrange so that it comes in the order
			// f1(x1) f1(x2) ... f1(xl) f2(x1)...
			// as this fits better with the later computations.
			// Split into seperate function for profiling.
			rearrange_F( result ) ;
		}

		void MultinomialRegressionLogLikelihood::rearrange_F( Matrix* result ) const {
			// We've computed F in the order
			// f1(x1) f2(x1) ... fM(x1) f1(x2) ...
			// We now re-arrange so that it comes in the order
			// f1(x1) f1(x2) ... f1(xl) f2(x1)...
			// as this fits better with the later computations.
			(*result) *= m_outcome_wise_to_predictor_wise_rearranger ;
		}

		void MultinomialRegressionLogLikelihood::compute_A_and_function_value(
			Matrix const& F,
			Matrix const& predictor_probabilities,
			std::vector< metro::SampleRange > const& included_samples,
			Matrix* result,
			double* value_of_function
		) const {
			// A is NxL.
			int const number_of_predictor_levels = predictor_probabilities.cols() ;
			*value_of_function = 0.0 ;
			result->setZero( m_number_of_samples, number_of_predictor_levels ) ;
			for( std::size_t i = 0; i < included_samples.size(); ++i ) {
				int const start_row = included_samples[i].begin() ;
				int const end_row = included_samples[i].end() ;
				for( int sample = start_row; sample < end_row; ++sample ) {
					// Look for the 1 indicating which outcome this sample has
					Matrix::Index outcome_i = 0 ;
					m_design->outcome().row( sample ).array().maxCoeff( &outcome_i ) ;
					// Find the F values corresponding to this sample's outcome
					ConstMatrixBlock block_of_F = F.block( sample, outcome_i * number_of_predictor_levels, 1, number_of_predictor_levels ) ;
					result->row( sample ) = predictor_probabilities.row( sample ).array() * block_of_F.array() ;
				}
				// Compute the log-likelihood for this block of samples as:
				// the sum over samples of...
				*value_of_function += neumaier_sum(
					// the likelihood terms for these samples...
					result->block( start_row, 0, end_row - start_row, number_of_predictor_levels )
					 // ...summed over predictor levels...
						.rowwise().sum()
					 // ...and logarithm-ed...
						.array().log()
				) ;
				// Now renormalise this block of A.
				for( int sample = start_row; sample < end_row; ++sample ) {
					result->row( sample ) /= result->row( sample ).sum() ;
				}
			}
		}

		void MultinomialRegressionLogLikelihood::compute_B( Matrix const& A, Matrix const& F, Matrix const& psi, Matrix* result ) const {
			// B is NxLM, thought of as M blocks of L.
			int const number_of_levels = m_design->get_number_of_predictor_levels() ;
			int const M = m_design->outcome().cols() - 1 ;
			result->setZero( m_number_of_samples, number_of_levels * M ) ;
			for( int j = 0; j < M; ++j ) {
				MatrixBlock block = result->block( 0, j * number_of_levels, m_number_of_samples, number_of_levels ) ;
				// We use j+1 here to skip the baseline columns in F and psi.
				ConstMatrixBlock block_of_F = F.block( 0, (j+1) * number_of_levels, m_number_of_samples, number_of_levels ) ;
				ConstMatrixBlock block_of_psi = psi.block( 0, (j+1) * number_of_levels, m_number_of_samples, number_of_levels ) ;
				block = A.array() * ( block_of_psi - block_of_F ).array() ;
			}
		}

		void MultinomialRegressionLogLikelihood::compute_value_of_first_derivative(
			Matrix const& B,
			std::vector< metro::SampleRange > const& included_samples,
			Vector* result,
			Matrix* terms
		) const {
			int const D = m_design->matrix().cols() ;
			int const number_of_levels = m_design->get_number_of_predictor_levels() ;
			int const M = m_design->outcome().cols() - 1 ;
			result->setZero( D * M ) ;
			terms->setZero( m_design->matrix().rows(), D*M ) ;
			for( int level_i = 0; level_i < number_of_levels; ++level_i ) {
				Matrix const& design_matrix = m_design->set_predictor_level( level_i ).matrix() ;
				for( int i = 0; i < included_samples.size(); ++i ) {
					int const start_row = included_samples[i].begin() ;
					int const end_row = included_samples[i].end() ;
					for( int j = 0; j < M; ++j ) {
						MatrixBlock block = terms->block( start_row, j*D, end_row - start_row, D ) ;
						// multiply each row of the design matrix by the relevant term in the relevant column of B.
						block += (
							B.col( j * number_of_levels + level_i ).segment( start_row, end_row - start_row ).asDiagonal()
							* design_matrix.block( start_row, 0, end_row - start_row, design_matrix.cols() )
						) ;
					}
				}
			}
			(*result) = m_first_derivative_terms.colwise().sum() ;
		}

		void MultinomialRegressionLogLikelihood::compute_C(
			Matrix const& A,
			Matrix const& F,
			Matrix const& psi,
			std::vector< metro::SampleRange > const& included_samples,
			Matrix* result
		) const {
			// B is NxLM^2, thought of as M blocks of ML, each block is M blocks of L.
			int const number_of_levels = m_design->get_number_of_predictor_levels() ;
			int const M = m_design->outcome().cols() - 1 ;
			int const outer_blocksize = number_of_levels * M ;
			result->setZero( m_number_of_samples, outer_blocksize * M ) ;
			for( int j = 0; j < M; ++j ) {
				ConstMatrixBlock jth_block_of_F = F.block( 0, (j+1) * number_of_levels, m_number_of_samples, number_of_levels ) ;
				ConstMatrixBlock jth_block_of_psi = psi.block( 0, (j+1) * number_of_levels, m_number_of_samples, number_of_levels ) ;
				for( int k = 0; k < M; ++k ) {
					ConstMatrixBlock kth_block_of_F = F.block( 0, (k+1) * number_of_levels, m_number_of_samples, number_of_levels ) ;
					ConstMatrixBlock kth_block_of_psi = psi.block( 0, (k+1) * number_of_levels, m_number_of_samples, number_of_levels ) ;
					Matrix Zjk = -kth_block_of_F ;
					if( j == k ) {
						Zjk.array() = 1.0 + Zjk.array() ;
					}
					result->block( 0, j * outer_blocksize + k * number_of_levels, m_number_of_samples, number_of_levels ).array()
						= A.array()
						* (
							(
								( jth_block_of_psi - jth_block_of_F ).array()
								* ( kth_block_of_psi - kth_block_of_F ).array()
							)
							- (
								jth_block_of_F.array() * Zjk.array()
							)
						) ;
				}
			}
		}

		void MultinomialRegressionLogLikelihood::compute_value_of_second_derivative(
			Matrix const& B,
			Matrix const& C,
			std::vector< metro::SampleRange > const& included_samples,
			Matrix* result
		) const {
			int const D = m_design->matrix().cols() ;
			int const number_of_levels = m_design->get_number_of_predictor_levels() ;
			int const M = m_design->outcome().cols() - 1 ;
			int const outer_blocksize = M * number_of_levels ;
			result->setZero( D*M, D*M ) ;
			m_design_matrix_tensor_square_rows.resize( m_design->matrix().rows(), D ) ;
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "MultinomialRegressionLogLikelihood::compute_value_of_second_derivative(): C =\n"
				<< C.block( 0, 0, std::min( int( C.rows() ), 10 ), C.cols() ) << "\n" ;
#endif
			for( int level_i = 0; level_i < number_of_levels; ++level_i ) {
				Matrix const& design_matrix = m_design->set_predictor_level( level_i ).matrix() ;
				for( int row_i = 0; row_i < D; ++row_i ) {
#ifdef EIGEN_RUNTIME_NO_MALLOC
					Eigen::internal::set_is_malloc_allowed( false ) ;
#endif
					m_design_matrix_tensor_square_rows = design_matrix.col( row_i ).asDiagonal() * design_matrix ;
#ifdef EIGEN_RUNTIME_NO_MALLOC
					Eigen::internal::set_is_malloc_allowed( true ) ;
#endif
					for( int i = 0; i < included_samples.size(); ++i ) {
						int const start_row = included_samples[i].begin() ;
						int const end_row = included_samples[i].end() ;
						MatrixBlock design_matrix_tensor_square_row_block
							= m_design_matrix_tensor_square_rows.block( start_row, 0, end_row - start_row, design_matrix.cols() ) ;
						for( int j = 0; j < M; ++j ) {
							for( int k = j; k < M; ++k ) {
								MatrixBlock resultRow = result->block( j*D+row_i, k*D, 1, D ) ;
									resultRow
										+= (
											C.block( start_row, j * outer_blocksize + k*number_of_levels + level_i, end_row - start_row, 1 ).transpose()
											* design_matrix_tensor_square_row_block
										)
									;
							}
						}
					}
				}
			}
#ifdef EIGEN_RUNTIME_NO_MALLOC
			Eigen::internal::set_is_malloc_allowed( false ) ;
#endif
			for( int i = 0; i < included_samples.size(); ++i ) {
				int const start_row = included_samples[i].begin() ;
				int const end_row = included_samples[i].end() ;
				for( int row_i = 0; row_i < M*D; ++row_i ) {
					MatrixBlock resultRow = result->block( row_i, 0, 1, M*D ) ;
					resultRow.noalias() -= (
						m_first_derivative_terms.col( row_i ).segment( start_row, end_row - start_row ).asDiagonal()
							* m_first_derivative_terms.block( start_row, 0, end_row - start_row, M*D )
						)
						.colwise().sum() ;
				}
			}
#ifdef EIGEN_RUNTIME_NO_MALLOC
			Eigen::internal::set_is_malloc_allowed( true ) ;
#endif

			// fill lower diagonal
			for( int j = 1; j < M; ++j ) {
				for( int k = 0; k < j; ++k ) {
					MatrixBlock target = result->block( j*D, k*D, D, D ) ;
					MatrixBlock resultBlock = result->block( k*D, j*D, D, D ) ;
					target.noalias() = resultBlock.transpose() ;
				}
			}
		}

		double MultinomialRegressionLogLikelihood::get_value_of_function() const {
			return m_value_of_function ;
		}

		MultinomialRegressionLogLikelihood::Vector MultinomialRegressionLogLikelihood::get_value_of_first_derivative() const {
			return m_value_of_first_derivative ;
		}

		MultinomialRegressionLogLikelihood::Matrix MultinomialRegressionLogLikelihood::get_value_of_second_derivative() const {
			return m_value_of_second_derivative ;
		}
	}
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_BITOFSIGN_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_BITOFSIGN_HPP_INCLUDED
#include <boost/simd/ieee/functions/bitofsign.hpp>
#include <boost/simd/include/constants/mzero.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitofsign_, tag::cpu_, (A0)(X)
                            , ((simd_<signed_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return b_and(a0, boost::simd::Signmask<A0>()); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitofsign_, tag::cpu_, (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(const A0&)const { return Zero<A0>(); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitofsign_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return b_and(a0, boost::simd::Mzero<result_type>()); }
  };
} } }

#endif


#include <DataManager.h>
#include <BookTools.h>
#include <cstdio>
#include <vector>
#include <string>
#include <boost/algorithm/string.hpp>

using namespace boost::algorithm;
using namespace std;
using namespace clite::util;
using namespace clite::message;

/* The complicated version? */
/*
class CrossTrader {
    : public UpdateListener
    , public Configurable {

        DataManager &dm;

        int max ( int a, int b ) { return a > b? a : b; }
        int abs ( int x ) { return max(x, -x); }
    public:

        void wakeup ( ) {

        }

        bool fillInTrade ( int cid, int pos, int target,
                Mkt::ECN *use_ecn, int *use_sz, double *use_px) {
            using namespace Cmp;

            double fees, price, best_fees = 1000000000, targ_px;
            int size, tr_size = abs(target - pos);

            Mkt::Trade trade;
            if (pos < target) trade = Mkt::BUY;
            else if (pos < 0) trade = Mkt::SHORT;
            else trade = Mkt::SELL;

            Mkt::Side side = trade == Mkt::BUY? Mkt::ASK : Mkt::BID;
            Mkt::ECN best_ecn = Mkt::UNKN;
            bool bigenough = false;

            if (!dm.getMarket(cid, side, 0, &targ_px, 0)) return false;
            for (int ecn = 0; ecn < Mkt::NUM_ECNS; ++i) {
                int ecn_sz = dm.getSubMktSize(cid, Mkt::ECN(ecn), side, targ_px);
                if (dm.checkOrder(

 */

class BSpecCrossTrader :
    public dispatch<OrderUpdate>::listener,
    public dispatch<TimeUpdate>::listener,
    public Configurable
{

    factory<DataManager>::pointer dm;
    bool open, waited;
    int deftarget;
    bool sweep;
    ECN::ECN ecn;
    string ecnstr;
    string trpx_offset_str;
    double trpx_offset;
    vector<string> symtarget;
    vector<int> target;
    Timer wait, every;

    public:

    BSpecCrossTrader() : dm(factory<DataManager>::get(only::one)), open(false), waited(false),
    wait(TimeVal(0,0)), every(TimeVal(0, 0)) {
        defOption("default-target", &deftarget, "target for each stock unless otherwise specified.", 0);
        defOption("symbol-target", &symtarget, "target for a particular symbol: eg 'MSFT,-50,+0.04'");
        defOption("default-limit", &trpx_offset_str, "price offset to go past the far side of the book.", "0.0");
        defOption("sweep", &sweep, "Use BATS intermarket routing");
        defOption("ecn", &ecnstr, "Which ECN to use for odd lots/non-sweep", "ISLD");
    }

    bool initialize ( ) {
        target.resize(dm->cidsize(), deftarget);
        for (vector<string>::iterator targ = symtarget.begin(); targ != symtarget.end(); ++targ) {
            vector<string> splitres;
            split(splitres, *targ, is_any_of(" \t,"), token_compress_on);
            int cid;
            if (splitres.size() ==2 && (cid = dm->cid(splitres[0].c_str())) != -1) {
                int t = atoi(splitres[1].c_str());
                target[cid] = t;
                printf("Target of %s is explicitly %d\n", splitres[0].c_str(), t);
            } else {
                printf("Failed to set target: \"%s\"\n", targ->c_str());
                return false;
            }
        }
            ecn = ECN::ECN_parse(ecnstr.c_str());
            if (ecn == ECN::UNKN) {
                printf("No ecn %s\n", ecnstr.c_str());
                return false;
            }

        if (trpx_offset_str[0] == '_')
            trpx_offset_str[0] = '-';

        trpx_offset = atof(trpx_offset_str.c_str());
        printf("Crossing the market %+4.2f\n", trpx_offset);

        return true;
    }

    void update ( const OrderUpdate &ou ) {
        char buf[256];
        ou.snprint(buf, 255);
        printf("%s\n", buf);
    }

    void update ( const TimeUpdate &t ) {
        if (t.timer() == dm->marketOpen()) {
            if (!dm->getPositions()) {
                printf("Failed to get positions!\n");
                dm->stop("No positions!");
            }
            wait = Timer(dm->curtv() + TimeVal(30,0));
            every = Timer(TimeVal(2,0), TimeVal(0,0));
            dm->addTimer(wait);
            dm->addTimer(every);
        }

        if (t.timer() == dm->marketClose()) open = false;

        if (t.timer() == wait) waited = true;
        if (t.timer() == every && waited) {
            bool done = true;
            for (int cid = 0; cid < dm->cidsize(); ++cid) {

                int pos = dm->position(cid);
                Mkt::Side trade = pos < target[cid]? Mkt::BID : Mkt::ASK;
                Mkt::Side side = trade == Mkt::BID? Mkt::ASK : Mkt::BID;
                size_t size;
                size_t tr_size = abs(target[cid] - pos);
                printf("Trading %s : have %d, target %d, trade is %s %d\n",
                        dm->symbol(cid), pos, target[cid], Mkt::SideDesc[trade], tr_size);

                double px = 0.0;

                if (getMarketCumSize(dm->orderBook(), cid, side == Mkt::BID? Mkt::ASK : Mkt::BID) != 0) {
                    done = false;
                    continue;
                }
                if (tr_size != 0) {
                    done = false;

                    if (getMarket(dm->masterBook(), cid, side, 0, &px, &size)) {

                        double tr_px = px + ( trade == Mkt::BID? 1.0 : -1.0 ) * trpx_offset;
                        if (tr_size > 100) tr_size = 100;
                        if (tr_size == 100 && sweep) {
                            Mkt::OrderResult res;
                            res = dm->placeBatsOrder(cid, tr_size, tr_px, trade, DataManager::BATS_ALLMARKETS);
                            if (res == Mkt::GOOD)
                                printf("%s: Placed BATS sweep order to %s %s - %lu @ %f \n",
                                        TVtoTimeStr(dm->curtv()), Mkt::TradeDesc[trade],
                                        dm->symbol(cid), tr_size, px);
                            else
                                printf("%s: Failed to place BATS sweep order to %s %s - %lu @ %f : %s \n",
                                        TVtoTimeStr(dm->curtv()), Mkt::TradeDesc[trade],
                                        dm->symbol(cid), tr_size, px, Mkt::OrderResultDesc[res]);
                        } else {
                            Mkt::OrderResult res;
                            res = dm->placeOrder(cid, ecn, tr_size, tr_px, trade, 0);
                            if (res == Mkt::GOOD)
                                printf("%s: Placed %s order to %s %s - %lu @ %f \n",
                                        TVtoTimeStr(dm->curtv()), ECN::desc(ecn), Mkt::TradeDesc[trade],
                                        dm->symbol(cid), tr_size, px);
                            else
                                printf("%s: Failed to place %s order to %s %s - %lu @ %f : %s \n",
                                        TVtoTimeStr(dm->curtv()), ECN::desc(ecn), Mkt::TradeDesc[trade],
                                        dm->symbol(cid), tr_size, px, Mkt::OrderResultDesc[res]);
                        }
                    }
                }
            }

            if (done) {
                dm->stop("Positions equal to targets.");
            }
        }
    }
};

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

    bool help;

    factory<DataManager>::pointer dm = factory<DataManager>::get(only::one);
    BSpecCrossTrader trd;
    CmdLineFileConfig cfg(argc, argv, "config,C");
        cfg.defSwitch("help,h", &help, "print this help.");
    cfg.add(*dm);
    cfg.add(trd);
    cfg.add(*file_table_config::get_config());
    cfg.add(*debug_stream_config::get_config());
    cfg.configure();
    if (help) { cerr << cfg << endl; return 1; }
    dm->initialize();
    trd.initialize();
    dm->add_listener(&trd);

    dm->run();
}


// [[Rcpp::plugins(openmp)]]
#ifdef _OPENMP
#include <omp.h>
#endif
#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Core>
#include <random>
#include "distributions.h"
#include "concurrentqueue.h"
// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using namespace RcppEigen;
using namespace Eigen;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::SparseVector;
using Eigen::LLT;
using Eigen::Lower;
using Eigen::Map;
using Eigen::Upper;
typedef Map<MatrixXd> MapMatd;

inline void initialize_file( std::ofstream& outFile,int M,int N,int groups, int F){
  bool queueFull;
  queueFull=0;

  outFile<< "iteration,"<<"mu,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "beta[" << (i+1) << "],";

  }
  outFile<<"sigmaE,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "comp[" << (i+1) << "],";
  }
  for(unsigned int i = 0; i < groups; ++i){
    outFile << "sigmaG[" << (i+1) << "],";
  }
  unsigned int i;
  for(i = 0; i < (N-1);i++){
    outFile << "epsilon[" << (i+1) << "],";
  }

  outFile << "epsilon[" << (i+1) << "],";

  for(i = 0; i < F; i ++){
    outFile << "alpha[" << (i+1) << "],";
  }
  outFile << "sigmaF";
  outFile<<"\n";
}

//' BayesRR sampler
//' @param outputFile The file in which the samples aftare burnin will be stored
//' @param seed random seed
//' @param max_iterations total of number of samples taken.
//' @param burn_in integer leq than max_iterations, number of samples used for burn in, after which, al samples will  be stored in the outputFile
//' @param thinning thinning regime, not implemented
//' @param X matrix of snp markers, or covariates of interest
//' @param Y vector of response variates, must have the same number of rows as X
//' @param sigma0 variance of the zero-centered normal prior over the intercept
//' @param v0E degrees of  freedom of the prior inverse scaled chi-squared distribution over residues variance
//' @param s02E scale parameter of the prior inverse scaled chi-squared distribution over residues variance
//' @param v0G degrees of freedom of the prior inverse scaled chi-squared distribution over genetic effects variance
//' @param s02G scale parameter of the prior inverse scaled chi-squared distribution over genetic effects variance
//' @param cva Matrix of mixture variances for groups, rows must be the same as number of groups, columns the same as number of mixtures.
//' @param groups number of groups
//' @param gAssign Vector of the same size as the number of columns of X, containing group assignments for each column(starting with group 0).
//' @param fixed Matrxi with same rows as X and Y , contains the fixed effects, those whose prior is always different than zero
// [[Rcpp::export]]
void BayesRSamplerV2Groups(std::string outputFile, int seed, int max_iterations, int burn_in, int thinning, Eigen::MatrixXd X, Eigen::VectorXd Y,double sigma0, double v0E, double s02E, double v0G, double s02G,Eigen::MatrixXd cva,int groups, Eigen::VectorXi gAssign, Eigen::MatrixXd fixed) {
  int flag;
  moodycamel::ConcurrentQueue<Eigen::VectorXd> q;
  flag=0;
  int N(Y.size());
  int M(X.cols());
  int F(fixed.cols());
  VectorXd components(M);
  std::ofstream outFile;

  outFile.open(outputFile);
  int K(cva.cols()+1);
  VectorXd sampleq(2*M+3+groups+ N + F+ 1);
  IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
  ////////////validate inputs

  if(max_iterations < burn_in || max_iterations<1 || burn_in<1) //validations related to mcmc burnin and iterations
  {
    Rcpp::Rcerr<<"error: burn_in has to be a positive integer and smaller than the maximum number of iterations ";
    return;
  }
  if(sigma0 < 0 || v0E < 0 || s02E < 0 || v0G < 0||  s02G < 0 )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: hyper parameters have to be positive";
    //return;
  }
  if((cva.array()==0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the zero component is already included in the model by default";
    //return;
  }
  if((cva.array()<0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the variance of the components should be positive";
    //return;
  }
  /////end of declarations//////

  initialize_file(outFile,M,N,groups,F);
 // Eigen::initParallel();
//  Eigen::setNbThreads(10);
  //double sum_beta_sqr;
#ifdef _OPENMP
  omp_set_num_threads(2);
#endif
#pragma omp parallel num_threads(2) shared(flag,q,M,N,F)
{
#pragma omp sections
{
  //producer
  {

    //mean and residual variables
    double mu; // mean or intercept
    double sigmaG; //genetic variance
    double sigmaE; // residuals variance
    VectorXd sigmaGG(groups);
    double sigmaF; // fixed effects variance

    //component variables
    MatrixXd priorPi(groups,K); // prior probabilities for each component
    MatrixXd pi(groups,K); // mixture probabilities
    VectorXd cVa(K); //component-specific variance
    VectorXd logL(K); // log likelihood of component
    VectorXd muk(K); // mean of k-th component marker effect size
    VectorXd denom(K-1); // temporal variable for computing the inflation of the effect variance for a given non-zero componnet
    int m0; // total num ber of markes in model
    MatrixXd v(groups,K); //variable storing the component assignment
    VectorXd cVaI(K);// inverse of the component variances

    //linear model variables
    MatrixXd beta(M,1); // effect sizes
    VectorXd alpha(F); // fixed effects
    VectorXd y_tilde(N); // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd epsilon(N); // variable containing the residuals

    //sampler variables
    VectorXd sample(2*M+3+groups+ N + F+ 1); // varible containg a sambple of all variables in the model, M marker effects, M component assigned to markers, sigmaE, sigmaG, mu, iteration number and Explained variance
    std::vector<int> markerI;
    for (int i=0; i<M; ++i) {
      markerI.push_back(i);
    }
    std::vector<int> fixedI;
    for(int j=0; j<F; ++j){
      fixedI.push_back(j);
    }
    VectorXd xsquared(M);
    double num;

    int marker;
    double acum;
    VectorXd betaAcum(groups);

        for(int i=0; i < groups; i++){
        	priorPi.row(i)(0)=0.5;
        	for(int k=1;k<K;k++){
        	priorPi.row(i)(k)=0.5/K;
        	}
        }
    y_tilde.setZero();
    //cVa[0] = 0;
    //cVa.segment(1,(K-1))=cva;

    //cVaI[0] = 0;
    //cVaI.segment(1,(K-1))=cVa.segment(1,(K-1)).cwiseInverse();
    //beta=beta.setRandom();

    //beta=(beta.array().abs() > 1e-6  ).select(beta, MatrixXd::Zero(M,1));
    beta.setZero();
    alpha.setZero();

    //mu=norm_rng(0,1);
    mu=0;

    betaAcum.setZero();
    // sigmaG=(1*cVa).sum()/M;
    for(int i=0; i<groups;i++)
      sigmaGG[i]=beta_rng(1,1);

    sigmaF= (double)R::runif(0,1);

    pi=priorPi;

    components.setZero();
    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    epsilon= Y.array() - mu;
    sigmaE=epsilon.squaredNorm()/N*0.5;
    xsquared=X.colwise().squaredNorm();
    for(int iteration=0; iteration < max_iterations; iteration++){

      if(iteration>0)
        if( iteration % (int)std::ceil(max_iterations/10) ==0)
          Rcpp::Rcout << "iteration: "<<iteration <<"\n";

        epsilon= epsilon.array()+mu;//  we substract previous value
        mu = norm_rng(epsilon.sum()/(double)N, sigmaE/(double)N); //update mu
        epsilon= epsilon.array()-mu;// we substract again now epsilon =Y-mu-X*beta

        std::random_shuffle(fixedI.begin(), fixedI.end());
        for(int cur_fix=0; cur_fix < F; cur_fix++){
          int cur = fixedI[cur_fix];
          auto cur_alpha = alpha[cur];
          y_tilde= epsilon.array()+(fixed.col(cur)*cur_alpha).array();
          auto denom_f = (N-1) + (sigmaE/sigmaF);
          auto num_f = (fixed.col(cur).cwiseProduct(y_tilde)).sum();
          alpha[cur] = norm_rng(num_f/denom_f,sigmaE/denom_f);
          epsilon = y_tilde - fixed.col(cur)*alpha[cur];
        }

        std::random_shuffle(markerI.begin(), markerI.end());

        m0=0;
        v.setZero();
        betaAcum.setZero();
        for(int j=0; j < M; j++){

          marker= markerI[j];
          sigmaG=sigmaGG[gAssign(marker)];

          cVa[0]=0;
          cVaI[0]=0;
          cVa.segment(1,(K-1))=cva.row(gAssign(marker));
          cVaI.segment(1,(K-1))=(cVa.segment(1,(K-1))).cwiseInverse();

          y_tilde= epsilon.array()+(X.col(marker)*beta(marker,0)).array();//now y_tilde= Y-mu-X*beta+ X.col(marker)*beta(marker)_old

          muk[0]=0.0;//muk for the zeroth component=0

          // Rcpp::Rcout<< muk;
          //we compute the denominator in the variance expression to save computations
          denom=xsquared(marker)+(sigmaE/sigmaG)*cVaI.segment(1,(K-1)).array();
          //muk for the other components is computed according to equaitons
          num=(X.col(marker).cwiseProduct(y_tilde)).sum();
          muk.segment(1,(K-1))= num/denom.array();

          logL= pi.row(gAssign(marker)).array().log();//first component probabilities remain unchanged

          // Here we reproduce the fortran code
          logL.segment(1,(K-1))=logL.segment(1,(K-1)).array() - 0.5*((((sigmaG/sigmaE)*(xsquared(marker))*cVa.segment(1,(K-1)).array() + 1).array().log()))+
            0.5*( muk.segment(1,(K-1)).array()*num)/sigmaE;

          double p((double)R::runif( 0.0, 1.0 ));//

          if(((logL.segment(1,(K-1)).array()-logL[0]).abs().array() >700 ).any() ){
            acum=0;
          }else{
            acum=1.0/((logL.array()-logL[0]).exp().sum());
          }

          for(int k=0;k<K;k++){
            if(p<=acum){
              if(k==0){
                beta(marker,0)=0;
              }else{
                beta(marker,0)=norm_rng(muk[k],sigmaE/denom[k-1]);
                betaAcum(gAssign(marker))+= pow(beta(marker,0),2);
                // beta(marker,0)=norm_rng(rhs/denom[k-1],sigmaE/denom[k-1]);
              }
              v.row(gAssign(marker))(k)+=1.0;
              components[marker]=k;
              break;
            }else{
              if(((logL.segment(1,(K-1)).array()-logL[k+1]).abs().array() >700 ).any() ){
                acum+=0;
              }
              else{
                acum+=1.0/((logL.array()-logL[k+1]).exp().sum());
              }
            }
          }
          epsilon=y_tilde-X.col(marker)*beta(marker,0);//now epsilon contains Y-mu - X*beta+ X.col(marker)*beta(marker)_old- X.col(marker)*beta(marker)_new

        }

        sigmaF = inv_scaled_chisq_rng(v0E+F,(alpha.squaredNorm()+v0E*s02E)/(v0E+F));
        // Rcpp::Rcout << sigmaF;

        sigmaE=inv_scaled_chisq_rng(v0E+N,((epsilon).squaredNorm()+v0E*s02E)/(v0E+N));

        for(int i=0; i<groups; i++){
          m0=v.row(i).sum()-v.row(i)(0);
          sigmaGG[i]=inv_scaled_chisq_rng(v0G+m0,(betaAcum(i)*m0+v0G*s02G)/(v0G+m0));
          pi.row(i)=dirichilet_rng(v.row(i).array() + 1.0);

        }

        if(iteration >= burn_in)
        {
          if(iteration % thinning == 0){
            sample<< iteration,mu,beta,sigmaE,components,sigmaGG,epsilon,alpha,sigmaF;

            q.enqueue(sample);
            //here we have the consumer
#ifdef _OPENMP

#else
            if(q.try_dequeue(sampleq)){

              outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
            }
#endif
          }

        }

    }

    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>( t2 - t1 ).count();
    Rcpp::Rcout << "duration: "<<duration << "s\n";
    flag=1;
  }//end producer
  //consumer
    #ifdef _OPENMP
    #pragma omp section
    {

      while(!flag ){
        if(q.try_dequeue(sampleq)){
          outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
        }
      }//while

    }// endconsumer
    #endif
}

}

}


#include <boost/accumulators/statistics/weighted_sum.hpp>


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file EvaluatorBase.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_EVALUATOR_BASE_HPP
#define STEAM_EVALUATOR_BASE_HPP

#include <Eigen/Core>

#include <steam/state/StateVector.hpp>
#include <steam/problem/Jacobian.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Base class that defines the general 'evaluator' interface
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename TYPE,                      // The output 'evaluation' type
          int LHS_DIM       = Eigen::Dynamic, // The LHS dim of the 'most' left-ward
                                              // Jacobian in the evaluation tree
          int INNER_DIM     = Eigen::Dynamic, // The LHS dimension of 'this' evaluators Jacobian
          int MAX_STATE_DIM = Eigen::Dynamic> // The maximum dimension of a single state variable
                                              // perturbation (aka the 'most' RHS Jacobian dim)
class EvaluatorBase
{
 public:

  /// Convenience typedefs
  typedef std::shared_ptr<EvaluatorBase<TYPE,LHS_DIM,INNER_DIM,MAX_STATE_DIM> > Ptr;
  typedef std::shared_ptr<const EvaluatorBase<TYPE,LHS_DIM,INNER_DIM,MAX_STATE_DIM> > ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  EvaluatorBase() {}

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns whether or not an evaluator contains unlocked state variables
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool isActive() const = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Interface for the general 'evaluation'
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual TYPE evaluate() const = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Interface for the general 'evaluation', with Jacobians
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual TYPE evaluate(const Eigen::Matrix<double, LHS_DIM, INNER_DIM>& lhs,
                        std::vector<Jacobian<LHS_DIM, MAX_STATE_DIM> >* jacs) const = 0;
};

} // steam

#endif // STEAM_EVALUATOR_BASE_HPP