#undef BOOST_UBLAS_NO_EXCEPTIONS
#include "common/testhelper.hpp"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <string>
#include <sstream>
#include <complex>
#include <iomanip>
#include "utils.hpp"

#ifdef BOOST_UBLAS_CPP_GE_2011

using namespace boost::numeric::ublas;

using std::cout;
using std::endl;

template < class T >
bool test_vector( std::string type_name)
{
    std::stringstream stream;
    stream << "Testing for: " << type_name;
    BOOST_UBLAS_DEBUG_TRACE( stream.str() );

    bool pass = true;

    {
        typedef fixed_vector<T, 1> vec1;

        vec1 v1( 122.0 );

        pass &= ( v1(0) == (T)122 );

    }

    {
        typedef fixed_vector<T, 3> vec3;

        vec3 v1((T)0.0, (T)0.0, (T)0.0);

        pass &=(sizeof( vec3 )  == v1.size()*sizeof( T ) ) ;

        vector<T> v( 3, 0 ) ;

        pass &= compare( v1, v );

        v1 <<= 10.0, 10, 33;
        v  <<= 10.0, 10, 33;

        //cout << std::setprecision(20) << v1 << '\n' << v;

        pass &= compare( v1, v );

        vec3 v2;

        v2( 0 ) = 10.0; v2( 1 ) = 10; v2( 2 ) = 33;
        pass &= compare( v, v2 );

        v2 += v;

        pass &= compare( v2, 2*v );

        v1 = 2*v1 + v - 6*v2;
        pass &= compare( v1, (3-2*6)*v );

        vec3 v3{ (T)-90.0, (T)-90.0, (T)-297.0 };
        pass &= compare( v3, v1 );

        vec3 v4 =  { (T)-90.0, (T)-90.0, (T)-297.0 };
        pass &= compare( v4, v1 );

        vec3 v5( (T)-90.0, (T)-90.0, (T)-297.0 );
        pass &= compare( v5, v1 );

        vec3 v6((T) 5.0, (T)8.0, (T)9.0);

        matrix<T> M = outer_prod( v6, v6), L( 3, 3);

        L <<= 25, 40, 45, 40, 64, 72, 45, 72, 81;

        pass &= compare( M, L );

        L  <<= 1, 2, 3, 4, 5, 6, 7, 8, 9;
        v6 <<= 4, 5, 6;
        vec3 v7 ( (T)32.0, (T)77.0, (T)122.0 );

        pass &= compare( v7, prod(L, v6) );

        vec3 v8;
        noalias( v8 ) = prod(L, v6);

        pass &= compare( v7, v8 );

    }

    {
        const std::size_t N = 33;
        typedef fixed_vector<T, N> vec33;

        vec33 v1;
        vector<T> v( N );

        for ( std::size_t i = 0; i!= v1.size(); i++)
        {
            v1( i ) = 3.14159*i;
            v ( i ) = 3.14159*i;
        }

        pass &= compare( v1, v );

        auto ip = inner_prod( v, v);
        auto ip1 = inner_prod( v1, v1);

        pass &= (  ip == ip1 ) ;

        T c = 0;
        for (auto i = v1.begin(); i != v1.end(); i++)
        {
            *i = c;
            c = c + 1;
        }

        c = 0;
        for (auto i = v.begin(); i != v.end(); i++)
        {
            *i = c;
            c = c + 1;
        }

        pass &= compare( v1, v );

        // Check if bad index indeed works
        try {
            T a;
            a=v1( 100 );
            BOOST_UBLAS_NOT_USED( a );

        } catch ( bad_index &e) {
            std::cout << " Caught (GOOD): " << e.what() << endl;
            pass &= true;
        }

    }
    return pass;
}

template < class T >
bool test_matrix( std::string type_name)
{
    std::stringstream stream;
    stream << "Testing for: " << type_name;
    BOOST_UBLAS_DEBUG_TRACE( stream.str() );

    bool pass = true;

    typedef fixed_matrix<T, 3, 4> mat34;
    typedef fixed_matrix<T, 4, 3> mat43;
    typedef fixed_matrix<T, 3, 3> mat33;

    {
        typedef fixed_matrix<T, 1, 1> mat1;

        mat1 m1( 122.0 );

        pass &= ( m1(0, 0) == (T)122 );
    }

    {
        mat34 m1( 3.0 );

        pass &=(sizeof( mat34 )  == m1.size1()*m1.size2()*sizeof( T ) ) ;

        matrix<T> m( 3.0, 4.0, 3.0 ) ;

        pass &= compare( m1, m );

        cout << m1 << endl;
        cout << m << endl;

        m1 <<= 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;
        m  <<= 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;

        pass &= compare( m1, m );

        cout << m1 << endl;
        cout << m << endl;

        mat34 m2( 0.0 );

        T count = 1 ;
        for ( std::size_t i = 0; i != m2.size1(); i++)
        {
            for (std::size_t j = 0; j!= m2.size2(); j++)
            {
                m2( i, j ) = count;
                count = count + 1;
            }

        }
        pass &= compare( m2, m );
        cout << m2 << endl;

    }
    {
        mat34 m1 = { (T)1, (T)2, (T)3, (T)3, (T)3, (T)2, (T)5, (T)4, (T)2, (T)6, (T)5, (T)2 };
        mat43 m2 = { (T)4, (T)5, (T)6, (T)3, (T)2, (T)2, (T)1, (T)4, (T)2, (T)6, (T)5, (T)2 };

        mat33 m3 = prod(m1, m2);

        matrix<T> m(3, 3);
        m <<= 31,36,22,47,59,40,43,52,38;

        pass &= compare(m ,m3);

        mat33 m4;
        m4 <<= (T)1, (T)2, (T)1, (T)2, (T)1, (T)3, (T)1, (T)2, (T) 5;
        m3  = prod(m4, trans(m4));

        m<<=6,7,10,7,14,19,10,19,30;

        cout << m3 << endl;
        pass &= compare(m ,m3);

        m3 = 2 * m4 - 1 * m3;

        cout << m3;

        m <<= -4,-3,-8,-3,-12,-13,-8,-15,-20;

        pass &= compare(m, m3);

        m = m3;

        m3 = trans(m);

        pass &= compare(m3, trans(m));

        // Check if bad index indeed works
        try {
            T a;
            a=m1( 100, 100 );
            BOOST_UBLAS_NOT_USED( a );

        } catch ( bad_index &e) {
            std::cout << " Caught (GOOD): " << e.what() << endl;
            pass &= true;
        }

    }

    return pass;

}

BOOST_UBLAS_TEST_DEF (test_fixed) {

    BOOST_UBLAS_DEBUG_TRACE( "Starting fixed container tests" );

    BOOST_UBLAS_TEST_CHECK(  test_vector< double >( "double") );
    BOOST_UBLAS_TEST_CHECK(  test_vector< float >( "float") );
    BOOST_UBLAS_TEST_CHECK(  test_vector< int >( "int") );

    BOOST_UBLAS_TEST_CHECK(  test_vector< std::complex<double> >( "std::complex<double>") );
    BOOST_UBLAS_TEST_CHECK(  test_vector< std::complex<float> >( "std::complex<float>") );
    BOOST_UBLAS_TEST_CHECK(  test_vector< std::complex<int> >( "std::complex<int>") );

    BOOST_UBLAS_TEST_CHECK(  test_matrix< double >( "double") );
    BOOST_UBLAS_TEST_CHECK(  test_matrix< float >( "float") );
    BOOST_UBLAS_TEST_CHECK(  test_matrix< int >( "int") );

    BOOST_UBLAS_TEST_CHECK(  test_matrix< std::complex<double> >( "std::complex<double>") );
    BOOST_UBLAS_TEST_CHECK(  test_matrix< std::complex<float> >( "std::complex<float>") );
    BOOST_UBLAS_TEST_CHECK(  test_matrix< std::complex<int> >( "std::complex<int>") );
}

int main () {

    BOOST_UBLAS_TEST_BEGIN();

    BOOST_UBLAS_TEST_DO( test_fixed );

    BOOST_UBLAS_TEST_END();
    return EXIT_SUCCESS;

}

#else

int main () {

    BOOST_UBLAS_TEST_BEGIN();
    BOOST_UBLAS_TEST_END();

    return EXIT_SUCCESS;

}
#endif // BOOST_UBLAS_CPP_GE_2011


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/matrix/comparison.hpp>
#include <fcppt/math/matrix/output.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

namespace
{

typedef
fcppt::math::matrix::static_<
	int,
	2,
	2
>
matrix_type;

}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_operators_add
)
{
FCPPT_PP_POP_WARNING

	matrix_type const first(
		fcppt::math::matrix::row(
			1, 2
		),
		fcppt::math::matrix::row(
			3, 4
		)
	);

	matrix_type second(
		fcppt::math::matrix::row(
			2, 3
		),
		fcppt::math::matrix::row(
			4, 5
		)
	);

	second += first;

	BOOST_CHECK_EQUAL(
		second,
		matrix_type(
			fcppt::math::matrix::row(
				3, 5
			),
			fcppt::math::matrix::row(
				7, 9
			)
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_operators_scalar
)
{
FCPPT_PP_POP_WARNING

	matrix_type first(
		fcppt::math::matrix::row(
			1, 2
		),
		fcppt::math::matrix::row(
			3, 4
		)
	);

	first *= 3;

	BOOST_CHECK_EQUAL(
		first,
		matrix_type(
			fcppt::math::matrix::row(
				3, 6
			),
			fcppt::math::matrix::row(
				9, 12
			)
		)
	);
}


#pragma once

#include "gtest/gtest.h"

#include "../util/Maybe.hh"
#include "../geometry/LineSegment/linesegment.hh"

#include <Eigen/Core>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

inline ::testing::AssertionResult VectorsEqual(Eigen::Vector3d const& expected, Eigen::Vector3d const& actual, const float delta = 0.00001) {
  double d = (expected-actual).norm();
  if (d < delta)
    return ::testing::AssertionSuccess();
  else
    return ::testing::AssertionFailure() << "Actual: " << actual.transpose() << ", expected: " << expected.transpose() << " d = " << d;
}

inline ::testing::AssertionResult VectorsEqual(Eigen::Vector2d const& expected, Eigen::Vector2d const& actual, const double delta = 0.00001) {
  double d = (expected-actual).norm();
  if (d < delta)
    return ::testing::AssertionSuccess();
  else
    return ::testing::AssertionFailure() << "Actual: " << actual.transpose() << ", expected: " << expected.transpose() << " d = " << d;
}

template<typename T, int N>
inline ::testing::AssertionResult VectorsEqual(Eigen::Matrix<T,N,1> const& expected, Eigen::Matrix<T,N,1> const& actual, const double delta = 0.00001) {
  double d = (expected-actual).norm();
  if (d < delta)
    return ::testing::AssertionSuccess();
  else
    return ::testing::AssertionFailure() << "Actual: " << actual.transpose() << ", expected: " << expected.transpose() << " d = " << d;
}

template<int N>
inline ::testing::AssertionResult VectorsEqual(Eigen::Matrix<int,N,1> const& expected, Eigen::Matrix<int,N,1> const& actual) {
  double d = (expected-actual).norm();
  if (d == 0)
    return ::testing::AssertionSuccess();
  else
    return ::testing::AssertionFailure() << "Actual: " << actual.transpose() << ", expected: " << expected.transpose() << " d = " << d;
}

inline ::testing::AssertionResult MatricesEqual(Eigen::MatrixXd const& expected, Eigen::MatrixXd const& actual, double delta = 0.000001) {
  double d = (expected-actual).array().abs().sum();
  if (d < delta)
    return ::testing::AssertionSuccess();
  else
    return ::testing::AssertionFailure() << "Actual: " << std::endl << actual << std::endl << "Expected: " << std::endl << expected << std::endl << " d = " << d;
}

template<typename T, int dim>
inline ::testing::AssertionResult LinesEqual(bold::LineSegment<T,dim> const& expected, bold::LineSegment<T,dim> const& actual, const double delta = 0.000001) {
  double d1 = (expected.p1()-actual.p1()).norm();
  double d2 = (expected.p2()-actual.p2()).norm();
  if (d1 <= delta && d2 <= delta)
    return ::testing::AssertionSuccess();
  else
    return ::testing::AssertionFailure() << "Actual: " << actual << ", expected: " << expected << " d1=" << d1 << " d2=" << d2;
}

//PrintTo(const T&, ostream*)

inline std::ostream& operator<<(std::ostream& stream, Eigen::Vector2i const& v)
{
  return stream << "(" << v.x() << ", " << v.y() << ")";
}

inline std::ostream& operator<<(std::ostream& stream, Eigen::Vector2f const& v)
{
  return stream << "(" << v.x() << ", " << v.y() << ")";
}

inline std::ostream& operator<<(std::ostream& stream, Eigen::Vector2d const& v)
{
  return stream << "(" << v.x() << ", " << v.y() << ")";
}

inline std::ostream& operator<<(std::ostream& stream, Eigen::Vector3d const& v)
{
  return stream << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")";
}

inline bool operator==(Eigen::Vector2i const& expected, Eigen::Vector2i const& actual)
{
  return expected.x() == actual.x() && expected.y() == actual.y();
}

#define ASSERT_EMPTY(condition) \
  GTEST_TEST_BOOLEAN_(!(condition.hasValue()), #condition, Non-empty, Empty, \
                      GTEST_FATAL_FAILURE_) << *condition

#define EXPECT_EMPTY(condition) \
  GTEST_TEST_BOOLEAN_(!(condition.hasValue()), #condition, Non-empty, Empty, \
                      GTEST_NONFATAL_FAILURE_) << *condition

#define EXPECT_BETWEEN(lower, upper, val) \
  do { \
  EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperGE, val, lower); \
  EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperLE, val, upper); \
  } while (0)


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

#include "Geomlib_PolylineIntersection.h"
#include "Geomlib_BestFitPlane.h"
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include "Polyline.h"

using Urho3D::Vector;
using Urho3D::Vector3;
using Urho3D::Vector2;

using namespace Urho3D;

// [t1, t2] interval of real number line; no promise that t1 <= t2
// [s1, s2] interval of real number line; no promise that s1 <= s2
// Returns:
//   1 if [t1, t2] and [s1, s2] have non-empty intersection
//   0 otherwise
int Geomlib::IntervalIntervalIntersection(
	double t1,
	double t2,
	double s1,
	double s2)
{
	if (t1 > t2) {
		double x = t1;
		t1 = t2;
		t2 = x;
	}
	if (s1 > s2) {
		double y = s1;
		s1 = s2;
		s2 = y;
	}

	if (s1 <= t1 && t1 <= s2) {
		return 1;
	}
	else if (s1 <= t2 && t2 <= s2) {
		return 1;
	}
	else if (t1 <= s1 && s1 <= t2) {
		return 1;
	}

	return 0;
}

//   1 if (A,B) and (C,D) have non-empty intersection
//   0 otherwise
int Geomlib::SegmentSegmentIntersection2D(
	Vector2& A,
	Vector2& B,
	Vector2& C,
	Vector2& D)
{
	// left & right half plane checks
	if (
		(A.x_ > 0 && B.x_ > 0) && // A,B in right half plane
		(C.x_ < 0 && D.x_ < 0)    // C,D in left half plane
		) {
		return 0;
	}
	if (
		(A.x_ < 0 && B.y_ < 0) && // A,B in left half plane
		(C.x_ > 0 && D.y_ > 0)    // C,D in right half plane
		) {
		return 0;
	}
	// upper & lower half plane checks
	if (
		(A.y_ > 0 && B.y_ > 0) && // A,B in upper half plane
		(C.y_ < 0 && D.y_ < 0)    // C,D in lower half plane
		) {
		return 0;
	}
	if (
		(A.y_ < 0 && B.y_ < 0) && // A,B in lower half plane
		(C.y_ > 0 && D.y_ > 0)    // C,D in upper half plane
		) {
		return 0;
	}

	// do C and D lie strictly on the same side of line through AB?
	double cval = (B.y_ - A.y_) * (C.x_ - A.x_) + (A.x_ - B.x_) * (C.y_ - A.y_);
	double dval = (B.y_ - A.y_) * (D.x_ - A.x_) + (A.x_ - B.x_) * (D.y_ - A.y_);
	if ((cval > 0 && dval > 0) || (cval < 0 && dval < 0)) {
		return 0;
	}
	// do A and B lie strictly on the same side of line through CD?
	double aval = (D.y_ - C.y_) * (A.x_ - C.x_) + (C.x_ - D.x_) * (A.y_ - C.y_);
	double bval = (D.y_ - C.y_) * (B.x_ - C.x_) + (C.x_ - D.x_) * (B.y_ - C.y_);
	if ((aval > 0 && bval > 0) || (aval < 0 && bval < 0)) {
		return 0;
	}

	// lines through AB and CD definitely intersect
	double k11 = B.x_ - A.x_;
	double k12 = C.x_ - D.x_;
	double k21 = B.y_ - A.y_;
	double k22 = C.y_ - D.y_;
	double z1 = C.x_ - A.x_;
	double z2 = C.y_ - A.y_;
	double prod1 = k11 * k22;
	double prod2 = k12 * k21;
	double prod3 = k22 * z1;
	double prod4 = k12 * z2;
    if (std::abs(prod1) > 1e16 || std::abs(prod2) > 1e16 || std::abs(prod3) > 1e16 || std::abs(prod4) > 1e16) {
		double shrink = 1e-6;
		Vector2 newA = shrink * A;
		Vector2 newB = shrink * B;
		Vector2 newC = shrink * C;
		Vector2 newD = shrink * D;
		return Geomlib::SegmentSegmentIntersection2D(newA, newB, newC, newD);
	}
	double denom = prod1 - prod2;
	double numer = prod3 - prod4;
	if (prod1 == prod2) {
		denom = 0;
	}

	const double eps = 1e-8;

	if (denom > eps || -denom > eps) {
		// there is unique point of intersection: does it lie in segment AB?
		double t = (k22 * z1 - k12 * z2) / denom;
		return 0 <= t && t <= 1;
	}
	else {
		// lines through AB and CD coincide
		double rise = B.y_ - A.y_;
		double run = B.x_ - A.x_;
		// normal is (-rise, run)
		if (rise >= run) { // x-coord of normal is bigger, project segments to y-axis
			return Geomlib::IntervalIntervalIntersection(A.y_, B.y_, C.y_, D.y_);
		}
		else { // y-coord of normal is bigger, project segments to x-axis
			return Geomlib::IntervalIntervalIntersection(A.x_, B.x_, C.x_, D.x_);
		}
	}
}

	int Geomlib::SegmentSegmentIntersection3D(
		Vector3& A,
		Vector3& B,
		Vector3& C,
		Vector3& D,
		Vector3& normal)
	{

		// converting to regular vector to use max_element
		std::vector<double> Nreg;
		Nreg.push_back(std::abs(normal.x_));
		Nreg.push_back(std::abs(normal.y_));
		Nreg.push_back(std::abs(normal.z_));

		// find greatest coordinate from plane normal
		// projection plane will be the plane generate by the OTHER coordinate axes
		// e.g. if N.y is largest, project to xz-plane
		int maxCoord = std::max_element(Nreg.begin(), Nreg.end()) - Nreg.begin();

		Vector<Vector3> startVerts;
		startVerts.Push(A);
		startVerts.Push(B);
		startVerts.Push(C);
		startVerts.Push(D);

		// project coords to the appropriate plane
		// NOTE SYNTAX!!!!!!!!!!!
		std::vector<Eigen::RowVector2d, Eigen::aligned_allocator<Eigen::RowVector2d>> flatVerts;
		//Vector<Vector2> flatVerts;
		for (int i = 0; i < startVerts.Size(); ++i)
		{
			int counter = 0;
			Eigen::RowVector2d tmpCoords;
			//Vector2 tmpCoords;
			for (int j = 0; j < 3; ++j)
			{
				if (j != maxCoord)
				{
					//std::cout << "counter=" << counter << " j=" << j << std::endl;
					//std::cout << "startVerts[" << i << "](" << j << ")=" << startVerts[i](j) << std::endl;
					if (j == 0)
						tmpCoords(counter) = startVerts[i].x_;
					else if (j == 1)
						tmpCoords(counter) = startVerts[i].y_;
					else if (j == 2)
						tmpCoords(counter) = startVerts[i].z_;
					++counter;
				}
			}
			//std::cout << "tmpCoords=" << tmpCoords << std::endl;
			flatVerts.push_back(tmpCoords);
		}

		Vector2 newA(flatVerts[0](0), flatVerts[0](1));
		Vector2 newB(flatVerts[1](0), flatVerts[1](1));
		Vector2 newC(flatVerts[2](0), flatVerts[2](1));
		Vector2 newD(flatVerts[3](0), flatVerts[3](1));

		int ret = Geomlib::SegmentSegmentIntersection2D(
			newA,
			newB,
			newC,
			newD);

		return ret;

	}

	bool Geomlib::HasSelfIntersection(const Urho3D::Variant polyline)
	{
		VariantVector verts = Polyline_ComputeSequentialVertexList(polyline);

		// This will only work for planar polylines!
		Vector3 point, normal;
		Geomlib::BestFitPlane(Polyline_ComputePointCloud(polyline), point, normal);

		bool closed = Polyline_IsClosed(polyline);
		int counter = verts.Size()-4;
		if (counter < 4)
			return false;

		for (int i = 0; i < counter; ++i) {
			Vector3 A = verts[i].GetVector3();
			Vector3 B = verts[i + 1].GetVector3();
			for (int j = i + 2; j < counter+2; ++j) {
				Vector3 C = verts[j].GetVector3();
				Vector3 D = verts[j + 1].GetVector3();
				int ret = Geomlib::SegmentSegmentIntersection3D(A, B, C, D, normal);
				if (ret == 1)
					return true;
			}
		}
		return false;
	}

	int Geomlib::GetSelfIntersections(const Urho3D::Variant polyline, Urho3D::Vector<Urho3D::Vector3>& intersections, Urho3D::Variant& revised_polyline)
	{
		VariantVector verts = Polyline_ComputeSequentialVertexList(polyline);
		Vector<Vector3> revised_verts;
		for (int i = 0; i < verts.Size(); ++i)
			revised_verts.Push(verts[i].GetVector3());

		// This will only work for planar polylines!
		Vector3 point, normal;
		Geomlib::BestFitPlane(Polyline_ComputePointCloud(polyline), point, normal);

		bool closed = Polyline_IsClosed(polyline);
		int counter = verts.Size() - 4;
		if (counter < 4)
			return 0;

		for (int i = 0; i < counter; ++i) {
			Vector3 A = verts[i].GetVector3();
			Vector3 B = verts[i + 1].GetVector3();
			for (int j = i + 2; j < counter + 2; ++j) {
				Vector3 C = verts[j].GetVector3();
				Vector3 D = verts[j + 1].GetVector3();
				int ret = Geomlib::SegmentSegmentIntersection3D(A, B, C, D, normal);
				if (ret == 1) {
					Vector3 point;
					int success = GetPointOfIntersection(A, B, C, D, point);
					if (success) {
						intersections.Push(point);
						InsertSelfIntersectionsAsPoint(A, B, C, D, point, revised_verts);
					}
				}
			}
		}
		revised_polyline = Geomlib::Polyline_Make_With_Intersections(revised_verts);
		return 1;
	}

	// inserts the point into the vertex list between A, B and C, D
	int Geomlib::InsertSelfIntersectionsAsPoint(
		const Urho3D::Vector3& A,
		const Urho3D::Vector3& B,
		const Urho3D::Vector3& C,
		const Urho3D::Vector3& D,
		const Urho3D::Vector3& point,
		Urho3D::Vector<Urho3D::Vector3>& verts)
	{
		// Assuming this inserts before the destination element?
		Vector<Vector3>::Iterator it_D= verts.Find(D);
		verts.Insert(it_D, point);

		Vector<Vector3>::Iterator it_B = verts.Find(B);
		verts.Insert(it_B, point);

		return 0;
	}

	int Geomlib::GetPointOfIntersection( const Urho3D::Vector3 & A, const Urho3D::Vector3 & B, const Urho3D::Vector3 & C, const Urho3D::Vector3 & D, Urho3D::Vector3& point)
	{
		double ta, tb;
		bool success = Geomlib::ShortestDistance(A, B, C, D, ta, tb);
		if (ta < 0 || ta > 1 || tb < 0 || tb >1)
			return 0;

		// find the points on the line segments
		Vector3 pa = A + ta*(B - A);
		Vector3 pb = C + tb*(D - C);

		// if the distance between the points is small, this is a true intersection
		double dist = (pb - pa).Length();
		if (dist < 0.0001) {
			point = pa;
			return 1;
		}

		return 0;
	}

	/// returns parameters on each line. If they are between 0-1, the closest point lies on the line. Otherwise,
	/// the point is on the ray defined by the line direction.
	/// if return is false, the closest point could not be calculated (probably due to the lines being parallel)
	bool Geomlib::ShortestDistance(
		const Vector3& A,
		const Vector3& B,
		const Vector3& C,
		const Vector3& D,
		double& ta, double& tb)
	{

		Vector3 u = B - A;
		Vector3 v = D - C;
		Vector3 w = A - C;

		double dot = u.Normalized().DotProduct(v.Normalized());

        if (std::abs(dot - 1.0) < 10e-7)
			return false;

		double uu = u.DotProduct(u);
		double uv = u.DotProduct(v);
		double vv = v.DotProduct(v);
		double uw = u.DotProduct(w);
		double vw = v.DotProduct(w);

		double t = 1.0 / (uu * vv - uv * uv);
		ta = (uv * vw - vv * uw) * t;
		tb = (uu * vw - uv * uw) * t;

		return true;

	}

	Urho3D::Variant Geomlib::Polyline_Make_With_Intersections(const Urho3D::Vector<Urho3D::Vector3>& vertexList)
	{
		Variant earlyRet;
		if (vertexList.Size() < 2) {
			return earlyRet;
		}

		Vector<Variant> vertices;
		Vector<Variant> edges;

		for (int i = 0; i < vertexList.Size(); ++i) {
			vertices.Push(Variant(vertexList[i]));
			edges.Push(Variant(i));
		}
		VariantMap var_map;
		var_map["type"] = Variant(String("Polyline"));
		var_map["vertices"] = Variant(vertices);
		var_map["edges"] = Variant(edges);

		return Variant(var_map);
	}


/*=============================================================================
    Copyright (c) 2002-2003 Hartmut Kaiser
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  Full calculator example
//  [ demonstrating phoenix and subrules ]
//
//  [ Hartmut Kaiser 10/8/2002 ]
//
///////////////////////////////////////////////////////////////////////////////

//#define BOOST_SPIRIT_DEBUG        // define this for debug output

#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_attribute.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;
using namespace phoenix;

///////////////////////////////////////////////////////////////////////////////
//
//  Our calculator grammar using phoenix to do the semantics and subrule's
//  as it's working horses
//
//  Note:   The top rule propagates the expression result (value) upwards
//          to the calculator grammar self.val closure member which is
//          then visible outside the grammar (i.e. since self.val is the
//          member1 of the closure, it becomes the attribute passed by
//          the calculator to an attached semantic action. See the
//          driver code that uses the calculator below).
//
///////////////////////////////////////////////////////////////////////////////
struct calc_closure : BOOST_SPIRIT_CLASSIC_NS::closure<calc_closure, double>
{
    member1 val;
};

struct calculator : public grammar<calculator, calc_closure::context_t>
{
    template <typename ScannerT>
    struct definition
    {
        definition(calculator const& self)
        {
            top = (
                expression =
                    term[self.val = arg1]
                    >> *(   ('+' >> term[self.val += arg1])
                        |   ('-' >> term[self.val -= arg1])
                        )
                ,

                term =
                    factor[term.val = arg1]
                    >> *(   ('*' >> factor[term.val *= arg1])
                        |   ('/' >> factor[term.val /= arg1])
                        )
                ,

                factor
                    =    ureal_p[factor.val = arg1]
                    |   '(' >> expression[factor.val = arg1] >> ')'
                    |   ('-' >> factor[factor.val = -arg1])
                    |   ('+' >> factor[factor.val = arg1])
            );

            BOOST_SPIRIT_DEBUG_NODE(top);
            BOOST_SPIRIT_DEBUG_NODE(expression);
            BOOST_SPIRIT_DEBUG_NODE(term);
            BOOST_SPIRIT_DEBUG_NODE(factor);
        }

        subrule<0, calc_closure::context_t>  expression;
        subrule<1, calc_closure::context_t>  term;
        subrule<2, calc_closure::context_t>  factor;

        rule<ScannerT> top;

        rule<ScannerT> const&
        start() const { return top; }
    };
};

///////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tExpression parser using Phoenix...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type an expression...or [q or Q] to quit\n\n";

    calculator calc;    //  Our parser

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        double n = 0;
        parse_info<> info = parse(str.c_str(), calc[var(n) = arg1], space_p);

        //  calc[var(n) = arg1] invokes the calculator and extracts
        //  the result of the computation. See calculator grammar
        //  note above.

        if (info.full)
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << "result = " << n << endl;
            cout << "-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "stopped at: \": " << info.stop << "\"\n";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


/*
matrix/MatrixRxC.hpp
-----------------------
Copyright (c) 2014, theJ89

Description:
    Adds a generic matrix class, Matrix.
    You can specify what type of data the matrix uses (int, float, etc) as well as the number of rows and columns in the matrix.
    The code in this file handles rectangular matrices.
    Common specializations of Matrix are provided in Matrix2x2.hpp, Matrix3x3.hpp, Matrix4x4.hpp, and MatrixNxN.hpp.
*/
#ifndef BS_MATRIX_MATRIXRXC_HPP
#define BS_MATRIX_MATRIXRXC_HPP

//Includes
#include <iostream>                     //std::ostream
#include <iomanip>                      //std::setw, std::setprecision

#include <boost/format.hpp>             //boost::format

#include <brimstone/util/Array.hpp>     //BS_ARRAY_DECLARE_METHODS, BS_ARRAY_DEFINE_METHODS, etc
#include <brimstone/Vector.hpp>         //Vector
#include <brimstone/util/MinMax.hpp>    //electMax

//Macros
#define BS_MATRIX_DECLARE_METHODS( R, C )                                   \
    Matrix();                                                               \
                                                                            \
    template< typename T2 >                                                 \
    Matrix( const Matrix< T2, R, C >& toCopy );                             \
                                                                            \
    bool isSquare() const;                                                  \
    size_t getRows() const;                                                 \
    size_t getColumns() const;                                              \
                                                                            \
    void zero();                                                            \
    bool isZero() const;                                                    \
    void identity();                                                        \
    bool isIdentity() const;                                                \
                                                                            \
    void setRow( const size_t row, const Vector< T, C >& values );          \
    void setColumn( const size_t col, const Vector< T, R >& values );       \
                                                                            \
    Vector< T, C > getRow( const size_t row ) const;                        \
    Vector< T, R > getColumn( const size_t col ) const;                     \
                                                                            \
    template< typename T2 >                                                 \
    Matrix& operator =( const Matrix< T2, R, C >& right );                  \
    Matrix& operator +=( const Matrix& right );                             \
    Matrix& operator -=( const Matrix& right );                             \
    Matrix& operator *=( const Matrix< T, C, C >& right );                  \
    Matrix& operator +=( const T right );                                   \
    Matrix& operator -=( const T right );                                   \
    Matrix& operator *=( const T right );                                   \
    Matrix& operator /=( const T right );                                   \
    T&      operator ()( const size_t row, const size_t col );              \
    T       operator ()( const size_t row, const size_t col ) const;

#define BS_MATRIX_DEFINE_METHODS( R, C, tmpl, spec )                        \
    tmpl                                                                    \
    size_t Matrix spec::getRows() const {                                   \
        return R;                                                           \
    }                                                                       \
    tmpl                                                                    \
    size_t Matrix spec::getColumns() const {                                \
        return C;                                                           \
    }                                                                       \
    tmpl                                                                    \
    void Matrix spec::identity() {                                          \
        (*this) = m_identity;                                               \
    }                                                                       \
    tmpl                                                                    \
    bool Matrix spec::isIdentity() const {                                  \
        return (*this) == m_identity;                                       \
    }                                                                       \
    tmpl                                                                    \
    T& Matrix spec::operator()( const size_t row, const size_t col ) {      \
        BS_ASSERT_INDEX( row, R - 1 );                                      \
        BS_ASSERT_INDEX( col, C - 1 );                                      \
        return elem[row][col];                                              \
    }                                                                       \
    tmpl                                                                    \
    T Matrix spec::operator()( const size_t row, const size_t col ) const { \
        BS_ASSERT_INDEX( row, R - 1 );                                      \
        BS_ASSERT_INDEX( col, C - 1 );                                      \
        return elem[row][col];                                              \
    }

namespace Brimstone {

template< typename T, size_t R, size_t C >
class Matrix {
public:
//C4201: nonstandard extension used : nameless struct/union
//It's a non-standard feature, but VC++, G++, and LLVM support it so it shouldn't be too much of an issue
#pragma warning( push )
#pragma warning( disable: 4201 )

    union {
        T              data[ R * C ];
        T              elem[R][C];
        struct {
            Vector< T, C > row[ R ];
        };
    };

#pragma warning( pop )
public:
    BS_ARRAY_DECLARE_INHERITED_METHODS( Matrix, T )
    BS_ARRAY_DECLARE_METHODS( Matrix, T )
    BS_MATRIX_DECLARE_METHODS( R, C )
private:
    struct ConstructIdentity {};
    Matrix( const ConstructIdentity );
    static const Matrix m_identity;
};
BS_ARRAY_DEFINE_GENERIC_METHODS( Matrix, T, data, BS_TMPL_3( typename T, size_t R, size_t C ), BS_SPEC_3( T, R, C ) )
BS_ARRAY_DEFINE_METHODS( Matrix, T, data, BS_TMPL_3( typename T, size_t R, size_t C ), BS_SPEC_3( T, R, C ) )
BS_MATRIX_DEFINE_METHODS( R, C, BS_TMPL_3( typename T, size_t R, size_t C ), BS_SPEC_3( T, R, C ) )

//Forward declarations
template< typename T, size_t R, size_t C >
Matrix< T, C, R > transpose( const Matrix< T, R, C >& matrix );

//Note: We have a private constructor specifically for constructing
//m_identity. To differentiate it from other constructors,
//we pass an unused argument of type "ConstructIdentity",
//ConstructIdentity is a private inner class in Matrix, which means that only something
//belonging to Matrix can instantiate ConstructIdentity objects. Normally this
//means "a method in Matrix", but this also works with statements that initialize
//static members of Matrix as well.
template< typename T, size_t R, size_t C >
const Matrix< T, R, C > Matrix< T, R, C >::m_identity = Matrix< T, R, C >( ConstructIdentity() );

//We could just put this in the default constructor, but the advantage of this approach
//is that by copying a pre-built matrix, we can avoid using two loops and running a conditional
//on each element of every matrix that is default-constructed, and instead only do this once
//when constructing the identity matrix.
template< typename T, size_t R, size_t C >
Matrix< T, R, C >::Matrix( const ConstructIdentity ) {
    for( size_t r = 0; r < R; ++r )
        for( size_t c = 0; c < C; ++c )
            elem[r][c] = ( r == c ? static_cast< T >( 1 ) : static_cast< T >( 0 ) );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >::Matrix()
#ifdef BS_ZERO
    : Matrix( m_identity )
#endif //BS_ZERO
{
}

template< typename T, size_t R, size_t C >
template< typename T2 >
Matrix< T, R, C >::Matrix( const Matrix< T2, R, C >& toCopy ) {
    (*this) = toCopy;
}

template< typename T, size_t R, size_t C >
bool Matrix< T, R, C >::isSquare() const {
    //This function always returns false because the generic case of Matrix handles rectangular matrices.
    return false;
}

template< typename T, size_t R, size_t C >
void Matrix< T, R, C >::zero() {
    T zero = static_cast< T >( 0 );
    for( size_t i = 0; i < R * C; ++i )
        data[i] = zero;
}

template< typename T, size_t R, size_t C >
bool Matrix< T, R, C >::isZero() const {
    T zero = static_cast< T >( 0 );
    for( size_t i = 0; i < R * C; ++i )
        if( data[i] != zero )
            return false;
    return true;
}

template< typename T, size_t R, size_t C >
void Matrix< T, R, C >::setRow( const size_t row, const Vector< T, C >& values ) {
    BS_ASSERT_INDEX( row, R - 1 );
    for( size_t c = 0; c < C; ++c )
        elem[row][c] = values.data[c];
}

template< typename T, size_t R, size_t C >
void Matrix< T, R, C >::setColumn( const size_t col, const Vector< T, R >& values ) {
    BS_ASSERT_INDEX( col, C - 1 );
    for( size_t r = 0; r < R; ++r )
        elem[r][col] = values.data[r];
}

template< typename T, size_t R, size_t C >
Vector< T, C > Matrix< T, R, C >::getRow( const size_t row ) const {
    BS_ASSERT_INDEX( row, R - 1 );
    Vector< T, C > out;

    for( size_t c = 0; c < C; ++c )
        out.data[c] = elem[row][c];

    return out;
}

template< typename T, size_t R, size_t C >
Vector< T, R > Matrix< T, R, C >::getColumn( const size_t col ) const {
    BS_ASSERT_INDEX( col, C - 1 );
    Vector< T, R > out;

    for( size_t r = 0; r < R; ++r )
        out.data[r] = elem[r][col];

    return out;
}

template< typename T, size_t R, size_t C >
template< typename T2 >
Matrix< T, R, C >& Matrix< T, R, C >::operator =( const Matrix< T2, R, C >& right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] = static_cast<T>( right.data[i] );

    return (*this);
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator +=( const Matrix& right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] += right.data[i];

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator -=( const Matrix& right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] -= right.data[i];

    return ( *this );
}

namespace Private {

    //Note: You can only multiply an IxJ matrix with an JxK matrix.
    //      The result is an IxK matrix.
    //      Since this function multiplies in-place (i.e. it implements leftInOut *= right ),
    //      the resulting matrix must be of size IxJ.
    //      The only way this is possible is if K = J;
    //      in other words, the given matrix must be a square JxJ matrix.
    template< typename T, size_t R, size_t C >
    void MatrixMultiplyInPlace( Matrix< T, R, C >& leftInOut, const Matrix< T, C, C >& right ) {
        T oldRow[ C - 1 ];
        T dot;
        for( size_t r = 0; r < R; ++r ) {
            //Store the old values for the current row we're working on
            //We only need to store the first C-1 values, because we need to know
            //their old values when setting their new values for all but the last
            //element in the current row
            for( size_t c = 0; c < C - 1; ++c )
                oldRow[c] = leftInOut.elem[r][c];

            //Update the values
            for( size_t c = 0; c < C; ++c ) {
                dot = static_cast< T >( 0 );

                //Partially calculate the dot product between the current row in the left matrix,
                //and the current column in the right matrix.
                for( size_t c2 = 0; c2 < C - 1; ++c2 )
                    dot += oldRow[ c2 ] * right.elem[c2][c];

                //Finalize the dot product and update the element in the left matrix.
                leftInOut.elem[r][c] = dot +
                                       leftInOut.elem[r][C-1] * right.elem[C-1][c];
            }
        }
    }
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator *=( const Matrix< T, C, C >& right ) {
    Private::MatrixMultiplyInPlace( *this, right );
    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator +=( const T right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] += right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator -=( const T right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] -= right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator *=( const T right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] *= right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator /=( const T right ) {
    BS_ASSERT_NONZERO_DIVISOR( right )

    for( size_t i = 0; i < R * C; ++i )
        data[i] /= right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
std::ostream& operator <<( std::ostream& left, const Matrix< T, R, C >& right ) {
    size_t maxWidth = 1;
    std::string str[ R * C ];
    for( size_t i = 0; i < R * C; ++i ) {
        str[ i ] = ( boost::format( "%|.5f|" ) % right.data[i] ).str();
        electMax( maxWidth, str[ i ].size() );
    }

    for( size_t r = 0; r < R; ++r ) {
        left << std::setw( maxWidth ) << str[r*C];
        for( size_t c = 1; c < C; ++c )
            left << " " << std::setw( maxWidth ) << str[r*C + c];
        left << std::endl;
    }
    return left;
}

template< typename T, size_t R, size_t C >
bool operator ==( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    for( size_t i = 0; i < R * C; ++i )
        if( left.data[i] != right.data[i] )
            return false;
    return true;
}

template< typename T, size_t R, size_t C >
bool operator !=( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    for( size_t i = 0; i < R * C; ++i )
        if( left.data[i] != right.data[i] )
            return true;
    return false;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const Matrix< T, R, C >& right ) {
    return right;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator -( const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = -right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] + right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator -( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] - right.data[i];

    return out;
}

template< typename T, size_t R, size_t C, size_t C2 >
Matrix< T, R, C2 > operator *( const Matrix< T, R, C >& left, const Matrix< T, C, C2 >& right ) {
    Matrix< T, R, C2 > out;

    T dot;
    for( size_t r = 0; r < R; ++r ) {
        for( size_t c2 = 0; c2 < C2; ++c2 ) {
            dot = static_cast< T >( 0 );

            for( size_t c = 0; c < C; ++c )
                dot += left.elem[r][c] * right.elem[c][c2];

            out.elem[r][c2] = dot;
        }
    }

    return out;
}

//R-vector (as a 1xR matrix) * RxC matrix = C-vector (as a 1xC matrix)
template< typename T, size_t R, size_t C >
Vector< T, C > operator *( const Vector< T, R >& left, const Matrix< T, R, C >& right ) {
    Vector< T, C > out;

    T dot;
    for( size_t c = 0; c < C; ++c ) {
        dot = static_cast< T >( 0 );

        for( size_t r = 0; r < R; ++r )
            dot += left[r] * right.elem[r][c];

        out.data[c] = dot;
    }

    return out;
}

//RxC matrix * C-vector (as a Cx1 matrix) = R-vector (as a Rx1 matrix)
template< typename T, size_t R, size_t C >
Vector< T, R > operator *( const Matrix< T, R, C >& left, const Vector< T, C >& right ) {
    Vector< T, R > out;

    T dot;
    for( size_t r = 0; r < R; ++r ) {
        dot = static_cast< T >( 0 );

        for( size_t c = 0; c < C; ++c )
            dot += left.elem[r][c] * right[c];

        out.data[r] = dot;
    }

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const T left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left + right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const Matrix< T, R, C >& left, const T right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] + right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator -( const Matrix< T, R, C >& left, const T right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] - right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator *( const T left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left * right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator *( const Matrix< T, R, C >& left, const T right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] * right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator /( const Matrix< T, R, C >& left, const T right ) {
    BS_ASSERT_NONZERO_DIVISOR( right );

    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] / right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, C, R > transpose( const Matrix< T, R, C >& matrix ) {
    Matrix< T, C, R > out;

    for( size_t r = 0; r < R; ++r )
        for( size_t c = 0; c < C; ++c )
            out.elem[c][r] = matrix.elem[r][c];

    return out;
}

}

#endif //BS_MATRIX_MATRIXRXC_HPP


#include "select_brir.hpp"

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

#include "utils.hpp"

namespace bear {
SelectBRIR::SelectBRIR(const SignalFlowContext &ctx,
                       const char *name,
                       CompositeComponent *parent,
                       const ConfigImpl &config,
                       std::shared_ptr<Panner> panner_)
    : AtomicComponent(ctx, name, parent),
      panner(std::move(panner_)),
      views(panner->get_views()),
      listener_in("listener_in", *this, pml::EmptyParameterConfig()),
      brir_index_out("brir_index_out", *this, pml::EmptyParameterConfig()),
      listener_out("listener_out", *this, pml::EmptyParameterConfig())
{
  views.rowwise().normalize();

  for (const auto &view : views.rowwise()) {
    // calculate a rotation from the view vector, assuming it's a rotation around z
    bear_assert(std::abs(view.z()) < 1e-6, "expected BRIR rotations around z only");
    double z = std::sin(-std::atan2(view.x(), view.y()) / 2.0);
    double w = std::sqrt(1.0 - z * z);
    Eigen::Quaterniond rot(w, 0.0, 0.0, z);

    view_rotations.push_back(rot);
  }
}

void SelectBRIR::process()
{
  if (listener_in.changed()) {
    Eigen::Vector3d look_vector = listener_in.data().look();

    // find the closest BRIR view, by maximum dot-product
    unsigned int max_dp_idx;
    (views * look_vector).maxCoeff(&max_dp_idx);

    brir_index_out.data() = max_dp_idx;
    brir_index_out.swapBuffers();

    // calculate the 'residual' listener with the BRIR rotation removed
    Eigen::Vector3d front{0.0, 1.0, 0.0};
    Eigen::Quaterniond brir_rot = Eigen::Quaterniond::FromTwoVectors(front, views.row(max_dp_idx));
    listener_out.data().position = listener_in.data().position;
    listener_out.data().orientation = view_rotations[max_dp_idx] * listener_in.data().orientation;
    listener_out.swapBuffers();

    listener_in.resetChanged();
  }
}
}  // namespace bear


#include "test-results.hpp"
#include <boost/range/irange.hpp>
#include <cmath>
#include <spdlog/fmt/fmt.h>
#include <spdlog/spdlog.h>

#define VALUES_RANGE irange(0, 10000, 1)

float errorPForErrorInt(int e) { return (e / 1000000.0) * 20; }

TestResults::TestResults(std::string protocolName)
    : m_protocolName(protocolName) {}
TestResults::~TestResults() {}
void TestResults::addResult(int errorPropability, int trips,
                            bool successfulTransmission,
                            bool correctDiagnosis) {
  m_results[errorPropability].trips(trips);
  m_results[errorPropability].successfulTransmissions(
      successfulTransmission ? 1 : 0);
  m_results[errorPropability].correctDiagnosis(correctDiagnosis ? 1 : 0);
}

void TestResults::writeResults(std::ofstream &out) {
  using namespace fmt;
  using namespace boost;
  using namespace boost::accumulators;

  out << fmt::format(
      "p\ttrips_min\ttrips_max\ttrips_mean\ttrips_med\ttrips_sd\tsucc_mean\t"
      "succ_med\tdiag_mean\tdiag_med\n");
  for (auto errorProp : VALUES_RANGE) {
    auto &res = m_results[errorProp];
    out << fmt::format(
        "{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\n",
        errorPForErrorInt(errorProp), min(res.trips), max(res.trips),
        mean(res.trips), median(res.trips), sqrt(variance(res.trips)),
        mean(res.successfulTransmissions), median(res.successfulTransmissions),
        mean(res.correctDiagnosis), median(res.correctDiagnosis));
  }
}

std::string_view TestResults::name() { return m_protocolName; }

std::unique_ptr<TestResults>
TestResults::startAutomatedTest(protocol::Protocol &protocol) {
  std::unique_ptr<TestResults> results =
      std::make_unique<TestResults>(protocol.name());

  using namespace boost;

  // Simulate 1 minute at 120 messages per second.
  const int messageCount = 7200;
  const std::size_t messageSize =
      4; // Message size (in bytes) should be 4. This represents a single
         // integer of data to be transmitted.

  protocol.setMessageSize(messageSize);

  protocol::Protocol::Message msg[2];

  for (auto errorProp : VALUES_RANGE) {
    float p = errorPForErrorInt(errorProp);
    protocol.setErrorPropability(p);

    if (errorProp % 100 == 0) {
      spdlog::info("Currently at p={} for protocol \"{}\"", p, results->name());
    }

    for (auto _msgNumber : irange(0, messageCount)) {
      int trips = protocol.startTransfer(msg[0], msg[1]);
      results->addResult(errorProp, trips,
                         protocol.lastTransmissionSuccessful(),
                         protocol.lastDiagnosisCorrect());
    }
  }

  return results;
}


/* Copyright 2020 CNRS-AIST JRL */

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/QR>

#include <benchmark/benchmark.h>

#include "common.h"

using namespace Eigen;

// A = B
static void BM_Copy_MatrixXd(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target = source;
}
MAT_BENCHMARK(BM_Copy_MatrixXd);

// C = A*B
static void BM_Mult_MatrixXd(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.noalias() = A * B;
}
MAT_BENCHMARK(BM_Mult_MatrixXd);

// LLT of A
static void BM_LLT_Decomposition(benchmark::State & state)
{
  MatrixXd R = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd A = R.transpose() * R;
  LLT<MatrixXd> llt(state.range(0));
  for(auto _ : state)
  {
    llt.compute(A);
  }
}
MAT_BENCHMARK(BM_LLT_Decomposition);

// QR of A
static void BM_QR_Decomposition(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  HouseholderQR<MatrixXd> qr(state.range(0), state.range(0));
  for(auto _ : state)
  {
    qr.compute(A);
  }
}
MAT_BENCHMARK(BM_QR_Decomposition);

// QR of A
static void BM_QR_Decomposition_Inplace(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  HouseholderQR<Ref<MatrixXd>> qr(A);
  for(auto _ : state)
  {
    qr.compute(A);
  }
}
MAT_BENCHMARK(BM_QR_Decomposition_Inplace);

// Col piv QR of A
static void BM_ColPivQR_Decomposition(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  ColPivHouseholderQR<MatrixXd> qr(state.range(0), state.range(0));
  for(auto _ : state)
  {
    qr.compute(A);
  }
}
MAT_BENCHMARK(BM_ColPivQR_Decomposition);

// Col piv QR of A in place
static void BM_ColPivQR_Decomposition_Inplace(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  ColPivHouseholderQR<Ref<MatrixXd>> qr(A);
  for(auto _ : state)
  {
    qr.compute(A);
  }
}
MAT_BENCHMARK(BM_ColPivQR_Decomposition_Inplace);

// Col piv QR of A^T
static void BM_ColPivQR_Decomposition_Transpose(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  ColPivHouseholderQR<MatrixXd> qr(state.range(0), state.range(0));
  for(auto _ : state)
  {
    qr.compute(A.transpose());
  }
}
MAT_BENCHMARK(BM_ColPivQR_Decomposition_Transpose);

BENCHMARK_MAIN();


#include <boost/gil/image_processing/numeric.hpp>
#include <boost/gil/detail/math.hpp>
#include <boost/core/lightweight_test.hpp>

#include <algorithm>

namespace gil = boost::gil;

void test_dx_sobel_kernel()
{
    const auto kernel = gil::generate_dx_sobel(1);
    BOOST_TEST(std::equal(kernel.begin(), kernel.end(), gil::dx_sobel.begin()));
}

void test_dx_scharr_kernel()
{
    const auto kernel = gil::generate_dx_scharr(1);
    BOOST_TEST(std::equal(kernel.begin(), kernel.end(), gil::dx_scharr.begin()));
}

void test_dy_sobel_kernel()
{
    const auto kernel = gil::generate_dy_sobel(1);
    BOOST_TEST(std::equal(kernel.begin(), kernel.end(), gil::dy_sobel.begin()));
}

void test_dy_scharr_kernel()
{
    const auto kernel = gil::generate_dy_scharr(1);
    BOOST_TEST(std::equal(kernel.begin(), kernel.end(), gil::dy_scharr.begin()));
}

int main()
{
    test_dx_sobel_kernel();
    test_dx_scharr_kernel();
    test_dy_sobel_kernel();
    test_dy_scharr_kernel();
    return boost::report_errors();
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file sigma_point_additive_prediction_policy.hpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class SigmaPointPredictPolicy;

template <
    typename SigmaPointQuadrature,
    typename AdditiveTransitionFunction
>
class SigmaPointPredictPolicy<
          SigmaPointQuadrature,
          Additive<AdditiveTransitionFunction>>
    : public Descriptor
{
public:
    typedef typename AdditiveTransitionFunction::State State;
    typedef typename AdditiveTransitionFunction::Input Input;

    enum : signed int
    {
        NumberOfPoints = SigmaPointQuadrature
                            ::number_of_points(SizeOf<State>::Value)
    };

    typedef PointSet<State, NumberOfPoints> StatePointSet;

    template <
        typename Belief
    >
    void operator()(const AdditiveTransitionFunction&
                              additive_transition_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Input& u,
                    Belief& predicted_belief)
    {
        auto f = [&](const State& x)
        {
            return additive_transition_function.expected_state(x, u);
        };

        quadrature.propergate_gaussian(f, prior_belief, Y, Z);

        /*
         * Obtain the centered points matrix of the prediction. The columns of
         * this matrix are the predicted points with zero mean. That is, the
         * sum of the columns in P is zero.
         *
         * P = [X_r[1]-mu_r  X_r[2]-mu_r  ... X_r[n]-mu_r]
         *
         * with weighted mean
         *
         * mu_r = Sum w_mean[i] X_r[i]
         */
        auto X_c = Z.centered_points();

        /*
         * Obtain the weights of point as a vector
         *
         * W = [w_cov[1]  w_cov[2]  ... w_cov[n]]
         *
         * Note that the covariance weights are used.
         */
        auto W = Z.covariance_weights_vector();

        /*
         * Compute and set the moments
         *
         * The first moment is simply the weighted mean of points.
         * The second centered moment is determined by
         *
         * C = Sum W[i,i] * (X_r[i]-mu_r)(X_r[i]-mu_r)^T
         *   = P * W * P^T
         *
         * given that W is the diagonal matrix
         */
        predicted_belief.dimension(prior_belief.dimension());
        predicted_belief.mean(Z.mean());
        predicted_belief.covariance(
            X_c * W.asDiagonal() * X_c.transpose()
            + additive_transition_function.noise_covariance());
    }

    virtual std::string name() const
    {
        return "SigmaPointPredictPolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "Additive<AdditiveTransitionFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Sigma Point based filter prediction policy for state "
               "transition model with additive noise";
    }

protected:
    StatePointSet Y;
    StatePointSet Z;
};

}


/*
Copyright (c) 2020 Naomasa Matsubayashi

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <boost/program_options.hpp>
#include <enoki/cuda.h>
#include <enoki/dynamic.h>
#include <enoki/autodiff.h>
#include <enoki/special.h>
#include <enoki/array_router.h>
#include "ifm/fft.h"
#include "ifm/load_monoral.h"
#include "ifm/spectrum_image.h"

using i_t = enoki::DiffArray< enoki::CUDAArray< int > >;
using v_t = enoki::DiffArray< enoki::CUDAArray< float > >;

float j1( float b, int n ) {
  float prev = 0;
  float width = 0.001;
  float sum = 0;
  for( float w = width; w < float( M_PI/2 ); w += width ) {
    float cur = std::sin( b * std::sin( w ) ) * std::sin( n*w );
    sum += ( cur + prev ) * width / 2;
    prev = cur;
  }
  return 2.f / M_PI * sum;
}

float j0( float b, int n ) {
  float prev = 1;
  float width = 0.001;
  float sum = 0;
  for( float w = width; w < float( M_PI/2 ); w += width ) {
    float cur = std::cos( b * std::sin( w ) ) * std::cos( n*w );
    sum += ( cur + prev ) * width / 2;
    prev = cur;
  }
  return 2.f / M_PI * sum;
}

float j( float b, int n ) {
  if( n % 2 ) return j1( b, n );
  else return j0( b, n );
}

int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("harmonic,H", boost::program_options::value<int>()->default_value(0),  "倍音のみ")
    ("match,m", boost::program_options::value<bool>()->default_value(false),  "減衰曲線をマッチさせる")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const auto [audio,sample_rate] = ifm::load_monoral( input_filename, true );
  ifm::spectrum_image conv( params["note"].as<int>(), sample_rate, 1 << params["resolution"].as<int>() );
  auto [image,delay] = conv( audio );
  unsigned int width = conv.get_width();
  float resolution = 1 << params["resolution"].as<int>();
  std::vector< float > envelope;

  if( params["harmonic"].as<int>() > 0 ) {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = 0.f;
      for( unsigned int x = 1; x < width; x += 24 )
        sum += image[ x + y * width ];
      envelope.push_back( sum );
    }
  }
  else if( params["harmonic"].as<int>() < 0 ) {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = 0.f;
      for( unsigned int x = 25; x < width; ++x ) {
        unsigned int step =  x % 24;
        if( step > 1 && step < 23 )
          sum += image[ x + y * width ];
      }
      envelope.push_back( sum );
    }
  }
  else {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = std::accumulate( image.data() + y * width, image.data() + ( y + 1 ) * width, 0.f );
      envelope.push_back( sum );
    }
  }
  if( params["match"].as<bool>() ) {
    using v_t = enoki::DiffArray< enoki::CUDAArray< float > >;
    const auto max = std::max_element( envelope.begin(), envelope.end() );
    envelope.erase( envelope.begin(), max );
    const auto min = std::find_if( envelope.begin(), envelope.end(), []( float v ) { return v < 0.01f; } );
    envelope.erase( min, envelope.end() );
    if( params["harmonic"].as<int>() < 0 )
      envelope.resize( std::min( size_t( 50 ), envelope.size() ) );
    auto expected = v_t::copy( envelope.data(), envelope.size() );
    float a = 1;
    float b = 10;
    float c = 0.3;
    v_t beta1_ = 0.9;
    v_t beta2_ = 0.999;
    v_t alpha = 0.001;
    v_t beta1 = 0.9;
    v_t beta2 = 0.999;
    v_t a_( a );
    v_t b_( b );
    v_t c_( c );
    v_t m_a( 0 );
    v_t m_b( 0 );
    v_t m_c( 0 );
    v_t v_a( 0 );
    v_t v_b( 0 );
    v_t v_c( 0 );
    auto t = enoki::arange< v_t >( envelope.size() ) * v_t( 0.01 );
    for( int i = 1; i != 100000; ++i ) {
      a_ = a;
      b_ = b;
      c_ = c;
      enoki::set_requires_gradient( a_ );
      enoki::set_requires_gradient( b_ );
      enoki::set_requires_gradient( c_ );
      auto generated = a_ * enoki::exp( -t * b_ ) + c_;
      auto diff = ( expected - generated );
      //auto loss = enoki::hsum( diff * diff );
      //std::cout << loss << " ";
      enoki::backward( diff * diff );

      v_t g_a = gradient( a_ );
      v_t g_b = gradient( b_ );
      v_t g_c = gradient( c_ );
      auto ms = beta1;//enoki::pow( beta1, i_ );
      auto vs = beta2;//enoki::pow( beta2, i_ );
      beta1 *= beta1_;
      beta2 *= beta2_;
      m_a = beta1 * m_a + ( 1 - beta1 ) * g_a;
      v_a = beta2 * v_a + ( 1 - beta2 ) * g_a * g_a;
      auto mhat_a = m_a / ( 1 - ms );
      auto vhat_a = v_a / ( 1 - vs );
      a_ -= ( alpha * mhat_a / ( enoki::sqrt( vhat_a ) + 0.000001f ) );

      m_b = beta1 * m_b + ( 1 - beta1 ) * g_b;
      v_b = beta2 * v_b + ( 1 - beta2 ) * g_b * g_b;
      auto mhat_b = m_b / ( 1 - ms );
      auto vhat_b = v_b / ( 1 - vs );
      b_ -= ( alpha * mhat_b / ( enoki::sqrt( vhat_b ) + 0.000001f ) );

      m_c = beta1 * m_c + ( 1 - beta1 ) * g_c;
      v_c = beta2 * v_c + ( 1 - beta2 ) * g_c * g_c;
      auto mhat_c = m_c / ( 1 - ms );
      auto vhat_c = v_c / ( 1 - vs );
      c_ -= ( alpha * mhat_c / ( enoki::sqrt( vhat_c ) + 0.000001f ) );
      a = a_[ 0 ];
      b = b_[ 0 ];
      c = c_[ 0 ];
      if( !( i % 100 ) )
        std::cout << enoki::hsum( diff * diff ) << " " << a_ << " " << b_ << " " << c_ << std::endl;
      enoki::cuda_eval( true );
      enoki::cuda_sync();

    }
  }
  else {
    for( unsigned int y = 0; y != envelope.size(); ++y ) {
      std::cout << 0.01f * y << " " << envelope[ y ] << std::endl;
    }
  }
}


/******************************************************************************
 * 反射衛星砲の実計算関数群
 *****************************************************************************/

#ifndef __SATELLITE_REFLECTOR_HPP__
#define __SATELLITE_REFLECTOR_HPP__

#include <vector>
#include <Eigen/Core>
#include "mathematic_utils.hpp"
#include "coodinate_system.hpp"
Vector3dSet find_impact_point(
    const Eigen::Vector3d& P0,
    const Eigen::Vector3d& P1, const Eigen::Vector3d& N,
    const Eigen::Vector3d& O,  const Eigen::Vector3d& R);

bool find_impact(const geodetic& P, geodetic* X,
                 const std::string& tle_str, const time_t* t);

#endif // __SATELLITE_REFLETCTOR_HPP__


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ASEC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ASEC_HPP_INCLUDED

#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/acsc.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( asec_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      if (is_equal(a0, One<A0>())) return Zero<A0>();
      return (Pio_2<A0>()-acsc(a0)) +  Constant<A0, 0x3c91a62633145c07ll>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( asec_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      A0 ax =  bs::abs(a0);
      if (ax <  One<A0>()) return Nan<A0>();
      A0 ax1 =  dec(ax);

      if (ax1 < 0.001f)
      {
        A0 tmp = sqrt(Two<A0>()*ax1)
          *oneminus(ax1*(Ratio<A0, 5, 12>()
                         +ax1*(Ratio<A0, 43, 160>()
                               -ax1*(Ratio<A0, 177, 896>()
                                     +ax1*Ratio<A0, 2867, 18432>()
                                    )
                              )
                        )
                   );

        return (is_ltz(a0)) ? Pi<A0>()-tmp : tmp;
      }

      if (is_equal(a0, One<A0>())) return Zero<A0>();
      return  (Pio_2<A0>()-acsc(a0));
    }
  };
} } }

#endif


#pragma once

#include<vector>
#include <Eigen/Geometry>
#include "NeuralNetwork.h"
#include "Sample.h"
#include <fstream>
#include <iterator>

using namespace Eigen;

namespace simulator {

	void NeuralNetwork::SetHiddenLayers(int numberOfHiddenLayers, int hiddenLayersLength, int numberOfOutputNode) {

		_net.add_layer(new FullyConnected<Identity>(3, hiddenLayersLength));//3 in input because there are 3 independant rotation speeds

		for (int i = 0; i < numberOfHiddenLayers; i++) {
			_net.add_layer(new FullyConnected<Sigmoid>(hiddenLayersLength, hiddenLayersLength));
		}
		_net.add_layer(new FullyConnected<Identity>(hiddenLayersLength, numberOfOutputNode));
		_net.set_output(new RegressionMSE());
	}
	void NeuralNetwork::SetLearningRate(double learningRate) {
		_opt.m_lrate = learningRate;
	}
	Scalar operator ""_eng(long double d) {
		// Add a cast here if the compiler still complains
		return d;
	}
	void NeuralNetwork::Train(Sample &sample, int batchSize, int epoch) {
		VerboseCallback callback;
		_net.set_callback(callback);
		_net.init(NORMALDISTRIBUTIONMEAN, STANDARDDIVIATION, RANDOMSEED);
		MatrixXd input = sample.GetRotSpeed();
		MatrixXd output = sample.GetEnergy();
		_net.fit(_opt, input, output, batchSize, epoch, RANDOMSEED);
	}
	void NeuralNetwork::Save(string fileName) {
		vector<vector<Scalar>> parameters = _net.get_parameters();
		ofstream output_file(fileName);
		ostream_iterator<Scalar> output_iterator(output_file, " ");
		for (const auto& row : parameters) {
			copy(row.cbegin(), row.cend(), output_iterator);
			output_file << endl;
		}
	}
	void NeuralNetwork::LoadParameters(string fileName) {
		vector<vector<Scalar>> parameters;
		ifstream file(fileName);
		string line;
		while (getline(file, line)) {
			stringstream linestream(line);
			string item;
			vector<Scalar> tempVec;
			while (getline(linestream, item, ' ')) {
				tempVec.push_back(stold(item));
			}
			parameters.push_back(tempVec);
		}
		_net.init(NORMALDISTRIBUTIONMEAN, STANDARDDIVIATION, RANDOMSEED);
		_net.set_parameters(parameters);
	}
	MatrixXd NeuralNetwork::Predict(MatrixXd &input) {
		MatrixXd pred = _net.predict(input);
		return pred;
	}

}


#pragma once

#include <utility>
#include <vector>
#include <Eigen/Dense>
#include "../linear_solver.h"

class SmoothBoundaries
{
    const Eigen::MatrixXi& F;
    LDLTSolver solver;
    std::vector< std::vector<int> > adj;

public:

    typedef std::vector< std::pair<int, Eigen::Vector3d> > ImplicitBoundary;

    SmoothBoundaries(const Eigen::MatrixXi& Fin);

    void initGeometry(const Eigen::MatrixXd& V);

    ImplicitBoundary smooth(const std::vector<int>& loop,
                            const Eigen::MatrixXd& V, const Eigen::MatrixXi& F);

    void getCoordinates(const Eigen::MatrixXd& V, const ImplicitBoundary& boudary, Eigen::MatrixXd& out);
};


///////////////////////////////////////////////////////////////////////////////
// weighted_covariance.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_COVARIANCE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_COVARIANCE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <numeric>
#include <functional>
#include <complex>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/range.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/type_traits/is_scalar.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/covariance.hpp> // for numeric::outer_product() and type traits
#include <boost/accumulators/statistics/weighted_mean.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_covariance_impl
    //
    /**
        @brief Weighted Covariance Estimator

        An iterative Monte Carlo estimator for the weighted covariance \f$\mathrm{Cov}(X,X')\f$, where \f$X\f$ is a sample
        and \f$X'\f$ a variate, is given by:

        \f[
            \hat{c}_n = \frac{\bar{w}_n-w_n}{\bar{w}_n} \hat{c}_{n-1} + \frac{w_n}{\bar{w}_n-w_n}(X_n - \hat{\mu}_n)(X_n' - \hat{\mu}_n'),
            \quad n\ge2,\quad\hat{c}_1 = 0,
        \f]

        \f$\hat{\mu}_n\f$ and \f$\hat{\mu}_n'\f$ being the weighted means of the samples and variates and
        \f$\bar{w}_n\f$ the sum of the \f$n\f$ first weights \f$w_i\f$.
    */
    template<typename Sample, typename Weight, typename VariateType, typename VariateTag>
    struct weighted_covariance_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Weight, typename numeric::functional::average<Sample, std::size_t>::result_type>::result_type weighted_sample_type;
        typedef typename numeric::functional::multiplies<Weight, typename numeric::functional::average<VariateType, std::size_t>::result_type>::result_type weighted_variate_type;
        // for boost::result_of
        typedef typename numeric::functional::outer_product<weighted_sample_type, weighted_variate_type>::result_type result_type;

        template<typename Args>
        weighted_covariance_impl(Args const &args)
          : cov_(
                numeric::outer_product(
                    numeric::average(args[sample | Sample()], (std::size_t)1)
                      * numeric::one<Weight>::value
                  , numeric::average(args[parameter::keyword<VariateTag>::get() | VariateType()], (std::size_t)1)
                      * numeric::one<Weight>::value
                )
            )
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            if (cnt > 1)
            {
                extractor<tag::weighted_mean_of_variates<VariateType, VariateTag> > const some_weighted_mean_of_variates = {};

                this->cov_ = this->cov_ * (sum_of_weights(args) - args[weight]) / sum_of_weights(args)
                           + numeric::outer_product(
                                 some_weighted_mean_of_variates(args) - args[parameter::keyword<VariateTag>::get()]
                               , weighted_mean(args) - args[sample]
                             ) * args[weight] / (sum_of_weights(args) - args[weight]);
            }
        }

        result_type result(dont_care) const
        {
            return this->cov_;
        }

    private:
        result_type cov_;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_covariance
//
namespace tag
{
    template<typename VariateType, typename VariateTag>
    struct weighted_covariance
      : depends_on<count, sum_of_weights, weighted_mean, weighted_mean_of_variates<VariateType, VariateTag> >
    {
        typedef accumulators::impl::weighted_covariance_impl<mpl::_1, mpl::_2, VariateType, VariateTag> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_covariance
//
namespace extract
{
    extractor<tag::abstract_covariance> const weighted_covariance = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_covariance)
}

using extract::weighted_covariance;

}} // namespace boost::accumulators

#endif


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Incompressible module (body solver)
// Compute sources on the body surface from the updated  impermeability B.C. and solve for doublets on the body surface
//
// Ref: Katz & Plotkin (2001), Low-Speed Aerodynamics
// 1) tau = - (U_inf + Usigma) \cdot n
// 2) mu = A \ (B * tau)
//
// I/O:
// - vInf: freestream velocity vector
// - RHS: right-hand side of the linear system of equations
// - vSigma: field source induced body velocity
// - bPan: (network of) body panels (structure)
// - b2bAIC: body to body AIC (structure)

#include <iostream>
#include <Eigen/Dense>
#include "solve_body.h"

using namespace std;
using namespace Eigen;

void solve_body(Vector3d &vInf, VectorXd &RHS, MatrixX3d &vSigma, Network &bPan, Body_AIC &b2bAIC) {

    //// Begin
    cout << "Computing surface singularities... " << flush;

    //// Update B.C.
    // BC: tau_i = n_i * V_inf + n_i * V_sigma
    for (int i = 0; i < bPan.nP; ++i)
        bPan.tau(i) = - bPan.n.row(i).dot(vInf.transpose()) - bPan.n.row(i).dot(vSigma.row(i));

    //// Solve
    // Compute RHS
    RHS = - b2bAIC.B * bPan.tau;
    // Solve: A*mu + B*sigma = 0 (Dirichlet: interior potential = 0)
    #ifdef ON_UNIX
        bPan.mu = b2bAIC.A.householderQr().solve(RHS);
    #else
        bPan.mu = b2bAIC.A.fullPivLu().solve(RHS);
    #endif

    //// Control display
    cout << "Done!" << endl;
    cout << "Surface sources strength: " << bPan.tau.norm() << endl;
    #ifdef VERBOSE
        cout << "Sources min. value: " << bPan.tau.minCoeff() << endl;
        cout << "Sources max. value: " << bPan.tau.maxCoeff() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.tau(i) << endl;
    #endif
    cout << "Surface doublets strength: " << bPan.mu.norm() << endl;
    #ifdef VERBOSE
        cout << "Doublets min. value: " << bPan.mu.minCoeff() << endl;
        cout << "Doublets max. value: " << bPan.mu.maxCoeff() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.mu(i) << endl;
            cout << "System solved with relative error: " << (b2bAIC.A * bPan.mu - RHS).norm() / RHS.norm() << endl;
    #endif
    cout << endl;
}


// Boost.Geometry Index
//
// n-dimensional content (hypervolume) - 2d area, 3d volume, ...
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// 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_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_CONTENT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_CONTENT_HPP

#include <boost/geometry/index/detail/algorithms/content.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

namespace dispatch {

// TODO replace it by comparable_content?
// probably radius^Dimension would be sufficient
// WARNING! this would work only if the same Geometries were compared
// so it shouldn't be used in the case of Variants!
// The same with margin()!

template <typename NSphere, size_t Dimension>
struct content_nsphere
{
    BOOST_STATIC_ASSERT(2 < Dimension);

    typedef typename detail::default_content_result<NSphere>::type result_type;

    static inline result_type apply(NSphere const& s)
    {
        return (content_nsphere<NSphere, Dimension - 2>::apply(s)
                    * 2 * get_radius<0>(s) * get_radius<0>(s)
                    * ::boost::math::constants::pi<result_type>()) / Dimension;
    }
};

template <typename NSphere>
struct content_nsphere<NSphere, 2>
{
    typedef typename detail::default_content_result<NSphere>::type result_type;

    static inline result_type apply(NSphere const& s)
    {
        return ::boost::math::constants::pi<result_type>() * get_radius<0>(s) * get_radius<0>(s);
    }
};

template <typename NSphere>
struct content_nsphere<NSphere, 1>
{
    typedef typename detail::default_content_result<NSphere>::type result_type;

    static inline result_type apply(NSphere const& s)
    {
        return 2 * get_radius<0>(s);
    }
};

template <typename Indexable>
struct content<Indexable, nsphere_tag>
{
    static typename default_content_result<Indexable>::type apply(Indexable const& i)
    {
        return dispatch::content_nsphere<Indexable, dimension<Indexable>::value>::apply(i);
    }
};

} // namespace dispatch

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_CONTENT_HPP