/**
 * @file transpsemilagr_main.cc
 * @brief NPDE homework TranspSemiLagr Main file
 * @author Philippe Peter
 * @date November 2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <filesystem>
#include <memory>

#include "transpsemilagr.h"

int main() {
  // The equation is solved on the test mesh circle.msh
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/../meshes/circle.msh");
  auto mesh_p = reader.mesh();

  // construct linear finite element space
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // initial conditions:
  // const auto u0 = [](const Eigen::Vector2d& x){ return 1-(x(0)* x(0) +
  // x(1)*x(1));};
  const auto u0 = [](const Eigen::Vector2d& x) {
    if (x(1) * x(1) + x(0) * x(0) > 0.99) {
      return 0.0;
    } else {
      return -x(1) - x(0);
    }
  };

  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0));

  // compute the solutions after 1st timestep
  Eigen::VectorXd sol_rot_1 =
      TranspSemiLagr::solverot(fe_space, u0_vector, 1, 0.1);
  lf::fe::MeshFunctionFE mf_sol_rot_1(fe_space, sol_rot_1);
  Eigen::VectorXd sol_trp_1 =
      TranspSemiLagr::solvetrp(fe_space, u0_vector, 1, 0.1);
  lf::fe::MeshFunctionFE mf_sol_trp_1(fe_space, sol_trp_1);

  // compute solutions at final time
  Eigen::VectorXd sol_rot_10 =
      TranspSemiLagr::solverot(fe_space, u0_vector, 10, 1.0);
  lf::fe::MeshFunctionFE mf_sol_rot_10(fe_space, sol_rot_10);
  Eigen::VectorXd sol_trp_10 =
      TranspSemiLagr::solvetrp(fe_space, u0_vector, 10, 1.0);
  lf::fe::MeshFunctionFE mf_sol_trp_10(fe_space, sol_trp_10);

  // OUTPUT RESULTS
  // construct writers
  lf::io::VtkWriter vtk_writer_rot_1(mesh_p, CURRENT_BINARY_DIR "rot_1.vtk");
  lf::io::VtkWriter vtk_writer_rot_10(mesh_p, CURRENT_BINARY_DIR "rot_10.vtk");
  lf::io::VtkWriter vtk_writer_trp_1(mesh_p, CURRENT_BINARY_DIR "trp_1.vtk");
  lf::io::VtkWriter vtk_writer_trp_10(mesh_p, CURRENT_BINARY_DIR "trp_10.vtk");

  // output data
  vtk_writer_rot_1.WritePointData("rot_1", mf_sol_rot_1);
  vtk_writer_rot_10.WritePointData("rot_10", mf_sol_rot_10);
  vtk_writer_trp_1.WritePointData("trp_1", mf_sol_trp_1);
  vtk_writer_trp_10.WritePointData("trp_10", mf_sol_trp_10);

  return 0;
}


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of dual annealing
// derived from https://github.com/scipy/scipy/blob/master/scipy/optimize/_dual_annealing.py
// Implementation only differs regarding boundary handling - this implementattion
// uses boundary-normalized X values. Local search is fixed to LBFGS-B, see
// https://github.com/yixuan/LBFGSpp/tree/master/include
// requires https://github.com/imneme/pcg-cpp

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <math.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"
#include <LBFGSB.h>

using namespace LBFGSpp;
using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

namespace dual_annealing {

typedef double (*callback_type)(int, const double*);

// wrapper around the fitness function, scales according to boundaries

class Fitness;

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);
static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static Eigen::MatrixXd normalVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return gauss_01(rs);
    });
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static vec emptyVec = { };

static vec logv(vec v) {
    return v.unaryExpr([](double x) {
        return log(x);
    });
}

static vec expv(vec v) {
    return v.unaryExpr([](double x) {
        return exp(x);
    });
}

double minLBFGS(Fitness *fitfun, vec &X0_, int maxIterations);

class Fitness {

public:

    vec lower;
    vec upper;

    Fitness(callback_type pfunc, vec *lower_limit, vec *upper_limit,
            long maxEvals_) {
        func = pfunc;
        lower = *lower_limit;
        upper = *upper_limit;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
        maxEvals = maxEvals_;
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0) {
            return X.cwiseMin(1.0).cwiseMax(-1.0);
        }
        return X;
    }

    double eval(const vec &X) {
        int n = X.size();
        double res = func(n, X.data());
        evaluationCounter++;
        return res;
    }

    double value(const vec &X) {
        double res = DBL_MAX;
        if (lower.size() > 0)
            res = eval(decode(getClosestFeasible(X)));
        else
            res = eval(X);
        if (res < bestY) {
            bestY = res;
            bestX = vec(X);
        }
        return res;
    }

    const double LS_MAXITER_RATIO = 6;
    const double LS_MAXITER_MIN = 100;
    const double LS_MAXITER_MAX = 1000;

    double local_search(const vec &x0, double currval, vec &res) {
        vec init = getClosestFeasible(x0);
        bestY = DBL_MAX;
        int maxIter = LS_MAXITER_RATIO * x0.size();
        if (maxIter > LS_MAXITER_MAX)
            maxIter = LS_MAXITER_MAX;
        if (maxIter < LS_MAXITER_MIN)
            maxIter = LS_MAXITER_MIN;
        minLBFGS(this, init, maxIter);
        if (bestY < DBL_MAX) {
            for (int i = 0; i < res.size(); i++)
                res[i] = bestX(i);
        }
        return bestY;
    }

    vec encode(const vec &X) const {
        if (lower.size() > 0)
            return (X - lower).array() / scale.array();
        else
            return X;
    }

    vec decode(const vec &X) const {
        if (lower.size() > 0)
            return X.cwiseProduct(scale) + lower;
        else
            return X;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

    bool maxEvalReached() {
        return evaluationCounter >= maxEvals;
    }

private:
    callback_type func;
    long evaluationCounter = 0;
    long maxEvals;
    vec scale;
    double bestY = DBL_MAX;
    vec bestX;
};

class LBFGSFunc {
private:
    Fitness *func;
    int dim;

public:

    LBFGSFunc(Fitness *Fitness_, int dim_) {
        func = Fitness_;
        dim = dim_;
    }

    double operator()(const vec &x, vec &grad) {
        if (!x.allFinite())
            return DBL_MAX;
        double eps = 1E-6;
        vec arg = vec(dim);
        for (int i = 0; i < dim; i++)
            arg[i] = x(i);
        for (int i = 0; i < dim; i++) {
            vec x1 = vec(arg);
            vec x2 = vec(arg);
            double e1 = eps;
            double e2 = eps;
            x1[i] += eps;
            if (x1[i] > 1) {
                x1[i] = 1;
                e1 = 1 - arg[i];
            }
            x2[i] -= eps;
            if (x2[i] < 0) {
                x2[i] = 0;
                e2 = arg[i];
            }
            double f1 = func->value(x1);
            double f2 = func->value(x2);
            grad[i] = (f1 - f2) / (e1 + e2);
        }
        double f = func->value(arg);
        return f;
    }
};

double minLBFGS(Fitness *fitfun, vec &X0, int maxIterations) {
    int dim = X0.size();
    LBFGSFunc fun = LBFGSFunc(fitfun, dim);

    LBFGSBParam<double> param;
    param.max_iterations = maxIterations;
    LBFGSBSolver<double> solver(param);
    vec lb = vec::Constant(dim, 0.0);
    vec ub = vec::Constant(dim, 1.0);
    // Initial values
    vec x = vec::Constant(dim, 0);
    for (int i = 0; i < dim; i++)
        x[i] = X0[i];
    double fx;
    int niter;
    try {
        niter = solver.minimize(fun, x, fx, lb, ub);
    } catch (std::exception &e) {
        //cout << e.what() << endl;
        return DBL_MAX;
    }
    return fx;
}

class VisitingDistribution {

    //Class used to generate new coordinates based on the distorted
    //Cauchy-Lorentz distribution. Depending on the steps within the Markov
    //chain, the class implements the strategy for generating new location
    //changes.

public:

    VisitingDistribution(int dim, double visiting_param_, pcg64 *rs_) {
        _visiting_param = visiting_param_;
        rs = rs_;

        // these are invariant numbers unless visiting_param changes
        double factor2 = exp(
                (4.0 - _visiting_param) * log(_visiting_param - 1.0));
        double factor3 = exp(
                (2.0 - _visiting_param) * log(2.0) / (_visiting_param - 1.0));
        _factor4_p = sqrt(M_PI) * factor2 / (factor3 * (3.0 - _visiting_param));

        double factor5 = 1.0 / (_visiting_param - 1.0) - 0.5;
        double d1 = 2.0 - factor5;
        _factor6 = M_PI * (1.0 - factor5) / sin(M_PI * (1.0 - factor5))
                / exp(lgamma(d1));
    }

    vec visiting(const vec &x, int step, double temperature) {
        //Based on the step in the strategy chain, new coordinated are
        //generated by changing all components is the same time or only
        //one of them, the new values are computed with visit_fn method

        int dim = x.size();
        if (step < dim) {
            // Changing all coordinates with a new visiting value
            double upper_sample = distr_01(*rs);
            double lower_sample = distr_01(*rs);
            vec visits = visit_fn(temperature, dim);
            for (int i = 0; i < dim; i++) {
                if (visits[i] > TAIL_LIMIT)
                    visits[i] = TAIL_LIMIT * upper_sample;
                else if (visits[i] < -TAIL_LIMIT)
                    visits[i] = -TAIL_LIMIT * lower_sample;
            }
            vec x_visit = visits + x;
            vec a = x_visit;
            vec b = vec(dim);
            for (int i = 0; i < dim; i++) {
                b[i] = fmod(a[i], 1) + 1;
                x_visit[i] = fmod(b[i], 1);
                if (abs(x_visit[i]) < MIN_VISIT_BOUND)
                    x_visit[i] += 1.e-10;
            }
            //cerr << step << " " << temperature <<  endl;// << x_visit << endl;
            return x_visit;
        } else {
            // Changing only one coordinate at a time based on strategy
            // chain step
            vec x_visit = vec(x);
            double visit = visit_fn(temperature, 1)[0];
            if (visit > TAIL_LIMIT)
                visit = TAIL_LIMIT * distr_01(*rs);
            else if (visit < -TAIL_LIMIT)
                visit = -TAIL_LIMIT * distr_01(*rs);
            int index = step - dim;
            x_visit[index] = visit + x[index];
            double a = x_visit[index];
            double b = fmod(a, 1) + 1;
            x_visit[index] = fmod(b, 1);
            if (abs(x_visit[index]) < MIN_VISIT_BOUND)
                x_visit[index] += MIN_VISIT_BOUND;
            //cerr << step << " " << temperature <<  endl;// << x_visit << endl;
            return x_visit;
        }
    }

    vec visit_fn(double temperature, int dim) {

        //Formula Visita from p. 405 of reference [2]
        vec x = normalVec(dim, *rs);
        vec y = normalVec(dim, *rs);
        ;

        double factor1 = exp(log(temperature) / (_visiting_param - 1.0));
        double factor4 = _factor4_p * factor1;

        // sigmax
        x = x
                * exp(
                        -(_visiting_param - 1.0) * log(_factor6 / factor4)
                                / (3.0 - _visiting_param));

        vec den = expv(
                logv(y.cwiseAbs() * (_visiting_param - 1.0))
                        / (3.0 - _visiting_param));
        return x.cwiseQuotient(den);
    }

private:

    pcg64 *rs;
    double _visiting_param;
    double _factor4_p;
    double _factor6;

    const double TAIL_LIMIT = 1.e8;
    const double MIN_VISIT_BOUND = 1.e-10;
};

class nanexception: public exception {
    virtual const char* what() const throw () {
        return "Objective function is returning nan";
    }
} naneexc;

const double BIG_VALUE = 1e16;

class EnergyState {

    //Class used to record the energy state-> At any time, it knows what is the
    //currently used coordinates and the most recent best location
public:

    double ebest;
    vec xbest;
    double current_energy;
    vec current_location;

    EnergyState(int dim_) {
        dim = dim_;
        ebest = DBL_MAX;
        xbest = { };
        current_energy = DBL_MAX;
        current_location = { };
    }

    void reset(Fitness *owf, pcg64 *rs, const vec &x0) {
        if (x0.size() == 0)
            current_location = normalVec(dim, *rs);
        else
            current_location = vec(x0);
        bool init_error = true;
        int reinit_counter = 0;
        while (init_error) {
            current_energy = owf->value(current_location);
            if (current_energy >= BIG_VALUE || isnan(current_energy)) {
                if (reinit_counter >= MAX_REINIT_COUNT) {
                    init_error = false;
                    throw naneexc;
                }
                current_location = uniformVec(dim, *rs);
                reinit_counter++;
            } else
                init_error = false;
            // If first time reset, initialize ebest and xbest
            if (ebest == DBL_MAX && xbest.size() == 0) {
                ebest = current_energy;
                xbest = vec(current_location);
            }
            // Otherwise, keep them in case of reannealing reset
        }
    }

    void update_best(double e, const vec &x) {
        ebest = e;
        xbest = vec(x);
    }

    void update_current(double e, const vec &x) {
        current_energy = e;
        current_location = vec(x);
    }

private:
    // Maximimum number of trials for generating a valid starting point
    int MAX_REINIT_COUNT = 1000;
    int dim;
};

class StrategyChain {
    // Class used for the Markov chain and related strategy for local search
    // decision
public:

    StrategyChain(double acceptance_param_, VisitingDistribution *vd_,
            Fitness *ofw_, pcg64 *rs_, EnergyState *state_) {
        // Global optimizer state
        state = state_;
        // Local markov chain minimum energy and location
        emin = state->current_energy;
        xmin = vec(state->current_location);
        // Acceptance parameter
        acceptance_param = acceptance_param_;
        // Visiting distribution instance
        vd = vd_;
        // Wrapper to objective function and related local minimizer
        ofw = ofw_;
        not_improved_idx = 0;
        not_improved_max_idx = 1000;
        rs = rs_;
        temperature_step = 0;
        K = 100 * (state->current_location).size();
    }

    void accept_reject(int j, double e, const vec &x_visit) {
        double r = distr_01(*rs);
        double pqv_temp = (acceptance_param - 1.0) * (e - state->current_energy)
                / (temperature_step + 1.);
        double pqv = 0;
        if (pqv_temp < 0.)
            pqv = 0.;
        else
            pqv = exp(log(pqv_temp) / (1. - acceptance_param));
        if (r <= pqv) {
            // We accept the new location and update state
            state->update_current(e, x_visit);
            xmin = vec(state->current_location);
        }
        // No improvement since long time
        if (not_improved_idx >= not_improved_max_idx) {
            if (j == 0 || state->current_energy < emin) {
                emin = state->current_energy;
                xmin = vec(state->current_location);
            }
        }
    }

    void run(int step, double temperature) {
        temperature_step = temperature / (double) (step + 1);
        not_improved_idx += 1;
        for (unsigned int j = 0; j < (state->current_location).size() * 2;
                j++) {
            if (j == 0)
                state_improved = false;
            if (step == 0 && j == 0)
                state_improved = true;
            vec x_visit = vd->visiting(state->current_location, j, temperature);
            // Calling the objective function
            double e = ofw->value(x_visit);
            if (e < state->current_energy) {
                // We have got a better energy value
                state->update_current(e, x_visit);
                if (e < state->ebest) {
                    state->update_best(e, x_visit);
                    state_improved = true;
                    not_improved_idx = 0;
                }
            } else {
                // We have not improved but do we accept the new location?
                accept_reject(j, e, x_visit);
            }
            if (ofw->maxEvalReached())
                return;
        }	// End of StrategyChain loop
    }

    void local_search() {
        // Decision making for performing a local search
        // based on Markov chain results
        // If energy has been improved or no improvement since too long,
        // performing a local search with the best Markov chain location
        int dim = state->xbest.size();
        if (state_improved) {
            // Global energy has improved, let's see if LS improved further
            vec x = vec(dim);
            double e = ofw->local_search(state->xbest, state->ebest, x);
            if (e < state->ebest) {
                not_improved_idx = 0;
                state->update_best(e, x);
                state->update_current(e, x);
                if (ofw->maxEvalReached())
                    return;
            }
        }
        // Check probability of a need to perform a LS even if no improvment
        bool do_ls = false;
        if (K < 90 * state->current_location.size()) {
            double pls = exp(
                    K * (state->ebest - state->current_energy)
                            / temperature_step);
            if (pls >= distr_01(*rs))
                do_ls = true;
        }
        // Global energy not improved, let's see what LS gives
        // on the best strategy chain location
        if (not_improved_idx >= not_improved_max_idx)
            do_ls = true;
        if (do_ls) {
            vec x = vec(dim);
            double e = ofw->local_search(xmin, state->ebest, x);
            xmin = vec(x);
            emin = e;
            not_improved_idx = 0;
            not_improved_max_idx = state->current_location.size();
            if (e < state->ebest) {
                state->update_best(emin, xmin);
                state->update_current(e, x);
            }
        }
    }

private:

    double emin;
    vec xmin;
    EnergyState *state;
    double acceptance_param;
    VisitingDistribution *vd;
    int not_improved_idx;
    int not_improved_max_idx;
    pcg64 *rs;
    Fitness *ofw;
    double temperature_step;
    double K;
    bool state_improved = false;
};

class sizeexception: public exception {
    virtual const char* what() const throw () {
        return "Bounds size does not match x0";
    }
} sizeeexc;

class DARunner {

public:

    DARunner(Fitness *fun_, vec &x0_, long seed_, bool use_local_search_) {
        owf = fun_;
        if (x0_.size() > 0 && x0_.size() != owf->lower.size())
            throw sizeeexc;
        //Initialization of RandomState for reproducible runs if seed provided
        rs = new pcg64(seed_);
        use_local_search = use_local_search_;
        // Initialization of the energy state
        es = new EnergyState(owf->lower.size());
        es->reset(owf, rs, x0_);
        // VisitingDistribution instance
        vd = new VisitingDistribution(owf->lower.size(), qv, rs);
        // Markov chain instance
        sc = new StrategyChain(qa, vd, owf, rs, es);
    }

    ~DARunner() {
        delete rs;
        delete vd;
        delete sc;
        delete es;
    }

    void search() {
        iter = 0;
        double t1 = exp((qv - 1) * log(2.0)) - 1.0;
        for (;;) {
            for (int i = 0; i < maxsteps; i++) {
                // Compute temperature for this step
                double s = i + 2.0;
                double t2 = exp((qv - 1) * log(s)) - 1.0;
                double temperature = temperature_start * t1 / t2;
                if (iter++ >= maxsteps)
                    return;
                // Need a re-annealing process?
                if (temperature < temperature_restart) {
                    es->reset(owf, rs, emptyVec);
                    break;
                }
                // starting strategy chain
                sc->run(i, temperature);
                if (owf->maxEvalReached())
                    return;
                if (use_local_search) {
                    sc->local_search();
                    if (owf->maxEvalReached())
                        return;
                }
            }
        }
    }

    vec bestX() {
        return es->xbest;
    }

    double bestY() {
        return es->ebest;
    }

private:

    int MAX_REINIT_COUNT = 1000;
    double temperature_start = 5230;
    double qv = 2.62;
    double qa = -5.0;
    bool use_local_search;
    // maximum number of step (main iteration)
    double maxsteps = 1000;
    // minimum value of annealing temperature reached to perform
    // re-annealing temperature_start
    double temperature_restart = 0.1;
    Fitness *owf;
    pcg64 *rs;
    EnergyState *es;
    StrategyChain *sc;
    VisitingDistribution *vd;
    int iter = 0;
};

double minimize(Fitness *fun, vec &x0, long seed, bool use_local_search,
        vec &X) {
    DARunner gr = DARunner(fun, x0, seed, use_local_search);
    gr.search();
    int dim = x0.size();
    vec bx = gr.bestX();
    for (int i = 0; i < dim; i++)
        X[i] = bx[i];
    return gr.bestY();
}
}

using namespace dual_annealing;

extern "C" {
void optimizeDA_C(long runid, callback_type func, int dim, int seed,
        double *init, double *lower, double *upper, int maxEvals,
        bool use_local_search, double* res) {
    int n = dim;
    vec guess(n), lower_limit(n), upper_limit(n);
    bool useLimit = false;
    for (int i = 0; i < n; i++) {
        guess[i] = init[i];
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    if (maxEvals <= 0)
        maxEvals = 1E7;
    Fitness fitfun(func, &lower_limit, &upper_limit, maxEvals);

    try {
        vec X = zeros(dim);
        vec enc = fitfun.encode(guess);
        double bestY = minimize(&fitfun, enc, seed, use_local_search, X);
        vec bestX = fitfun.decode(X);
        for (int i = 0; i < n; i++)
            res[i] = bestX[i];
        res[n] = bestY;
        res[n + 1] = fitfun.getEvaluations();
        res[n + 2] = 0;
        res[n + 3] = 0;
    } catch (std::exception &e) {
        cerr << e.what() << endl;
    }
}
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HPSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HPSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A Hermitian indefinite matrix stored in packed format
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * hpsv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N Hermitian matrix in packed
     * storage and X and B are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^H,  if UPLO = 'U',
     *   A = L * D * L^H,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is Hermitian and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks. The factored
     * form of A is then used to solve the system of equations A * X = B.
     */

    namespace detail {

      inline
      void hpsv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* ap, int* ipiv,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CHPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void hpsv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* ap, int* ipiv,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZHPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      template <typename HermA, typename MatrB, typename IVec>
      inline
      int hpsv (HermA& a, IVec& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<HermA>::matrix_structure,
          traits::hermitian_packed_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
#endif

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));

        char uplo = traits::matrix_uplo_tag (a);
        int info;
        hpsv (uplo, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              &info);
        return info;
      }

    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hpsv (HermA& a, IVec& i, MatrB& b) {
      assert (traits::matrix_size1 (a) == traits::vector_size (i));
      return detail::hpsv (a, i, b);
    }

    template <typename HermA, typename MatrB>
    inline
    int hpsv (HermA& a, MatrB& b) {
      // with 'internal' pivot vector

      int info = -101;
      traits::detail::array<int> i (traits::matrix_size1 (a));

      if (i.valid())
        info = detail::hpsv (a, i, b);
      return info;
    }

    /*
     * hptrf() computes the factorization of a Hermitian matrix A
     * in packed storage using the  Bunch-Kaufman diagonal pivoting
     * method. The form of the factorization is
     *    A = U * D * U^H  or  A = L * D * L^H
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is Hermitian and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail {

      inline
      void hptrf (char const uplo, int const n,
                  traits::complex_f* ap, int* ipiv, int* info)
      {
        LAPACK_CHPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

      inline
      void hptrf (char const uplo, int const n,
                  traits::complex_d* ap, int* ipiv, int* info)
      {
        LAPACK_ZHPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

      template <typename HermA, typename IVec, typename Work>
      inline
      int hptrf (char const ul, HermA& a, IVec& i, Work& w, int const lw) {

      }

    }

    template <typename HermA, typename IVec>
    inline
    int hptrf (HermA& a, IVec& i) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::hptrf (uplo, n, traits::matrix_storage (a),
                     traits::vector_storage (i), &info);
      return info;
    }

    /*
     * hptrs() solves a system of linear equations A*X = B with
     * a Hermitian matrix A in packed storage using the factorization
     *    A = U * D * U^H   or  A = L * D * L^H
     * computed by hptrf().
     */

    namespace detail {

      inline
      void hptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* ap, int const* ipiv,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CHPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (ap), ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void hptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* ap, int const* ipiv,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZHPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (ap), ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hptrs (HermA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::hptrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
                     traits::vector_storage (i),
#else
                     traits::matrix_storage_const (a),
                     traits::vector_storage_const (i),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
        return info;
    }

    // TO DO: hptri

  }

}}}

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_HAVERSINE_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_HAVERSINE_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

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

#include <boost/geometry/strategies/distance.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

namespace comparable
{

// Comparable haversine.
// To compare distances, we can avoid:
// - multiplication with radius and 2.0
// - applying sqrt
// - applying asin (which is strictly (monotone) increasing)
template
<
    typename RadiusType,
    typename CalculationType = void
>
class haversine
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef RadiusType radius_type;

    explicit inline haversine(RadiusType const& r = 1.0)
        : m_radius(r)
    {}

    template <typename Point1, typename Point2>
    static inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& p1, Point2 const& p2)
    {
        return calculate<typename calculation_type<Point1, Point2>::type>(
                   get_as_radian<0>(p1), get_as_radian<1>(p1),
                   get_as_radian<0>(p2), get_as_radian<1>(p2)
               );
    }

    inline RadiusType radius() const
    {
        return m_radius;
    }

private :
    template <typename R, typename T1, typename T2>
    static inline R calculate(T1 const& lon1, T1 const& lat1,
                              T2 const& lon2, T2 const& lat2)
    {
        return math::hav(lat2 - lat1)
                + cos(lat1) * cos(lat2) * math::hav(lon2 - lon1);
    }

    RadiusType m_radius;
};

} // namespace comparable

/*!
\brief Distance calculation for spherical coordinates
on a perfect sphere using haversine
\ingroup strategies
\tparam RadiusType \tparam_radius
\tparam CalculationType \tparam_calculation
\author Adapted from: http://williams.best.vwh.net/avform.htm
\see http://en.wikipedia.org/wiki/Great-circle_distance
\note (from Wiki:) The great circle distance d between two
points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
    d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
A mathematically equivalent formula, which is less subject
    to rounding error for short distances is:
    d=2*asin(sqrt((sin((lat1-lat2) / 2))^2
    + cos(lat1)*cos(lat2)*(sin((lon1-lon2) / 2))^2))

\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename RadiusType,
    typename CalculationType = void
>
class haversine
{
    typedef comparable::haversine<RadiusType, CalculationType> comparable_type;

public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : services::return_type<comparable_type, Point1, Point2>
    {};

    typedef RadiusType radius_type;

    /*!
    \brief Constructor
    \param radius radius of the sphere, defaults to 1.0 for the unit sphere
    */
    inline haversine(RadiusType const& radius = 1.0)
        : m_radius(radius)
    {}

    /*!
    \brief applies the distance calculation
    \return the calculated distance (including multiplying with radius)
    \param p1 first point
    \param p2 second point
    */
    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& p1, Point2 const& p2) const
    {
        typedef typename calculation_type<Point1, Point2>::type calculation_type;
        calculation_type const a = comparable_type::apply(p1, p2);
        calculation_type const c = calculation_type(2.0) * asin(math::sqrt(a));
        return calculation_type(m_radius) * c;
    }

    /*!
    \brief access to radius value
    \return the radius
    */
    inline RadiusType radius() const
    {
        return m_radius;
    }

private :
    RadiusType m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename RadiusType, typename CalculationType>
struct tag<haversine<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<haversine<RadiusType, CalculationType>, P1, P2>
    : haversine<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<haversine<RadiusType, CalculationType> >
{
    typedef comparable::haversine<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<haversine<RadiusType, CalculationType> >
{
private :
    typedef haversine<RadiusType, CalculationType> this_type;
    typedef comparable::haversine<RadiusType, CalculationType> comparable_type;
public :
    static inline comparable_type apply(this_type const& input)
    {
        return comparable_type(input.radius());
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<haversine<RadiusType, CalculationType>, P1, P2>
{
private :
    typedef haversine<RadiusType, CalculationType> this_type;
    typedef typename return_type<this_type, P1, P2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(this_type const& , T const& value)
    {
        return return_type(value);
    }
};

// Specializations for comparable::haversine
template <typename RadiusType, typename CalculationType>
struct tag<comparable::haversine<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<comparable::haversine<RadiusType, CalculationType>, P1, P2>
    : comparable::haversine<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<comparable::haversine<RadiusType, CalculationType> >
{
    typedef comparable::haversine<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<comparable::haversine<RadiusType, CalculationType> >
{
private :
    typedef comparable::haversine<RadiusType, CalculationType> this_type;
public :
    static inline this_type apply(this_type const& input)
    {
        return input;
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<comparable::haversine<RadiusType, CalculationType>, P1, P2>
{
private :
    typedef comparable::haversine<RadiusType, CalculationType> strategy_type;
    typedef typename return_type<strategy_type, P1, P2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(strategy_type const& strategy, T const& distance)
    {
        return_type const s = sin((distance / strategy.radius()) / return_type(2));
        return s * s;
    }
};

// Register it as the default for point-types
// in a spherical equatorial coordinate system
template <typename Point1, typename Point2>
struct default_strategy
    <
        point_tag, point_tag, Point1, Point2,
        spherical_equatorial_tag, spherical_equatorial_tag
    >
{
    typedef strategy::distance::haversine<typename select_coordinate_type<Point1, Point2>::type> type;
};

// Note: spherical polar coordinate system requires "get_as_radian_equatorial"

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_HAVERSINE_HPP


#ifndef __EIGENINTERFACE_HXX__
#define __EIGENINTERFACE_HXX__

#ifdef LEGOLAS_USE_EIGEN

#include <Eigen/Core>

namespace Legolas{

  template <class REAL_TYPE>
  class EigenInterface{
  public:
    typedef REAL_TYPE RealType;
    //    typedef Eigen::Matrix<RealType,Eigen::Dynamic,1>     EigenVector;
    typedef Eigen::Array<RealType,Eigen::Dynamic,1>     EigenVector;
    typedef Eigen::Matrix<RealType,Eigen::Dynamic,1>    EigenMatrix;
    //    typedef Eigen::Map<EigenVector>        EigenVectorView;
    //    typedef Eigen::Map<EigenVector,Eigen::Aligned>        EigenVectorView;
    //    typedef Eigen::Map<EigenVector>        EigenVectorView;
    typedef Eigen::Map<EigenVector,Eigen::Aligned>        EigenVectorView;
    typedef Eigen::Map<EigenMatrix,Eigen::Aligned>        EigenMatrixView;

  };

}

#endif

#endif


#ifndef NBT_INTEGRATOR_HPP
#define NBT_INTEGRATOR_HPP

#include <Eigen>

/**
 * Abstract function object for integrators. Integrators
 * are functional interfaces that update the position and
 * velocity matrices using an acceleration matrix.
 */
class Integrator {
    public:
        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        virtual void integrate(double dt,
                               const Eigen::Ref<const Eigen::MatrixXd>& a,
                               Eigen::Ref<Eigen::MatrixXd> v,
                               Eigen::Ref<Eigen::MatrixXd> x) = 0;
};

/**
 * Performs Euler integration. Fast but inaccurate.
 */
class EulerIntegrator: public Integrator {
    public:
        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        void integrate(double dt,
                       const Eigen::Ref<const Eigen::MatrixXd>& a,
                       Eigen::Ref<Eigen::MatrixXd> v,
                       Eigen::Ref<Eigen::MatrixXd> x);
};

/**
 * Performs Verlet integration. Is symplectic but not as accurate as RungeKuttaIntegrator.
 */
class VerletIntegrator: public Integrator {
    public:
        bool isFirstIteration = true;
        Eigen::MatrixXd aPrev;

        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        void integrate(double dt,
                       const Eigen::Ref<const Eigen::MatrixXd>& a,
                       Eigen::Ref<Eigen::MatrixXd> v,
                       Eigen::Ref<Eigen::MatrixXd> x);
};

/**
 * Performs Runge-Kutta 4th order integration method. More accurate than EulerIntegrator but slower.
 */
class RungeKuttaIntegrator: public Integrator {
    public:
        /**
         * @brief Computes velocities and positions from accelerations and time step.
         *
         * @param dt Time step
         * @param a Acceleration matrix
         * @param v Velocity matrix
         * @param x Position matrix
         */
        void integrate(double dt,
                       const Eigen::Ref<const Eigen::MatrixXd>& a,
                       Eigen::Ref<Eigen::MatrixXd> v,
                       Eigen::Ref<Eigen::MatrixXd> x); // TODO runge kutta
};

#endif


#define BOOST_TEST_DYN_LINK // optional
#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE MyTest // specify the name of your test module
#if defined (__GNUC__) && defined(__unix__)
    #include <boost/test/included/unit_test.hpp> // include this to get main()
#elif defined (WIN32)
    #include <boost/test/unit_test.hpp> // include this to get main()
#endif

#include <iostream>
#include <FixedPoints.h>
#include <FixedPointsCommon.h>

using namespace boost::unit_test;
using namespace std;

template class UFixed<8, 8>;
template class SFixed<7, 8>;

BOOST_AUTO_TEST_SUITE(testsuite)

BOOST_AUTO_TEST_CASE(test_getters_ufixed) {
  UQ8x8 a = 1.5;
  BOOST_CHECK( sizeof(uint16_t) == sizeof(a.getInternal()));
  BOOST_CHECK( 0x0180 == a.getInternal());

  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getInteger()));
  BOOST_CHECK( 0x01 == a.getInteger());

  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getFraction()));
  BOOST_CHECK( 0x80 == a.getFraction());
}

BOOST_AUTO_TEST_CASE(test_cast_operators_ufixed) {
  UQ8x8 a = 1.5;
  BOOST_CHECK( 1.5 == static_cast<float>(a) );
  BOOST_CHECK( 1.5 == static_cast<double>(a) );
  BOOST_CHECK( 1.5 == static_cast<long double>(a) );
  BOOST_CHECK( 1 == static_cast<unsigned char>(a) );
  BOOST_CHECK( 1.5 == static_cast<UQ16x16>(a) );
  BOOST_CHECK(1.5 == (static_cast<UFixed<16, 8> >(a)));
  BOOST_CHECK(1.5 == (static_cast<UFixed<20, 4> >(a)));

  BOOST_CHECK(1.5 == static_cast<SQ7x8>(a));
  BOOST_CHECK(1.5 == (static_cast<SFixed<7, 16> >(a)));
  BOOST_CHECK(1.5 == (static_cast<SFixed<11, 4> >(a)));
  a = 0;
  BOOST_CHECK(0 == static_cast<SQ7x8>(a));
}

BOOST_AUTO_TEST_CASE(test_getters_sfixed) {
  SQ7x8 a = 1.5;
  BOOST_CHECK( sizeof(uint16_t) == sizeof(a.getInternal()));
  BOOST_CHECK( 0x0180 == a.getInternal());

  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getInteger()));
  BOOST_CHECK( 0x01 == a.getInteger());

  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getFraction()));
  BOOST_CHECK( 0x80 == a.getFraction());
}

BOOST_AUTO_TEST_CASE(test_cast_operators_sfixed) {
  SQ7x8 a = 1.5;
  SQ15x16 b = 1.5;
  BOOST_CHECK( 1.5 == static_cast<float>(a) );
  BOOST_CHECK( 1.5 == static_cast<double>(a) );
  BOOST_CHECK( 1.5 == static_cast<long double>(a) );
  BOOST_CHECK( 1 == static_cast<signed char>(a) );
  BOOST_CHECK( b == static_cast<SQ15x16>(a) );
  BOOST_CHECK(1.5 == static_cast<UQ8x8>(a));
  a = 0;
  BOOST_CHECK(0 == static_cast<UQ8x8>(a));
  a = -1;
  BOOST_CHECK(255 == static_cast<UQ8x8>(a));
  a = -12.5;
  BOOST_CHECK(243.5 == static_cast<UQ8x8>(a));
}

BOOST_AUTO_TEST_CASE(test_utils) {
  UQ8x8 a = 1.5;
  UQ8x8 b = floorFixed(a);
  BOOST_CHECK( 1.0 == static_cast<float>(b));
}

BOOST_AUTO_TEST_CASE(test_random_ufixed) {
  auto a = randomUFixed<8, 8>();
  BOOST_CHECK( sizeof(uint16_t) == sizeof(a.getInternal()));
  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getInteger()));
  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getFraction()));
}

BOOST_AUTO_TEST_CASE(test_random_sfixed) {
  auto a = randomSFixed<7, 8>();
  BOOST_CHECK( sizeof(uint16_t) == sizeof(a.getInternal()));
  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getInteger()));
  BOOST_CHECK( sizeof(uint8_t) == sizeof(a.getFraction()));
}

BOOST_AUTO_TEST_CASE(test_multiply_ufixed) {
  UQ8x8 a = 200.0;
  UQ8x8 b = 1.5;
  BOOST_CHECK( 300.0 == static_cast<float>(multiply(a, b)));
}

BOOST_AUTO_TEST_CASE(test_multiply_sfixed) {
  SQ7x8 a = 100.0;
  SQ7x8 b = 1.5;
  BOOST_CHECK( 150.0 == static_cast<float>(multiply(a, b)));
}

BOOST_AUTO_TEST_CASE(test_basic_arithmetic_operations_same_size_ufixed) {
  UQ8x8 a = 100.0;
  UQ8x8 b = 1.5;
  BOOST_CHECK( 101.5 == a + b);
  BOOST_CHECK( 98.5 == a - b);
  BOOST_CHECK( 150 == a * b);
  BOOST_CHECK( UQ8x8(66.66666666666667) == a / b);
}

BOOST_AUTO_TEST_CASE(test_basic_arithmetic_operations_same_size_sfixed) {
  SQ7x8 a = 100.0;
  SQ7x8 b = 1.5;
  BOOST_CHECK( 101.5 == a + b);
  BOOST_CHECK( 98.5 == a - b);
  BOOST_CHECK( 150 == a * b);
  BOOST_CHECK( SQ7x8(66.66666666666667) == a / b);
}

BOOST_AUTO_TEST_CASE(test_compound_assignemnt_operators_ufixed) {
  UQ8x8 a = 100.0;
  UQ8x8 b = 1.5;
  a += b;
  BOOST_CHECK( 101.5 == a);
  a -= b;
  BOOST_CHECK( 100.0 == a);
  a *= b;
  BOOST_CHECK( 150 == a);
  a /= b;
  BOOST_CHECK( 100.0 == a);
}

BOOST_AUTO_TEST_CASE(test_compound_assignemnt_operators_sfixed) {
  SQ7x8 a = 100.0;
  SQ7x8 b = 1.25;
  a += b;
  BOOST_CHECK( 101.25 == a);
  a -= b;
  BOOST_CHECK( 100.0 == a);
  a *= b;
  BOOST_CHECK( 125 == a);
  a /= b;
  BOOST_CHECK( 100.0 == a);

  a = -100.0;
  a += b;
  BOOST_CHECK( -98.75 == a);
  a -= b;
  BOOST_CHECK( -100.0 == a);
  a *= b;
  BOOST_CHECK( -125 == a);
  a /= b;
  BOOST_CHECK( -100.0 == a);
}

BOOST_AUTO_TEST_CASE(test_increment_and_decrement_ufixed) {
  UQ8x8 a = 100.0;
  ++a;
  BOOST_CHECK( 101.0 == a);
  --a;
  BOOST_CHECK( 100.0 == a);
  UQ8x8 b = a++;
  BOOST_CHECK( 100.0 == b);
  BOOST_CHECK( 101.0 == a);
  b = a--;
  BOOST_CHECK( 100.0 == a);
  BOOST_CHECK( 101.0 == b);
}

BOOST_AUTO_TEST_CASE(test_increment_and_decrement_sfixed) {
  SQ7x8 a = 100.0;
  ++a;
  BOOST_CHECK( 101.0 == a);
  --a;
  BOOST_CHECK( 100.0 == a);
  SQ7x8 b = a++;
  BOOST_CHECK( 100.0 == b);
  BOOST_CHECK( 101.0 == a);
  b = a--;
  BOOST_CHECK( 100.0 == a);
  BOOST_CHECK( 101.0 == b);
}

BOOST_AUTO_TEST_CASE(test_statics_ufixed) {
  UQ8x8 a = 100.0;
  UQ8x8 b = UQ8x8::fromInternal(a.getInternal());
  BOOST_CHECK( 100.0 == b);
}

BOOST_AUTO_TEST_CASE(test_statics_sfixed) {
  SQ7x8 a = 100.0;
  SQ7x8 b = SQ7x8::fromInternal(a.getInternal());
  BOOST_CHECK( 100.0 == b);
  BOOST_CHECK( -100.0 == -b);
}

BOOST_AUTO_TEST_CASE(test_constructors_ufixed) {
  UQ8x8 a;
  BOOST_CHECK(0.0 == a);
  UQ8x8 b(static_cast<const char> (5));
  BOOST_CHECK(5 == b);
  UQ8x8 c(static_cast<const unsigned char> (5));
  BOOST_CHECK(5 == c);
  UQ8x8 d(static_cast<const signed char> (5));
  BOOST_CHECK(5 == d);
  UQ8x8 e(static_cast<const unsigned short> (5));
  BOOST_CHECK(5 == e);
  UQ8x8 f(static_cast<const signed short> (5));
  BOOST_CHECK(5 == f);
  UQ8x8 g(static_cast<const unsigned int> (5));
  BOOST_CHECK(5 == g);
  UQ8x8 h(static_cast<const signed int> (5));
  BOOST_CHECK(5 == h);
  UQ8x8 i(static_cast<const unsigned long> (5));
  BOOST_CHECK(5 == i);
  UQ8x8 j(static_cast<const signed long> (5));
  BOOST_CHECK(5 == j);
  UQ8x8 k(static_cast<const unsigned long long> (5));
  BOOST_CHECK(5 == k);
  UQ8x8 l(static_cast<const signed long long>(5));
  BOOST_CHECK(5 == l);
  UQ8x8 m(static_cast<float>(5));
  BOOST_CHECK(5 == m);
  UQ8x8 n(static_cast<long double>(5));
  BOOST_CHECK(5 == n);
  UQ8x8 o(static_cast<long double>(5));
  BOOST_CHECK(5 == o);
}

BOOST_AUTO_TEST_SUITE_END()


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-arithmetic
    This function object computes the truncation toward @ref Zero
    of its parameter.

    @par Header <boost/simd/function/trunc.hpp>

    @par Notes
      - the call to `trunc(x)` is similar to `sign(x)*floor(abs(x))`

      - For floating point number it is also one of the two ouputs of
        the @ref modf function.
        And we have:
        @code
          trunc(x) + frac(x) == x;
        @endcode
        except for nans

    @par Decorators

       - std_ for floating entries call std::trunc

    @par Alias:

       fix

    @see abs, frac, floor, sign, modf, itrunc

    @par Example:

      @snippet trunc.cpp trunc

    @par Possible output:

      @snippet trunc.txt trunc

  **/
  Value trunc(Value const& x);
} }
#endif

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

#endif


/**
 * @author Tomas Polasek
 * @date 11.20.2019
 * @version 1.0
 * @brief Utilities and statistics for the treeio::Tree class.
 */

#include "TreeIOAccelerationUtils.h"

#include <Eigen/Eigen>

#include <intersect/intersect.h>

namespace treeacc
{

Line Line::fromDirection(const Vector3D &origin, const Vector3D &direction)
{ return Line{ origin, origin + direction }; }

BoundingBox3D::BoundingBox3D() :
    BoundingBox3D( Vector3D{ } )
{ }
BoundingBox3D::BoundingBox3D(const Vector3D &pos) :
    BoundingBox3D(pos, pos)
{ }
BoundingBox3D::BoundingBox3D(const Vector3D &lv, const Vector3D &hv)
{ min = lv; max = hv; }

bool BoundingBox3D::intersects(const Ray &ray) const
{ return intersection(ray) < std::numeric_limits<float>::max(); }

float BoundingBox3D::intersection(const Ray &ray) const
{
    const auto tMinCorner{ (min - ray.origin) / ray.direction };
    const auto tMaxCorner{ (max - ray.origin) / ray.direction };
    const auto tMin{ Vector3D::elementMin(tMinCorner, tMaxCorner).max() };
    const auto tMax{ Vector3D::elementMax(tMinCorner, tMaxCorner).min() };

    return (tMax < 0.0f || tMin > tMax) ? std::numeric_limits<float>::max() : tMin;
}

float calculateTriangleArea(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto a{ p2 - p1 };
    const auto b{ p3 - p1 };

    const auto prod{ Vector3D::crossProduct(a, b) };

    return prod.length() / 2.0f;
}

float calculateTrianglePerimeter(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto a{ p2 - p1 };
    const auto b{ p3 - p1 };
    const auto c{ p2 - p3 };

    return a.length() + b.length() + c.length();
}

Vector3D calculateTriangleMedianPoint(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{ return (p1 + p2 + p3) / 3.0f; }

float calculateTriangleMedian(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto p1p2{ p1 + p2 / 2.0f };
    return (p3 - p1p2).length();
}

float calculateTriangleLongestMedian(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    const auto m1{ calculateTriangleMedian(p1, p2, p3) };
    const auto m2{ calculateTriangleMedian(p2, p3, p1) };
    const auto m3{ calculateTriangleMedian(p1, p3, p2) };

    return std::max<float>({ m1, m2, m3 });
}

Vector3D sampleTriangle(const Vector3D &a1, const Vector3D &a2, const Vector3D &a3, const Vector3D &w)
{
    // TODO - Vector3D scalar multiplication + addition seems broken?
    return {
        w.x * a1.x + w.y * a2.x + w.z * a3.x,
        w.x * a1.y + w.y * a2.y + w.z * a3.y,
        w.x * a1.z + w.y * a2.z + w.z * a3.z
    };
}

/**
 * Sample barycentric U coordinate using provided random value.
 * Source: Portsmouth, Jamie. "Efficient barycentric point sampling on meshes."
 *
 * @param phiU Normalized relative weight for U-axis.
 * @param phiV Normalized relative weight for V-axis.
 * @param rand Random value used for sampling U coordinate.
 * @param limit Quality limit for inner loop.
 *
 * @return Returns sampled U coordinate.
 */
float sampleBarycentricU(float phiU, float phiV, float rand, float limit = 5.0e-3)
{
    static constexpr auto MAX_ITERATIONS{ 20u };
    static constexpr auto EPSILON{ std::numeric_limits<float>::epsilon() };
    const auto l{ (2.0f * phiU - phiV) / 3.0f };
    auto u{ 0.5f };
    for (std::size_t iter = 0u; iter < MAX_ITERATIONS; ++iter)
    {
        const auto u1{ 1.0f - u };
        const auto P{ u * (2.0f - u) - l * u * u1 * u1 - rand};
        const auto Pd{ std::max<float>(u1 * (2.0f + l * (3.0f * u - 1.0f)), EPSILON)};
        const auto du{ std::max<float>(std::min<float>(P / Pd, 0.25f), -0.25f) };

        u -= du;
        u = std::max<float>(std::min<float>(u, 1.0f - EPSILON), EPSILON);
        if (std::fabs(du) < limit)
        { break; }
    }

    return u;
}

/**
 * Sample barycentric V coordinate using provided random value.
 * Source: Portsmouth, Jamie. "Efficient barycentric point sampling on meshes."
 *
 * @param u Previously sampled u coordinate.
 * @param phiU Normalized relative weight for U-axis.
 * @param phiV Normalized relative weight for V-axis.
 * @param rand Random value used for sampling V coordinate.
 *
 * @return Returns sampled V coordinate.
 */
float sampleBarycentricV(float u, float phiU, float phiV, float rand)
{
    static constexpr auto EPSILON{ std::numeric_limits<float>::epsilon() };

    if (std::fabs(phiV) < EPSILON)
    { return (1.0f - u) * rand; }

    const auto tau{ 1.0f / 3.0f - (1.0f + (u - 1.0f / 3.0f) * phiU) / phiV };
    const auto tmp{ tau + u - 1.0f };
    const auto q{ std::sqrt(tau * tau * (1.0f - rand) + tmp * tmp * rand) };

    return tau <= 0.5f * (1.0f - u) ? tau + q : tau - q;
}

Vector3D triangleWeights(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3, const Vector3D &phi)
{
    /*
     * Source: Portsmouth, Jamie. "Efficient barycentric point sampling on meshes."
     */

    // Recover densities at each of the triangle vertices.
    const auto phi1{ phi.x };
    const auto phi2{ phi.y };
    const auto phi3{ phi.z };
    const auto averagePhi{ (phi1 + phi2 + phi3) / 3.0f};

    // Transform absolute distances to normalized relative weights.
    const auto relativePhi1{ (phi1 - phi3) / averagePhi };
    const auto relativePhi2{ (phi2 - phi3) / averagePhi };

    // Get independent random samples from uniform distribution.
    const auto randU{ treeutil::uniformZeroToOne<float>() };
    const auto randV{ treeutil::uniformZeroToOne<float>() };

    // Sample barycentric coordinates.
    const auto u{ sampleBarycentricU(relativePhi1, relativePhi2, randU) };
    const auto v{ sampleBarycentricV(u, relativePhi1, relativePhi2, randV) };
    const auto w{ 1.0f - u - v };

    return { u, v, w };
}

Vector3D uniformTriangleWeights(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{ return triangleWeights(p1, p2, p3, { 1.0f, 1.0f, 1.0f }); }

Vector3D medianTriangleWeights(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3)
{
    // Median - center of gravity of input triangle.
    const auto median{ calculateTriangleMedianPoint(p1, p2, p3) };

    // Calculate distances median <-> point.
    const auto phi1{ (p1 - median).length() };
    const auto phi2{ (p2 - median).length() };
    const auto phi3{ (p3 - median).length() };

    return triangleWeights(p1, p2, p3, { phi1, phi2, phi3 });
}

Vertex3D uniformSampleTriangle(const Vertex3D &v1, const Vertex3D &v2, const Vertex3D &v3)
{
    const auto barycentricCoordinates{ uniformTriangleWeights(v1.p, v2.p, v3.p) };

    const auto p{ sampleTriangle(v1.p, v2.p, v3.p, barycentricCoordinates) };
    auto n{ sampleTriangle(v1.n, v2.n, v3.n, barycentricCoordinates) };
    n.normalize();

    return { p, n };
}

Vertex3D uniformSampleTriangle(const Vertex3D &v1, const Vertex3D &v2, const Vertex3D &v3, const Vector3D &weights)
{
    const auto barycentricCoordinates{ triangleWeights(v1.p, v2.p, v3.p, weights) };

    const auto p{ sampleTriangle(v1.p, v2.p, v3.p, barycentricCoordinates) };
    auto n{ sampleTriangle(v1.n, v2.n, v3.n, barycentricCoordinates) };
    n.normalize();

    return { p, n };
}

Vector3D middleSampleTriangle(const Vector3D &a1, const Vector3D &a2, const Vector3D &a3)
{ return sampleTriangle(a1, a2, a3, Vector3D{ 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 3.0f }); }

Vector3D triangleNormal(const Vector3D &a1, const Vector3D &a2, const Vector3D &a3)
{
    const auto f{ a2 - a1 }; const auto s{ a3 - a1 };
    return Vector3D{
        f.y * s.z - f.z * s.y,
        f.z * s.x - f.x * s.z,
        f.x * s.y - f.y * s.x
    };
}

Triangle::Triangle(std::size_t idx,
    const treeio::ModelImporterBase::Vertex *vb,
    const treeio::ModelImporterBase::IndexElementT *ib):
    tIdx{ idx }
{
    const auto triangleBaseIndex{ idx * 3u };

    i1 = ib[triangleBaseIndex + 0u];
    i2 = ib[triangleBaseIndex + 1u];
    i3 = ib[triangleBaseIndex + 2u];

    v1 = modelVertexToCloudPoint(vb[i1]);
    v2 = modelVertexToCloudPoint(vb[i2]);
    v3 = modelVertexToCloudPoint(vb[i3]);
    lv = Vector3D{
        std::min<float>(std::min<float>(v1.x, v2.x), v3.x),
        std::min<float>(std::min<float>(v1.y, v2.y), v3.y),
        std::min<float>(std::min<float>(v1.z, v2.z), v3.z),
    };
    hv = Vector3D{
        std::max<float>(std::max<float>(v1.x, v2.x), v3.x),
        std::max<float>(std::max<float>(v1.y, v2.y), v3.y),
        std::max<float>(std::max<float>(v1.z, v2.z), v3.z),
    };
    c = middleSampleTriangle(v1, v2, v3);

    n1 = modelNormalToCloudNormal(vb[i1]);
    n2 = modelNormalToCloudNormal(vb[i2]);
    n3 = modelNormalToCloudNormal(vb[i3]);
}

float Triangle::centroidDistance(const Triangle &other) const
{ return Vector3D::distance(c, other.c); }

float Triangle::centroidSquaredDistance(const Triangle &other) const
{ return Vector3D::squaredDistance(c, other.c); }

float Triangle::centroidSquaredDistance(const Vector3D &point) const
{ return Vector3D::squaredDistance(c, point); }

bool Triangle::intersects(const Triangle &other) const
{
    // Vertices of this triangle:
    double v11[3]{ v1.x, v1.y, v1.z };
    double v12[3]{ v2.x, v2.y, v2.z };
    double v13[3]{ v3.x, v3.y, v3.z };

    // Vertices of the other triangle:
    double v21[3]{ other.v1.x, other.v1.y, other.v1.z };
    double v22[3]{ other.v2.x, other.v2.y, other.v2.z };
    double v23[3]{ other.v3.x, other.v3.y, other.v3.z };

    const auto result{ tri_tri_overlap_test_3d(
        v11, v12, v13,
        v21, v22, v23
    ) };

    return result != 0;
}

bool Triangle::intersects(const Ray &ray) const
{ return intersection(ray) < std::numeric_limits<float>::max(); }

float Triangle::intersection(const Ray &ray) const
{
    // Constants:
    static constexpr auto EPSILON{ std::numeric_limits<float>::epsilon() };

    // Moller Trumbore algorithm:
    const auto edge1{ v2 - v1 };
    const auto edge2{ v3 - v1 };
    const auto cross{ Vector3D::crossProduct(ray.direction, edge2) };
    const auto det{ Vector3D::dotProduct(edge1, cross) };

    // Ray is parallel to the triangle.
    /*
    std::cout << "v1: " << v1 << " v2: " << v2 << " v3: " << v2 << std::endl;
    std::cout << "edg1: " << edge1 << std::endl;
    std::cout << "edg2: " << edge2 << std::endl;
    std::cout << "cross: " << cross << std::endl;
    std::cout << "det: " << det << std::endl;
     */
    if (std::abs<float>(det) < EPSILON)
    { return std::numeric_limits<float>::max(); }

    const auto invDet{ 1.0f / det };
    const auto uVec{ ray.origin - v1 };
    const auto vVec{ Vector3D::crossProduct(uVec, edge1) };

    // Check intersection point is within triangle.
    const auto u{ invDet * Vector3D::dotProduct(uVec, cross) };
    //std::cout << "u: " << u << std::endl;
    if (u < -EPSILON || u > 1.0f + EPSILON)
    { return std::numeric_limits<float>::max(); }
    const auto v{ invDet * Vector3D::dotProduct(ray.direction, vVec) };
    //std::cout << "v " << v << std::endl;
    if (v < -EPSILON || u + v > 1.0f + EPSILON)
    { return std::numeric_limits<float>::max(); }

    // Calculate intersection time.
    const auto t{ invDet * Vector3D::dotProduct(edge2, vVec) };
    return t;
}

bool Triangle::contains(const Vector3D &point, float epsilon) const
{
#if 0
    /*
     * Using volume of a tetrahedron v1, v2, v3, point:
     *
     * 1) Calculate volume.
     * 2) Point is inside <-> volume < epsilon
     */
    return std::abs<float>(
        Vector3D::dotProduct(
            v1 - point,
            Vector3D::crossProduct(v2 - point, v3 - point)
        )
    ) < epsilon;

#else

    /*
     * Using barycentric coordinates:
     *
     * 1) Project point onto the triangle plane and get its distance -> distance.
     * 2) Calculate barycentrics on the 2D plane -> alpha, beta.
     * 3) Point is inside <-> distance < epsilon && alpha > -epsilon && beta > -epsilon && alpha + beta <= 1 + epsilon
     */

    const auto pn{ centralPlaneNormal() };
    const auto distance{ Vector3D::dotProduct(pn, point) };
    const auto projected{ pn * distance };

    const auto s2{ v2 - v1 };
    const auto s3{ v3 - v1 };
    const auto sp{ point - v1 };

    const auto d2p{ Vector3D::dotProduct(s2, sp) };
    const auto d3p{ Vector3D::dotProduct(s2, sp) };
    const auto dsh{ Vector3D::dotProduct(s2, s3) };
    const auto s2l{ s2.length() };
    const auto s3l{ s3.length() };
    // Normalize for alpha and beta to be 1.0 when reaching v2 and v3 respectively.
    const auto normalizer{ 1.0f / (d2p * d3p - dsh * dsh) };
    // Alpha represents distance along s2.
    const auto alpha{ (d2p * s3l - dsh * d3p) * normalizer };
    const auto beta{ (d3p * s2l - dsh * d2p) * normalizer };

    return alpha > -epsilon && beta > -epsilon && alpha + beta <= 1.0f + epsilon;
#endif
}

Vector3D Triangle::barycentrics(const Vector3D &point) const
{
    // Source: https://en.wikipedia.org/wiki/Barycentric_coordinate_system#Conversion_between_barycentric_and_Cartesian_coordinates
    const auto &p{ point };
    const auto b1{
        ((v2.y - v3.y) * (p.x - v3.x) + (v3.x - v2.x) * (p.y - v3.y)) /
        ((v2.y - v3.y) * (v1.x - v3.x) + (v3.x - v2.x) * (v1.y - v3.y))
    };
    const auto b2{
        ((v3.y - v1.y) * (p.x - v3.x) + (v1.x - v3.x) * (p.y - v3.y)) /
        ((v2.y - v3.y) * (v1.x - v3.x) + (v3.x - v2.x) * (v1.y - v3.y))
    };
    const auto b3{ 1.0f - b1 - b2 };

    return { b1, b2, b3 };
}

BoundingBox3D Triangle::aabb() const
{ return BoundingBox3D(lv, hv); }

float Triangle::area() const
{ return calculateTriangleArea(v1, v2, v3); }

float Triangle::perimeter() const
{ return calculateTrianglePerimeter(v1, v2, v3); }

float Triangle::longestAltitude() const
{ return calculateTriangleLongestMedian(v1, v2, v3); }

Vector3D Triangle::centralPlaneNormal() const
{
    /*
     * Use cross product of two sides:
     *    3
     *  /   \
     * 1 --- 2
     * Counter clock-wise -> front-face.
     * normal = cross(1->2, 1->3)
     */
    const auto c1{ v2 - v1 };
    const auto c2{ v3 - v1 };
    auto planeNormal{ Vector3D::crossProduct(c1, c2) };
    planeNormal.normalize();

    return planeNormal;
}

void Triangle::providePlaneNormals(const Vector3D &pNormal1, const Vector3D &pNormal2, const Vector3D &pNormal3)
{
    providedPlaneNormals = true;

    pn1 = pNormal1;
    pn2 = pNormal2;
    pn3 = pNormal3;

    const auto l1{ Line::fromDirection(v1, pn1) };
    const auto l2{ Line::fromDirection(v2, pn2) };
    const auto l3{ Line::fromDirection(v3, pn3) };

    const auto i12{ lsIntersect({ l1, l2 }) };
    const auto i13{ lsIntersect({ l1, l3 }) };
    const auto i23{ lsIntersect({ l2, l3 }) };

    //d12 = Vector3D::distance(v1 + (v2 - v1) * Vector3D::dotProduct((v2 - v1), (i12 - v1)), i12);
    //d13 = Vector3D::distance(v1 + (v3 - v1) * Vector3D::dotProduct((v3 - v1), (i13 - v1)), i13);
    //d23 = Vector3D::distance(v2 + (v3 - v2) * Vector3D::dotProduct((v3 - v2), (i23 - v2)), i13);

    pr12 = (Vector3D::distance(i12, v1) + Vector3D::distance(i12, v2)) / 2.0f;
    pr13 = (Vector3D::distance(i13, v1) + Vector3D::distance(i13, v3)) / 2.0f;
    pr23 = (Vector3D::distance(i23, v2) + Vector3D::distance(i23, v3)) / 2.0f;
}

Vector3D Triangle::smoothPoint(const Vector3D &point) const
{
    if (!providedPlaneNormals)
    { return point; }

    const auto bary{ barycentrics(point) };
    const auto shiftedBary{ bary - Vector3D{ 0.5f, 0.5f, 0.5f } };
    const auto radiusWeights{ Vector3D{ 1.0f, 1.0f, 1.0f } - 4.0f * (shiftedBary * shiftedBary) };

    return point + centralPlaneNormal() * radiusWeights * Vector3D{ pr12, pr13, pr23 };
}

float Triangle::curvature() const
{
    // TODO - Better curvature metric?
    return (Vector3D::dotProduct(pn1, pn2) + Vector3D::dotProduct(pn2, pn3) + Vector3D::dotProduct(pn3, pn1)) / 3.0f;
}

Vector3D Triangle::curvatureWeights() const
{
    const auto cpn{ centralPlaneNormal() };

    // Calculate weights as offset from central plane normal.
    const auto w1{ std::abs(Vector3D::dotProduct(cpn, pn1)) };
    const auto w2{ std::abs(Vector3D::dotProduct(cpn, pn2)) };
    const auto w3{ std::abs(Vector3D::dotProduct(cpn, pn3)) };

    // Fallback to uniform if we don't have valid values.
    const auto weightSum{ w1 + w2 + w3};
    if (weightSum <= std::numeric_limits<float>::epsilon() || !std::isfinite(weightSum))
    { return { 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 3.0f }; }

    // Normalize weights to be in <0.0f, 1.0f> and sum to 1.0f .
    const auto normalizer{ 1.0f / (w1 + w2 + w3) };
    return { w1 * normalizer, w2 * normalizer, w3 * normalizer };
}

float Triangle::longestSide() const
{
    return std::max<float>(
        std::max<float>(
            v1.length(),
            v2.length()),
        v3.length()
    );
}

Vector3D lsIntersect(const std::vector<Line> &lines)
{
    // Initialize accumulation variables:

    // Coefficients of the left sides.
    Eigen::Matrix3f A{ Eigen::Matrix3f::Zero() };
    // Coefficients of the right sides.
    Eigen::Vector3f b{ Eigen::Vector3f::Zero() };
    // Constants.
    float c{ 0.0f };

    //std::cout << "Starting lsIntersect..." << std::endl;

    // Populate the coefficient matrices:
    for (const auto &line : lines)
    { // Process each line in turn.
        // Recover points on the line.
        const Eigen::Vector3f v{ line.v.x, line.v.y, line.v.z };
        const Eigen::Vector3f w{ line.w.x, line.w.y, line.w.z };

        //std::cout << "Adding line: " << v.transpose() << " -> " << w.transpose() << std::endl;

        // Calculate line direction.
        const Eigen::Vector3f u{ (v - w).normalized() };
        const Eigen::Vector3f ut{ u.transpose() };

        // Calculate projection to hyper-plane orthogonal to this line.
        Eigen::Matrix3f kroneckerProduct{ };
        kroneckerProduct <<
                         u(0) * ut(0), u(0) * ut(1), u(0) * ut(2),
            u(1) * ut(0), u(1) * ut(1), u(1) * ut(2),
            u(2) * ut(0), u(2) * ut(1), u(2) * ut(2);
        const Eigen::Matrix3f projection{ Eigen::Matrix3f::Identity() - kroneckerProduct };

        // Calculate distance of our line to the projection plane.
        const Eigen::Vector3f p{ projection * w };

        // Populate coefficients of the least-squares system of equations.
        A += projection;
        b += p;
        c += p(0) * p(0) + p(1) * p(1) + p(2) * p(2);
    }

    // Solve least-squares problem.
    //const Eigen::Vector3f result{ A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b) };
    const Eigen::Vector3f result{ A.bdcSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b) };

    //std::cout << "LS intersection result: " << result.transpose() << std::endl;

    return { result(0), result(1), result(2) };
}

Vector3D lsIntersect(std::initializer_list<Line> lines)
{ return lsIntersect({ lines.begin(), lines.end() }); }

} // namespace treeacc


/***********************************************************************************************************************
 *  OpenStudio(R), Copyright (c) 2008-2017, Alliance for Sustainable Energy, LLC. All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *  following conditions are met:
 *
 *  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *  disclaimer.
 *
 *  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *  following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 *  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote
 *  products derived from this software without specific prior written permission from the respective party.
 *
 *  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative
 *  works may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without
 *  specific prior written permission from Alliance for Sustainable Energy, LLC.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER, THE UNITED STATES GOVERNMENT, OR ANY CONTRIBUTORS BE LIABLE FOR
 *  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **********************************************************************************************************************/

#ifndef UTILITIES_GEOMETRY_VECTOR3D_HPP
#define UTILITIES_GEOMETRY_VECTOR3D_HPP

#include "../UtilitiesAPI.hpp"
#include "../data/Vector.hpp"
#include "../core/Logger.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  class UTILITIES_API Vector3d{
  public:

    /// default constructor creates vector with 0, 0, 0
    Vector3d();

    /// constructor with x, y, z
    Vector3d(double x, double y, double z);

    /// copy constructor
    Vector3d(const Vector3d& other);

    /// get x
    double x() const;

    /// get y
    double y() const;

    /// get z
    double z() const;

    /// addition
    Vector3d operator+(const Vector3d& other) const;

    /// addition
    Vector3d& operator+=(const Vector3d& other);

    /// subtraction
    Vector3d operator-(const Vector3d& other) const;

    /// subtraction
    Vector3d& operator-=(const Vector3d& other);

    /// check equality
    bool operator==(const Vector3d& other) const;

    /// normalize to one
    bool normalize();

    /// get a vector which is the reverse of this
    Vector3d reverseVector() const;

    /// get length
    double length() const;

    /// set length
    bool setLength(double newLength);

    /// dot product with another Vector3d
    double dot(const Vector3d& other) const;

    /// cross product with another Vector3d
    Vector3d cross(const Vector3d& other) const;

    /// get the Vector directly
    Vector vector() const;

  private:

    REGISTER_LOGGER("utilities.Vector3d");

    Vector m_storage;

  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const Vector3d& vec);

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const std::vector<Vector3d>& vecVector);

  /// negation
  UTILITIES_API Vector3d operator-(const Vector3d& vec);

  /// multiplication by a scalar
  UTILITIES_API Vector3d operator*(double mult, const Vector3d& vec);

  // optional Vector3d
  typedef boost::optional<Vector3d> OptionalVector3d;

  // vector of Vector3d
  typedef std::vector<Vector3d> Vector3dVector;

} // openstudio

#endif //UTILITIES_GEOMETRY_VECTOR3D_HPP


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

#include <iostream>
#include <utility>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void init(Matrix& A)
{
    A= 0.0;
    mtl::mat::inserter<Matrix>  ins(A);

    ins[0][1] << 3;  ins[1][4] << 7; ins[0][0] << 1; ins[4][4] << 17;
    ins[2][3] << -2; ins[2][4] << 5; ins[4][0] << 2; ins[4][1] <<  3;
    ins[3][2] << 4;
}

template <typename Coll>
void test(Coll& coll, const char* name)
{
    cout << "\n" << name << " =\n" << coll << "\n";

    Coll E(coll), F(coll); F=0;
    E= hessenberg(coll);
    F=tril(E,-2);
    std::cout<< "Hessenberg=\n" << E << "\n";
    std::cout<< "triu=\n" << F << "\n";

    MTL_THROW_IF(one_norm(F)>0.000004, mtl::runtime_error("No Hessenberg-Form"));
    F=extract_householder_hessenberg(coll);
    std::cout<< "extract_householder_hessenberg=\n" << F << "\n";
    F=extract_hessenberg(coll);
    std::cout<< "extract_hessenberg=\n" << F << "\n";
    F=householder_hessenberg(coll);
    std::cout<< "householder_hessenberg=\n" << F << "\n";
    F=hessenberg_factors(coll);
    std::cout<< "hessenberg_factors=\n" << F << "\n";
    //      F=hessenberg_q(coll);
    //      std::cout<< "hessenberg_q=\n" << F << "\n";
}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double>       A(5, 5);
    init(A);
    compressed2D<double>  B(5, 5);
    init(B);

    test(A, "dense matrix");
//      test(B, "sparse matrix");  // TODO sparse Form

    return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example program shows you how to create your own custom binary classification
    trainer object and use it with the multiclass classification tools in the dlib C++
    library.  This example assumes you have already become familiar with the concepts
    introduced in the multiclass_classification_ex.cpp example program.

    In this example we will create a very simple trainer object that takes a binary
    classification problem and produces a decision rule which says a test point has the
    same class as whichever centroid it is closest to.

    The multiclass training dataset will consist of four classes.  Each class will be a blob
    of points in one of the quadrants of the cartesian plane.   For fun, we will use
    std::string labels and therefore the labels of these classes will be the following:
        "upper_left",
        "upper_right",
        "lower_left",
        "lower_right"
*/

#include <dlib/svm_threaded.h>

#include <iostream>
#include <vector>

#include <dlib/rand.h>

using namespace std;
using namespace dlib;

// Our data will be 2-dimensional data. So declare an appropriate type to contain these points.
typedef matrix<double,2,1> sample_type;

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

struct custom_decision_function
{
    /*!
        WHAT THIS OBJECT REPRESENTS
            This object is the representation of our binary decision rule.
    !*/

    // centers of the two classes
    sample_type positive_center, negative_center;

    double operator() (
        const sample_type& x
    ) const
    {
        // if x is closer to the positive class then return +1
        if (length(positive_center - x) < length(negative_center - x))
            return +1;
        else
            return -1;
    }
};

// Later on in this example we will save our decision functions to disk.  This
// pair of routines is needed for this functionality.
void serialize (const custom_decision_function& item, std::ostream& out)
{
    // write the state of item to the output stream
    serialize(item.positive_center, out);
    serialize(item.negative_center, out);
}

void deserialize (custom_decision_function& item, std::istream& in)
{
    // read the data from the input stream and store it in item
    deserialize(item.positive_center, in);
    deserialize(item.negative_center, in);
}

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

class simple_custom_trainer
{
    /*!
        WHAT THIS OBJECT REPRESENTS
            This is our example custom binary classifier trainer object.  It simply
            computes the means of the +1 and -1 classes, puts them into our
            custom_decision_function, and returns the results.

            Below we define the train() function.  I have also included the
            requires/ensures definition for a generic binary classifier's train()
    !*/
public:

    custom_decision_function train (
        const std::vector<sample_type>& samples,
        const std::vector<double>& labels
    ) const
    /*!
        requires
            - is_binary_classification_problem(samples, labels) == true
              (e.g. labels consists of only +1 and -1 values, samples.size() == labels.size())
        ensures
            - returns a decision function F with the following properties:
                - if (new_x is a sample predicted have +1 label) then
                    - F(new_x) >= 0
                - else
                    - F(new_x) < 0
    !*/
    {
        sample_type positive_center, negative_center;

        // compute sums of each class
        positive_center = 0;
        negative_center = 0;
        for (unsigned long i = 0; i < samples.size(); ++i)
        {
            if (labels[i] == +1)
                positive_center += samples[i];
            else // this is a -1 sample
                negative_center += samples[i];
        }

        // divide by number of +1 samples
        positive_center /= sum(mat(labels) == +1);
        // divide by number of -1 samples
        negative_center /= sum(mat(labels) == -1);

        custom_decision_function df;
        df.positive_center = positive_center;
        df.negative_center = negative_center;

        return df;
    }
};

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

void generate_data (
    std::vector<sample_type>& samples,
    std::vector<string>& labels
);
/*!
    ensures
        - make some four class data as described above.
        - each class will have 50 samples in it
!*/

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

int main()
{
    std::vector<sample_type> samples;
    std::vector<string> labels;

    // First, get our labeled set of training data
    generate_data(samples, labels);

    cout << "samples.size(): "<< samples.size() << endl;

    // Define the trainer we will use.  The second template argument specifies the type
    // of label used, which is string in this case.
    typedef one_vs_one_trainer<any_trainer<sample_type>, string> ovo_trainer;

    ovo_trainer trainer;

    // Now tell the one_vs_one_trainer that, by default, it should use the simple_custom_trainer
    // to solve the individual binary classification subproblems.
    trainer.set_trainer(simple_custom_trainer());

    // Next, to make things a little more interesting, we will setup the one_vs_one_trainer
    // to use kernel ridge regression to solve the upper_left vs lower_right binary classification
    // subproblem.
    typedef radial_basis_kernel<sample_type> rbf_kernel;
    krr_trainer<rbf_kernel> rbf_trainer;
    rbf_trainer.set_kernel(rbf_kernel(0.1));
    trainer.set_trainer(rbf_trainer, "upper_left", "lower_right");

    // Now let's do 5-fold cross-validation using the one_vs_one_trainer we just setup.
    // As an aside, always shuffle the order of the samples before doing cross validation.
    // For a discussion of why this is a good idea see the svm_ex.cpp example.
    randomize_samples(samples, labels);
    cout << "cross validation: \n" << cross_validate_multiclass_trainer(trainer, samples, labels, 5) << endl;
    // This dataset is very easy and everything is correctly classified.  Therefore, the output of
    // cross validation is the following confusion matrix.
    /*
        50  0  0  0
         0 50  0  0
         0  0 50  0
         0  0  0 50
    */

    // We can also obtain the decision rule as always.
    one_vs_one_decision_function<ovo_trainer> df = trainer.train(samples, labels);

    cout << "predicted label: "<< df(samples[0])  << ", true label: "<< labels[0] << endl;
    cout << "predicted label: "<< df(samples[90]) << ", true label: "<< labels[90] << endl;
    // The output is:
    /*
        predicted label: upper_right, true label: upper_right
        predicted label: lower_left, true label: lower_left
    */

    // Finally, let's save our multiclass decision rule to disk.  Remember that we have
    // to specify the types of binary decision function used inside the one_vs_one_decision_function.
    one_vs_one_decision_function<ovo_trainer,
            custom_decision_function,                             // This is the output of the simple_custom_trainer
            decision_function<radial_basis_kernel<sample_type> >  // This is the output of the rbf_trainer
        > df2, df3;

    df2 = df;
    // save to a file called df.dat
    serialize("df.dat") << df2;

    // load the function back in from disk and store it in df3.
    deserialize("df.dat") >> df3;

    // Test df3 to see that this worked.
    cout << endl;
    cout << "predicted label: "<< df3(samples[0])  << ", true label: "<< labels[0] << endl;
    cout << "predicted label: "<< df3(samples[90]) << ", true label: "<< labels[90] << endl;
    // Test df3 on the samples and labels and print the confusion matrix.
    cout << "test deserialized function: \n" << test_multiclass_decision_function(df3, samples, labels) << endl;

}

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

void generate_data (
    std::vector<sample_type>& samples,
    std::vector<string>& labels
)
{
    const long num = 50;

    sample_type m;

    dlib::rand rnd;

    // add some points in the upper right quadrant
    m = 10, 10;
    for (long i = 0; i < num; ++i)
    {
        samples.push_back(m + randm(2,1,rnd));
        labels.push_back("upper_right");
    }

    // add some points in the upper left quadrant
    m = -10, 10;
    for (long i = 0; i < num; ++i)
    {
        samples.push_back(m + randm(2,1,rnd));
        labels.push_back("upper_left");
    }

    // add some points in the lower right quadrant
    m = 10, -10;
    for (long i = 0; i < num; ++i)
    {
        samples.push_back(m + randm(2,1,rnd));
        labels.push_back("lower_right");
    }

    // add some points in the lower left quadrant
    m = -10, -10;
    for (long i = 0; i < num; ++i)
    {
        samples.push_back(m + randm(2,1,rnd));
        labels.push_back("lower_left");
    }

}

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


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2018 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Clock.h>
#include <gudhi/Points_off_io.h>

#include <boost/graph/adjacency_list.hpp>

#include <iostream>
#include <string>
#include <vector>
#include <limits>  // for numeric limits
#include <fstream>
#include <cassert>

std::ofstream results_csv("results.csv");

template< typename Adjacency_list, typename ForwardPointRange, typename Distance >
Adjacency_list proximity_graph_computation(const ForwardPointRange& points, double threshold, Distance distance) {
  std::vector<std::pair< int, int >> edges;
  std::vector< double > edges_fil;
  std::map< int, double > vertices;

  int idx_u, idx_v;
  double fil;
  idx_u = 0;
  for (auto it_u = points.begin(); it_u != points.end(); ++it_u) {
    idx_v = idx_u + 1;
    for (auto it_v = it_u + 1; it_v != points.end(); ++it_v, ++idx_v) {
      fil = distance(*it_u, *it_v);
      if (fil <= threshold) {
        edges.emplace_back(idx_u, idx_v);
        edges_fil.push_back(fil);
      }
    }
    ++idx_u;
  }

  // Points are labeled from 0 to idx_u-1
  Adjacency_list skel_graph(edges.begin(), edges.end(), edges_fil.begin(), idx_u);

  auto vertex_prop = boost::get(Gudhi::vertex_filtration_t(), skel_graph);

  typename boost::graph_traits<Adjacency_list>::vertex_iterator vi, vi_end;
  for (std::tie(vi, vi_end) = boost::vertices(skel_graph);
       vi != vi_end; ++vi) {
    boost::put(vertex_prop, *vi, 0.);
  }

  return skel_graph;
}

template <typename Adjacency_list>
void benchmark_proximity_graph(const std::string& msg, const std::string& off_file_name) {
  Gudhi::Points_off_reader<std::vector<double>> off_reader(off_file_name);
  assert(off_reader.is_valid());

  std::cout << "+ " << msg << std::endl;

  results_csv << "\"nb_points\";"
              << "\"nb_simplices\";"
              << "\"compute proximity graph(sec.)\";"
              << "\"complex_creation_time(sec.)\";"
              << "\"" << msg << "\";" << std::endl;

  Gudhi::Clock pg_compute_proximity_graph("    benchmark_proximity_graph - compute proximity graph");
  pg_compute_proximity_graph.begin();
  // benchmark begin
  Adjacency_list proximity_graph = proximity_graph_computation<Adjacency_list>(off_reader.get_point_cloud(),
                                                                               std::numeric_limits<double>::infinity(),
                                                                               Gudhi::Euclidean_distance());
  // benchmark end
  pg_compute_proximity_graph.end();
  std::cout << pg_compute_proximity_graph;

  Gudhi::Simplex_tree<> complex;
  Gudhi::Clock st_create_clock("    benchmark_proximity_graph - complex creation");
  st_create_clock.begin();
  // benchmark begin
  complex.insert_graph(proximity_graph);
  // benchmark end
  st_create_clock.end();
  std::cout << st_create_clock;

  results_csv << off_reader.get_point_cloud().size() << ";" << complex.num_simplices() << ";"
              << pg_compute_proximity_graph.num_seconds() << ";"
              << st_create_clock.num_seconds() << ";" << std::endl;

  std::cout << "    benchmark_proximity_graph - nb simplices = " << complex.num_simplices() << std::endl;
}

int main(int argc, char * const argv[]) {
  std::string off_file_name(argv[1]);

  // The fastest, the less memory used
  using vecSdirectedS = boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                                              boost::property<Gudhi::vertex_filtration_t, double>,
                                              boost::property<Gudhi::edge_filtration_t, double>>;
  benchmark_proximity_graph<vecSdirectedS>("vecSdirectedS", off_file_name);

  using vecSundirectedS = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                                                boost::property<Gudhi::vertex_filtration_t, double>,
                                                boost::property<Gudhi::edge_filtration_t, double>>;
  benchmark_proximity_graph<vecSundirectedS>("vecSundirectedS", off_file_name);

  using vecSbidirectionalS = boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
                                                   boost::property<Gudhi::vertex_filtration_t, double>,
                                                   boost::property<Gudhi::edge_filtration_t, double>>;
  benchmark_proximity_graph<vecSbidirectionalS>("vecSbidirectionalS", off_file_name);

  using setSdirectedS = boost::adjacency_list<boost::setS, boost::vecS, boost::directedS,
                                              boost::property<Gudhi::vertex_filtration_t, double>,
                                              boost::property<Gudhi::edge_filtration_t, double>>;
  benchmark_proximity_graph<setSdirectedS>("setSdirectedS", off_file_name);

  using setSundirectedS = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                                boost::property<Gudhi::vertex_filtration_t, double>,
                                                boost::property<Gudhi::edge_filtration_t, double>>;
  benchmark_proximity_graph<setSundirectedS>("setSundirectedS", off_file_name);

  using setSbidirectionalS = boost::adjacency_list<boost::setS, boost::vecS, boost::bidirectionalS,
                                                   boost::property<Gudhi::vertex_filtration_t, double>,
                                                   boost::property<Gudhi::edge_filtration_t, double>>;
  benchmark_proximity_graph<setSbidirectionalS>("setSbidirectionalS", off_file_name);

  return 0;
}


#ifndef SHASTA_APPROXIMATE_COLORING_HPP
#define SHASTA_APPROXIMATE_COLORING_HPP

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

Approximate coloring of a Boost undirected graph using the Dsatur algorithm.

See this Wikipedia article for greedy coloring in general:
https://en.wikipedia.org/wiki/Greedy_coloring

This code uses the Brelaz (1979) Dsatur
algorithm described there in this section
https://en.wikipedia.org/wiki/Greedy_coloring#Adaptive

The main Wikipedia article for this algorithm is here:
https://en.wikipedia.org/wiki/DSatur

The Dsatur algorithm was ingtroduced in this paper:
Brelaz, Daniel (April 1979),
"New methods to color the vertices of a graph",
Communications of the ACM, 22 (4): 251–256, doi:10.1145/359094.359101

Many alternative approximate coloring methods are also available, for example
https://www.gerad.ca/~alainh/RLFPaper.pdf

For a partial coloring of a graph, the Dsatur algorithm defines
the saturation degree of a vertex as the number of distinct colors
to which the vertex is adjacent (only counting, of course,
vertices that have already been colored).
With this definition, the Dsatur algorithm is simply described
as follows:

- Starting with the uncolored graph, iterate over vertices in the order
  defined below.
- At each iteration, assign to each vertex the lowest possible color.
  This is the lowest color that the vertex is not adjacent to.
- At each iteration choose the vertex with maximum saturation degree
  (maximum number of adjacent colors). In case of ties, break
  the ties by selecting the vertex with maximum degree in the uncolored
  subgraph - that is, the vertex with the greatest number of uncolored
  adjacent vertices.

The DSatur algorithm is an approximate coloring algorithm, but it
is exact for 2-colorable graphs.

This function applies the above algorithm to a Boost undirected graph.
It assumes that the graph is defined as a
boost::adjacency_list<
    OutEdgeList,
    VertexList,
    Directed,
    VertexProperties, EdgeProperties, GraphProperties,
    EdgeList>
with:

- VertexList = boost::vecS (so vertex descriptors are integers starting at zero).
- Directed = boost::undirectedS.

It returns the computed coloring in its second argument, indexed
by vertex_descriptor.

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

// Shasta.
#include "SHASTA_ASSERT.hpp"

// Boost libraries.
#include <boost/container/flat_set.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/iteration_macros.hpp>

// Standard library.
#include "algorithm.hpp"
#include "cstdint.hpp"
#include <map>
#include <numeric>
#include "vector.hpp"

namespace shasta {
    template<class Graph> void approximateColoring(
        const Graph&,
        vector<uint64_t>&
    );
}

template<class Graph> void shasta::approximateColoring(
    const Graph& graph,
    vector<uint64_t>& color
)
{
    // Get the number of vertices and initialize the colors.
    const uint64_t n = boost::num_vertices(graph);
    color.resize(n);
    const uint64_t noColor = std::numeric_limits<uint64_t>::max();
    fill(color.begin(), color.end(), noColor);

    // Vector to contain the number of colored vertices adjacent to
    // each uncolored vertex. This is used to break ties in the Dsatur algorithm.
    // We only maintain this for uncolored vertices.
    // Once a vertex gets colored we stop updating this as it will never be needed.
    vector<uint64_t> adjacentColoredCount(n, 0);

    // The colors adjacent to each uncolored vertex. The number of colors adjacent
    // to each vertex is small, so we use boost::container::flat_set
    // instead of std::set.
    // We only maintain this for uncolored vertices.
    // Once a vertex gets colored we stop updating this as it will never be needed.
    using boost::container::flat_set;
    vector< flat_set<uint64_t> > adjacentColors(n);

    // A multimap that stores uncolored vertices
    // keyed by saturation degree (that is, the number of colors adjacent to
    // each vertex), in decreasing order.
    // This is used to select, at each iteration, the next vertex to be colored.
    using MultiMap = std::multimap<uint64_t, uint64_t, std::greater<uint64_t> >;
    MultiMap verticesBySaturationDegree;
    for(uint64_t v=0; v<n; v++) {
        verticesBySaturationDegree.insert(make_pair(0, v));
    }

    // Main iteration loop. At each iteration we color one vertex.
    for(uint64_t iteration=0; iteration<n; iteration++) {

        // To choose the vertex to color at this iteration, loop over
        // all uncolored vertices with maximum saturation degree.
        SHASTA_ASSERT(not verticesBySaturationDegree.empty());
        const uint64_t highestSaturationDegree = verticesBySaturationDegree.begin()->first;
        MultiMap::iterator begin, end;
        tie(begin, end) = verticesBySaturationDegree.equal_range(highestSaturationDegree);
        SHASTA_ASSERT(begin != end);
        uint64_t v0 = begin->second;
        uint64_t adjacentColoredCount0 = adjacentColoredCount[v0];
        MultiMap::iterator it0 = begin;
        for(MultiMap::iterator it=begin; it!=end; ++it) {
            const uint64_t v1 = it->second;
            if(adjacentColoredCount[v1] > adjacentColoredCount0) {
                v0 = v1;
                adjacentColoredCount0 = adjacentColoredCount[v1];
                it0 = it;
            }
        }

        // Remove v0 from verticesBySaturationDegree.
        verticesBySaturationDegree.erase(it0);

        // This iteration is coloring LocalVertexId v0.
        // We choose the lowest color that is not present in adjacentColors[v0].
        const auto& adjacentColors0 = adjacentColors[v0];
        uint64_t color0;
        for(uint64_t color=0; ; color++) {
            if(adjacentColors0.find(color) == adjacentColors0.end()) {
                color0 = color;
                break;
            }
        }

        // Color v0 with the color chosen in this way.
        color[v0] = color0;

        // Increment adjacentColoredCount for adjacent uncolored vertices.
        BGL_FORALL_ADJ_T(v0, v1, graph, Graph) {
            if(color[v1] == noColor) {
                adjacentColoredCount[v1]++;
            }
        }

        // Update adjacentColors for adjacent uncolored vertices, and
        // update verticesBySaturationDegree accordingly.
        BGL_FORALL_ADJ_T(v0, v1, graph, Graph) {
            if(color[v1] == noColor) {
                auto& adjacentColors1 = adjacentColors[v1];
                if(adjacentColors1.find(color0) == adjacentColors1.end()) {
                    adjacentColors1.insert(color0);

                    // The saturation degree of this vertex has increased.
                    MultiMap::iterator begin, end;
                    tie(begin, end) = verticesBySaturationDegree.equal_range(adjacentColors1.size()-1);
                    SHASTA_ASSERT(begin != end);
                    bool done = false;
                    for(MultiMap::iterator it=begin; it!=end; ++it) {
                        if(it->second == v1) {
                            verticesBySaturationDegree.erase(it);
                            verticesBySaturationDegree.insert(make_pair(adjacentColors1.size(), v1));
                            done = true;
                            break;
                        }
                    }
                    SHASTA_ASSERT(done);
                }
            }
        }
    }

    // Verify that we colored all the vertices.
    SHASTA_ASSERT(find(color.begin(), color.end(), noColor) == color.end());

}

#endif