/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/test/pow.cpp
 *
 * \brief Test suite for the \c pow operation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * 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 <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/pow.hpp>
#include <cmath>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const double tol = 1.0e-5;

BOOST_UBLASX_TEST_DEF( test_real_vector_1 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vector -> [vector .^ scalar]" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n = 5;
    const double exp = 3;

    vector_type v(n);

    v(0) = -1.9;
    v(1) = -0.2;
    v(2) =  3.4;
    v(3) =  5.6;
    v(4) =  7.0;

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::pow(v, exp);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "pow(v," << exp << ") = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = std::pow(v(i), exp);
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_vector_2 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vector -> [scalar .^ vector]" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n = 5;
    const double base = 10;

    vector_type v(n);

    v(0) = -1.9;
    v(1) = -0.2;
    v(2) =  3.4;
    v(3) =  5.6;
    v(4) =  7.0;

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::pow(base, v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "pow(" << base << ",v) = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = std::pow(base, v(i));
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_vector_1 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vector -> [vector .^ scalar]" );

    typedef std::complex<double> in_value_type;
    typedef in_value_type out_value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<in_value_type> in_vector_type;
    typedef ublas::vector<out_value_type> out_vector_type;

    const size_type n = 4;
    const double exp = 3;

    in_vector_type v(n);

    v(0) = in_value_type(1,2);
    v(1) = in_value_type(2,3);
    v(2) = in_value_type(3,4);
    v(3) = in_value_type(4,5);

    out_vector_type res;
    out_vector_type expect_res(n);

    res = ublasx::pow(v, exp);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "pow(v, " << exp << ") = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = std::pow(v(i), exp);
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_vector_2 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vector -> [scalar .^ vector]" );

    typedef std::complex<double> in_value_type;
    typedef in_value_type out_value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<in_value_type> in_vector_type;
    typedef ublas::vector<out_value_type> out_vector_type;

    const size_type n = 4;
    const double base = 10;

    in_vector_type v(n);

    v(0) = in_value_type(1,2);
    v(1) = in_value_type(2,3);
    v(2) = in_value_type(3,4);
    v(3) = in_value_type(4,5);

    out_vector_type res;
    out_vector_type expect_res(n);

    res = ublasx::pow(base, v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "pow(" << base << ",v) = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        // Remember that c^{a+ib} == e^{log_{10}(c)*(a+ib)}
        expect_res(i) = std::exp(std::log(base)*v(i));
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix_1 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Matrix -> [matrix .^ scalar]" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type> matrix_type;

    const size_type nr = 2;
    const size_type nc = 3;
    const double exp = 3;

    matrix_type A(nr,nc);

    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type R;
    matrix_type expect_R(nr,nc);

    R = ublasx::pow(A, exp);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "pow(A, " << exp << ") = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = std::pow(A(r,c), exp);
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix_2 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Matrix -> [scalar .^ matrix]" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type> matrix_type;

    const size_type nr = 2;
    const size_type nc = 3;
    const double base = 10;

    matrix_type A(nr,nc);

    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type R;
    matrix_type expect_R(nr,nc);

    R = ublasx::pow(base, A);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "pow(" << base << ",A) = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = std::pow(base, A(r,c));
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_matrix_1 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Matrix -> [matrix .^ scalar]" );

    typedef std::complex<double> in_value_type;
    typedef in_value_type out_value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<in_value_type> in_matrix_type;
    typedef ublas::matrix<out_value_type> out_matrix_type;

    const size_type nr = 2;
    const size_type nc = 3;
    const double exp = 3;

    in_matrix_type A(nr,nc);

    A(0,0) = in_value_type(1,2); A(0,1) = in_value_type(2,3); A(0,2) = in_value_type(3,4);
    A(1,0) = in_value_type(4,5); A(1,1) = in_value_type(5,6); A(1,2) = in_value_type(6,7);

    out_matrix_type R;
    out_matrix_type expect_R(nr,nc);

    R = ublasx::pow(A, exp);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "pow(A, " << exp << ") = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = std::pow(A(r,c), exp);
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_matrix_2 )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Matrix -> [scalar .^ matrix]" );

    typedef std::complex<double> in_value_type;
    typedef in_value_type out_value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<in_value_type> in_matrix_type;
    typedef ublas::matrix<out_value_type> out_matrix_type;

    const size_type nr = 2;
    const size_type nc = 3;
    const double base = 10;

    in_matrix_type A(nr,nc);

    A(0,0) = in_value_type(1,2); A(0,1) = in_value_type(2,3); A(0,2) = in_value_type(3,4);
    A(1,0) = in_value_type(4,5); A(1,1) = in_value_type(5,6); A(1,2) = in_value_type(6,7);

    out_matrix_type R;
    out_matrix_type expect_R(nr,nc);

    R = ublasx::pow(base, A);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "pow(" << base << ",A) = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = std::exp(std::log(base)*A(r,c));
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

int main()
{

    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'pow' operation");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_real_vector_1 );
    BOOST_UBLASX_TEST_DO( test_real_vector_2 );
    BOOST_UBLASX_TEST_DO( test_complex_vector_1 );
    BOOST_UBLASX_TEST_DO( test_complex_vector_2 );
    BOOST_UBLASX_TEST_DO( test_real_matrix_1 );
    BOOST_UBLASX_TEST_DO( test_real_matrix_2 );
    BOOST_UBLASX_TEST_DO( test_complex_matrix_1 );
    BOOST_UBLASX_TEST_DO( test_complex_matrix_2 );

    BOOST_UBLASX_TEST_END();
}


#include <ImathEuler.h>
#include <anim/datatypes/transform.h>

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <cstdlib>

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

float compareMatrices(const Imath::M44f& m1, const Imath::M44f& m2) {
	// cout << m1 << endl << m2 << endl << endl;

	float result = 0.0f;
	for(unsigned a = 0; a < 4; ++a)
		for(unsigned b = 0; b < 4; ++b)
			result += std::fabs(m1[a][b] - m2[a][b]);
	return result;
}

float compareMatrices(const Imath::M33f& m1, const Imath::M33f& m2) {
	// cout << m1 << endl << m2 << endl << endl;

	float result = 0.0f;
	for(unsigned a = 0; a < 3; ++a)
		for(unsigned b = 0; b < 3; ++b)
			result += std::fabs(m1[a][b] - m2[a][b]);
	return result;
}

float compareTransforms(const anim::Transform& m1, const anim::Transform& m2) {
	// cout << m1 << endl << m2 << endl << endl;

	// antipodality - two possible solutions to matrix-to-quat transformation - make sure we handle both
	float antipod1 = 0.0f, antipod2 = 0.0f;
	for(unsigned a = 0; a < 4; ++a) {
		antipod1 += std::fabs(m1.rotation[a] - m2.rotation[a]);
		antipod2 += std::fabs(m1.rotation[a] + m2.rotation[a]);
	}
	float result = std::min(antipod1, antipod2);

	for(unsigned a = 0; a < 3; ++a)
		result += std::fabs(m1.translation[a] - m2.translation[a]);

	return result;
}

namespace {
static const float EPS = 1e-4f;
}

/////////////
// tests mainly the correspondence between the transformation class
// and a 4x4 matrix.

BOOST_AUTO_TEST_CASE(transform_init) {
	std::vector<std::pair<anim::Transform, Imath::M44f>> data = {
	    {anim::Transform(), Imath::M44f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1)},
	    {anim::Transform(Imath::V3f(1, 2, 3)), Imath::M44f(1, 0, 0, 1, 0, 1, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1)},
	    {anim::Transform(Imath::Quatf(-0.326096f, -0.0849528, -0.190674, 0.922001)),
	     Imath::M44f(-0.772889, 0.633718, -0.0322979, 0, -0.568925, -0.71461, -0.407009, 0, -0.28101, -0.296198,
	                 0.912853, 0, 0, 0, 0, 1)},
	    {anim::Transform(Imath::Quatf(-0.326096f, -0.0849528, -0.190674, 0.922001), Imath::V3f(3, 4, 5)),
	     Imath::M44f(-0.772889, 0.633718, -0.0322979, 3, -0.568925, -0.71461, -0.407009, 4, -0.28101, -0.296198,
	                 0.912853, 5, 0, 0, 0, 1)}};

	// compare the resulting matrices (transform-to-matrix)
	for(auto& i : data)
		BOOST_REQUIRE_SMALL(compareMatrices(i.first.toMatrix44(), i.second), EPS);

	// and compare the transformation (matrix-to-transform)
	for(auto& i : data)
		BOOST_REQUIRE_SMALL(compareTransforms(i.first, anim::Transform(i.second)), EPS);

	// test inverses
	for(auto& i : data) {
		// cout << i.first.inverse().toMatrix44() << endl;
		// cout << i.second.inverse() << endl << endl;

		BOOST_REQUIRE_SMALL(compareTransforms(i.first.inverse(), i.second.inverse()), EPS);
	}

	// and test that multiplication with inverse always leads to unit
	for(auto& i : data) {
		BOOST_REQUIRE_SMALL(compareTransforms(i.first.inverse() * i.first, anim::Transform(Imath::Quatf(1, 0, 0, 0))),
		                    EPS);

		BOOST_REQUIRE_SMALL(compareTransforms(i.first * i.first.inverse(), anim::Transform(Imath::Quatf(1, 0, 0, 0))),
		                    EPS);
	}

	///
	{
		Imath::Quatf q;
		q.setAxisAngle(Imath::V3f(0, 0, 1), 0);

		BOOST_REQUIRE_SMALL(compareMatrices(q.toMatrix33(), Imath::M33f(1, 0, 0, 0, 1, 0, 0, 0, 1)), EPS);
	}

	for(unsigned a = 0; a < 100; ++a) {
		float angle = 8.0f * M_PI * (float)rand() / (float)RAND_MAX;

		Imath::Quatf q;
		q.setAxisAngle(Imath::V3f(1, 0, 0), angle);

		BOOST_REQUIRE_SMALL(compareMatrices(q.toMatrix33(), Imath::M33f(1, 0, 0, 0, cos(angle), sin(angle), 0,
		                                                                -sin(angle), cos(angle))),
		                    EPS);
	}

	for(unsigned a = 0; a < 100; ++a) {
		float angle = 8.0f * M_PI * (float)rand() / (float)RAND_MAX;

		Imath::Quatf q;
		q.setAxisAngle(Imath::V3f(0, 1, 0), angle);

		BOOST_REQUIRE_SMALL(compareMatrices(q.toMatrix33(), Imath::M33f(cos(angle), 0, -sin(angle), 0, 1, 0, sin(angle),
		                                                                0, cos(angle))),
		                    EPS);
	}
}

////////////////

namespace {
const Imath::M44f toMatrix44(const std::pair<Imath::Eulerf, Imath::V3f>& data) {
	Imath::M44f result = data.first.toMatrix44().transpose();
	result[0][3] = data.second[0];
	result[1][3] = data.second[1];
	result[2][3] = data.second[2];

	return result;
}
}  // namespace

BOOST_AUTO_TEST_CASE(transform_operations) {
	std::vector<std::pair<std::pair<Imath::Eulerf, Imath::V3f>, std::pair<Imath::Eulerf, Imath::V3f>>> data = {
	    {{Imath::Eulerf(-25, 20, 36), Imath::V3f(0, 0, 0)}, {Imath::Eulerf(89, -93, 63), Imath::V3f(0, 0, 0)}},
	    {{Imath::Eulerf(-25, 20, 36), Imath::V3f(1, 2, 3)}, {Imath::Eulerf(89, -93, 63), Imath::V3f(0, 0, 0)}},
	    {{Imath::Eulerf(-25, 20, 36), Imath::V3f(1, 2, 3)}, {Imath::Eulerf(89, -93, 63), Imath::V3f(7, 6, 5)}}};

	for(auto& i : data) {
		// cout << (anim::Transform(~i.first.first.toQuat(), i.first.second) * anim::Transform(~i.second.first.toQuat(),
		// i.second.second)).toMatrix44() << endl; cout << toMatrix44(i.first) * toMatrix44(i.second) << endl;

		BOOST_REQUIRE_SMALL(compareMatrices((anim::Transform(i.first.first.toQuat(), i.first.second) *
		                                     anim::Transform(i.second.first.toQuat(), i.second.second))
		                                        .toMatrix44(),
		                                    toMatrix44(i.first) * toMatrix44(i.second)),
		                    EPS);
	}

	// cout << "-----" << endl;

	for(auto& i : data)
		BOOST_REQUIRE_SMALL(compareTransforms(anim::Transform(i.first.first.toQuat(), i.first.second) *
		                                          anim::Transform(i.second.first.toQuat(), i.second.second),
		                                      anim::Transform(toMatrix44(i.first) * toMatrix44(i.second))),
		                    EPS);

	for(auto& i : data) {
		anim::Transform t(i.first.first.toQuat(), i.first.second);
		t *= anim::Transform(i.second.first.toQuat(), i.second.second);

		// cout << t.toMatrix44() << endl;
		// cout << toMatrix44(i.first) * toMatrix44(i.second) << endl;

		BOOST_REQUIRE_SMALL(compareMatrices(t.toMatrix44(), toMatrix44(i.first) * toMatrix44(i.second)), EPS);
	}
}

namespace {
anim::Transform randomTransform() {
	const float a = (float)rand() / (float)RAND_MAX * 180.0f - 90.0f;
	const float b = (float)rand() / (float)RAND_MAX * 180.0f - 90.0f;
	const float c = (float)rand() / (float)RAND_MAX * 180.0f - 90.0f;
	Imath::Eulerf angle(a, b, c);

	const float x = (float)rand() / (float)RAND_MAX * 20.0f - 10.0f;
	const float y = (float)rand() / (float)RAND_MAX * 20.0f - 10.0f;
	const float z = (float)rand() / (float)RAND_MAX * 20.0f - 10.0f;
	Imath::V3f v(x, y, z);

	return anim::Transform(angle.toQuat(), v);
}
}  // namespace

BOOST_AUTO_TEST_CASE(transform_operations_random) {
	for(unsigned a = 0; a < 1000; ++a) {
		anim::Transform t1 = randomTransform();
		anim::Transform t2 = randomTransform();

		BOOST_REQUIRE_SMALL(compareMatrices((t1 * t2).toMatrix44(), t1.toMatrix44() * t2.toMatrix44()), EPS);
	}

	for(unsigned a = 0; a < 1000; ++a) {
		anim::Transform t1 = randomTransform();
		anim::Transform t2 = randomTransform();
		anim::Transform t3 = randomTransform();

		BOOST_REQUIRE_SMALL(
		    compareMatrices((t1 * t2 * t3).toMatrix44(), t1.toMatrix44() * t2.toMatrix44() * t3.toMatrix44()), EPS);
	}
}

BOOST_AUTO_TEST_CASE(transform_operations_1) {
	///
	{
		Imath::Quatf q;
		q.setAxisAngle(Imath::V3f(0, 0, 1), 0);

		BOOST_REQUIRE_SMALL(compareMatrices(q.toMatrix33(), Imath::M33f(1, 0, 0, 0, 1, 0, 0, 0, 1)), EPS);
	}

	for(unsigned a = 0; a < 100; ++a) {
		float angle = 8.0f * M_PI * (float)rand() / (float)RAND_MAX;

		Imath::Quatf q;
		q.setAxisAngle(Imath::V3f(1, 0, 0), angle);

		BOOST_REQUIRE_SMALL(compareMatrices(q.toMatrix33(), Imath::M33f(1, 0, 0, 0, cos(angle), sin(angle), 0,
		                                                                -sin(angle), cos(angle))),
		                    EPS);
	}

	for(unsigned a = 0; a < 100; ++a) {
		float angle = 8.0f * M_PI * (float)rand() / (float)RAND_MAX;

		Imath::Quatf q;
		q.setAxisAngle(Imath::V3f(0, 1, 0), angle);

		BOOST_REQUIRE_SMALL(compareMatrices(q.toMatrix33(), Imath::M33f(cos(angle), 0, -sin(angle), 0, 1, 0, sin(angle),
		                                                                0, cos(angle))),
		                    EPS);
	}
}


#include "polyscope/polyscope.h"

#include <igl/PI.h>
#include <igl/avg_edge_length.h>
#include <igl/barycenter.h>
#include <igl/boundary_loop.h>
#include <igl/exact_geodesic.h>
#include <igl/gaussian_curvature.h>
#include <igl/invert_diag.h>
#include <igl/lscm.h>
#include <igl/massmatrix.h>
#include <igl/per_vertex_normals.h>
#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/doublearea.h>
#include <igl/file_dialog_open.h>
#include <igl/file_dialog_save.h>
#include <igl/boundary_loop.h>
#include <igl/cotmatrix_entries.h>
#include <igl/triangle/triangulate.h>
#include <filesystem>
#include "polyscope/messages.h"
#include "polyscope/point_cloud.h"
#include "polyscope/surface_mesh.h"

#include <iostream>
#include <fstream>
#include <unordered_set>
#include <utility>
#include <Eigen/SparseQR>

#include "../../include/CommonTools.h"
#include "../../include/MeshLib/MeshConnectivity.h"
#include "../../include/MeshLib/MeshUpsampling.h"
#include "../../include/Visualization/PaintGeometry.h"
#include "../../include/InterpolationScheme/VecFieldSplit.h"
#include "../../include/Optimization/NewtonDescent.h"
#include "../../include/Optimization/LinearConstrainedSolver.h"
#include "../../include/IntrinsicFormula/InterpolateZvalsFromEdgeOmega.h"
#include "../../include/DynamicInterpolation/ComputeZandZdot.h"
#include "../../include/DynamicInterpolation/InterpolateKeyFrames.h"
//#include "../../include/IntrinsicFormula/ComputeZdotFromEdgeOmega.h"
#include "../../include/IntrinsicFormula/ComputeZdotFromHalfEdgeOmega.h"
//#include "../../include/IntrinsicFormula/IntrinsicKeyFrameInterpolationFromEdge.h"
#include "../../include/IntrinsicFormula/IntrinsicKeyFrameInterpolationFromHalfEdge.h"
#include "../../include/IntrinsicFormula/KnoppelStripePattern.h"

Eigen::MatrixXd triV, upsampledTriV;
Eigen::MatrixXi triF, upsampledTriF;
MeshConnectivity triMesh, upsampledTriMesh;
std::vector<std::pair<int, Eigen::Vector3d>> bary;

Eigen::MatrixXd sourceOmegaFields, tarOmegaFields;
Eigen::MatrixXd theoOmega, tarTheoOmega;

std::vector<std::complex<double>> sourceZvals, sourceTheoZVals, upsampledTheoZVals;
std::vector<std::complex<double>> tarZvals, tarTheoZVals, upsampledTarTheoZVals;
std::vector<Eigen::Vector2cd> sourceTheoGradZvals, tarTheoGradZvals;

std::vector<Eigen::MatrixXd> omegaList;
std::vector<Eigen::MatrixXd> theoOmegaList;
std::vector<std::vector<std::complex<double>>> zList;
std::vector<std::vector<std::complex<double>>> theoZList;

std::vector<Eigen::VectorXd> phaseFieldsList;
std::vector<Eigen::VectorXd> ampFieldsList;

std::vector<Eigen::VectorXd> theoPhaseFieldsList;
std::vector<Eigen::VectorXd> theoAmpFieldsList;

Eigen::MatrixXd dataV;
Eigen::MatrixXi dataF;
Eigen::MatrixXd dataVec;
Eigen::MatrixXd curColor;

int loopLevel = 1;
bool isFixedSource = true;
bool isFixedTar = true;

bool isForceOptimize = false;
bool isTwoTriangles = true;

PaintGeometry mPaint;

int numFrames = 2;
int curFrame = 0;

int singIndSource = 1, singIndSource1 = 1;
int singIndTar = 1, singIndTar1 = 1;
int numWavesSource = 2, numWaveTar = 4;

double globalAmpMax = 1;

double dragSpeed = 0.5;

double triarea = 0.04;

float vecratio = 0.1;

double sourceCenter1x = 0, sourceCenter1y = 0, sourceCenter2x = 0.8, sourceCenter2y = -0.2, targetCenter1x = 0, targetCenter1y = 0, targetCenter2x = 0.3, targetCenter2y = -0.7;
double sourceDirx = 1.0, sourceDiry = 0, targetDirx = 1, targetDiry = 0;

double gradTol = 1e-6;
double xTol = 0;
double fTol = 0;
int numIter = 1000;
int quadOrder = 4;

enum FunctionType {
	Whirlpool = 0,
	PlaneWave = 1,
	Summation = 2,
	YShape = 3,
	TwoWhirlPool = 4,
};

enum InitializationType{
  Random = 0,
  Linear = 1,
  Theoretical = 2
};

bool isUseUpMesh = false;

FunctionType functionType = FunctionType::PlaneWave;
FunctionType tarFunctionType = FunctionType::PlaneWave;
InitializationType initializationType = InitializationType::Linear;

void generateSquare(double length, double width, double triarea, Eigen::MatrixXd& irregularV, Eigen::MatrixXi& irregularF)
{
	double area = length * width;
	int N = (0.25 * std::sqrt(area / triarea));
	N = N > 1 ? N : 1;
	double deltaX = length / (4.0 * N);
	double deltaY = width / (4.0 * N);

	Eigen::MatrixXd planeV;
	Eigen::MatrixXi planeE;

//	planeV.resize(10, 2);
//	planeE.resize(10, 2);

//	for (int i = -2; i <= 2; i++)
//	{
//		planeV.row(i + 2) << length / 4.0 * i, -width / 2.0;
//	}
//
//	for (int i = 2; i >= -2; i--)
//	{
//		planeV.row(5 + 2 - i) << length / 4.0 * i, width / 2.0;
//	}
//
//	for (int i = 0; i < 10; i++)
//	{
//		planeE.row(i) << i, (i + 1) % 10;
//	}

	int M = 2 * N + 1;
	planeV.resize(4 * M - 4, 2);
	planeE.resize(4 * M - 4, 2);

	for (int i = 0; i < M; i++)
	{
	    planeV.row(i) << -length / 2, i * width / (M - 1) - width / 2;
	}
	for (int i = 1; i < M; i++)
	{
	    planeV.row(M - 1 + i) << i * length / (M - 1)-length / 2, width / 2;
	}
	for (int i = 1; i < M; i++)
	{
	    planeV.row(2 * (M - 1) + i) << length / 2, width/2 - i * width / (M - 1);
	}
	for (int i = 1; i < M - 1; i++)
	{
	    planeV.row(3 * (M - 1) + i) << length / 2- i * length / (M - 1), - width / 2;
	}

	for (int i = 0; i < 4 * (M - 1); i++)
	{
	    planeE.row(i) << i, (i + 1) % (4 * (M - 1));
	}

	Eigen::MatrixXd V2d;
	Eigen::MatrixXi F;
	Eigen::MatrixXi H(0, 2);
	std::cout << triarea << std::endl;
	// Create an output string stream
	std::ostringstream streamObj;
	//Add double to stream
	streamObj << triarea;
	const std::string flags = "q20a" + std::to_string(triarea);

	igl::triangle::triangulate(planeV, planeE, H, flags, V2d, F);

	if (isTwoTriangles)
	{
		/*V2d.resize(4, 3);
		V2d << -1, -1, 0,
			1, -1, 0,
			1, 1, 0,
			-1, 1, 0;

		F.resize(2, 3);
		F << 0, 1, 2,
			2, 3, 0;*/

		V2d.resize(3, 3);
		V2d << 0, 0, 0,
			1, 0, 0,
			0, 1, 0;

		F.resize(1, 3);
		F << 0, 1, 2;
	}

	irregularV.resize(V2d.rows(), 3);
	irregularV.setZero();
	irregularV.block(0, 0, irregularV.rows(), 2) = V2d.block(0, 0, irregularV.rows(), 2);
	irregularF = F;
	igl::writeOBJ("irregularPlane.obj", irregularV, irregularF);
}

void generateWhirlPool(double centerx, double centery, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, int pow = 1, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
	z.resize(triV.rows());
	w.resize(triV.rows(), 2);
	std::cout << "whirl pool center: " << centerx << ", " << centery << std::endl;
	bool isnegative = false;
	if(pow < 0)
	{
	    isnegative = true;
	    pow *= -1;
	}

	for (int i = 0; i < z.size(); i++)
	{
		double x = triV(i, 0) - centerx;
		double y = triV(i, 1) - centery;
		double rsquare = x * x + y * y;

		if(isnegative)
		{
		    z[i] = std::pow(std::complex<double>(x, -y), pow);

		    if (std::abs(std::sqrt(rsquare)) < 1e-10)
		        w.row(i) << 0, 0;
		    else
		        w.row(i) << pow * y / rsquare, -pow * x / rsquare;
		}
		else
		{
		    z[i] = std::pow(std::complex<double>(x, y), pow);

		    if (std::abs(std::sqrt(rsquare)) < 1e-10)
		        w.row(i) << 0, 0;
		    else
		        //			w.row(i) << -y / rsquare, x / rsquare;
		        w.row(i) << -pow * y / rsquare, pow * x / rsquare;
		}
	}

	if(upsampledZ)
	{
	    upsampledZ->resize(upsampledTriV.rows());
	    for(int i = 0; i < upsampledZ->size(); i++)
	    {
	        double x = upsampledTriV(i, 0) - centerx;
	        double y = upsampledTriV(i, 1) - centery;
	        double rsquare = x * x + y * y;

			upsampledZ->at(i) = std::pow(std::complex<double>(x, y), pow);
			if(isnegative)
			    upsampledZ->at(i) = std::pow(std::complex<double>(x, -y), pow);
	    }
	}
	if(gradZ)
	{
	    gradZ->resize(triV.rows());
	    for(int i = 0; i < gradZ->size(); i++)
	    {
	        double x = upsampledTriV(i, 0) - centerx;
	        double y = upsampledTriV(i, 1) - centery;

	        Eigen::Vector2cd tmpGrad;
	        tmpGrad << 1, std::complex<double>(0, 1);
	        if(isnegative)
	            tmpGrad(1) *= -1;

	        (*gradZ)[i] = std::pow(std::complex<double>(x, y), pow - 1) * tmpGrad;
	        (*gradZ)[i] *= pow;

	    }

	}
}

void generatePlaneWave(Eigen::Vector2d v, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    z.resize(triV.rows());
    w.resize(triV.rows(), 2);
    std::cout << "plane wave direction: " << v.transpose() << std::endl;

    for (int i = 0; i < z.size(); i++)
    {
        double theta = v.dot(triV.row(i).segment<2>(0));
        double x = std::cos(theta);
        double y = std::sin(theta);
        z[i] = std::complex<double>(x, y);
        w.row(i) = v;
    }

    if(upsampledZ)
    {
        upsampledZ->resize(upsampledTriV.rows());
        for(int i = 0; i < upsampledZ->size(); i++)
        {
            double theta = v.dot(upsampledTriV.row(i).segment<2>(0));
            double x = std::cos(theta);
            double y = std::sin(theta);
            upsampledZ->at(i) = std::complex<double>(x, y);
        }
    }
    if(gradZ)
    {
        gradZ->resize(triV.rows());
        for(int i = 0; i < gradZ->size(); i++)
        {
            double theta = v.dot(triV.row(i).segment<2>(0));
            double x = std::cos(theta);
            double y = std::sin(theta);
            std::complex<double> tmpZ = std::complex<double>(x, y);
            std::complex<double> I = std::complex<double>(0, 1);

            (*gradZ)[i] << I * tmpZ * v(0), I * tmpZ * v(1);
        }

    }
}

void generatePeriodicWave(int waveNum, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    Eigen::Vector2d v(2 * M_PI * waveNum, 0);
    generatePlaneWave(v, w, z, gradZ, upsampledZ);
}

void generateTwoWhirlPool(double centerx0, double centery0, double centerx1, double centery1, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, int n0 = 1, int n1 = 1, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>>* upsampledZ = NULL)
{
	Eigen::MatrixXd w0, w1;
	std::vector<Eigen::Vector2cd> gradZ0, gradZ1;
	std::vector<std::complex<double>> z0, z1, upsampledZ0, upsampledZ1;

	generateWhirlPool(centerx0, centery0, w0, z0, n0, gradZ ? &gradZ0 : NULL, upsampledZ ? &upsampledZ0 : NULL);
	generateWhirlPool(centerx1, centery1, w1, z1, n1, gradZ ? &gradZ1 : NULL, upsampledZ ? &upsampledZ1 : NULL);

	std::cout << "whirl pool center: " << centerx0 << ", " << centery0 << std::endl;
	std::cout << "whirl pool center: " << centerx1 << ", " << centery1 << std::endl;

	z.resize(triV.rows());
	w.resize(triV.rows(), 2);

	w = w0 + w1;

	for (int i = 0; i < z.size(); i++)
	{
		z[i] = z0[i] * z1[i];
	}

	if (upsampledZ)
	{
		upsampledZ->resize(upsampledTriV.rows());
		for (int i = 0; i < upsampledZ->size(); i++)
		{
			upsampledZ->at(i) = upsampledZ0[i] * upsampledZ1[i];
		}
	}

	if(gradZ)
	{
	    *gradZ = gradZ0;
	    for(int i = 0; i < gradZ0.size(); i++)
	    {
	        (*gradZ)[i] = z0[i] * gradZ1[i] + z1[i] * gradZ0[i];
	    }
	}
}

void generatePlaneSumWhirl(double centerx, double centery, Eigen::Vector2d v, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, int pow = 1, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    z.resize(triV.rows());
    w.resize(triV.rows(), 2);
    std::cout << "whirl pool center: " << centerx << ", " << centery << std::endl;
    std::cout << "plane wave direction: " << v.transpose() << std::endl;

    std::vector<Eigen::Vector2cd> gradWZ, gradPZ;
    Eigen::MatrixXd whw, plw;
    std::vector<std::complex<double>> wz, pz, upWz, upPz;

    generatePlaneWave(v, plw, pz, gradZ ? &gradPZ : NULL, upsampledZ ? &upPz : NULL);
    generateWhirlPool(centerx, centery, whw, wz, pow, gradZ? &gradWZ : NULL, upsampledZ? &upWz : NULL);

    for (int i = 0; i < z.size(); i++)
    {
        z[i] = pz[i] * wz[i];
        w = plw + whw;
    }

    if(upsampledZ)
    {
        upsampledZ->resize(upsampledTriV.rows());

        for(int i = 0; i < upsampledZ->size(); i++)
        {
            (*upsampledZ)[i] = upPz[i] * upWz[i];
        }
    }

    if(gradZ)
    {
        *gradZ = gradWZ;
        for(int i = 0; i < gradWZ.size(); i++)
        {
            (*gradZ)[i] = pz[i] * gradWZ[i] + wz[i] * gradPZ[i];
        }
    }

}

void generateYshape(Eigen::Vector2d w1, Eigen::Vector2d w2, Eigen::MatrixXd &w, std::vector<std::complex<double>> &z, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    z.resize(triV.rows());
    w.resize(triV.rows(), 2);

    if(gradZ)
        gradZ->resize(triV.rows());

    std::cout << "w1: " << w1.transpose() << std::endl;
    std::cout << "w2: " << w2.transpose() << std::endl;

    Eigen::MatrixXd pw1, pw2;
    std::vector<std::complex<double>> pz1, pz2;
    std::vector<Eigen::Vector2cd> gradPZ1, gradPZ2;
    std::vector<std::complex<double>> upsampledPZ1, upsampledPZ2;

    generatePlaneWave(w1, pw1, pz1, gradZ ? & gradPZ1 : NULL, upsampledZ? &upsampledPZ1 : NULL);
    generatePlaneWave(w2, pw2, pz2, gradZ ? & gradPZ2 : NULL, upsampledZ? &upsampledPZ2 : NULL);

    double ymax = triV.col(1).maxCoeff();
    double ymin = triV.col(1).minCoeff();

    for (int i = 0; i < z.size(); i++)
    {

        double weight = (triV(i, 1) - triV.col(1).minCoeff()) / (triV.col(1).maxCoeff() - triV.col(1).minCoeff());
        z[i] = (1 - weight) * pz1[i] + weight * pz2[i];
        Eigen::Vector2cd dz = (1 - weight) * gradPZ1[i] + weight * gradPZ2[i];
        if(gradZ)
            (*gradZ)[i] = dz;

        double wx = 0;
        double wy = 1 / (ymax - ymin);

        w.row(i) = (std::conj(z[i]) * dz).imag() / (std::abs(z[i]) * std::abs(z[i]));
    }

    if(upsampledZ)
    {
        upsampledZ->resize(upsampledTriV.rows());
        for(int i = 0; i < upsampledZ->size(); i++)
        {
            double theta = w1.dot(upsampledTriV.row(i).segment<2>(0));
            double x = std::cos(theta);
            double y = std::sin(theta);
            std::complex<double> z1 = std::complex<double>(x, y);

            theta = w2.dot(upsampledTriV.row(i).segment<2>(0));
            x = std::cos(theta);
            y = std::sin(theta);
            std::complex<double> z2 = std::complex<double>(x, y);

            double weight = (upsampledTriV(i, 1) - upsampledTriV.col(1).minCoeff()) / (upsampledTriV.col(1).maxCoeff() - upsampledTriV.col(1).minCoeff());
            upsampledZ->at(i) = (1 - weight) * z1 + weight * z2;
        }
    }
}

void initialization()
{
	generateSquare(2.0, 2.0, triarea, triV, triF);

	Eigen::SparseMatrix<double> S;
	std::vector<int> facemap;

	meshUpSampling(triV, triF, upsampledTriV, upsampledTriF, loopLevel, &S, &facemap, &bary);
	std::cout << "upsampling finished" << std::endl;

	triMesh = MeshConnectivity(triF);
	upsampledTriMesh = MeshConnectivity(upsampledTriF);

    std::cout << "nverts: " << triV.rows() << ", nedges: " << triMesh.nEdges() << std::endl;
}

void generateValues(FunctionType funType, Eigen::MatrixXd &vecFields, std::vector<std::complex<double>> &zvalues, std::vector<Eigen::Vector2cd> &gradZvals, std::vector<std::complex<double>> &upZvals, int singularityInd1 = 1, int singularityInd2 = 1, bool isFixedGenerator = false, double fixedx = 0, double fixedy = 0, Eigen::Vector2d fixedv = Eigen::Vector2d::Constant(1.0))
{
	Eigen::MatrixXd vertFields;
	if (funType == FunctionType::Whirlpool)
	{
		Eigen::Vector2d center = Eigen::Vector2d::Random();
		if (isFixedGenerator)
			center << fixedx, fixedy;
		generateWhirlPool(center(0), center(1), vertFields, zvalues, singularityInd1, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::PlaneWave)
	{
		Eigen::Vector2d v = Eigen::Vector2d::Random();
		if (isFixedGenerator)
			v = fixedv;
		generatePlaneWave(v, vertFields, zvalues, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::Summation)
	{
		Eigen::Vector2d center = Eigen::Vector2d::Random();
		Eigen::Vector2d v = Eigen::Vector2d::Random();
		if (isFixedGenerator)
		{
			v = fixedv;
			center << fixedx, fixedy;
		}
		generatePlaneSumWhirl(center(0), center(1), v, vertFields, zvalues, singularityInd1, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::YShape)
	{
		Eigen::Vector2d w1(1, 0);
		Eigen::Vector2d w2(1, 0);

		w1(0) = 2 * 3.1415926;
		w2(0) = 4 * 3.1415926;
		generateYshape(w1, w2, vertFields, zvalues, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::TwoWhirlPool)
	{
		Eigen::Vector2d center0 = Eigen::Vector2d::Random();
		Eigen::Vector2d center1 = Eigen::Vector2d::Random();
		if (isFixedGenerator)
		{
			center0 << fixedx, fixedy;
			center1 << 0.8, -0.3;
		}
		generateTwoWhirlPool(center0(0), center0(1), center1(0), center1(1), vertFields, zvalues, singularityInd1, singularityInd2, &gradZvals, &upZvals);
	}
	Eigen::MatrixXd vertFields3D(triV.rows(), 3);
	vertFields3D.block(0, 0, triV.rows(), 2) = vertFields;
	vertFields3D.col(2).setZero();

	vecFields = vertexVec2IntrinsicHalfEdgeVec(vertFields3D, triV, MeshConnectivity(triF));
}

// TODO: remove this
Eigen::MatrixXd vertexVec2IntrinsicHalfEdgeVec2D(const Eigen::MatrixXd& v, const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, std::vector<Eigen::Triplet<double>> &T)
{
    int nedges = mesh.nEdges();
    Eigen::MatrixXd edgeOmega(nedges, 2);
    T.clear();

    for (int i = 0; i < nedges; i++)
    {
        int vid0 = mesh.edgeVertex(i, 0);
        int vid1 = mesh.edgeVertex(i, 1);

        Eigen::Vector2d e = (pos.row(vid1) - pos.row(vid0)).segment(0, 2);
        edgeOmega(i, 0) = v.row(vid0).dot(e);
        edgeOmega(i, 1) = -v.row(vid1).dot(e);

        T.push_back({2 * i, 2 * vid0, e(0)});
        T.push_back({2 * i, 2 * vid0 + 1, e(1)});

        T.push_back({2 * i + 1, 2 * vid1, -e(0)});
        T.push_back({2 * i + 1, 2 * vid1 + 1, -e(1)});
    }
    return edgeOmega;
}

void solveKeyFrames(const Eigen::MatrixXd& sourceVec, const Eigen::MatrixXd& tarVec, const std::vector<std::complex<double>>& sourceZvals, const std::vector<std::complex<double>>& tarZvals, const int numKeyFrames, std::vector<Eigen::MatrixXd>& wFrames, std::vector<std::vector<std::complex<double>>>& zFrames)
{
	Eigen::VectorXd faceArea;
	igl::doublearea(triV, triF, faceArea);
	faceArea /= 2;
	IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge interpModel = IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge(MeshConnectivity(triF), faceArea, numFrames, quadOrder, sourceZvals, sourceVec, tarZvals, tarVec);
	Eigen::VectorXd x;
	interpModel.convertList2Variable(x);        // linear initialization

    std::vector<std::complex<double>> testzvals;
    Eigen::MatrixXd cotEntries;
    igl::cotmatrix_entries(triV, triF, cotEntries);
    IntrinsicFormula::roundVertexZvalsFromHalfEdgeOmega(triMesh, sourceVec, faceArea, cotEntries, triV.rows(), testzvals);
    IntrinsicFormula::testRoundingEnergy(triMesh, sourceVec, faceArea, cotEntries, triV.rows(), testzvals);
	for (auto& z : testzvals)
	{
		Eigen::Vector2d rndvec;
		rndvec.setRandom();
		z = std::complex<double>(rndvec(0), rndvec(1));
	}
	IntrinsicFormula::testRoundingEnergy(triMesh, sourceVec, faceArea, cotEntries, triV.rows(), testzvals);

	//interpModel.testEnergy(x);
	//		std::cout << "starting energy: " << interpModel.computeEnergy(x) << std::endl;
	if (initializationType == InitializationType::Theoretical)
	{
		interpModel.setwzLists(theoZList, theoOmegaList);
		interpModel.convertList2Variable(x);
	}
	else if (initializationType == InitializationType::Random)
	{
		x.setRandom();
		interpModel.convertVariable2List(x);
		interpModel.convertList2Variable(x);
	}
	else
	{
		// do nothing, since it is initialized as the linear interpolation.
	}

	auto initWFrames = interpModel.getWList();
	auto initZFrames = interpModel.getVertValsList();
	if (isForceOptimize)
	{
		auto funVal = [&](const Eigen::VectorXd& x, Eigen::VectorXd* grad, Eigen::SparseMatrix<double>* hess, bool isProj) {
			Eigen::VectorXd deriv;
			Eigen::SparseMatrix<double> H;
			double E = interpModel.computeEnergy(x, grad ? &deriv : NULL, hess ? &H : NULL, isProj);

			if (grad)
			{
				(*grad) = deriv;
			}

			if (hess)
			{
				(*hess) = H;
			}

			return E;
		};
		auto maxStep = [&](const Eigen::VectorXd& x, const Eigen::VectorXd& dir) {
			return 1.0;
		};

		auto getVecNorm = [&](const Eigen::VectorXd& x, double& znorm, double& wnorm) {
			interpModel.getComponentNorm(x, znorm, wnorm);
		};

		OptSolver::testFuncGradHessian(funVal, x);

		auto x0 = x;
		OptSolver::newtonSolver(funVal, maxStep, x, numIter, gradTol, xTol, fTol, true, getVecNorm);
		std::cout << "before optimization: " << x0.norm() << ", after optimization: " << x.norm() << ", difference: " << (x - x0).norm() << std::endl;
		std::cout << "x norm: " << x.norm() << std::endl;

		/*Eigen::VectorXd deriv;
		Eigen::SparseMatrix<double> H;
		double E = interpModel.computeEnergy(x, &deriv, &H, false);

		std::cout << deriv.norm() << std::endl;*/
		//std::cout << "hessian: \n" << H.toDense() << std::endl;

		/*Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
		es.compute(H.toDense());
		Eigen::VectorXd evals = es.eigenvalues();
		std::cout << "evals: " << evals.transpose() << std::endl;*/

	}
	interpModel.convertVariable2List(x);

	wFrames = interpModel.getWList();
	zFrames = interpModel.getVertValsList();

	for (int i = 0; i < wFrames.size() - 1; i++)
	{
		double zdotNorm = interpModel._zdotModel.computeZdotIntegration(zFrames[i], wFrames[i], zFrames[i + 1], wFrames[i + 1], NULL, NULL);

		double initZdotNorm = interpModel._zdotModel.computeZdotIntegration(initZFrames[i], initWFrames[i], initZFrames[i + 1], initWFrames[i + 1], NULL, NULL);

		std::cout << "frame " << i << ", before optimization: ||zdot||^2: " << initZdotNorm << ", after optimization, ||zdot||^2 = " << zdotNorm << std::endl;
	}

}

void updateMagnitudePhase(const std::vector<Eigen::MatrixXd>& wFrames, const std::vector<std::vector<std::complex<double>>>& zFrames, std::vector<Eigen::VectorXd>& magList, std::vector<Eigen::VectorXd>& phaseList)
{
	std::vector<std::vector<std::complex<double>>> interpZList(wFrames.size());
	magList.resize(wFrames.size());
	phaseList.resize(wFrames.size());

	MeshConnectivity mesh(triF);

	auto computeMagPhase = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			interpZList[i] = IntrinsicFormula::upsamplingZvals(mesh, zFrames[i], wFrames[i], bary);
			magList[i].setZero(interpZList[i].size());
			phaseList[i].setZero(interpZList[i].size());

			for (int j = 0; j < magList[i].size(); j++)
			{
				magList[i](j) = std::abs(interpZList[i][j]);
				phaseList[i](j) = std::arg(interpZList[i][j]);
			}
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)interpZList.size(), GRAIN_SIZE);
	tbb::parallel_for(rangex, computeMagPhase);
}

void updateTheoMagnitudePhase(const std::vector<std::complex<double>>& sourceZvals, const std::vector<std::complex<double>>& tarZvals,
                              const std::vector<Eigen::Vector2cd>& sourceGradZvals, const std::vector<Eigen::Vector2cd>& tarGradZvals,
                              const std::vector<std::complex<double>>& upSourceZvals, const std::vector<std::complex<double>>& upTarZvals,
                              const int num, std::vector<Eigen::VectorXd>& magList, std::vector<Eigen::VectorXd>& phaseList,    // upsampled information
                              std::vector<Eigen::MatrixXd>& wList, std::vector<std::vector<std::complex<double>>> &zvalList        // raw information
                              )
{
	magList.resize(num + 2);
	phaseList.resize(num + 2);
	wList.resize(num + 2);
	zvalList.resize(num + 2);

	double dt = 1.0 / (num + 1);

	auto computeMagPhase = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			double w = i * dt;
			magList[i].setZero(upSourceZvals.size());
			phaseList[i].setZero(upSourceZvals.size());
			for (int j = 0; j < upSourceZvals.size(); j++)
			{
			    std::complex<double> z = (1 - w) * upSourceZvals[j] + w * upTarZvals[j];
			    magList[i][j] = std::abs(z);
			    phaseList[i][j] = std::arg(z);
			}
			Eigen::MatrixXd vertOmega = Eigen::MatrixXd::Zero(sourceZvals.size(), 3);
			zvalList[i].resize(sourceZvals.size());

			for(int j = 0; j < sourceZvals.size(); j++)
			{
			    Eigen::Vector2cd gradf = (1 - w) * sourceGradZvals[j] + w * tarGradZvals[j];
			    std::complex<double> fbar = (1 - w) * std::conj(sourceZvals[j]) + w * std::conj(tarZvals[j]);

				vertOmega.row(j).segment<2>(0) = ((gradf * fbar) / (std::abs(fbar) * std::abs(fbar))).imag();
			    zvalList[i][j] = (1 - w) * sourceZvals[j] + w * tarZvals[j];
			}
			wList[i] = vertexVec2IntrinsicHalfEdgeVec(vertOmega, triV, triMesh);
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)(num + 2), GRAIN_SIZE);
	tbb::parallel_for(rangex, computeMagPhase);
}

void registerMeshByPart(const Eigen::MatrixXd& basePos, const Eigen::MatrixXi& baseF,
	const Eigen::MatrixXd& upPos, const Eigen::MatrixXi& upF, const double& shifty, const double& ampMax,
	Eigen::VectorXd ampVec, const Eigen::VectorXd& phaseVec,
	Eigen::VectorXd theoAmpVec, const Eigen::VectorXd& theoPhaseVec,
	Eigen::MatrixXd& renderV, Eigen::MatrixXi& renderF, Eigen::MatrixXd& renderColor)
{
	int nverts = basePos.rows();
	int nfaces = baseF.rows();

	int nupverts = upPos.rows();
	int nupfaces = upF.rows();

	int ndataVerts = nverts + 4 * nupverts;
	int ndataFaces = nfaces + 4 * nupfaces;

	renderV.resize(ndataVerts, 3);
	renderF.resize(ndataFaces, 3);
	renderColor.setZero(ndataVerts, 3);

	renderColor.col(0).setConstant(1.0);
	renderColor.col(1).setConstant(1.0);
	renderColor.col(2).setConstant(1.0);

	int curVerts = 0;
	int curFaces = 0;

	Eigen::VectorXd normalizedAmp = ampVec / ampMax, normalizedTheoAmp = theoAmpVec / ampMax;

	Eigen::MatrixXd shiftV = basePos;
	shiftV.col(0).setConstant(0);
	shiftV.col(1).setConstant(shifty);
	shiftV.col(2).setConstant(0);

	renderV.block(0, 0, nverts, 3) = basePos - shiftV;
	renderF.block(0, 0, nfaces, 3) = baseF;
	curVerts += nverts;
	curFaces += nfaces;

	double shiftx = 1.5 * (basePos.col(0).maxCoeff() - basePos.col(0).minCoeff());

	shiftV = upPos;
	shiftV.col(0).setConstant(shiftx);
	shiftV.col(1).setConstant(shifty);
	shiftV.col(2).setConstant(0);

	Eigen::MatrixXi shiftF = upF;
	shiftF.setConstant(curVerts);

	// interpolated phase
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	Eigen::MatrixXd phiColor = mPaint.paintPhi(phaseVec);
	renderColor.block(curVerts, 0, nupverts, 3) = phiColor;

	curVerts += nupverts;
	curFaces += nupfaces;

	// interpolated amp
	shiftF.setConstant(curVerts);
	shiftV.col(0).setConstant(2 * shiftx);
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	Eigen::MatrixXd ampColor = mPaint.paintAmplitude(ampVec / globalAmpMax);
	renderColor.block(curVerts, 0, nupverts, 3) = ampColor;

	curVerts += nupverts;
	curFaces += nupfaces;

	// theoretical phase
	shiftF.setConstant(curVerts);
	shiftV.col(0).setConstant(3 * shiftx);
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	phiColor = mPaint.paintPhi(theoPhaseVec);
	renderColor.block(curVerts, 0, nupverts, 3) = phiColor;

	curVerts += nupverts;
	curFaces += nupfaces;

	// theoretical amp
	shiftF.setConstant(curVerts);
	shiftV.col(0).setConstant(4 * shiftx);
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	ampColor = mPaint.paintAmplitude(theoAmpVec / globalAmpMax);
	renderColor.block(curVerts, 0, nupverts, 3) = ampColor;

}

void registerMesh(int frameId)
{
	Eigen::MatrixXd sourceP, tarP, interpP;
	Eigen::MatrixXi sourceF, tarF, interpF;
	Eigen::MatrixXd sourceColor, tarColor, interpColor;

	double shiftx = 1.5 * (triV.col(0).maxCoeff() - triV.col(0).minCoeff());
	double shifty = 1.5 * (triV.col(1).maxCoeff() - triV.col(1).minCoeff());
	int totalfames = ampFieldsList.size();
	registerMeshByPart(triV, triF, upsampledTriV, upsampledTriF, 0, globalAmpMax, ampFieldsList[0], phaseFieldsList[0], theoAmpFieldsList[0], theoPhaseFieldsList[0], sourceP, sourceF, sourceColor);
	registerMeshByPart(triV, triF, upsampledTriV, upsampledTriF, shifty, globalAmpMax, ampFieldsList[totalfames - 1], phaseFieldsList[totalfames - 1], theoAmpFieldsList[totalfames - 1], theoPhaseFieldsList[totalfames - 1], tarP, tarF, tarColor);
	registerMeshByPart(triV, triF, upsampledTriV, upsampledTriF, 2 * shifty, globalAmpMax, ampFieldsList[frameId], phaseFieldsList[frameId], theoAmpFieldsList[frameId], theoPhaseFieldsList[frameId], interpP, interpF, interpColor);

	Eigen::MatrixXi shifF = sourceF;

	int nPartVerts = sourceP.rows();
	int nPartFaces = sourceF.rows();
	int nverts = triV.rows();
	int nfaces = triF.rows();

	dataV.setZero(3 * nPartVerts + nverts, 3);
	curColor.setZero(3 * nPartVerts + nverts, 3);
	dataF.setZero(3 * nPartFaces + nfaces, 3);

	shifF.setConstant(nPartVerts);

	dataV.block(0, 0, nPartVerts, 3) = sourceP;
	curColor.block(0, 0, nPartVerts, 3) = sourceColor;
	dataF.block(0, 0, nPartFaces, 3) = sourceF;

	dataV.block(nPartVerts, 0, nPartVerts, 3) = tarP;
	curColor.block(nPartVerts, 0, nPartVerts, 3) = tarColor;
	dataF.block(nPartFaces, 0, nPartFaces, 3) = tarF + shifF;

	dataV.block(nPartVerts * 2, 0, nPartVerts, 3) = interpP;
	curColor.block(nPartVerts * 2, 0, nPartVerts, 3) = interpColor;
	dataF.block(nPartFaces * 2, 0, nPartFaces, 3) = interpF + 2 * shifF;

	Eigen::MatrixXd shiftV = triV;
	shiftV.col(0).setConstant(shiftx);
	shiftV.col(1).setConstant(-2 * shifty);
	shiftV.col(2).setZero();

	shifF = triF;
	shifF.setConstant(3 * nPartVerts);

	dataV.block(nPartVerts * 3, 0, nverts, 3) = triV + shiftV;
	curColor.block(nPartVerts * 3, 0, nverts, 3).setConstant(1.0);
	dataF.block(nPartFaces * 3, 0, nfaces, 3) = triF + shifF;

	polyscope::registerSurfaceMesh("input mesh", dataV, dataF);

}

void updateFieldsInView(int frameId)
{
	registerMesh(frameId);
	polyscope::getSurfaceMesh("input mesh")->addVertexColorQuantity("VertexColor", curColor);
	polyscope::getSurfaceMesh("input mesh")->getQuantity("VertexColor")->setEnabled(true);

	/*polyscope::getSurfaceMesh("input mesh")->addVertexVectorQuantity("vertex vector field", dataVec * vecratio, polyscope::VectorType::AMBIENT);
	polyscope::getSurfaceMesh("input mesh")->getQuantity("vertex vector field")->setEnabled(true);*/
}

void callback() {
	ImGui::PushItemWidth(100);
	if (ImGui::Button("Reset", ImVec2(-1, 0)))
	{
		curFrame = 0;
		updateFieldsInView(curFrame);
	}

	if (ImGui::InputDouble("triangle area", &triarea))
	{
		if (triarea > 0)
			initialization();
	}
	if (ImGui::Checkbox("Two Triangle Mesh", &isTwoTriangles))
	{
		initialization();
	}
	if (ImGui::InputInt("upsampled times", &loopLevel))
	{
		if (loopLevel >= 0)
			initialization();
	}

	if (ImGui::CollapsingHeader("source Vector Fields Info", ImGuiTreeNodeFlags_DefaultOpen))
	{
		if (ImGui::Combo("source vec types", (int*)&functionType, "Whirl pool\0plane wave\0sum\0Y shape\0Two Whirl Pool\0Periodic\0\0")) {}
		if (ImGui::Checkbox("Fixed source center and dir", &isFixedSource)) {}

		if (ImGui::CollapsingHeader("source whirl  pool Info"))
		{
		    if (ImGui::InputInt("source singularity index 1", &singIndSource)){}
		    if (ImGui::InputDouble("source center 1 x: ", &sourceCenter1x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("source center 1 y: ", &sourceCenter1y)) {}

		    if (ImGui::InputInt("source singularity index 2", &singIndSource1)){}
		    if (ImGui::InputDouble("source center 2 x: ", &sourceCenter2x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("source center 2 y: ", &sourceCenter2y)) {}
		}

		if (ImGui::CollapsingHeader("source plane wave Info"))
		{
		    if (ImGui::InputInt("source num waves", &numWavesSource)){}
		    if (ImGui::InputDouble("source dir x: ", &sourceDirx)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("source dir y: ", &sourceDiry)) {}
		}

	}
	if (ImGui::CollapsingHeader("target Vector Fields Info", ImGuiTreeNodeFlags_DefaultOpen))
	{
		if (ImGui::Combo("target vec types", (int*)&tarFunctionType, "Whirl pool\0plane wave\0sum\0Y shape\0Two Whirl Pool\0Periodic\0\0")) {}

		if (ImGui::Checkbox("Fixed target center and dir", &isFixedTar)) {}
		if (ImGui::CollapsingHeader("target whirl  pool Info"))
		{
		    if (ImGui::InputInt("target singularity index 1", &singIndTar)) {}
		    if (ImGui::InputDouble("target center 1 x: ", &targetCenter1x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("target center 1 y: ", &targetCenter1y)) {}

		    if (ImGui::InputInt("target singularity index 2", &singIndTar1)) {}
		    if (ImGui::InputDouble("target center 2 x: ", &targetCenter2x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("target center 2 y: ", &targetCenter2y)) {}
		}

		if (ImGui::CollapsingHeader("target plane wave Info"))
		{
		    if (ImGui::InputInt("target num waves", &numWaveTar)){}
		    if (ImGui::InputDouble("target dir x: ", &sourceDirx)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("target dir y: ", &sourceDiry)) {}
		}

	}

	if (ImGui::InputInt("num of frames", &numFrames))
	{
		if (numFrames <= 0)
			numFrames = 10;
	}

	if (ImGui::InputDouble("drag speed", &dragSpeed))
	{
		if (dragSpeed <= 0)
			dragSpeed = 0.5;
	}

	if (ImGui::DragInt("current frame", &curFrame, dragSpeed, 0, numFrames + 1))
	{
		updateFieldsInView(curFrame);
	}
	if (ImGui::DragFloat("vec ratio", &(vecratio), 0.005, 0, 1))
	{
		updateFieldsInView(curFrame);
	}
	if (ImGui::CollapsingHeader("optimzation parameters", ImGuiTreeNodeFlags_DefaultOpen))
	{
		if (ImGui::InputInt("num iterations", &numIter))
		{
			if (numIter < 0)
				numIter = 1000;
		}
		if (ImGui::InputDouble("grad tol", &gradTol))
		{
			if (gradTol < 0)
				gradTol = 1e-6;
		}
		if (ImGui::InputDouble("x tol", &xTol))
		{
			if (xTol < 0)
				xTol = 0;
		}
		if (ImGui::InputDouble("f tol", &fTol))
		{
			if (fTol < 0)
				fTol = 0;
		}
		if (ImGui::InputInt("quad order", &quadOrder))
		{
		    if (quadOrder <= 0 || quadOrder > 20)
		        quadOrder = 4;
		}
		ImGui::Checkbox("use upsampled mesh", &isUseUpMesh);

	}
	if (ImGui::Combo("initialization types", (int*)&initializationType, "Random\0Linear\0Theoretical\0")) {}

	ImGui::Checkbox("Try Optimization", &isForceOptimize);

	if (ImGui::Button("update values", ImVec2(-1, 0)))
	{
		double fixedx = 0;
		double fixedy = 0;
		Eigen::Vector2d fixedv(1, 0);
		// source vector fields
		if(isFixedSource)
		{
		    fixedx = sourceCenter1x;
		    fixedy = sourceCenter1y;
		    fixedv << sourceDirx, sourceDiry;
		    fixedv *= numWavesSource * 2 * M_PI;
		}
		generateValues(functionType, sourceOmegaFields, sourceZvals, sourceTheoGradZvals, upsampledTheoZVals, singIndSource, singIndSource1, isFixedSource, fixedx, fixedy, fixedv);
		/*std::cout << "triV: \n" << triV << std::endl;

		std::cout << fixedv.transpose() << std::endl;
		for (int i = 0; i < sourceOmegaFields.rows(); i++)
		{
			std::cout << "edge id: " << i << ", vid: " << triMesh.edgeVertex(i, 0) << ", zval: " << sourceZvals[triMesh.edgeVertex(i, 0)] << ",  " << triMesh.edgeVertex(i, 1) << " " << sourceZvals[triMesh.edgeVertex(i, 1)] << ", w: " << sourceOmegaFields(i) << std::endl;
		}*/

		if(isFixedTar)
		{
		   fixedx = targetCenter1x;
		   fixedy = targetCenter1y;
		   fixedv << targetDirx, targetDiry;
		   fixedv *= numWaveTar * 2 * M_PI;
		}
		// target vector fields
		generateValues(tarFunctionType, tarOmegaFields, tarZvals, tarTheoGradZvals, upsampledTarTheoZVals, singIndTar, singIndTar1, isFixedTar, fixedx, fixedy, fixedv);

		// update the theoretic ones
		updateTheoMagnitudePhase(sourceZvals, tarZvals, sourceTheoGradZvals, tarTheoGradZvals, upsampledTheoZVals, upsampledTarTheoZVals, numFrames, theoAmpFieldsList, theoPhaseFieldsList, theoOmegaList, theoZList);

		// solve for the path from source to target
		solveKeyFrames(sourceOmegaFields, tarOmegaFields, sourceZvals, tarZvals, numFrames, omegaList, zList);
		// get interploated amp and phase frames
		updateMagnitudePhase(omegaList, zList, ampFieldsList, phaseFieldsList);

		// update amp max
		if (theoAmpFieldsList.size() > 0)
		{
			globalAmpMax = theoAmpFieldsList[0].maxCoeff();
			for (int i = 0; i < theoAmpFieldsList.size(); i++)
			{
				globalAmpMax = std::max(theoAmpFieldsList[i].maxCoeff(), globalAmpMax);
				globalAmpMax = std::max(ampFieldsList[i].maxCoeff(), globalAmpMax);
			}
		}

		updateFieldsInView(curFrame);

	}

	ImGui::PopItemWidth();
}

int main(int argc, char** argv)
{
	initialization();
//
//	Eigen::MatrixXd testV(3, 3);
//	testV << 0, 0, 0,
//		1, 0, 0,
//		0, 1, 0;
//	Eigen::MatrixXi testF(1, 3);
//	testF << 0, 1, 2;
//
//	MeshConnectivity testMesh(testF);
//
//	Eigen::MatrixXd testw(3, 3), testw1;
//	testw << 1, -1, 0,
//		1, 2, 0,
//		-1, 0, 0;
//
//	testw1 = testw;
//	testw1.block<3, 2>(0, 0).setRandom();
//
//	Eigen::Vector3d testbary;
//	std::vector<std::complex<double>> testZvals(3), testZvals1(3);
//	testZvals[0] = std::complex<double>(0.1, 0.8);
//	testZvals[1] = std::complex<double>(0.7, 0.3);
//	testZvals[2] = std::complex<double>(0.34, 7.8);
//
//	testZvals1[0] = std::complex<double>(0.5, 0.6);
//	testZvals1[1] = std::complex<double>(0.7, 0.4);
//	testZvals1[2] = std::complex<double>(0.4, 0.8);
//
//
//	Eigen::Matrix<double, 3, 2> edgew = vertexVec2IntrinsicHalfEdgeVec(testw, testV, testMesh);
//	Eigen::Matrix<double, 3, 2> edgew1 = vertexVec2IntrinsicHalfEdgeVec(testw1, testV, testMesh);
//
//	for (int i = 0; i < 3; i++)
//	{
//	    int eid = testMesh.faceEdge(0, i);
//		std::cout << "eid: " << eid << ", v0: " << testMesh.edgeVertex(eid, 0) << ", v1: " << testMesh.edgeVertex(eid, 1) << std::endl;
//		std::cout << "edge w: " << edgew.row(eid) << std::endl;
//	}
//	int testqid = 3;
//	ComputeZandZdot preModel(testV, testF, 4);
//	testbary << 1 - preModel.getQuadPts()[testqid].u - preModel.getQuadPts()[testqid].v, preModel.getQuadPts()[testqid].u, preModel.getQuadPts()[testqid].v;
//
//	Eigen::VectorXd testFaceArea;
//	igl::doublearea(testV, testF, testFaceArea);
//	testFaceArea /= 2;
//	IntrinsicFormula::ComputeZdotFromHalfEdgeOmega zdotmodel1(MeshConnectivity(testF), testFaceArea, 4, 0.1);
//
//	double zdotnorm = zdotmodel1.computeZdotIntegration(testZvals, edgew, testZvals1, edgew1, NULL, NULL);
//	double zdotnorm1 = preModel.zDotSquareIntegration(testw.block<3, 2>(0, 0), testw1.block<3, 2>(0, 0), testZvals, testZvals1, 0.1, NULL, NULL);
//
//	std::cout << "zdot: " << zdotnorm << ", " << "zdot1: " << zdotnorm1 << std::endl;
//
//	IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge halfEdgeFormula(testMesh, testFaceArea, 50, 4, testZvals, edgew, testZvals1, edgew1);
//	Eigen::VectorXd x;
//	halfEdgeFormula.convertList2Variable(x);
//	zdotnorm = halfEdgeFormula.computeEnergy(x);
//
//	Eigen::MatrixXd uptestV;
//	Eigen::MatrixXi uptestF;
//
//	std::vector<std::pair<int, Eigen::Vector3d>> testBary;
//    meshUpSampling(testV, testF, uptestV, uptestF, 2, NULL, NULL, &testBary);
//
//    InterpolateKeyFrames interpModelTest(testV, testF, uptestV, uptestF, testBary, testw.block<3, 2>(0, 0), testw1.block<3, 2>(0, 0), testZvals, testZvals1, 50, 4, false);
//    interpModelTest.convertList2Variable(x);
//    zdotnorm1 = interpModelTest.computeEnergy(x);

//    std::cout << "zdot: " << zdotnorm << ", " << "zdot1: " << zdotnorm1 << std::endl;
//	IntrinsicFormula::testZvalsFromHalfEdgeOmega(testbary, testZvals, edgew);
//	Eigen::VectorXd faceArea;
//
//	igl::doublearea(triV, triF, faceArea);
//	std::vector<std::complex<double>> testZvals0(triV.rows()), testZvals1(triV.rows());
//	for (int i = 0; i < triV.rows(); i++)
//	{
//		Eigen::Vector2d randvec;
//		randvec.setRandom();
//		testZvals0[i] = std::complex<double>(randvec(0), randvec(1));
//
//		randvec.setRandom();
//		testZvals1[i] = std::complex<double>(randvec(0), randvec(1));
//	}
//	Eigen::MatrixXd edgew0, edgew1;
//	edgew0.setRandom(triMesh.nEdges(), 2);
//	edgew1.setRandom(triMesh.nEdges(), 2);
//
//	IntrinsicFormula::ComputeZdotFromHalfEdgeOmega testmodel(triMesh, faceArea, 4, 1);
//	testmodel.testZdotIntegrationPerface(testZvals0, edgew0, testZvals1, edgew1, 0);
//
//	IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge halfedgemodel(triMesh, faceArea, 3, 4, testZvals0, edgew0, testZvals1, edgew1);
//	Eigen::VectorXd testx;
//	halfedgemodel.convertList2Variable(testx);
//	halfedgemodel.testEnergy(testx);

	// Options
    polyscope::options::autocenterStructures = true;
    polyscope::view::windowWidth = 1024;
    polyscope::view::windowHeight = 1024;

    // Initialize polyscope
    polyscope::init();

    // Register the mesh with Polyscope
    polyscope::registerSurfaceMesh("input mesh", triV, triF);

    // Add the callback
    polyscope::state::userCallback = callback;

    polyscope::options::groundPlaneHeightFactor = 0.25; // adjust the plane height
    // Show the gui
    polyscope::show();

    return 0;
}


//==================================================================================================
/*!
  @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_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TENPOWER_HPP_INCLUDED

#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>
#include <boost/config.hpp>
#include <boost/mpl/if.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( tenpower_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    using result_t = bd::as_floating_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 exp) const BOOST_NOEXCEPT
    {

      result_t result = One<result_t>();
      result_t base = Ten<result_t>();
      auto neg = is_ltz(exp);
      exp =  boost::simd::abs(exp);
      while(exp)
      {
        if (is_odd(exp)) result *= base;
        exp >>= 1;
        base = sqr(base);
      }
      return neg ? rec(result) : result;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( tenpower_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::uint_<A0> >
                          )
  {
    using result_t = bd::as_floating_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 exp) const BOOST_NOEXCEPT
    {
      result_t result = One<result_t>();
      result_t base = Ten<result_t>();
      while(exp)
      {
        if (is_odd(exp)) result *= base;
        exp >>= 1;
        base = sqr(base);
      }
      return result;
    }
  };
} } }

#endif


#pragma once

#include <boost/random/random_number_generator.hpp>
#include <boost/random/uniform_int_distribution.hpp>

namespace calotypes
{

/*! \brief Boost-based reservoir sampling implementation. Best used when sampling
 * a medium subset. */
template<class Engine>
void ReservoirSampling( unsigned int numItems, unsigned int subsetSize,
						std::vector<unsigned int>& inds, Engine& engine )
{
	inds.resize( subsetSize );
	for( unsigned int i = 0; i < subsetSize; i++ )
	{
		inds[i] = i;
	}

	for( unsigned int i = subsetSize; i < numItems; i++ )
	{
		boost::random::uniform_int_distribution<> dist( 0, i ); // i inclusive
		unsigned int j = (unsigned int) dist( engine );
		std::cout << "j: " << j << " ";
		if( j < subsetSize )
		{
			inds[j] = i;
		}
	}
	std::cout << std::endl;
}

/*! \brief Boost-based brute force sampling implementation. Best used when
 * sampling a small subset. */
template<class Engine>
void BitmapSampling( unsigned int numItems, unsigned int subsetSize,
					 std::vector<unsigned int>& inds, Engine& engine )
{
	inds.resize( subsetSize );

	std::vector<bool> bitmap( numItems, false );
	unsigned int count = 0;
	boost::random::uniform_int_distribution<> dist( 0, numItems - 1 );
	while( count < subsetSize )
	{
		unsigned int j = (unsigned int) dist( engine );
		if( bitmap[j] ) { continue; }
		bitmap[j] = true;
		inds[count] = j;
		count++;
	}

}

} // end namespace calotypes


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_NEXTPOW2_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_NEXTPOW2_HPP_INCLUDED
#include <boost/simd/ieee/functions/nextpow2.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/scalar/frexp.hpp>
#include <boost/simd/include/functions/scalar/tofloat.hpp>
#include <boost/simd/include/functions/scalar/minusone.hpp>
#include <boost/simd/include/functions/scalar/abs.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::nextpow2_, tag::cpu_,
                          (A0),
                          (scalar_ < arithmetic_<A0> > )
                         )
  {
    typedef typename dispatch::meta::as_integer<typename boost::dispatch::meta::as_floating<A0>::type, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::nextpow2(tofloat(a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::nextpow2_, tag::cpu_,
                          (A0),
                          (scalar_ < floating_<A0> > )
                         )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 m;
      result_type p;
      boost::simd::frexp(boost::simd::abs(a0), m, p);
      return (m == Half<A0>())  ? minusone(p) :  p;
    }
  };
} } }
#endif


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main()
{
    int t;
    cin >> t;
    while (t--)
    {
        int n;
        cpp_int fact = 1;
        cin >> n;
        while (n != 0)
        {
            fact *= n--;
        }
        cout << fact << endl;
    }
    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>

#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>

#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/trace.hpp>

using namespace std;

template <typename MatrixA>
void test(MatrixA& a, unsigned dim1, unsigned dim2, const char* name)
{
    laplacian_setup(a, dim1, dim2);

    std::cout << "\n" << name << " a = \n" << a << "\n"
	      << "trace(a) = " << trace(a) << "\n"; std::cout.flush();

    // Due to rounding errors, dimensions shouldn't be too large (or test less naive)
    MTL_THROW_IF(trace(a) != 4.0 * int(dim1*dim2), mtl::runtime_error("wrong trace"));
}

int main(int argc, char* argv[])
{
    using namespace mtl;
    unsigned dim1= 3, dim2= 2;

    if (argc > 2) {dim1= atoi(argv[1]);dim2= atoi(argv[2]);}
    unsigned size= dim1 * dim2;

    dense2D<double>                                      dr(size, size), dr2(0,0);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, dim1, dim2, "Dense row major");
    test(dc, dim1, dim2, "Dense column major");
    test(mzd, dim1, dim2, "Morton Z-order");
    test(d2r, dim1, dim2, "Hybrid 2 row-major");
    test(cr, dim1, dim2, "Compressed row major");
    test(cc, dim1, dim2, "Compressed column major");
    test(drc, dim1, dim2, "Dense row major complex");
    test(crc, dim1, dim2, "Compressed row major complex");
    test(dr2, 0, 0, "Dense row major");

    return 0;
}


/**
 * @file tests/cosine_tree_test.cpp
 * @author Siddharth Agrawal
 *
 * Test file for CosineTree class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>
#include <mlpack/core/tree/cosine_tree/cosine_tree.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(CosineTreeTest);

using namespace mlpack;
using namespace mlpack::tree;

/**
 * Constructs a cosine tree with epsilon = 1. Checks if the root node is split
 * further, as it shouldn't be.
 */
BOOST_AUTO_TEST_CASE(CosineTreeNoSplit)
{
  // Initialize constants required for the test.
  const size_t numRows = 10;
  const size_t numCols = 15;
  const double epsilon = 1;
  const double delta = 0.1;

  // Make a random dataset.
  arma::mat data = arma::randu(numRows, numCols);

  // Make a cosine tree, with the generated dataset and the defined constants.
  // Note that the value of epsilon is one.
  CosineTree ctree(data, epsilon, delta);
  arma::mat basis;
  ctree.GetFinalBasis(basis);

  // Since epsilon is one, there should be no splitting and the only vector in
  // the basis should come from the root node.
  BOOST_REQUIRE_EQUAL(basis.n_cols, 1);
}

/**
 * Checks CosineTree::CosineNodeSplit() by doing a depth first search on a
 * random dataset and checking if it satisfies the split condition.
 */
BOOST_AUTO_TEST_CASE(CosineNodeCosineSplit)
{
  // Initialize constants required for the test.
  const size_t numRows = 500;
  const size_t numCols = 1000;
  // Calculation accuracy.
  const double precision = 1e-15;

  // Make a random dataset and the root object.
  arma::mat data = arma::randu(numRows, numCols);
  CosineTree root(data);

  // Stack for depth first search of the tree.
  std::vector<CosineTree*> nodeStack;
  nodeStack.push_back(&root);

  // While stack is not empty.
  while (nodeStack.size())
  {
    // Pop a node from the stack and split it.
    CosineTree *currentNode, *currentLeft, *currentRight;
    currentNode = nodeStack.back();
    currentNode->CosineNodeSplit();
    nodeStack.pop_back();

    // Obtain pointers to the children of the node.
    currentLeft = currentNode->Left();
    currentRight = currentNode->Right();

    // If children exist.
    if (currentLeft && currentRight)
    {
      // Push the child nodes on to the stack.
      nodeStack.push_back(currentLeft);
      nodeStack.push_back(currentRight);

      // Obtain the split point of the popped node.
      arma::vec splitPoint = data.col(currentNode->SplitPointIndex());

      // Column indices of the the child nodes.
      std::vector<size_t> leftIndices, rightIndices;
      leftIndices = currentLeft->VectorIndices();
      rightIndices = currentRight->VectorIndices();

      // The columns in the popped should be split into left and right nodes.
      BOOST_REQUIRE_EQUAL(currentNode->NumColumns(), leftIndices.size() +
          rightIndices.size());

      // Calculate the cosine values for each of the columns in the node.
      arma::vec cosines;
      cosines.zeros(currentNode->NumColumns());

      size_t i, j, k;
      for (i = 0; i < leftIndices.size(); i++)
        cosines(i) = arma::norm_dot(data.col(leftIndices[i]), splitPoint);

      for (j = 0, k = i; j < rightIndices.size(); j++, k++)
        cosines(k) = arma::norm_dot(data.col(rightIndices[j]), splitPoint);

      // Check if the columns assigned to the children agree with the splitting
      // condition.  Due to miscalculations cosineMax calculated by
      // CosineNodeSplit may differ from cosineMax below, so we have to handle
      // minor differences.
      double cosineMax = arma::max(cosines % (cosines < 1.0 + precision));
      double cosineMin = arma::min(cosines);
      // If max(cosines) is close to 1.0 cosineMax and cosineMax2 may
      // differ significantly.
      double cosineMax2 = arma::max(cosines % (cosines < 1.0 - precision));

      if (std::fabs(cosineMax - cosineMax2) < precision)
      {
        // Check with some precision.
        for (i = 0; i < leftIndices.size(); i++)
          BOOST_REQUIRE_LT(cosineMax - cosines(i),
                           cosines(i) - cosineMin + precision);

        for (j = 0, k = i; j < rightIndices.size(); j++, k++)
          BOOST_REQUIRE_GT(cosineMax - cosines(k),
                           cosines(k) - cosineMin - precision);
      }
      else
      {
        size_t numMax1Errors = 0;
        size_t numMax2Errors = 0;

        // Find errors for cosineMax.
        for (i = 0; i < leftIndices.size(); i++)
          if (cosineMax - cosines(i) >= cosines(i) - cosineMin + precision)
            numMax1Errors++;

        for (j = 0, k = i; j < rightIndices.size(); j++, k++)
          if (cosineMax - cosines(k) <= cosines(k) - cosineMin - precision)
            numMax1Errors++;

        // Find errors for cosineMax2.
        for (i = 0; i < leftIndices.size(); i++)
          if (cosineMax2 - cosines(i) >= cosines(i) - cosineMin + precision)
            numMax2Errors++;

        for (j = 0, k = i; j < rightIndices.size(); j++, k++)
          if (cosineMax2 - cosines(k) <= cosines(k) - cosineMin - precision)
            numMax2Errors++;

        // One of the maximum cosine values should be correct
        BOOST_REQUIRE_EQUAL(std::min(numMax1Errors, numMax2Errors), 0);
      }
    }
  }
}

/**
 * Checks CosineTree::ModifiedGramSchmidt() by creating a random basis for the
 * vector subspace and checking if all the vectors are orthogonal to each other.
 */
BOOST_AUTO_TEST_CASE(CosineTreeModifiedGramSchmidt)
{
  // Initialize constants required for the test.
  const size_t numRows = 100;
  const size_t numCols = 50;
  const double epsilon = 1;
  const double delta = 0.1;

  // Make a random dataset.
  arma::mat data = arma::randu(numRows, numCols);

  // Declare a queue and a dummy CosineTree object.
  CosineNodeQueue basisQueue;
  CosineTree dummyTree(data, epsilon, delta);

  for (size_t i = 0; i < numCols; i++)
  {
    // Make a new CosineNode object.
    CosineTree* basisNode;
    basisNode = new CosineTree(data);

    // Use the columns of the dataset as random centroids.
    arma::vec centroid = data.col(i);
    arma::vec newBasisVector;

    // Obtain the orthonormalized version of the centroid.
    dummyTree.ModifiedGramSchmidt(basisQueue, centroid, newBasisVector);

    // Check if the obtained vector is orthonormal to the basis vectors.
    CosineNodeQueue::const_iterator j = basisQueue.begin();
    CosineTree* currentNode;

    for (; j != basisQueue.end(); j++)
    {
      currentNode = *j;
      BOOST_REQUIRE_SMALL(arma::dot(currentNode->BasisVector(), newBasisVector),
                          1e-5);
    }

    // Add the obtained vector to the basis.
    basisNode->BasisVector(newBasisVector);
    basisNode->L2Error(arma::randu());
    basisQueue.push(basisNode);
  }

  // Deallocate memory given to the objects.
  for (size_t i = 0; i < numCols; i++)
  {
    CosineTree* currentNode;
    currentNode = basisQueue.top();
    basisQueue.pop();

    delete currentNode;
  }
}

/**
 * Test the copy constructor & copy assignment using Cosine trees.
 */
BOOST_AUTO_TEST_CASE(CopyConstructorAndOperatorCosineTreeTest)
{
  // Initialize constants required for the test.
  const size_t numRows = 10;
  const size_t numCols = 15;

  // Vectors to hold depth-first traversal
  // of the number of columns in each node.
  std::vector<int> v1, v2, v3;

  // Make a random dataset.
  arma::mat* data = new arma::mat(numRows, numCols, arma::fill::randu);

  // Make a cosine tree, with the generated dataset.
  CosineTree* ctree1 = new CosineTree(*data);

  // Stacks for depth first search of the tree.
  std::vector<CosineTree*> nodeStack1, nodeStack2, nodeStack3;
  nodeStack1.push_back(ctree1);

  // While stack is not empty.
  while (nodeStack1.size())
  {
    // Pop a node from the stack and split it.
    CosineTree *currentNode1, *currentLeft1, *currentRight1;

    currentNode1 = nodeStack1.back();
    currentNode1->CosineNodeSplit();
    nodeStack1.pop_back();

    // Obtain pointers to the children of the node.
    currentLeft1 = currentNode1->Left();
    currentRight1 = currentNode1->Right();

    // If children exist.
    if (currentLeft1 && currentRight1)
    {
      // Push the child nodes on to the stack.
      nodeStack1.push_back(currentLeft1);
      nodeStack1.push_back(currentRight1);

      v1.push_back(currentNode1->NumColumns());
    }
  }

  // Copy constructor and operator.
  CosineTree ctree2(*ctree1);
  CosineTree ctree3 = *ctree1;

  delete ctree1;
  delete data;

  nodeStack2.push_back(&ctree2);
  nodeStack3.push_back(&ctree3);

  // While stacks are not empty.
  while (nodeStack2.size() && nodeStack3.size())
  {
    // Pop a node from the stack and split it.
    CosineTree *currentNode2, *currentLeft2, *currentRight2;
    CosineTree *currentNode3, *currentLeft3, *currentRight3;

    currentNode2 = nodeStack2.back();
    nodeStack2.pop_back();

    currentNode3 = nodeStack3.back();
    nodeStack3.pop_back();

    // Obtain pointers to the children of the node.
    currentLeft2 = currentNode2->Left();
    currentRight2 = currentNode2->Right();

    currentLeft3 = currentNode3->Left();
    currentRight3 = currentNode3->Right();

    // If children exist.
    if (currentLeft2 && currentRight2 && currentLeft3 && currentRight3)
    {
      // Push the child nodes on to the stack.
      nodeStack2.push_back(currentLeft2);
      nodeStack2.push_back(currentRight2);

      v2.push_back(currentNode2->NumColumns());

      nodeStack3.push_back(currentLeft3);
      nodeStack3.push_back(currentRight3);

      v3.push_back(currentNode3->NumColumns());
    }
  }

  for (size_t i = 0; i < v1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(v1.at(i), v2.at(i));
    BOOST_REQUIRE_EQUAL(v1.at(i), v3.at(i));
  }
}

/**
 * Test the move constructor & move assignment using Cosine trees.
 */
BOOST_AUTO_TEST_CASE(MoveConstructorAndOperatorCosineTreeTest)
{
  // Initialize constants required for the test.
  const size_t numRows = 10;
  const size_t numCols = 15;

  // Vectors to hold depth-first traversal
  // of the number of columns in each node.
  std::vector<int> v1, v2, v3;

  // Make a random dataset.
  arma::mat data = arma::randu(numRows, numCols);

  // Make a cosine tree, with the generated dataset.
  CosineTree ctree1(data);

  // Stacks for depth first search of the tree.
  std::vector<CosineTree*> nodeStack1, nodeStack2, nodeStack3;
  nodeStack1.push_back(&ctree1);

  // While stack is not empty.
  while (nodeStack1.size())
  {
    // Pop a node from the stack and split it.
    CosineTree *currentNode1, *currentLeft1, *currentRight1;

    currentNode1 = nodeStack1.back();
    currentNode1->CosineNodeSplit();
    nodeStack1.pop_back();

    // Obtain pointers to the children of the node.
    currentLeft1 = currentNode1->Left();
    currentRight1 = currentNode1->Right();

    // If children exist.
    if (currentLeft1 && currentRight1)
    {
      // Push the child nodes on to the stack.
      nodeStack1.push_back(currentLeft1);
      nodeStack1.push_back(currentRight1);

      v1.push_back(currentNode1->NumColumns());
    }
  }

  // Move constructor.
  CosineTree ctree2(std::move(ctree1));

  nodeStack2.push_back(&ctree2);

  // While stacks are not empty.
  while (nodeStack2.size())
  {
    // Pop a node from the stack and split it.
    CosineTree *currentNode2, *currentLeft2, *currentRight2;

    currentNode2 = nodeStack2.back();
    nodeStack2.pop_back();

    // Obtain pointers to the children of the node.
    currentLeft2 = currentNode2->Left();
    currentRight2 = currentNode2->Right();

    // If children exist.
    if (currentLeft2 && currentRight2)
    {
      // Push the child nodes on to the stack.
      nodeStack2.push_back(currentLeft2);
      nodeStack2.push_back(currentRight2);

      v2.push_back(currentNode2->NumColumns());
    }
  }

  // Move operator.
  CosineTree ctree3 = std::move(ctree2);

  nodeStack3.push_back(&ctree3);

  // While stacks are not empty.
  while (nodeStack3.size())
  {
    // Pop a node from the stack and split it.
    CosineTree *currentNode3, *currentLeft3, *currentRight3;

    currentNode3 = nodeStack3.back();
    nodeStack3.pop_back();

    // Obtain pointers to the children of the node.
    currentLeft3 = currentNode3->Left();
    currentRight3 = currentNode3->Right();

    // If children exist.
    if (currentLeft3 && currentRight3)
    {
      // Push the child nodes on to the stack.
      nodeStack3.push_back(currentLeft3);
      nodeStack3.push_back(currentRight3);

      v3.push_back(currentNode3->NumColumns());
    }
  }

  for (size_t i = 0; i < v1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(v1.at(i), v2.at(i));
    BOOST_REQUIRE_EQUAL(v1.at(i), v3.at(i));
  }
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @file ess.hpp
 * @author Vahid Bastani
 *
 * Effective Sample Size (ESS) resampling criterion
 */
#ifndef SSMPACK_FILTER_RESAMPLER_CRITERION_ESS
#define SSMPACK_FILTER_RESAMPLER_CRITERION_ESS

#include <armadillo>

namespace ssmkit {
namespace filter {
namespace resampler {
namespace criterion {

struct ESS{
  double th;
  /**
   * @param threshold: minimum number of effective sample.
   */
  ESS(double threshold) : th{threshold} {}
  ESS() = delete;
  /**
   * return true if number of effective samples with weights w falls bellow the
   * threshold
   */
  bool operator()(const arma::vec &w){
    return arma::sum(w)/arma::sum(arma::square(w)) < th;
  }

};
} // namespace criterion
} // namespace resampler
} // namespace filter
} // namespace ssmkit
#endif // SSMPACK_FILTER_RESAMPLER_CRITERION_ESS


#ifndef HAMILTONIANS_XYZNNNSTO2_HPP
#define HAMILTONIANS_XYZNNNSTO2_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

/* Stoquastic for |a|, |b| > 1*/
class XYZNNNSto2
{
private:
	int n_;
	double a_;
	double b_;
	constexpr static double J1 = -1.0;
	constexpr static double J2 = -1.0;
public:

	XYZNNNSto2(int n, double a, double b)
		: n_(n), a_(a), b_(b)
	{
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "XYZNNNSto2"},
			{"n", n_},
			{"a", a_},
			{"b", b_}
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += -J1*b_*yysign; //zz
			s += J1*(a_+1.0*yysign)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		//Next-nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt((i+2)%n_);
			s += -J2*a_*yysign; //zz
			s += J2*(b_+1.0*yysign)*smp.ratio(i, (i+2)%n_); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col ^ x] += J1*(a_ - 1.0*sgn);
			m[col] += J1*b_*sgn;
		}
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+2)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+2)%(n_)));
			m[col ^ x] += J2*(b_ - sgn*1.0);
			m[col] += J2*a_*sgn;
		}
		return m;
	}
};
#endif//HAMILTONIANS_XYZNNNSTO2_HPP


#ifndef _CELERITE2_TERMS_HPP_DEFINED_
#define _CELERITE2_TERMS_HPP_DEFINED_

#include <tuple>
#include <exception>
#include <Eigen/Core>

namespace celerite2 {

#ifndef CELERITE_MAX_WIDTH
#define CELERITE_MAX_WIDTH 32
#endif

struct dimension_mismatch : public std::exception {
  const char *what() const throw() { return "dimension mismatch"; }
};

template <int J1, int J2>
struct sum_width {
  constexpr static int value = (J1 == Eigen::Dynamic || J2 == Eigen::Dynamic) ? Eigen::Dynamic : (J1 + J2);
};

/**
 * The abstract base class from which terms should inherit
 */
template <typename T, int J_ = Eigen::Dynamic>
class Term {
  protected:
  constexpr static int Width = ((0 < J_) && (J_ <= CELERITE_MAX_WIDTH)) ? J_ : Eigen::Dynamic;
  static constexpr int Order = (Width != 1) ? Eigen::RowMajor : Eigen::ColMajor;

  public:
  /**
   * \typedef Scalar
   * The underlying scalar type of this `Term` (should probably always be `double`)
   */
  typedef T Scalar;

  /**
   * \typedef Vector
   * An `Eigen` vector with data type `Scalar`
   */
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;

  /**
   * \typedef LowRank
   * The `Eigen` type for the low-rank matrices used internally
   */
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Width, Order> LowRank;

  /**
   * \typedef CoeffVector
   * The `Eigen` type for a fixed width vector of coefficients
   */
  typedef Eigen::Matrix<Scalar, Width, 1> CoeffVector;

  /**
   * \typedef Coeffs
   * A tuple of vectors giving the coefficients for the celerite model
   */
  typedef std::tuple<Vector, Vector, Vector, Vector, Vector, Vector> Coeffs;

  /**
   * \typedef Matrices
   * A tuple of matrices representing this celerite process
   */
  typedef std::tuple<CoeffVector, Vector, LowRank, LowRank> Matrices;

  Term(){};

  int get_width() const { return Width; }

  /**
   * Set the coefficients of the term
   *
   * @param ar     (J_real,): The real amplitudes.
   * @param cr     (J_real,): The real exponential.
   * @param ac     (J_comp,): The complex even amplitude.
   * @param bc     (J_comp,): The complex odd amplitude.
   * @param cc     (J_comp,): The complex exponential.
   * @param dc     (J_comp,): The complex frequency.
   */
  void set_coefficients(const Vector &ar, const Vector &cr, const Vector &ac, const Vector &bc, const Vector &cc, const Vector &dc) {
    Eigen::Index nr = ar.rows(), nc = ac.rows();

    ar_.resize(nr);
    cr_.resize(nr);
    ac_.resize(nc);
    bc_.resize(nc);
    cc_.resize(nc);
    dc_.resize(nc);

    ar_ << ar;
    cr_ << cr;
    ac_ << ac;
    bc_ << bc;
    cc_ << cc;
    dc_ << dc;
  }

  /**
   * Get the coefficients of the term as a tuple
   */
  Coeffs get_coefficients() const { return std::make_tuple(ar_, cr_, ac_, bc_, cc_, dc_); }

  /**
   * Get the matrices required to represent the celerite process
   *
   * @param x    (N,): The independent coordinates of the data.
   * @param diag (N,): The diagonal variance of the process.
   */
  Matrices get_celerite_matrices(const Vector &x, const Vector &diag) const {
    Eigen::Index N = x.rows();
    if (diag.rows() != N) throw dimension_mismatch();

    Eigen::Index nr = ar_.rows();
    Eigen::Index nc = ac_.rows();
    Eigen::Index J  = nr + 2 * nc;
    if (Width != Eigen::Dynamic && Width != J) throw dimension_mismatch();

    CoeffVector c(J);
    Vector a = diag.array() + (ar_.sum() + ac_.sum());
    LowRank U(N, J), V(N, J);

    c << cr_, cc_, cc_;

    U.block(0, 0, N, nr).rowwise() = ar_.transpose();
    V.block(0, 0, N, nr).setConstant(Scalar(1));

    auto arg                   = (x * dc_.transpose()).array().eval();
    auto ca                    = cos(arg).eval();
    auto sa                    = sin(arg).eval();
    U.block(0, nr, N, nc)      = ca.array().rowwise() * ac_.transpose().array() + sa.array().rowwise() * bc_.transpose().array();
    U.block(0, nr + nc, N, nc) = sa.array().rowwise() * ac_.transpose().array() - ca.array().rowwise() * bc_.transpose().array();
    V.block(0, nr, N, nc)      = ca;
    V.block(0, nr + nc, N, nc) = sa;

    return std::make_tuple(c, a, U, V);
  }

  /**
   * Adding two terms builds a new term where the coefficients have been concatenated
   *
   * @param other (Term): The term to add to this one.
   */
  template <typename Other>
  Term<typename std::common_type<Scalar, typename Other::Scalar>::type, sum_width<Width, Other::Width>::value> operator+(const Other &other) const {
    typedef typename std::common_type<Scalar, typename Other::Scalar>::type NewScalar;

    auto coeffs = other.get_coefficients();

    Eigen::Index nr = ar_.rows() + std::get<0>(coeffs).rows();
    Eigen::Index nc = ac_.rows() + std::get<2>(coeffs).rows();

    Eigen::Matrix<NewScalar, Eigen::Dynamic, 1> ar(nr), cr(nr), ac(nc), bc(nc), cc(nc), dc(nc);

    ar << ar_, std::get<0>(coeffs);
    cr << cr_, std::get<1>(coeffs);
    ac << ac_, std::get<2>(coeffs);
    bc << ac_, std::get<3>(coeffs);
    cc << ac_, std::get<4>(coeffs);
    dc << ac_, std::get<5>(coeffs);

    Term<NewScalar, sum_width<Width, Other::Width>::value> new_term;
    new_term.set_coefficients(ar, cr, ac, bc, cc, dc);

    return new_term;
  }

  private:
  Vector ar_, cr_, ac_, bc_, cc_, dc_;
};

/**
 * \class RealTerm
 * The simplest celerite model
 *
 * @param a: The amplitude of the term.
 * @param c: The exponent of the term.
 */
template <typename T>
class RealTerm : public Term<T, 1> {
  public:
  /**
   * \typedef Scalar
   * The underlying scalar type of this `Term` (should probably always be `double`)
   */
  typedef T Scalar;
  constexpr static int Width = 1;
  using typename Term<Scalar, 1>::Vector;
  using typename Term<Scalar, 1>::LowRank;
  RealTerm(const Scalar &a, const Scalar &c) {
    Vector ar(1), cr(1), ac, bc, cc, dc;
    ar << a;
    cr << c;
    this->set_coefficients(ar, cr, ac, bc, cc, dc);
  };
};

/**
 * \class ComplexTerm
 * A general celerite model
 *
 * @param a: The real part of the amplitude.
 * @param b: The complex part of the amplitude.
 * @param c: The real part of the exponent.
 * @param d: The complex part of the exponent.
 */
template <typename T>
class ComplexTerm : public Term<T, 2> {
  public:
  /**
   * \typedef Scalar
   * The underlying scalar type of this `Term` (should probably always be `double`)
   */
  typedef T Scalar;
  constexpr static int Width = 2;
  using typename Term<Scalar, 2>::Vector;
  using typename Term<Scalar, 2>::LowRank;
  ComplexTerm(const Scalar &a, const Scalar &b, const Scalar &c, const Scalar &d) {
    Vector ar, cr, ac(1), bc(1), cc(1), dc(1);
    ac << a;
    bc << b;
    cc << c;
    dc << d;
    this->set_coefficients(ar, cr, ac, bc, cc, dc);
  };
};

/**
 * \class SHOTerm
 * A term representing a stochastically-driven, damped harmonic oscillator
 *
 * @param S0:  The power at `omega = 0`.
 * @param w0:  The undamped angular frequency.
 * @param Q:   The quality factor.
 * @param eps: A regularization parameter used for numerical stability.
 */
template <typename T>
class SHOTerm : public Term<T, 2> {
  public:
  /**
   * \typedef Scalar
   * The underlying scalar type of this `Term` (should probably always be `double`)
   */
  typedef T Scalar;
  constexpr static int Width = 2;
  using typename Term<Scalar, 2>::Vector;
  using typename Term<Scalar, 2>::LowRank;
  SHOTerm(const Scalar &S0, const Scalar &w0, const Scalar &Q, const Scalar &eps = 1e-5) {
    Vector ar, cr, ac, bc, cc, dc;
    if (Q < 0.5) {
      ar.resize(2);
      cr.resize(2);
      auto f = std::sqrt(std::max(1.0 - 4.0 * Q * Q, eps));
      auto a = 0.5 * S0 * w0 * Q;
      auto c = 0.5 * w0 / Q;
      ar(0)  = a * (1 + 1 / f);
      ar(1)  = a * (1 - 1 / f);
      cr(0)  = c * (1 - f);
      cr(1)  = c * (1 + f);
    } else {
      ac.resize(1);
      bc.resize(1);
      cc.resize(1);
      dc.resize(1);
      auto f = std::sqrt(std::max(4.0 * Q * Q - 1, eps));
      auto a = S0 * w0 * Q;
      auto c = 0.5 * w0 / Q;
      ac(0)  = a;
      bc(0)  = a / f;
      cc(0)  = c;
      dc(0)  = c * f;
    }
    this->set_coefficients(ar, cr, ac, bc, cc, dc);
  };
};

} // namespace celerite2

#endif // _CELERITE2_TERMS_HPP_DEFINED_


// 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_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

// TODO: once change this to proper strategy
// It is different from current segment intersection because these are not segments but lines
// If we have the Line concept, we can create a strategy
template <typename Point, typename Line1, typename Line2 = Line1>
struct line_line_intersection
{
    template <typename A, typename B, typename C, typename D>
    static inline A det(A const& a, B const& b, C const& c, D const& d)
    {
        return a * d - b * c;
    }

    static inline bool apply(Line1 const& line1, Line2 const& line2, Point& p)
    {
        // See http://mathworld.wolfram.com/Line-LineIntersection.html
        typedef typename coordinate_type<Point>::type coordinate_type;
        coordinate_type x1 = get<0,0>(line1), y1 = get<0,1>(line1);
        coordinate_type x2 = get<1,0>(line1), y2 = get<1,1>(line1);
        coordinate_type x3 = get<0,0>(line2), y3 = get<0,1>(line2);
        coordinate_type x4 = get<1,0>(line2), y4 = get<1,1>(line2);

        coordinate_type denominator = det(x1 - x2, y1 - y2, x3 - x4, y3 - y4);

        // TODO: use something else then denominator (sides?) to determine this.

        // If denominator is zero, segments are parallel.
        // We have context information, so know that it should then
        // be the case that line1.p2 == line2.p1, and that is the
        // intersection point.
        if (geometry::math::equals(denominator, 0.0))
        {
            set<0>(p, x2);
            set<1>(p, y2);
            return false;
        }

        coordinate_type d1 = det(x1, y1, x2, y2);
        coordinate_type d2 = det(x3, y3, x4, y4);
        coordinate_type px = det(d1, x1 - x2, d2, x3 - x4) / denominator;
        coordinate_type py = det(d1, y1 - y2, d2, y3 - y4) / denominator;

        set<0>(p, px);
        set<1>(p, py);

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER
        if (geometry::math::abs(denominator) < 1.0e-7)
        {
            std::cout << "small " << denominator << std::endl;
        }
#endif
        return geometry::math::abs(denominator) > 1.0e-7;
    }
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP