/*
    Lightmetrica - A modern, research-oriented renderer

    Copyright (c) 2015 Hisanari Otsu

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

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

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

#include "manifoldutils.h"
#include "debugio.h"
#include <cereal/archives/json.hpp>
#include <cereal/types/vector.hpp>

#if LM_COMPILER_MSVC
#pragma warning(disable:4714)
#pragma warning(disable:4701)
#pragma warning(disable:4456)
#include <Eigen/Dense>
#else
#include <eigen3/Eigen/Dense>
#endif

#define INVERSEMAP_MANIFOLDWALK_USE_EIGEN_SOLVER 1
#define INVERSEMAP_MANIFOLDWALK_BETA_EXT 0

LM_NAMESPACE_BEGIN

using Matrix = Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic>;
using Vector = Eigen::Matrix<Float, Eigen::Dynamic, 1>;

template <class Archive>
auto serialize(Archive& archive, Vec3& v) -> void
{
    archive(cereal::make_nvp("x", v.x), cereal::make_nvp("y", v.y), cereal::make_nvp("z", v.z));
}

namespace
{

auto SolveBlockLinearEq(const ConstraintJacobian& nablaC, const std::vector<Vec2>& V, std::vector<Vec2>& W) -> void
{
	const int n = (int)(nablaC.size());
	assert(V.size() == nablaC.size());

	// --------------------------------------------------------------------------------

	#pragma region LU decomposition

	// A'_{0,n-1} = B_{0,n-1}
	// B'_{0,n-2} = C_{0,n-2}
	// C'_{0,n-2} = A_{1,n-1}
	std::vector<Mat2> L(n);
	std::vector<Mat2> U(n);
	{
		// U_1 = A'_1
		U[0] = nablaC[0].B;
		for (int i = 1; i < n; i++)
		{
			L[i] = nablaC[i].A * Math::Inverse(U[i-1]);		// L_i = C'_i U_{i-1}^-1
			U[i] = nablaC[i].B - L[i] * nablaC[i-1].C;		// U_i = A'_i - L_i * B'_{i-1}
		}
	}

	#pragma endregion

	// --------------------------------------------------------------------------------

	#pragma region Forward substitution

	// Solve L V' = V
	std::vector<Vec2> Vp(n);
	Vp[0] = V[0];
	for (int i = 1; i < n; i++)
	{
		// V'_i = V_i - L_i V'_{i-1}
		Vp[i] = V[i] - L[i] * Vp[i - 1];
	}

	#pragma endregion

	// --------------------------------------------------------------------------------

	#pragma region Backward substitution

	W.assign(n, Vec2());

	// Solve U_n W_n = V'_n
	W[n - 1] = Math::Inverse(U[n - 1]) * Vp[n - 1];

	for (int i = n - 2; i >= 0; i--)
	{
		// Solve U_i W_i = V'_i - V_i W_{i+1}
		W[i] = Math::Inverse(U[i]) * (Vp[i] - V[i] * W[i + 1]);
	}

	#pragma endregion
}

}

#if 1
auto ManifoldUtils::ComputeConstraintJacobian(const Subpath& path, ConstraintJacobian& nablaC) -> void
{
	const int n = (int)(path.vertices.size());
	for (int i = 1; i < n - 1; i++)
	{
		#pragma region Some precomputation

        const auto vi  = path.vertices[i];
        const auto vip = path.vertices[i - 1];
        const auto vin = path.vertices[i + 1];

		const auto& x  = vi.geom;
		const auto& xp = vip.geom;
		const auto& xn = vin.geom;

		const auto wi  = Math::Normalize(xp.p - x.p);
		const auto wo  = Math::Normalize(xn.p - x.p);
        const auto eta = 1_f / vi.primitive->bsdf->Eta(x, wi);
		const auto H   = Math::Normalize(wi + eta * wo);

		const auto inv_wiL = 1_f / Math::Length(xp.p - x.p);     // ili
		const auto inv_woL = 1_f / Math::Length(xn.p - x.p);     // ilo
		const auto inv_HL  = 1_f / Math::Length(wi + eta * wo);  // ilh

		const auto dot_H_n    = Math::Dot(x.sn, H);
		const auto dot_H_dndu = Math::Dot(x.dndu, H);
		const auto dot_H_dndv = Math::Dot(x.dndv, H);
		const auto dot_u_n    = Math::Dot(x.dpdu, x.sn);
		const auto dot_v_n    = Math::Dot(x.dpdv, x.sn);

		const auto s = x.dpdu - dot_u_n * x.sn;
		const auto t = x.dpdv - dot_v_n * x.sn;

		const auto div_inv_wiL_HL = inv_wiL * inv_HL;         // ili := ili * ilh
		const auto div_inv_woL_HL = inv_woL * inv_HL * eta;   // ilo := ilo * eta * ilh

		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute A_i (derivative w.r.t. x_{i-1})

		{
			const auto tu = (xp.dpdu - wi * Math::Dot(wi, xp.dpdu)) * div_inv_wiL_HL;
			const auto tv = (xp.dpdv - wi * Math::Dot(wi, xp.dpdv)) * div_inv_wiL_HL;
			const auto dHdu = tu - H * Math::Dot(tu, H);
			const auto dHdv = tv - H * Math::Dot(tv, H);
			nablaC[i-1].A = Mat2(
				Math::Dot(dHdu, s), Math::Dot(dHdu, t),
				Math::Dot(dHdv, s), Math::Dot(dHdv, t));
		}

		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute B_i (derivative w.r.t. x_i)

		{
			const auto tu = -x.dpdu * (div_inv_wiL_HL + div_inv_woL_HL) + wi * (Math::Dot(wi, x.dpdu) * div_inv_wiL_HL) + wo * (Math::Dot(wo, x.dpdu) * div_inv_woL_HL);
			const auto tv = -x.dpdv * (div_inv_wiL_HL + div_inv_woL_HL) + wi * (Math::Dot(wi, x.dpdv) * div_inv_wiL_HL) + wo * (Math::Dot(wo, x.dpdv) * div_inv_woL_HL);
			const auto dHdu = tu - H * Math::Dot(tu, H);
			const auto dHdv = tv - H * Math::Dot(tv, H);
			nablaC[i-1].B = Mat2(
				Math::Dot(dHdu, s) - Math::Dot(x.dpdu, x.dndu) * dot_H_n - dot_u_n * dot_H_dndu,
				Math::Dot(dHdu, t) - Math::Dot(x.dpdv, x.dndu) * dot_H_n - dot_v_n * dot_H_dndu,
				Math::Dot(dHdv, s) - Math::Dot(x.dpdu, x.dndv) * dot_H_n - dot_u_n * dot_H_dndv,
				Math::Dot(dHdv, t) - Math::Dot(x.dpdv, x.dndv) * dot_H_n - dot_v_n * dot_H_dndv);
		}

		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute C_i (derivative w.r.t. x_{i+1})

		{
			const auto tu = (xn.dpdu - wo * Math::Dot(wo, xn.dpdu)) * div_inv_woL_HL;
			const auto tv = (xn.dpdv - wo * Math::Dot(wo, xn.dpdv)) * div_inv_woL_HL;
			const auto dHdu = tu - H * Math::Dot(tu, H);
			const auto dHdv = tv - H * Math::Dot(tv, H);
			nablaC[i - 1].C = Mat2(
				Math::Dot(dHdu, s), Math::Dot(dHdu, t),
				Math::Dot(dHdv, s), Math::Dot(dHdv, t));
		}

		#pragma endregion
	}
}
#else
auto ManifoldUtils::ComputeConstraintJacobian(const Subpath& path, ConstraintJacobian& nablaC) -> void
{
	const int n = (int)(path.vertices.size());
	for (int i = 1; i < n - 1; i++)
	{
		#pragma region Some precomputation

        const auto vi  = path.vertices[i];
        const auto vip = path.vertices[i - 1];
        const auto vin = path.vertices[i + 1];

		const auto& x  = vi.geom;
		const auto& xp = vip.geom;
		const auto& xn = vin.geom;

		const auto wi  = Math::Normalize(xp.p - x.p);
		const auto wo  = Math::Normalize(xn.p - x.p);
        const auto eta = 1_f / vi.primitive->bsdf->Eta(x, wi);
        // No need to normalize H for index-matched materials or reflections
        const bool normalizeH = eta != 1_f;
		const auto H   = normalizeH ? Math::Normalize(wi + eta * wo) : wi + wo;

		const auto inv_wiL = 1_f / Math::Length(xp.p - x.p);                        // ili
		const auto inv_woL = 1_f / Math::Length(xn.p - x.p);                        // ilo
        const auto inv_HL = normalizeH ? 1_f / Math::Length(wi + eta * wo) : 1_f;  // ilh

		const auto dot_H_n    = Math::Dot(x.sn, H);
		const auto dot_H_dndu = Math::Dot(x.dndu, H);
		const auto dot_H_dndv = Math::Dot(x.dndv, H);
		const auto dot_u_n    = Math::Dot(x.dpdu, x.sn);
		const auto dot_v_n    = Math::Dot(x.dpdv, x.sn);

		const auto s = x.dpdu - dot_u_n * x.sn;
		const auto t = x.dpdv - dot_v_n * x.sn;

		const auto div_inv_wiL_HL = inv_wiL * inv_HL;         // ili := ili * ilh
		const auto div_inv_woL_HL = inv_woL * inv_HL * eta;   // ilo := ilo * eta * ilh

		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute A_i (derivative w.r.t. x_{i-1})

		{
			const auto tu = (xp.dpdu - wi * Math::Dot(wi, xp.dpdu)) * div_inv_wiL_HL;
			const auto tv = (xp.dpdv - wi * Math::Dot(wi, xp.dpdv)) * div_inv_wiL_HL;
			const auto dHdu = normalizeH ? tu - H * Math::Dot(tu, H) : tu;
			const auto dHdv = normalizeH ? tv - H * Math::Dot(tv, H) : tv;
			nablaC[i-1].A = Mat2(
				Math::Dot(dHdu, s), Math::Dot(dHdu, t),
				Math::Dot(dHdv, s), Math::Dot(dHdv, t));
		}

		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute B_i (derivative w.r.t. x_i)

		{
			const auto tu = -x.dpdu * (div_inv_wiL_HL + div_inv_woL_HL) + wi * (Math::Dot(wi, x.dpdu) * div_inv_wiL_HL) + wo * (Math::Dot(wo, x.dpdu) * div_inv_woL_HL);
			const auto tv = -x.dpdv * (div_inv_wiL_HL + div_inv_woL_HL) + wi * (Math::Dot(wi, x.dpdv) * div_inv_wiL_HL) + wo * (Math::Dot(wo, x.dpdv) * div_inv_woL_HL);
			const auto dHdu = normalizeH ? tu - H * Math::Dot(tu, H) : tu;
			const auto dHdv = normalizeH ? tv - H * Math::Dot(tv, H) : tv;
			nablaC[i-1].B = Mat2(
				Math::Dot(dHdu, s) - Math::Dot(x.dpdu, x.dndu) * dot_H_n - dot_u_n * dot_H_dndu,
				Math::Dot(dHdu, t) - Math::Dot(x.dpdv, x.dndu) * dot_H_n - dot_v_n * dot_H_dndu,
				Math::Dot(dHdv, s) - Math::Dot(x.dpdu, x.dndv) * dot_H_n - dot_u_n * dot_H_dndv,
				Math::Dot(dHdv, t) - Math::Dot(x.dpdv, x.dndv) * dot_H_n - dot_v_n * dot_H_dndv);
		}

		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute C_i (derivative w.r.t. x_{i+1})

		{
			const auto tu = (xn.dpdu - wo * Math::Dot(wo, xn.dpdu)) * div_inv_woL_HL;
			const auto tv = (xn.dpdv - wo * Math::Dot(wo, xn.dpdv)) * div_inv_woL_HL;
			const auto dHdu = normalizeH ? tu - H * Math::Dot(tu, H) : tu;
			const auto dHdv = normalizeH ? tv - H * Math::Dot(tv, H) : tv;
			nablaC[i - 1].C = Mat2(
				Math::Dot(dHdu, s), Math::Dot(dHdu, t),
				Math::Dot(dHdv, s), Math::Dot(dHdv, t));
		}

		#pragma endregion
	}
}
#endif

// --------------------------------------------------------------------------------

namespace
{
    template <typename t_matrix>
    t_matrix PseudoInverse(const t_matrix& m, const double &tolerance = 1.e-6)
    {
        using namespace Eigen;
        typedef JacobiSVD<t_matrix> TSVD;
        unsigned int svd_opt(ComputeThinU | ComputeThinV);
        if (m.RowsAtCompileTime != Dynamic || m.ColsAtCompileTime != Dynamic)
            svd_opt = ComputeFullU | ComputeFullV;
        TSVD svd(m, svd_opt);
        const typename TSVD::SingularValuesType &sigma(svd.singularValues());
        typename TSVD::SingularValuesType sigma_inv(sigma.size());
        for (long i = 0; i<sigma.size(); ++i)
        {
            if (sigma(i) > tolerance)
                sigma_inv(i) = 1.0 / sigma(i);
            else
                sigma_inv(i) = 0.0;
        }
        return svd.matrixV()*sigma_inv.asDiagonal()*svd.matrixU().transpose();
    }
}

auto ManifoldUtils::ComputeConstraintJacobianDeterminant(const Subpath& subpath) -> Float
{
    const int n = (int)(subpath.vertices.size());

    // --------------------------------------------------------------------------------

    ConstraintJacobian nablaC;
    nablaC.assign(n - 2, VertexConstraintJacobian());
    ComputeConstraintJacobian(subpath, nablaC);

    // --------------------------------------------------------------------------------

    // A
    Matrix A;
    A.setZero(2 * (n - 2), 2 * (n - 2));
    for (int i = 0; i < n - 2; i++)
    {
        if (i > 0)
        {
            const auto& A_ = nablaC[i].A;
            Eigen::Array22d m;
            m << A_[0][0], A_[1][0],
                 A_[0][1], A_[1][1];
            A.block<2, 2>(i * 2, (i - 1) * 2) = m;
        }
        {
            const auto& B_ = nablaC[i].B;
            Eigen::Array22d m;
            m << B_[0][0], B_[1][0],
                 B_[0][1], B_[1][1];
            A.block<2, 2>(i * 2, i * 2) = m;
        }
        if (i < n - 2 - 1)
        {
            const auto& C_ = nablaC[i].C;
            Eigen::Array22d m;
            m << C_[0][0], C_[1][0],
                 C_[0][1], C_[1][1];
            A.block<2, 2>(i * 2, (i + 1) * 2) = m;
        }
    }

    // A^-1
    const decltype(A) invA = A.inverse();
    //const decltype(A) invA = PseudoInverse(A);

    // P_2 A^-1 B_n
    const auto Bn_np   = nablaC[n - 3].C;
    const auto invA_0n = Mat2(invA(0, 2*(n-3)), invA(1, 2*(n-3)), invA(0, 2*(n-3)+1), invA(1, 2*(n-3)+1));
    const auto invA_Bn = invA_0n * Bn_np;
    const auto det     = invA_Bn[0][0] * invA_Bn[1][1] - invA_Bn[1][0] * invA_Bn[0][1];

    // --------------------------------------------------------------------------------
    return Math::Abs(det);
}

auto ManifoldUtils::WalkManifold(const Scene3* scene, const Subpath& seedPath, const Vec3& target) -> boost::optional<Subpath>
{
    Subpath subpath;
    if (!WalkManifold(scene, seedPath, target, subpath))
    {
        return boost::none;
    }
    return subpath;
}

// Returns the converged path. Returns none if not converged.
auto ManifoldUtils::WalkManifold(const Scene3* scene, const Subpath& seedPath, const Vec3& target, Subpath& connPath) -> bool
{
	#pragma region Preprocess

	// Number of path vertices
	const int n = (int)(seedPath.vertices.size());

	// Initial path
    Subpath currP;
    currP = seedPath;

	#pragma endregion

    // --------------------------------------------------------------------------------

    #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
    LM_LOG_DEBUG("seed_path");
    {
        DebugIO::Wait();
        std::vector<double> vs;
        for (const auto& v : currP.vertices)
        {
            for (int i = 0; i < 3; i++) vs.push_back(v.geom.p[i]);
        }
        std::stringstream ss;
        {
            cereal::JSONOutputArchive oa(ss);
            oa(vs);
        }
        DebugIO::Output("seed_path", ss.str());
    }
    #endif

	// --------------------------------------------------------------------------------

    #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
    LM_LOG_DEBUG("target");
    {
        DebugIO::Wait();
        std::vector<double> vs;
        for (int i = 0; i < 3; i++) vs.push_back(target[i]);
        std::stringstream ss;
        {
            cereal::JSONOutputArchive oa(ss);
            oa(vs);
        }
        DebugIO::Output("target", ss.str());
    }
    #endif

    // --------------------------------------------------------------------------------

    #if 0
    #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
    LM_LOG_DEBUG("tanget_frame_v1");
    {
        DebugIO::Wait();
        std::stringstream ss;
        {
            cereal::JSONOutputArchive oa(ss);
            const auto& v = currP.vertices[1];
            oa(v.geom.p, v.geom.sn, v.geom.dpdu, v.geom.dpdv);
        }
        DebugIO::Output("tanget_frame_v1", ss.str());
    }
    #endif
    #endif

    // --------------------------------------------------------------------------------

	#pragma region Optimization loop

	const Float MaxBeta = 100.0;
    #if INVERSEMAP_MANIFOLDWALK_BETA_EXT
    Vec2 beta(MaxBeta, MaxBeta);
    #else
	Float beta = MaxBeta;
    #endif
	const Float Eps = 1e-4;
	const int MaxIter = 50;

	for (int iteration = 0; iteration < MaxIter; iteration++)
	{
        #if INVERSEMAP_MANIFOLDWALK_OUTPUT_FAILED_TRIAL_PATHS
        {
            static long long count = 0;
            if (count == 0)
            {
                boost::filesystem::remove("dirs.out");
            }
            {
                count++;
                std::ofstream out("dirs.out", std::ios::out | std::ios::app);
                for (const auto& v : currP.vertices)
                {
                    out << boost::str(boost::format("%.10f %.10f %.10f ") % v.geom.p.x % v.geom.p.y % v.geom.p.z);
                }
                out << std::endl;
            }
        }
        #endif

        // --------------------------------------------------------------------------------

        #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
        LM_LOG_DEBUG("current_path");
        {
            DebugIO::Wait();
            std::vector<double> vs;
            for (const auto& v : currP.vertices)
            {
                for (int i = 0; i < 3; i++) vs.push_back(v.geom.p[i]);
            }
            std::stringstream ss;
            {
                cereal::JSONOutputArchive oa(ss);
                oa(vs);
            }
            DebugIO::Output("current_path", ss.str());
        }
        #endif

        // --------------------------------------------------------------------------------

        #if 0
        #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
        LM_LOG_DEBUG("current_tanget_frame_v1");
        {
            DebugIO::Wait();
            std::stringstream ss;
            {
                cereal::JSONOutputArchive oa(ss);
                const auto& v = currP.vertices[1];
                oa(v.geom.p, v.geom.sn, v.geom.dpdu, v.geom.dpdv);
            }
            DebugIO::Output("current_tanget_frame_v1", ss.str());
        }
        #endif
        #endif

        // --------------------------------------------------------------------------------

        #if 0
        {
            const auto d = Math::Length(currP.vertices.back().geom.p - target);
            LM_LOG_DEBUG(boost::str(boost::format("#%02d: Dist to target %.15f") % iteration % d));
        }
        #endif

        // --------------------------------------------------------------------------------

        // Compute \nabla C
        ConstraintJacobian nablaC;
        nablaC.assign(n - 2, VertexConstraintJacobian());
        ComputeConstraintJacobian(currP, nablaC);

        // Compute L
        Float L = 0;
        for (const auto& x : currP.vertices) { L = Math::Max(L, Math::Length(x.geom.p)); }

        // --------------------------------------------------------------------------------

		#pragma region Stop condition
		if (Math::Length(currP.vertices[n - 1].geom.p - target) < Eps * L)
		{
            //LM_LOG_INFO("Converged");
            connPath = currP;
            return true;
		}
		#pragma endregion

		// --------------------------------------------------------------------------------

		#pragma region Compute movement in tangement plane
		// New position of initial specular vertex
		const auto p = [&]() -> Vec3
		{
			// x_n, x'_n
			const auto& xn = currP.vertices[n - 1].geom.p;
			const auto& xnp = target;

			// T(x_n)^T
            const auto Txn = Mat3x2(currP.vertices[n - 1].geom.dpdu, currP.vertices[n - 1].geom.dpdv);
			const auto TxnT = Math::Transpose(Txn);

			// V \equiv B_n T(x_n)^T (x'_n - x)
			const auto Bn_n2p = nablaC[n - 3].C;
			const auto V_n2p = Bn_n2p * TxnT * (xnp - xn);

			// Solve AW = V
            std::vector<Vec2> V;
            V.assign(n - 2, Vec2());
            std::vector<Vec2> W;
            W.assign(n - 2, Vec2());
			for (int i = 0; i < n - 2; i++) { V[i] = i == n - 3 ? V_n2p : Vec2(); }
            #if !INVERSEMAP_MANIFOLDWALK_USE_EIGEN_SOLVER
			SolveBlockLinearEq(nablaC, V, W);
            #else
            Matrix nablaC_;
            nablaC_.setZero(2 * (n - 2), 2 * (n - 2));
            for (int i = 0; i < n - 2; i++)
            {
                if (i > 0)
                {
                    const auto& A = nablaC[i].A;
                    Eigen::Array22d m;
                    m << A[0][0], A[1][0],
                         A[0][1], A[1][1];
                    nablaC_.block<2, 2>(i * 2, (i - 1) * 2) = m;
                }
                {
                    const auto& B = nablaC[i].B;
                    Eigen::Array22d m;
                    m << B[0][0], B[1][0],
                         B[0][1], B[1][1];
                    nablaC_.block<2, 2>(i * 2, i * 2) = m;
                }
                if (i < n - 2 - 1)
                {
                    const auto& C = nablaC[i].C;
                    Eigen::Array22d m;
                    m << C[0][0], C[1][0],
                         C[0][1], C[1][1];
                    nablaC_.block<2, 2>(i * 2, (i + 1) * 2) = m;

                }
            }
            Vector V_;
            V_.setZero(2 * (n - 2));
            for (int i = 0; i < n - 2; i++) { V_(2*i) = V[i].x; V_(2*i+1) = V[i].y; }
            Vector W_;
            W_ = nablaC_.colPivHouseholderQr().solve(V_);
            for (int i = 0; i < n - 2; i++) { W[i].x = W_(2 * i); W[i].y = W_(2 * i + 1); }
            #endif

			// x_2, T(x_2)
			const auto& x2 = currP.vertices[1].geom.p;
			const Mat3x2 Tx2(currP.vertices[1].geom.dpdu, currP.vertices[1].geom.dpdv);

            #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
            LM_LOG_DEBUG("points_on_tangent_s");
            {
                DebugIO::Wait();
                std::vector<Vec3> vs;
                for (int i = 0; i < n - 2; i++)
                {
                    const Mat3x2 Tx(currP.vertices[i + 1].geom.dpdu, currP.vertices[i + 1].geom.dpdv);
                    vs.push_back(currP.vertices[i + 1].geom.p +  Tx * W[i]);
                }
                std::stringstream ss;
                {
                    cereal::JSONOutputArchive oa(ss);
                    oa(vs);
                }
                DebugIO::Output("points_on_tangent_s", ss.str());
            }
            #endif

			// W_{n-2} = P_2 W
			//const auto Wn2p = W[n - 3];
            const auto Wn2p = W[0];

			// p = x_2 - \beta T(x_2) P_2 W_{n-2}
            #if INVERSEMAP_MANIFOLDWALK_BETA_EXT
            const auto t1 = Vec2(Wn2p.x * beta.x, Wn2p.y * beta.y);
            const auto t2 = Tx2 * t1;
            #else
            const auto t1 = Tx2 * Wn2p;
            const auto t2 = t1 * beta;
            #endif
			return x2 - t2;
        }();
		#pragma endregion

        // --------------------------------------------------------------------------------

        #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
        LM_LOG_DEBUG("point_on_tangent");
        {
            DebugIO::Wait();
            std::vector<double> vs;
            for (int i = 0; i < 3; i++) vs.push_back(p[i]);
            std::stringstream ss;
            {
                cereal::JSONOutputArchive oa(ss);
                oa(vs);
            }
            DebugIO::Output("point_on_tangent", ss.str());
        }
        #endif

		// --------------------------------------------------------------------------------

		#pragma region Propagate light path to p - x1
        const auto nextP = [&]() -> boost::optional<Subpath>
        {
            Subpath nextP;
            nextP.vertices.clear();
            nextP.vertices.push_back(currP.vertices[0]);
            for (int i = 1; i < n; i++)
            {
                // Current vertex & previous vertex
                const auto* vp = &nextP.vertices[i - 1];
                const auto* vpp = i - 2 >= 0 ? &nextP.vertices[i - 2] : nullptr;

                // Next ray direction
                const auto wo = [&]()
                {
                    if (i == 1) { return Math::Normalize(p - vp->geom.p); }
                    else
                    {
                        assert(vp->type == SurfaceInteractionType::S);
                        const auto uC = [&]() -> Float
                        {
                            // Fix sampled component for Flesnel material (TODO. refactor it)
                            // Vertices in current path
                            const auto& curr_v  = currP.vertices[i - 1];
                            const auto& curr_vp = currP.vertices[i - 2];
                            const auto& curr_vn = currP.vertices[i];
                            const auto wo = Math::Normalize(curr_vn.geom.p - curr_v.geom.p);
                            const auto wi = Math::Normalize(curr_vp.geom.p - curr_v.geom.p);
                            const auto localWo = curr_v.geom.ToLocal * wo;
                            const auto localWi = curr_v.geom.ToLocal * wi;
                            return Math::LocalCos(localWi) * Math::LocalCos(localWo) >= 0_f ? 0_f : 1_f;
                        }();
                        Vec3 wo;
                        vp->primitive->SampleDirection(Vec2(), uC, vp->type, vp->geom, Math::Normalize(vpp->geom.p - vp->geom.p), wo);
                        return wo;
                    }
                }();

                // Intersection query
                Ray ray = { vp->geom.p, wo };
                Intersection isect;
                if (!scene->Intersect(ray, isect))
                {
                    return boost::none;
                }

                // Fails if not intersected with specular vertex, except for the last vertex.
                if (i <= n - 2 && (isect.primitive->Type() & SurfaceInteractionType::S) == 0)
                {
                    return boost::none;
                }

                // Fails if the last vertex is S
                if (i == n - 1 && (isect.primitive->Type() & SurfaceInteractionType::S) != 0)
                {
                    return boost::none;
                }

                // Add vertex
                SubpathSampler::PathVertex v;
                v.geom = isect.geom;
                v.primitive = isect.primitive;
                v.type = isect.primitive->Type() & ~SurfaceInteractionType::Emitter;
                nextP.vertices.push_back(v);
            }
            return nextP;
        }();
		#pragma endregion

        // --------------------------------------------------------------------------------

        #if INVERSEMAP_MANIFOLDWALK_DEBUG_IO
        if (nextP)
        {
            LM_LOG_DEBUG("next_path");
            DebugIO::Wait();
            std::vector<double> vs;
            for (const auto& v : nextP->vertices)
            {
                for (int i = 0; i < 3; i++) vs.push_back(v.geom.p[i]);
            }
            std::stringstream ss;
            {
                cereal::JSONOutputArchive oa(ss);
                oa(vs);
            }
            DebugIO::Output("next_path", ss.str());
        }
        #endif

        // --------------------------------------------------------------------------------

        #if INVERSEMAP_MANIFOLDWALK_OUTPUT_FAILED_TRIAL_PATHS
        if (nextP)
        {
            static long long count = 0;
            if (count == 0)
            {
                boost::filesystem::remove("dirs_next.out");
            }
            {
                count++;
                std::ofstream out("dirs_next.out", std::ios::out | std::ios::app);
                for (const auto& v : nextP->vertices)
                {
                    out << boost::str(boost::format("%.10f %.10f %.10f ") % v.geom.p.x % v.geom.p.y % v.geom.p.z);
                }
                out << std::endl;
            }
        }
        #endif

        // --------------------------------------------------------------------------------

		#pragma region Update beta
        const auto update = [&]() -> bool
        {
            if (!nextP)
            {
                return true;
            }
            // Update beta if nextP shows larger difference to target
            const auto d  = Math::Length2(currP.vertices.back().geom.p - target);
            const auto dn = Math::Length2(nextP->vertices.back().geom.p - target);
            //LM_LOG_INFO(boost::str(boost::format("d, dn: %.15f %.15f") % d % dn));
            if (dn >= d)
            {
                return true;
            }
            return false;
        }();
        if (update)
        {
            #if INVERSEMAP_MANIFOLDWALK_BETA_EXT
            if (Math::Abs(beta.x) > Math::Abs(beta.y)) { beta.x *= -0.5_f; }
            else { beta.y *= -0.5_f; }
            #else
            beta *= -0.5_f;
            #endif
            //LM_LOG_INFO(boost::str(boost::format("- beta: %.15f") % beta));
        }
        else
        {
            #if INVERSEMAP_MANIFOLDWALK_BETA_EXT
            beta.x = Math::Clamp(beta.x * 2_f, -MaxBeta, MaxBeta);
            beta.y = Math::Clamp(beta.y * 2_f, -MaxBeta, MaxBeta);
            //if (Math::Abs(beta.x) > Math::Abs(beta.y)) { beta.y = Math::Clamp(beta.y * 2_f, -MaxBeta, MaxBeta); }
            //else { beta.x = Math::Clamp(beta.x * 2_f, -MaxBeta, MaxBeta); }
            #else
            beta = Math::Clamp(beta * 2_f, -MaxBeta, MaxBeta);
            #endif
            //LM_LOG_INFO(boost::str(boost::format("+ beta: %.15f") % beta));
            currP = *nextP;
        }
		#pragma endregion
	}

	#pragma endregion

	// --------------------------------------------------------------------------------
	return false;
}

LM_NAMESPACE_END


/*
 * adjointness.cpp
 *
 *  Created on: 22.07.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>

#include <base/AdaptiveMesh.h>
#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/MacroFunctionParser.h>
#include <base/SpaceTimeMesh.h>
#include <base/Transformation.h>
#include <base/Tuple.h>
#include <base/Util.h>
#include <forward/L2RightHandSide.h>
#include <forward/WaveEquation.h>
#include <forward/WaveEquationAdjoint.h>
#include <forward/WaveEquationBase.h>
#include <measurements/ConvolutionMeasure.h>
#include <measurements/DeltaMeasure.h>
#include <measurements/GridDistribution.h>
#include <measurements/Measure.h>
#include <measurements/SensorDistribution.h>
#include <norms/L2L2.h>
#include <problems/QProblem.h>
#include <problems/WaveProblem.h>

#include <gtest/gtest.h>

#include <stddef.h>
#include <cmath>
#include <iostream>
#include <map>
#include <memory>
#include <string>
#include <vector>

namespace {

using namespace dealii;
using namespace wavepi::base;
using namespace wavepi;
using namespace wavepi::measurements;

enum class MeasureType { convolution = 1, delta };

template <int dim>
void run_sensor_measure_adjoint_test(MeasureType measure_type, int fe_order, int quad_order, int refines, int n_steps) {
  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 1.0, dt = t_end / n_steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(fe_order);
  Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

  std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  const size_t PPD = 2;

  std::vector<double> mtimes(PPD, 0.0);
  for (size_t i = 0; i < mtimes.size(); i++)
    mtimes[i] = (i + 1.0) / (mtimes.size() + 1.0);

  std::vector<std::vector<double>> spatial_points;
  for (size_t d = 0; d < dim; d++) {
    std::vector<double> tmp(PPD, 0.0);

    for (size_t i = 0; i < tmp.size(); i++)
      tmp[i] = ((i + 1.0) / (tmp.size() + 1.0)) * 2.0 - 1.0;

    spatial_points.push_back(tmp);
  }

  auto grid = std::make_shared<GridDistribution<dim>>(mtimes, spatial_points);
  std::shared_ptr<Measure<DiscretizedFunction<dim>, SensorValues<dim>>> measure;

  if (measure_type == MeasureType::convolution)
    measure = std::make_shared<ConvolutionMeasure<dim>>(mesh, grid, std::make_shared<norms::L2L2<dim>>(),
                                                        std::make_shared<typename ConvolutionMeasure<dim>::HatShape>(),
                                                        0.1, 0.2);
  else if (measure_type == MeasureType::delta)
    measure = std::make_shared<DeltaMeasure<dim>>(mesh, grid, std::make_shared<norms::L2L2<dim>>());

  double tol = 1e-06;

  for (int i = 0; i < 10; i++) {
    DiscretizedFunction<dim> f = DiscretizedFunction<dim>::noise(mesh);
    f.set_norm(std::make_shared<norms::L2L2<dim>>());

    SensorValues<dim> g = SensorValues<dim>::noise(grid);

    Timer eval_timer;
    eval_timer.start();
    auto Psif = measure->evaluate(f);
    eval_timer.stop();

    Timer adj_timer;
    adj_timer.start();
    auto PsiAdjg = measure->adjoint(g);
    AssertThrow(*PsiAdjg.get_norm() == norms::L2L2<dim>(), ExcInternalError());
    adj_timer.stop();

    double dot_Psif_g    = Psif * g;
    double dot_f_Psiadjg = f * PsiAdjg;
    double mfg_err       = std::abs(dot_Psif_g - dot_f_Psiadjg) / (std::abs(dot_Psif_g) + 1e-300);

    deallog << "wall time evaluate: " << std::fixed << eval_timer.wall_time() << " s" << std::endl;
    deallog << "wall time adjoint: " << std::fixed << adj_timer.wall_time() << " s" << std::endl;

    deallog << std::scientific << "(Ψf, g) = " << dot_Psif_g << ", (f, Ψ*g) = " << dot_f_Psiadjg
            << ", rel. error = " << mfg_err << std::endl;

    EXPECT_LT(mfg_err, tol);
  }

  deallog << std::endl;
}

// tests whether the specialized implementation of delta_measure for constant meshes yields the same result as for
// general meshes
template <int dim>
void run_delta_measure_implementation_test(int fe_order, int quad_order, int refines, int n_steps) {
  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 1.0, dt = t_end / n_steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(fe_order);
  Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

  std::shared_ptr<SpaceTimeMesh<dim>> mesh  = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);
  std::shared_ptr<SpaceTimeMesh<dim>> amesh = std::make_shared<AdaptiveMesh<dim>>(times, fe, quad, triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  const size_t PPD = 10;

  std::vector<double> mtimes(PPD, 0.0);
  for (size_t i = 0; i < mtimes.size(); i++)
    mtimes[i] = (i + 1.0) / (mtimes.size() + 1.0);

  std::vector<std::vector<double>> spatial_points;
  for (size_t d = 0; d < dim; d++) {
    std::vector<double> tmp(PPD, 0.0);

    for (size_t i = 0; i < tmp.size(); i++)
      tmp[i] = ((i + 1.0) / (tmp.size() + 1.0)) * 2.0 - 1.0;

    spatial_points.push_back(tmp);
  }

  auto grid     = std::make_shared<GridDistribution<dim>>(mtimes, spatial_points);
  auto measure  = std::make_shared<DeltaMeasure<dim>>(mesh, grid, std::make_shared<norms::L2L2<dim>>());
  auto ameasure = std::make_shared<DeltaMeasure<dim>>(amesh, grid, std::make_shared<norms::L2L2<dim>>());

  double tol = 1e-06;

  for (int i = 0; i < 10; i++) {
    DiscretizedFunction<dim> f = DiscretizedFunction<dim>::noise(mesh);
    f.set_norm(std::make_shared<norms::L2L2<dim>>());

    DiscretizedFunction<dim> af = DiscretizedFunction<dim>(amesh);
    for (size_t ti = 0; ti < mesh->length(); ti++)
      af[ti] = f[ti];

    SensorValues<dim> g = SensorValues<dim>::noise(grid);

    Timer timer;
    timer.restart();
    auto Psif = measure->evaluate(f);
    timer.stop();
    deallog << "wall time evaluate (ConstantMesh): " << std::fixed << timer.wall_time() << " s" << std::endl;

    timer.restart();
    auto aPsif = ameasure->evaluate(af);
    timer.stop();
    deallog << "wall time evaluate (AdaptiveMesh): " << std::fixed << timer.wall_time() << " s" << std::endl;

    timer.restart();
    auto PsiAdjg = measure->adjoint(g);
    AssertThrow(*PsiAdjg.get_norm() == norms::L2L2<dim>(), ExcInternalError());
    timer.stop();
    deallog << "wall time adjoint (ConstantMesh): " << std::fixed << timer.wall_time() << " s" << std::endl;

    timer.restart();
    auto aPsiAdjg = ameasure->adjoint(g);
    AssertThrow(*aPsiAdjg.get_norm() == norms::L2L2<dim>(), ExcInternalError());
    timer.stop();
    deallog << "wall time adjoint (AdaptiveMesh): " << std::fixed << timer.wall_time() << " s" << std::endl;

    DiscretizedFunction<dim> PsiAdjg_copy = DiscretizedFunction<dim>(amesh, PsiAdjg.get_norm());
    for (size_t ti = 0; ti < mesh->length(); ti++)
      PsiAdjg_copy[ti] = PsiAdjg[ti];

    double psi_err = aPsif.relative_error(Psif);
    deallog << std::scientific << "error [ Ψf (ConstantMesh - Ψf (AdaptiveMesh) ] = " << psi_err << std::endl;

    EXPECT_LT(psi_err, tol);

    double psiadj_err = aPsiAdjg.relative_error(PsiAdjg_copy);
    deallog << std::scientific << "error [ Ψ*f (ConstantMesh - Ψ*f (AdaptiveMesh) ] = " << psiadj_err << std::endl;

    EXPECT_LT(psiadj_err, tol);
  }

  deallog << std::endl;
}

}  // namespace

TEST(Measurements, ConvolutionMeasureAdjointness1DFE1) {
  run_sensor_measure_adjoint_test<1>(MeasureType::convolution, 1, 4, 9, 256);
}

TEST(Measurements, ConvolutionMeasureAdjointness1DFE2) {
  run_sensor_measure_adjoint_test<1>(MeasureType::convolution, 2, 4, 7, 128);
}

TEST(Measurements, ConvolutionMeasureAdjointness2DFE1) {
  run_sensor_measure_adjoint_test<2>(MeasureType::convolution, 1, 4, 5, 256);
}

TEST(Measurements, ConvolutionMeasureAdjointness2DFE2) {
  run_sensor_measure_adjoint_test<2>(MeasureType::convolution, 2, 4, 5, 128);
}

TEST(Measurements, ConvolutionMeasureAdjointness3DFE1) {
  run_sensor_measure_adjoint_test<3>(MeasureType::convolution, 1, 4, 4, 128);
}

TEST(Measurements, DeltaMeasureAdjointness1DFE1) {
  run_sensor_measure_adjoint_test<1>(MeasureType::delta, 1, 4, 9, 256);
}

TEST(Measurements, DeltaMeasureAdjointness1DFE2) {
  run_sensor_measure_adjoint_test<1>(MeasureType::delta, 2, 4, 7, 128);
}

TEST(Measurements, DeltaMeasureAdjointness2DFE1) {
  run_sensor_measure_adjoint_test<2>(MeasureType::delta, 1, 4, 5, 256);
}

TEST(Measurements, DeltaMeasureAdjointness2DFE2) {
  run_sensor_measure_adjoint_test<2>(MeasureType::delta, 2, 4, 5, 128);
}

TEST(Measurements, DeltaMeasureAdjointness3DFE1) {
  run_sensor_measure_adjoint_test<3>(MeasureType::delta, 1, 4, 4, 128);
}

TEST(Measurements, DeltaMeasureImplementation1DFE1) { run_delta_measure_implementation_test<1>(1, 4, 9, 256); }

TEST(Measurements, DeltaMeasureImplementation1DFE2) { run_delta_measure_implementation_test<1>(2, 4, 7, 128); }

TEST(Measurements, DeltaMeasureImplementation2DFE1) { run_delta_measure_implementation_test<2>(1, 4, 5, 256); }

TEST(Measurements, DeltaMeasureImplementation2DFE2) { run_delta_measure_implementation_test<2>(2, 4, 5, 128); }

TEST(Measurements, DeltaMeasureImplementation3DFE1) { run_delta_measure_implementation_test<3>(1, 4, 3, 128); }


#include <boost/math/distributions/chi_squared.hpp>


#pragma once

#include "quadrature/qhermitew.hpp"
#include <Eigen/Dense>

namespace boltzmann {

template <typename DERIVED, typename FUNC>
void
to_nodal(Eigen::DenseBase<DERIVED>& dst, const FUNC f)
{
  assert(dst.rows() == dst.cols());

  int K = dst.rows();
  QHermiteW quad(1.0, K);

  auto& x = quad.pts();
  auto& w = quad.wts();

  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      // convection: x -> rows, y -> cols (for example, see Polar2Nodal)
      dst(i, j) = f(x[j], x[i]) * std::sqrt(w[i] * w[j]);
    }
  }
}

}  // end namespace boltzmann


#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes the average value of the function f over all possible inputs
 * @param f The function to compute the average of
 * Is interpreted to be unsigned
 * @return the average value of f as a high precision float
 */
boost::multiprecision::cpp_dec_float_100 average_value(const std::vector<BDD>& f);

/**
 * @brief Computes the average squared value of the function f over all possible inputs
 * @param f The function to compute the average squared value of
 * Is interpreted to be unsigned
 * @return the mean squared value of f as a high precision float
 */
boost::multiprecision::cpp_dec_float_100 mean_squared_value(const std::vector<BDD>& f);

/**
 * @brief Computes the average absolute difference between the functions f and f_hat
 * The computation is performed symbolically with BDD forests and may take exponential time
 * @param mgr Cudd manager object
 * @param f The original function
 * @param f_hat The approximated function
 * @param num_rep The number representation for f and f_hat
 * @return The average absolute difference between f and f_hat
 */
boost::multiprecision::cpp_dec_float_100
average_case_error(const Cudd& mgr, const std::vector<BDD>& f, const std::vector<BDD>& f_hat,
                   const abo::util::NumberRepresentation num_rep = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average squared absolute difference between the functions f and f_hat
 * The computation is performed symbolically with BDD forests and may take exponential time
 * @param mgr Cudd manager object
 * @param f The original function
 * @param f_hat The approximated function
 * @param num_rep The number representation for f and f_hat
 * @return The average squared absolute difference between f and f_hat
 */
boost::multiprecision::cpp_dec_float_100
mean_squared_error(const Cudd& mgr, const std::vector<BDD>& f,
                   const std::vector<BDD>& f_hat,
                   const abo::util::NumberRepresentation num_rep
                        = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average absolute difference between the functions f and f_hat
 * The computation is performed symbolically with ADDs and may take exponential time
 * This function is generally slower than the equivalent average_case_error() function
 * and should only be used as a reference
 * @param mgr Cudd manager object
 * @param f The original function
 * @param f_hat The approximated function
 * @param num_rep The number representation for f and f_hat
 * @return The average absolute difference between f and f_hat
 */
boost::multiprecision::cpp_dec_float_100
average_case_error_add(const Cudd& mgr, const std::vector<BDD>& f,
                       const std::vector<BDD>& f_hat,
                       const abo::util::NumberRepresentation num_rep
                            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average squared absolute difference between the functions f and f_hat
 * The computation is performed symbolically with ADDs and may take exponential time
 * This function is generally slower than the equivalent mean_squared_error() function
 * and should only be used as a reference
 * @param mgr Cudd manager object
 * @param f The original function
 * @param f_hat The approximated function
 * @param num_rep The number representation for f and f_hat
 * @return The average squared absolute difference between f and f_hat
 */
boost::multiprecision::cpp_dec_float_100
mean_squared_error_add(const Cudd& mgr, const std::vector<BDD>& f,
                       const std::vector<BDD>& f_hat,
                       const abo::util::NumberRepresentation num_rep
                            = abo::util::NumberRepresentation::BaseTwo);

} // namespace abo::error_metrics


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Perlin

#include <boost/test/unit_test.hpp>
#include <geomc/function/PerlinNoise.h>
#include <geomc/random/MTRand.h>
#include <geomc/random/RandomTools.h>

using namespace geom;
using namespace std;

// todo: should figure out a way to get a copy of a PerlinNoise in Dual form,
//       and verify the gradient that way.

// return rms(error), max(error)
template <typename T, index_t N>
std::pair<T,T> perlin_gradient(Random* rng, index_t n_trials) {
    PerlinNoise<T,N> pn(rng);
    Sampler<T> smp(rng);
    const T eps = 0.00001;

    T err_sq  = (T)0;
    T err_max = (T)0;

    for (index_t i = 0; i < n_trials; ++i) {
        Vec<T,N>  x = 512 * smp.template solidball<N>(); // :G
        auto   x_dx = pn.gradient(x);
        Vec<T,N> g;

        // finite difference the gradient
        for (index_t axis = 0; axis < N; axis++) {
            Vec<T,N> dx;
            dx[axis] = eps;
            g[axis]  = (pn.eval(x + dx) - pn.eval(x - dx)) / (2 * eps);
        }

        // gradient()'s opinion on f(x) should be the same as eval()'s.
        BOOST_CHECK_CLOSE(x_dx.first, pn.eval(x), eps);

        g      -= x_dx.second;
        T e     = g.dot(g);
        err_sq += e;
        err_max = std::max(e, err_max);
    }

    return std::pair<T,T>(
        std::sqrt(err_sq) / n_trials,
        std::sqrt(err_max));
}

BOOST_AUTO_TEST_SUITE(perlin_noise)

BOOST_AUTO_TEST_CASE(test_perlin_gradient) {
    MTRand rng = MTRand(1017381749271967481LL);
    std::pair<double, double> k;
    k = perlin_gradient<double,2>(&rng, 10000);
    BOOST_CHECK_SMALL(k.first,  5e-11);
    BOOST_CHECK_SMALL(k.second, 1e-8);
    k = perlin_gradient<double,3>(&rng, 10000);
    BOOST_CHECK_SMALL(k.first,  5e-11);
    BOOST_CHECK_SMALL(k.second, 1e-8);
    k = perlin_gradient<double,4>(&rng, 10000);
    BOOST_CHECK_SMALL(k.first,  5e-11);
    BOOST_CHECK_SMALL(k.second, 1e-8);
}

BOOST_AUTO_TEST_SUITE_END()


#include <cmath>

#include <functional>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Propulsion/costateBasedThrustGuidance.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/modifiedEquinoctialElementConversions.h"

namespace tudat
{

namespace propulsion
{

//! Constructor
MeeCostateBasedThrustGuidance::MeeCostateBasedThrustGuidance(
        const std::function< Eigen::Vector6d( ) > thrustingBodyStateFunction,
        const std::function< Eigen::Vector6d( ) > centralBodyStateFunction,
        const std::function< double( ) > centralBodyGravitationalParameterFunction,
        std::function< Eigen::VectorXd( const double ) > costateFunction,
        const std::function< Eigen::Vector3d( ) > bodyFixedForceDirection )
    : BodyFixedForceDirectionGuidance( bodyFixedForceDirection ),
      thrustingBodyStateFunction_( thrustingBodyStateFunction ),
      centralBodyStateFunction_( centralBodyStateFunction ),
      centralBodyGravitationalParameterFunction_( centralBodyGravitationalParameterFunction ),
      costateFunction_( costateFunction ){ }

//! Function to update the force direction to the current time.
void MeeCostateBasedThrustGuidance::updateForceDirection( const double time )
{
    if( !( time == currentTime_ ) )
    {
        Eigen::VectorXd costates_ = costateFunction_( time );

        // Get the current state in cartesian coordinates and keplerian elements, and some convenient parameters
        Eigen::Vector6d currentState = thrustingBodyStateFunction_( ) - centralBodyStateFunction_( );
        double centralBodyGravitationalParameter = centralBodyGravitationalParameterFunction_( );

        // Obtain ModifiedEquinoctial elements, flag of 0 indicates that singularity occurs at 180 deg inclination.
        Eigen::Vector6d modifiedEquinoctialElements =
                orbital_element_conversions::convertCartesianToModifiedEquinoctialElements(
                    currentState, centralBodyGravitationalParameter, 0 );

        // Optimal control laws local variables declared for clarity
        double auxiliaryParameterW = ( 1.0 + modifiedEquinoctialElements( 1 ) * cos( modifiedEquinoctialElements( 5 ) )
                     + modifiedEquinoctialElements( 2 ) * sin( modifiedEquinoctialElements( 5 ) ) );
        double auxiliaryParameterSSquared = 1.0 + modifiedEquinoctialElements( 3 ) * modifiedEquinoctialElements( 3 )
                + modifiedEquinoctialElements( 4 ) * modifiedEquinoctialElements( 4 );

        // Local variables for al constant terms for the calculation of pitch angle
        double Lap = costates_( 0 ) * 2.0 * modifiedEquinoctialElements( 0 ) / auxiliaryParameterW;
        double Laf1 = costates_( 1 )  * sin( modifiedEquinoctialElements( 5 ) ) ;
        double Laf2 = costates_( 1 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * cos( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 1 ) ) ;
        double Lag1 = costates_( 2 ) * cos( modifiedEquinoctialElements( 5 ) );
        double Lag2 = costates_( 2 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * sin( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 2 ) );

        // Calculate pitch angle, NOTE: denomitator ommitted since it is not relevant for the atan2 function,
        // since both denominators are the same.
        double thrustAngleAlpha = std::atan2( -Laf1+Lag1, -Lap-Laf2-Lag2);

        // Local variables for al constant terms for the calculation of yaw angle
        double Lbp = costates_( 0 ) * 2.0 * modifiedEquinoctialElements( 0 ) * cos( thrustAngleAlpha) / auxiliaryParameterW;
        double Lbf1 = costates_( 1 )  * sin( modifiedEquinoctialElements( 5 ) ) * sin( thrustAngleAlpha );
        double Lbf2 = costates_( 1 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * cos( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 1 ) ) * cos( thrustAngleAlpha );
        double Lbf3 = costates_( 1 ) / auxiliaryParameterW * ( modifiedEquinoctialElements( 2 ) *(
                                                 modifiedEquinoctialElements( 3 ) * sin( modifiedEquinoctialElements( 5 ) )
                                                 - modifiedEquinoctialElements( 4 ) * cos( modifiedEquinoctialElements( 5 ) ) ) );

        double Lbg1 = costates_( 2 ) * cos( modifiedEquinoctialElements( 5 ) ) * sin( thrustAngleAlpha);
        double Lbg2 = costates_( 2 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * sin( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 2 ) ) * cos( thrustAngleAlpha);
        double Lbg3 = costates_( 2 ) / auxiliaryParameterW * ( modifiedEquinoctialElements( 1 ) *(
                                                 modifiedEquinoctialElements( 3 ) * sin( modifiedEquinoctialElements( 5 ) )
                                                 - modifiedEquinoctialElements( 4 ) * cos( modifiedEquinoctialElements( 5 ) ) ) );
        double Lbh = costates_( 3 ) * auxiliaryParameterSSquared *
                cos( modifiedEquinoctialElements( 5 ) ) / ( 2.0 * auxiliaryParameterW );
        double Lbk = costates_( 4 ) * auxiliaryParameterSSquared *
                sin( modifiedEquinoctialElements( 5 ) ) / ( 2.0 * auxiliaryParameterW );

        // Calculate yaw angle, NOTE: denomitator ommitted since it is not relevant for the atan2 function,
        // since both denominators are the same.
        double thrustAngleBeta = std::atan2( Lbf3 - Lbg3 - Lbh - Lbk, - Lbp - Lbf1 - Lbf2 + Lbg1 - Lbg2 );

        // Calculate thrust direction
        currentForceDirection_ = reference_frames::getVelocityBasedLvlhToInertialRotation(
                    currentState, Eigen::Vector6d::Zero( ), false ) *
                ( ( Eigen::Vector3d( ) <<
                    cos( thrustAngleAlpha ) * cos( thrustAngleBeta ), sin( thrustAngleAlpha ) * cos( thrustAngleBeta ) ,
                    sin( thrustAngleBeta )  ).finished( ).normalized( ) );
        currentTime_ = time;

//        // Switching function for the thrust magnitude.
//        double thrustMagnitudeSwitchingCondition = /*( 1.0 / thrustingBodyMassFunction_( ) ) **/
//                ( Lbp * cos( thrustAngleBeta ) + Lbh * sin( thrustAngleBeta ) + Lbk * sin( thrustAngleBeta )
//                + Lbf1 * cos( thrustAngleBeta ) + Lbf2 * cos( thrustAngleBeta ) - Lbf3 * sin( thrustAngleBeta )
//                - Lbg1 * cos( thrustAngleBeta ) + Lbg2 * cos( thrustAngleBeta ) + Lbg3 * sin( thrustAngleBeta ) );
//        if ( thrustMagnitudeSwitchingCondition <= 0.0 )
//        {
//            std::cout << "INSIDE THRUST DIRECTION FUNCTION, THRUST ON. " << "\n\n";
//        }
//        else
//        {
//            std::cout << "INSIDE THRUST DIRECTION FUNCTION, THRUST OFF. " << "\n\n";
//        }
    }

}

} // namespace propulsion

} // namespace tudat


#include "../../include/IntrinsicFormula/KnoppelStripePattern.h"
#include "../../include/IntrinsicFormula/AmpSolver.h"
#include "../../include/Optimization/NewtonDescent.h"
#include <igl/cotmatrix.h>
#include <SymGEigsShiftSolver.h>
#include <MatOp/SparseCholesky.h>
#include <Eigen/CholmodSupport>
#include <MatOp/SparseSymShiftSolve.h>
#include <iostream>

using namespace IntrinsicFormula;

void IntrinsicFormula::computeMatrixA(const MeshConnectivity &mesh, const Eigen::MatrixXd &halfEdgeW,
									 const Eigen::VectorXd &faceArea, const Eigen::MatrixXd &cotEntries,
									 const int nverts, Eigen::SparseMatrix<double> &A)
{
	std::vector<Eigen::Triplet<double>> AT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	Eigen::VectorXd halfEdgeWeight(nedges);
	halfEdgeWeight.setConstant(1.0);

//    for (int i = 0; i < nfaces; i++)  // form mass matrix
//    {
//        for (int j = 0; j < 3; j++)
//        {
//            int eid = mesh.faceEdge(i, j);
//            halfEdgeWeight(eid) += cotEntries(i, j);
//        }
//    }
	for(int i = 0; i < nedges; i++)
	{
		int vid0 = mesh.edgeVertex(i, 0);
		int vid1 = mesh.edgeVertex(i, 1);

		AT.push_back({2 * vid0, 2 * vid0, 2 * halfEdgeWeight(i)});
		AT.push_back({2 * vid0 + 1, 2 * vid0 + 1, 2 * halfEdgeWeight(i)});

		AT.push_back({2 * vid1, 2 * vid1, 2 * halfEdgeWeight(i)});
		AT.push_back({2 * vid1 + 1, 2 * vid1 + 1, 2 * halfEdgeWeight(i)});

		std::complex<double> expw0 = std::complex<double>(std::cos(halfEdgeW(i, 0)), std::sin(halfEdgeW(i, 0)));
		std::complex<double> expw1 = std::complex<double>(std::cos(halfEdgeW(i, 1)), std::sin(halfEdgeW(i, 1)));

		AT.push_back({2 * vid0, 2 * vid1, -halfEdgeWeight(i) * (expw0.real() + expw1.real())});
		AT.push_back({2 * vid0 + 1, 2 * vid1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag())});
		AT.push_back({2 * vid0, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag())});
		AT.push_back({2 * vid0 + 1, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real())});

		AT.push_back({ 2 * vid1, 2 * vid0, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) });
		AT.push_back({ 2 * vid1, 2 * vid0 + 1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) });
		AT.push_back({ 2 * vid1 + 1, 2 * vid0, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) });
		AT.push_back({ 2 * vid1 + 1, 2 * vid0 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) });

	}
	A.resize(2 * nverts, 2 * nverts);
	A.setFromTriplets(AT.begin(), AT.end());
}

void IntrinsicFormula::computeMatrixAGivenMag(const MeshConnectivity &mesh, const Eigen::MatrixXd &halfEdgeW, const Eigen::VectorXd& vertAmp, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const int nverts, Eigen::SparseMatrix<double>& A) {
	std::vector<Eigen::Triplet<double>> AT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	Eigen::VectorXd halfEdgeWeight(nedges);
	halfEdgeWeight.setConstant(1.0);

//    for (int i = 0; i < nfaces; i++)  // form mass matrix
//    {
//        for (int j = 0; j < 3; j++)
//        {
//            int eid = mesh.faceEdge(i, j);
//            halfEdgeWeight(eid) += cotEntries(i, j);
//        }
//    }

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

		double r0 = vertAmp(vid0);
		double r1 = vertAmp(vid1);

		std::complex<double> expw0 = std::complex<double>(std::cos(halfEdgeW(i, 0)), std::sin(halfEdgeW(i, 0)));
		std::complex<double> expw1 = std::complex<double>(std::cos(halfEdgeW(i, 1)), std::sin(halfEdgeW(i, 1)));

		AT.push_back({2 * vid0, 2 * vid0, 2 * r1 * r1 * halfEdgeWeight(i)});
		AT.push_back({2 * vid0 + 1, 2 * vid0 + 1, 2 * r1 * r1 * halfEdgeWeight(i)});

		AT.push_back({2 * vid1, 2 * vid1, 2 * r0 * r0 * halfEdgeWeight(i)});
		AT.push_back({2 * vid1 + 1, 2 * vid1 + 1, 2 * r0 * r0 * halfEdgeWeight(i)});

		AT.push_back({2 * vid0, 2 * vid1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1});
		AT.push_back({2 * vid0 + 1, 2 * vid1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) * r0 * r1});
		AT.push_back({2 * vid0, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) * r0 * r1});
		AT.push_back({2 * vid0 + 1, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1});

		AT.push_back({2 * vid1, 2 * vid0, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1});
		AT.push_back({2 * vid1, 2 * vid0 + 1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) * r0 * r1});
		AT.push_back({2 * vid1 + 1, 2 * vid0, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) * r0 * r1});
		AT.push_back({2 * vid1 + 1, 2 * vid0 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1});
	}
	A.resize(2 * nverts, 2 * nverts);
	A.setFromTriplets(AT.begin(), AT.end());
}

double IntrinsicFormula::KnoppelEnergy(const MeshConnectivity& mesh, const Eigen::MatrixXd& halfEdgeW, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const std::vector<std::complex<double>>& zvals, Eigen::VectorXd* deriv, std::vector<Eigen::Triplet<double>>* hess)
{
	std::vector<Eigen::Triplet<double>> AT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();
	int nverts = zvals.size();

	Eigen::VectorXd halfEdgeWeight(nedges);
	halfEdgeWeight.setConstant(1.0);

//    for (int i = 0; i < nfaces; i++)  // form mass matrix
//    {
//        for (int j = 0; j < 3; j++)
//        {
//            int eid = mesh.faceEdge(i, j);
//            halfEdgeWeight(eid) += cotEntries(i, j);
//        }
//    }
	double energy = 0;

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

		std::complex<double> expw0 = std::complex<double>(std::cos(halfEdgeW(i, 0)), std::sin(halfEdgeW(i, 0)));
		std::complex<double> expw1 = std::complex<double>(std::cos(halfEdgeW(i, 1)), std::sin(halfEdgeW(i, 1)));

		std::complex<double> z0 = zvals[vid0];
		std::complex<double> z1 = zvals[vid1];

		energy += 0.5 * (norm((z0 * expw0 - z1)) + norm((z1 * expw1 - z0))) * halfEdgeWeight(i);

		if (deriv || hess)
		{
			AT.push_back({ 2 * vid0, 2 * vid0, 2 * halfEdgeWeight(i) });
			AT.push_back({ 2 * vid0 + 1, 2 * vid0 + 1, 2 * halfEdgeWeight(i) });

			AT.push_back({ 2 * vid1, 2 * vid1, 2 * halfEdgeWeight(i) });
			AT.push_back({ 2 * vid1 + 1, 2 * vid1 + 1, 2 * halfEdgeWeight(i) });

			AT.push_back({ 2 * vid0, 2 * vid1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) });
			AT.push_back({ 2 * vid0 + 1, 2 * vid1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) });
			AT.push_back({ 2 * vid0, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) });
			AT.push_back({ 2 * vid0 + 1, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) });

			AT.push_back({ 2 * vid1, 2 * vid0, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) });
			AT.push_back({ 2 * vid1, 2 * vid0 + 1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) });
			AT.push_back({ 2 * vid1 + 1, 2 * vid0, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) });
			AT.push_back({ 2 * vid1 + 1, 2 * vid0 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) });
		}
	}

	if (deriv || hess)
	{
		Eigen::SparseMatrix<double> A;

		A.resize(2 * nverts, 2 * nverts);
		A.setFromTriplets(AT.begin(), AT.end());

		if (deriv)
		{
			Eigen::VectorXd fvals(2 * nverts);
			for (int i = 0; i < nverts; i++)
			{
				fvals(2 * i) = zvals[i].real();
				fvals(2 * i + 1) = zvals[i].imag();
			}
			(*deriv) = A * fvals;
		}

		if (hess)
			(*hess) = AT;
	}

	return energy;
}

double IntrinsicFormula::KnoppelEnergyGivenMag(const MeshConnectivity& mesh, const Eigen::MatrixXd& halfEdgeW, const Eigen::VectorXd& vertAmp, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const std::vector<std::complex<double>>& zvals, Eigen::VectorXd* deriv, std::vector<Eigen::Triplet<double>>* hess)
{
	std::vector<Eigen::Triplet<double>> AT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();
	int nverts = vertAmp.size();

	Eigen::VectorXd halfEdgeWeight(nedges);
	halfEdgeWeight.setConstant(1.0);

//    for (int i = 0; i < nfaces; i++)  // form mass matrix
//    {
//        for (int j = 0; j < 3; j++)
//        {
//            int eid = mesh.faceEdge(i, j);
//            halfEdgeWeight(eid) += cotEntries(i, j);
//        }
//    }
	double energy = 0;

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

		double r0 = vertAmp(vid0);
		double r1 = vertAmp(vid1);

		std::complex<double> expw0 = std::complex<double>(std::cos(halfEdgeW(i, 0)), std::sin(halfEdgeW(i, 0)));
		std::complex<double> expw1 = std::complex<double>(std::cos(halfEdgeW(i, 1)), std::sin(halfEdgeW(i, 1)));

		std::complex<double> z0 = zvals[vid0];
		std::complex<double> z1 = zvals[vid1];

		energy += 0.5 * (norm((r1 * z0 * expw0 - r0 * z1)) + norm((r0 * z1 * expw1 - r1 * z0))) * halfEdgeWeight(i);

		if (deriv || hess)
		{
			AT.push_back({ 2 * vid0, 2 * vid0, 2 * r1 * r1 * halfEdgeWeight(i) });
			AT.push_back({ 2 * vid0 + 1, 2 * vid0 + 1, 2 * r1 * r1 * halfEdgeWeight(i) });

			AT.push_back({ 2 * vid1, 2 * vid1, 2 * r0 * r0 * halfEdgeWeight(i) });
			AT.push_back({ 2 * vid1 + 1, 2 * vid1 + 1, 2 * r0 * r0 * halfEdgeWeight(i) });

			AT.push_back({ 2 * vid0, 2 * vid1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1 });
			AT.push_back({ 2 * vid0 + 1, 2 * vid1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) * r0 * r1 });
			AT.push_back({ 2 * vid0, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) * r0 * r1 });
			AT.push_back({ 2 * vid0 + 1, 2 * vid1 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1 });

			AT.push_back({ 2 * vid1, 2 * vid0, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1 });
			AT.push_back({ 2 * vid1, 2 * vid0 + 1, -halfEdgeWeight(i) * (-expw0.imag() + expw1.imag()) * r0 * r1 });
			AT.push_back({ 2 * vid1 + 1, 2 * vid0, -halfEdgeWeight(i) * (expw0.imag() - expw1.imag()) * r0 * r1 });
			AT.push_back({ 2 * vid1 + 1, 2 * vid0 + 1, -halfEdgeWeight(i) * (expw0.real() + expw1.real()) * r0 * r1 });
		}
	}

	if (deriv || hess)
	{
		Eigen::SparseMatrix<double> A;

		A.resize(2 * nverts, 2 * nverts);
		A.setFromTriplets(AT.begin(), AT.end());

		// check whether A is PD

		if (deriv)
		{
			Eigen::VectorXd fvals(2 * nverts);
			for (int i = 0; i < nverts; i++)
			{
				fvals(2 * i) = zvals[i].real();
				fvals(2 * i + 1) = zvals[i].imag();
			}
			(*deriv) = A * fvals;
		}

		if (hess)
			(*hess) = AT;
	}

	return energy;
}

double IntrinsicFormula::KnoppelEnergyFor2DVertexOmegaPerEdge(const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const std::vector<std::complex<double>>& zvals, const Eigen::MatrixXd& vertexOmega, const double edgeWeight, int eid, Eigen::Matrix<double, 8, 1>* deriv, Eigen::Matrix<double, 8, 8>* hess, bool isProj)
{
	int vid0 = mesh.edgeVertex(eid, 0);
	int vid1 = mesh.edgeVertex(eid, 1);

	Eigen::Vector2d e = (pos.row(vid1) - pos.row(vid0)).segment<2>(0);

	double w01 = vertexOmega.row(vid0).dot(e);
	double w10 = -vertexOmega.row(vid1).dot(e);

	double sin0 = std::sin(w01), cos0 = std::cos(w01);
	double sin1 = std::sin(w10), cos1 = std::cos(w10);

	Eigen::Vector2d z0(zvals[vid0].real(), zvals[vid0].imag());
	Eigen::Vector2d z1(zvals[vid1].real(), zvals[vid1].imag());

	Eigen::Matrix2d mat0, mat1;
	mat0 << cos0, -sin0, sin0, cos0;
	mat1 << cos1, -sin1, sin1, cos1;

	Eigen::Vector2d f0, f1;
	f0 = mat0 * z0 - z1;
	f1 = mat1 * z1 - z0;

	double energy = 0.5 * (f0.dot(f0) + f1.dot(f1)) * edgeWeight;

	if (deriv)
		deriv->setZero();

	if (deriv || hess)
	{
		Eigen::Vector2d e0, e1;
		e0 << 1, 0;
		e1 << 0, 1;

		Eigen::Matrix2d dmat0, dmat1;
		dmat0 << -sin0, -cos0, cos0, -sin0;
		dmat1 << -sin1, -cos1, cos1, -sin1;

		Eigen::Matrix<double, 2, 8> gradF0, gradF1;
		gradF0.setZero();
		gradF1.setZero();

		gradF0.col(0) = mat0 * e0;
		gradF0.col(1) = mat0 * e1;
		gradF0.col(2) = -e0;
		gradF0.col(3) = -e1;

		gradF0.col(4) = dmat0 * z0 * e(0);
		gradF0.col(5) = dmat0 * z0 * e(1);

		gradF1.col(0) = -e0;
		gradF1.col(1) = -e1;
		gradF1.col(2) = mat1 * e0;
		gradF1.col(3) = mat1 * e1;

		gradF1.col(6) = -dmat1 * z1 * e(0);
		gradF1.col(7) = -dmat1 * z1 * e(1);

		if (deriv)
			*deriv = (f0.transpose() * gradF0 + f1.transpose() * gradF1) * edgeWeight;

		if (hess)
		{
			*hess = (gradF0.transpose() * gradF0 + gradF1.transpose() * gradF1) * edgeWeight;

			(*hess)(0, 4) += edgeWeight * (f0(0) * -sin0 * e(0) + f0(1) * cos0 * e(0));
			(*hess)(0, 5) += edgeWeight * (f0(0) * -sin0 * e(1) + f0(1) * cos0 * e(1));

			(*hess)(1, 4) += edgeWeight * (f0(0) * -cos0 * e(0) + f0(1) * -sin0 * e(0));
			(*hess)(1, 5) += edgeWeight * (f0(0) * -cos0 * e(1) + f0(1) * -sin0 * e(1));

			(*hess)(2, 6) += edgeWeight * (f1(0) * sin1 * e(0) - f1(1) * cos1 * e(0));
			(*hess)(2, 7) += edgeWeight * (f1(0) * sin1 * e(1) - f1(1) * cos1 * e(1));

			(*hess)(3, 6) += edgeWeight * (f1(0) * cos1 * e(0) + f1(1) * sin1 * e(0));
			(*hess)(3, 7) += edgeWeight * (f1(0) * cos1 * e(1) + f1(1) * sin1 * e(1));

			(*hess)(4, 0) = (*hess)(0, 4);
			(*hess)(4, 1) = (*hess)(1, 4);
			(*hess)(4, 4) += edgeWeight * (f0(0) * (-z0(0) * cos0 + z0(1) * sin0) + f0(1) * (-z0(0) * sin0 - z0(1) * cos0)) * e(0) * e(0);
			(*hess)(4, 5) += edgeWeight * (f0(0) * (-z0(0) * cos0 + z0(1) * sin0) + f0(1) * (-z0(0) * sin0 - z0(1) * cos0)) * e(0) * e(1);

			(*hess)(5, 0) = (*hess)(0, 5);
			(*hess)(5, 1) = (*hess)(1, 5);
			(*hess)(5, 4) += edgeWeight * (f0(0) * (-z0(0) * cos0 + z0(1) * sin0) + f0(1) * (-z0(0) * sin0 - z0(1) * cos0)) * e(1) * e(0);
			(*hess)(5, 5) += edgeWeight * (f0(0) * (-z0(0) * cos0 + z0(1) * sin0) + f0(1) * (-z0(0) * sin0 - z0(1) * cos0)) * e(1) * e(1);

			(*hess)(6, 2) = (*hess)(2, 6);
			(*hess)(6, 3) = (*hess)(3, 6);
			(*hess)(6, 6) += edgeWeight * (f1(0) * (-z1(0) * cos1 + z1(1) * sin1) + f1(1) * (-z1(0) * sin1 - z1(1) * cos1)) * e(0) * e(0);
			(*hess)(6, 7) += edgeWeight * (f1(0) * (-z1(0) * cos1 + z1(1) * sin1) + f1(1) * (-z1(0) * sin1 - z1(1) * cos1)) * e(0) * e(1);

			(*hess)(7, 2) = (*hess)(2, 7);
			(*hess)(7, 3) = (*hess)(3, 7);
			(*hess)(7, 6) += edgeWeight * (f1(0) * (-z1(0) * cos1 + z1(1) * sin1) + f1(1) * (-z1(0) * sin1 - z1(1) * cos1)) * e(1) * e(0);
			(*hess)(7, 7) += edgeWeight * (f1(0) * (-z1(0) * cos1 + z1(1) * sin1) + f1(1) * (-z1(0) * sin1 - z1(1) * cos1)) * e(1) * e(1);

			if (isProj)
				(*hess) = SPDProjection(*hess);

		}

	}

	return energy;
}

double IntrinsicFormula::KnoppelEnergyFor2DVertexOmega(const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const std::vector<std::complex<double>>& zvals, const Eigen::MatrixXd& vertexOmega, Eigen::VectorXd* deriv, std::vector<Eigen::Triplet<double>>* hess, bool isProj)
{
	std::vector<Eigen::Triplet<double>> AT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();
	int nverts = zvals.size();

	Eigen::VectorXd edgeWeight(nedges);
	edgeWeight.setZero();

	for (int i = 0; i < nfaces; i++)  // form mass matrix
	{
		for (int j = 0; j < 3; j++)
		{
			int eid = mesh.faceEdge(i, j);
			edgeWeight(eid) += cotEntries(i, j);
		}
	}
	double energy = 0;
	edgeWeight.setConstant(1.0);

	if (deriv)
	{
		deriv->setZero(4 * nverts);
	}

	for (int i = 0; i < nedges; i++)
	{
		Eigen::Matrix<double, 8, 1> edgeDeriv;
		Eigen::Matrix<double, 8, 8> edgeHess;

		energy += KnoppelEnergyFor2DVertexOmegaPerEdge(pos, mesh, faceArea, cotEntries, zvals, vertexOmega, edgeWeight(i), i, deriv ? &edgeDeriv : NULL, hess ? &edgeHess : NULL, isProj);

		int vid0 = mesh.edgeVertex(i, 0);
		int vid1 = mesh.edgeVertex(i, 1);

		if (deriv)
		{
			deriv->segment<2>(2 * vid0) += edgeDeriv.segment<2>(0);
			deriv->segment<2>(2 * vid1) += edgeDeriv.segment<2>(2);

			deriv->segment<2>(2 * vid0 + 2 * nverts) += edgeDeriv.segment<2>(4);
			deriv->segment<2>(2 * vid1 + 2 * nverts) += edgeDeriv.segment<2>(6);

		}

		if (hess)
		{
			for(int m = 0; m < 2; m++)
				for (int n = 0; n < 2; n++)
				{
					hess->push_back({ 2 * vid0 + m , 2 * vid0 + n, edgeHess(m, n) });
					hess->push_back({ 2 * vid0 + m , 2 * vid1 + n, edgeHess(m, 2 + n) });
					hess->push_back({ 2 * vid0 + m , 2 * vid0 + 2 * nverts + n, edgeHess(m, 4 + n) });
					hess->push_back({ 2 * vid0 + m , 2 * vid1 + 2 * nverts + n, edgeHess(m, 6 + n) });

					hess->push_back({ 2 * vid1 + m , 2 * vid0 + n, edgeHess(2 + m, n) });
					hess->push_back({ 2 * vid1 + m , 2 * vid1 + n, edgeHess(2 + m, 2 + n) });
					hess->push_back({ 2 * vid1 + m , 2 * vid0 + 2 * nverts + n, edgeHess(2 + m, 4 + n) });
					hess->push_back({ 2 * vid1 + m , 2 * vid1 + 2 * nverts + n, edgeHess(2 + m, 6 + n) });

					hess->push_back({ 2 * vid0 + 2 * nverts + m , 2 * vid0 + n, edgeHess(4 + m, n) });
					hess->push_back({ 2 * vid0 + 2 * nverts + m , 2 * vid1 + n, edgeHess(4 + m, 2 + n) });
					hess->push_back({ 2 * vid0 + 2 * nverts + m , 2 * vid0 + 2 * nverts + n, edgeHess(4 + m, 4 + n) });
					hess->push_back({ 2 * vid0 + 2 * nverts + m , 2 * vid1 + 2 * nverts + n, edgeHess(4 + m, 6 + n) });

					hess->push_back({ 2 * vid1 + 2 * nverts + m , 2 * vid0 + n, edgeHess(6 + m, n) });
					hess->push_back({ 2 * vid1 + 2 * nverts + m , 2 * vid1 + n, edgeHess(6 + m, 2 + n) });
					hess->push_back({ 2 * vid1 + 2 * nverts + m , 2 * vid0 + 2 * nverts + n, edgeHess(6 + m, 4 + n) });
					hess->push_back({ 2 * vid1 + 2 * nverts + m , 2 * vid1 + 2 * nverts + n, edgeHess(6 + m, 6 + n) });

				}

		}
	}
	return energy;
}

void IntrinsicFormula::roundVertexZvalsFromHalfEdgeOmega(const MeshConnectivity &mesh, const Eigen::MatrixXd &halfEdgeW,
														 const Eigen::VectorXd &faceArea,
														 const Eigen::MatrixXd &cotEntries, const int nverts,
														 std::vector<std::complex<double>> &zvals)
{
	std::vector<Eigen::Triplet<double>> BT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	for(int i = 0; i < nfaces; i++)  // form mass matrix
	{
		for(int j =0; j < 3; j++)
		{
			int vid = mesh.faceVertex(i, j);
			BT.push_back({2 * vid, 2 * vid, faceArea(i) / 3.0});
			BT.push_back({2 * vid + 1, 2 * vid + 1, faceArea(i) / 3.0});
		}
	}
	Eigen::SparseMatrix<double> A;
	computeMatrixA(mesh, halfEdgeW, faceArea, cotEntries, nverts, A);

	Eigen::SparseMatrix<double> B(2 * nverts, 2 * nverts);
	B.setFromTriplets(BT.begin(), BT.end());
   /* std::cout << A.toDense() << std::endl;
	std::cout << B.toDense() << std::endl;*/

	Spectra::SymShiftInvert<double> op(A, B);
	Spectra::SparseSymMatProd<double> Bop(B);
	Spectra::SymGEigsShiftSolver<Spectra::SymShiftInvert<double>, Spectra::SparseSymMatProd<double>, Spectra::GEigsMode::ShiftInvert> geigs(op, Bop, 1, 6, -1e-6);
	geigs.init();
	int nconv = geigs.compute(Spectra::SortRule::LargestMagn, 1e6);

	Eigen::VectorXd evalues;
	Eigen::MatrixXd evecs;

	evalues = geigs.eigenvalues();
	evecs = geigs.eigenvectors();
	if (nconv != 1 || geigs.info() != Spectra::CompInfo::Successful)
	{
		std::cout << "Eigensolver failed to converge!!" << std::endl;
	}

	std::cout << "Eigenvalue is " << evalues[0] << std::endl;

	zvals.clear();
	for(int i = 0; i < nverts; i++)
	{
		zvals.push_back(std::complex<double>(evecs(2 * i, 0), evecs(2 * i + 1, 0)));
	}
}

void IntrinsicFormula::roundVertexZvalsFromHalfEdgeOmegaVertexMag(const MeshConnectivity &mesh, const Eigen::MatrixXd& halfEdgeW, const Eigen::VectorXd& vertAmp, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const int nverts, std::vector<std::complex<double>>& zvals)
{
	std::vector<Eigen::Triplet<double>> BT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	for(int i = 0; i < nfaces; i++)  // form mass matrix
	{
		for(int j =0; j < 3; j++)
		{
			int vid = mesh.faceVertex(i, j);
			BT.push_back({2 * vid, 2 * vid, faceArea(i) / 3.0});
			BT.push_back({2 * vid + 1, 2 * vid + 1, faceArea(i) / 3.0});
		}
	}
	Eigen::SparseMatrix<double> A;
	computeMatrixAGivenMag(mesh, halfEdgeW, vertAmp, faceArea, cotEntries, nverts, A);

	Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver;
	Eigen::SparseMatrix<double> I = A;
	I.setIdentity();
	double eps = 1e-16;
    Eigen::SparseMatrix<double> tmpA = A + eps * I;
	solver.compute(tmpA);
	while(solver.info() != Eigen::Success)
	{
		std::cout << "matrix is not PD after adding "<< eps << " * I" << std::endl;
		solver.compute(tmpA);
		eps *= 2;
        tmpA = A + eps * I;
	}

	Eigen::SparseMatrix<double> B(2 * nverts, 2 * nverts);
	B.setFromTriplets(BT.begin(), BT.end());
	/* std::cout << A.toDense() << std::endl;
	 std::cout << B.toDense() << std::endl;*/

	Spectra::SymShiftInvert<double> op(A, B);
	Spectra::SparseSymMatProd<double> Bop(B);
	Spectra::SymGEigsShiftSolver<Spectra::SymShiftInvert<double>, Spectra::SparseSymMatProd<double>, Spectra::GEigsMode::ShiftInvert> geigs(op, Bop, 1, 6, -2 * eps);
	geigs.init();
	int nconv = geigs.compute(Spectra::SortRule::LargestMagn, 1e6);

	Eigen::VectorXd evalues;
	Eigen::MatrixXd evecs;

	evalues = geigs.eigenvalues();
	evecs = geigs.eigenvectors();
	if (nconv != 1 || geigs.info() != Spectra::CompInfo::Successful)
	{
		std::cout << "Eigensolver failed to converge!!" << std::endl;
		exit(1);
	}

	std::cout << "Eigenvalue is " << evalues[0] << std::endl;

	zvals.clear();
	for(int i = 0; i < nverts; i++)
	{
		std::complex<double> z = std::complex<double>(evecs(2 * i, 0), evecs(2 * i + 1, 0));
		z *= vertAmp(i) / std::abs(z);
		zvals.push_back(z);
	}
}

void IntrinsicFormula::roundZvalsForSpecificDomainWithGivenMag(const MeshConnectivity& mesh, const Eigen::MatrixXd& halfEdgeW, const Eigen::VectorXd& vertAmp, const Eigen::VectorXi& vertFlags, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const int nverts, std::vector<std::complex<double>>& zvals)
{
	std::vector<Eigen::Triplet<double>> BT;
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	for (int i = 0; i < nfaces; i++)  // form mass matrix
	{
		for (int j = 0; j < 3; j++)
		{
			int vid = mesh.faceVertex(i, j);
			BT.push_back({ 2 * vid, 2 * vid, faceArea(i) / 3.0 });
			BT.push_back({ 2 * vid + 1, 2 * vid + 1, faceArea(i) / 3.0 });
		}
	}
	Eigen::SparseMatrix<double> A;
	computeMatrixAGivenMag(mesh, halfEdgeW, vertAmp, faceArea, cotEntries, nverts, A);
//    computeMatrixA(mesh, halfEdgeW, faceArea, cotEntries, nverts, A);

	Eigen::SparseMatrix<double> B(2 * nverts, 2 * nverts);
	B.setFromTriplets(BT.begin(), BT.end());

    Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver;
    Eigen::SparseMatrix<double> I = A;
    I.setIdentity();
    double eps = 1e-16;
    Eigen::SparseMatrix<double> tmpA = A + eps * I;
    solver.compute(tmpA);
    while(solver.info() != Eigen::Success)
    {
        std::cout << "matrix is not PD after adding "<< eps << " * I" << std::endl;
        solver.compute(tmpA);
        eps *= 2;
        tmpA = A + eps * I;
    }

	Spectra::SymShiftInvert<double> op(A, B);
	Spectra::SparseSymMatProd<double> Bop(B);
	Spectra::SymGEigsShiftSolver<Spectra::SymShiftInvert<double>, Spectra::SparseSymMatProd<double>, Spectra::GEigsMode::ShiftInvert> geigs(op, Bop, 1, 6, -2 * eps);
	geigs.init();
	int nconv = geigs.compute(Spectra::SortRule::LargestMagn, 1e6);

	Eigen::VectorXd evalues;
	Eigen::MatrixXd evecs;

	evalues = geigs.eigenvalues();
	evecs = geigs.eigenvectors();
	if (nconv != 1 || geigs.info() != Spectra::CompInfo::Successful)
	{
		std::cout << "Eigensolver failed to converge!!" << std::endl;
	}

	std::cout << "Eigenvalue is " << evalues[0] << std::endl;

	zvals.clear();
    Eigen::VectorXd fullVar = evecs;
//	Eigen::VectorXd fullVar = projM.transpose() * evecs;
	for (int i = 0; i < nverts; i++)
	{
		std::complex<double> z = std::complex<double>(fullVar(2 * i, 0), fullVar(2 * i + 1, 0));

        if (vertFlags(i) == 1)
        {
            z *= vertAmp(i) / std::abs(z);
        }

		zvals.push_back(z);
	}
}

void IntrinsicFormula::roundZvalsForSpecificDomainWithBndValues(const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, const Eigen::MatrixXd& halfEdgeW, const Eigen::VectorXi& vertFlags, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const int nverts, std::vector<std::complex<double>>& vertZvals, double smoothnessCoeff)
{
	Eigen::VectorXd clampedVals(2 * nverts);
	clampedVals.setZero();

	std::vector<Eigen::Triplet<double>> PT;
	int nDOFs = 0;
	for (int i = 0; i < nverts; i++)
	{
		if (vertFlags(i) == 0)	// free variables
		{
			PT.push_back({ 2 * nDOFs, 2 * i, 1.0 });
			PT.push_back({ 2 * nDOFs + 1, 2 * i + 1, 1.0 });
			nDOFs += 1;
		}
		else
		{
			clampedVals(2 * i) = vertZvals[i].real();
			clampedVals(2 * i + 1) = vertZvals[i].imag();
		}
	}
	if (nDOFs == 0)
		return;
	Eigen::SparseMatrix<double> projM, unProjM;
	projM.resize(2 * nDOFs, 2 * nverts);
	projM.setFromTriplets(PT.begin(), PT.end());

	unProjM = projM.transpose();

	auto zList2CoordVec = [&](const std::vector<std::complex<double>>& zvals, Eigen::VectorXd& xvec, Eigen::VectorXd& yvec)
	{
		xvec.setZero(zvals.size());
		yvec.setZero(zvals.size());

		for (int i = 0; i < zvals.size(); i++)
		{
			xvec(i) = zvals[i].real();
			yvec(i) = zvals[i].imag();
		}
	};

	auto zList2Vec = [&](const std::vector<std::complex<double>>& zvals)
	{
		Eigen::VectorXd zvec(2 * zvals.size());
		for (int i = 0; i < zvals.size(); i++)
		{
			zvec(2 * i) = zvals[i].real();
			zvec(2 * i + 1) = zvals[i].imag();
		}
		return zvec;
	};

	auto vec2zList = [&](const Eigen::VectorXd& zvec)
	{
		std::vector<std::complex<double>> zList;
		for (int i = 0; i < zvec.size() / 2; i++)
		{
			zList.push_back(std::complex<double>(zvec(2 * i), zvec(2 * i + 1)));
		}
		return zList;
	};

	auto projVar = [&](const std::vector<std::complex<double>>& zvals)
	{
		Eigen::VectorXd zvec = zList2Vec(zvals);
		return projM * zvec;
	};

	auto unprojVar = [&](const Eigen::VectorXd& zvec, const Eigen::VectorXd& clampedZvecs)
	{
		Eigen::VectorXd fullZvec = unProjM * zvec + clampedVals;
		return vec2zList(fullZvec);
	};

	Eigen::SparseMatrix<double> L, lapL;
	igl::cotmatrix(pos, mesh.faces(), L);

	std::vector<Eigen::Triplet<double>> lapT;

	for (int k = 0; k < L.outerSize(); ++k)
		for (Eigen::SparseMatrix<double>::InnerIterator it(L, k); it; ++it)
		{
			lapT.push_back(Eigen::Triplet<double>(2 * it.row(), 2 * it.col(), -it.value()));
			lapT.push_back(Eigen::Triplet<double>(2 * it.row() + 1, 2 * it.col() + 1, -it.value()));
		}

	lapL.resize(2 * nverts, 2 * nverts);
	lapL.setFromTriplets(lapT.begin(), lapT.end());

	auto funVal = [&](const Eigen::VectorXd& x, Eigen::VectorXd* grad, Eigen::SparseMatrix<double>* hess, bool isProj)
	{
		std::vector<std::complex<double>> zList = unprojVar(x, clampedVals);
		Eigen::VectorXd deriv;
		std::vector<Eigen::Triplet<double>> T;
		Eigen::SparseMatrix<double> H;
		double E = KnoppelEnergy(mesh, halfEdgeW, faceArea, cotEntries, zList, grad ? &deriv : NULL, hess ? &T : NULL);

		Eigen::VectorXd fullx = zList2Vec(zList);

		if (smoothnessCoeff > 0)
			E += smoothnessCoeff * fullx.dot(lapL * fullx) / 2;

		if (grad)
		{
			if (smoothnessCoeff > 0)
				deriv += smoothnessCoeff * lapL * fullx;
			(*grad) = projM * deriv;
		}
		if (hess)
		{
			H.resize(2 * nverts, 2 * nverts);
			H.setFromTriplets(T.begin(), T.end());

			if (smoothnessCoeff > 0)
				H += smoothnessCoeff * lapL;
			(*hess) = projM * H * unProjM;
		}

		return E;
	};

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

	Eigen::VectorXd x0 = projVar(vertZvals);
	OptSolver::newtonSolver(funVal, maxStep, x0, 1000, 1e-6, 1e-10, 1e-15, true);

	Eigen::VectorXd deriv;
	double E = funVal(x0, &deriv, NULL, false);
	std::cout << "terminated with energy : " << E << ", gradient norm : " << deriv.norm() << std::endl << std::endl;
	vertZvals = unprojVar(x0, clampedVals);
}

double IntrinsicFormula::lArg(const long &n, const Eigen::Vector3d &bary)
{
	double larg = 0;
	double ti = bary(0), tj = bary(1), tk = bary(2);
	if(tk <= ti && tk <= tj)
		larg = M_PI / 3 * n * (1 + (tj - ti) / (1 - 3 * tk));
	else if (ti <= tj && ti <= tk)
		larg = M_PI / 3 * n * (3 + (tk - tj) / (1 - 3 * ti));
	else
		larg = M_PI / 3 * n * (5 + (ti - tk) / (1 - 3 * tj));
	return larg;
}

void IntrinsicFormula::getUpsamplingTheta(const MeshConnectivity &mesh, const Eigen::MatrixXd &halfEdgeW,
										  const std::vector<std::complex<double>> &zvals,
										  const std::vector<std::pair<int, Eigen::Vector3d>> &bary, Eigen::VectorXd& upTheta)
{
	Eigen::VectorXd edgeW = (halfEdgeW.col(0) - halfEdgeW.col(1)) / 2;
	int upsize = bary.size();
	upTheta.setZero(upsize);

	for(int i = 0; i < bary.size(); i++)
	{
		int fid = bary[i].first;
		double omegaIJ = edgeW(mesh.faceEdge(fid, 2));
		double omegaJK = edgeW(mesh.faceEdge(fid, 0));
		double omegaKI = edgeW(mesh.faceEdge(fid, 1));

		double cIJ = mesh.faceVertex(fid, 0) == mesh.edgeVertex(mesh.faceEdge(fid, 2), 0) ? 1 : -1;
		double cJK = mesh.faceVertex(fid, 1) == mesh.edgeVertex(mesh.faceEdge(fid, 0), 0) ? 1 : -1;
		double cKI = mesh.faceVertex(fid, 2) == mesh.edgeVertex(mesh.faceEdge(fid, 1), 0) ? 1 : -1;

		omegaIJ *= cIJ;
		omegaJK *= cJK;
		omegaKI *= cKI;

		std::complex<double> rij( std::cos(omegaIJ), std::sin(omegaIJ) );
		std::complex<double> rjk( std::cos(omegaJK), std::sin(omegaJK) );
		std::complex<double> rki( std::cos(omegaKI), std::sin(omegaKI) );

		std::complex<double> psiI = zvals[mesh.faceVertex(fid, 0)];
		std::complex<double> psiJ = zvals[mesh.faceVertex(fid, 1)];
		std::complex<double> psiK = zvals[mesh.faceVertex(fid, 2)];

		double alphaI = std::arg(psiI);
		double alphaJ = alphaI + omegaIJ - std::arg(rij*psiI/psiJ); //fmodPI((varphiI + omegaIJ) - varphiJ); // could do this in terms of angles instead of complex numbers...
		double alphaK = alphaJ + omegaJK - std::arg(rjk*psiJ/psiK); //fmodPI((varphiJ + omegaJK) - varphiK); // mostly a matter of taste---possibly a matter of performance?
		double alphaL = alphaK + omegaKI - std::arg(rki*psiK/psiI); //fmodPI((varphiK + omegaKI) - varphiI);

		// adjust triangles containing zeros
		long n = std::lround((alphaL-alphaI)/(2.*M_PI));
		alphaJ -= 2.*M_PI*n/3.;
		alphaK -= 4.*M_PI*n/3.;

		double theta = lArg(n, bary[i].second);
		upTheta(i) = theta + bary[i].second(0) * alphaI + bary[i].second(1) * alphaJ + bary[i].second(2) * alphaK;
	}
}

void IntrinsicFormula::testRoundingEnergy(const MeshConnectivity &mesh, const Eigen::MatrixXd &halfEdgeW,
										  const Eigen::VectorXd &faceArea, const Eigen::MatrixXd &cotEntries,
										  const int nverts, std::vector<std::complex<double>> zvals)
{
	Eigen::SparseMatrix<double> A;
	computeMatrixA(mesh, halfEdgeW, faceArea, cotEntries, nverts, A);
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	Eigen::VectorXd halfEdgeWeight(nedges);
	halfEdgeWeight.setZero();

	for(int i = 0; i < nfaces; i++)  // form mass matrix
	{
		for(int j =0; j < 3; j++)
		{
			int eid = mesh.faceEdge(i, j);
			halfEdgeWeight(eid) += cotEntries(i, j);
		}
	}
	double energy = 0;
	for(int i = 0; i < nedges; i++)
	{
		int vid0 = mesh.edgeVertex(i, 0);
		int vid1 = mesh.edgeVertex(i, 1);

		std::complex<double> expw0 = std::complex<double>(std::cos(halfEdgeW(i, 0)), std::sin(halfEdgeW(i, 0)));
		std::complex<double> expw1 = std::complex<double>(std::cos(halfEdgeW(i, 1)), std::sin(halfEdgeW(i, 1)));

		Eigen::Matrix2d tmpMat;

		tmpMat(0, 0) = expw0.real();
		tmpMat(0, 1) = expw0.imag();
		tmpMat(1, 0) = -expw0.imag();
		tmpMat(1, 1) = expw0.real();
		Eigen::Vector2d aibi(zvals[vid0].real(), zvals[vid0].imag());
		Eigen::Vector2d ajbj(zvals[vid1].real(), zvals[vid1].imag());

		double part1 = 2 * aibi.dot(tmpMat * ajbj) + aibi.squaredNorm() + ajbj.squaredNorm();
		double part2 = ((zvals[vid0] * expw0 - zvals[vid1])).real() * ((zvals[vid0] * expw0 - zvals[vid1])).real() + ((zvals[vid0] * expw0 - zvals[vid1])).imag() * ((zvals[vid0] * expw0 - zvals[vid1])).imag();

		energy += 0.5 * (norm((zvals[vid0] * expw0 - zvals[vid1])) + norm((zvals[vid1] * expw1 - zvals[vid0]))) * halfEdgeWeight(i);

	}

	Eigen::VectorXd x(2 * nverts);
	for(int i = 0; i < nverts; i++)
	{
		x(2 * i) = zvals[i].real();
		x(2 * i + 1) = zvals[i].imag();
	}
}

void IntrinsicFormula::testKnoppelEnergyFor2DVertexOmegaPerEdge(const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const std::vector<std::complex<double>>& zvals, const Eigen::MatrixXd& vertexOmega, const double edgeWeight, int eid)
{
	int nverts = pos.rows();
	auto zbackup = zvals;
	auto wbackup = vertexOmega;

	Eigen::Matrix<double, 8, 1> deriv;
	Eigen::Matrix<double, 8, 8> hess;

	double e = KnoppelEnergyFor2DVertexOmegaPerEdge(pos, mesh, faceArea, cotEntries, zvals, vertexOmega, edgeWeight, eid, &deriv, &hess, false);
	std::cout << "energy: " << e << std::endl;

	int vid0 = mesh.edgeVertex(eid, 0);
	int vid1 = mesh.edgeVertex(eid, 1);

	Eigen::Vector2d edge = (pos.row(vid1) - pos.row(vid0)).segment<2>(0);
	Eigen::Vector2d z0(zvals[vid0].real(), zvals[vid0].imag());
	Eigen::Vector2d z1(zvals[vid1].real(), zvals[vid1].imag());

	std::cout << "(a0, b0): " << z0.transpose() << std::endl;
	std::cout << "(a1, b1): " << z1.transpose() << std::endl;
	std::cout << "e: " << edge.transpose() << std::endl;
	std::cout << "w0: " << vertexOmega.row(vid0) << std::endl;
	std::cout << "w1: " << vertexOmega.row(vid1) << std::endl;

	std::cout << "hess: \n" << hess << std::endl;

	Eigen::Matrix<double, 8, 1> dir = deriv;
	dir.setRandom();

	for (int i = 3; i < 9; i++)
	{
		double eps = std::pow(0.1, i);

		for (int j = 0; j < 2; j++)
		{
			int v = mesh.edgeVertex(eid, j);
			zbackup[v] = std::complex<double>(zvals[v].real() + dir(2 * j) * eps, zvals[v].imag() + dir(1 + 2 * j) * eps);
			wbackup(v, 0) = vertexOmega(v, 0) + eps * dir(4 + 2 * j);
			wbackup(v, 1) = vertexOmega(v, 1) + eps * dir(5 + 2 * j);
		}

		Eigen::Matrix<double, 8, 1> deriv1;
		double e1 = KnoppelEnergyFor2DVertexOmegaPerEdge(pos, mesh, faceArea, cotEntries, zbackup, wbackup, edgeWeight, eid, &deriv1, NULL, false);

		std::cout << "eps: " << eps << std::endl;
		std::cout << "value-gradient check: " << (e1 - e) / eps - dir.dot(deriv) << std::endl;
		std::cout << "gradient-hessian check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}
}

void IntrinsicFormula::testKnoppelEnergyFor2DVertexOmega(const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, const Eigen::VectorXd& faceArea, const Eigen::MatrixXd& cotEntries, const std::vector<std::complex<double>>& zvals, const Eigen::MatrixXd& vertexOmega)
{
	Eigen::VectorXd deriv;
	Eigen::SparseMatrix<double> hess;
	std::vector<Eigen::Triplet<double>> hessT;

	double e = KnoppelEnergyFor2DVertexOmega(pos, mesh, faceArea, cotEntries, zvals, vertexOmega, &deriv, &hessT, false);
	std::cout << "energy: " << e << std::endl;
	hess.resize(deriv.rows(), deriv.rows());
	hess.setFromTriplets(hessT.begin(), hessT.end());

	Eigen::VectorXd dir = deriv;
	dir.setRandom();

	int nverts = pos.rows();
	auto zbackup = zvals;
	auto wbackup = vertexOmega;

	for (int i = 3; i < 9; i++)
	{
		double eps = std::pow(0.1, i);

		for (int v = 0; v < nverts; v++)
		{
			zbackup[v] = std::complex<double>(zvals[v].real() + dir(2 * v) * eps, zvals[v].imag() + dir(2 * v + 1) * eps);
			wbackup(v, 0) = vertexOmega(v, 0) + eps * dir(2 * v + 2 * nverts);
			wbackup(v, 1) = vertexOmega(v, 1) + eps * dir(2 * v + 2 * nverts + 1);
		}

		Eigen::VectorXd deriv1;
		double e1 = KnoppelEnergyFor2DVertexOmega(pos, mesh, faceArea, cotEntries, zbackup, wbackup, &deriv1, NULL, false);

		std::cout << "eps: " << eps << std::endl;
		std::cout << "value-gradient check: " << (e1 - e) / eps - dir.dot(deriv) << std::endl;
		std::cout << "gradient-hessian check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}
}