//Link to Boost
 #define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
#include <boost/test/unit_test.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <iostream>
#include "test.h"
#include "../RandomAlgebricEquations.h"
#include "../EquationSolver.h"
using namespace std;

BOOST_FIXTURE_TEST_SUITE(RandomAlgebricEquations_suite, SimpleTestFixture, * utf::label("RandomAlgebricEquations"))

BOOST_DATA_TEST_CASE(Constructor_test, bdata::random(5, 10) ^ bdata::random(0, 2) ^ bdata::random(2, 5) ^ bdata::xrange(TCNumber), functionNumber, orderDiff, tryOrder, index)
{
    int maxOrder = orderDiff + tryOrder;
    RandomAlgebricEquations equations(functionNumber, maxOrder, tryOrder);
    BOOST_TEST(equations.ParameterNumber(), functionNumber * (tryOrder + 1));
    BOOST_TEST(equations.SolutionNumber(), functionNumber * (maxOrder + 1));
    BOOST_TEST(equations.EquationNumber(), functionNumber);
//    MY_FLOAT_EQUAL(equations.EvaluateConstraints(), .0, tol);

    vector<float_type> inputs = equations.GenerateInputs(3);
    for (int i = 0; i < inputs.size(); i++) {
        for (int j = 0; j < equations.EquationNumber(); j++) {
            float_type res = equations.VerifySolution(inputs[i], j);
            BOOST_TEST_INFO("input=" << inputs[i] << ", equationId=" << j);
            MY_FLOAT_EQUAL(res, 0.0, tol);
        }
    }
}

BOOST_DATA_TEST_CASE(SaveLoad_test, bdata::random(5, 10) ^ bdata::random(0, 2) ^ bdata::random(2, 5) ^ bdata::xrange(TCNumber), functionNumber, orderDiff, tryOrder, index)
{
    int maxOrder = orderDiff + tryOrder;
    RandomAlgebricEquations eqs1(functionNumber, maxOrder, tryOrder);
    string file = "SaveLoad_test" + ToString(index) + ".txt";
    eqs1.SaveToFile(file);
    RandomAlgebricEquations eqs2;
    eqs2.LoadFromFile(file);

    BOOST_TEST(eqs2.ParameterNumber(), eqs1.ParameterNumber());
    BOOST_TEST(eqs2.SolutionNumber(), eqs1.SolutionNumber());
    BOOST_TEST(eqs2.EquationNumber(), eqs1.EquationNumber());

    // compare parameters and solutions.
    for (int i = 0; i < eqs2.ParameterNumber(); i++) {
        BOOST_TEST_INFO("i=" << i);
        MY_FLOAT_EQUAL(eqs2.GetParameter(i), eqs1.GetParameter(i), tol);
    }

    for (int i = 0; i < eqs2.SolutionNumber(); i++) {
        BOOST_TEST_INFO("i=" << i);
        MY_FLOAT_EQUAL(eqs2.GetSolution(i), eqs1.GetSolution(i), tol);
    }

    // compare coefficients and constTerms.
    for (int i = 0; i < eqs2.EquationNumber(); i++) {
        for (int j = 0; j < functionNumber; j++) {
            for (int k = 0; k < functionNumber; k++) {
                MY_FLOAT_EQUAL(eqs2.GetCoefficient(i, j, k), eqs1.GetCoefficient(i, j, k), tol);
            }
        }

        for (int j = 0; j <= maxOrder; j++) {
            MY_FLOAT_EQUAL(eqs2.GetConstTerm(i, j), eqs1.GetConstTerm(i, j), tol);
        }
    }
}

BOOST_DATA_TEST_CASE(GetSetParameter_test, bdata::random(5, 10) ^ bdata::random(0, 2) ^ bdata::random(2, 5) ^ bdata::xrange(TCNumber), functionNumber, orderDiff, tryOrder, index)
{
    int maxOrder = orderDiff + tryOrder;
    RandomAlgebricEquations equations(functionNumber, maxOrder, tryOrder);
    int parameterId = randomint(0, equations.ParameterNumber() - 1);
    float_type expected = random(-10.0, 10.0);
    equations.SetParameter(parameterId, expected);
    float_type actual = equations.GetParameter(parameterId);

    MY_FLOAT_EQUAL(actual, expected, tol);
}

BOOST_DATA_TEST_CASE(EquationDerivativeByParameter_test, bdata::random(5, 10) ^ bdata::random(0, 2) ^ bdata::random(2, 5) ^ bdata::xrange(TCNumber), functionNumber, orderDiff, tryOrder, index)
{
    int maxOrder = orderDiff + tryOrder;
    RandomAlgebricEquations equations(functionNumber, maxOrder, tryOrder);
    int parameterId = randomint(0, equations.ParameterNumber() - 1);
    int equationId = randomint(0, equations.EquationNumber() - 1);
    float_type input = random(-1.0, 1.0);

    float_type actual = equations.EquationDerivativeByParameter(input, equationId, parameterId);

    float_type param = equations.GetParameter(parameterId);
    equations.SetParameter(parameterId, param - inc);
    float_type value1 = equations.EvaluateEquation(input, equationId);

    equations.SetParameter(parameterId, param + inc);
    float_type value2 = equations.EvaluateEquation(input, equationId);

    float_type expected = (value2 - value1) / (2 * inc);
    BOOST_TEST_INFO("input=" << input << ", equationId=" << equationId << ", parameterId=" << parameterId);
    MY_FLOAT_EQUAL(actual, expected, tol);
}

BOOST_DATA_TEST_CASE(ConstraintsDerivativeByParameter_test, bdata::random(5, 10) ^ bdata::random(0, 2) ^ bdata::random(2, 5) ^ bdata::xrange(TCNumber), functionNumber, orderDiff, tryOrder, index)
{
    int maxOrder = orderDiff + tryOrder;
    RandomAlgebricEquations equations(functionNumber, maxOrder, tryOrder);
    int parameterId = randomint(0, tryOrder);
    float_type param = random(-.1, .0);
    equations.SetParameter(parameterId, param);

    float_type actual = equations.EvaluateConstraints();
//    MY_FLOAT_EQUAL(actual, -param, tol);

    actual = equations.ConstraintsDerivativeByParameter(functionNumber + 1);
    MY_FLOAT_EQUAL(actual, .0, tol);

    actual = equations.ConstraintsDerivativeByParameter(parameterId);
    equations.SetParameter(parameterId, param - inc);
    float_type value1 = equations.EvaluateConstraints();

    equations.SetParameter(parameterId, param + inc);
    float_type value2 = equations.EvaluateConstraints();

    float_type expected = (value2 - value1) / (2 * inc);
    BOOST_TEST_INFO("tryOrder=" << tryOrder << ", parameterId=" << parameterId << ", value=" << param);
    MY_FLOAT_EQUAL(actual, expected, tol);
}

// test suite end
BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(EquationSolver_suite, SimpleTestFixture, * utf::disabled())

BOOST_AUTO_TEST_CASE(EquationSolver_test1, * utf::label("EquationSolver1"))
{
    int functionNumber = randomint(3, 4);
    int tryOrder = randomint(2, 3);
    int maxOrder = tryOrder;
    RandomAlgebricEquations equations(functionNumber, maxOrder, tryOrder);
    GradientDescentConfig config;
    config.StepsToPrintResult = 1000;
    config.InitialStepSize = 0.1;
    config.LoopNumber = 5;
    config.InitialTry = 3;
    config.SamplesEachStep = 25;
    config.RequiredAccuracy = 1.0;
    config.FilePrefix = "unittest1";
    EquationSolver<float_type> solver(&equations, &config);
    vector<float_type> inputs;
    float_type cost = solver.Run(inputs);
    equations.OutputSolutions();

    if (cost < config.RequiredAccuracy) {
        float_type diff = .0;
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            diff += (equations.GetParameter(i) - equations.GetSolution(i)) * (equations.GetParameter(i) - equations.GetSolution(i));
        }

        BOOST_TEST(diff < 1.0);
    } else {
        cout << "Warning: only local minimum is returned.";
        equations.SaveToFile("EquationSolver_test.txt");

        vector<vector<float_type> > hessian;
        solver.CalculateHessian(inputs, hessian);

        std::cout << "save hessian to file." << endl;
        ofstream out("EquationSolver_test_hessian.txt");
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            out << "{";
            for (int j = 0; j < equations.ParameterNumber(); j++) {
                if (j > 0) out << ", ";
                out << hessian[i][j];
            }
            out << "}," << endl;
        }

        out.close();
    }
}

BOOST_AUTO_TEST_CASE(EquationSolver_test2, * utf::label("EquationSolver2"))
{
    int functionNumber = randomint(3, 4);
    int tryOrder = randomint(2, 3);
    int maxOrder = randomint(1, 2) + tryOrder;
    RandomAlgebricEquations equations(functionNumber, maxOrder, tryOrder);
    GradientDescentConfig config;
    config.StepsToPrintResult = 1000;
    config.InitialStepSize = 0.1;
    config.LoopNumber = 6;
    config.InitialTry = 3;
    config.SamplesEachStep = 25;
    config.RequiredAccuracy = 1.0;
    config.FilePrefix = "unittest2";
    EquationSolver<float_type> solver(&equations, &config);
    vector<float_type> inputs;
    float_type cost = solver.Run(inputs);
    equations.OutputSolutions();

    if (cost < config.RequiredAccuracy) {
        float_type diff = .0;
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            diff += (equations.GetParameter(i) - equations.GetSolution(i)) * (equations.GetParameter(i) - equations.GetSolution(i));
        }

        BOOST_TEST(diff < 1.0);
    } else {
        cout << "Warning: only local minimum is returned.";
        equations.SaveToFile("EquationSolver_test.txt");

        vector<vector<float_type> > hessian;
        solver.CalculateHessian(inputs, hessian);

        std::cout << "save hessian to file." << endl;
        ofstream out("EquationSolver_test_hessian.txt");
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            out << "{";
            for (int j = 0; j < equations.ParameterNumber(); j++) {
                if (j > 0) out << ", ";
                out << hessian[i][j];
            }
            out << "}," << endl;
        }

        out.close();
    }
}

BOOST_AUTO_TEST_CASE(EquationSolver_test3, * utf::label("EquationSolver3"))
{
    RandomAlgebricEquations equations;
    equations.LoadFromFile("RandomAlgebricEquations_unittest3_input.txt");
    GradientDescentConfig config;
    config.StepsToPrintResult = 1000;
    config.InitialStepSize = 0.1;
    config.LoopNumber = 6;
    config.InitialTry = 3;
    config.SamplesEachStep = 20;
    config.RequiredAccuracy = 1.0;
    config.FilePrefix = "unittest3";
    EquationSolver<float_type> solver(&equations, &config);
    vector<float_type> inputs;
    float_type cost = solver.Run(inputs);
    equations.OutputSolutions();

    if (cost < config.RequiredAccuracy) {
        float_type diff = .0;
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            diff += (equations.GetParameter(i) - equations.GetSolution(i)) * (equations.GetParameter(i) - equations.GetSolution(i));
        }

        BOOST_TEST(diff < 1.0);
    } else {
        cout << "Warning: only local minimum is returned.";
        equations.SaveToFile("EquationSolver_test2.txt");

        vector<vector<float_type> > hessian;
        solver.CalculateHessian(inputs, hessian);

        std::cout << "save hessian to file." << endl;
        ofstream out("EquationSolver_test2_hessian.txt");
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            out << "{";
            for (int j = 0; j < equations.ParameterNumber(); j++) {
                if (j > 0) out << ", ";
                out << hessian[i][j];
            }
            out << "}," << endl;
        }

        out.close();
    }
}

BOOST_AUTO_TEST_CASE(EquationSolver_test4, * utf::label("EquationSolver4"))
{
    RandomAlgebricEquations equations;
    equations.LoadFromFile("RandomAlgebricEquations_unittest4_input.txt");
    GradientDescentConfig config;
    config.StepsToPrintResult = 1000;
    config.InitialStepSize = 0.1;
    config.LoopNumber = 5;
    config.InitialTry = 3;
    config.SamplesEachStep = 20;
    config.RequiredAccuracy = 1.0;
    config.FilePrefix = "unittest4";
    EquationSolver<float_type> solver(&equations, &config);
    vector<float_type> inputs;
    float_type cost = solver.Run(inputs);
    equations.OutputSolutions();

    if (cost < config.RequiredAccuracy) {
        float_type diff = .0;
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            diff += (equations.GetParameter(i) - equations.GetSolution(i)) * (equations.GetParameter(i) - equations.GetSolution(i));
        }

        BOOST_TEST(diff < 1.0);
    } else {
        cout << "Warning: only local minimum is returned.";
        equations.SaveToFile("EquationSolver_test2.txt");

        vector<vector<float_type> > hessian;
        solver.CalculateHessian(inputs, hessian);

        std::cout << "save hessian to file." << endl;
        ofstream out("EquationSolver_test2_hessian.txt");
        for (int i = 0; i < equations.ParameterNumber(); i++) {
            out << "{";
            for (int j = 0; j < equations.ParameterNumber(); j++) {
                if (j > 0) out << ", ";
                out << hessian[i][j];
            }
            out << "}," << endl;
        }

        out.close();
    }
}

// test suite end
BOOST_AUTO_TEST_SUITE_END()


//
//  su2_x.hpp
//
//  Created by Evan Owen on 4/3/21.
//  Copyright © 2021 Evan Owen. All rights reserved.
//

#ifndef su2_x_hpp
#define su2_x_hpp

#include <random>
#include <complex>
#include <Eigen/Dense>

class su2_x_lattice;

typedef Eigen::Matrix<std::complex<double>, 2, 2> su2_link;
typedef Eigen::Matrix<std::complex<double>, 1, 2> su2_vector;
#define su2_identity su2_link::Identity()
#define su2_zero su2_link::Zero()

#define I complex<double>(0,1)
#define Q(s) s->link[0]
#define D_MAX 6
#define SQRT3   1.73205080756887729352744634151  // sqrt(3)
#define SQRT1_3 0.577350269189625764509148780502 // 1 / sqrt(3)

class su2_x_site {
public:
    // variables
    su2_x_lattice* lattice; // parent lattice
    su2_link link[D_MAX]; // link values in each direction
    su2_link link_inverse[D_MAX]; // link inverse values
    su2_link p_link[D_MAX]; // conjugate momenta in each direction
    su2_x_site* forward[D_MAX]; // adjacent sites in the forward direction
    su2_x_site* backward[D_MAX]; // adjacent sites in the backward direction
    std::mt19937* gen; // random number generator for this time-slice
    bool forward_edge; // site is at the forward edge of the extra dimension
    bool backward_edge; // site is at the backward edge of the extra dimension
    bool is_locked;
    double eps;
    su2_link wf_z[D_MAX]; // exponent for wilson flow

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // methods
    void init(su2_x_lattice* lattice, su2_x_site* lattice_sites, int s);
    su2_link make_unitary(const su2_link& g);
    su2_link exp_su2(const su2_link& g);
    bool lock();
    void unlock();
    double rand_double(double min = 0.0, double max = 1.0);
    int rand_int(int min, int max);
    double rand_normal(double mean = 0.0, double stdev = 1.0);
    void reset_links(bool cold);
    void copy_links(su2_x_site* site);
    void read_links(std::ifstream& ckptFile, bool bigEndian);
    void set_link(int d, const su2_link& value);
    void init_momenta();
    su2_link create_random_link();
    su2_link create_link(double a);
    double action();
    double hamiltonian();
    void hmc_step_p(double frac);
    void hmc_step_link();
    su2_link p_link_dot(int d);
    double link_trace();
    double plaq();
    su2_link plaquette(int d1, int d2);
    su2_link staple(int d1);
    su2_link reverse_staple(int d1);
    su2_link staple_x(int d1);
    su2_link reverse_staple_x(int d1);
    su2_link cloverleaf(int d1, int d2);
    double wilson_loop(int a, int b);
    std::complex<double> polyakov_loop(int r);
    double correlator(int T);
    double field_strength();
//    double field_strength_x();
    double topological_charge();
    double mag_U();
    double abs_U();
    double four_point(int T, int R);
    su2_link overrelax(su2_link g);
    void relax(bool coulomb);
    double sum_landau();
    double sum_coulomb();
    su2_link sum_G(bool coulomb);
    void heat_bath();
    void heat_bath_link(int d1);
    void cool();
    void cool_link(int d1);
    void wilson_flow(su2_x_site* target, int step);
    void wilson_flow_link(su2_x_site* target, int step, int d);
    void stout_smear(su2_x_site* target, double rho);
    void stout_smear_link(su2_x_site* target, double rho, int d1);
};

class su2_x_lattice {
public:
    // variables
    int N; // array size (spacial)
    int T; // array size (time)
    int N5; // 5th dimension size
    int D; // number of dimensions
    double beta; // coupling factor
    double eps5; // ratio of coupling in extra dimension to normal coupling
    int n_sites; // number of sites in the entire lattice
    int n_sites_5; // number of sites in a 5d sub-lattice
    int n_slice; // number of sites in each time slice of the entire lattice
    int n5_center; // center lattice
    std::vector<su2_x_site> site; // site values
    std::vector<std::mt19937> gen; // array of random number generators (one for each time-slice)
    std::vector<su2_link> sigma; // pauli spin matrices
    int verbose;
    bool parallel;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // hmc
    std::vector<su2_x_site> site_1; // site values for hmc
    double dt; // hmc step size
    int n_steps; // hmc step count
    int hmc_accept; // number of accepted hmc configurations
    int hmc_count; // number of attempted hmc configurations

    // time step for wilson flow algorithm
    double wf_dt;

    // methods
    su2_x_lattice(int N, int T, int N5, int D, double beta, double eps5, bool cold = false);
    su2_x_lattice(su2_x_lattice* lattice);
    su2_x_lattice(int N, int T, int N5, int D, double beta, double eps5, std::ifstream& ckptFile, bool isNersc = false);
    ~su2_x_lattice();
    void init();
    double rand_double(double min = 0.0, double max = 1.0);
    int rand_int(int min, int max);
    double rand_normal(double mean = 0.0, double stdev = 1.0);
    int get_site(int s, int d, int n);
    double link_trace();
    double plaq(int n5);
    double action(int n5);
    double wilson_loop(int a, int b, int n5);
    double polyakov_loop(int R, int n5);
    double correlator(int T, int n5);
    double four_point(int T, int R, int n5);
    double hamiltonian();
    void hmc(int n_sweeps = 0, bool update_dt = false, bool no_metropolis = false);
    void heat_bath(int n_sweeps = 0);
    void cool(int n5, int n_sweeps = 0);
    void wilson_flow_x(int n_sweeps = 0);
    void wilson_flow(int n5, int n_sweeps = 0);
    void stout_smear(int n5, double rho = 0.1, int n_sweeps = 0);
    double field_strength(int n5);
    double field_strength_x(int n5);
    double topological_charge(int n5);
    int thermalize(int n_min = 0, int n_max = 0);
    double ave_link_t(int n5);
    double theta(bool coulomb, int n5);
    long double relax(long double error_target, bool coulomb, int n5);
};

#endif /* su2_x_hpp */


#include "CMeshModelInstance.h"
#include <Eigen/Dense>
#include <igl/per_vertex_normals.h>
#include <assert.h>
#include <totalmodel.h>
#include <ceres/rotation.h>
#include <chrono>

// Function equivalent and improved from igl::per_vertex_normals
// [270, 452] ms
template <typename T>
inline T getNormTriplet(const T* const ptr)
{
    return std::sqrt(ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2]);
}
template <typename T>
inline void normalizeTriplet(T* ptr, const T norm)
{
    ptr[0] /= norm;
    ptr[1] /= norm;
    ptr[2] /= norm;
}
template <typename T>
inline void normalizeTriplet(T* ptr)
{
    const auto norm = getNormTriplet(ptr);
    normalizeTriplet(ptr, norm);
}
void per_vertex_normals(
  const Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>& V,
  const Eigen::Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor>& F,
  Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>& N
)
{
    Eigen::Matrix<double, Eigen::Dynamic,3, Eigen::RowMajor> FN;
    FN.resize(F.rows(),3);
    auto* FN_data = FN.data();
    const auto* const F_data = F.data();
    const auto* const V_data = V.data();
    // loop over faces
    for (int i = 0; i < F.rows();i++)
    {
        const auto baseIndex = 3*i;
        const auto F_data0 = 3*F_data[baseIndex];
        const auto F_data1 = 3*F_data[baseIndex+1];
        const auto F_data2 = 3*F_data[baseIndex+2];
        const Eigen::Matrix<double, 1, 3> v1(
            V_data[F_data1] - V_data[F_data0],
            V_data[F_data1+1] - V_data[F_data0+1],
            V_data[F_data1+2] - V_data[F_data0+2]);
        const Eigen::Matrix<double, 1, 3> v2(
            V_data[F_data2] - V_data[F_data0],
            V_data[F_data2+1] - V_data[F_data0+1],
            V_data[F_data2+2] - V_data[F_data0+2]);
        FN.row(i) = v1.cross(v2);
        auto* fnRowPtr = &FN_data[baseIndex];
        const double norm = getNormTriplet(fnRowPtr);
        if (norm == 0)
        {
            fnRowPtr[0] = 0;
            fnRowPtr[1] = 0;
            fnRowPtr[2] = 0;
        }
        else
            normalizeTriplet(fnRowPtr, norm);
    }

    // Resize for output
    N.resize(V.rows(),3);
    std::fill(N.data(), N.data() + N.rows() * N.cols(), 0.0);

    Eigen::Matrix<double, Eigen::Dynamic, 1> A(F.rows(), 1);
    auto* A_data = A.data();
    const auto Fcols = F.cols();
    const auto Vcols = V.cols();

  // Projected area helper
  const auto & proj_doublearea =
    [&V_data,&F_data, &Vcols, &Fcols](const int x, const int y, const int f)
    ->double
  {
    const auto baseIndex = f*Fcols;
    const auto baseIndex2 = F_data[baseIndex + 2]*Vcols;
    const auto rx = V_data[F_data[baseIndex]*Vcols + x] - V_data[baseIndex2 + x];
    const auto sx = V_data[F_data[baseIndex + 1]*Vcols + x] - V_data[baseIndex2 + x];
    const auto ry = V_data[F_data[baseIndex]*Vcols + y] - V_data[baseIndex2 + y];
    const auto sy = V_data[F_data[baseIndex + 1]*Vcols + y] - V_data[baseIndex2 + y];
    return rx*sy - ry*sx;
  };

  for (auto f = 0;f<F.rows();f++)
  {
    const auto dblAd1 = proj_doublearea(0,1,f);
    const auto dblAd2 = proj_doublearea(1,2,f);
    const auto dblAd3 = proj_doublearea(2,0,f);
    A_data[f] = std::sqrt(dblAd1*dblAd1 + dblAd2*dblAd2 + dblAd3*dblAd3);
  }

    auto* N_data = N.data();
    // loop over faces
    for (int i = 0 ; i < F.rows();i++)
    {
        const auto baseIndex = i*Fcols;
        // throw normal at each corner
        for (int j = 0; j < 3;j++)
        {
            // auto* nRowPtr = &N_data[3*F(i,j)];
            auto* nRowPtr = &N_data[3*F_data[baseIndex + j]];
            const auto* const fnRowPtr = &FN_data[3*i];
            for (int subIndex = 0; subIndex < FN.cols(); subIndex++)
                nRowPtr[subIndex] += A_data[i] * fnRowPtr[subIndex];
            // Vector equilvanet
            // N.row(F(i,j)) += A_data[i] * FN.row(i);
        }
    }

    // take average via normalization
    // loop over faces
    for (int i = 0;i<N.rows();i++)
        normalizeTriplet(&N_data[3*i]);
    // Matrix equivalent
    // N.rowwise().normalize();
}

void CMeshModelInstance::RecomputeNormal(const TotalModel& model)
{
    // Compute Normal
    Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> V_3(m_vertices.size(), 3);
    auto* V_3_data = V_3.data();

    for (int r = 0; r < V_3.rows(); ++r)
    {
        auto* v3rowPtr = &V_3_data[3*r];
        v3rowPtr[0] = m_vertices[r].x; // V_3(r, 0)
        v3rowPtr[1] = m_vertices[r].y; // V_3(r, 1)
        v3rowPtr[2] = m_vertices[r].z; // V_3(r, 2)
    }
    // Eigen::MatrixXd NV;
    Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> NV;

    if (m_meshType==MESH_TYPE_SMPL)
    {
        std::string errorMessage("Not supporting MESH_TYPE_SMPL currently");
        throw std::runtime_error(errorMessage);
        // igl::per_vertex_normals(V_3, g_smpl.faces_, NV);
    }
    if (m_meshType == MESH_TYPE_TOTAL || m_meshType == MESH_TYPE_ADAM)
    {
        // igl::per_vertex_normals(V_3, model.m_faces, NV);
        per_vertex_normals(V_3, model.m_faces, NV);
        // Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> NVAux;
        // igl::per_vertex_normals(V_3, model.m_faces, NVAux);
        // std::cout << (NV - NVAux).norm() << std::endl;
        // assert((NV - NVAux).norm() < 1e-6);
    }
    m_normals.resize(NV.rows());
    auto* NV_data = NV.data();
    for (int r = 0; r < NV.rows(); ++r)
    {
        const auto* const nvRow = &NV_data[3*r];
        m_normals[r] = cv::Point3f(nvRow[0], nvRow[1], nvRow[2]); // cv::Point3f(NV(r, 0), NV(r, 1), NV(r, 2))
    }
}

void CMeshModelInstance::clearMesh()
{
    m_face_vertexIndices.clear();
    m_vertices.clear();
    m_colors.clear();
    m_normals.clear();
    m_uvs.clear();
    m_joints.clear();
    m_joints_regress.clear();
    m_alpha.clear();
}

bool compareTupleDepth(const std::tuple<double, double, cv::Point3i>& a, const std::tuple<double, double, cv::Point3i>& b)
{
    return std::get<1>(a) > std::get<1>(b);   // from far to near
}

bool compareTupleAlpha(const std::tuple<double, double, cv::Point3i>& a, const std::tuple<double, double, cv::Point3i>& b)
{
    return std::get<0>(a) > std::get<0>(b);   // opaque first, then transparent
}

void CMeshModelInstance::sortFaceDepth(const cv::Point3d angleaxis)
{
    // const auto start = std::chrono::high_resolution_clock::now();
    std::vector<double> depth_vertex(m_vertices.size());
    if (angleaxis == cv::Point3d(0., 0., 0.))   // no rotation
    {
        for (auto i = 0u; i < m_vertices.size(); i++)
            depth_vertex[i] = m_vertices[i].z;
    }
    else
    {
        const double angle_axis[3] = {angleaxis.x, angleaxis.y, angleaxis.z};
        for (auto i = 0u; i < m_vertices.size(); i++)
        {
            const double pt[3] = {m_vertices[i].x, m_vertices[i].y, m_vertices[i].z};
            double result[3];
            ceres::AngleAxisRotatePoint(angle_axis, pt, result);
            depth_vertex[i] = result[2];
        }
    }

    assert(m_face_vertexIndices.size() % 3 == 0);
    std::vector<std::tuple<double, double, cv::Point3i>> vecSort;
    const uint num_face = m_face_vertexIndices.size() / 3;
    vecSort.reserve(num_face);
    for (auto i = 0u; i < num_face; i++)
    {
        const uint I1 = m_face_vertexIndices[3 * i];
        const uint I2 = m_face_vertexIndices[3 * i + 1];
        const uint I3 = m_face_vertexIndices[3 * i + 2];
        const double depth = (depth_vertex[I1] + depth_vertex[I2] + depth_vertex[I3]) / 3;
        const double alpha = (m_alpha[I1] + m_alpha[I2] + m_alpha[I3]) / 3;  // opaqueness
        vecSort.emplace_back(std::make_tuple(alpha, depth, cv::Point3i(I1, I2, I3)));
    }
    std::sort(vecSort.begin(), vecSort.end(), compareTupleAlpha);  // first sort according to opacity, put opaque face first
    auto it = vecSort.begin();
    while(it != vecSort.end())
    {
        // find the first element that is not completely opaque
        if (std::get<0>(*it) != 1.0)
            break;
        it++;
    }
    std::sort(it, vecSort.end(), compareTupleDepth);  // now sort according to depth, far first
    for (auto i = 0u; i < num_face; i++)
    {
        auto& pt = std::get<2>(vecSort[i]);
        m_face_vertexIndices[3 * i + 0] = pt.x;
        m_face_vertexIndices[3 * i + 1] = pt.y;
        m_face_vertexIndices[3 * i + 2] = pt.z;
    }
    // const auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::high_resolution_clock::now() - start).count();
    // std::cout << "Sort depth duration " << duration * 1e-6 << " ms" << std::endl;
}


#ifndef SKYLARK_WZT_HPP
#define SKYLARK_WZT_HPP

#ifndef SKYLARK_SKETCH_HPP
#error "Include top-level sketch.hpp instead of including individuals headers"
#endif

#include <boost/random.hpp>

namespace skylark { namespace sketch {

/**
 * Woodruff-Zhang Transform (data)
 *
 * Woodruff-Zhang Transform is very similar to the Clarkson-Woodruff Transform:
 * it replaces the +1/-1 diagonal with reciprocal exponentia random enteries.
 * It is sutiable for lp regression with 1 <= p <= 2.
 *
 * Reference:
 * D. Woodruff and Q. Zhang
 * Subspace Embeddings and L_p Regression Using Exponential Random
 * COLT 2013
 *
 * TODO current implementation is only one sketch index, when for 1 <= p <= 2
 *      you want more than one.
 */

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct WZT_t :
        public WZT_data_t,
        virtual public sketch_transform_t<InputMatrixType, OutputMatrixType > {

public:

    // We use composition to defer calls to hash_transform_t
    typedef hash_transform_t< InputMatrixType, OutputMatrixType,
                              boost::random::uniform_int_distribution,
                              boost::random::exponential_distribution > transform_t;

    typedef WZT_data_t data_type;
    typedef data_type::params_t params_t;

    WZT_t(int N, int S, double p, base::context_t& context)
        : data_type(N, S, p, context), _transform(*this) {

    }

    WZT_t(int N, int S, const params_t& params, base::context_t& context)
        : data_type(N, S, params, context),
          _transform(*this) {

    }

    WZT_t(const boost::property_tree::ptree &pt)
        : data_type(pt), _transform(*this) {

    }

    template< typename OtherInputMatrixType,
              typename OtherOutputMatrixType >
    WZT_t(const WZT_t<OtherInputMatrixType,OtherOutputMatrixType>& other)
        : data_type(other), _transform(*this) {

    }

    WZT_t(const data_type& other)
        : data_type(other), _transform(*this) {

    }

    /**
     * Apply columnwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const typename transform_t::matrix_type& A,
                typename transform_t::output_matrix_type& sketch_of_A,
                columnwise_tag dimension) const {
        _transform.apply(A, sketch_of_A, dimension);
    }

    /**
     * Apply rowwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const typename transform_t::matrix_type& A,
                typename transform_t::output_matrix_type& sketch_of_A,
                rowwise_tag dimension) const {
        _transform.apply(A, sketch_of_A, dimension);
    }

    int get_N() const { return this->_N; } /**< Get input dimesion. */
    int get_S() const { return this->_S; } /**< Get output dimesion. */

    const sketch_transform_data_t* get_data() const { return this; }

private:
    transform_t _transform;
};

} } /** namespace skylark::sketch */

#endif // SKYLARK_WZT_HPP


#include <iostream>

#define BOOST_TEST_MODULE MIAUnaryTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include "DenseMIA.h"
#include "SparseMIA.h"
#include "LibMIAHelpers.h"
#include "Index.h"

//typedef LibMIA::DenseMIA<double,3> dmia;

template<class data_type>
void sparse_unary_work(){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
    LibMIA::MIAINDEX m;
    LibMIA::MIAINDEX n;
    size_t _dim=5;

    LibMIA::SparseMIA<data_type,6> a(_dim,_dim+1,_dim,_dim+1,_dim-1,_dim-2);

    LibMIA::SparseMIA<data_type,4> b;
    LibMIA::SparseMIA<data_type,4> other_b;
    LibMIA::SparseMIA<data_type,3> c;
    LibMIA::SparseMIA<data_type,3> other_c;
    LibMIA::SparseMIA<data_type,2> d;
    LibMIA::SparseMIA<data_type,2> other_d;
    LibMIA::SparseMIA<data_type,1> e;
    LibMIA::SparseMIA<data_type,1> other_e;
    LibMIA::SparseMIA<data_type,5> f;
    LibMIA::SparseMIA<data_type,5> other_f;
    auto delta=LibMIA::create_delta<data_type,2>(_dim);
    auto delta2=LibMIA::create_delta<data_type,2>(_dim+1);
    auto delta_3=LibMIA::create_delta<data_type,3>(_dim);

    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();

    b(j,k,l,m)=a(i,j,i,k,l,m); //contraction
    other_b(j,k,l,m)=a(i,j,n,k,l,m)*delta(i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Simple Contraction 1 for Sparse ")+typeid(data_type).name());

    c=LibMIA::SparseMIA<data_type,3>(_dim,_dim+1,_dim);
    c.resize(c.dimensionality()/3);
    c.randu(-5,5);
    c.rand_indices();
    c.collect_duplicates();
    e(i)=c(j,i,j);
    other_e(i)=c(j,i,k)*delta(j,k);
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Simple Contraction 2 for Sparse ")+typeid(data_type).name());

//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+2,_dim-2);
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    c(j,l,m)=a(i,j,i,i,l,m); //contraction
    other_c(j,l,m)=a(i,j,n,k,l,m)*delta_3(i,n,k); //equivalent operation
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(other_c,test_precision<data_type>()),std::string("Triple Contraction 1 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    d(l,m)=a(i,j,i,j,l,m); //contraction
    other_d(l,m)=a(i,j,n,k,l,m)*delta(i,n)*delta2(j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Complex Contraction 1 for Sparse ")+typeid(data_type).name());

    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    e(m)=a(i,j,i,j,i,m); //contraction
    other_e(m)=a(i,j,n,k,l,m)*delta_3(i,n,l)*delta2(j,k); //equivalent operation
//    e.print();
//    other_e.print();
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Complex Contraction 2 for Sparse ")+typeid(data_type).name());

    d=LibMIA::SparseMIA<data_type,2>(_dim,_dim);
    d.resize(a.dimensionality()/3);
    d.randu(-5,5);
    d.rand_indices();
    d.collect_duplicates();
    e(i)=d(!i,!i);
    other_e(i)=d(!i,j)*delta(!i,j);
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Very Simple Attraction 1 for Sparse ")+typeid(data_type).name());
//
    c=LibMIA::SparseMIA<data_type,3>(_dim,_dim+1,_dim); //now try with non-uniform dimensions
    c.resize(a.dimensionality()/3);
    c.randu(-5,5);
    c.rand_indices();
    c.collect_duplicates();
    d(i,j)=c(!i,j,!i); //attraction
    other_d(i,j)=c(!i,j,n)*delta(!i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Simple Attraction 1 for Sparse ")+typeid(data_type).name());
//
//
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+2,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    f(i,j,k,l,m)=a(!i,j,!i,k,l,m); //attraction
    other_f(i,j,k,l,m)=a(!i,j,n,k,l,m)*delta(!i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(f.fuzzy_equals(other_f,test_precision<data_type>()),std::string("Simple Attraction 2 for Sparse ")+typeid(data_type).name());

//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+2,_dim-2);
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    b(i,j,l,m)=a(!i,j,!i,!i,l,m); //attraction
    other_b(i,j,l,m)=a(!i,j,n,k,l,m)*delta_3(!i,n,k); //equivalent operation
    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Triple Attraction 1 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    b(i,j,l,m)=a(!i,!j,!i,!j,l,m); //attraction
    other_b(i,j,l,m)=a(!i,!j,n,k,l,m)*delta(!i,n)*delta2(!j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Complex Attraction 1 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    c(i,j,m)=a(!i,!j,!i,!j,!i,m); //attraction
    other_c(i,j,m)=a(!i,!j,n,k,l,m)*delta_3(!i,n,l)*delta2(!j,k); //equivalent operation
    //c.print();
    //other_c.print();
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(other_c,test_precision<data_type>()),std::string("Complex Attraction 2 for Sparse ")+typeid(data_type).name());
//
    b=LibMIA::SparseMIA<data_type,4>(_dim,_dim+1,_dim,_dim+1); //now try with non-uniform dimensions
    b.resize(a.dimensionality()/3);
    b.randu(-5,5);
    b.rand_indices();
    b.collect_duplicates();
    d(i,j)=b(!i,!j,!i,!j); //attraction
    other_d(i,j)=b(!i,!j,n,k)*delta(!i,n)*delta2(!j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Complex Attraction 3 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    c(i,k,m)=a(!i,j,!i,j,k,m); //attraction
    other_c(i,k,m)=a(n,l,!i,j,k,m)*delta(!i,n)*delta2(j,l); //attraction
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(other_c,test_precision<data_type>()),std::string("Combined Contraction/Attraction 1 for Sparse ")+typeid(data_type).name());

    b=LibMIA::SparseMIA<data_type,4>(_dim,_dim+1,_dim,_dim+1); //now try with non-uniform dimensions
    b.resize(a.dimensionality()/3);
    b.randu(-5,5);
    b.rand_indices();
    b.collect_duplicates();
    e(i)=b(!i,j,!i,j); //attraction
    other_e(i)=b(n,l,!i,j)*delta(!i,n)*delta2(j,l); //attraction
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Combined Contraction/Attraction 2 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+1,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    d(i,m)=a(!i,j,!i,!i,j,m); //attraction
    other_d(i,m)=a(!i,j,n,k,l,m)*delta_3(!i,n,k)*delta2(j,l); //attraction
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Combined Contraction/Attraction 3 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+1,_dim-2); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    d(j,m)=a(i,!j,i,i,!j,m); //attraction
    other_d(j,m)=a(i,!j,n,k,l,m)*delta_3(i,n,k)*delta2(!j,l); //attraction
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Combined Contraction/Attraction 4 for Sparse ")+typeid(data_type).name());
//
    a=LibMIA::SparseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim); //now try with non-uniform dimensions
    a.resize(a.dimensionality()/3);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    d(i,k)=a(!i,j,!i,j,!k,!k); //attraction
    other_d(i,k)=a(n,l,!i,j,!k,m)*delta(!i,n)*delta2(j,l)*delta(!k,m); //attraction
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Combined Contraction/Attraction 5 for Sparse ")+typeid(data_type).name());

//

}

template<class data_type>
void dense_unary_work(){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
    LibMIA::MIAINDEX m;
    LibMIA::MIAINDEX n;
    size_t _dim=5;

    LibMIA::DenseMIA<data_type,6> a(_dim,_dim,_dim,_dim,_dim,_dim);

    LibMIA::DenseMIA<data_type,4> b;
    LibMIA::DenseMIA<data_type,4> other_b;
    LibMIA::DenseMIA<data_type,3> c;
    LibMIA::DenseMIA<data_type,3> other_c;
    LibMIA::DenseMIA<data_type,2> d;
    LibMIA::DenseMIA<data_type,2> other_d;
    LibMIA::DenseMIA<data_type,1> e;
    LibMIA::DenseMIA<data_type,1> other_e;
    LibMIA::DenseMIA<data_type,5> f;
    LibMIA::DenseMIA<data_type,5> other_f;
    auto delta=LibMIA::create_delta<data_type,2>(_dim);
    auto delta2=LibMIA::create_delta<data_type,2>(_dim+1);
    auto delta_3=LibMIA::create_delta<data_type,3>(_dim);
    a.randu(-5,5);

    const LibMIA::DenseMIA<data_type,6> temp_a(a); //check const_correctness
    b(j,k,l,m)=temp_a(i,j,i,k,l,m); //contraction
    other_b(j,k,l,m)=temp_a(i,j,n,k,l,m)*delta(i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Simple Contraction 1 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+2,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    b(j,k,l,m)=a(i,j,i,k,l,m); //contraction
    other_b(j,k,l,m)=a(i,j,n,k,l,m)*delta(i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Simple Contraction 2 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+2,_dim-2);
    a.randu(-5,5);
    c(j,l,m)=a(i,j,i,i,l,m); //contraction
    other_c(j,l,m)=a(i,j,n,k,l,m)*delta_3(i,n,k); //equivalent operation
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(other_c,test_precision<data_type>()),std::string("Triple Contraction 1 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    d(l,m)=a(i,j,i,j,l,m); //contraction
    other_d(l,m)=a(i,j,n,k,l,m)*delta(i,n)*delta2(j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Complex Contraction 1 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    e(m)=a(i,j,i,j,i,m); //contraction
    other_e(m)=a(i,j,n,k,l,m)*delta_3(i,n,l)*delta2(j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Complex Contraction 2 for Dense ")+typeid(data_type).name());

    d=LibMIA::DenseMIA<data_type,2>(_dim,_dim);
    d.randu(-5,5);
    e(i)=d(!i,!i);
    other_e(i)=d(!i,j)*delta(!i,j);
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Very Simple Attraction 1 for Dense ")+typeid(data_type).name());

    c=LibMIA::DenseMIA<data_type,3>(_dim,_dim+1,_dim); //now try with non-uniform dimensions
    c.randu(-5,5);
    d(i,j)=c(!i,j,!i); //attraction
    other_d(i,j)=c(!i,j,n)*delta(!i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Simple Attraction 1 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+2,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    f(i,j,k,l,m)=a(!i,j,!i,k,l,m); //attraction
    other_f(i,j,k,l,m)=a(!i,j,n,k,l,m)*delta(!i,n); //equivalent operation
    BOOST_CHECK_MESSAGE(f.fuzzy_equals(other_f,test_precision<data_type>()),std::string("Simple Attraction 2 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+2,_dim-2);
    a.randu(-5,5);
    b(i,j,l,m)=a(!i,j,!i,!i,l,m); //attraction
    other_b(i,j,l,m)=a(!i,j,n,k,l,m)*delta_3(!i,n,k); //equivalent operation
    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Triple Attraction 1 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim-1,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    b(i,j,l,m)=a(!i,!j,!i,!j,l,m); //attraction
    other_b(i,j,l,m)=a(!i,!j,n,k,l,m)*delta(!i,n)*delta2(!j,k); //equivalent operation

    BOOST_CHECK_MESSAGE(b.fuzzy_equals(other_b,test_precision<data_type>()),std::string("Complex Attraction 1 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    c(i,j,m)=a(!i,!j,!i,!j,!i,m); //attraction
    other_c(i,j,m)=a(!i,!j,n,k,l,m)*delta_3(!i,n,l)*delta2(!j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(other_c,test_precision<data_type>()),std::string("Complex Attraction 2 for Dense ")+typeid(data_type).name());

    b=LibMIA::DenseMIA<data_type,4>(_dim,_dim+1,_dim,_dim+1); //now try with non-uniform dimensions
    b.randu(-5,5);
    d(i,j)=b(!i,!j,!i,!j); //attraction
    other_d(i,j)=b(!i,!j,n,k)*delta(!i,n)*delta2(!j,k); //equivalent operation
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Complex Attraction 3 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    c(i,k,m)=a(!i,j,!i,j,k,m); //attraction
    other_c(i,k,m)=a(n,l,!i,j,k,m)*delta(!i,n)*delta2(j,l); //attraction
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(other_c,test_precision<data_type>()),std::string("Combined Contraction/Attraction 1 for Dense ")+typeid(data_type).name());

    b=LibMIA::DenseMIA<data_type,4>(_dim,_dim+1,_dim,_dim+1); //now try with non-uniform dimensions
    b.randu(-5,5);
    e(i)=b(!i,j,!i,j); //attraction
    other_e(i)=b(n,l,!i,j)*delta(!i,n)*delta2(j,l); //attraction
    BOOST_CHECK_MESSAGE(e.fuzzy_equals(other_e,test_precision<data_type>()),std::string("Combined Contraction/Attraction 2 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+1,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    d(i,m)=a(!i,j,!i,!i,j,m); //attraction
    other_d(i,m)=a(!i,j,n,k,l,m)*delta_3(!i,n,k)*delta2(j,l); //attraction
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Combined Contraction/Attraction 3 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim,_dim+1,_dim-2); //now try with non-uniform dimensions
    a.randu(-5,5);
    d(j,m)=a(i,!j,i,i,!j,m); //attraction
    other_d(j,m)=a(i,!j,n,k,l,m)*delta_3(i,n,k)*delta2(!j,l); //attraction
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Combined Contraction/Attraction 4 for Dense ")+typeid(data_type).name());

    a=LibMIA::DenseMIA<data_type,6>(_dim,_dim+1,_dim,_dim+1,_dim,_dim); //now try with non-uniform dimensions
    a.randu(-5,5);
    d(i,k)=a(!i,j,!i,j,!k,!k); //attraction
    other_d(i,k)=a(n,l,!i,j,!k,m)*delta(!i,n)*delta2(j,l)*delta(!k,m); //attraction
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(other_d,test_precision<data_type>()),std::string("Combined Contraction/Attraction 5 for Dense ")+typeid(data_type).name());
//    d.print();
//    other_d.print();

//

}

BOOST_AUTO_TEST_CASE( MIAUnaryTests )
{

    dense_unary_work<double>();
    dense_unary_work<float>();
    dense_unary_work<int>();
    dense_unary_work<long>();

    sparse_unary_work<double>();
    sparse_unary_work<float>();
    sparse_unary_work<int>();
    sparse_unary_work<long>();

}


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

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/isnormal.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

// Determines if the given argument is normal, i.e. is neither zero, subnormal, infinite nor NaN.
template <typename T>
void test()
{
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(!boost::math::ccmath::isnormal(std::numeric_limits<T>::quiet_NaN()), "Wrong response to quiet NAN");
    }

    static_assert(!boost::math::ccmath::isnormal(T(0)), "Wrong response to 0");

    if constexpr (!std::is_integral_v<T>)
    {
        static_assert(!boost::math::ccmath::isnormal((std::numeric_limits<T>::min)() / 2), "Wrong response to subnormal");
        static_assert(!boost::math::ccmath::isnormal(std::numeric_limits<T>::infinity()), "Wrong response to infinity");
    }

    static_assert(boost::math::ccmath::isnormal(T(1)), "Wrong response to normal number");
}

#ifndef BOOST_MATH_NO_CONSTEXPR_DETECTION
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    test<boost::multiprecision::float128>();
    #endif

    test<int>();
    test<unsigned>();
    test<long>();
    test<std::int32_t>();
    test<std::int64_t>();
    test<std::uint32_t>();

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


#include <Rcpp.h>
#include <RcppEigen.h>
#include <boost/math/special_functions/binomial.hpp>

//[[Rcpp::export(.partialSumEigen)]]
Eigen::VectorXd partialSumEigen(const Eigen::VectorXd & x)
{
    const std::size_t N = x.size();
    Eigen::VectorXd partialSum(N + 1);

    partialSum[0] = 0;
    for (std::size_t i = 1; i < N + 1; i++)
    {
        partialSum[i] = x[i - 1] + partialSum[i - 1];
    }

    return partialSum;
}

Eigen::MatrixXd bindColumns(const Eigen::MatrixXd & A, const Eigen::MatrixXd & B)
{
    Eigen::MatrixXd C(A.rows(), A.cols() + B.cols());
    C << A, B;
    return C;
}

Eigen::VectorXd STDEigen(const std::vector<double> &x)
{
    Eigen::VectorXd xEigen(x.size());
    for (std::size_t i = 0; i < x.size(); i++)
        xEigen[i] = x[i];

    return xEigen;
}

std::vector<double> EigenSTD(const Eigen::VectorXd &x)
{
    std::vector<double> xNEWMAT(x.size());
    for (std::size_t i = 0; i < x.size(); i++)
        xNEWMAT[i] = x[i];

    return xNEWMAT;
}

Eigen::Vector2i nonZeroElementsOfMarker(const Eigen::VectorXd & decodedProfile_m)
{
    const std::size_t N = decodedProfile_m.size();

    Eigen::Vector2i res;
    std::size_t i = 0;
    std::size_t j = 0;
    while(j < 2)
    {
        if (decodedProfile_m[i] != 0)
        {
            int k = static_cast<int>(decodedProfile_m[i]);
            while(k > 0)
            {
                res[j] = i;
                k -= 1;
                j += 1;
            }
        }
        i += 1;
    }

    return res;
}

std::vector<int> sortedIndex(const Eigen::VectorXd & x)
{
    std::vector<int> x_sorted(x.size());
    std::iota(x_sorted.begin(), x_sorted.end(), 0);
    auto comparator = [&x](int i, int j){ return x[i] > x[j]; };

    std::sort(x_sorted.begin(), x_sorted.end(), comparator);

    return x_sorted;
}

//[[Rcpp::export()]]
Eigen::MatrixXd generatePossibleGenotypes(const std::size_t & N)
{
    if (N == 1)
    {
        Eigen::MatrixXd possibleSingleprofilesMatrix = 2*Eigen::MatrixXd::Ones(N, N);
        return possibleSingleprofilesMatrix;
    }

    const std::size_t possibleSingles = N;
    const std::size_t possiblePairs = boost::math::binomial_coefficient<double>(N, 2);

    const std::size_t totalPossibleOutcomes = possibleSingles + possiblePairs;
    const std::size_t totalRuns = std::floor(possiblePairs / N);

    Eigen::MatrixXd possibleSingleprofilesMatrix = Eigen::MatrixXd::Zero(N, totalPossibleOutcomes);
    for (std::size_t i = 0; i < N; i++)
    {
        // Single Values
        Eigen::VectorXd singleVector = Eigen::VectorXd::Zero(N);
        singleVector(i) = 2;
        possibleSingleprofilesMatrix.col(i) = singleVector;

        // Paired Values
        for (std::size_t j = 1; j <= totalRuns; j++)
        {
            Eigen::VectorXd pairsVector = Eigen::VectorXd::Zero(N);
            pairsVector(i) = 1;
            pairsVector((i + j) % N) = 1;
            possibleSingleprofilesMatrix.col(j*N + i) = pairsVector;
        }
    }

    // Remaining Paired Values
    for (std::size_t k = (totalRuns + 1)*N; k < totalPossibleOutcomes; k++)
    {
        Eigen::VectorXd pairsVector = Eigen::VectorXd::Zero(N);
        pairsVector(k % N) = 1;
        pairsVector(((k % N) + (totalRuns + 1)) % N) = 1;
        possibleSingleprofilesMatrix.col(k) = pairsVector;
    }

    return possibleSingleprofilesMatrix;
}


/** @file */
#pragma once
#include <boost/math/tools/minima.hpp>
#include <cmath>
#include <string_view>

namespace pw85 {

namespace metadata {
constexpr std::string_view author{"S. Brisard"};
// clang-format off
constexpr std::string_view description{"Implementation of the \"contact function\" defined by Perram and Wertheim (J. Comp. Phys. 58(3), 409-416, DOI:10.1016/0021-9991(85)90171-8) for two ellipsoids."};
// clang-format on
constexpr std::string_view author_email{"sebastien.brisard@univ-eiffel.fr"};
constexpr std::string_view license{"BSD 3-Clause License"};
constexpr std::string_view name{"pw85"};
constexpr std::string_view url{"https://github.com/sbrisard/pw85"};
constexpr std::string_view version{"2.0"};
constexpr std::string_view year{"2021"};
}  // namespace metadata

/** The dimension of the physical space (3).*/
constexpr size_t dim = 3;

/** The dimension of the space of symmetric matrices (6). */
constexpr size_t sym = 6;

/*
 * For the Brent algorithm, these two constants should be such that
 *
 * [(3 - sqrt(5)) / 2] ** n < eps
 *
 * where
 *
 *     n = max_iter
 *     eps = lambda_atol
 */

/**
 * The absolute tolerance for the stopping criterion of Brent’s method (in
 * function contact_function()).
 */
constexpr double lambda_atol = 1e-6;

/**
 * The maximum number of iterations of Brent’s method (in function
 * contact_function()).
 */
constexpr size_t max_iter = 25;

/**
 * The total number of iterations of the Newton–Raphson refinement phase (in
 * function contact_function()).
 */
constexpr size_t nr_iter = 3;

/**
 * Compute the Cholesky decomposition of a symmetric, positive matrix.
 *
 * Let `A` be a symmetric, positive matrix, defined by the `double[6]` array
 * `a`. This function computes the lower-triangular matrix `L`, defined by
 * the `double[6]` array `l`, such that `Lᵀ⋅L = A`.
 *
 * The array `l` must be pre-allocated; it is modified by this function. Note
 * that storage of the coefficients of `L` is as follows
 *
 * ```
 *     ⎡ l[0]    0    0 ⎤
 * L = ⎢ l[1] l[3]    0 ⎥.
 *     ⎣ l[2] l[4] l[5] ⎦
 * ```
 */
void _cholesky_decomp(double const *a, double *l) {
  l[0] = sqrt(a[0]);
  l[1] = a[1] / l[0];
  l[2] = a[2] / l[0];
  l[3] = sqrt(a[3] - l[1] * l[1]);
  l[4] = (a[4] - l[1] * l[2]) / l[3];
  l[5] = sqrt(a[5] - l[2] * l[2] - l[4] * l[4]);
}

/**
 * Compute the solution to a previously Cholesky decomposed linear system.
 *
 * Let `L` be a lower-triangular matrix, defined by the `double[6]` array `l`
 * (see pw85::_cholesky_decomp() for ordering of the coefficients). This
 * function solves (by substitution) the linear system `Lᵀ⋅L⋅x = b`, where the
 * vectors `x` and `b` are specified through their `double[3]` array of
 * coordinates; `x` is modified by this function.
 */
void _cholesky_solve(const double *l, const double *b, double *x) {
  /* Solve L.y = b */
  double const y0 = b[0] / l[0];
  double const y1 = (b[1] - l[1] * y0) / l[3];
  double const y2 = (b[2] - l[2] * y0 - l[4] * y1) / l[5];

  /* Solve L^T.x = y */
  x[2] = y2 / l[5];
  x[1] = (y1 - l[4] * x[2]) / l[3];
  x[0] = (y0 - l[1] * x[1] - l[2] * x[2]) / l[0];
}

/**
 * Compute the quadratic form associated to a spheroid.
 *
 * The spheroid is defined by its equatorial radius `a`, its polar radius `c`
 * and the direction of its axis of revolution, `n` (unit-vector, `double[3]`
 * array).
 *
 * `q` is the representation of a symmetric matrix as a `double[6]` array. It is
 * modified in-place.
 */
void spheroid(double a, double c, const double *n, double *q) {
  double const a2 = a * a;
  double const c2_minus_a2 = c * c - a2;
  double const nx = n[0];
  double const ny = n[1];
  double const nz = n[2];
  q[0] = nx * nx * c2_minus_a2 + a2;
  q[3] = ny * ny * c2_minus_a2 + a2;
  q[5] = nz * nz * c2_minus_a2 + a2;
  q[4] = ny * nz * c2_minus_a2;
  q[2] = nx * nz * c2_minus_a2;
  q[1] = nx * ny * c2_minus_a2;
}

/**
 * Return the value of the opposite of the function ``f`` defined as (see
 * @verbatim embed:rst:inline :ref:`theory`@endverbatim).
 *
 * @f[f(\lambda)=\lambda\bigl(1-\lambda\bigr)r_{12}^{\mathsf{T}}
 *               \cdot Q^{-1}\cdot r_{12},@f]
 *
 * with
 *
 * @f[Q = \bigl(1-\lambda\bigr)Q_1 + \lambda Q_2,@f]
 *
 * where ellipsoids 1 and 2 are defined as the sets of points @f$m@f$
 * (column-vector) such that
 *
 * @f[\bigl(m-c_i\bigr)\cdot Q_i^{-1}\cdot\bigl(m-c_i\bigr)\leq1.@f]
 *
 * In the above inequality, @f$c_i@f$ is the center; @f$r_{12}=c_2-c_1@f$ is the
 * center-to-center radius-vector, represented by the `double[3]` array `r12`.
 * The symmetric, positive-definite matrices @f$Q_1@f$ and @f$Q_2@f$ are
 * specified through the `double[6]` arrays `q1` and `q2`.
 *
 * The value of @f$\lambda@f$ is specified through the parameter ``lambda``.
 *
 * This function returns the value of @f$−f(\lambda)@f$ (the “minus” sign comes
 * from the fact that we seek the maximum of @f$f@f$, or the minimum of
 * @f$−f@f$).
 *
 * This implementation uses
 * @verbatim embed:rst:inline:ref:`Cholesky decompositions
 * <implementation-cholesky>`@endverbatim.
 */
double f_neg(double lambda, const double *r12, const double *q1,
             const double *q2) {
  double const *q1_i = q1;
  double const *q2_i = q2;
  double q[sym];
  double *q_i = q;
  double q12[sym];
  double *q12_i = q12;
  for (size_t i = 0; i < sym; i++, q1_i++, q2_i++, q_i++, q12_i++) {
    *q_i = (1 - lambda) * (*q1_i) + lambda * (*q2_i);
    *q12_i = (*q2_i) - (*q1_i);
  }
  double l[sym];
  _cholesky_decomp(q, l);
  double s[dim];
  _cholesky_solve(l, r12, s);
  double const *r_i = r12;
  double *s_i = s;
  double rs = 0.;
  for (size_t i = 0; i < dim; i++, r_i++, s_i++) {
    rs += (*r_i) * (*s_i);
  }
  return -lambda * (1. - lambda) * rs;
}

/**
 * Compute the residual \f$g(\lambda)=\mu_2^2-\mu_1^2\f$.
 *
 * See @verbatim embed:rst:inline :ref:`optimization` @endverbatim for the
 * definition of \f$g\f$. The value of \f$\lambda\f$ is specified through the
 * parameter `lambda`. See contact_function() for the definition of the
 * parameters `r12`, `q1` and `q2`.
 *
 * The preallocated `double[3]` array `out` is updated as follows:
 * `out[0]=`\f$f(\lambda)\f$, `out[1]=`\f$g(\lambda)\f$ and
 * `out[2]=`\f$g'(\lambda)\f$.
 *
 * This function is used in function
 * @verbatim embed:rst:inline :cpp:func:`pw85::contact_function` @endverbatim
 * for the final Newton–Raphson refinement step.
 */
void _residual(double lambda, const double *r12, const double *q1,
               const double *q2, double *out) {
  double q[sym];
  double q12[sym];
  for (size_t i = 0; i < sym; i++) {
    q[i] = (1. - lambda) * q1[i] + lambda * q2[i];
    q12[i] = q2[i] - q1[i];
  }
  double l[sym];
  _cholesky_decomp(q, l);
  double s[dim];
  _cholesky_solve(l, r12, s);
  double u[] = {q12[0] * s[0] + q12[1] * s[1] + q12[2] * s[2],
                q12[1] * s[0] + q12[3] * s[1] + q12[4] * s[2],
                q12[2] * s[0] + q12[4] * s[1] + q12[5] * s[2]};
  double v[dim];
  _cholesky_solve(l, u, v);
  double rs = r12[0] * s[0] + r12[1] * s[1] + r12[2] * s[2];
  double su = s[0] * u[0] + s[1] * u[1] + s[2] * u[2];
  double uv = u[0] * v[0] + u[1] * v[1] + u[2] * v[2];

  out[0] = lambda * (1. - lambda) * rs;
  out[1] = (2 * lambda - 1.) * rs + lambda * (1. - lambda) * su;
  out[2] =
      2. * rs + 2. * (1. - 2. * lambda) * su - 2. * lambda * (1. - lambda) * uv;
}

/**
 * Compute the value of the contact function of two ellipsoids.
 *
 * The center-to-center radius-vector @f$r_{12}@f$ is specified by the
 * `double[3]` array `r12`. The symmetric, positive-definite matrices @f$Q_1@f$
 * and @f$Q_2@f$ that define the two ellipsoids are specified through the
 * `double[6]` arrays `q1` and `q2`.
 *
 * This function computes the value of @f$\mu^2@f$, defined as
 *
 * @f[\mu^2=\max_{0\leq\lambda\leq 1}\bigl\{\lambda\bigl(1-\lambda\bigr)
 * r_{12}^{\mathsf{T}}\cdot\bigl[\bigl(1-\lambda\bigr)Q_1+\lambda Q_2\bigr]^{-1}
 * \cdot r_{12}\bigr\},@f]
 *
 * and the maximizer @f$\lambda@f$, see
 * @verbatim embed:rst:inline:ref:`theory` @endverbatim. Both values are stored
 * in the preallocated `double[2]` array `out`: `out[0] = `@f$\mu^2@f$ and
 * `out[1] = `@f$\lambda@f$.
 *
 * @f$\mu@f$ is the common factor by which the two ellipsoids must be scaled
 * (their centers being fixed) in order to be tangentially in contact.
 *
 * This function returns `0`.
 *
 * @verbatim embed:rst:leading-asterisk
 * .. todo:: This function should return an error code.
 * @endverbatim
 */
int contact_function(const double *r12, const double *q1, const double *q2,
                     double *out) {
  auto f = [r12, q1, q2](double lambda) { return f_neg(lambda, r12, q1, q2); };

  auto r = boost::math::tools::brent_find_minima(
      f, 0., 1., std::numeric_limits<double>::digits / 2);
  auto lambda_brent = r.first;
  auto mu2_brent = -r.second;

  double out_res[3];
  _residual(lambda_brent, r12, q1, q2, out_res);
  double res_brent = fabs(out_res[1]);

  /* Try to refine the estimate. */
  double lambda_nr = lambda_brent;
  double mu2_nr = mu2_brent;
  double res_nr = res_brent;

  for (size_t i = 0; i < nr_iter; i++) {
    double lambda_trial = lambda_nr - out_res[1] / out_res[2];
    if ((lambda_trial < 0.) || (lambda_trial > 1.)) {
      break;
    }
    _residual(lambda_trial, r12, q1, q2, out_res);
    lambda_nr = lambda_trial;
    mu2_nr = out_res[0];
    res_nr = fabs(out_res[1]);
  }

  if (res_nr < res_brent) {
    out[0] = mu2_nr;
    out[1] = lambda_nr;
  } else {
    out[0] = mu2_brent;
    out[1] = lambda_brent;
  }

  /* TODO: return error code. */
  return 0;
}
}  // namespace pw85


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <unordered_map>
#include <map>
#include <deque>
#include <time.h>

#include <boost/random/linear_congruential.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/topological_sort.hpp>

#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/king_ordering.hpp>
//#include <boost/graph/minimum_degree_ordering.hpp>
#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/properties.hpp>

#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/plod_generator.hpp>

#include "options.h"
#include "utils.h"

using namespace std;
using namespace boost;

class custom_bfs_visitor : public default_bfs_visitor {
public:
	template <typename Vertex, typename Graph>
	void discover_vertex(Vertex v, const Graph& g) const {
		cout << v << endl;
	}
};

class custom_dfs_visitor : public default_dfs_visitor {
public:
	template <typename Vertex, typename Graph>
	void discover_vertex(Vertex v, const Graph& g) const {
		cout << v << endl;
	}
};

template <class Graph>
unsigned int get_edges(char* edges_file, Graph& g) {
	unsigned int u, v, total = 0;
	string line;
	if (edges_file) {
		ifstream ifs(edges_file);
		if(ifs) {
			while (getline(ifs, line)) {
				SSCANF((line.c_str(), "%u %u", &u, &v));
				add_edge(u, v, g);
				++total;
			}
			ifs.close();
		}
	} else {
		while (getline(cin, line)) {
			SSCANF((line.c_str(), "%u %u", &u, &v));
			add_edge(u, v, g);
			++total;
		}
	}
	return total;
}

int graph_gen(char* generator, char* parameters) {
	unsigned int total_vertices = 256, total_edges = 0;
	boost::minstd_rand gen;
	gen.seed((unsigned int)time(NULL));

	if (!generator || !(strcmp(generator, "rmat") && strcmp(generator, "RMAT"))) {
		typedef rmat_iterator<boost::minstd_rand, adjacency_list<>> rmat_gen;
		unsigned int scale_v = 8, scale_e = 8;
		if (parameters) SSCANF((parameters, "%u:%u", &scale_v, &scale_e));
		total_vertices = 1 << scale_v;
		total_edges    = total_vertices * scale_e;
		auto it       = rmat_gen(gen, total_vertices, total_edges, 0.57, 0.19, 0.19, 0.05);
		auto it_last  = rmat_gen();
		for (;it != it_last; ++it) cout << it->first << " " << it->second << endl;
	} else if (!(strcmp(generator, "er") && strcmp(generator, "ER"))) {
		typedef erdos_renyi_iterator<boost::minstd_rand, adjacency_list<>> er_gen;
		double probability = 0.05;
		if (parameters) SSCANF((parameters, "%u:%lf", &total_vertices, &probability));
		auto it       = er_gen(gen, total_vertices, probability);
		auto it_last  = er_gen();
		for (;it != it_last; ++it) cout << it->first << " " << it->second << endl;
	} else if (!(strcmp(generator, "sw") && strcmp(generator, "SW"))) {
		typedef small_world_iterator<boost::minstd_rand, adjacency_list<>> sw_gen;
		unsigned int   knn         = 6;
		double probability = 0.03;
		if (parameters) SSCANF((parameters, "%u:%u:%lf", &total_vertices, &knn, &probability));
		auto it       = sw_gen(gen, total_vertices, knn, probability);
		auto it_last  = sw_gen();
		for (;it != it_last; ++it) cout << it->first << " " << it->second << endl;
	} else if (!(strcmp(generator, "sf") && strcmp(generator, "SF"))) {
		typedef plod_iterator<boost::minstd_rand, adjacency_list<>> sf_gen;
		double alpha = 2.7;
		unsigned int   beta  = 256;
		if (parameters) SSCANF((parameters, "%u:%lf:%u", &total_vertices, &alpha, &beta));
		auto it       = sf_gen(gen, total_vertices, alpha, beta);
		auto it_last  = sf_gen();
		for (;it != it_last; ++it) cout << it->first << " " << it->second << endl;
	} else {
		cout << "Available generators: RMAT, ER, SW, SF." << endl;
		return -1;
	}
	return 0;
}

template <class Graph>
bool has_self_loop(const Graph& g) {
	BGL_FORALL_EDGES_T(e, g, Graph)
		if (source(e, g) == target(e, g)) return true;
	return false;
}

template <class Graph>
bool is_dag(const Graph& g) {
	unsigned int total_vertices = num_vertices(g);
	vector<unsigned int> component(total_vertices);
	unsigned int total_components = strong_components(g, make_iterator_property_map(component.begin(), get(vertex_index, g)));
	return (total_components == total_vertices) && !has_self_loop(g);
}

//template <class Graph>
//bool isSymmetric(const Graph &g) {
//    std::pair<graph_traits<adjacency_list<setS, vecS, directedS> >::edge_descriptor, bool> tmp;
//	BGL_FORALL_EDGES_T(e, g, Graph){
//        tmp = edge(target(e, g), source(e, g), g);
//        if (tmp.second == false) return false;
//    }
//	return true;
//}

template <class Graph>
int dir_graph_op(Graph& g, char* algorithm, unsigned int v_root) {
	if (!(strcmp(algorithm, "bfs") && strcmp(algorithm, "BFS"))) {
		custom_bfs_visitor b_v;
		breadth_first_search(g, vertex(v_root, g), visitor(b_v));
	} else if (!(strcmp(algorithm, "dfs") && strcmp(algorithm, "DFS"))) {
		custom_dfs_visitor d_v;
		depth_first_search(g, root_vertex(vertex(v_root, g)).visitor(d_v));
	} else if (!(strcmp(algorithm, "scc") && strcmp(algorithm, "SCC"))) {
		unsigned int total_vertices = num_vertices(g);
		vector<unsigned int> component(total_vertices);
		unsigned int total_components = strong_components(g, make_iterator_property_map(component.begin(), get(vertex_index, g)));
		unsigned int i = 0;
		for (auto c: component) cout << i++ << " " << c << endl;
	} else if (!(strcmp(algorithm, "ts") && strcmp(algorithm, "TS"))) {
		if (is_dag(g)) {
			deque<unsigned int> topological_order;
			topological_sort(g, front_inserter(topological_order));
			for (auto o: topological_order) cout << o << endl;
		} else cout << "not DAG" << endl;
	//} else if(!(strcmp(algorithm, "mdo") && strcmp(algorithm, "MDO"))) {
	//	if(isSymmetric(g)) {
	//		unsigned int n = num_vertices(g);
	//		vector<int> degree(n, 0);
	//		vector<int> supernode_sizes(n, 1);
	//		vector<int> inv_perm(n, 0);//build_permutation(InversePermutationMap next,PermutationMap prev)(minimum_degree_ordering.hpp)
	//		vector<int> perm(n, 0); //prev[i] and next[i] can be negative,so vector inv_perm and perm shouldn't be unsigned int
	//		minimum_degree_ordering(g,
	//			make_iterator_property_map(&degree[0], get(vertex_index,g), degree[0]),
	//			&inv_perm[0],
	//			&perm[0],
	//			make_iterator_property_map(&supernode_sizes[0], get(vertex_index, g), supernode_sizes[0]), 0, get(vertex_index, g));
	//		for (auto p: inv_perm) cout << p << endl;
	//	} else cout << "The metrix of graph is not SYMMETRIC !" << endl;
	} else {
		cout << "Algorithm [" << algorithm << "] is not available (for directed graph)." << endl;
		return -1;
	}
	return 0;
}

template <class Graph>
int undir_graph_op(Graph& g, char* algorithm, unsigned int v_root) {
	if (!(strcmp(algorithm, "bfs") && strcmp(algorithm, "BFS"))) {
		custom_bfs_visitor b_v;
		breadth_first_search(g, vertex(v_root, g), visitor(b_v));
	} else if (!(strcmp(algorithm, "dfs") && strcmp(algorithm, "DFS"))) {
		custom_dfs_visitor d_v;
		depth_first_search(g, root_vertex(vertex(v_root, g)).visitor(d_v));
	} else if (!(strcmp(algorithm, "cmo") && strcmp(algorithm, "CMO"))) {
		vector<unsigned int> cuthill_mckee_order(num_vertices(g));
		cuthill_mckee_ordering(g, cuthill_mckee_order.rbegin());
		for (auto cmo: cuthill_mckee_order) cout << cmo << endl;
	} else if (!(strcmp(algorithm, "ko") && strcmp(algorithm, "KO"))) {
		vector<unsigned int> king_order(num_vertices(g));
		king_ordering(g, king_order.rbegin());
		for (auto ko: king_order) cout << ko << endl;
	} else if(!(strcmp(algorithm, "so") && strcmp(algorithm, "SO"))) {
		vector<unsigned int> sloan_order(num_vertices(g));
		sloan_ordering(g, sloan_order.begin(), get(vertex_color, g), make_degree_map(g), get(vertex_priority, g));
		for (auto so: sloan_order) cout << so <<endl;
	} else {
		cout << "Algorithm [" << algorithm << "] is not available (for undirected graph)." << endl;
		return -1;
	}
	return 0;
}

template <class Graph>
void print_edges(const Graph& g) {
	BGL_FORALL_EDGES_T(e, g, Graph)
		cout << source(e, g) << " " << target(e, g) << endl;
}

int main(int argc, char* argv[]) {
	using namespace opt;

	const char* usage =
		"bgl-shell [options]\n"
		" -h:\t ask for help\n"
		"\n"
		" generators\n"
		" -g:\t (RMAT) use generator [RMAT|ER|SW|SF]\n"
		" -p:\t set graph generator parameters\n"
		" \t   Generator         Parameters\n"
		" \t   ---------         ----------\n"
		" \t   Recursive-MATrix  8:8\n"
		" \t   Erdos-Renyi       256:0.05\n"
		" \t   Small-World       256:6:0.03\n"
		" \t   Scale-Free        256:2.7:256\n"
		"\n"
		" algorithms\n"
		" default to print adjacency list\n"
		" -e:\t perform [BFS|DFS|SCC|TS|CMO|KO|SO], etc.\n"
		" \t   BFS: breadth-first traversal       (directed|undirected)\n"
		" \t   DFS: depth-first traversal         (directed|undirected)\n"
		" \t   SCC: strongly connected components (directed)\n"
		" \t   TS:  topological sort              (directed acyclic graph)\n"
		" \t   CMO: cuthill mckee ordering        (undirected)\n"
		" \t   KO:  king ordering                 (undirected)\n"
		" \t   SO:  sloan ordering                (undirected)\n"
		" -i:\t (cin) input edge list\n"
		" -u:\t treat input edge list as undirected\n"
		" -r:\t specify root vertex for graph traversal\n"
		"\n";

	if (chkOption(argv, argv + argc, "-h")) {
		cout << usage;
		return 0;
	}

	bool  use_gen    = chkOption(argv, argv + argc, "-g");
	bool  is_undir   = chkOption(argv, argv + argc, "-u");
	char* generator  = getOption(argv, argv + argc, "-g");
	char* gen_param  = getOption(argv, argv + argc, "-p");
	char* edges_file = getOption(argv, argv + argc, "-i");
	char* algorithm  = getOption(argv, argv + argc, "-e");
	unsigned int  v_root     = getInt(argv, argv + argc, "-s", 0);

	typedef adjacency_list<setS, vecS,   directedS, no_property> graph_t;
	typedef property<vertex_color_t, default_color_type, property<vertex_degree_t, int, property<vertex_priority_t, double> > > vertex_p;
	typedef adjacency_list<setS, vecS, undirectedS, vertex_p> graph_u_p_t;

	if (use_gen) {
		graph_gen(generator, gen_param);
	} else if (is_undir) {
		graph_u_p_t g;
		get_edges(edges_file, g);
		if(algorithm) {
			undir_graph_op(g, algorithm, v_root);
		}
	} else {
		graph_t g;
		get_edges(edges_file, g);
		if(algorithm) {
			dir_graph_op(g, algorithm, v_root);
		}
	}
}


#include <stdlib.h>
#include <vector>
#include <stdint.h>
#include <iostream>
//#include <boost/range.hpp>
#include <algorithm>
#include <boost/range/irange.hpp>
//#include <boost/range/algorithm.hpp>
#include <boost/range/adaptors.hpp>
#include <random>
#include <stack>

namespace tmpl {

void printList( const std::vector< int64_t > & list, const char * text = "" )
{
    std::cout << text;

    for( int64_t i = 0; i < list.size(); ++i )
    {
        std::cout << list[i] << " ";
    }

    printf( "\n\n" );
}

void printList( const std::vector< int64_t > & list, const std::vector< int64_t > & pivots, const int64_t to_swap, const char * text = "" )
{
    std::cout << text;

    for( int64_t i = 0; i < list.size(); ++i )
    {
        auto iter = std::find( std::begin(pivots), std::end(pivots), i);
        if( iter != std::end(pivots) )
        {
            std::cout << "[" << list[i] << "],";
        }
        else if( i == to_swap )
        {
            std::cout << "{" << list[i] << "},";
        }
        else
            std::cout << list[i] << ",";
    }

    printf( "\n\n" );
}

void printPivots( const std::vector< int64_t > & arr, const std::vector< int64_t > & pivots )
{
    std::cout << "PivotValues=";

    for( auto & pivot : pivots ) {
        std::cout << arr[pivot] << ",";
    }

    std::cout << "\n\n";
}

template< int Pivots >
inline std::array<int64_t, Pivots> sort_pivots( std::vector<int64_t> & arr, int64_t start, int64_t end, std::array< int64_t, Pivots > && pivots )
{
    std::sort( std::begin( pivots ), std::end( pivots ) );

    std::array< int64_t, Pivots > new_pivots;

    int64_t first_pivot = start;
    for( const auto p : pivots  | boost::adaptors::indexed(0) )
    {
        new_pivots[p.index()] = first_pivot;

        std::swap( arr[p.value()], arr[first_pivot]);

        ++first_pivot;
    }

    auto iter = std::begin( arr ) + start;

    std::sort( iter, iter + pivots.size() );

    return new_pivots;
}

template< int Pivots >
inline std::array< int64_t, Pivots > get_pivot( std::vector< int64_t > & arr, int64_t l, int64_t r )
{
    static std::random_device rd; // obtain a random number from hardware
    static std::mt19937 gen(rd()); // seed the generator

    std::uniform_int_distribution<> distr(l, r); // define the range

    std::array< int64_t, Pivots > pivots;

    for( int64_t i = 0; i < Pivots; ++i )
    {
        while( true ) {
            auto new_pivot = distr(gen);
            auto iter = std::find( std::begin( pivots ), std::end( pivots ), new_pivot );

            if( iter != std::end( pivots ) ) continue;

            pivots[i] = new_pivot;
            break;
        }
    }

    // sorting part
    return sort_pivots< Pivots >( arr, l, r, std::move(pivots) );
}

template< int Pivots >
inline std::array< bool, Pivots+1 > less_or_equal( const std::vector< int64_t > & arr, const std::array<int64_t, Pivots> & pivots, const int64_t value )
{
    std::array< bool, Pivots+1 > output;
    output.fill( true );

    int64_t index = 0;

    for( const auto p : pivots ) {
        const auto res = arr[p] >= value;

        if( res ) break;

        output[index++] = res;
    }

    return output;
}

template< int Pivots >
inline std::array< bool, Pivots+1 > greater( const std::vector< int64_t > & arr, const std::array<int64_t, Pivots> & pivots, const int64_t value )
{
    std::array< bool, Pivots+1 > output;
    output.fill( true );

    int64_t index = pivots.size();

    for( const auto p : pivots | boost::adaptors::reversed ) {
        const auto res = arr[p] < value;

        // since all the pivot values are sorted, thus if value greater
        // then last pivot implies that for the remaining pivots it also greater
        if( res ) break;

        output[index--] = res;
    }

    return output;
}

// TODO: cover with test
template< int Pivots >
inline int64_t identify_new_sector( const std::vector< int64_t > & arr, const std::array<int64_t, Pivots> & pivots, const int64_t value )
{
    const auto lq = less_or_equal<Pivots>( arr, pivots, value );
    const auto gt = greater<Pivots>( arr, pivots, value );

    int64_t index = 0;

    for( const auto l : lq | boost::adaptors::indexed(0) )
    {
        const auto idx = l.index();

        if( l.value() && gt[index] )
        {
            index = idx;
            break;
        }
    }

    return index;
}

template< int Pivots >
inline std::array<int64_t, Pivots> general_partition( std::vector<int64_t> & arr, int64_t l, int64_t r, std::array<int64_t, Pivots> && pivots )
{
    for( const auto i : boost::irange<int64_t>( l + pivots.size()-1, r+1 ) )
    {
        const auto iter = std::find( std::begin( pivots ), std::end( pivots ), i );

        if( iter != std::end( pivots ) )
            continue;

        const auto new_sector = identify_new_sector<Pivots>( arr, pivots, arr[i] );

        // check if value already in right sector
        if( new_sector == pivots.size() )
            continue;

        int64_t new_i = i;
        for( const auto sector : boost::irange<int64_t>( new_sector, pivots.size() ) | boost::adaptors::reversed )
        {
            auto & pivot( pivots[sector] );

            if( new_i > sector+1 )
                std::swap( arr[ pivot + 1 ], arr[new_i] );

            std::swap( arr[ pivot ], arr[ pivot + 1 ] );

            new_i = pivot;
            pivot += 1;
        }
    }

    return std::move( pivots );
}

struct stack_node {
    int64_t l;
    int64_t r;
};

template< int Pivots >
void quicksort( std::vector< int64_t > & arr )
{
    std::stack< stack_node > stck;
    stck.push( stack_node{0, static_cast<int64_t>(arr.size() - 1) } );

    while( !stck.empty() )
    {
        auto sn = stck.top();
        stck.pop();

        int64_t l = sn.l;
        int64_t r = sn.r;

        if( abs(r-l) <= 10 ) {
            std::sort( std::begin( arr )+l, std::begin( arr )+r+1 );
            continue;
        }

        auto pivots = general_partition<Pivots>( arr, l, r, get_pivot< Pivots >( arr, l, r ) );

        for( const auto pivot : pivots )
        {
            stck.push( stack_node{l, pivot - 1} );

            l = pivot + 1;
        }

        stck.push( stack_node{pivots.back()+1, r} );
    }
}

} // namespace tmpl


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE DenseMIAAddSubtractTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "DenseMIA.h"
#include "Index.h"

template<class _data_type>
void do_work(size_t dim1,size_t dim2){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
//    LibMIA::MIAINDEX m;
//    LibMIA::MIAINDEX n;
//    LibMIA::MIAINDEX o;
//    LibMIA::MIAINDEX p;

    LibMIA::DenseMIA<_data_type,4> a(dim1,dim1,dim2,dim2);
    LibMIA::DenseMIA<_data_type,4> b(dim2,dim1,dim2,dim1);
    LibMIA::DenseMIA<_data_type,4> c(dim2,dim1,dim2,dim1);
    LibMIA::DenseMIA<_data_type,4> c2(dim2,dim1,dim2,dim1);
    LibMIA::DenseMIA<_data_type,4> b2(dim1,dim1,dim2,dim2);

    a.ones();
    b.ones();
    const LibMIA::DenseMIA<_data_type,4> temp_a(a);
    //c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);

    c(i,j,k,l)=b(i,j,k,l)+temp_a(j,l,i,k);
    c2.fill(2);
    BOOST_CHECK_MESSAGE(c==c2,std::string("Non-destructive Add 1 for ")+typeid(_data_type).name());

    b(i,j,k,l)+=temp_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==c2,std::string("Destructive Add 1 for ")+typeid(_data_type).name());

    b.fill(3);
    const LibMIA::DenseMIA<_data_type,4> temp_b(b);
    c(i,j,k,l)=temp_b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==c2,std::string("Non-destructive Subtract 1 for ")+typeid(_data_type).name());

    b(i,j,k,l)-=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==c2,std::string("Destructive Subtract 1 for ")+typeid(_data_type).name());

    a.zeros();
    b.zeros();
    a.at(dim1-1,dim1-1,dim2-1,dim2-1)=1;
    b.at(dim2-1,dim1-1,dim2-1,dim1-1)=1;

    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    c2.zeros();
    c2.at(dim2-1,dim1-1,dim2-1,dim1-1)=2;
    BOOST_CHECK_MESSAGE(c==c2,std::string("Non-destructive Add 2 for ")+typeid(_data_type).name());

    b(i,j,k,l)+=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==c2,std::string("Destructive Add 2 for ")+typeid(_data_type).name());

    a.at(dim1-1,dim1-1,dim2-1,dim2-1)=3;
    b.at(dim2-1,dim1-1,dim2-1,dim1-1)=5;
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==c2,std::string("Non-destructive Subtract 2 for ")+typeid(_data_type).name());

    b(i,j,k,l)-=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==c2,std::string("Destructive Subtract 2 for ")+typeid(_data_type).name());

    LibMIA::DenseMIA<_data_type,2> x(dim1,dim1);
    LibMIA::DenseMIA<_data_type,2> y(dim1,dim1);
    LibMIA::DenseMIA<_data_type,2> n(dim1,dim1);
    LibMIA::DenseMIA<_data_type,2> n_final;
    n.ones();
    y.ones();
    n.ones();
    n_final=n;
    n(i,j)=n(i,j)+y(i,j)-x(i,j)-(y(i,j)-x(j,i))+(y(j,i)-x(i,j))-(y(j,i)-x(j,i));

    BOOST_CHECK_MESSAGE(n==n_final,std::string("Complicated expression 1 for ")+typeid(_data_type).name());

}

BOOST_AUTO_TEST_CASE( DenseMIAAddSubtractTests )
{

    do_work<double>(8,10);
    do_work<float>(8,10);
    do_work<int>(8,10);
    do_work<long long int>(8,10);

    do_work<double>(10,8);
    do_work<float>(10,8);
    do_work<int>(10,8);
    do_work<long long int>(10,8);

}