#include "ImplicitIntegrator.h"
#include "Deformation.hpp"
#include "ProtoConverter.hpp"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SVD>
#include <Eigen/IterativeLinearSolvers>
#include <Mathematics/SymmetricEigensolver3x3.h>

#include <iostream>
#include <glm/gtc/constants.hpp>

using Eigen::MatrixXd;

namespace Deformation
{
    using FloatT = double;
    using Mat3 = Eigen::Matrix<FloatT, 3, 3>;
    using Mat3x4 = Eigen::Matrix<FloatT, 3, 4>;
    using Mat9x12 = Eigen::Matrix<FloatT, 9, 12>;
    using Mat9 = Eigen::Matrix<FloatT, 9, 9>;
    using Mat12 = Eigen::Matrix<FloatT, 12, 12>;
    using Vec3 = Eigen::Matrix<FloatT, 3, 1>;
    using Vec9 = Eigen::Matrix<FloatT, 9, 1>;
    using Vec12 = Eigen::Matrix<FloatT, 12, 1>;

    Mat3 Calc_Ds(const Deformation::Tetrahedra& tet, const std::vector<Vertex>& vertices, bool restState)
    {
        Mat3 ds = Mat3::Zero();

        for (int row = 0; row < 3; row++)
        {
            for (int col = 1; col < 4; col++)
            {
                if (restState)
					ds(row, col - 1) = vertices[tet.mIndices[col]].mMaterialCoordinates[row] - vertices[tet.mIndices[0]].mMaterialCoordinates[row];
                else
                    ds(row, col - 1) = vertices[tet.mIndices[col]].mPosition[row] - vertices[tet.mIndices[0]].mPosition[row];
            }
        }

        return ds;
    }

    Mat3 Calc_DmInv(const Deformation::Tetrahedra& tet, const std::vector<Vertex>& vertices)
    {
        auto mat = Calc_Ds(tet, vertices, true);
        if (std::abs(mat.determinant()) < 1e-10f)
            throw std::exception("Small");
        return mat.inverse();
    }

    Mat3 Calc_F(const Mat3& ds, const Mat3& dmInv)
    {
        return ds * dmInv;
    }

    Mat3 Calc_E(const Mat3& f)
    {
        return 0.5f * (f * f.transpose() - Mat3::Identity());
    }

    Mat3 Calc_dPsiDf(const Mat3& f, const Mat3& e)
    {
        FloatT youngsModulus = 8 * 10e8f;
        FloatT poissonRatio = 0.45f;
        FloatT lamba = youngsModulus * poissonRatio / ((1.0f + poissonRatio) * (1.0f - 2.0f * poissonRatio));
        FloatT mu = youngsModulus / (2.0f + 2.0f * poissonRatio);
        return mu * f * e + lamba * e.trace() * f;
    }

    Vec9 Reshape3x3(const Mat3& m)
    {
        Vec9 vec = Vec9::Zero();
        vec(0, 0) = m(0, 0);
        vec(1, 0) = m(1, 0);
        vec(2, 0) = m(2, 0);

        vec(3, 0) = m(0, 1);
        vec(4, 0) = m(1, 1);
        vec(5, 0) = m(2, 1);

        vec(6, 0) = m(0, 2);
        vec(7, 0) = m(1, 2);
        vec(8, 0) = m(2, 2);

        return vec;
    }

    Vec12 Reshape3x4(const Mat3x4& m)
    {
        Vec12 vec = Vec12::Zero();
        vec(0, 0) = m(0, 0);
        vec(1, 0) = m(1, 0);
        vec(2, 0) = m(2, 0);

        vec(3, 0) = m(0, 1);
        vec(4, 0) = m(1, 1);
        vec(5, 0) = m(2, 1);

        vec(6, 0) = m(0, 2);
        vec(7, 0) = m(1, 2);
        vec(8, 0) = m(2, 2);

        vec(9, 0) = m(0, 3);
        vec(10, 0) = m(1, 3);
        vec(11, 0) = m(2, 3);
        return vec;
    }

    Mat9x12 Calc_dFdx(const Mat3& dmInv)
    {
        auto m = dmInv(0, 0);
        auto n = dmInv(0, 1);
        auto o = dmInv(0, 2);
        auto p = dmInv(1, 0);
        auto q = dmInv(1, 1);
        auto r = dmInv(1, 2);
        auto s = dmInv(2, 0);
        auto t = dmInv(2, 1);
        auto u = dmInv(2, 2);

        auto t1 = -m - p - s;
        auto t2 = -n - q - t;
        auto t3 = -o - r - u;

        Mat9x12 dFdx = Mat9x12::Zero();

        dFdx(0, 0) = t1;
        dFdx(0, 3) = m;
        dFdx(0, 6) = p;
        dFdx(0, 9) = s;
        dFdx(1, 1) = t1;
        dFdx(1, 4) = m;
        dFdx(1, 7) = p;
        dFdx(1, 10) = s;

        dFdx(2, 2) = t1;
        dFdx(2, 5) = m;
        dFdx(2, 8) = p;
        dFdx(2, 11) = s;
        dFdx(3, 0) = t2;
        dFdx(3, 3) = n;
        dFdx(3, 6) = q;
        dFdx(3, 9) = t;
        dFdx(4, 1) = t2;
        dFdx(4, 4) = n;

        dFdx(4, 7) = q;
        dFdx(4, 10) = t;
        dFdx(5, 2) = t2;
        dFdx(5, 5) = n;
        dFdx(5, 8) = q;
        dFdx(5, 11) = t;
        dFdx(6, 0) = t3;
        dFdx(6, 3) = o;
        dFdx(6, 6) = r;
        dFdx(6, 9) = u;
        dFdx(7, 1) = t3;
        dFdx(7, 4) = o;

        dFdx(7, 7) = r;
        dFdx(7, 10) = u;
        dFdx(8, 2) = t3;
        dFdx(8, 5) = o;
        dFdx(8, 8) = r;
        dFdx(8, 11) = u;

        return dFdx;
    }

    Vec9 Calc_g_i(const Mat3& f)
    {
        Vec9 g_i = Vec9::Zero();
        g_i(0, 0) = 2 * f(0, 0);
        g_i(1, 0) = 2 * f(1, 0);
        g_i(2, 0) = 2 * f(2, 0);

        g_i(3, 0) = 2 * f(0, 1);
        g_i(4, 0) = 2 * f(1, 1);
        g_i(5, 0) = 2 * f(2, 1);

        g_i(6, 0) = 2 * f(0, 2);
        g_i(7, 0) = 2 * f(1, 2);
        g_i(8, 0) = 2 * f(2, 2);
        return g_i;
    }

    Mat3 Calc_dj_df(const Mat3& f)
    {
        Mat3 dj_df = Mat3::Zero();
        dj_df.col(0) = f.col(1).cross(f.col(2));
        dj_df.col(1) = f.col(2).cross(f.col(0));
        dj_df.col(2) = f.col(0).cross(f.col(1));

        //dj_df.block(0, 0, 3, 1) = f.col(1).cross(f.col(2));
        //dj_df.block(0, 1, 3, 1) = f.col(2).cross(f.col(0));
        //dj_df.block(0, 2, 3, 1) = f.col(0).cross(f.col(1));
        return dj_df;
    }

    FloatT Calc_i_c(const Mat3& f)
    {
        auto frob = f.norm();
        return frob * frob;
    }

    Mat9 Calc_h_i()
    {
        return 2 * Mat9::Identity();
    }

    Mat3 Calc_f_hat(const Vec3& f_i)
    {
        Mat3 f_hat = Mat3::Zero();
        // col 0
        f_hat(1, 0) = f_i(2);
        f_hat(2, 0) = -f_i(1);

        // col 1
        f_hat(0, 1) = -f_i(2);
        f_hat(2, 1) = f_i(0);

        // col 2
        f_hat(0, 2) = f_i(1);
        f_hat(1, 2) = -f_i(0);

        return f_hat;
    }

    Mat9 Calc_h_j(const Mat3& f)
    {
        Mat9 h_j = Mat9::Zero();
        auto f0_hat = Calc_f_hat(f.col(0));
        auto f1_hat = Calc_f_hat(f.col(1));
        auto f2_hat = Calc_f_hat(f.col(2));

        // col 0
        h_j.block(3, 0, 3, 3) = f2_hat;
        h_j.block(6, 0, 3, 3) = -f1_hat;

        // col 1
        h_j.block(0, 3, 3, 3) = -f2_hat;
        h_j.block(6, 3, 3, 3) = f0_hat;

        // col 2
        h_j.block(0, 6, 3, 3) = f1_hat;
        h_j.block(3, 6, 3, 3) = -f0_hat;

        return h_j;
    }

    Mat9 Calc_D(const Mat3& f)
    {
        Mat9 d = Mat9::Zero();
        const auto& f0 = f.col(0);
        const auto& f1 = f.col(1);
        const auto& f2 = f.col(2);

        d.block(0, 0, 3, 3) = f0 * f0.transpose();
        d.block(3, 0, 3, 3) = f0 * f1.transpose();
        d.block(6, 0, 3, 3) = f0 * f2.transpose();

        d.block(0, 3, 3, 3) = f1 * f0.transpose();
        d.block(3, 3, 3, 3) = f1 * f1.transpose();
        d.block(6, 3, 3, 3) = f1 * f2.transpose();

        d.block(0, 6, 3, 3) = f2 * f0.transpose();
        d.block(3, 6, 3, 3) = f2 * f1.transpose();
        d.block(6, 6, 3, 3) = f2 * f2.transpose();

        return d;
    }

    Mat9 KroneckerProduct(const Mat3& a, const Mat3& b)
    {
        Mat9 product = Mat9::Zero();
        product.block(0, 0, 3, 3) = a(0, 0) * b;
        product.block(3, 0, 3, 3) = a(1, 0) * b;
        product.block(6, 0, 3, 3) = a(2, 0) * b;

        product.block(0, 3, 3, 3) = a(0, 1) * b;
        product.block(3, 3, 3, 3) = a(1, 1) * b;
        product.block(6, 3, 3, 3) = a(2, 1) * b;

        product.block(0, 6, 3, 3) = a(0, 2) * b;
        product.block(3, 6, 3, 3) = a(1, 2) * b;
        product.block(6, 6, 3, 3) = a(2, 2) * b;

        return product;
    }

    Mat9 Calc_h_2(const Mat3& f, const Mat9& d)
    {
        return 4 * (KroneckerProduct(Mat3::Identity(), f * f.transpose()) + KroneckerProduct(f * f.transpose(), Mat3::Identity()) + d);
    }

    Mat9 Calc_vec_dPsi2_dF2(const Vec9& g_i, FloatT i_c, const Mat9& h_i, const Mat9& h_2, FloatT mu, FloatT lambda)
    {
        return lambda / 4.0f * (g_i * g_i.transpose()) + (-mu / 2.0f + lambda / 4.0f * (i_c - 3.0f)) * h_i + mu / 4.0f * h_2;
    }

    Mat12 Calc_dfdx(const Mat9x12& dFdx, const Mat9 & vec_dPsi2_dF2, FloatT restVol)
    {
        return -restVol * dFdx.transpose() * vec_dPsi2_dF2 * dFdx;
    }

    Mat3 Get_Q(int i, const std::array<FloatT, 3>& epsilon, const std::array<FloatT, 3>& sigma, const Mat3& u, const Mat3& v_transpose)
    {
        Mat3 d = Mat3::Zero();
        d(0, 0) = sigma[0] * sigma[2] + sigma[1] * epsilon[i];
        d(1, 1) = sigma[1] * sigma[2] + sigma[0] * epsilon[i];
		d(2, 2) = epsilon[i] * epsilon[i] - sigma[2] * sigma[2];
        return 1.0f / d.norm() * u * d * v_transpose;
    }

    Mat3 Get_Q(const Mat3 & U, const Mat3 & V, int i)
    {
        //Mat3 q = Mat3::Zero();

        //if (i == 3)
        //{
        //    q(0, 1) = -1;
        //    q(1, 0) = 1;
        //}
        //else if (i == 4)
        //{
        //    q(1, 2) = 1;
        //    q(2, 1) = -1;
        //}
        //else if (i == 5)
        //{
        //    q(0, 2) = 1;
        //    q(2, 0) = -1;
        //}
        //else if (i == 6)
        //{
        //    q(0, 1) = 1;
        //    q(1, 0) = 1;
        //}
        //else if (i == 7)
        //{
        //    q(1, 2) = 1;
        //    q(2, 1) = 1;
        //}
        //else if (i == 8)
        //{
        //    q(0, 2) = 1;
        //    q(2, 0) = 1;
        //}

        //return 1 / sqrt(2.0f) * u * q * v_transpose;

        static const FloatT scale = 1.0 / std::sqrt(2.0);
        const Mat3 sV = scale * V;

        using M3 = Eigen::Matrix<FloatT, 3, 3, Eigen::ColMajor>;

        M3 A;
        A << sV(0, 2) * U(0, 1), sV(1, 2)* U(0, 1), sV(2, 2)* U(0, 1),
            sV(0, 2)* U(1, 1), sV(1, 2)* U(1, 1), sV(2, 2)* U(1, 1),
            sV(0, 2)* U(2, 1), sV(1, 2)* U(2, 1), sV(2, 2)* U(2, 1);

        M3 B;
        B << sV(0, 1) * U(0, 2), sV(1, 1)* U(0, 2), sV(2, 1)* U(0, 2),
            sV(0, 1)* U(1, 2), sV(1, 1)* U(1, 2), sV(2, 1)* U(1, 2),
            sV(0, 1)* U(2, 2), sV(1, 1)* U(2, 2), sV(2, 1)* U(2, 2);

        M3 C;
        C << sV(0, 2) * U(0, 0), sV(1, 2)* U(0, 0), sV(2, 2)* U(0, 0),
            sV(0, 2)* U(1, 0), sV(1, 2)* U(1, 0), sV(2, 2)* U(1, 0),
            sV(0, 2)* U(2, 0), sV(1, 2)* U(2, 0), sV(2, 2)* U(2, 0);

        M3 D;
        D << sV(0, 0) * U(0, 2), sV(1, 0)* U(0, 2), sV(2, 0)* U(0, 2),
            sV(0, 0)* U(1, 2), sV(1, 0)* U(1, 2), sV(2, 0)* U(1, 2),
            sV(0, 0)* U(2, 2), sV(1, 0)* U(2, 2), sV(2, 0)* U(2, 2);

        M3 E;
        E << sV(0, 1) * U(0, 0), sV(1, 1)* U(0, 0), sV(2, 1)* U(0, 0),
            sV(0, 1)* U(1, 0), sV(1, 1)* U(1, 0), sV(2, 1)* U(1, 0),
            sV(0, 1)* U(2, 0), sV(1, 1)* U(2, 0), sV(2, 1)* U(2, 0);

        M3 F;
        F << sV(0, 0) * U(0, 1), sV(1, 0)* U(0, 1), sV(2, 0)* U(0, 1),
            sV(0, 0)* U(1, 1), sV(1, 0)* U(1, 1), sV(2, 0)* U(1, 1),
            sV(0, 0)* U(2, 1), sV(1, 0)* U(2, 1), sV(2, 0)* U(2, 1);

        if (i == 3)
            return B - A;
        else if (i == 4)
            return D - C;
        else if (i == 5)
            return F - E;
        else if (i == 6)
            return A + B;
        else if (i == 7)
            return C + D;
        else if (i == 8)
            return E + F;
    }

    FloatT Calc_Epsilon(int i, FloatT I2, FloatT I3)
    {
        return 2 * sqrtf(I2 / 3.0f) * cosf(1.0f / 3.0f * (acosf(3*I3/I2*sqrtf(3/I2)) + 2*glm::pi<FloatT>()*(i-1)));
    }

    void BuildTwistAndFlipEigenvectors(const Mat3& U, const Mat3& V, Mat9& Q)
    {
        static const FloatT scale = 1.0 / std::sqrt(2.0);
        const Mat3 sV = scale * V;

        using M3 = Eigen::Matrix<FloatT, 3, 3, Eigen::ColMajor>;

        M3 A;
        A << sV(0, 2) * U(0, 1), sV(1, 2)* U(0, 1), sV(2, 2)* U(0, 1),
            sV(0, 2)* U(1, 1), sV(1, 2)* U(1, 1), sV(2, 2)* U(1, 1),
            sV(0, 2)* U(2, 1), sV(1, 2)* U(2, 1), sV(2, 2)* U(2, 1);

        M3 B;
        B << sV(0, 1) * U(0, 2), sV(1, 1)* U(0, 2), sV(2, 1)* U(0, 2),
            sV(0, 1)* U(1, 2), sV(1, 1)* U(1, 2), sV(2, 1)* U(1, 2),
            sV(0, 1)* U(2, 2), sV(1, 1)* U(2, 2), sV(2, 1)* U(2, 2);

        M3 C;
        C << sV(0, 2) * U(0, 0), sV(1, 2)* U(0, 0), sV(2, 2)* U(0, 0),
            sV(0, 2)* U(1, 0), sV(1, 2)* U(1, 0), sV(2, 2)* U(1, 0),
            sV(0, 2)* U(2, 0), sV(1, 2)* U(2, 0), sV(2, 2)* U(2, 0);

        M3 D;
        D << sV(0, 0) * U(0, 2), sV(1, 0)* U(0, 2), sV(2, 0)* U(0, 2),
            sV(0, 0)* U(1, 2), sV(1, 0)* U(1, 2), sV(2, 0)* U(1, 2),
            sV(0, 0)* U(2, 2), sV(1, 0)* U(2, 2), sV(2, 0)* U(2, 2);

        M3 E;
        E << sV(0, 1) * U(0, 0), sV(1, 1)* U(0, 0), sV(2, 1)* U(0, 0),
            sV(0, 1)* U(1, 0), sV(1, 1)* U(1, 0), sV(2, 1)* U(1, 0),
            sV(0, 1)* U(2, 0), sV(1, 1)* U(2, 0), sV(2, 1)* U(2, 0);

        M3 F;
        F << sV(0, 0) * U(0, 1), sV(1, 0)* U(0, 1), sV(2, 0)* U(0, 1),
            sV(0, 0)* U(1, 1), sV(1, 0)* U(1, 1), sV(2, 0)* U(1, 1),
            sV(0, 0)* U(2, 1), sV(1, 0)* U(2, 1), sV(2, 0)* U(2, 1);

        // Twist eigenvectors
        //std::cout << "eigenvectors before mapping" << std::endl;
        //std::cout << Q << std::endl;
        //std::cout << "B-A" << std::endl;
        //std::cout << B - A << std::endl;
        Eigen::Map<M3>(Q.data()) = B - A;
        //std::cout << "eigenvectors after single mapping" << std::endl;
        //std::cout << Q << std::endl;
        Eigen::Map<M3>(Q.data() + 9) = D - C;
        Eigen::Map<M3>(Q.data() + 18) = F - E;

        // Flip eigenvectors
        Eigen::Map<M3>(Q.data() + 27) = A + B;
        Eigen::Map<M3>(Q.data() + 36) = C + D;
        Eigen::Map<M3>(Q.data() + 45) = E + F;
    }

    Mat9 CalcProjectedHessian(FloatT mu, FloatT lambda, const Mat3& F, const Mat3& U, const Mat3& V, const Vec3& S)
    {
        Vec9 eigenvalues;
        Mat9 eigenvectors;

        const FloatT J = F.determinant();

        // Compute the twist and flip eigenvalues
        {
            // Twist eigenvalues
            eigenvalues.segment<3>(0) = S;
            // Flip eigenvalues
            eigenvalues.segment<3>(3) = -S;
            const FloatT evScale = lambda * (J - 1.0) - mu;
            eigenvalues.segment<6>(0) *= evScale;
            eigenvalues.segment<6>(0).array() += mu;
        }

        // Compute the twist and flip eigenvectors
        BuildTwistAndFlipEigenvectors(U, V, eigenvectors);

        // Compute the remaining three eigenvalues and eigenvectors
        {
            Mat3 A;
            const FloatT s0s0 = S(0) * S(0);
            const FloatT s1s1 = S(1) * S(1);
            const FloatT s2s2 = S(2) * S(2);
            A(0, 0) = mu + lambda * s1s1 * s2s2;
            A(1, 1) = mu + lambda * s0s0 * s2s2;
            A(2, 2) = mu + lambda * s0s0 * s1s1;
            const FloatT evScale = lambda * (2.0 * J - 1.0) - mu;
            A(0, 1) = evScale * S(2);
            A(1, 0) = A(0, 1);
            A(0, 2) = evScale * S(1);
            A(2, 0) = A(0, 2);
            A(1, 2) = evScale * S(0);
            A(2, 1) = A(1, 2);

            //std::cout << "A" << std::endl;
            //std::cout << A << std::endl;

            const Eigen::SelfAdjointEigenSolver<Mat3> Aeigs(A);
            eigenvalues.segment<3>(6) = Aeigs.eigenvalues();

   //         std::cout << "u" << std::endl;
   //         std::cout << U << std::endl;

   //         std::cout << "v" << std::endl;
   //         std::cout << V << std::endl;

   //         std::cout << "A eigs" << std::endl;
   //         std::cout << Aeigs.eigenvalues() << std::endl;
   //         std::cout << "Unrotated eig vectors" << std::endl;
			//std::cout << Aeigs.eigenvectors() << std::endl;

   //         std::cout << "Rotated eigen 0 matrix" << std::endl;
   //         std::cout << U * Aeigs.eigenvectors().col(0).asDiagonal() * V.transpose() << std::endl;

   //         std::cout << "eigenvectors before mapping" << std::endl;
   //         std::cout << eigenvectors << std::endl;
            Eigen::Map<Mat3>(eigenvectors.data() + 54) = U * Aeigs.eigenvectors().col(0).asDiagonal() * V.transpose();
            //std::cout << "eigenvectors after single mapping" << std::endl;
            //std::cout << eigenvectors << std::endl;
            Eigen::Map<Mat3>(eigenvectors.data() + 63) = U * Aeigs.eigenvectors().col(1).asDiagonal() * V.transpose();
            Eigen::Map<Mat3>(eigenvectors.data() + 72) = U * Aeigs.eigenvectors().col(2).asDiagonal() * V.transpose();
        }

        // Clamp the eigenvalues
        for (int i = 0; i < 9; i++)
        {
            //std::cout << "lambda_" << i << " " << eigenvalues(i) << std::endl;

            if (eigenvalues(i) < 0.0)
            {
                eigenvalues(i) = 0.0;
                //eigenvalues(i) = -eigenvalues(i);

            }
        }
        //std::cout << "eigenvalues" << std::endl;
        //std::cout << eigenvalues << std::endl;

        //std::cout << "eigenvectors" << std::endl;
        //std::cout << eigenvectors << std::endl;

        return eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose();
    }

    Mat3x4 CalcBm(const std::array<size_t, 4>& vertIndices, const std::vector<Vertex>& vertices)
    {
        std::vector<Vec3> vertices_converted;
        glm::ivec4 tetIndices({ vertIndices[0], vertIndices[1], vertIndices[2], vertIndices[3] });

        for (int i = 0; i < 4; i++)
        {
            const auto& pos = vertices[tetIndices[i]].mMaterialCoordinates;
            vertices_converted.push_back(Vec3(pos.x, pos.y, pos.z));
        }

        // TODO: Eliminate this class, it is just used to get the area normal
        class Triangle final
        {
        public:
            Triangle(const Vec3& v0, const Vec3& v1, const Vec3& v2)
                : _x0(v0)
                , _x1(v1)
                , _x2(v2)
            {}

            // TODO: Make this a constructor
            static Triangle GenerateFace(const int faceNum, const glm::ivec4& tet, const std::vector<Vec3>& verts)
            {
                assert(faceNum >= 0); assert(faceNum < 4);
                if (faceNum == 0)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[0])], verts[static_cast<unsigned long>(tet[1])], verts[static_cast<unsigned long>(tet[3])]);
                }
                else if (faceNum == 1)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[0])], verts[static_cast<unsigned long>(tet[2])], verts[static_cast<unsigned long>(tet[1])]);
                }
                else if (faceNum == 2)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[3])], verts[static_cast<unsigned long>(tet[2])], verts[static_cast<unsigned long>(tet[0])]);
                }
                else if (faceNum == 3)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[1])], verts[static_cast<unsigned long>(tet[2])], verts[static_cast<unsigned long>(tet[3])]);
                }
                else
                {
                    std::cerr << "Error, impossible code path hit." << std::endl;
                    std::exit(EXIT_FAILURE);
                }
            }

            // TODO: Roll these into one computation
            Vec3 normal() const
            {
                return ((_x1 - _x0).cross(_x2 - _x0)).normalized();
            }
            FloatT area() const
            {
                return 0.5 * ((_x1 - _x0).cross(_x2 - _x0)).norm();
            }
        private:
            const Vec3& _x0;
            const Vec3& _x1;
            const Vec3& _x2;
        };

        Mat3x4 Bm;
		const Triangle tri0 = Triangle::GenerateFace(0, tetIndices, vertices_converted);
		const Triangle tri1 = Triangle::GenerateFace(1, tetIndices, vertices_converted);
		const Triangle tri2 = Triangle::GenerateFace(2, tetIndices, vertices_converted);
		const Triangle tri3 = Triangle::GenerateFace(3, tetIndices, vertices_converted);

		// Calculate the area vectors
		// v0 is incident on faces (0,1,2)
		Bm.col(0) = tri0.normal() * tri0.area() + tri1.normal() * tri1.area() + tri2.normal() * tri2.area();
		// v1 is incident on faces (0,1,3)
		Bm.col(1) = tri0.normal() * tri0.area() + tri1.normal() * tri1.area() + tri3.normal() * tri3.area();
		// v2 is incident on faces (1,2,3)
		Bm.col(2) = tri1.normal() * tri1.area() + tri2.normal() * tri2.area() + tri3.normal() * tri3.area();
		// v3 is incident on faces (0,2,3)
		Bm.col(3) = tri0.normal() * tri0.area() + tri2.normal() * tri2.area() + tri3.normal() * tri3.area();
		Bm /= -3.0;

        return Bm;
    }

    Eigen::VectorXf ComputePerturbedForce(const TetraGroup& group, const Eigen::VectorXf& deltaX)
    {
        size_t numVertices = group.mVertices.size();
        Eigen::VectorXf globalForce = Eigen::VectorXf::Zero(3 * numVertices);

        for (const auto& pair : group.mIdToTetrahedra)
        {
            const auto& tet = pair.second;

            auto ds = Calc_Ds(tet, group.mVertices, false);
            auto dmInv = Calc_DmInv(tet, group.mVertices);
            auto f = Calc_F(ds, dmInv);

            auto j = f.determinant();
            auto dj_df = Calc_dj_df(f);
            auto g_j = Reshape3x3(dj_df);
            Mat3 dPsi_dF = group.mMu * f + (group.mLambda * (j - 1.0f) - group.mMu) * dj_df;

            auto dF_dx = Calc_dFdx(dmInv);
            auto dPsi_dx = dF_dx.transpose() * Reshape3x3(dPsi_dF);
            Vec12 force = -tet.mRestVolume * dPsi_dx; // 12x1 atm

            Eigen::JacobiSVD<Mat3, Eigen::NoQRPreconditioner> svd_f(f, Eigen::ComputeFullU | Eigen::ComputeFullV);
            auto sigmas = svd_f.singularValues();
            auto u = svd_f.matrixU();
            auto v = svd_f.matrixV();

            if (u.determinant() < 0.0)
            {
                u.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }
            if (v.determinant() < 0.0)
            {
                v.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }

            // Contribution to globalB
            for (size_t i = 0; i < 4; i++)
            {
                auto vert_i = tet.mIndices[i];
                globalForce(3 * vert_i + 0) += force(3 * i + 0);
                globalForce(3 * vert_i + 1) += force(3 * i + 1);
                globalForce(3 * vert_i + 2) += force(3 * i + 2);
            }
        }

        return globalForce;
    }

    bool ImplicitUpdate(TetraGroup& group, float timestep, bool saveFrame, IronGames::SimulationFrame* frame, float deltaVThreshold)
    {
        size_t numVertices = group.mVertices.size();
        Eigen::SparseMatrix<float> globalA(3* numVertices, 3* numVertices);
        Eigen::VectorXf globalB = Eigen::VectorXf::Zero(3 * numVertices);
		Eigen::VectorXf globalX = Eigen::VectorXf::Zero(3 * numVertices);
        Vec12 nodeVelocities = Vec12::Zero();
        Vec12 localB = Vec12::Zero();

        Eigen::VectorXf globalForce = Eigen::VectorXf::Zero(3 * numVertices);
        Eigen::VectorXf globalVelocities = Eigen::VectorXf::Zero(3 * numVertices);
        Eigen::SparseMatrix<float> globalDfDx(3 * numVertices, 3 * numVertices);
        Eigen::SparseMatrix<float> massMatrix(3 * numVertices, 3 * numVertices);

        //std::cout << "timestep " << timestep << std::endl;

        for (size_t i = 0; i < group.mVertices.size(); i++)
        {
            globalA.coeffRef(3 * i, 3 * i) = group.mVertices[i].mMass;
            globalA.coeffRef(3 * i + 1, 3 * i + 1) = group.mVertices[i].mMass;
			globalA.coeffRef(3 * i + 2, 3 * i + 2) = group.mVertices[i].mMass;
        }

        for (const auto& pair : group.mIdToTetrahedra)
        {
            const auto& tet = pair.second;

            auto ds = Calc_Ds(tet, group.mVertices, false);
            //std::cout << "ds" << std::endl;
            //std::cout << ds << std::endl;
            auto dmInv = Calc_DmInv(tet, group.mVertices);
            //std::cout << "dmInv" << std::endl;
            //std::cout << dmInv << std::endl;
            auto f = Calc_F(ds, dmInv);
            //std::cout << "f" << std::endl;
            //std::cout << f << std::endl;

            // Stable Neo-hookean potential
			auto j = f.determinant();
			auto dj_df = Calc_dj_df(f); // SEEMS OK
            //std::cout << "dj_df" << std::endl;
            //std::cout << dj_df << std::endl;
			auto g_j = Reshape3x3(dj_df);
            //auto dPsi_dF = group.mMu * Reshape3x3(f) + group.mLambda * (j - 1.0f - group.mMu/group.mLambda) * g_j;
            Mat3 dPsi_dF = group.mMu * f + (group.mLambda * (j - 1.0f) - group.mMu) * dj_df;
            //auto elementForce = dPsi_dF * CalcBm(tet.mIndices, group.mVertices);

			auto dF_dx = Calc_dFdx(dmInv);
            //std::cout << "dfdx" << std::endl;
            //std::cout << dF_dx << std::endl;
			auto dPsi_dx = dF_dx.transpose() * Reshape3x3(dPsi_dF);
			Vec12 force = -tet.mRestVolume * dPsi_dx; // 12x1 atm

            //std::cout << "Force" << std::endl;
            //std::cout << force << std::endl;
            //force = -Reshape3x4(elementForce); // 12x1 atm

			auto h_j = Calc_h_j(f);
            //std::cout << "h_j" << std::endl;
            //std::cout << h_j << std::endl;
            //std::cout << "g_j * g_j^T" << std::endl;
            //std::cout << (g_j * g_j.transpose()) << std::endl;
			//Mat9 dPsi2_dF2 = group.mMu * Mat9::Identity() + group.mLambda * (j - 1.0f - group.mMu / group.mLambda) * h_j + group.mLambda * g_j * g_j.transpose();

            Eigen::JacobiSVD<Mat3, Eigen::NoQRPreconditioner> svd_f(f, Eigen::ComputeFullU | Eigen::ComputeFullV);
            auto sigmas = svd_f.singularValues();
            auto u = svd_f.matrixU();
            auto v = svd_f.matrixV();

            if (u.determinant() < 0.0)
            {
                u.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }
            if (v.determinant() < 0.0)
            {
                v.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }

            auto dPsi2_dF2 = CalcProjectedHessian(group.mMu, group.mLambda, f, u, v, sigmas);

			auto dfdx = Calc_dfdx(dF_dx, dPsi2_dF2, tet.mRestVolume);

            //std::cout << "dfdx" << std::endl;
            //std::cout << dfdx << std::endl;

            // Contribution to globalA
            // 12 x 12 matrix df/dx
            // the force on one of the 4 vertices is a 3-vector
            // the derivative of that with respect to the spatial coordinates of all four node spatial coordinates should give me a 3x12 matrix (?)
            // then we have that for each of the nodes
            // and we get a 12x12
            // so a single element tells me how the force in coordinate (x,y,z) on node i changes with respect to the (x,y,z) position of node j, makes sense
            // and a 3x3 block tells me how the force on node i changes with respect to the position of node j
            // so it's probably easier to consider the global matrix as n x n where elements are 3x3 rather than 3n x 3n with scalar elements.
            for (size_t i = 0; i < 4; i++)
            {
                auto vert_i = tet.mIndices[i];

                for (size_t j = 0; j < 4; j++)
                {
                    auto vert_j = tet.mIndices[j];

                    // we want to set a 3x3 block in globalA here from (3*vertId to 3*vertId + 2, 3*otherVertId to 3*otherVertId + 2)
                    // and we should draw from (3*i to 3*i + 2, 3*j to 3*j + 2)
                    for (size_t sub_i = 0; sub_i < 3; sub_i++)
                    {
                        for (size_t sub_j = 0; sub_j < 3; sub_j++)
                        {
                            globalA.coeffRef(3 * vert_i + sub_i, 3 * vert_j + sub_j) -= timestep * timestep * dfdx(3 * i + sub_i, 3 * j + sub_j);
                            globalDfDx.coeffRef(3 * vert_i + sub_i, 3 * vert_j + sub_j) += dfdx(3 * i + sub_i, 3 * j + sub_j);
                        }
                    }
                }
            }

            for (size_t i = 0; i < 4; i++)
            {
                nodeVelocities(3 * i + 0) = group.mVertices[tet.mIndices[i]].mVelocity.x;
                nodeVelocities(3 * i + 1) = group.mVertices[tet.mIndices[i]].mVelocity.y;
                nodeVelocities(3 * i + 2) = group.mVertices[tet.mIndices[i]].mVelocity.z;
            }

            localB = timestep * force + timestep * timestep * dfdx * nodeVelocities;

            if (isnan(localB.norm()))
                throw std::exception("nan detected in local forces");

            // Contribution to globalB
            for (size_t i = 0; i < 4; i++)
            {
                auto vert_i = tet.mIndices[i];
                globalB(3 * vert_i + 0) += localB(3 * i + 0);
                globalB(3 * vert_i + 1) += localB(3 * i + 1);
                globalB(3 * vert_i + 2) += localB(3 * i + 2);

                globalForce(3 * vert_i + 0) += force(3 * i + 0);
                globalForce(3 * vert_i + 1) += force(3 * i + 1);
                globalForce(3 * vert_i + 2) += force(3 * i + 2);
            }
        }

        // Apply additional per-node forces
        for (int i = 0; i < group.mVertices.size(); i++)
        {
            // Gravity
            globalB(3 * i + 1) += -9.8 * timestep * group.mVertices[i].mMass;

            // Collision forces
            //globalB(3 * i + 0) += group.mVertices[i].mForce.x;
            //globalB(3 * i + 1) += group.mVertices[i].mForce.y;
            //globalB(3 * i + 2) += group.mVertices[i].mForce.z;
        }

        if (isnan(globalA.norm()))
            throw std::exception("Nan detected.");

        if (isnan(globalB.norm()))
            throw std::exception("Nan detected.");

        //{
        //    auto denseGlobalA = globalA.toDense();

        //    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(denseGlobalA);
        //    auto eigs = solver.eigenvalues();
        //    auto minVal = 1000.0f;
        //    for (int i = 0; i < eigs.size(); i++)
        //        minVal = std::min(minVal, eigs[i]);

        //    if (minVal < 0)
        //    {
        //        std::cout << "Timestep : " << timestep << std::endl;
        //        std::cout << "Negative eig" << std::endl;
        //    }
        //}

        //std::cout << "globalA" << std::endl;
        //std::cout << globalA << std::endl;

        //std::cout << "globalB" << std::endl;
        //std::cout << globalB << std::endl;

        // PCG Solver
        {
            Eigen::ConjugateGradient<Eigen::SparseMatrix<float>, Eigen::Lower | Eigen::Upper> solver;
            solver.compute(globalA);

            globalX = solver.solve(globalB);
            if (isnan(solver.error()))
                throw std::exception("Solver failed.");

            //std::cout << "globalX" << std::endl;
            //std::cout << globalX << std::endl;
            //std::cout << "globalX inf norm: " << globalX.lpNorm<Eigen::Infinity>() << std::endl;
            //if (globalX.lpNorm<Eigen::Infinity>() > deltaVThreshold)
            //    return false;
            // lets see, make the threshold value here 1

            std::string solverInfo;
            if (solver.info() == 0)
                solverInfo = std::string("Success");
            else if (solver.info() == 1)
                solverInfo = std::string("Numerical issue");
            else if (solver.info() == 2)
                solverInfo = std::string("No convergence");
            else if (solver.info() == 3)
                solverInfo = std::string("Invalid input");

            if (solver.info() != 0)
				std::cout << "Solver info : " << solverInfo << std::endl;
        }

        // Check error against nonlinear equation
        {
            for (int i = 0; i < numVertices; i++)
            {
                globalVelocities(3 * i + 0) = group.mVertices[i].mVelocity[0];
                globalVelocities(3 * i + 1) = group.mVertices[i].mVelocity[1];
                globalVelocities(3 * i + 2) = group.mVertices[i].mVelocity[2];
                massMatrix.coeffRef(3 * i, 3 * i) = 1.0/group.mVertices[i].mMass;
                massMatrix.coeffRef(3 * i + 1, 3 * i + 1) = 1.0/group.mVertices[i].mMass;
                massMatrix.coeffRef(3 * i + 2, 3 * i + 2) = 1.0/group.mVertices[i].mMass;
            }

            // dv = h M^-1 (f0 + df/dx * dx)
            auto dx = timestep * (globalVelocities + globalX);
            auto perturbedForce = ComputePerturbedForce(group, dx);
            auto residual = timestep * massMatrix * perturbedForce - globalX;
            // this isn't the nonlinear eq. unfortunately, we'd need to evaluate the force with new position
            // to get that
            std::cout << "timestep: " << timestep << std::endl;
            std::cout << "infinity error: " << residual.lpNorm<Eigen::Infinity>() << std::endl;
            std::cout << "l2 error: " << residual.norm() << std::endl;
            if (residual.lpNorm<Eigen::Infinity>() > 10.0)
            {
                return false;
            }
        }

        // SparseLU Solver
        //{
        //    Eigen::SparseLU<Eigen::SparseMatrix<float>, Eigen::COLAMDOrdering<int> >   solver;
        //    solver.analyzePattern(globalA);
        //    solver.factorize(globalA);
        //    globalX = solver.solve(globalB);

        //    if (isnan(globalX.norm()))
        //        throw std::exception("Solver failed.");
        //}

        // Integrate
        for (int i = 0; i < group.mVertices.size(); i++)
        {
            auto& vertex = group.mVertices[i];
            auto deltaV = glm::dvec3(globalX(3 * i + 0), globalX(3 * i + 1), globalX(3 * i + 2));

            if (saveFrame)
            {
                auto& vert = *frame->mutable_vertices(i);
                *vert.mutable_force() = ProtoConverter::Convert(deltaV);
            }

            vertex.mVelocity += deltaV;
            vertex.mPosition += (double)timestep * vertex.mVelocity;
        }

        return true;

        // things to try:
        // - 'solveWithGuess' and providing the previous timestep's change in velocities as the guess?
        // - other preconditioners, solvers
        // - the doc's have globalX = solver.solve(globalB) twice, so maybe it tries for a specified number of
        //   iterations each time and refines the solution.. ?
    }
}

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


#include "alpha.h"

#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <numeric>

#include "operationVector.h" // ceci va chercher le fichier qui contient les operations sur les vecteurs

using namespace std;
using namespace boost::numeric::ublas;

matrix<double> Alpha(matrix<double> Obs, matrix<double> M, std::vector<double> pie) {

// 	Obs est la matrice des observations
// 	M est la matrice de transition
// 	pie est le vecteur de probabilite initiale

	const int m = Obs.size2(); // Nombre detat, de cases
	const int n = Obs.size1(); // Nombre dobservation, duree de la partie

//	matrix Alpha = matrix(NbLigne= n, NbCol= m);  Initialisation de la matric Alpha de tailler n*m
	matrix<double> matAlpha(n, m);
	auto alph = std::vector<double>(m, 0); // Declaration d'un vecteur de longueur m remplie de 0

	for (int i = 0; i < m; ++i) {
		alph[i] = pie[i] * Obs(0, i);
	}

	double log1surc = log(sum(alph)); // J'ai ajouter la fonction sum, qui fait la somme d'une matrice ou d'un vecteur

// 	alph = alph * (1 / std::accumulate(alph.begin(), alph.end(), 0));
	alph = scalarMultiplication(alph, 1.0 / sum(alph));

	for (int i = 0; i < m; ++i) {
		matAlpha(0, i) = log(alph[i]) + log1surc;
	}

	auto Alph = matrix<double>(1, m);

	for (int i = 0 ; i < m ; ++i) {
		Alph(0, i) = alph[i];
	}

	for (int i = 1; i < n ; ++i) { // J'ai changer la partie "i < n, ++j" pour "i < n ; ++i"

		Alph = prod(Alph, M); // prod est le produit matriciel

		for (int j = 0; j < m ; ++j) {
			Alph(0, j) = Alph(0, j) * Obs(i, j);
		}

		log1surc += log(sum(Alph));
		Alph /= sum(Alph);

		for (int j = 0; j < m ; ++j) {
			matAlpha(i, j) = log(Alph(0, j)) + log1surc;
		}

	}

	return matAlpha;

}


// The template and inlines for the -*- C++ -*- finite field classes.
// Initially implemented by Wai-Shing Luk <luk036@gmail.com>
//

/** @file include/GF.hpp
 *  This is a C++ Library header.
 */

#ifndef FUN_GF_HPP
#define FUN_GF_HPP 1

#include <boost/operators.hpp>
#include <cassert>

namespace fun {
/**
 * @defgroup GF Finite (Galois) Field
 * @ingroup arithmetic
 *
 * Classes and functions for (extended) finite field.
 * @{
 */

// Forward declarations.
template <unsigned int _p> struct GF;

/**
 *  finite (Galois) field.
 *
 *  @param  p  Prime number
 */
template <unsigned int _p = 3> struct GF : boost::field_operators<GF<_p>> {
  /// Default constructor.
  constexpr GF() noexcept : _m(0) {}

  ///  Unspecified parameters default to 0.
  explicit GF(unsigned int m) noexcept : _m(m) { assert(_m < _p); }

  // Lets the compiler synthesize the copy constructor
  // GF (const GF<_p>&);
  /// Copy constructor
  GF(const GF<_p> &s) noexcept : _m(s.value()) { assert(_m >= 0); }

  /// Return internal element of finite field.
  constexpr unsigned int value() const noexcept { return _m; }

  // Lets the compiler synthesize the assignment operator
  // GF<_p>& operator= (const GF<_p>&);
  /// Assign this finite field to finite field @a s.
  GF<_p> &operator=(const GF<_p> &s) {
    _m = s.value();
    return *this;
  }

  /// Add @a s to this finite field.
  GF<_p> &operator+=(const GF<_p> &s) {
    _m += s.value();
    _m %= _p;
    assert(_m < _p);
    return *this;
  }

  /// Subtract @a s from this finite field.
  GF<_p> &operator-=(const GF<_p> &s) {
    _m += _p - s.value();
    _m = _m % _p;
    assert(_m < _p);
    return *this;
  }

  /// Multiply @a s to this finite field.
  GF<_p> &operator*=(const GF<_p> &s) {
    _m *= s.value();
    _m %= _p;
    assert(_m < _p);
    return *this;
  }

private:
  unsigned int _m;
};

// Operators:
///  Return new finite field @a r plus @a s.
template <unsigned int _p> inline GF<_p> operator+(GF<_p> r, const GF<_p> &s) {
  return r += s;
}

///  Return new finite field @a r minus @a s.
template <unsigned int _p> inline GF<_p> operator-(GF<_p> r, const GF<_p> &s) {
  return r -= s;
}

//@{
///  Return new finite field @a r times @a a.
template <unsigned int _p> inline GF<_p> operator*(GF<_p> r, const GF<_p> &s) {
  return r *= s;
}
//@}

/// Return @a r.
template <unsigned int _p>
inline constexpr GF<_p> operator+(const GF<_p> &r) noexcept {
  return r;
}

/// Return negation of @a r
template <unsigned int _p> inline GF<_p> operator-(const GF<_p> &r) {
  if (r.value() == 0)
    return r;
  return GF<_p>(_p - r.value());
}

/// Return true if @a r is equal to @a s.
template <unsigned int _p>
inline constexpr bool operator==(const GF<_p> &r, const GF<_p> &s) noexcept {
  return r.value() == s.value();
}

/// Return false if @a r is equal to @a s.
template <unsigned int _p>
inline constexpr bool operator!=(const GF<_p> &r, const GF<_p> &s) noexcept {
  return !(r == s);
}

///  Insertion operator for finite field values.
template <unsigned int _p, class _Stream>
_Stream &operator<<(_Stream &os, const GF<_p> &r) {
  os << '(' << r.value() << " mod " << _p << ')';
  return os;
}
}

#endif


/** @file
 * @ingroup utils
 *
 * Utility mathematical functions that do not fit anywhere else.
 *
 * The type definitions are shorthands for declaring `Eigen::Vector`,
 * `Eigen::Matrix` and `Eigen::Quaternion` objects. At current we assume all
 * slam code development will be using double precision floating points.
 */

#ifndef WAVE_UTILS_MATH_HPP
#define WAVE_UTILS_MATH_HPP

#include <iostream>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace wave {
/** @addtogroup utils
 *  @{ */

#ifndef EIGEN_TYPEDEF
#define EIGEN_TYPEDEF
typedef Eigen::Matrix<double, 1, 1> Vec1;
typedef Eigen::Vector2d Vec2;
typedef Eigen::Vector3d Vec3;
typedef Eigen::Vector4d Vec4;
typedef Eigen::Matrix<double, 5, 1> Vec5;
typedef Eigen::Matrix<double, 6, 1> Vec6;
typedef Eigen::Matrix<double, 12, 1> Vec12;
typedef Eigen::VectorXd VecX;

typedef Eigen::Matrix2d Mat2;
typedef Eigen::Matrix3d Mat3;
typedef Eigen::Matrix4d Mat4;
typedef Eigen::Matrix<double, 5, 5> Mat5;
typedef Eigen::Matrix<double, 6, 6> Mat6;
typedef Eigen::Matrix<double, 12, 12> Mat12;
typedef Eigen::MatrixXd MatX;

typedef Eigen::Matrix<double, 3, 4> Mat34;

typedef Eigen::Matrix<float, 1, 1> Vec1f;
typedef Eigen::Vector2f Vec2f;
typedef Eigen::Vector3f Vec3f;
typedef Eigen::Vector4f Vec4f;
typedef Eigen::Matrix<float, 5, 1> Vec5f;
typedef Eigen::Matrix<float, 6, 1> Vec6f;
typedef Eigen::Matrix<float, 12, 1> Vec12f;
typedef Eigen::VectorXf VecXf;

typedef Eigen::Matrix2f Mat2f;
typedef Eigen::Matrix3f Mat3f;
typedef Eigen::Matrix4f Mat4f;
typedef Eigen::Matrix<float, 5, 5> Mat5f;
typedef Eigen::Matrix<float, 6, 6> Mat6f;
typedef Eigen::Matrix<float, 12, 12> Mat12f;
typedef Eigen::MatrixXf MatXf;

typedef Eigen::Matrix<float, 3, 4> Mat34f;

typedef Eigen::Affine3d Affine3;

typedef Eigen::Quaterniond Quaternion;
#endif

/**
 * Eigen vector comparator
 */
struct VecComparator {
    bool operator()(const VecX &a, const VecX &b) const {
        return std::lexicographical_compare(
          a.data(), a.data() + a.size(), b.data(), b.data() + b.size());
    }
};

/**
 * Eigen matrix comparator
 */
struct MatComparator {
    bool operator()(const MatX &a, const MatX &b) const {
        return std::lexicographical_compare(
          a.data(), a.data() + a.size(), b.data(), b.data() + b.size());
    }
};

/** Generates random integer with a upper bound `ub` and lower bound `lb` using
 * a uniform random distribution. */
int randi(int ub, int lb);

/** Generates random float with a upper bound `ub` and lower bound `lb` using a
 * uniform random distribution. */
double randf(double ub, double lb);

/** Compares two floating point numbers `f1` and `f2` with an error threshold
 * defined by `threshold`.
 * @return
 * - `0`: If `f1` == `f2` or the difference is less then `threshold`
 * - `1`: If `f1` > `f2`
 * - `-1`: If `f1` < `f2`
 */
int fltcmp(double f1, double f2, double threshold = 0.0001);

/** @return the median of `v`. */
double median(std::vector<double> v);

/** Converts degrees to radians. */
double deg2rad(double d);

/** Converts radians to degrees. */
double rad2deg(double r);

/** Reshapes a vector `x` to matrix `y` of size `rows` and `cols` */
void vec2mat(std::vector<double> x, int rows, int cols, MatX &y);

/** Reshapes a matrix to a vector*/
void mat2vec(MatX A, std::vector<double> &x);

/** Wraps `euler_angle` to 180 degrees **/
double wrapTo180(double euler_angle);

/** Wraps `euler_angle` to 360 degrees **/
double wrapTo360(double euler_angle);

/** Convert euler angle to rotation matrix **/
int euler2rot(Vec3 euler, int euler_seq, Mat3 &R);

/** Convert euler angle to quaternion **/
int euler2quat(Vec3 euler, int euler_seq, Quaternion &q);

/** Convert quaternion to euler angles **/
int quat2euler(Quaternion q, int euler_seq, Vec3 &euler);

/** Convert quaternion to rotation matrix **/
int quat2rot(Quaternion q, Mat3 &R);

/** ENU to NWU coordinate system **/
void enu2nwu(const Vec3 &enu, Vec3 &nwu);

/** NED to ENU coordinate system **/
void ned2enu(const Vec3 &ned, Vec3 &enu);

/** NED to NWU coordinate system **/
void ned2nwu(const Quaternion &ned, Quaternion &enu);

/** NWU to ENU coordinate system **/
void nwu2enu(const Vec3 &nwu, Vec3 &enu);

/** NWU to NED coordinate system **/
void nwu2ned(const Quaternion &nwu, Quaternion &ned);

/** NWU to EDN coordinate system **/
void nwu2edn(const Vec3 &nwu, Vec3 &edn);

/** @} group utils */
}  // namespace wave

#endif  // WAVE_UTILS_MATH_HPP


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

namespace ublas = boost::numeric::ublas;
/* Matrix inversion routine.
   Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */

bool InvertMatrix (const ublas::matrix<double>& input, ublas::matrix<double>& inverse)
{
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
  // create a working copy of the input
  matrix<double> A(input);
  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());
  // perform LU-factorization
  int res = lu_factorize(A,pm);
  if( res != 0 ) return false;
  // create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<double>(A.size1()));
  // backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);
  return true;
}

bool InvertMatrixGen(const ublas::matrix<double>& input, ublas::matrix<double>& inverse)
{
  const int n = input.size1();
  gsl_matrix *A = gsl_matrix_alloc(n,n);
  gsl_matrix *V = gsl_matrix_alloc(n,n);
  gsl_vector *S = gsl_vector_alloc(n);
  gsl_vector *work = gsl_vector_alloc(n);

  for(int i = 0; i < n; ++i) {
    for(int j = 0; j < n; ++j) {
      gsl_matrix_set(A,i,j,input(i,j));
      inverse(i,j) = 0.0;
    }
  }

  gsl_linalg_SV_decomp(A,V,S,work);

  const double smax = gsl_vector_get(S,0);
  for(int k = 0; k < n; ++k) {
    const double s = gsl_vector_get(S,k);
    if(s/smax > 1.0e-8) {
      for(int i = 0; i < n; ++i) {
	for(int j = 0; j < n; ++j) {
	  inverse(i,j) += gsl_matrix_get(V,i,k) * gsl_matrix_get(A,j,k) / s;
	}
      }
    }
  }

  gsl_matrix_free(A);
  gsl_matrix_free(V);
  gsl_vector_free(S);
  gsl_vector_free(work);
  return true;
}


#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Hyperbolic_octagon_translation.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

#include <iostream>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>           Traits;
typedef Traits::FT                                                              NT;
typedef Traits::Point_2                                                         Point;
typedef Traits::Circle_2                                                        Circle;
typedef Traits::Euclidean_line_2                                                Line;
typedef Traits::Construct_intersection_2                                        Construct_intersection_2;

int main(int /*argc*/, char** /*argv*/)
{
  NT F2(2);

  Line ell1(NT(1),  sqrt(F2 - sqrt(F2)), NT(0));
  Line ell2(NT(1), -sqrt(F2 + sqrt(F2)), NT(0));

  // In the Poincaré disk, all Euclidean lines pass through the origin
  NT sx = NT(0);
  NT sy = NT(0);
  Point sol(sx, sy);

  Point p = Construct_intersection_2()(ell1, ell2);
  std::cout << "line-line intersection: " << p << ", solution is: " << sol << std::endl;
  assert(p == sol);
  std::cout << "The solution is exact!" << std::endl;

  Point pc1(NT(2)/NT(10), NT(5)/NT(10));
  Point pc2(-NT(3)/NT(10), NT(15)/NT(100));
  Point pc3(NT(20)/NT(29), NT(50)/NT(29));
  Circle c1(pc1, pc2, pc3);
  std::pair<Point, Point> ipt = Construct_intersection_2()(ell1, c1);
  Point ip1 = ipt.first, ip2 = ipt.second;

  NT sq(sqrt(NT(2)));
  NT nsq( sqrt( NT(2) - sqrt(NT(2)) ) );
  NT sol1x( nsq*(NT(2438)+NT(1451)*nsq-sqrt(NT(3933846)-NT(31801)*sq+NT(7075076)*nsq))/(-NT(4320)+NT(1440)*sq) );
  NT sol1y( (-NT(2438)-NT(1451)*nsq+sqrt(NT(3933846)-NT(31801)*sq+NT(7075076)*nsq))/(-NT(4320)+NT(1440)*sq) );
  Point solution1(sol1x, sol1y);

  std::cout << "Intersection of circle and line: " << ip1 << " and " << ip2 << std::endl;
  std::cout << "Solution: " << solution1 << std::endl;
  assert(ip1 == solution1 || ip2 == solution1);
  std::cout << "The solution is exact!" << std::endl;

  Point pc4(NT(2)/NT(10), -NT(15)/NT(100));
  Circle c2(pc1, pc4, pc3);

  std::pair<Point, Point> cpt = Construct_intersection_2()(c1, c2);
  ip1 = cpt.first;
  ip2 = cpt.second;
  std::cout << "Intersection of circles: " << ip1 << " and " << ip2 << std::endl;
  assert(ip1 == pc1 || ip2 == pc1);
  std::cout << "The solution is exact!" << std::endl;

  return EXIT_SUCCESS;
}


/*
 * DataSet.cpp
 *
 *  Created on: 2014/06/11
 *      Author: kryozahiro
 */

#include "DataSet.h"

#include <cfloat>
#include <boost/lexical_cast.hpp>
#include "cpputil/GenericIo.h"
using namespace std;
using namespace cpputil;

std::vector<double> DataSet::nguyen(int dimension, const std::vector<double>& input) {
	vector<double> output(1, 0);
	for (int i = 1; i <= dimension; ++i) {
		output[0] += pow(input[0], (double)i);
	}
	return output;
}

std::vector<double> DataSet::rosenbrock(const std::vector<double>& input) {
	vector<double> output(1, 0);
	for (unsigned int i = 0; i < input.size() - 1; ++i) {
		output[0] += 100.0 * pow(input[i + 1] - input[i] * input[i], 2.0) + pow(input[i] - 1.0, 2.0);
	}
	return output;
}

std::vector<double> DataSet::schwefel(const std::vector<double>& input) {
	vector<double> output(1, 418.9829 * input.size());
	for (unsigned int i = 0; i < input.size(); ++i) {
		output[0] -= input[i] * sin(sqrt(abs(input[i])));
	}
	return output;
}

std::vector<double> DataSet::sphere(const std::vector<double>& input) {
	vector<double> output(1, 0);
	for (unsigned int i = 0; i < input.size(); ++i) {
		output[0] += input[i] * input[i];
	}
	return output;
}

std::vector<double> DataSet::threeHumpCamel(const std::vector<double>& input) {
	vector<double> output(1, 0);
	output[0] = 2.0*input[0]*input[0] - 1.05*pow(input[0], 4.0) + pow(input[0], 6.0)/6.0 + input[0]*input[1] + input[1]*input[1];
	return output;
}

DataSet::DataSet() {
}

DataSet::DataSet(const ProgramType& programType) : programType(programType) {
}

DataSet::DataSet(const boost::property_tree::ptree& dataTree, mt19937_64& randomEngine) {
	string name = dataTree.get<string>("Name");
	int dimension = dataTree.get<int>("Dimension");
	int size = dataTree.get<int>("Size");
	if (name.find("Nguyen") != string::npos) {
		programType = ProgramType(DataType(-1, 1, 0.001, 1), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, [=](const vector<double>& input) {
			return nguyen(dimension, input);
		});
	} else if (name.find("Rosenbrock") != string::npos) {
		programType = ProgramType(DataType(-5, 10, 0.001, dimension), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, rosenbrock);
	} else if(name.find("Schwefel") != string::npos) {
		programType = ProgramType(DataType(-500, 500, 0.001, dimension), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, schwefel);
	} else if (name.find("Sphere") != string::npos) {
		programType = ProgramType(DataType(-5.12, 5.12, 0.001, dimension), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, sphere);
	} else if (name.find("ThreeHumpCamel") != string::npos) {
		programType = ProgramType(DataType(-5, 5, 0.001, 2), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, threeHumpCamel);
	} else {
		assert(false);
	}
}

const ProgramType& DataSet::getProgramType() const {
	return programType;
}

int DataSet::getSize() const {
	return data.size();
}

pair<vector<double>, vector<double>>& DataSet::operator[](int i) {
	return data[i];
}

const pair<vector<double>, vector<double>>& DataSet::operator[](int i) const {
	return data[i];
}

vector<double>& DataSet::getInput(int i) {
	return data[i].first;
}

vector<double>& DataSet::getOutput(int i) {
	return data[i].second;
}

void DataSet::addData(const vector<double>& input, const vector<double>& output) {
	data.push_back(make_pair(input, output));
}

string DataSet::toString() const {
	string ret = "DataSet\n";
	for (const pair<vector<double>, vector<double>>& point : data) {
		ret += boost::lexical_cast<string>(point) + "\n";
	}
	return ret;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
                  {

 /*!
    @ingroup group-arithmetic
    Function object implementing average capabilities

    Computes the arithmetic mean of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = average(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = (x+y)/T(2);
    @endcode

    for integer types  it returns a rounded value at a distance guaranteed
    less or equal to 0.5 of the average floating value,  but can differ
    of one unity from the truncation given by (x1+x2)/T(2).

    @par Note:

    This function does not overflow.

    @see meanof

  **/
  const boost::dispatch::functor<tag::average_> average = {};
} }
#endif

#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/simd/average.hpp>

#endif


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <vector>
using namespace std;

/**********************************************************

This is the program for the bifurcation diagram! :D
The flow is simple: For each r value between 1 and 4 (with
resolution of dr), 100 values of the logistic map are saved
in the solutions vector. The values are chosen far enough
away from the origin to remove the effects of the initial
behavior. The values are paired with the r value they were
calculated at and then sent to the plotter.

**********************************************************/

const int START = 900;
const int CUTOFF = 1000;	// Cutoff value for finding attractors
const double dr = 0.001;
const double x0 = 0.5;		// Initial value

void plotStuff(vector<double> x, vector<double> y){	// Pretty plotting shenanigans
	Gnuplot gp;

	gp << "set xrange [1:4]\n";
	gp << "set yrange [0:1]\n";
	gp << "set term wxt font \"FreeSerif,12\"\n";
	gp << "set title \"End Behavior of the Logistic Function vs. Growth Rate (x_0 = 0.5)\"\n";
	gp << "set xlabel \"r\"\n";
	gp << "set ylabel \"x_n\"\n";
	gp << "set output \"plot.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(x,y));
}

vector<double> getSolutions(double r){
	double x = x0;							// Set up x_0
	vector<double> out;						// All the solutions for this value of r

	for(int i = 0; i < CUTOFF; i++){		// Loop until the cutoff
		if(i >= START){out.push_back(x);}	// Push the x value onto the vector of values, but only after a few (900) iterations to observe only the long-term behavior
		x = r*x*(1-x);						// Perform the logistic map operation x_n+1 = r*x_n*(1-x_n)
	}

	return out;
}

int main(){
	vector<double> rvals;					// R values (there will be lots of repeats since each r value has 100 solutions)
	vector<double> solutions;				// Solutions

	for(double r = 1; r < 4; r += dr){		// between r=1 and r=4
		vector<double> s = getSolutions(r);	// Get the solutions for that value of r

		for(int i = 0; i < s.size(); i++){	// Go through all solutions
			solutions.push_back(s[i]);		// Add them to the main list
			rvals.push_back(r);				// Along with the value of r they were recorded at
		}
	}

	cout << "Done with calculation!\n";		// This is for debug

	plotStuff(rvals,solutions);				// Plot it!
}


/*
 * Main.cpp
 *
 *  Created on: Nov 10, 2015
 *      Author: doncole
 */

#include <stdio.h>
#include <iostream>
#include <Eigen>
#include <Dense>

using namespace std;
using namespace Eigen;

ArrayXXf InitMatrix(ArrayXXf matrix);
ArrayXf InitVector(ArrayXf vector);
ArrayXXf AddRowToMatrix(ArrayXf rowA, ArrayXXf matrixU, int i);
ArrayXf AddNumBToVectorB(float numB,ArrayXf vectorB, int j);

int main(void){

	int squareNum = 7;	//square matrix 7x7 and vector = 7

	ArrayXXf matrixA(squareNum,squareNum);
	ArrayXXf matrixU(squareNum,squareNum);
	ArrayXf vectorB(squareNum);
	ArrayXf rowA(squareNum), rowB(squareNum), solutionX(squareNum);
	float numA, numB;
	//Init the Matrix and Vector
	matrixA = InitMatrix(matrixA);
	vectorB = InitVector(vectorB);
	matrixU = matrixA;

	cout << "Matrix A " << endl << matrixA << endl;
	cout << "Vector B " << endl << vectorB << endl;
	//Solve by G.E.
	//iterate squareNum-1 times
	for (int i = 0; i < squareNum -1; i++)
	{
		rowA = matrixU.row(i);	//Get the row
		numA = rowA[i];	//Get the Rows first number
		rowA = rowA/numA; 	//divide the row by the number
		numB = vectorB[i]/numA;	//divide the Vector by the number
		//numB = vectorB[i];	//copy the number for the rest of the vector

		for (int j = i+1; j< squareNum; j++)
		{
			rowB = rowA;	//get the beginning Row again
//			cout << "Matrix U" << endl << matrixU << endl;	//print the Matrix out
			//Multiply the Row by the Next row's first Integer
			//get the next rows first integer
//			cout << "The next Rows integer" << matrixU.row(j)[i] << endl;
			rowB = rowB * (-1.0) * matrixU.row(j)[i];
			numB = (numB * (-1.0) * matrixU.row(j)[i]);
//			cout << "The num B " << endl << numB << endl;
//			cout << "The row multiplied By" << endl << rowB << endl;
			//Add the row to the next Rows
			matrixU = AddRowToMatrix(rowB, matrixU, j);		//add the row to the rest of the matrix
			vectorB = AddNumBToVectorB(numB, vectorB, j);	//add the number to the rest of the vector
		}
	}
	solutionX[squareNum-1] = vectorB[squareNum-1]/matrixU.row(squareNum-1)[squareNum-1];
//	cout << "Solution X" << endl << solutionX << endl;

	for (int i = squareNum-2; i>-1; i--)
	{
		float sum = 0;
//		cout << "VectorB" << endl << vectorB[i] << endl;
//		matrixU.row(i)/matrixU.row(i)[i];
//		vectorB[i] = vectorB[i]/matrixU.row(i)[i];
//		cout << "Matrix U" << endl << matrixU << endl;
//		cout << "Vector B" << endl << vectorB << endl;

		for (int j = 6; j > i; j--)
		{
			sum = sum + matrixU.row(i)[j] * solutionX[i+1];
			//cout << "sum" << endl << sum << endl;
		}
		solutionX[i] = (vectorB[i] - sum)/matrixU.row(i)[i];
//		cout << "Solution X" << endl << solutionX << endl;
	}
	//Print the outputs
	cout << "Matrix U " << endl << matrixU << endl;
	cout << "Vector B " << endl << vectorB << endl;
	cout << "The Solution Matrix " << endl << solutionX << endl;

	return 0;
}

ArrayXf AddNumBToVectorB(float numB, ArrayXf vectorB, int row)
{
	//vectorB[0] = 100.0;
	while (row < 7)
	{
		vectorB[row] = vectorB[row] + numB;
		row++;
	}
	//print the Vector
	//cout << "VectorB" << endl << vectorB << endl;
	return vectorB;
}

ArrayXXf AddRowToMatrix(ArrayXf rowA, ArrayXXf matrixU, int row)
{
	//we need to construct the matrices
	//By first adding the row to the rest of the matrix
//	ArrayXXf tempArray(7, 7);
//	tempArray = matrixU.array();
	while (row < 7)
	{
		//add to the rest of the rows
		matrixU.row(row) += rowA;
		row++;
	}
	//print the array
	//cout << "MatrixU" << endl << matrixU << endl;
	return matrixU;
}

//Initialize the Given 7x7 Matrix
ArrayXXf InitMatrix(ArrayXXf matrix)
{
	int i= 0, j = 0;
	//First Row
	matrix(i, j++) = .1897;
	matrix(i, j++) = .3784;
	matrix(i, j++) = .6449;
	matrix(i, j++) = .7271;
	matrix(i, j++) = .4449;
	matrix(i, j++) = .1730;
	matrix(i, j++) = .0118;
	j = 0;
	i++;
	//Second Row
	matrix(i, j++) = .1934;
	matrix(i, j++) = .8600;
	matrix(i, j++) = .8180;
	matrix(i, j++) = .3093;
	matrix(i, j++) = .6946;
	matrix(i, j++) = .9797;
	matrix(i, j++) = .8939;
	j = 0;
	i++;
	//Third Row
	matrix(i, j++) = .6822;
	matrix(i, j++) = .8537;
	matrix(i, j++) = .6602;
	matrix(i, j++) = .8385;
	matrix(i, j++) = .6213;
	matrix(i, j++) = .2714;
	matrix(i, j++) = .1991;
	j = 0;
	i++;
	//FourthRow
	matrix(i, j++) = .3028;
	matrix(i, j++) = .5936;
	matrix(i, j++) = .3420;
	matrix(i, j++) = .5681;
	matrix(i, j++) = .7948;
	matrix(i, j++) = .2523;
	matrix(i, j++) = .2987;
	j = 0;
	i++;
	//Fifth Row
	matrix(i, j++) = .5417;
	matrix(i, j++) = .4966;
	matrix(i, j++) = .2897;
	matrix(i, j++) = .3704;
	matrix(i, j++) = .9568;
	matrix(i, j++) = .8757;
	matrix(i, j++) = .6614;
	j = 0;
	i++;
	//6th Row
	matrix(i, j++) = .1509;
	matrix(i, j++) = .8998;
	matrix(i, j++) = .3412;
	matrix(i, j++) = .7027;
	matrix(i, j++) = .5226;
	matrix(i, j++) = .7373;
	matrix(i, j++) = .2844;
	j = 0;
	i++;
	//7th Row
	matrix(i, j++) = .6979;
	matrix(i, j++) = .8216;
	matrix(i, j++) = .5341;
	matrix(i, j++) = .5466;
	matrix(i, j++) = .8801;
	matrix(i, j++) = .1365;
	matrix(i, j++) = .4692;

	return matrix;
}

ArrayXf InitVector(ArrayXf vector){
	int i = 0;
	vector(i++) = .5000;
	vector(i++) = -.3000;
	vector(i++) = -.2000;
	vector(i++) = .7000;
	vector(i++) = -.9000;
	vector(i++) = .3000;
	vector(i++) = .1000;
	return vector;
}


#pragma once
#include <Eigen/Core>
#include <boost/math/constants/constants.hpp>

#include "ear/common_types.hpp"
#include "ear/metadata.hpp"

namespace bear {
// internal utilities from libear

template <typename T>
inline T radians(T d)
{
  return d * static_cast<T>(boost::math::constants::pi<double>() / 180.0);
}

template <typename T>
inline T degrees(T r)
{
  return r * static_cast<T>(180.0 / boost::math::constants::pi<double>());
}

double azimuth(Eigen::Vector3d position);
double elevation(Eigen::Vector3d position);
double distance(Eigen::Vector3d position);

Eigen::Vector3d cart(double azimuth, double elevation, double distance);

Eigen::Vector3d toCartesianVector3d(ear::PolarPosition position);
Eigen::Vector3d toCartesianVector3d(ear::CartesianPosition position);
Eigen::Vector3d toCartesianVector3d(ear::PolarSpeakerPosition position);
Eigen::Vector3d toCartesianVector3d(ear::CartesianSpeakerPosition position);

}  // namespace bear


/* Last Revised Date: 13_DEC_2014 */
/* Lastest status: All ok    */

/*
  det_model_hh_post.hpp : implimentation of HH system (similar but not exact for the classical HH equations)
  !!!NOTE: all units are in SI system

  Copyright (C) 2015 Anup Gopalakrishna Pillai, Suhita Nadkarni Lab, IISER Pune <anupgpillai@gmail.com>

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef DET_MODEL_HH_POST_HPP_INCLUDED
#define DET_MODEL_HH_POST_HPP_INCLUDED

#include <vector>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>

//UNITS: all SI units - seconds, Volts, Ampere, Meters, Simenes, Farads

//DECLARATIONS FOR HH_POST_MODEL

class DET_MODEL_HH_POST
{
private:
  const double pi = boost::math::constants::pi<double>();
  double Cm = 1.0E-02;   // F/m^2 Specific capacitance
  //----Reversal potentials for Na, K, channels in mV
  double ENa = 50.0E-03;   // Reversal potential for Sodium currents in Volts
  double EK = -85.0E-03;    // Reversal potential for Potassium currents in Volts
  double ELeak = -81E-03; // Reversal potential for Leak currents in Volts

  double GNa = 130E01;   // Maximum conductance for Sodium channels S/m^2
  double GK = 36E01;    // Maximum conductance for Potassium channels S/m^2
  double GLeak = 0.3E01;  // Maximum conductance for Leak channels S/m^2

public:
  std::vector<double> X;

  double IExt = 0.0; //extern current in Amps
  double Area = 0.0;
  double Cap = 0.0;
  double V = 0.0;      // Membrane voltage V
  /* NEURON class explicit constructor */
  explicit DET_MODEL_HH_POST( std::vector<double> X_ = {0,0,0,0},double Area_ = 8.5E-12): Area(Area_), X(X_), Cap(Cm * Area) // Area in m^2
  {
  };
  //---Equations for bouton Na Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
  double alpha_Na_act(const double V);
  double beta_Na_act(const double V);
  double alpha_Na_inact(const double V);
  double beta_Na_inact(const double V);
  //---Equations for bouton K Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
  double alpha_K_act(const double V);
  double beta_K_act(const double V);
  //---Declaration of the ODE solver
  template <class State >
  void operator() ( const State &x, State &dxdt , const double  t );
};
//Function declarations
//------------- Na+ channels --- implimented from classical HH (1952)
double DET_MODEL_HH_POST::alpha_Na_act(const double V)
{
  double a = 0.1E03;
  double b = 45.33E-03;
  double c = 10E-03;
  double val = 0.0;
  val = (- a * ( (V + b) / (exp( -(V + b) / c ) - 1 ) ) );
  return (val);
}
double DET_MODEL_HH_POST::beta_Na_act(const double V)
{
  double a = 4;
  double b = 70.33E-03;
  double c = 18E-03;
  double val = 0.0;
  val = ( a * ( exp( - (V + b) / c )) );
  return (val);
}
double DET_MODEL_HH_POST::alpha_Na_inact(const double V)
{
  double val = 0.0;
  val = 0.07 * exp( -( V + 70.33E-03) /20E-03 );
  return (val);
}
double DET_MODEL_HH_POST::beta_Na_inact(const double V)
{
  double val = 0.0;
  val = ( 1 / ( exp( - (V + 40.33E-03) / 10E-03) + 1) );
  return (val);
}
//--------K+ channel alpha & beta
double DET_MODEL_HH_POST::alpha_K_act(const double V)
{
  double val = 0;
  val = -0.01E03 * (V + 60.33E-03) / ( exp( -(V +  60.33E-03) / 10E-03) - 1 );
  return (val);
};
double DET_MODEL_HH_POST::beta_K_act(const double V)
{
  double val = 0.0;
  val = 0.125 * exp (- (V + 70.33E-03) / 80E-03);
  return (val);
};
//-------HH_PRE class ODE Function

template <class State>
void DET_MODEL_HH_POST::operator() ( const State &x, State &dxdt , const double t )
{

  dxdt[0] = 1E03 * (( alpha_Na_act(x[3]) * (1 - x[0]) ) - (beta_Na_act(x[3]) * x[0]) ); // Na activation
  dxdt[1] = 1E03 * ( ( alpha_Na_inact(x[3]) * (1 - x[1]) ) - (beta_Na_inact(x[3]) * x[1])) ; // Na inactivation
  dxdt[2] = 1E03 * ( ( alpha_K_act(x[3]) * (1 - x[2]) ) - (beta_K_act(x[3]) * x[2])); // K activation
  double INa =   -(Area * GNa * pow(x[0],3) * x[1] * (x[3] - ENa) );
  double IK =   -(Area * GK * pow(x[2],4) * (x[3] - EK) );
  double ILeak = -(Area * GLeak *  (x[3] - ELeak) );
  dxdt[3] =  (1/Cap) * (INa + IK + ILeak  + (IExt * Area)  );
};
#endif // DET_MODEL_HH_POST_HPP_INCLUDED


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix
  Eigen::Matrix<double, 4, 4> elem_mat;

  //====================

  // Get element matrix for negative laplacian of given cell
  lf::uscalfe::LinearFELaplaceElementMatrix prov;
  Eigen::MatrixXd A = prov.Eval(cell);

  // First get the area
  double K = lf::geometry::Volume(*geo_ptr);

  // Initialize elem. stiffness matrix
  Eigen::Matrix<double, 4, 4> M;

  if (ref_el == lf::base::RefEl::kTria()) {
    M << 2.0, 1.0, 1.0, 0.0,
         1.0, 2.0, 1.0, 0.0,
         1.0, 1.0, 2.0, 0.0,
         0.0, 0.0, 0.0, 0.0;
    M *= K/12.0;
  } else if (ref_el == lf::base::RefEl::kQuad()) {
    M << 4.0, 2.0, 1.0, 2.0,
         2.0, 4.0, 2.0, 1.0,
         1.0, 2.0, 4.0, 2.0,
         2.0, 1.0, 2.0, 4.0;
    M *= K/36.0;
  }

  elem_mat = A + M;

  //====================

  return elem_mat;
}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


/**
   \file conjugate_gradient.hpp
   \brief powerll optimization method
   \author Junhua Gu
 */

#ifndef CONJUGATE_GRADIENT
#define CONJUGATE_GRADIENT
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cassert>
#include <cmath>
#include "../linmin/linmin.hpp"
#include <math/num_diff.hpp>
#include <algorithm>
#include <iostream>

namespace opt_utilities
{
  /**
     \brief Impliment of an optimization method
     \tparam rT return type of the object function
     \tparam pT parameter type of the object function
   */
  template <typename rT,typename pT>
  class conjugate_gradient
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;
    volatile bool bstop;
    //typedef blitz::Array<rT,2> array2d_type;

    const char* do_get_type_name()const
    {
      return "conjugate gradient method";
    }
  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    rT threshold;
    array1d_type g;
    array1d_type h;
    array1d_type xi;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

  private:
    void clear_xi()
    {
    }

    void init_xi(int n)
    {
      clear_xi();
      g=array1d_type(n);
      h=array1d_type(n);
      xi=array1d_type(n);
    }

    void cg(array1d_type& p,const T ftol,
	   int& iter,T& fret)
    {
      const int ITMAX=200;
      const T EPS=std::numeric_limits<T>::epsilon();

      int j,its;
      int n=p.size();
      T gg,gam,fp,dgg;
      fp=func(p);
      xi=gradient(*p_fo,p);
      for(j=0;j<n;++j)
	{
	  g[j]=-xi[j];
	  xi[j]=h[j]=g[j];
	}
      for(its=1;its<=ITMAX;++its)
	{
	  iter=its;
	  linmin(p,xi,fret,*p_fo);
	  //std::cerr<<"######:"<<its<<"\t"<<abs(fret-fp)/(abs(fret)+fabs(fp)+EPS)<<std::endl;
	  if(2.0*abs(fret-fp)<=ftol*(abs(fret)+fabs(fp)+EPS))
	    {
	      return;
	    }
	    fp=func(p);
	    xi=gradient(*p_fo,p);
	    dgg=gg=0;
	    for(j=0;j<n;++j)
	      {
		gg+=g[j]*g[j];
		//dgg+=(xi[j]+g[j])*xi[j];
		dgg+=xi[j]*xi[j];
	      }
	    //std::cerr<<its<<"\t"<<gg<<std::endl;
	    if(gg==0.0)
	      {
		return;
	      }
	    gam=dgg/gg;
	    for(j=0;j<n;++j)
	      {
		g[j]=-xi[j];
		xi[j]=h[j]=g[j]+gam*h[j];
	      }
	}
      //std::cerr<<"Too many iterations in cg"<<std::endl;
    }

  public:

    conjugate_gradient()
      :threshold(1e-4),g(0),h(0),xi(0)
    {}

    virtual ~conjugate_gradient()
    {
      clear_xi();
    };

    conjugate_gradient(const conjugate_gradient<rT,pT>& rhs)
      :opt_method<rT,pT>(rhs),p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       threshold(rhs.threshold),g(0),h(0),xi(0)
    {
    }

    conjugate_gradient<rT,pT>& operator=(const conjugate_gradient<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      start_point=rhs.start_point;
      end_point=rhs.end_point;
      threshold=rhs.threshold;
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new conjugate_gradient<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      resize(start_point,get_size(p));
      opt_eq(start_point,p);
    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_lower_limit(const array1d_type& p)
    {}

    void do_set_upper_limit(const array1d_type& p)
    {}

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      bstop=false;
      init_xi((int)get_size(start_point));

      int iter=100;
      opt_eq(end_point,start_point);
      rT fret;
#if 0
      for(int i=0;i<get_size(start_point);++i)
	{
	  array1d_type direction(start_point.size());
	  direction[i]=1;
	  linmin(end_point,direction,fret,(*p_fo));
	}
#endif
      cg(end_point,threshold,iter,fret);
      return end_point;
    }

    void do_stop()
    {
      bstop=true;
    }

  };

}

#endif
//EOF


//---------------------------------------------------------------------------//
//!
//! \file   tstExponentiationAlgorithms.cpp
//! \author Alex Robinson
//! \brief  Exponentiation algorithm unit tests.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/length.hpp>

// FRENSIE Includes
#include "Utility_ExponentiationAlgorithms.hpp"
#include "Utility_UnitTestHarnessWithMain.hpp"

//---------------------------------------------------------------------------//
// Testing Functions.
//---------------------------------------------------------------------------//
// Test the recursive exponentiation algorithm
FRENSIE_UNIT_TEST_TEMPLATE_EXPAND( Exponentiation, recursive,
				   std::tuple<int,int>,
                                   std::tuple<double,long>,
                                   std::tuple<double,unsigned long long> )
{
  FETCH_TEMPLATE_PARAM( 0, BaseScalarType );
  FETCH_TEMPLATE_PARAM( 1, ExponentOrdinalType );

  // Compute 2^30
  BaseScalarType value =
    Utility::Exponentiation::recursive<BaseScalarType,ExponentOrdinalType>(2,
									   30);
  FRENSIE_CHECK_EQUAL( value, 1073741824 );
}

//---------------------------------------------------------------------------//
// Test the static recursive exponentiation algorithm
FRENSIE_UNIT_TEST_TEMPLATE( Exponentiation, recursive_static,
                            double,
                            boost::units::quantity<boost::units::cgs::length>,
                            boost::units::quantity<boost::units::si::length> )
{
  FETCH_TEMPLATE_PARAM( 0, BaseScalarType );

  typedef typename Utility::QuantityTraits<BaseScalarType>::template GetQuantityToPowerType<30>::type ReturnType30;

  // Compute 2^30
  ReturnType30 value_30 = Utility::Exponentiation::recursive<30>( 2.*Utility::QuantityTraits<BaseScalarType>::one() );

  FRENSIE_CHECK_EQUAL( value_30, Utility::QuantityTraits<ReturnType30>::one()*1073741824. );

  // Compute 2^29
  typedef typename Utility::QuantityTraits<BaseScalarType>::template GetQuantityToPowerType<29>::type ReturnType29;

  ReturnType29 value_29 = Utility::Exponentiation::recursive<29>( 2.*Utility::QuantityTraits<BaseScalarType>::one() );

  FRENSIE_CHECK_EQUAL( value_29, Utility::QuantityTraits<ReturnType29>::one()*536870912. );
}

//---------------------------------------------------------------------------//
// Test the recursive modular exponentiation algorithm
FRENSIE_UNIT_TEST_TEMPLATE( Exponentiation, recursiveMod,
                            unsigned,
                            unsigned long,
                            unsigned long long )
{
  FETCH_TEMPLATE_PARAM( 0, OrdinalType );

  // Compute (2^30)mod(3)
  OrdinalType value =
    Utility::Exponentiation::recursiveMod<OrdinalType>(2,30,3);

  FRENSIE_CHECK_EQUAL( value, 1 );

  OrdinalType value2 =
    Utility::Exponentiation::recursiveMod<OrdinalType>(2,15,3);

  FRENSIE_CHECK_EQUAL( value2, 2 );
}

//---------------------------------------------------------------------------//
// end tstExponentiationAlgorithms.cpp
//---------------------------------------------------------------------------//


/// \file
// --------------------------------------------------------------------------
// This file is part of the reference implementation for the paper
//    QFib: Fast and Efficient Brain Tractogram Compression
//    C. Mercier*, S. Rousseau*, P. Gori, I. Bloch and T. Boubekeur
//    NeuroInformatics 2020
//    DOI: 10.1007/s12021-020-09452-0
//
// All rights reserved. Use of this source code is governed by a
// MIT license that can be found in the LICENSE file.
// --------------------------------------------------------------------------
#pragma once

#include "saveload.hpp"
#include <fstream>
#include <iostream>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Dense>

using namespace Eigen;

using namespace std;

namespace vc
{
int my_round(const unsigned type, const float value)
{
	switch (type) {
	case 0:
		return floor(value);
		break;
	case 1:
		return ceil(value);
		break;
	default:
		return round(value);
		break;
	}
}

///
/// \brief computeTrilinearInterpolatedValue Compute an interpolated value at the position, considering the grid
/// \param grid Grid to consider for the interpolated value
/// \param dim Dimension of the grid
/// \param gridPosition Position at which to compute the interpolated value
/// \return Return the computed value
///

float computeTrilinearInterpolatedValue(std::vector<float> const & grid, std::vector<unsigned> const & dim, Vector3f const & gridPosition)
{
	std::vector<float> value;
	for (unsigned pos2=0; pos2<2; pos2++)
		for (unsigned pos3=0; pos3<2; pos3++)
			for (unsigned pos1=0; pos1<2; pos1++)
				value.push_back(grid[(int)my_round(pos1,gridPosition(0))+dim[0]*(int)my_round(pos2,gridPosition(1))+dim[0]*dim[1]*(int)my_round(pos3,gridPosition(2))]);
	float xd = gridPosition(0) - floor(gridPosition(0));
	float yd = gridPosition(1) - floor(gridPosition(1));
	float zd = gridPosition(2) - floor(gridPosition(2));

	float c00 = value[0]*(1-xd)+value[1]*xd;
	float c01 = value[2]*(1-xd)+value[3]*xd;
	float c10 = value[4]*(1-xd)+value[5]*xd;
	float c11 = value[6]*(1-xd)+value[7]*xd;

	float c0 = c00*(1-yd)+c10*yd;
	float c1 = c01*(1-yd)+c11*yd;

	float val = c0*(1-zd)+c1*zd;

	return val;
}

//positive filter
int getSign(const float value)
{
	if (value>0)
		return 1;
	else
		return 0;
}

float k(const float x, const float dirX, const float dimV)
{
	return (getSign(dirX) + round(x/dimV)) * dimV;
}

float k2(const float x, const float dirX)
{
	return floor(x) + getSign(dirX);
}

///
/// \brief getValueFromSegment Compute the value along a segment
/// \param grid Grid to consider
/// \param dim Dimensions of the grid
/// \param p1 First point of the segment
/// \param p2 Second point of the segment
/// \return Return the value of the segment
///

float getValueFromSegment(std::vector<float> const & grid, std::vector<unsigned> const & dim, Vector3f const & p1, Vector3f const & p2)
{
	Vector3f direction = p2-p1;
	Vector3f directionN = direction.normalized();
	float totalDist = direction.norm();
	//Number of voxels changes along each axis
	unsigned voxelsX = static_cast<unsigned>(fabs(floor(p2.x())-floor(p1.x())));
	unsigned voxelsY = static_cast<unsigned>(fabs(floor(p2.y())-floor(p1.y())));
	unsigned voxelsZ = static_cast<unsigned>(fabs(floor(p2.z())-floor(p1.z())));
	unsigned totalVoxelsCrossed = 1 + voxelsX + voxelsY + voxelsZ;
	Vector3f pTemp = p1;
	float value = 0;
	float epsilon = 0.0001f;
	float gammaX, gammaY, gammaZ, gamma;

	for (unsigned i=0; i<totalVoxelsCrossed - 1; i++)
	{
		if (fabs(directionN.x())<epsilon)
			gammaX =  numeric_limits<float>::max();
		else
			gammaX = (k2(pTemp.x(), direction.x()) - pTemp.x()) / direction.x();
		if (fabs(directionN.y())<epsilon)
			gammaY =  numeric_limits<float>::max();
		else
			gammaY = (k2(pTemp.y(), direction.y()) - pTemp.y()) / direction.y();
		if (fabs(directionN.z())<epsilon)
			gammaZ =  numeric_limits<float>::max();
		else
			gammaZ = (k2(pTemp.z(), direction.z()) - pTemp.z()) / direction.z();
		gamma = min(min(gammaX, gammaY), gammaZ);
		pTemp += (gamma) * direction;
		value += (grid[(int)floor(pTemp.x())+dim[0]*(int)floor(pTemp.y())+(dim[0]*dim[1]*(int)floor(pTemp.z()))]);
	}
	value += (grid[(int)floor(p2.x())+dim[0]*(int)floor(p2.y())+(dim[0]*dim[1]*(int)floor(p2.z()))]);
	return (value / (float)(totalVoxelsCrossed));
}

///
/// \brief compareFA Function to compare the FA values and display the results
/// \param fibers1 First set of fibers
/// \param fibers2 Second set of fibers
/// \param faFilename FA file to use for FA computation
///

void compareFA(std::vector<std::vector<Vector3f>> const & fibers1, std::vector<std::vector<Vector3f>> const & fibers2, const string & faFilename)
{
	cout << faFilename << endl;
	std::vector<float> faValues, vox;
	std::vector<unsigned> dim;
	MatrixXf transform;
	fls::loadFAValues(faFilename, faValues, dim, transform, vox);

	transform.conservativeResize(4,4);
	transform.row(3) << 0,0,0,1;
	transform = transform.inverse();

	unsigned maxThreads = std::max(unsigned(1), std::thread::hardware_concurrency());
	std::vector<float> ecartMoyenTab(maxThreads, 0);
	std::vector<uint64_t> nbPointsTab(maxThreads, 0);
	std::vector<float> ecartMaxTab(maxThreads, 0);
	std::vector<float> averageFiber1Error(maxThreads, 0);
	std::vector<float> averageFiber2Error(maxThreads, 0);
#ifdef USE_OPENMP
#pragma omp parallel for
	for (unsigned i=0; i<fibers1.size(); i++)
	{
		if (fibers1[i].size() == fibers2[i].size())
		{
			nbPointsTab[omp_get_thread_num()] += fibers1[i].size();
			for (unsigned j=0; j<fibers1[i].size(); j++)
			{
				Vector3f p = fibers1[i][j];
				for (unsigned k=0; k<3; k++)
					p(k)/=vox[k];
				Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value1 = computeTrilinearInterpolatedValue(faValues, dim, gridPosition);
				Vector3f p2 = fibers2[i][j];
				for (unsigned k=0; k<3; k++)
					p2(k)/=vox[k];
				Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value2=computeTrilinearInterpolatedValue(faValues, dim, gridPosition2);
				float ecart = fabs(value1-value2);
				ecartMaxTab[omp_get_thread_num()] = max(ecart, ecartMaxTab[omp_get_thread_num()]);
				ecartMoyenTab[omp_get_thread_num()]+=ecart;
			}
		}

		//Per fiber error
		float fiber1Value = 0.f;
		float fiber2Value = 0.f;
		Vector3f p = fibers1[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber1Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers1[i].size()-1; j++)
		{
			Vector3f p1 = fibers1[i][j];
			Vector3f p2 = fibers1[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber1Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		p = fibers2[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber2Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers2[i].size()-1; j++)
		{
			Vector3f p1 = fibers2[i][j];
			Vector3f p2 = fibers2[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber2Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		fiber1Value /= float(fibers1[i].size());
		fiber2Value /= float(fibers2[i].size());
		averageFiber1Error[omp_get_thread_num()] += fiber1Value;
		averageFiber2Error[omp_get_thread_num()] += fiber2Value;
	}
#else
	parallel.For<unsigned>(0, fibers1.size(), 1, [&](unsigned i)
	{
		if (fibers1[i].size() == fibers2[i].size())
		{
			nbPointsTab[parallel.getID()] += fibers1[i].size();
			for (unsigned j=0; j<fibers1[i].size(); j++)
			{
				Vector3f p = fibers1[i][j];
				for (unsigned k=0; k<3; k++)
					p(k)/=vox[k];
				Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value1 = computeTrilinearInterpolatedValue(faValues, dim, gridPosition);
				Vector3f p2 = fibers2[i][j];
				for (unsigned k=0; k<3; k++)
					p2(k)/=vox[k];
				Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value2=computeTrilinearInterpolatedValue(faValues, dim, gridPosition2);
				float ecart = fabs(value1-value2);
				ecartMaxTab[parallel.getID()] = max(ecart, ecartMaxTab[parallel.getID()]);
				ecartMoyenTab[parallel.getID()]+=ecart;
			}
		}

		//Per fiber error
		float fiber1Value = 0.f;
		float fiber2Value = 0.f;
		Vector3f p = fibers1[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber1Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers1[i].size()-1; j++)
		{
			Vector3f p1 = fibers1[i][j];
			Vector3f p2 = fibers1[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber1Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		p = fibers2[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber2Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers2[i].size()-1; j++)
		{
			Vector3f p1 = fibers2[i][j];
			Vector3f p2 = fibers2[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber2Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		fiber1Value /= float(fibers1[i].size());
		fiber2Value /= float(fibers2[i].size());
		averageFiber1Error[parallel.getID()] += fiber1Value;
		averageFiber2Error[parallel.getID()] += fiber2Value;
	});
#endif

	float ecartMoyen = ecartMoyenTab[0], ecartMax = ecartMaxTab[0];
	float averageFiber1 = averageFiber1Error[0], averageFiber2 = averageFiber2Error[0];
	uint64_t nbPoints = nbPointsTab[0];
	for (unsigned i=1; i<maxThreads; i++)
	{
		nbPoints += nbPointsTab[i];
		ecartMoyen += ecartMoyenTab[i];
		ecartMax = max(ecartMax, ecartMaxTab[i]);
		averageFiber1 += averageFiber1Error[i];
		averageFiber2 += averageFiber2Error[i];
	}

	ecartMoyen /= nbPoints;
	averageFiber1 /= (float)maxThreads;
	averageFiber2 /= (float)maxThreads;
	cout << "fa per fiber mean error: " << fabs(averageFiber2 - averageFiber1) / averageFiber1 * 100 << "%" << endl;
	cout << "fa max error: " << ecartMax << endl;
	cout << "fa mean error: " << ecartMoyen << endl;
}
}


#ifndef CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHODNM_HPP_
#define CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHODNM_HPP_

#include <cstddef> // size_t

#include <Eigen/Dense>

namespace cppmath
{
    /**
     * Implementation of the original Downhill Simplex or Nelder-Mead method for nonlinear optimization from 1965.
     * J. Nelder, R. Mead, "A Simplex Method for Function Minimization," Computer Journal, 1965, 7, 308-313
     *
     * \author cpieloth
     * \copyright Copyright 2014 Christof Pieloth, Licensed under the Apache License, Version 2.0
     */
    template< size_t DIM >
    class DownhillSimplexMethodNM
    {
    public:
        typedef Eigen::Matrix< double, DIM, 1 > ParamsT; /**< Abbreviation for a vector of parameters. */

        /**
         * Enum to indicate how the optimization was converged.
         */
        enum Converged
        {
            CONVERGED_NO, /**< Optimization was not started. */
            CONVERGED_EPSILON, /**< Optimization is smaller than epsilon/threshold. */
            CONVERGED_ITERATIONS, /**< Maximum iterations was reached. */
            CONVERGED_YES /**< Optimization is converged, but not specified how. */
        };

        DownhillSimplexMethodNM();
        virtual ~DownhillSimplexMethodNM();

        /**
         * Implementation of the function to minimize.
         *
         * \param x  n-dimensional parameter vector.
         * \return function value for vector x.
         */
        virtual double func( const ParamsT& x ) const = 0;

        /**
         * Indicates how the optimization was converged.
         *
         * \return Enum::Converged
         */
        virtual Converged converged() const;

        double getReflectionCoeff() const;

        void setReflectionCoeff( double alpha );

        double getContractionCoeff() const;

        void setContractionCoeff( double beta );

        double getExpansionCoeff() const;

        void setExpansionCoeff( double gamma );

        size_t getMaximumIterations() const;

        void setMaximumIterations( size_t iterations );

        double getEpsilon() const;

        void setEpsilon( double eps );

        double getInitialFactor() const;

        void setInitialFactor( double factor );

        size_t getResultIterations() const;

        ParamsT getResultParams() const;

        double getResultError() const;

        /**
         * Starts the optimization.
         *
         * \param initial Initial start point.
         */
        void optimize( const ParamsT& initial );

    protected:
        /**
         * Orders the parameters x and f(x) from min to max.
         */
        virtual void order();

        /**
         * Creates the initial parameter set and their function values.
         *
         * \param initial Start parameter used to calculate initials.
         */
        virtual void createInitials( const ParamsT& initial );
        double m_initFactor; /**< Factor to create the initial parameter set. */

        ParamsT m_x[DIM + 1]; /**< Vector of all n+1 points. */
        double m_y[DIM + 1]; /**< Stores the function values to reduce re-calculation. */

        double m_epsilon; /**< Threshold or deviation for convergence. */
        size_t m_maxIterations; /**< Maximum iterations until the algorithm is canceled. */
        size_t m_iterations; /**< Iteration counter used for break condition. */

        const size_t DIMENSION; /**< Constant for dimension. */
        const size_t VALUES; /**< Constant for DIM+1. */

    private:
        enum Step
        {
            STEP_START, STEP_EXIT, STEP_REFLECTION, STEP_EXPANSION, STEP_CONTRACTION
        };

        double m_alpha; /**< Reflection coefficient. */
        double m_beta; /**< Contraction coefficient. */
        double m_gamma; /**< Expansion coefficient. */

        void centroid();
        Step reflection();
        Step expansion();
        Step contraction();

        ParamsT m_xo;
        ParamsT m_xr;
        double m_yr;
    };
} /* namespace cppmath */

// Load the implementation
#include "DownhillSimplexMethodNM-impl.hpp"

#endif  // CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHODNM_HPP_


#pragma once
#include <algorithm>
#include <iomanip>
#include <iostream>

#include <armadillo>
#include <fmt/format.h>

#define SIGM 1
#define TAHN 2
#define RELU 3

namespace net{
    template < typename Scalar >
    class Perceptron {
        // friend class Layer;
        private:
            Scalar val;
            Scalar act;
            Scalar der;
            int id;
        public:
            Perceptron( Scalar val );
            Scalar& VAL();
            Scalar& ACT( int FUNC = SIGM );
            Scalar& DER( int FUNC = SIGM );
            int& ID() { return id; }
            void show();
    };

    template < typename Scalar >
    Perceptron< Scalar >::Perceptron( Scalar val ) {
        this->val = val;
        ACT();
        DER();
    }
    template < typename Scalar >
    Scalar& Perceptron< Scalar >::VAL() {
        return this->val;
    }
    template < typename Scalar >
    Scalar& Perceptron< Scalar >::ACT( int FUNC ) {
        switch( FUNC ) {
            case SIGM: this->act = 1.0 / ( 1.0 + exp( -this->val ) ); break;               // sigmoid activation
            case TAHN: this->act = std::tanh( this->val ); break;                          // tahn activation
            case RELU: this->act = this->val > 0 ? this->val : this->val * 0.01; break;    // LeakyReLU activation
        }
        return this->act;
    }
    template < typename Scalar >
    Scalar& Perceptron< Scalar >::DER( int FUNC ) {
        switch( FUNC ) {
            case SIGM: this->der = this->act * ( 1.0 - this->act ); break;             // sigmoid derivation
            case TAHN: this->der = 1.0 - std::exp( std::tanh( this->val ) ); break;    // tahn derivation
            case RELU: this->der = this->val >= 0 ? 1.0 : 0.01; break;                 // LeakyReLU derivation
        }
        return this->der;
    }
    template < typename Scalar >
    void Perceptron< Scalar >::show() {
        std::string s = fmt::format( "\t{0} \t {1} \t {2}\n", this->VAL(), this->ACT(), this->DER() );
        fmt::print( s );    // Python-like format string syntax
    }
}


#include <Eigen/Dense>
#include <iostream>

int main() {
    Eigen::MatrixXd m(4, 4);

    // Output a matrix that counts from 0 to 15
    for (size_t i = 0; i < 4; ++i) {
        for (size_t j = 0; j < 4; ++j) {
            m(i, j) = i * 4 + j;
        }
    }

    std::cout << m;

    return 0;
}


#include <iostream>
#include <cmath>

#include <taylor.hpp>

#include <boost/numeric/odeint.hpp>

template <typename T> class pendulum {

private:
  /// Length of the pendulum
  double length_;
  /// Gravitational constant (little g)
  double g_;

public:
  pendulum(double l, double g = 9.81) : length_(l), g_(g) {}

  void operator()(const T &theta, T &dtheta_dt, const double /* t */) {
    dtheta_dt[0][0] = theta[0][1];
    dtheta_dt[0][1] = -(g_ / length_) * theta[0][0];
  }
};

class pendulum_exact {
private:
  /// Length of the pendulum
  double length_;
  /// Initial position (angle)
  double theta0_;
  /// Gravitational constant (little g)
  double g_;

public:
  pendulum_exact(double l, double theta0, double g = 9.81) : length_(l), theta0_(theta0), g_(g) {}
  double position(double t) {
    return theta0_ * std::cos(std::sqrt(g_/length_) * t);
  }
  double speed(double t) {
    return (-theta0_ * std::sqrt(g_/length_) * std::sin(std::sqrt(g_/length_) * t));
  }
};

int main() {
  using namespace std;
  using namespace boost::numeric::odeint;

  typedef std::vector< taylor<double, 1, 1> > state_type;

  //[ state_initialization
  state_type x(2);
  x[0][0] = 1.0; // start at x=1.0, p=0.0
  x[0][1] = 0.0; // start at x=1.0, p=0.0
  x[1][0] = 0.0;
  x[1][1] = 0.0;
  //]

  double length = 10.0;
  pendulum<state_type> approx(length);
  pendulum_exact exact(length, 1.0);

  runge_kutta4< state_type> stepper;

  const double dt = 0.1;
  const double tstart = 0.0;
  const double tend = 1.0;
  for( double t = tstart ; t < tend ; t += dt )
  {
    std::cout << t << "   " << std::setprecision(16) << exact.position(t) << "   " << std::setprecision(16) << exact.speed(t) << std::endl;
    std::cout << t << "   " << std::setprecision(16) << x[0]  << "   " << std::setprecision(16) << x[1] << std::endl;
    stepper.do_step(approx, x, t, dt);
  }

  return 0;
}


#include <CGAL/Random.h>
#include <CGAL/Cartesian_matrix.h>
#include <CGAL/sorted_matrix_search.h>
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/functional.hpp>

typedef int                                     Value;
typedef std::vector<Value>                      Vector;
typedef Vector::iterator                        Value_iterator;
typedef std::vector<Vector>                     Vector_cont;
typedef CGAL::Cartesian_matrix<std::plus<int>,
                               Value_iterator,
                               Value_iterator>  Matrix;

int main()
{
  // set of vectors the matrices are build from:
  Vector_cont vectors;

  // generate a random vector and sort it:
  Vector a;
  const int n = 5;
  for (int i = 0; i < n; ++i)
    a.push_back(CGAL::get_default_random()(100));
  std::sort(a.begin(), a.end());
  std::cout << "a = ( ";
  std::copy(a.begin(), a.end(), std::ostream_iterator<int>(std::cout," "));
  std::cout << ")\n";

  // build a Cartesian matrix from a:
  Matrix M(a.begin(), a.end(), a.begin(), a.end());

  // search for an upper bound for max(a):
  Value bound = a[n-1];
  Value upper_bound =
  CGAL::sorted_matrix_search(
    &M, &M + 1,
    CGAL::sorted_matrix_search_traits_adaptor(
      [&bound](const auto& m){ return std::greater_equal<Value>()(m, bound); }, M));
  std::cout << "Upper bound for " << bound << " is "
            << upper_bound << "." << std::endl;

  return 0;
}


/*
 * utilities_cumtrapz_test.cpp Test fixtures for the cumtrapz function
 *
 * Author:                   Tom Clark (thclark @ github)
 *
 * Copyright (c) 2016-9 Octue Ltd. All Rights Reserved.
 *
 */

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "utilities/integration.h"

using namespace utilities;

template<typename T_scalar>
class PowFunctor {
public:
    PowFunctor() {};
    T_scalar operator() (T_scalar eta) const{
        return pow(eta, 2.0);
    };
};

class IntegrationTest : public ::testing::Test {};

TEST_F(IntegrationTest, test_colwise_cumulative_integrate_1_row) {

    // Test that zeros are returned when trying to colwise integrate an array with 1 row
    Eigen::ArrayXd integrand(1);
    integrand << 1;
    Eigen::ArrayXd integral_correct(1);
    integral_correct << 0;
    PowFunctor<double> functor;
    Eigen::ArrayXd integral = cumulative_integrate(integrand, functor);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(IntegrationTest, test_colwise_cumulative_integrate) {
    Eigen::ArrayXd integrand(3);
    Eigen::ArrayXd integral_correct(3);
    integrand << 0, 2, 4;
    integral_correct << 0, 8.0/3.0, 64.0/3.0;
    PowFunctor<double> pow_functor;
    Eigen::ArrayXd integral = cumulative_integrate(integrand, pow_functor);
    ASSERT_TRUE(integral.isApprox(integral_correct));
}