#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <sstream>
#include <math.h>
#include "testing.h"

class Suspension
{

	// variables
private:
	// suffix ref stands for ref, so reference/initial coordinates can be easily
	// differentiated from coordinates of the same points during wheel movement
	// hardpoints
	// LCA
	Eigen::Vector3f lca1ref;
	Eigen::Vector3f lca2ref;
	Eigen::Vector3f lca3ref;

	Eigen::Vector3f uca1ref;
	Eigen::Vector3f uca2ref;
	Eigen::Vector3f uca3ref;

	Eigen::Vector3f tr1ref;
	Eigen::Vector3f tr2ref;

	Eigen::Vector3f wcnref;
	Eigen::Vector3f spnref;

	// wheel radius
	float wRadius=210;
	// wheel vertical movement
	float wVert=30;
	// wheel steering movement
	float wSteer=30;
	// number of increments between reference position and
	// downmost/upmost/leftmost/rightmost position
	int vertIncr=30;
	int steerIncr=0;
	// precision- at what value has the iterator converged, in percentage- 0...1
	float precision;

	// derived values

	Eigen::Vector3f lca12;
	Eigen::Vector3f uca12;

	/* initializers */

public:

	Suspension()
	{
		std::vector<float> hps;
		std::string userArrayInput;
		std::cout << "enter numbers: ";
		std::cin >> userArrayInput;

		std::istringstream iss(userArrayInput);
		std::string item;

		while (std::getline(iss, item, ','))
		{
			hps.push_back(std::stof(item));
		}

		std::cout << "for loop vector output" << std::endl;

		for (auto i = hps.begin(); i != hps.end(); i++)
			std::cout << *i << std::endl;

		lca1ref << hps[0], hps[1], hps[2];
		lca2ref << hps[3], hps[4], hps[5];
		lca3ref << hps[6], hps[7], hps[8];

		uca1ref << hps[9], hps[10], hps[11];
		uca2ref << hps[12], hps[13], hps[14];
		uca3ref << hps[15], hps[16], hps[17];

		tr1ref << hps[18], hps[19], hps[20];
		tr2ref << hps[21], hps[22], hps[23];

		wcnref << hps[24], hps[25], hps[26];
		spnref << hps[27], hps[28], hps[29];

	}

	Suspension(float lca1xin, float lca1yin, float lca1zin, // in suffix stands for input
		float lca2xin, float lca2yin, float lca2zin,
		float lca3xin, float lca3yin, float lca3zin,
		float uca1xin, float uca1yin, float uca1zin,
		float uca2xin, float uca2yin, float uca2zin,
		float uca3xin, float uca3yin, float uca3zin,
		float tr1xin, float tr1yin, float tr1zin,
		float tr2xin, float tr2yin, float tr2zin,
		float wcnxin, float wcnyin, float wcnzin,
		float spnxin, float spnyin, float spnzin,
		float wRadiusin, float wVertin, float wSteerin,
		int vertIncrin, int steerIncrin, float precisionin)
	{
		lca1ref << lca1xin, lca1yin, lca1zin;
		lca2ref << lca2xin, lca2yin, lca2zin;
		lca3ref << lca3xin, lca3yin, lca3zin;

		uca1ref << uca1xin, uca1yin, uca1zin;
		uca2ref << uca2xin, uca2yin, uca2zin;
		uca3ref << uca3xin, uca3yin, uca3zin;

		tr1ref << tr1xin, tr1yin, tr1zin;
		tr2ref << tr2xin, tr2yin, tr2zin;

		wcnref << wcnxin, wcnyin, wcnzin;
		spnref << spnxin, spnyin, spnzin;

		wRadius = wRadiusin;
		wVert = wVertin;
		wSteer = wSteerin;
		vertIncr = vertIncrin;
		steerIncr = steerIncrin;
		precision = precisionin;

	}

	// FUNCTIONS
private:

	// place as inputs variables rLCA, rUCA, uca12, lca12, etc. so those can be only temporary
	// values and not use up stack memory

		void CalculateConstants(Eigen::Matrix3f& _rotLCA, Eigen::Matrix3f& _rotUCA, float& _rLCA, float& _rUCA, float& _rCA, Eigen::ArrayXf& _zLocLca, float& _rST, float& _t_param, float& _rTR, Eigen::Vector3f& _wcnlocTRk, Eigen::Vector3f& _spnlocTRk)
	{

		// create line connecting lca1 and lca2
		Eigen::ParametrizedLine<float, 3> lca1lca2 = Eigen::ParametrizedLine<float, 3>::Through(lca1ref, lca2ref);

		// local LCA plane for determining max z value of wheel parameters
		Eigen::Vector4f abcd;
		Eigen::Vector3f _tr2prref;

		// z local maximum value
		float zLocHi;
		// z local minimum value
		float zLocLo;

		lca12 = lca1lca2.projection(lca3ref);
		_rLCA = lca1lca2.distance(lca3ref);

		// create rotation matrix for LCA cs
		_rotLCA <<
			(lca1ref - lca2ref).normalized(),
			(lca12 - lca3ref).normalized(),
			((lca1ref - lca2ref).cross(lca12 - lca3ref)).normalized();

		// calculate parameters for plane in LCA cs
		abcd <<
			_rotLCA.row(2)(0),
			_rotLCA.row(2)(1),
			_rotLCA.row(2)(2),
			-_rotLCA.row(2) * (_rotLCA.transpose() * Eigen::Vector3f{ -lca12(0),-lca12(1),lca3ref(2) - wVert - lca12(2) });

		// calculates z value for upmost movement of wheel for intersection of plane and circle in LCA
		zLocHi =
			(-abcd(2) * abcd(3) + abcd(1) *
			sqrt(abcd(2) * abcd(2) * _rLCA * _rLCA +
				abcd(1) * abcd(1) * _rLCA * _rLCA -
				abcd(3) * abcd(3))) /
			(abcd(1) * abcd(1) + abcd(2) * abcd(2));

		// reuses previous parameters for LCA plane, only 4th parameter is changed
		abcd(3) =
			-_rotLCA.row(2) *
			_rotLCA.transpose() *
			Eigen::Vector3f{ -lca12(0),-lca12(1),lca3ref(2) + wVert - lca12(2) };

		// calculates z value for downmost movement of wheel for intersection of plane and circle in LCA
		zLocLo =
			(-abcd(2) * abcd(3) +
			abcd(1) * sqrt(abcd(2) * abcd(2) * _rLCA * _rLCA +
				abcd(1) * abcd(1) * _rLCA * _rLCA -
				abcd(3) * abcd(3))) /
			(abcd(1) * abcd(1) + abcd(2) * abcd(2));

		_zLocLca <<
			Eigen::VectorXf::LinSpaced(vertIncr, zLocLo, zLocLo / vertIncr),
			0,
			Eigen::VectorXf::LinSpaced(vertIncr, zLocHi / vertIncr, zLocHi);

		// create line connecting uca1 and uca2
		Eigen::ParametrizedLine<float, 3> uca1uca2 = Eigen::ParametrizedLine<float, 3>::Through(uca1ref, uca2ref);

		uca12 = uca1uca2.projection(uca3ref);
		_rUCA = uca1uca2.distance(uca3ref);

		_rCA = (uca3ref - lca3ref).norm();

		// create rotation matrix for UCA cs
		_rotUCA <<
			(uca1ref - uca2ref).normalized(),
			(uca12 - uca3ref).normalized(),
			((uca1ref - uca2ref).cross(uca12 - uca3ref)).normalized();

		// calculate TR2 projection point and rTR
		Eigen::ParametrizedLine<float, 3> lca3uca3 = Eigen::ParametrizedLine<float, 3>::Through(lca3ref, uca3ref);

		_tr2prref = lca3uca3.projection(tr2ref);

		_rST = (tr2ref - _tr2prref).norm();

		_rTR = (tr2ref - tr1ref).norm();

		_t_param = (_tr2prref(0) - lca3ref(0)) / (uca3ref(0) - lca3ref(0));

		Eigen::Matrix3f _rotTRk; // TR rotation matrix defined by TR2ref

		Eigen::Vector3f _xCol{ _tr2prref - tr2ref };
		Eigen::Vector3f _zCol{ lca3ref - uca3ref };
		Eigen::Vector3f _yCol{ _zCol.cross(_xCol) };

		_rotTRk.col(0) << _xCol / _xCol.norm();
		_rotTRk.col(1) << _yCol / _yCol.norm();
		_rotTRk.col(2) << _zCol / _zCol.norm();

		_wcnlocTRk << _rotTRk.transpose() * (wcnref - _tr2prref);
		_spnlocTRk << _rotTRk.transpose() * (spnref - _tr2prref);

		std::cout << "spnlocTRk" << std::endl;
		std::cout << _spnlocTRk << std::endl;
	}

public:

	int CalculateMovement()
	{
		Eigen::Matrix3f rotLCA;
		Eigen::Matrix3f rotUCA;
		Eigen::MatrixXf tr2prGlob;
		float rLCA;
		float rUCA;
		float rCA;   // distance between LCA3 and UCA3
		float rST;   // distance between TR2 and UCA3LCA3 axis
		float rTR;   // distance between TR2 and TR1 axis
		float t_param;   // parameter to determine position of TR2pr on uca3lca3 line
		// coordinates of wcn and spn in TR cs defined by tr2ref
		Eigen::Vector3f wcnlocTRk;
		Eigen::Vector3f spnlocTRk;

		Eigen::ArrayXf zLCA3LocLCA(vertIncr * 2 + 1);

		Suspension::CalculateConstants(rotLCA, rotUCA, rLCA, rUCA, rCA, zLCA3LocLCA, rST, t_param, rTR, wcnlocTRk, spnlocTRk);

		Eigen::MatrixXf lca3LocLCA(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf lca3Glob(vertIncr * 2 + 1, 3);

		// populating positions of local LCA3 in a matrix
		lca3LocLCA.col(0) << Eigen::VectorXf::Zero(vertIncr * 2 + 1);
		lca3LocLCA.col(1) << -(rLCA*rLCA - zLCA3LocLCA * zLCA3LocLCA).sqrt();
		lca3LocLCA.col(2) << zLCA3LocLCA;

		// global positions of LCA3 for whole wheel movement
		lca3Glob = (lca3LocLCA * rotLCA.transpose()).array().rowwise() + lca12.array().transpose();

		// global position of UCA3 for whole wheel movement
		Eigen::MatrixXf uca3Glob(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf uca3LocUCA(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf lca3LocUCA(vertIncr * 2 + 1, 3);

		lca3LocUCA = lca3Glob.rowwise() - uca12.transpose();
		lca3LocUCA = lca3LocUCA * rotUCA;

		// temporary values for calculating UCA3 in UCA cs, correspond to chunks of expression in word
		Eigen::ArrayXf temp1UCA3 =
			-rCA * rCA + rUCA * rUCA +
			lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp2UCA3 =
			2 * (lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array());

		Eigen::ArrayXf temp3UCA3 =
			-rCA * rCA * rCA * rCA + 2 * rCA * rCA * rUCA * rUCA +
			2 * rCA * rCA * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			2 * rCA * rCA * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			2 * rCA * rCA * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp4UCA3 =
			-rUCA * rUCA * rUCA * rUCA -
			2 * rUCA * rUCA * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			2 * rUCA * rUCA * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			2 * rUCA * rUCA * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp5UCA3 =
			-lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array()* lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() -
			2 * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() -
			2 * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp6UCA3 =
			-lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array()* lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() -
			2 * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() -
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp7UCA3 = (temp3UCA3 + temp4UCA3 + temp5UCA3 + temp6UCA3).sqrt();

		uca3LocUCA.col(0) << Eigen::VectorXf::Zero(vertIncr * 2 + 1);
		uca3LocUCA.col(1) << (lca3LocUCA.col(1).array() * temp1UCA3 - lca3LocUCA.col(2).array() * temp7UCA3) / temp2UCA3;
		uca3LocUCA.col(2) << (lca3LocUCA.col(2).array() * temp1UCA3 + lca3LocUCA.col(1).array() * temp7UCA3) / temp2UCA3;

		uca3Glob = (uca3LocUCA * rotUCA.transpose()).array().rowwise() + uca12.array().transpose();

		// calculating TR2, WCN, SPN and CP
		// steering is not enabled, for optimization
		if (steerIncr!=0 || wSteer!=0)
		{

			std::cout << "no steer movement" << std::endl;
			// calculating TR2 position
			Eigen::MatrixXf tr2Glob(vertIncr * 2 + 1, 3);
			Eigen::MatrixXf wcnGlob(vertIncr * 2 + 1, 3);
			Eigen::MatrixXf spnGlob(vertIncr * 2 + 1, 3);

			tr2prGlob = (uca3Glob - lca3Glob) * t_param + lca3Glob;

			for (int i = 0; i < vertIncr * 2 + 1; i++)
			{
				Eigen::Matrix3f rotTR;
				Eigen::Vector3f tr2locTR;

				Eigen::Vector3f xCol{ 30, 0, -30 * (uca3Glob(i, 0) - lca3Glob(i, 0)) / (uca3Glob(i, 2) - lca3Glob(i, 2)) };
				Eigen::Vector3f zCol{ (lca3Glob.row(i) - uca3Glob.row(i)).transpose() };
				Eigen::Vector3f yCol{ zCol.cross(xCol) };

				rotTR.col(0) << xCol / xCol.norm();
				rotTR.col(1) << yCol / yCol.norm();
				rotTR.col(2) << zCol / zCol.norm();

				// calculate local position of TR1
				Eigen::Vector3f tr1locTR;

				//lca3LocUCA = lca3Glob.rowwise() - uca12.transpose();
				//lca3LocUCA = lca3LocUCA * rotUCA;

				tr1locTR = -tr2prGlob.row(i) + tr1ref.transpose();
				tr1locTR = rotTR.transpose() * tr1locTR;

				// calculate local position of TR2
				float temp1TR2 =
					rST * rST - rTR * rTR +
					tr1locTR(0) * tr1locTR(0) +
					tr1locTR(1) * tr1locTR(1) +
					tr1locTR(2) * tr1locTR(2);

				float temp2TR2 = 2 * (tr1locTR(0) * tr1locTR(0) + tr1locTR(1) * tr1locTR(1));

				float temp3TR2 =
					-rST * rST * rST * rST +
					2 * rTR * rTR * rST * rST +
					2 * rST * rST * tr1locTR(0) * tr1locTR(0) +
					2 * rST * rST * tr1locTR(1) * tr1locTR(1) -
					2 * rST * rST * tr1locTR(2) * tr1locTR(2);

				float temp4TR2 =
					-rTR * rTR * rTR * rTR +
					2 * rTR * rTR * tr1locTR(0) * tr1locTR(0) +
					2 * rTR * rTR * tr1locTR(1) * tr1locTR(1) +
					2 * rTR * rTR * tr1locTR(2) * tr1locTR(2);

				float temp5TR2 =
					-tr1locTR(0) * tr1locTR(0) * tr1locTR(0) * tr1locTR(0) -
					2 * tr1locTR(0) * tr1locTR(0) * tr1locTR(1) * tr1locTR(1) -
					2 * tr1locTR(0) * tr1locTR(0) * tr1locTR(2) * tr1locTR(2);

				float temp6TR2 =
					-2 * tr1locTR(1) * tr1locTR(1) * tr1locTR(2) * tr1locTR(2) -
					tr1locTR(1) * tr1locTR(1) * tr1locTR(1) * tr1locTR(1) -
					tr1locTR(2) * tr1locTR(2) * tr1locTR(2) * tr1locTR(2);

				float temp7TR2 = std::sqrt(temp3TR2 + temp4TR2 + temp5TR2 + temp6TR2);

				tr2locTR(0) = (tr1locTR(0) * temp1TR2 - tr1locTR(1) * temp7TR2) / temp2TR2;
				tr2locTR(1) = (tr1locTR(1) * temp1TR2 + tr1locTR(0) * temp7TR2) / temp2TR2;
				tr2locTR(2) = 0;

				tr2Glob.row(i) << (rotTR * tr2locTR).transpose() + tr2prGlob.row(i);

				// calculating WCN and SPN
				Eigen::Matrix3f _rotTRk; // TR rotation matrix defined by TR2ref

				Eigen::Vector3f _xCol{ tr2prGlob.row(i) - tr2Glob.row(i) };
				Eigen::Vector3f _zCol{ lca3Glob.row(i) - uca3Glob.row(i) };
				Eigen::Vector3f _yCol{ _zCol.cross(_xCol) };

				_rotTRk.col(0) << _xCol / _xCol.norm();
				_rotTRk.col(1) << _yCol / _yCol.norm();
				_rotTRk.col(2) << _zCol / _zCol.norm();

				wcnGlob.row(i) << (_rotTRk * wcnlocTRk).transpose() + tr2prGlob.row(i);
				spnGlob.row(i) << (_rotTRk * spnlocTRk).transpose() + tr2prGlob.row(i);

			}

			// CP calculation
			float temp1cp{ -20 }; // this is actually vector 0,0,-20
			Eigen::MatrixXf temp2cp(vertIncr * 2 + 1, 3);
			Eigen::MatrixXf temp3cp(vertIncr * 2 + 1, 3);

			temp2cp << spnGlob - wcnGlob;

			std::cout << "temp2cp  " << std::endl;
			std::cout << temp2cp << std::endl;

			temp3cp.col(0) << -temp2cp.col(0).array() * temp2cp.col(2).array() * temp1cp;
			temp3cp.col(1) << -temp2cp.col(1).array() * temp2cp.col(2).array() * temp1cp;
			temp3cp.col(2) <<
				temp2cp.col(1).array() * temp2cp.col(1).array() * temp1cp +
				temp2cp.col(0).array() * temp2cp.col(0).array() * temp1cp;

			temp3cp.rowwise().normalize();

			Eigen::MatrixXf cpGlob(vertIncr * 2 + 1, 3);

			cpGlob << -temp3cp * wRadius + wcnGlob;

			std::cout << "cp  " << std::endl;
			std::cout << cpGlob << std::endl;
			return 123;

		}

		// steering is enabled
		else
		{
		return 456;
		}

	}

};

int main()
{
	Suspension susp{
		-2038.666, -411.709, -132.316,
		-2241.147, -408.195, -126.205,
		-2135, -600, -140,
		-2040.563, -416.249, -275.203,
		-2241.481, -417.314, -270.739,
		-2153, -578, -315,
		-2234.8, -411.45, -194.6,
		-2225, -582, -220,
		-2143.6, -620.5, -220.07,
		-2143.6, -595.5, -219.34,
		210, 30, 30, 1,
		10, 0.01
	};

	int i = susp.CalculateMovement();
	std::cout << i << std::endl;

	std::cout << addition(15,20) << std::endl;

	Suspension susp2{};

	susp2.CalculateMovement();

	/*
	-2038.666,-411.709,-132.316,-2241.147,-408.195,-126.205,-2135,-600,-140,-2040.563,-416.249,-275.203,-2241.481,-417.314,-270.739,-2153,-578,-315,-2234.8,-411.45,-194.6,-2225,-582,-220,-2143.6,-620.5,-220.07,-2143.6,-595.5,-219.34

	*/

}


#include <cmath>
#include <functional>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <boost/functional/hash.hpp>
#include <boost/math/constants/constants.hpp>
#include <CGAL/boost/graph/helpers.h>
#include <CGAL/boost/graph/Dual.h>
#include <Euclid/Math/Vector.h>
#include <Euclid/Util/Assert.h>

namespace Euclid
{

template<typename Mesh>
Vector_3_t<Mesh> vertex_normal(vertex_t<Mesh> v,
                               const Mesh& mesh,
                               const VertexNormal& weight)
{
    Vector_3_t<Mesh> normal(0.0, 0.0, 0.0);
    for (auto he : CGAL::halfedges_around_source(v, mesh)) {
        if (!CGAL::is_border(he, mesh)) {
            auto f = face(he, mesh);
            auto fn = face_normal(f, mesh);

            if (weight == VertexNormal::uniform) {
                normal += fn;
            }
            else if (weight == VertexNormal::face_area) {
                auto area = face_area(f, mesh);
                normal += area * fn;
            }
            else { // incident_angle
                auto angle = corner_angle(he, mesh);
                normal += angle * fn;
            }
        }
    }
    return Euclid::normalized(normal);
}

template<typename Mesh>
Vector_3_t<Mesh> vertex_normal(
    vertex_t<Mesh> v,
    const Mesh& mesh,
    const std::vector<Vector_3_t<Mesh>>& face_normals,
    const VertexNormal& weight)
{
    auto fimap = get(boost::face_index, mesh);
    Vector_3_t<Mesh> normal(0.0, 0.0, 0.0);
    for (auto he : CGAL::halfedges_around_source(v, mesh)) {
        if (!CGAL::is_border(he, mesh)) {
            auto f = face(he, mesh);
            auto fi = get(fimap, f);
            auto fn = face_normals[fi];

            if (weight == VertexNormal::uniform) {
                normal += fn;
            }
            else if (weight == VertexNormal::face_area) {
                auto area = face_area(f, mesh);
                normal += area * fn;
            }
            else { // incident_angle
                auto angle = corner_angle(he, mesh);
                normal += angle * fn;
            }
        }
    }
    return Euclid::normalized(normal);
}

template<typename Mesh>
std::vector<Vector_3_t<Mesh>> vertex_normals(
    const Mesh& mesh,
    const std::vector<Vector_3_t<Mesh>>& face_normals,
    const VertexNormal& weight)
{
    std::vector<Vector_3_t<Mesh>> vnormals;
    vnormals.reserve(num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        vnormals.push_back(vertex_normal(v, mesh, face_normals, weight));
    }
    return vnormals;
}

template<typename Mesh>
FT_t<Mesh> vertex_area(vertex_t<Mesh> v,
                       const Mesh& mesh,
                       const VertexArea& method)
{
    using T = FT_t<Mesh>;
    auto vpmap = get(boost::vertex_point, mesh);
    auto va = T(0);
    if (method == VertexArea::barycentric) {
        const auto one_third = boost::math::constants::third<T>();
        for (auto he : CGAL::halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(he, mesh)) {
                auto p1 = get(vpmap, source(he, mesh));
                auto p2 = get(vpmap, target(he, mesh));
                auto p3 = get(vpmap, target(next(he, mesh), mesh));
                va += area(p1, p2, p3);
            }
        }
        va *= one_third;
    }
    else if (method == VertexArea::voronoi) {
        for (auto he : CGAL::halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(he, mesh)) {
                auto p1 = get(vpmap, source(he, mesh));
                auto p2 = get(vpmap, target(he, mesh));
                auto p3 = get(vpmap, target(next(he, mesh), mesh));
                auto mid1 = CGAL::midpoint(p2, p1);
                auto mid2 = CGAL::midpoint(p2, p3);
                auto center = CGAL::circumcenter(p1, p2, p3);
                if (CGAL::angle(p2, p1, p3) == CGAL::OBTUSE) {
                    va += area(mid1, p2, center) - area(mid2, center, p2);
                }
                else if (CGAL::angle(p2, p3, p1) == CGAL::OBTUSE) {
                    va += area(mid2, center, p2) - area(mid1, p2, center);
                }
                else {
                    va += area(mid1, p2, center) + area(mid2, center, p2);
                }
            }
        }
    }
    else { // method == VertexArea::mixed_voronoi
        for (auto he : CGAL::halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(he, mesh)) {
                auto p1 = get(vpmap, source(he, mesh));
                auto p2 = get(vpmap, target(he, mesh));
                auto p3 = get(vpmap, target(next(he, mesh), mesh));
                if (CGAL::angle(p1, p2, p3) == CGAL::OBTUSE) {
                    va += area(p1, p2, p3) * 0.5;
                }
                else if (CGAL::angle(p3, p1, p2) == CGAL::OBTUSE ||
                         CGAL::angle(p1, p3, p2) == CGAL::OBTUSE) {
                    va += area(p1, p2, p3) * 0.25;
                }
                else { // triangle is acute or right
                    auto mid1 = CGAL::midpoint(p2, p1);
                    auto mid2 = CGAL::midpoint(p2, p3);
                    auto center = CGAL::circumcenter(p1, p2, p3);
                    va += area(mid1, p2, center) + area(mid2, center, p2);
                }
            }
        }
    }
    return va;
}

template<typename Mesh>
std::vector<FT_t<Mesh>> vertex_areas(const Mesh& mesh, const VertexArea& method)
{
    std::vector<FT_t<Mesh>> vareas;
    vareas.reserve(num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        vareas.push_back(vertex_area(v, mesh, method));
    }
    return vareas;
}

// partial specialization for dual mesh
template<typename Mesh>
FT_t<Mesh> edge_length(halfedge_t<Mesh> h, const CGAL::Dual<Mesh>& dual)
{
    auto primal = dual.primal();
    auto f0 = source(h, dual);
    auto f1 = target(h, dual);
    auto p0 = barycenter(f0, primal);
    auto p1 = barycenter(f1, primal);
    return length(p1 - p0);
}

template<typename Mesh>
FT_t<Mesh> edge_length(halfedge_t<Mesh> he, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    return length(p2 - p1);
}

template<typename Mesh>
FT_t<Mesh> edge_length(edge_t<Mesh> e, const Mesh& mesh)
{
    auto he = halfedge(e, mesh);
    return edge_length(he, mesh);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> edge_lengths(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> elens;
    elens.reserve(num_edges(mesh));
    for (auto he : edges(mesh)) {
        elens.push_back(edge_length(he, mesh));
    }
    return elens;
}

// partial specialization for dual mesh
template<typename Mesh>
FT_t<Mesh> squared_edge_length(edge_t<Mesh> e, const CGAL::Dual<Mesh>& dual)
{
    auto primal = dual.primal();
    auto h = halfedge(e, dual);
    auto f0 = source(h, dual);
    auto f1 = target(h, dual);
    auto p0 = barycenter(f0, primal);
    auto p1 = barycenter(f1, primal);
    return (p1 - p0).squared_length();
}

template<typename Mesh>
FT_t<Mesh> squared_edge_length(halfedge_t<Mesh> he, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    return (p1 - p2).squared_length();
}

template<typename Mesh>
FT_t<Mesh> squared_edge_length(edge_t<Mesh> e, const Mesh& mesh)
{
    auto he = halfedge(e, mesh);
    return squared_edge_length(he, mesh);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> squared_edge_lengths(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> elens;
    elens.reserve(num_edges(mesh));
    for (auto he : edges(mesh)) {
        elens.push_back(squared_edge_length(he, mesh));
    }
    return elens;
}

template<typename Mesh>
FT_t<Mesh> dihedral_angle(edge_t<Mesh> e, const Mesh& mesh)
{
    auto h1 = halfedge(e, mesh);
    auto h2 = opposite(h1, mesh);
    auto f1 = face(h1, mesh);
    auto f2 = face(h2, mesh);
    auto n1 = face_normal(f1, mesh);
    auto n2 = face_normal(f2, mesh);
    return boost::math::constants::pi<FT_t<Mesh>>() - std::acos(cosine(n1, n2));
}

template<typename Mesh>
FT_t<Mesh> corner_angle(halfedge_t<Mesh> h, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto p1 = get(vpmap, source(h, mesh));
    auto p2 = get(vpmap, target(h, mesh));
    auto p3 = get(vpmap, target(next(h, mesh), mesh));
    return CGAL::approximate_angle(p1, p2, p3);
}

template<typename Mesh>
Vector_3_t<Mesh> face_normal(face_t<Mesh> f, const Mesh& mesh)
{
    using Vector_3 = Vector_3_t<Mesh>;
    auto vpmap = get(boost::vertex_point, mesh);
    auto he = halfedge(f, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    auto p3 = get(vpmap, target(next(he, mesh), mesh));

    Vector_3 result;
    if (CGAL::collinear(p1, p2, p3)) {
        EWARNING("Degenerate face, normal is set to zero vector");
        result = Vector_3(0.0, 0.0, 0.0);
    }
    else {
        result = normalized(CGAL::normal(p1, p2, p3));
    }
    return result;
}

template<typename Mesh>
std::vector<Vector_3_t<Mesh>> face_normals(const Mesh& mesh)
{
    std::vector<Vector_3_t<Mesh>> fnormals;
    fnormals.reserve(num_faces(mesh));
    for (auto f : faces(mesh)) {
        fnormals.push_back(face_normal(f, mesh));
    }
    return fnormals;
}

template<typename Mesh>
FT_t<Mesh> face_area(face_t<Mesh> f, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto he = halfedge(f, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    auto p3 = get(vpmap, target(next(he, mesh), mesh));
    return area(p1, p2, p3);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> face_areas(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> fareas;
    fareas.reserve(num_faces(mesh));
    for (auto f : faces(mesh)) {
        fareas.push_back(face_area(f, mesh));
    }
    return fareas;
}

template<typename Mesh>
Point_3_t<Mesh> barycenter(face_t<Mesh> f, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto [vbeg, vend] = CGAL::vertices_around_face(halfedge(f, mesh), mesh);
    auto p0 = get(vpmap, *vbeg++);
    auto p1 = get(vpmap, *vbeg++);
    auto p2 = get(vpmap, *vbeg);
    return CGAL::centroid(p0, p1, p2);
}

template<typename Mesh>
std::vector<Point_3_t<Mesh>> barycenters(const Mesh& mesh)
{
    std::vector<Point_3_t<Mesh>> centroids;
    centroids.reserve(num_faces(mesh));
    for (auto f : faces(mesh)) {
        centroids.push_back(barycenter(f, mesh));
    }
    return centroids;
}

template<typename Mesh>
FT_t<Mesh> gaussian_curvature(vertex_t<Mesh> v, const Mesh& mesh)
{
    auto angle_defect = boost::math::constants::two_pi<FT_t<Mesh>>();
    for (auto he : CGAL::halfedges_around_target(v, mesh)) {
        if (!CGAL::is_border(he, mesh)) {
            angle_defect -= corner_angle(he, mesh);
        }
    }
    return angle_defect / vertex_area(v, mesh);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> gaussian_curvatures(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> curvatures;
    curvatures.reserve(num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        curvatures.push_back(gaussian_curvature(v, mesh));
    }
    return curvatures;
}

template<typename Mesh>
std::tuple<Eigen::SparseMatrix<FT_t<Mesh>>, Eigen::SparseMatrix<FT_t<Mesh>>>
adjacency_matrix(const Mesh& mesh)
{
    using T = FT_t<Mesh>;
    using Triplet = Eigen::Triplet<T>;
    auto vimap = get(boost::vertex_index, mesh);
    const auto nv = num_vertices(mesh);

    std::vector<Triplet> adj, degree;
    for (auto vi : vertices(mesh)) {
        int i = get(vimap, vi);
        int d = 0;
        for (auto he : CGAL::halfedges_around_target(vi, mesh)) {
            auto vj = source(he, mesh);
            int j = get(vimap, vj);
            adj.emplace_back(i, j, 1);
            ++d;
        }
        degree.emplace_back(i, i, d);
    }

    Eigen::SparseMatrix<T> adj_mat(nv, nv), degree_mat(nv, nv);
    adj_mat.setFromTriplets(adj.begin(), adj.end());
    adj_mat.makeCompressed();
    degree_mat.setFromTriplets(degree.begin(), degree.end());
    degree_mat.makeCompressed();
    return std::make_tuple(adj_mat, degree_mat);
}

template<typename Mesh>
Eigen::SparseMatrix<FT_t<Mesh>> graph_laplacian_matrix(const Mesh& mesh)
{
    using T = FT_t<Mesh>;
    using Triplet = Eigen::Triplet<T>;
    auto vimap = get(boost::vertex_index, mesh);
    const auto nv = num_vertices(mesh);

    std::vector<Triplet> triplets;
    for (auto vi : vertices(mesh)) {
        int i = get(vimap, vi);
        int d = 0;
        for (auto he : CGAL::halfedges_around_target(vi, mesh)) {
            auto vj = source(he, mesh);
            int j = get(vimap, vj);
            triplets.emplace_back(i, j, -1);
            ++d;
        }
        triplets.emplace_back(i, i, d);
    }

    Eigen::SparseMatrix<T> L(nv, nv);
    L.setFromTriplets(triplets.begin(), triplets.end());
    L.makeCompressed();
    return L;
}

template<typename Mesh>
FT_t<Mesh> cotangent_weight(halfedge_t<Mesh> he, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto vi = source(he, mesh);
    auto vj = target(he, mesh);
    auto va = target(next(he, mesh), mesh);
    auto vb = target(next(opposite(he, mesh), mesh), mesh);
    auto pi = get(vpmap, vi);
    auto pj = get(vpmap, vj);
    auto pa = get(vpmap, va);
    auto pb = get(vpmap, vb);
    auto cota = cotangent(pi, pa, pj);
    auto cotb = cotangent(pi, pb, pj);
    return (cota + cotb) * static_cast<FT_t<Mesh>>(0.5);
}

template<typename Mesh>
FT_t<Mesh> cotangent_weight(edge_t<Mesh> e, const Mesh& mesh)
{
    auto he = halfedge(e, mesh);
    return cotangent_weight(he, mesh);
}

template<typename Mesh>
Eigen::SparseMatrix<FT_t<Mesh>> cotangent_matrix(const Mesh& mesh)
{
    using T = FT_t<Mesh>;
    using Triplet = Eigen::Triplet<T>;
    auto vimap = get(boost::vertex_index, mesh);
    const auto nv = num_vertices(mesh);

    auto hash_fcn = [](const Triplet& t) {
        size_t seed = 0;
        boost::hash_combine(seed, t.col());
        boost::hash_combine(seed, t.row());
        return seed;
    };
    auto eq_fcn = [](const Triplet& t1, const Triplet& t2) {
        return (t1.col() == t2.col()) && (t1.row() == t2.row());
    };
    std::unordered_set<Triplet, decltype(hash_fcn), decltype(eq_fcn)> values(
        nv, hash_fcn, eq_fcn);

    for (auto vi : vertices(mesh)) {
        int i = get(vimap, vi);
        T row_sum = 0.0;
        for (auto he : CGAL::halfedges_around_target(vi, mesh)) {
            auto vj = source(he, mesh);
            int j = get(vimap, vj);
            auto existing = values.find(Triplet(j, i, 0.0));
            if (existing != values.end()) {
                values.emplace(i, j, existing->value());
                row_sum -= existing->value();
            }
            else {
                auto value = cotangent_weight(he, mesh);
                values.emplace(i, j, -value);
                row_sum += value;
            }
        }
        values.emplace(i, i, row_sum);
    }

    Eigen::SparseMatrix<T> mat(nv, nv);
    mat.setFromTriplets(values.begin(), values.end());
    mat.makeCompressed();
    return mat;
}

template<typename Mesh>
Eigen::SparseMatrix<FT_t<Mesh>> mass_matrix(const Mesh& mesh,
                                            const VertexArea& method)
{
    using T = FT_t<Mesh>;
    const auto nv = num_vertices(mesh);
    Eigen::SparseMatrix<T> mass(nv, nv);
    std::vector<Eigen::Triplet<T>> values;

    int i = 0;
    for (auto v : vertices(mesh)) {
        auto area = vertex_area(v, mesh, method);
        values.emplace_back(i, i, area);
        ++i;
    }

    mass.setFromTriplets(values.begin(), values.end());
    mass.makeCompressed();
    return mass;
}

} // namespace Euclid


#include "gru_cell_old.hpp"
#include "generic/activity.hpp"
#include "generic/utils.hpp"
#include <Eigen/SVD>

namespace rnn {
gru_cell_old::gru_cell_old(const int inputDim, const int hiddenDim) {
  this->Wxr = MatD(hiddenDim, inputDim);
  this->Whr = MatD(hiddenDim, hiddenDim);
  this->br = VecD::Zero(hiddenDim);

  this->Wxz = MatD(hiddenDim, inputDim);
  this->Whz = MatD(hiddenDim, hiddenDim);
  this->bz = VecD::Zero(hiddenDim);

  this->Wxu = MatD(hiddenDim, inputDim);
  this->Whu = MatD(hiddenDim, hiddenDim);
  this->bu = VecD::Zero(hiddenDim);
}

void gru_cell_old::init(rnn::generic::rand &rnd, const real scale) {
  rnd.uniform(this->Wxr, scale);
  rnd.uniform(this->Whr, scale);

  rnd.uniform(this->Wxz, scale);
  rnd.uniform(this->Whz, scale);

  rnd.uniform(this->Wxu, scale);
  rnd.uniform(this->Whu, scale);

  this->Whr = Eigen::JacobiSVD<MatD>(this->Whr, Eigen::ComputeFullV |
      Eigen::ComputeFullU).matrixU();
  this->Whz = Eigen::JacobiSVD<MatD>(this->Whz, Eigen::ComputeFullV |
      Eigen::ComputeFullU).matrixU();
  this->Whu = Eigen::JacobiSVD<MatD>(this->Whu, Eigen::ComputeFullV |
      Eigen::ComputeFullU).matrixU();
}

gru_cell_old::State gru_cell_old::forward(const VecD &xt, const gru_cell_old::State &prev) {
//U=Wx, W=Wh, h^=u
  gru_cell_old::State cur;
  cur.r = br + Wxr * xt + Whr * prev.h;
  cur.z = bz + Wxz * xt + Whz * prev.h;

  activity::logistic(cur.r);
  activity::logistic(cur.z);

  cur.rh = cur.r.array() * prev.h.array();
  cur.u = bu + Wxu * xt + Whu * cur.rh;
  activity::tanh(cur.u);
  cur.h = (1.0 - cur.z.array()) * cur.u.array() +
      cur.z.array() * prev.h.array();
  return cur;
}

void gru_cell_old::backward(gru_cell_old::State *prev, gru_cell_old::State *cur, gru_cell_old::Grad &grad,
                        const VecD &xt) {
  VecD delr, delz, delu, delrh;
  //d7=delz
  delz = activity::logisticPrime(cur->z).array() * cur->delh.array() *
      (cur->u - prev->h).array();
  delu =
      activity::tanhPrime(cur->u).array() * cur->delh.array() * cur->z.array();
  delrh = Whu.transpose() * delu;
  delr =
      activity::logisticPrime(cur->r).array() * delrh.array() * prev->h.array();

  cur->delx = Wxr.transpose() * delr
      + Wxz.transpose() * delz
      + Wxu.transpose() * delu;

  prev->delh.noalias() += Whr.transpose() * delr + Whz.transpose() * delz;
  prev->delh.array() += delrh.array() * cur->r.array() +
          cur->delh.array() * (1.0 - cur->z.array());

  grad.Wxr.noalias() += delr * xt.transpose();
  grad.Whr.noalias() += delr * prev->h.transpose();

  grad.Wxz.noalias() += delz * xt.transpose();
  grad.Whz.noalias() += delz * prev->h.transpose();

  grad.Wxu.noalias() += delu * xt.transpose();
  grad.Whu.noalias() += delu * cur->rh.transpose();

  grad.br += delr;
  grad.bz += delz;
  grad.bu += delu;
}

void gru_cell_old::sgd(const gru_cell_old::Grad &grad, const real learningRate) {
  Wxr -= learningRate * grad.Wxr;
  Whr -= learningRate * grad.Whr;
  br -= learningRate * grad.br;

  Wxz -= learningRate * grad.Wxz;
  Whz -= learningRate * grad.Whz;
  bz -= learningRate * grad.bz;

  Wxu -= learningRate * grad.Wxu;
  Whu -= learningRate * grad.Whu;
  bu -= learningRate * grad.bu;
}

void gru_cell_old::save(std::ofstream &ofs) {
  rnn::generic::save(ofs, Wxr);
  rnn::generic::save(ofs, Whr);
  rnn::generic::save(ofs, br);
  rnn::generic::save(ofs, Wxz);
  rnn::generic::save(ofs, Whz);
  rnn::generic::save(ofs, bz);
  rnn::generic::save(ofs, Wxu);
  rnn::generic::save(ofs, Whu);
  rnn::generic::save(ofs, bu);
}

void gru_cell_old::load(std::ifstream &ifs) {
  rnn::generic::load(ifs, Wxr);
  rnn::generic::load(ifs, Whr);
  rnn::generic::load(ifs, br);
  rnn::generic::load(ifs, Wxz);
  rnn::generic::load(ifs, Whz);
  rnn::generic::load(ifs, bz);
  rnn::generic::load(ifs, Wxu);
  rnn::generic::load(ifs, Whu);
  rnn::generic::load(ifs, bu);
}

void gru_cell_old::State::clear() {
  this->h = VecD();
  this->u = VecD();
  this->r = VecD();
  this->z = VecD();
  this->rh = VecD();
  this->delh = VecD();
  this->delx = VecD();
}

gru_cell_old::Grad::Grad(const gru_cell_old &gru) {
  this->Wxr = MatD::Zero(gru.Wxr.rows(), gru.Wxr.cols());
  this->Whr = MatD::Zero(gru.Whr.rows(), gru.Whr.cols());
  this->br = VecD::Zero(gru.br.rows());

  this->Wxz = MatD::Zero(gru.Wxz.rows(), gru.Wxz.cols());
  this->Whz = MatD::Zero(gru.Whz.rows(), gru.Whz.cols());
  this->bz = VecD::Zero(gru.bz.rows());

  this->Wxu = MatD::Zero(gru.Wxu.rows(), gru.Wxu.cols());
  this->Whu = MatD::Zero(gru.Whu.rows(), gru.Whu.cols());
  this->bu = VecD::Zero(gru.bu.rows());
};

void gru_cell_old::Grad::init() {
  this->Wxr.setZero();
  this->Whr.setZero();
  this->br.setZero();
  this->Wxz.setZero();
  this->Whz.setZero();
  this->bz.setZero();
  this->Wxu.setZero();
  this->Whu.setZero();
  this->bu.setZero();
}

real gru_cell_old::Grad::norm() {
  return
      this->Wxr.squaredNorm() + this->Whr.squaredNorm() +
          this->br.squaredNorm() +
          this->Wxz.squaredNorm() + this->Whz.squaredNorm() +
          this->bz.squaredNorm() +
          this->Wxu.squaredNorm() + this->Whu.squaredNorm() +
          this->bu.squaredNorm();
}

void gru_cell_old::Grad::operator+=(const gru_cell_old::Grad &grad) {
  this->Wxr += grad.Wxr;
  this->Whr += grad.Whr;
  this->br += grad.br;
  this->Wxz += grad.Wxz;
  this->Whz += grad.Whz;
  this->bz += grad.bz;
  this->Wxu += grad.Wxu;
  this->Whu += grad.Whu;
  this->bu += grad.bu;
}
}


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of dual annealing
// derived from https://github.com/scipy/scipy/blob/master/scipy/optimize/_dual_annealing.py
// Implementation only differs regarding boundary handling - this implementattion
// uses boundary-normalized X values. Local search is fixed to LBFGS-B, see
// https://github.com/yixuan/LBFGSpp/tree/master/include
// requires https://github.com/imneme/pcg-cpp

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <math.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"
#include <LBFGSB.h>

using namespace LBFGSpp;
using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

namespace dual_annealing {

typedef double (*callback_type)(int, const double*);

// wrapper around the fitness function, scales according to boundaries

class Fitness;

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);
static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static Eigen::MatrixXd normalVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return gauss_01(rs);
    });
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static vec emptyVec = { };

static vec logv(vec v) {
    return v.unaryExpr([](double x) {
        return log(x);
    });
}

static vec expv(vec v) {
    return v.unaryExpr([](double x) {
        return exp(x);
    });
}

double minLBFGS(Fitness *fitfun, vec &X0_, int maxIterations);

class Fitness {

public:

    vec lower;
    vec upper;

    Fitness(callback_type pfunc, vec *lower_limit, vec *upper_limit,
            long maxEvals_) {
        func = pfunc;
        lower = *lower_limit;
        upper = *upper_limit;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
        maxEvals = maxEvals_;
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0) {
            return X.cwiseMin(1.0).cwiseMax(-1.0);
        }
        return X;
    }

    double eval(const vec &X) {
        int n = X.size();
        double res = func(n, X.data());
        evaluationCounter++;
        return res;
    }

    double value(const vec &X) {
        double res = DBL_MAX;
        if (lower.size() > 0)
            res = eval(decode(getClosestFeasible(X)));
        else
            res = eval(X);
        if (res < bestY) {
            bestY = res;
            bestX = vec(X);
        }
        return res;
    }

    const double LS_MAXITER_RATIO = 6;
    const double LS_MAXITER_MIN = 100;
    const double LS_MAXITER_MAX = 1000;

    double local_search(const vec &x0, double currval, vec &res) {
        vec init = getClosestFeasible(x0);
        bestY = DBL_MAX;
        int maxIter = LS_MAXITER_RATIO * x0.size();
        if (maxIter > LS_MAXITER_MAX)
            maxIter = LS_MAXITER_MAX;
        if (maxIter < LS_MAXITER_MIN)
            maxIter = LS_MAXITER_MIN;
        minLBFGS(this, init, maxIter);
        if (bestY < DBL_MAX) {
            for (int i = 0; i < res.size(); i++)
                res[i] = bestX(i);
        }
        return bestY;
    }

    vec encode(const vec &X) const {
        if (lower.size() > 0)
            return (X - lower).array() / scale.array();
        else
            return X;
    }

    vec decode(const vec &X) const {
        if (lower.size() > 0)
            return X.cwiseProduct(scale) + lower;
        else
            return X;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

    bool maxEvalReached() {
        return evaluationCounter >= maxEvals;
    }

private:
    callback_type func;
    long evaluationCounter = 0;
    long maxEvals;
    vec scale;
    double bestY = DBL_MAX;
    vec bestX;
};

class LBFGSFunc {
private:
    Fitness *func;
    int dim;

public:

    LBFGSFunc(Fitness *Fitness_, int dim_) {
        func = Fitness_;
        dim = dim_;
    }

    double operator()(const vec &x, vec &grad) {
        if (!x.allFinite())
            return DBL_MAX;
        double eps = 1E-6;
        vec arg = vec(dim);
        for (int i = 0; i < dim; i++)
            arg[i] = x(i);
        for (int i = 0; i < dim; i++) {
            vec x1 = vec(arg);
            vec x2 = vec(arg);
            double e1 = eps;
            double e2 = eps;
            x1[i] += eps;
            if (x1[i] > 1) {
                x1[i] = 1;
                e1 = 1 - arg[i];
            }
            x2[i] -= eps;
            if (x2[i] < 0) {
                x2[i] = 0;
                e2 = arg[i];
            }
            double f1 = func->value(x1);
            double f2 = func->value(x2);
            grad[i] = (f1 - f2) / (e1 + e2);
        }
        double f = func->value(arg);
        return f;
    }
};

double minLBFGS(Fitness *fitfun, vec &X0, int maxIterations) {
    int dim = X0.size();
    LBFGSFunc fun = LBFGSFunc(fitfun, dim);

    LBFGSBParam<double> param;
    param.max_iterations = maxIterations;
    LBFGSBSolver<double> solver(param);
    vec lb = vec::Constant(dim, 0.0);
    vec ub = vec::Constant(dim, 1.0);
    // Initial values
    vec x = vec::Constant(dim, 0);
    for (int i = 0; i < dim; i++)
        x[i] = X0[i];
    double fx;
    int niter;
    try {
        niter = solver.minimize(fun, x, fx, lb, ub);
    } catch (std::exception &e) {
        //cout << e.what() << endl;
        return DBL_MAX;
    }
    return fx;
}

class VisitingDistribution {

    //Class used to generate new coordinates based on the distorted
    //Cauchy-Lorentz distribution. Depending on the steps within the Markov
    //chain, the class implements the strategy for generating new location
    //changes.

public:

    VisitingDistribution(int dim, double visiting_param_, pcg64 *rs_) {
        _visiting_param = visiting_param_;
        rs = rs_;

        // these are invariant numbers unless visiting_param changes
        double factor2 = exp(
                (4.0 - _visiting_param) * log(_visiting_param - 1.0));
        double factor3 = exp(
                (2.0 - _visiting_param) * log(2.0) / (_visiting_param - 1.0));
        _factor4_p = sqrt(M_PI) * factor2 / (factor3 * (3.0 - _visiting_param));

        double factor5 = 1.0 / (_visiting_param - 1.0) - 0.5;
        double d1 = 2.0 - factor5;
        _factor6 = M_PI * (1.0 - factor5) / sin(M_PI * (1.0 - factor5))
                / exp(lgamma(d1));
    }

    vec visiting(const vec &x, int step, double temperature) {
        //Based on the step in the strategy chain, new coordinated are
        //generated by changing all components is the same time or only
        //one of them, the new values are computed with visit_fn method

        int dim = x.size();
        if (step < dim) {
            // Changing all coordinates with a new visiting value
            double upper_sample = distr_01(*rs);
            double lower_sample = distr_01(*rs);
            vec visits = visit_fn(temperature, dim);
            for (int i = 0; i < dim; i++) {
                if (visits[i] > TAIL_LIMIT)
                    visits[i] = TAIL_LIMIT * upper_sample;
                else if (visits[i] < -TAIL_LIMIT)
                    visits[i] = -TAIL_LIMIT * lower_sample;
            }
            vec x_visit = visits + x;
            vec a = x_visit;
            vec b = vec(dim);
            for (int i = 0; i < dim; i++) {
                b[i] = fmod(a[i], 1) + 1;
                x_visit[i] = fmod(b[i], 1);
                if (abs(x_visit[i]) < MIN_VISIT_BOUND)
                    x_visit[i] += 1.e-10;
            }
            //cerr << step << " " << temperature <<  endl;// << x_visit << endl;
            return x_visit;
        } else {
            // Changing only one coordinate at a time based on strategy
            // chain step
            vec x_visit = vec(x);
            double visit = visit_fn(temperature, 1)[0];
            if (visit > TAIL_LIMIT)
                visit = TAIL_LIMIT * distr_01(*rs);
            else if (visit < -TAIL_LIMIT)
                visit = -TAIL_LIMIT * distr_01(*rs);
            int index = step - dim;
            x_visit[index] = visit + x[index];
            double a = x_visit[index];
            double b = fmod(a, 1) + 1;
            x_visit[index] = fmod(b, 1);
            if (abs(x_visit[index]) < MIN_VISIT_BOUND)
                x_visit[index] += MIN_VISIT_BOUND;
            //cerr << step << " " << temperature <<  endl;// << x_visit << endl;
            return x_visit;
        }
    }

    vec visit_fn(double temperature, int dim) {

        //Formula Visita from p. 405 of reference [2]
        vec x = normalVec(dim, *rs);
        vec y = normalVec(dim, *rs);
        ;

        double factor1 = exp(log(temperature) / (_visiting_param - 1.0));
        double factor4 = _factor4_p * factor1;

        // sigmax
        x = x
                * exp(
                        -(_visiting_param - 1.0) * log(_factor6 / factor4)
                                / (3.0 - _visiting_param));

        vec den = expv(
                logv(y.cwiseAbs() * (_visiting_param - 1.0))
                        / (3.0 - _visiting_param));
        return x.cwiseQuotient(den);
    }

private:

    pcg64 *rs;
    double _visiting_param;
    double _factor4_p;
    double _factor6;

    const double TAIL_LIMIT = 1.e8;
    const double MIN_VISIT_BOUND = 1.e-10;
};

class nanexception: public exception {
    virtual const char* what() const throw () {
        return "Objective function is returning nan";
    }
} naneexc;

const double BIG_VALUE = 1e16;

class EnergyState {

    //Class used to record the energy state-> At any time, it knows what is the
    //currently used coordinates and the most recent best location
public:

    double ebest;
    vec xbest;
    double current_energy;
    vec current_location;

    EnergyState(int dim_) {
        dim = dim_;
        ebest = DBL_MAX;
        xbest = { };
        current_energy = DBL_MAX;
        current_location = { };
    }

    void reset(Fitness *owf, pcg64 *rs, const vec &x0) {
        if (x0.size() == 0)
            current_location = normalVec(dim, *rs);
        else
            current_location = vec(x0);
        bool init_error = true;
        int reinit_counter = 0;
        while (init_error) {
            current_energy = owf->value(current_location);
            if (current_energy >= BIG_VALUE || isnan(current_energy)) {
                if (reinit_counter >= MAX_REINIT_COUNT) {
                    init_error = false;
                    throw naneexc;
                }
                current_location = uniformVec(dim, *rs);
                reinit_counter++;
            } else
                init_error = false;
            // If first time reset, initialize ebest and xbest
            if (ebest == DBL_MAX && xbest.size() == 0) {
                ebest = current_energy;
                xbest = vec(current_location);
            }
            // Otherwise, keep them in case of reannealing reset
        }
    }

    void update_best(double e, const vec &x) {
        ebest = e;
        xbest = vec(x);
    }

    void update_current(double e, const vec &x) {
        current_energy = e;
        current_location = vec(x);
    }

private:
    // Maximimum number of trials for generating a valid starting point
    int MAX_REINIT_COUNT = 1000;
    int dim;
};

class StrategyChain {
    // Class used for the Markov chain and related strategy for local search
    // decision
public:

    StrategyChain(double acceptance_param_, VisitingDistribution *vd_,
            Fitness *ofw_, pcg64 *rs_, EnergyState *state_) {
        // Global optimizer state
        state = state_;
        // Local markov chain minimum energy and location
        emin = state->current_energy;
        xmin = vec(state->current_location);
        // Acceptance parameter
        acceptance_param = acceptance_param_;
        // Visiting distribution instance
        vd = vd_;
        // Wrapper to objective function and related local minimizer
        ofw = ofw_;
        not_improved_idx = 0;
        not_improved_max_idx = 1000;
        rs = rs_;
        temperature_step = 0;
        K = 100 * (state->current_location).size();
    }

    void accept_reject(int j, double e, const vec &x_visit) {
        double r = distr_01(*rs);
        double pqv_temp = (acceptance_param - 1.0) * (e - state->current_energy)
                / (temperature_step + 1.);
        double pqv = 0;
        if (pqv_temp < 0.)
            pqv = 0.;
        else
            pqv = exp(log(pqv_temp) / (1. - acceptance_param));
        if (r <= pqv) {
            // We accept the new location and update state
            state->update_current(e, x_visit);
            xmin = vec(state->current_location);
        }
        // No improvement since long time
        if (not_improved_idx >= not_improved_max_idx) {
            if (j == 0 || state->current_energy < emin) {
                emin = state->current_energy;
                xmin = vec(state->current_location);
            }
        }
    }

    void run(int step, double temperature) {
        temperature_step = temperature / (double) (step + 1);
        not_improved_idx += 1;
        for (unsigned int j = 0; j < (state->current_location).size() * 2;
                j++) {
            if (j == 0)
                state_improved = false;
            if (step == 0 && j == 0)
                state_improved = true;
            vec x_visit = vd->visiting(state->current_location, j, temperature);
            // Calling the objective function
            double e = ofw->value(x_visit);
            if (e < state->current_energy) {
                // We have got a better energy value
                state->update_current(e, x_visit);
                if (e < state->ebest) {
                    state->update_best(e, x_visit);
                    state_improved = true;
                    not_improved_idx = 0;
                }
            } else {
                // We have not improved but do we accept the new location?
                accept_reject(j, e, x_visit);
            }
            if (ofw->maxEvalReached())
                return;
        }	// End of StrategyChain loop
    }

    void local_search() {
        // Decision making for performing a local search
        // based on Markov chain results
        // If energy has been improved or no improvement since too long,
        // performing a local search with the best Markov chain location
        int dim = state->xbest.size();
        if (state_improved) {
            // Global energy has improved, let's see if LS improved further
            vec x = vec(dim);
            double e = ofw->local_search(state->xbest, state->ebest, x);
            if (e < state->ebest) {
                not_improved_idx = 0;
                state->update_best(e, x);
                state->update_current(e, x);
                if (ofw->maxEvalReached())
                    return;
            }
        }
        // Check probability of a need to perform a LS even if no improvment
        bool do_ls = false;
        if (K < 90 * state->current_location.size()) {
            double pls = exp(
                    K * (state->ebest - state->current_energy)
                            / temperature_step);
            if (pls >= distr_01(*rs))
                do_ls = true;
        }
        // Global energy not improved, let's see what LS gives
        // on the best strategy chain location
        if (not_improved_idx >= not_improved_max_idx)
            do_ls = true;
        if (do_ls) {
            vec x = vec(dim);
            double e = ofw->local_search(xmin, state->ebest, x);
            xmin = vec(x);
            emin = e;
            not_improved_idx = 0;
            not_improved_max_idx = state->current_location.size();
            if (e < state->ebest) {
                state->update_best(emin, xmin);
                state->update_current(e, x);
            }
        }
    }

private:

    double emin;
    vec xmin;
    EnergyState *state;
    double acceptance_param;
    VisitingDistribution *vd;
    int not_improved_idx;
    int not_improved_max_idx;
    pcg64 *rs;
    Fitness *ofw;
    double temperature_step;
    double K;
    bool state_improved = false;
};

class sizeexception: public exception {
    virtual const char* what() const throw () {
        return "Bounds size does not match x0";
    }
} sizeeexc;

class DARunner {

public:

    DARunner(Fitness *fun_, vec &x0_, long seed_, bool use_local_search_) {
        owf = fun_;
        if (x0_.size() > 0 && x0_.size() != owf->lower.size())
            throw sizeeexc;
        //Initialization of RandomState for reproducible runs if seed provided
        rs = new pcg64(seed_);
        use_local_search = use_local_search_;
        // Initialization of the energy state
        es = new EnergyState(owf->lower.size());
        es->reset(owf, rs, x0_);
        // VisitingDistribution instance
        vd = new VisitingDistribution(owf->lower.size(), qv, rs);
        // Markov chain instance
        sc = new StrategyChain(qa, vd, owf, rs, es);
    }

    ~DARunner() {
        delete rs;
        delete vd;
        delete sc;
        delete es;
    }

    void search() {
        iter = 0;
        double t1 = exp((qv - 1) * log(2.0)) - 1.0;
        for (;;) {
            for (int i = 0; i < maxsteps; i++) {
                // Compute temperature for this step
                double s = i + 2.0;
                double t2 = exp((qv - 1) * log(s)) - 1.0;
                double temperature = temperature_start * t1 / t2;
                if (iter++ >= maxsteps)
                    return;
                // Need a re-annealing process?
                if (temperature < temperature_restart) {
                    es->reset(owf, rs, emptyVec);
                    break;
                }
                // starting strategy chain
                sc->run(i, temperature);
                if (owf->maxEvalReached())
                    return;
                if (use_local_search) {
                    sc->local_search();
                    if (owf->maxEvalReached())
                        return;
                }
            }
        }
    }

    vec bestX() {
        return es->xbest;
    }

    double bestY() {
        return es->ebest;
    }

private:

    int MAX_REINIT_COUNT = 1000;
    double temperature_start = 5230;
    double qv = 2.62;
    double qa = -5.0;
    bool use_local_search;
    // maximum number of step (main iteration)
    double maxsteps = 1000;
    // minimum value of annealing temperature reached to perform
    // re-annealing temperature_start
    double temperature_restart = 0.1;
    Fitness *owf;
    pcg64 *rs;
    EnergyState *es;
    StrategyChain *sc;
    VisitingDistribution *vd;
    int iter = 0;
};

double minimize(Fitness *fun, vec &x0, long seed, bool use_local_search,
        vec &X) {
    DARunner gr = DARunner(fun, x0, seed, use_local_search);
    gr.search();
    int dim = x0.size();
    vec bx = gr.bestX();
    for (int i = 0; i < dim; i++)
        X[i] = bx[i];
    return gr.bestY();
}
}

using namespace dual_annealing;

extern "C" {
void optimizeDA_C(long runid, callback_type func, int dim, int seed,
        double *init, double *lower, double *upper, int maxEvals,
        bool use_local_search, double* res) {
    int n = dim;
    vec guess(n), lower_limit(n), upper_limit(n);
    bool useLimit = false;
    for (int i = 0; i < n; i++) {
        guess[i] = init[i];
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    if (maxEvals <= 0)
        maxEvals = 1E7;
    Fitness fitfun(func, &lower_limit, &upper_limit, maxEvals);

    try {
        vec X = zeros(dim);
        vec enc = fitfun.encode(guess);
        double bestY = minimize(&fitfun, enc, seed, use_local_search, X);
        vec bestX = fitfun.decode(X);
        for (int i = 0; i < n; i++)
            res[i] = bestX[i];
        res[n] = bestY;
        res[n + 1] = fitfun.getEvaluations();
        res[n + 2] = 0;
        res[n + 3] = 0;
    } catch (std::exception &e) {
        cerr << e.what() << endl;
    }
}
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HPSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HPSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A Hermitian indefinite matrix stored in packed format
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * hpsv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N Hermitian matrix in packed
     * storage and X and B are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^H,  if UPLO = 'U',
     *   A = L * D * L^H,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is Hermitian and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks. The factored
     * form of A is then used to solve the system of equations A * X = B.
     */

    namespace detail {

      inline
      void hpsv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* ap, int* ipiv,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CHPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void hpsv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* ap, int* ipiv,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZHPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      template <typename HermA, typename MatrB, typename IVec>
      inline
      int hpsv (HermA& a, IVec& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<HermA>::matrix_structure,
          traits::hermitian_packed_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
#endif

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));

        char uplo = traits::matrix_uplo_tag (a);
        int info;
        hpsv (uplo, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              &info);
        return info;
      }

    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hpsv (HermA& a, IVec& i, MatrB& b) {
      assert (traits::matrix_size1 (a) == traits::vector_size (i));
      return detail::hpsv (a, i, b);
    }

    template <typename HermA, typename MatrB>
    inline
    int hpsv (HermA& a, MatrB& b) {
      // with 'internal' pivot vector

      int info = -101;
      traits::detail::array<int> i (traits::matrix_size1 (a));

      if (i.valid())
        info = detail::hpsv (a, i, b);
      return info;
    }

    /*
     * hptrf() computes the factorization of a Hermitian matrix A
     * in packed storage using the  Bunch-Kaufman diagonal pivoting
     * method. The form of the factorization is
     *    A = U * D * U^H  or  A = L * D * L^H
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is Hermitian and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail {

      inline
      void hptrf (char const uplo, int const n,
                  traits::complex_f* ap, int* ipiv, int* info)
      {
        LAPACK_CHPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

      inline
      void hptrf (char const uplo, int const n,
                  traits::complex_d* ap, int* ipiv, int* info)
      {
        LAPACK_ZHPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

      template <typename HermA, typename IVec, typename Work>
      inline
      int hptrf (char const ul, HermA& a, IVec& i, Work& w, int const lw) {

      }

    }

    template <typename HermA, typename IVec>
    inline
    int hptrf (HermA& a, IVec& i) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::hptrf (uplo, n, traits::matrix_storage (a),
                     traits::vector_storage (i), &info);
      return info;
    }

    /*
     * hptrs() solves a system of linear equations A*X = B with
     * a Hermitian matrix A in packed storage using the factorization
     *    A = U * D * U^H   or  A = L * D * L^H
     * computed by hptrf().
     */

    namespace detail {

      inline
      void hptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* ap, int const* ipiv,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CHPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (ap), ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void hptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* ap, int const* ipiv,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZHPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (ap), ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hptrs (HermA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::hptrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
                     traits::vector_storage (i),
#else
                     traits::matrix_storage_const (a),
                     traits::vector_storage_const (i),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
        return info;
    }

    // TO DO: hptri

  }

}}}

#endif


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_HAVERSINE_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_HAVERSINE_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>

#include <boost/geometry/strategies/distance.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

namespace comparable
{

// Comparable haversine.
// To compare distances, we can avoid:
// - multiplication with radius and 2.0
// - applying sqrt
// - applying asin (which is strictly (monotone) increasing)
template
<
    typename RadiusType,
    typename CalculationType = void
>
class haversine
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef RadiusType radius_type;

    explicit inline haversine(RadiusType const& r = 1.0)
        : m_radius(r)
    {}

    template <typename Point1, typename Point2>
    static inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& p1, Point2 const& p2)
    {
        return calculate<typename calculation_type<Point1, Point2>::type>(
                   get_as_radian<0>(p1), get_as_radian<1>(p1),
                   get_as_radian<0>(p2), get_as_radian<1>(p2)
               );
    }

    inline RadiusType radius() const
    {
        return m_radius;
    }

private :
    template <typename R, typename T1, typename T2>
    static inline R calculate(T1 const& lon1, T1 const& lat1,
                              T2 const& lon2, T2 const& lat2)
    {
        return math::hav(lat2 - lat1)
                + cos(lat1) * cos(lat2) * math::hav(lon2 - lon1);
    }

    RadiusType m_radius;
};

} // namespace comparable

/*!
\brief Distance calculation for spherical coordinates
on a perfect sphere using haversine
\ingroup strategies
\tparam RadiusType \tparam_radius
\tparam CalculationType \tparam_calculation
\author Adapted from: http://williams.best.vwh.net/avform.htm
\see http://en.wikipedia.org/wiki/Great-circle_distance
\note (from Wiki:) The great circle distance d between two
points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
    d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
A mathematically equivalent formula, which is less subject
    to rounding error for short distances is:
    d=2*asin(sqrt((sin((lat1-lat2) / 2))^2
    + cos(lat1)*cos(lat2)*(sin((lon1-lon2) / 2))^2))

\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename RadiusType,
    typename CalculationType = void
>
class haversine
{
    typedef comparable::haversine<RadiusType, CalculationType> comparable_type;

public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : services::return_type<comparable_type, Point1, Point2>
    {};

    typedef RadiusType radius_type;

    /*!
    \brief Constructor
    \param radius radius of the sphere, defaults to 1.0 for the unit sphere
    */
    inline haversine(RadiusType const& radius = 1.0)
        : m_radius(radius)
    {}

    /*!
    \brief applies the distance calculation
    \return the calculated distance (including multiplying with radius)
    \param p1 first point
    \param p2 second point
    */
    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& p1, Point2 const& p2) const
    {
        typedef typename calculation_type<Point1, Point2>::type calculation_type;
        calculation_type const a = comparable_type::apply(p1, p2);
        calculation_type const c = calculation_type(2.0) * asin(math::sqrt(a));
        return calculation_type(m_radius) * c;
    }

    /*!
    \brief access to radius value
    \return the radius
    */
    inline RadiusType radius() const
    {
        return m_radius;
    }

private :
    RadiusType m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename RadiusType, typename CalculationType>
struct tag<haversine<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<haversine<RadiusType, CalculationType>, P1, P2>
    : haversine<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<haversine<RadiusType, CalculationType> >
{
    typedef comparable::haversine<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<haversine<RadiusType, CalculationType> >
{
private :
    typedef haversine<RadiusType, CalculationType> this_type;
    typedef comparable::haversine<RadiusType, CalculationType> comparable_type;
public :
    static inline comparable_type apply(this_type const& input)
    {
        return comparable_type(input.radius());
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<haversine<RadiusType, CalculationType>, P1, P2>
{
private :
    typedef haversine<RadiusType, CalculationType> this_type;
    typedef typename return_type<this_type, P1, P2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(this_type const& , T const& value)
    {
        return return_type(value);
    }
};

// Specializations for comparable::haversine
template <typename RadiusType, typename CalculationType>
struct tag<comparable::haversine<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<comparable::haversine<RadiusType, CalculationType>, P1, P2>
    : comparable::haversine<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<comparable::haversine<RadiusType, CalculationType> >
{
    typedef comparable::haversine<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<comparable::haversine<RadiusType, CalculationType> >
{
private :
    typedef comparable::haversine<RadiusType, CalculationType> this_type;
public :
    static inline this_type apply(this_type const& input)
    {
        return input;
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<comparable::haversine<RadiusType, CalculationType>, P1, P2>
{
private :
    typedef comparable::haversine<RadiusType, CalculationType> strategy_type;
    typedef typename return_type<strategy_type, P1, P2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(strategy_type const& strategy, T const& distance)
    {
        return_type const s = sin((distance / strategy.radius()) / return_type(2));
        return s * s;
    }
};

// Register it as the default for point-types
// in a spherical equatorial coordinate system
template <typename Point1, typename Point2>
struct default_strategy
    <
        point_tag, point_tag, Point1, Point2,
        spherical_equatorial_tag, spherical_equatorial_tag
    >
{
    typedef strategy::distance::haversine<typename select_coordinate_type<Point1, Point2>::type> type;
};

// Note: spherical polar coordinate system requires "get_as_radian_equatorial"

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_HAVERSINE_HPP


#ifndef NBT_INTEGRATOR_HPP
#define NBT_INTEGRATOR_HPP

#include <Eigen>

/**
 * Abstract function object for integrators. Integrators
 * are functional interfaces that update the position and
 * velocity matrices using an acceleration matrix.
 */
class Integrator {
    public:
        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        virtual void integrate(double dt,
                               const Eigen::Ref<const Eigen::MatrixXd>& a,
                               Eigen::Ref<Eigen::MatrixXd> v,
                               Eigen::Ref<Eigen::MatrixXd> x) = 0;
};

/**
 * Performs Euler integration. Fast but inaccurate.
 */
class EulerIntegrator: public Integrator {
    public:
        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        void integrate(double dt,
                       const Eigen::Ref<const Eigen::MatrixXd>& a,
                       Eigen::Ref<Eigen::MatrixXd> v,
                       Eigen::Ref<Eigen::MatrixXd> x);
};

/**
 * Performs Verlet integration. Is symplectic but not as accurate as RungeKuttaIntegrator.
 */
class VerletIntegrator: public Integrator {
    public:
        bool isFirstIteration = true;
        Eigen::MatrixXd aPrev;

        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        void integrate(double dt,
                       const Eigen::Ref<const Eigen::MatrixXd>& a,
                       Eigen::Ref<Eigen::MatrixXd> v,
                       Eigen::Ref<Eigen::MatrixXd> x);
};

/**
 * Performs Runge-Kutta 4th order integration method. More accurate than EulerIntegrator but slower.
 */
class RungeKuttaIntegrator: public Integrator {
    public:
        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        void integrate(double dt,
                       const Eigen::Ref<const Eigen::MatrixXd>& a,
                       Eigen::Ref<Eigen::MatrixXd> v,
                       Eigen::Ref<Eigen::MatrixXd> x); // TODO runge kutta
};

#endif


/**
 * @author Tomas Polasek
 * @date 11.20.2019
 * @version 1.0
 * @brief Utilities and statistics for the treeio::Tree class.
 */

#include "TreeIOAccelerationUtils.h"

#include <Eigen/Eigen>

#include <intersect/intersect.h>

namespace treeacc
{

Line Line::fromDirection(const Vector3D &origin, const Vector3D &direction)
{ return Line{ origin, origin + direction }; }

BoundingBox3D::BoundingBox3D() :
    BoundingBox3D( Vector3D{ } )
{ }
BoundingBox3D::BoundingBox3D(const Vector3D &pos) :
    BoundingBox3D(pos, pos)
{ }
BoundingBox3D::BoundingBox3D(const Vector3D &lv, const Vector3D &hv)
{ min = lv; max = hv; }

bool BoundingBox3D::intersects(const Ray &ray) const
{ return intersection(ray) < std::numeric_limits<float>::max(); }

float BoundingBox3D::intersection(const Ray &ray) const
{
    const auto tMinCorner{ (min - ray.origin) / ray.direction };
    const auto tMaxCorner{ (max - ray.origin) / ray.direction };
    const auto tMin{ Vector3D::elementMin(tMinCorner, tMaxCorner).max() };
    const auto tMax{ Vector3D::elementMax(tMinCorner, tMaxCorner).min() };

    return (tMax < 0.0f || tMin > tMax) ? std::numeric_limits<float>::max() : tMin;
}

float calculateTriangleArea(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto a{ p2 - p1 };
    const auto b{ p3 - p1 };

    const auto prod{ Vector3D::crossProduct(a, b) };

    return prod.length() / 2.0f;
}

float calculateTrianglePerimeter(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto a{ p2 - p1 };
    const auto b{ p3 - p1 };
    const auto c{ p2 - p3 };

    return a.length() + b.length() + c.length();
}

Vector3D calculateTriangleMedianPoint(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{ return (p1 + p2 + p3) / 3.0f; }

float calculateTriangleMedian(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto p1p2{ p1 + p2 / 2.0f };
    return (p3 - p1p2).length();
}

float calculateTriangleLongestMedian(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto m1{ calculateTriangleMedian(p1, p2, p3) };
    const auto m2{ calculateTriangleMedian(p2, p3, p1) };
    const auto m3{ calculateTriangleMedian(p1, p3, p2) };

    return std::max<float>({ m1, m2, m3 });
}

Vector3D sampleTriangle(const Vector3D &a1, const Vector3D &a2, const Vector3D &a3, const Vector3D &w)
{
    // TODO - Vector3D scalar multiplication + addition seems broken?
    return {
        w.x * a1.x + w.y * a2.x + w.z * a3.x,
        w.x * a1.y + w.y * a2.y + w.z * a3.y,
        w.x * a1.z + w.y * a2.z + w.z * a3.z
    };
}

/**
 * Sample barycentric U coordinate using provided random value.
 * Source: Portsmouth, Jamie. "Efficient barycentric point sampling on meshes."
 *
 * @param phiU Normalized relative weight for U-axis.
 * @param phiV Normalized relative weight for V-axis.
 * @param rand Random value used for sampling U coordinate.
 * @param limit Quality limit for inner loop.
 *
 * @return Returns sampled U coordinate.
 */
float sampleBarycentricU(float phiU, float phiV, float rand, float limit = 5.0e-3)
{
    static constexpr auto MAX_ITERATIONS{ 20u };
    static constexpr auto EPSILON{ std::numeric_limits<float>::epsilon() };
    const auto l{ (2.0f * phiU - phiV) / 3.0f };
    auto u{ 0.5f };
    for (std::size_t iter = 0u; iter < MAX_ITERATIONS; ++iter)
    {
        const auto u1{ 1.0f - u };
        const auto P{ u * (2.0f - u) - l * u * u1 * u1 - rand};
        const auto Pd{ std::max<float>(u1 * (2.0f + l * (3.0f * u - 1.0f)), EPSILON)};
        const auto du{ std::max<float>(std::min<float>(P / Pd, 0.25f), -0.25f) };

        u -= du;
        u = std::max<float>(std::min<float>(u, 1.0f - EPSILON), EPSILON);
        if (std::fabs(du) < limit)
        { break; }
    }

    return u;
}

/**
 * Sample barycentric V coordinate using provided random value.
 * Source: Portsmouth, Jamie. "Efficient barycentric point sampling on meshes."
 *
 * @param u Previously sampled u coordinate.
 * @param phiU Normalized relative weight for U-axis.
 * @param phiV Normalized relative weight for V-axis.
 * @param rand Random value used for sampling V coordinate.
 *
 * @return Returns sampled V coordinate.
 */
float sampleBarycentricV(float u, float phiU, float phiV, float rand)
{
    static constexpr auto EPSILON{ std::numeric_limits<float>::epsilon() };

    if (std::fabs(phiV) < EPSILON)
    { return (1.0f - u) * rand; }

    const auto tau{ 1.0f / 3.0f - (1.0f + (u - 1.0f / 3.0f) * phiU) / phiV };
    const auto tmp{ tau + u - 1.0f };
    const auto q{ std::sqrt(tau * tau * (1.0f - rand) + tmp * tmp * rand) };

    return tau <= 0.5f * (1.0f - u) ? tau + q : tau - q;
}

Vector3D triangleWeights(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3, const Vector3D &phi)
{
    /*
     * Source: Portsmouth, Jamie. "Efficient barycentric point sampling on meshes."
     */

    // Recover densities at each of the triangle vertices.
    const auto phi1{ phi.x };
    const auto phi2{ phi.y };
    const auto phi3{ phi.z };
    const auto averagePhi{ (phi1 + phi2 + phi3) / 3.0f};

    // Transform absolute distances to normalized relative weights.
    const auto relativePhi1{ (phi1 - phi3) / averagePhi };
    const auto relativePhi2{ (phi2 - phi3) / averagePhi };

    // Get independent random samples from uniform distribution.
    const auto randU{ treeutil::uniformZeroToOne<float>() };
    const auto randV{ treeutil::uniformZeroToOne<float>() };

    // Sample barycentric coordinates.
    const auto u{ sampleBarycentricU(relativePhi1, relativePhi2, randU) };
    const auto v{ sampleBarycentricV(u, relativePhi1, relativePhi2, randV) };
    const auto w{ 1.0f - u - v };

    return { u, v, w };
}

Vector3D uniformTriangleWeights(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{ return triangleWeights(p1, p2, p3, { 1.0f, 1.0f, 1.0f }); }

Vector3D medianTriangleWeights(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    // Median - center of gravity of input triangle.
    const auto median{ calculateTriangleMedianPoint(p1, p2, p3) };

    // Calculate distances median <-> point.
    const auto phi1{ (p1 - median).length() };
    const auto phi2{ (p2 - median).length() };
    const auto phi3{ (p3 - median).length() };

    return triangleWeights(p1, p2, p3, { phi1, phi2, phi3 });
}

Vertex3D uniformSampleTriangle(const Vertex3D &v1, const Vertex3D &v2, const Vertex3D &v3)
{
    const auto barycentricCoordinates{ uniformTriangleWeights(v1.p, v2.p, v3.p) };

    const auto p{ sampleTriangle(v1.p, v2.p, v3.p, barycentricCoordinates) };
    auto n{ sampleTriangle(v1.n, v2.n, v3.n, barycentricCoordinates) };
    n.normalize();

    return { p, n };
}

Vertex3D uniformSampleTriangle(const Vertex3D &v1, const Vertex3D &v2, const Vertex3D &v3, const Vector3D &weights)
{
    const auto barycentricCoordinates{ triangleWeights(v1.p, v2.p, v3.p, weights) };

    const auto p{ sampleTriangle(v1.p, v2.p, v3.p, barycentricCoordinates) };
    auto n{ sampleTriangle(v1.n, v2.n, v3.n, barycentricCoordinates) };
    n.normalize();

    return { p, n };
}

Vector3D middleSampleTriangle(const Vector3D &a1, const Vector3D &a2, const Vector3D &a3)
{ return sampleTriangle(a1, a2, a3, Vector3D{ 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 3.0f }); }

Vector3D triangleNormal(const Vector3D &a1, const Vector3D &a2, const Vector3D &a3)
{
    const auto f{ a2 - a1 }; const auto s{ a3 - a1 };
    return Vector3D{
        f.y * s.z - f.z * s.y,
        f.z * s.x - f.x * s.z,
        f.x * s.y - f.y * s.x
    };
}

Triangle::Triangle(std::size_t idx,
    const treeio::ModelImporterBase::Vertex *vb,
    const treeio::ModelImporterBase::IndexElementT *ib):
    tIdx{ idx }
{
    const auto triangleBaseIndex{ idx * 3u };

    i1 = ib[triangleBaseIndex + 0u];
    i2 = ib[triangleBaseIndex + 1u];
    i3 = ib[triangleBaseIndex + 2u];

    v1 = modelVertexToCloudPoint(vb[i1]);
    v2 = modelVertexToCloudPoint(vb[i2]);
    v3 = modelVertexToCloudPoint(vb[i3]);
    lv = Vector3D{
        std::min<float>(std::min<float>(v1.x, v2.x), v3.x),
        std::min<float>(std::min<float>(v1.y, v2.y), v3.y),
        std::min<float>(std::min<float>(v1.z, v2.z), v3.z),
    };
    hv = Vector3D{
        std::max<float>(std::max<float>(v1.x, v2.x), v3.x),
        std::max<float>(std::max<float>(v1.y, v2.y), v3.y),
        std::max<float>(std::max<float>(v1.z, v2.z), v3.z),
    };
    c = middleSampleTriangle(v1, v2, v3);

    n1 = modelNormalToCloudNormal(vb[i1]);
    n2 = modelNormalToCloudNormal(vb[i2]);
    n3 = modelNormalToCloudNormal(vb[i3]);
}

float Triangle::centroidDistance(const Triangle &other) const
{ return Vector3D::distance(c, other.c); }

float Triangle::centroidSquaredDistance(const Triangle &other) const
{ return Vector3D::squaredDistance(c, other.c); }

float Triangle::centroidSquaredDistance(const Vector3D &point) const
{ return Vector3D::squaredDistance(c, point); }

bool Triangle::intersects(const Triangle &other) const
{
    // Vertices of this triangle:
    double v11[3]{ v1.x, v1.y, v1.z };
    double v12[3]{ v2.x, v2.y, v2.z };
    double v13[3]{ v3.x, v3.y, v3.z };

    // Vertices of the other triangle:
    double v21[3]{ other.v1.x, other.v1.y, other.v1.z };
    double v22[3]{ other.v2.x, other.v2.y, other.v2.z };
    double v23[3]{ other.v3.x, other.v3.y, other.v3.z };

    const auto result{ tri_tri_overlap_test_3d(
        v11, v12, v13,
        v21, v22, v23
    ) };

    return result != 0;
}

bool Triangle::intersects(const Ray &ray) const
{ return intersection(ray) < std::numeric_limits<float>::max(); }

float Triangle::intersection(const Ray &ray) const
{
    // Constants:
    static constexpr auto EPSILON{ std::numeric_limits<float>::epsilon() };

    // Moller Trumbore algorithm:
    const auto edge1{ v2 - v1 };
    const auto edge2{ v3 - v1 };
    const auto cross{ Vector3D::crossProduct(ray.direction, edge2) };
    const auto det{ Vector3D::dotProduct(edge1, cross) };

    // Ray is parallel to the triangle.
    /*
    std::cout << "v1: " << v1 << " v2: " << v2 << " v3: " << v2 << std::endl;
    std::cout << "edg1: " << edge1 << std::endl;
    std::cout << "edg2: " << edge2 << std::endl;
    std::cout << "cross: " << cross << std::endl;
    std::cout << "det: " << det << std::endl;
     */
    if (std::abs<float>(det) < EPSILON)
    { return std::numeric_limits<float>::max(); }

    const auto invDet{ 1.0f / det };
    const auto uVec{ ray.origin - v1 };
    const auto vVec{ Vector3D::crossProduct(uVec, edge1) };

    // Check intersection point is within triangle.
    const auto u{ invDet * Vector3D::dotProduct(uVec, cross) };
    //std::cout << "u: " << u << std::endl;
    if (u < -EPSILON || u > 1.0f + EPSILON)
    { return std::numeric_limits<float>::max(); }
    const auto v{ invDet * Vector3D::dotProduct(ray.direction, vVec) };
    //std::cout << "v " << v << std::endl;
    if (v < -EPSILON || u + v > 1.0f + EPSILON)
    { return std::numeric_limits<float>::max(); }

    // Calculate intersection time.
    const auto t{ invDet * Vector3D::dotProduct(edge2, vVec) };
    return t;
}

bool Triangle::contains(const Vector3D &point, float epsilon) const
{
#if 0
    /*
     * Using volume of a tetrahedron v1, v2, v3, point:
     *
     * 1) Calculate volume.
     * 2) Point is inside <-> volume < epsilon
     */
    return std::abs<float>(
        Vector3D::dotProduct(
            v1 - point,
            Vector3D::crossProduct(v2 - point, v3 - point)
        )
    ) < epsilon;

#else

    /*
     * Using barycentric coordinates:
     *
     * 1) Project point onto the triangle plane and get its distance -> distance.
     * 2) Calculate barycentrics on the 2D plane -> alpha, beta.
     * 3) Point is inside <-> distance < epsilon && alpha > -epsilon && beta > -epsilon && alpha + beta <= 1 + epsilon
     */

    const auto pn{ centralPlaneNormal() };
    const auto distance{ Vector3D::dotProduct(pn, point) };
    const auto projected{ pn * distance };

    const auto s2{ v2 - v1 };
    const auto s3{ v3 - v1 };
    const auto sp{ point - v1 };

    const auto d2p{ Vector3D::dotProduct(s2, sp) };
    const auto d3p{ Vector3D::dotProduct(s2, sp) };
    const auto dsh{ Vector3D::dotProduct(s2, s3) };
    const auto s2l{ s2.length() };
    const auto s3l{ s3.length() };
    // Normalize for alpha and beta to be 1.0 when reaching v2 and v3 respectively.
    const auto normalizer{ 1.0f / (d2p * d3p - dsh * dsh) };
    // Alpha represents distance along s2.
    const auto alpha{ (d2p * s3l - dsh * d3p) * normalizer };
    const auto beta{ (d3p * s2l - dsh * d2p) * normalizer };

    return alpha > -epsilon && beta > -epsilon && alpha + beta <= 1.0f + epsilon;
#endif
}

Vector3D Triangle::barycentrics(const Vector3D &point) const
{
    // Source: https://en.wikipedia.org/wiki/Barycentric_coordinate_system#Conversion_between_barycentric_and_Cartesian_coordinates
    const auto &p{ point };
    const auto b1{
        ((v2.y - v3.y) * (p.x - v3.x) + (v3.x - v2.x) * (p.y - v3.y)) /
        ((v2.y - v3.y) * (v1.x - v3.x) + (v3.x - v2.x) * (v1.y - v3.y))
    };
    const auto b2{
        ((v3.y - v1.y) * (p.x - v3.x) + (v1.x - v3.x) * (p.y - v3.y)) /
        ((v2.y - v3.y) * (v1.x - v3.x) + (v3.x - v2.x) * (v1.y - v3.y))
    };
    const auto b3{ 1.0f - b1 - b2 };

    return { b1, b2, b3 };
}

BoundingBox3D Triangle::aabb() const
{ return BoundingBox3D(lv, hv); }

float Triangle::area() const
{ return calculateTriangleArea(v1, v2, v3); }

float Triangle::perimeter() const
{ return calculateTrianglePerimeter(v1, v2, v3); }

float Triangle::longestAltitude() const
{ return calculateTriangleLongestMedian(v1, v2, v3); }

Vector3D Triangle::centralPlaneNormal() const
{
    /*
     * Use cross product of two sides:
     *    3
     *  /   \
     * 1 --- 2
     * Counter clock-wise -> front-face.
     * normal = cross(1->2, 1->3)
     */
    const auto c1{ v2 - v1 };
    const auto c2{ v3 - v1 };
    auto planeNormal{ Vector3D::crossProduct(c1, c2) };
    planeNormal.normalize();

    return planeNormal;
}

void Triangle::providePlaneNormals(const Vector3D &pNormal1, const Vector3D &pNormal2, const Vector3D &pNormal3)
{
    providedPlaneNormals = true;

    pn1 = pNormal1;
    pn2 = pNormal2;
    pn3 = pNormal3;

    const auto l1{ Line::fromDirection(v1, pn1) };
    const auto l2{ Line::fromDirection(v2, pn2) };
    const auto l3{ Line::fromDirection(v3, pn3) };

    const auto i12{ lsIntersect({ l1, l2 }) };
    const auto i13{ lsIntersect({ l1, l3 }) };
    const auto i23{ lsIntersect({ l2, l3 }) };

    //d12 = Vector3D::distance(v1 + (v2 - v1) * Vector3D::dotProduct((v2 - v1), (i12 - v1)), i12);
    //d13 = Vector3D::distance(v1 + (v3 - v1) * Vector3D::dotProduct((v3 - v1), (i13 - v1)), i13);
    //d23 = Vector3D::distance(v2 + (v3 - v2) * Vector3D::dotProduct((v3 - v2), (i23 - v2)), i13);

    pr12 = (Vector3D::distance(i12, v1) + Vector3D::distance(i12, v2)) / 2.0f;
    pr13 = (Vector3D::distance(i13, v1) + Vector3D::distance(i13, v3)) / 2.0f;
    pr23 = (Vector3D::distance(i23, v2) + Vector3D::distance(i23, v3)) / 2.0f;
}

Vector3D Triangle::smoothPoint(const Vector3D &point) const
{
    if (!providedPlaneNormals)
    { return point; }

    const auto bary{ barycentrics(point) };
    const auto shiftedBary{ bary - Vector3D{ 0.5f, 0.5f, 0.5f } };
    const auto radiusWeights{ Vector3D{ 1.0f, 1.0f, 1.0f } - 4.0f * (shiftedBary * shiftedBary) };

    return point + centralPlaneNormal() * radiusWeights * Vector3D{ pr12, pr13, pr23 };
}

float Triangle::curvature() const
{
    // TODO - Better curvature metric?
    return (Vector3D::dotProduct(pn1, pn2) + Vector3D::dotProduct(pn2, pn3) + Vector3D::dotProduct(pn3, pn1)) / 3.0f;
}

Vector3D Triangle::curvatureWeights() const
{
    const auto cpn{ centralPlaneNormal() };

    // Calculate weights as offset from central plane normal.
    const auto w1{ std::abs(Vector3D::dotProduct(cpn, pn1)) };
    const auto w2{ std::abs(Vector3D::dotProduct(cpn, pn2)) };
    const auto w3{ std::abs(Vector3D::dotProduct(cpn, pn3)) };

    // Fallback to uniform if we don't have valid values.
    const auto weightSum{ w1 + w2 + w3};
    if (weightSum <= std::numeric_limits<float>::epsilon() || !std::isfinite(weightSum))
    { return { 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 3.0f }; }

    // Normalize weights to be in <0.0f, 1.0f> and sum to 1.0f .
    const auto normalizer{ 1.0f / (w1 + w2 + w3) };
    return { w1 * normalizer, w2 * normalizer, w3 * normalizer };
}

float Triangle::longestSide() const
{
    return std::max<float>(
        std::max<float>(
            v1.length(),
            v2.length()),
        v3.length()
    );
}

Vector3D lsIntersect(const std::vector<Line> &lines)
{
    // Initialize accumulation variables:

    // Coefficients of the left sides.
    Eigen::Matrix3f A{ Eigen::Matrix3f::Zero() };
    // Coefficients of the right sides.
    Eigen::Vector3f b{ Eigen::Vector3f::Zero() };
    // Constants.
    float c{ 0.0f };

    //std::cout << "Starting lsIntersect..." << std::endl;

    // Populate the coefficient matrices:
    for (const auto &line : lines)
    { // Process each line in turn.
        // Recover points on the line.
        const Eigen::Vector3f v{ line.v.x, line.v.y, line.v.z };
        const Eigen::Vector3f w{ line.w.x, line.w.y, line.w.z };

        //std::cout << "Adding line: " << v.transpose() << " -> " << w.transpose() << std::endl;

        // Calculate line direction.
        const Eigen::Vector3f u{ (v - w).normalized() };
        const Eigen::Vector3f ut{ u.transpose() };

        // Calculate projection to hyper-plane orthogonal to this line.
        Eigen::Matrix3f kroneckerProduct{ };
        kroneckerProduct <<
                         u(0) * ut(0), u(0) * ut(1), u(0) * ut(2),
            u(1) * ut(0), u(1) * ut(1), u(1) * ut(2),
            u(2) * ut(0), u(2) * ut(1), u(2) * ut(2);
        const Eigen::Matrix3f projection{ Eigen::Matrix3f::Identity() - kroneckerProduct };

        // Calculate distance of our line to the projection plane.
        const Eigen::Vector3f p{ projection * w };

        // Populate coefficients of the least-squares system of equations.
        A += projection;
        b += p;
        c += p(0) * p(0) + p(1) * p(1) + p(2) * p(2);
    }

    // Solve least-squares problem.
    //const Eigen::Vector3f result{ A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b) };
    const Eigen::Vector3f result{ A.bdcSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b) };

    //std::cout << "LS intersection result: " << result.transpose() << std::endl;

    return { result(0), result(1), result(2) };
}

Vector3D lsIntersect(std::initializer_list<Line> lines)
{ return lsIntersect({ lines.begin(), lines.end() }); }

} // namespace treeacc