#ifndef ZSVM_SPHERICAL_ECG_JACOBI_CONTEXT_HPP
#define ZSVM_SPHERICAL_ECG_JACOBI_CONTEXT_HPP

// C++ standard library headers
#include <cstddef> // for std::size_t
#include <vector>

// Eigen linear algebra library headers
#include <Eigen/Core>

// Project-specific headers
#include "Restrict.hpp"
#include "JacobiCoordinates.hpp"
#include "PackedLinearAlgebra.hpp"
#include "Particle.hpp"
#include "Permutation.hpp"

namespace zsvm {

    template <typename T>
    class SphericalECGJacobiContext {

    private: // =============================================== MEMBER VARIABLES

        typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXT;

        const std::size_t num_particles;
        const std::size_t num_pairs;
        const std::size_t num_permutations;
        const std::size_t matrix_size;
        const long long int space_dimension;
        const T dimension_factor;
        const T kinetic_factor;

        const std::vector<T> inverse_masses;
        const std::vector<T> weight_vectors;
        const std::vector<T> weight_matrices;
        const std::vector<T> charge_products;
        const std::vector<T> permutation_signs;
        const std::vector<T> permutation_matrices;

        std::vector<T> vax;
        std::vector<T> vbx;
        std::vector<T> vcx;
        std::vector<T> vdx;

        const packed_determinant_inverse_function<T> packed_determinant_inverse;
        const packed_kinetic_trace_function<T> packed_kinetic_trace;
        const packed_quadratic_form_function<T> packed_quadratic_form;
        const packed_permutation_conjugate_function<T> packed_permutation_conjugate;

    private: // ============================================ FACTORY CONSTRUCTOR

        T gamma(const T &x) {
            using std::tgamma;
            return tgamma(x);
        }

        explicit SphericalECGJacobiContext(
                const std::vector<T> &charges,
                std::size_t num_permutations,
                long long int space_dimension,
                const MatrixXT &inverse_mass_matrix,
                const MatrixXT &pairwise_weight_vectors,
                const std::vector<T> &permutation_sign_vector,
                const std::vector<MatrixXT> &permutation_matrix_vector)
                : num_particles(charges.size()),
                  num_pairs(num_particles * (num_particles - 1) / 2),
                  num_permutations(num_permutations),
                  matrix_size((num_particles - 1) * (num_particles - 1)),
                  space_dimension(space_dimension),
                  dimension_factor(gamma((space_dimension - static_cast<T>(1)) /
                                         static_cast<T>(2)) /
                                   gamma(space_dimension / static_cast<T>(2))),
                  kinetic_factor(space_dimension / (2 * dimension_factor)),
                  inverse_masses(num_particles - 1),
                  weight_vectors(num_pairs * (num_particles - 1)),
                  weight_matrices(num_pairs * num_pairs),
                  charge_products(num_pairs),
                  permutation_signs(num_permutations),
                  permutation_matrices(num_permutations * matrix_size),
                  vax(num_pairs),
                  vbx(num_pairs),
                  vcx(num_pairs),
                  vdx(num_pairs),
                  packed_determinant_inverse(
                          PackedDeterminantInverse<T>::functions()[
                                  num_particles - 2]),
                  packed_kinetic_trace(
                          PackedKineticTrace<T>::functions()[
                                  num_particles - 2]),
                  packed_quadratic_form(
                          PackedQuadraticForm<T>::functions()[
                                  num_particles - 2]),
                  packed_permutation_conjugate(
                          PackedPermutationConjugate<T>::functions()[
                                  num_particles - 2]) {
            auto inverse_mass_pointer = const_cast<T *>(inverse_masses.data());
            for (std::size_t i = 0; i < num_particles - 1; ++i) {
                inverse_mass_pointer[i] = inverse_mass_matrix(i, i);
            }
            auto weight_vector_pointer = const_cast<T *>(weight_vectors.data());
            for (std::size_t p = 0, k = 0; p < num_pairs; ++p) {
                for (std::size_t i = 0; i < num_particles - 1; ++i, ++k) {
                    weight_vector_pointer[k] = pairwise_weight_vectors(i, p);
                }
            }
            auto weight_matrix_pointer = const_cast<T *>(weight_matrices.data());
            for (std::size_t p = 0, k = 0; p < num_pairs; ++p) {
                for (std::size_t i = 0; i < num_particles - 1; ++i) {
                    for (std::size_t j = 0; j <= i; ++j, ++k) {
                        weight_matrix_pointer[k] =
                                pairwise_weight_vectors(i, p) *
                                pairwise_weight_vectors(j, p);
                    }
                }
            }
            auto charge_product_pointer = const_cast<T *>(charge_products.data());
            for (std::size_t i = 0, k = 0; i < num_particles - 1; ++i) {
                for (std::size_t j = i + 1; j < num_particles; ++j, ++k) {
                    charge_product_pointer[k] = charges[i] * charges[j];
                }
            }
            auto permutation_sign_pointer = const_cast<T *>(permutation_signs.data());
            for (std::size_t p = 0; p < num_permutations; ++p) {
                permutation_sign_pointer[p] = permutation_sign_vector[p];
            }
            auto permutation_matrix_pointer = const_cast<T *>(permutation_matrices.data());
            for (std::size_t p = 0, k = 0; p < num_permutations; ++p) {
                for (std::size_t i = 0; i < num_particles - 1; ++i) {
                    for (std::size_t j = 0; j < num_particles - 1; ++j, ++k) {
                        permutation_matrix_pointer[k] =
                                permutation_matrix_vector[p](j, i);
                    }
                }
            }
        }

    public: // =========================================== STATIC FACTORY METHOD

        static SphericalECGJacobiContext create(
                const std::vector<zsvm::Particle<T>> &particles,
                const std::string &mass_carrier,
                const std::string &charge_carrier,
                long long int space_dimension) {
            const std::size_t num_particles = particles.size();
            if (num_particles < 2) {
                throw std::invalid_argument(
                        "Attempted to construct SphericalECGJacobiContext "
                        "with fewer than 2 particles");
            }
            std::vector<T> masses(num_particles);
            std::vector<T> charges(num_particles);
            for (std::size_t i = 0; i < num_particles; ++i) {
                masses[i] = particles[i].carriers.at(mass_carrier);
                charges[i] = particles[i].carriers.at(charge_carrier);
            }
            const std::vector<std::vector<std::size_t>> allowed_permutations =
                    dznl::invariant_permutations(particles);
            std::vector<T> permutation_signs;
            for (const auto &permutation : allowed_permutations) {
                const std::size_t signature =
                        dznl::count_changes(particles, permutation) / 2 +
                        dznl::count_inversions(permutation);
                permutation_signs.push_back((signature % 2 == 0) ? +1 : -1);
            }
            return SphericalECGJacobiContext(
                    charges,
                    allowed_permutations.size(),
                    space_dimension,
                    jaco::reduced_inverse_mass_matrix(masses),
                    jaco::pairwise_weights(masses),
                    permutation_signs,
                    jaco::permutation_matrices(masses, allowed_permutations));
        }

    private: // ================================== MATRIX ELEMENT HELPER METHODS

        void matrix_element_kernel(
                T &RESTRICT overlap_kernel,
                T &RESTRICT hamiltonian_kernel) {
            using std::sqrt;
            T *RESTRICT const ax = vax.data();
            T *RESTRICT const bx = vbx.data();
            T *RESTRICT const cx = vcx.data();
            T *RESTRICT const dx = vdx.data();
            for (std::size_t i = 0; i < num_pairs; ++i) {
                cx[i] = ax[i] + bx[i];
            }
            overlap_kernel = half_inverse_pow(
                    packed_determinant_inverse(cx, dx), space_dimension);
            hamiltonian_kernel = kinetic_factor * packed_kinetic_trace(
                    ax, bx, dx, inverse_masses.data());
            for (std::size_t k = 0; k < num_pairs; ++k) {
                const T alpha = packed_quadratic_form(
                        dx, weight_vectors.data() + (num_particles - 1) * k);
                hamiltonian_kernel += charge_products[k] / sqrt(2 * alpha);
            }
            hamiltonian_kernel *= dimension_factor * overlap_kernel;
        }

    public: // ========================================== MATRIX ELEMENT METHODS

        void gaussian_parameter_matrix(
                const T *RESTRICT correlation_coefficients,
                T *RESTRICT result) const {
            using std::exp;
            for (std::size_t p = 0; p < num_pairs; ++p) { result[p] = 0; }
            for (std::size_t p = 0, k = 0; p < num_pairs; ++p) {
                const T c = exp(correlation_coefficients[p]);
                for (std::size_t q = 0; q < num_pairs; ++q, ++k) {
                    result[q] += c * weight_matrices[k];
                }
            }
        }

        void evaluate_matrix_elements(
                T &RESTRICT overlap_element,
                T &RESTRICT hamiltonian_element,
                const T *a, const T *b) {
            T *RESTRICT const ax = vax.data();
            T *RESTRICT const bx = vbx.data();
            overlap_element = hamiltonian_element = 0;
            T overlap_kernel, hamiltonian_kernel;
            for (std::size_t i = 0; i < num_permutations; ++i) {
                for (std::size_t j = 0; j < num_permutations; ++j) {
                    const T sign = permutation_signs[i] * permutation_signs[j];
                    packed_permutation_conjugate(
                            a, permutation_matrices.data() + i * matrix_size,
                            ax);
                    packed_permutation_conjugate(
                            b, permutation_matrices.data() + j * matrix_size,
                            bx);
                    matrix_element_kernel(overlap_kernel, hamiltonian_kernel);
                    overlap_element += sign * overlap_kernel;
                    hamiltonian_element += sign * hamiltonian_kernel;
                }
            }
        }

    }; // class SphericalECGJacobiContext

} // namespace zsvm

#endif // ZSVM_SPHERICAL_ECG_JACOBI_CONTEXT_HPP


#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/metric_tsp_approx.hpp>
#include "cPathFinder.h"
using namespace boost;

void cPathFinder::clear()
{
    myGraph.clear();
    myDirGraph.clear();
    myMaxNegCost = 0;
}

void cPathFinder::start(int start)
{
    myStart = start;
}
void cPathFinder::start(const std::string &start)
{
    myStart = find(start);
    if (myStart < 0)
        throw std::runtime_error("cPathFinder::bad start node");
}
int cPathFinder::start() const
{
    return myStart;
}

void cPathFinder::paths(int start)
{
    if (!myfDirected)
        pathsT(start, myGraph);
    else
        pathsT(start, myDirGraph);
}
template <typename T>
void cPathFinder::pathsT(int start, T &g)
{
    // run dijkstra algorithm
    myPred.clear();
    myDist.clear();
    myPred.resize(num_vertices(g));
    myDist.resize(num_vertices(g));
    dijkstra_shortest_paths(
        g,
        start,
        weight_map(get(&cEdge::myCost, g))
            .predecessor_map(boost::make_iterator_property_map(
                myPred.begin(), get(boost::vertex_index, g)))
            .distance_map(boost::make_iterator_property_map(
                myDist.begin(), get(boost::vertex_index, g))));
    // std::cout << "<-cPathFinder::path ";
}

void cPathFinder::path()
{
    if (myStart < 0)
        throw std::runtime_error("cPathFinder::path start node undefined");

    // run the Dijsktra algorithm
    paths(myStart);

    if (myEnd >= 0)

        // pick out the path from source to destination
        pathPick(myEnd);

    else
    {
        std::cout << "Hop count from root to every node:\n";
        for (int kv = 0; kv < num_vertices(myGraph); kv++)
        {
            if (kv == myStart)
                continue;
            pathPick(kv);
            //std::cout << pathText() << "\n";
            std::cout << nodeName(myStart) << " to "
                      << nodeName(kv) << " "
                      << myPath.size() - 1 << "\n";
        }
    }
}

int cPathFinder::distance(int end)
{
    if (0 > end || end >> (int)myDist.size())
        return -1;
    return myDist[end];
}

void cPathFinder::span()
{
    typedef graph_traits<graph_t>::edge_descriptor edge_t;
    std::vector<edge_t> spanning_tree;
    kruskal_minimum_spanning_tree(
        myGraph,
        std::back_inserter(spanning_tree),
        weight_map(get(&cEdge::myCost, myGraph)));

    std::cout << "spanning_tree " << spanning_tree.size() << "\n";
    mySpanCost = 0;
    mySpan.clear();
    for (auto e : spanning_tree)
    {
        mySpanCost += myGraph[e].myCost;
        std::vector<int> ve{
            (int)source(e, myGraph),
            (int)target(e, myGraph)};
        mySpan.push_back(ve);
    }
}

std::vector<int> cPathFinder::pathPick(int end)
{
    myPath.clear();
    // std::cout << "->cPathFinder::pathPick "
    //     << myStart <<" " << end << "\n";

    if (end < 0)
        throw std::runtime_error("cPathFinder::pathPick bad end node");
    if (myPred[end] == end)
        throw std::runtime_error("There is no path from " + std::to_string(myStart) + " to " + std::to_string(end));

    // pick out path, starting at goal and finishing at start
    myPath.push_back(end);
    int prev = end;
    while (1)
    {
        //std::cout << prev << " " << myPred[prev] << ", ";
        int next = myPred[prev];
        myPath.push_back(next);
        if (next == myStart)
            break;
        prev = next;
    }

    // reverse so path goes from start to goal
    std::reverse(myPath.begin(), myPath.end());

    return myPath;
}

void cPathFinder::tsp()
{
    if (myfDirected)
        throw std::runtime_error(
            "cPathFinder::tsp does not handle directed graphs");

    double len = 0; //length of the tour
    auto tour_visitor = make_tsp_tour_len_visitor(
        myGraph,
        std::back_inserter(myPath),
        len,
        get(&cEdge::myCost, myGraph));
    metric_tsp_approx(
        myGraph,
        get(&cEdge::myCost, myGraph),
        get(vertex_index, myGraph),
        tour_visitor);
    // metric_tsp_approx_from_vertex(
    //     myGraph,
    //     myStart,
    //     get(&cEdge::myCost, myGraph),
    //     get(vertex_index, myGraph),
    //     tour_visitor);
}

void cPathFinder::addLink(
    int u,
    int v,
    float cost)
{
    if (u < 0 || v < 0)
        throw std::runtime_error("cPathFinder::addLink bad node");
    if (!myfDirected)
        myGraph[add_edge(u, v, myGraph).first].myCost = cost;
    else
        myDirGraph[add_edge(u, v, myDirGraph).first].myCost = cost;
}

void cPathFinder::addLink(
    const std::string &su,
    const std::string &sv,
    float cost)
{
    addLink(
        findoradd(su),
        findoradd(sv),
        cost);
}

int cPathFinder::findoradd(const std::string &name)
{
    int n = find(name);
    if (n < 0)
        n = add_vertex(name, myGraph);
    return n;
}

int cPathFinder::addNode(const std::string &name)
{
    if (myfDirected)
        return add_vertex(name, myDirGraph);
    else
        return add_vertex(name, myGraph);
}

void cPathFinder::deleteNode(int n)
{
    if (myfDirected)
        return remove_vertex(n, myDirGraph);
    else
        return remove_vertex(n, myGraph);
}
int cPathFinder::find(const std::string &name)
{
    for (int n = 0; n < num_vertices(myGraph); n++)
    {
        //std::cout << myGraph[n].myName << " ";
        if (myGraph[n].myName == name)
        {
            return n;
        }
    }
    //std::cout << name << " not found\n";
    return -1;
}

int cPathFinder::nodeCount()
{
    if (!myfDirected)
        return num_vertices(myGraph);
    else
        return num_vertices(myDirGraph);
}
int cPathFinder::linkCount()
{
    if (!myfDirected)
        return num_edges(myGraph);
    else
        return num_edges(myDirGraph);
}

std::string cPathFinder::linksText()
{
    if (!myfDirected)
        return linksTextT(myGraph);
    else
        return linksTextT(myDirGraph);
}
template <typename T>
std::string cPathFinder::linksTextT(T &g)
{
    std::stringstream ss;
    typename graph_traits<T>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    {
        std::string un = g[source(*ei, g)].myName;
        if (un == "???")
            un = std::to_string(source(*ei, g));
        std::string vn = g[target(*ei, g)].myName;
        if (vn == "???")
            vn = std::to_string(target(*ei, g));
        ss << "("
           << un << ","
           << vn << ","
           << g[*ei].myCost
           << ") ";
    }
    ss << "\n";
    return ss.str();
}

bool cPathFinder::IsAdjacent(int u, int v)
{
    if (u < 0 || v < 0)
        return false;
    return edge(u, v, myGraph).second;
}

bool cPathFinder::IsConnected()
{
    return (1 == islandCount());
}
int cPathFinder::islandCount()
{
    std::vector<int> component(boost::num_vertices(myGraph));
    return boost::connected_components(myGraph, &component[0]);
}

std::string cPathFinder::nodeName(int n) const
{
    std::string sn = myGraph[n].myName;
    if (sn == "???")
        sn = std::to_string(n);
    return sn;
}

std::string cPathFinder::nodeColor(int n) const
{
    return myGraph[n].myColor;
}
void cPathFinder::nodeColor(int n, const std::string &color)
{
    if (myfDirected)
        myDirGraph[n].myColor = color;
    else
        myGraph[n].myColor = color;
}
std::string cPathFinder::pathText()
{
    std::stringstream ss;
    for (auto n : myPath)
    {
        std::string sn;
        if (myfDirected)
        {
            sn = myDirGraph[n].myName;
        }
        else
        {
            sn = myGraph[n].myName;
        }
        if (sn == "???")
            sn = std::to_string(n);
        ss << sn << " -> ";
        //ss << std::to_string(n) << " -> ";
    }

    if (myPath.size())
    {
        //std::cout << "dbg " << myDist[myPath.back()] << " " << myMaxNegCost << " " << myPath.size() << "\n";
        ss << " Cost is "
           << myDist[myPath.back()] + myMaxNegCost * (myPath.size() - 1)
           << "\n";
    }

    return ss.str();
}

std::string cPathFinder::spanText()
{
    // std::cout << "spanText " << mySpan.size()
    //           << " cost " << mySpanCost << "\n";

    std::stringstream ss;
    for (auto e : mySpan)
    {
        ss << nodeName(e[0])
           << " - "
           << nodeName(e[1])
           << ", ";
    }
    ss << " cost " << mySpanCost << "\n";
    return ss.str();
}

void cPathFinder::makeCostsPositive(int cost)
{
    graph_traits<graph_t>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(myGraph); ei != ei_end; ++ei)
    {
        myGraph[*ei].myCost -= cost;
    }
    myMaxNegCost = cost;
}

void cPathFinder::makeComplete()
{
    for (int u = 0; u < linkCount(); u++)
    {
        for (int v = u + 1; v < linkCount(); v++)
        {
            if (!edge(u, v, myGraph).second)
                addLink(
                    u, v,
                    INT_MAX);
        }
    }
}

std::string cPathFinder::pathViz()
{
    return pathViz(myPath);
}

std::string cPathFinder::pathViz(
    const std::vector<int> &vp,
    bool all)
{
    if (myfDirected)
        return pathVizT(vp, all, myDirGraph);
    else
        return pathVizT(vp, all, myGraph);
}

template <typename T>
std::string cPathFinder::pathVizT(
    const std::vector<int> &vp,
    bool all,
    T &g)
{
    std::string graphvizgraph = "graph";
    std::string graphvizlink = "--";
    if (myfDirected)
    {
        graphvizgraph = "digraph";
        graphvizlink = "->";
    }

    std::stringstream f;
    f << graphvizgraph << " G {\n";
    for (int v = *vertices(g).first; v != *vertices(g).second; ++v)
    {
        f << g[v].myName
          << " [color=\"" << g[v].myColor << "\"  penwidth = 3.0 ];\n";
    }

    // loop over links
    typename graph_traits<T>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    {
        // check if link between two nodes on path
        bool onpath = false;
        int src = source(*ei, g);
        int dst = target(*ei, g);
        auto pathItsrc = std::find(vp.begin(), vp.end(), src);
        auto pathItdst = std::find(vp.begin(), vp.end(), dst);
        if (pathItsrc != vp.end() && pathItdst != vp.end())
        {
            if (myfDirected)
            {
                if (pathItsrc == pathItdst - 1)
                    onpath = true;
            }
            else
            {
                if (pathItsrc == pathItdst + 1 || pathItsrc == pathItdst - 1)
                    onpath = true;
            }
        }

        if (all)
        {
            f << g[src].myName << graphvizlink
              << g[dst].myName << " ";
            if (onpath)
                f << "[color=\"red\"] ";
            f << ";\n";
        }
        else
        {
            if (onpath)
                f << g[src].myName << graphvizlink
                  << g[dst].myName << " ;\n";
        }
    }

    f << "}\n";
    return f.str();
}

std::string cPathFinder::spanViz(bool all)
{
    std::stringstream f;
    f << "graph G {\n";
    for (int v = *vertices(myGraph).first; v != *vertices(myGraph).second; ++v)
    {
        auto sn = myGraph[v].myName;
        int x = atoi(sn.substr(1).c_str());
        int y = atoi(sn.substr(sn.find("x") + 1).c_str());
        f << sn
          << " [pos=\""
          << std::to_string(x)
          << ","
          << std::to_string(y)
          << "\"];\n";
    }

    graph_traits<graph_t>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(myGraph); ei != ei_end; ++ei)
    {
        auto src = myGraph[source(*ei, myGraph)].myName;
        auto dst = myGraph[target(*ei, myGraph)].myName;
        bool span = false;
        for (auto &se : mySpan)
        {
            if (src == myGraph[se[0]].myName && dst == myGraph[se[1]].myName ||
                src == myGraph[se[1]].myName && dst == myGraph[se[0]].myName)
            {
                span = true;
                break;
            }
        }
        if (all)
        {
            f << src << "--"
              << dst << " ";
            if (span)
                f << "[color=\"red\"]";
            f << ";\n";
        }
        else if (span)
        {
            f << src << "--"
              << dst
              << " [color=\"red\"];\n";
        }
    }
    f << "}\n";
    return f.str();
}

void cPathFinder::cams()
{
    //     APPROXIMATION-VERTEX-COVER(G)=
    // C = ∅
    // E'= G.E

    // while E' ≠ ∅:
    //     let (u, v) be an arbitrary edge of E'
    //     C = C ∪ {u, v}
    //     remove from E' every edge incident on either u or v

    // remove all leaf nodes from C
    // return C

    // store indices of nodes that cover links
    std::set<int> setCover;

    // graph of links between covering nodes
    graph_t cover;

    // working copy on input graph
    auto work = myGraph;

    myPath.clear();

    graph_traits<graph_t>::out_edge_iterator ei, ei_end;

    // loop until all links are covered
    while (num_edges(work))
    {
        // select first link in working graph
        auto it = edges(work).first;
        int u = source(*it, work);
        int v = target(*it, work);

        // add  non leaf nodes on selected link to cover
        if (out_degree(u, myGraph) > 1)
            setCover.insert(u);
        if (out_degree(v, myGraph) > 1)
            setCover.insert(v);

        // remove all links that can be seen from new cover nodes
        for (boost::tie(ei, ei_end) = out_edges(u, work); ei != ei_end; ++ei)
        {
            remove_edge(*ei, work);
        }
        for (boost::tie(ei, ei_end) = out_edges(v, work); ei != ei_end; ++ei)
        {
            remove_edge(*ei, work);
        }
    }

    std::cout << "nodes that cover all vertices:\n";
    for (int u : setCover)
    {
        // exclude edge nodes, their link will be covered from other end
        std::cout << u << " ";
        myGraph[u].myColor = "red";
    }
    std::cout << "\n";
}

void cPathFinder::hills(
    const std::vector<std::vector<float>> &gheight)
{

    // cost links according to change in height they incur
    int rowCount = gheight.size();
    if (!rowCount)
        throw std::runtime_error(
            "cPathFinder::hills bad grid");
    int colCount = gheight[0].size();

    graph_traits<dir_graph_t>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(myDirGraph); ei != ei_end; ++ei)
    {
        int source = boost::source(*ei, myDirGraph);
        int target = boost::target(*ei, myDirGraph);
        int srow = source / colCount;
        int scol = source - srow * colCount;
        int trow = target / colCount;
        int tcol = target - trow * colCount;
        float sh = gheight[srow][scol];
        float th = gheight[trow][tcol];
        float delta = th - sh;
        myDirGraph[*ei].myCost = 1 + delta * delta;
    }

    path();

    std::cout << "hills " << pathText() << "\n";
}

void cPathFinder::gsingh()
{
    //loop over source nodes
    for (int src = 0; src < nodeCount(); src++)
    {
        // Run Dijsktra
        myStart = src;
        paths(src);

        // loop over destination nodes
        for (int dst = 0; dst < nodeCount(); dst++)
        {
            if (src == dst)
                continue;

            // display paths that visit 5 nodes
            if (pathPick(dst).size() == 5)
                std::cout << pathText();
        }
    }
}
void cPathFinder::shaun()
{
    int MaxNegCost = 0;
    graph_traits<graph_t>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(myGraph); ei != ei_end; ++ei)
    {
        int source = boost::source(*ei, myGraph);
        int target = boost::target(*ei, myGraph);
        int cost = atoi(myGraph[target].myColor.c_str()) - atoi(myGraph[source].myColor.c_str());
        myDirGraph[add_edge(source, target, myDirGraph).first].myCost = cost;
        myDirGraph[add_edge(target, source, myDirGraph).first].myCost = -cost;
        if (cost < MaxNegCost)
            MaxNegCost = cost;
        if (-cost < MaxNegCost)
            MaxNegCost = -cost;
    }
    graph_traits<dir_graph_t>::edge_iterator dei, dei_end;
    for (boost::tie(dei, dei_end) = edges(myDirGraph); dei != dei_end; ++dei)
    {
        std::cout << boost::source(*dei, myDirGraph) << " -> "
                  << boost::target(*dei, myDirGraph) << " cost "
                  << myDirGraph[*dei].myCost << " converts to ";

        myDirGraph[*dei].myCost -= MaxNegCost;
        myDirGraph[*dei].myCost += 2 * MaxNegCost;
        myDirGraph[*dei].myCost *= -1;

        std::cout
            << myDirGraph[*dei].myCost << "\n\n";
    }
    myfDirected = true;
    //tsp();
}


#include <iostream>
#include <cmath>
#include <fstream>
#include <functional>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/lexical_cast.hpp>
#include "fft.hpp"
#include "potgen.hpp"
#include "fileIO.hpp"
#include "potgen_args.h"
#include "profiling.hpp"
#include "correlation.hpp"
#include "discretize.hpp"

int main(int argc, const char* argv[])
{
	parse_parameters(argc, argv);

	PGOptions opt;

    // Get options from command line
	opt.randomSeed         = pargs::seed;
	opt.maxDerivativeOrder = pargs::derivative_order;
	opt.corrlength         = pargs::correlation_length;
	opt.numThreads         = pargs::threads;
	opt.verbose            = pargs::print_profile;

	// check that dimension is valid
	if( pargs::dim < 1 || pargs::dim > 3)
	{
		std::cerr << "invalid dimension " << pargs::dim << " specified\n";
		return EXIT_FAILURE;
	}

    try
	{
		// create output file, so if sth goes wrong we do not need to wait for the computation to finish
		// to issue an error
		std::fstream save(pargs::potential_outfile, std::fstream::out | std::fstream::binary);
		if( !save.good())
		{
			std::cerr << "could not open result file " << pargs::potential_outfile << " " << std::strerror(errno) << "\n";
			return EXIT_FAILURE;
		}

		// generate the correlation function
		opt.cor_fun = makeCorrelation( pargs::correlation_function, pargs::correlation_length, pargs::correlation_trafo);

		std::vector<std::size_t> extents(pargs::size.begin(), pargs::size.end());
		if(extents.size() == 1)
			extents.resize( pargs::dim, extents[0] );

		if( extents.size() != pargs::dim )
		{
			std::cerr << "Invalid number of size factors\n";
			exit(EXIT_FAILURE);
		}

		// debug output
		std::cout << "generate potential of size " << extents[0];
		for(int i = 1; i < pargs::dim; ++i)
			std::cout << "x"<<extents[i];
		std::cout << "\n";

		// make support area
		// we use the same aspect ratio as for the extents
		std::vector<double> support(pargs::dim);
		double min_ext = *std::min_element( extents.begin(), extents.end() );
		for(int i = 0; i < pargs::dim; ++i)
		{
			support[i] = (double)extents[i] / min_ext;
		}

		if(pargs::correlation_only)
		{
			auto grid = discretizeFunctionForFFT(extents, support, opt.cor_fun);

            // convert to real
            default_grid real_pot(extents, TransformationType::FFT_INDEX);

            auto it = real_pot.begin();
            for(const auto& value : grid)
            {
                *it = std::real(value);
                ++it;
            }

			std::cout << "saving correlation to " << pargs::potential_outfile << "\n";
            real_pot.dump(save);
			save.close();
		}
		else
		{
			auto pot = generatePotential(extents, support, opt);

			auto &pot_data = pot.getPotential();

			// control result
			std::cout << "Avg: " << std::accumulate(pot_data.begin(), pot_data.end(), 0.0) / (double) (pot_data.size())
					  << "\n";

			double variance = 0;
			double min = 0;
			double max = 0;;
			for (auto val : pot_data) {
				if (val > max) max = val;
				if (val < min) min = val;
				variance += val * val / pot_data.size();
			}

			std::cout << "Var: " << variance << "\n";

			pot.setStrength(pargs::strength);

			std::cout << "saving potential to " << pargs::potential_outfile << "\n";
			char write_buffer[1024 * 512];
			save.rdbuf()->pubsetbuf(write_buffer, sizeof(write_buffer));
			pot.writeToFile(save);
			save.close();
		}

		if( !pargs::no_wisdom )
		{
			saveFFTWisdom();
		}

		// profiling output
		if( pargs::print_profile )
		{
			ProfileRecord::print_profiling_data();
		}
	} catch ( boost::exception& e )
	{
		std::cerr << "an exception occurred: " << boost::diagnostic_information(e) << "\n";
		return 1;
	}
}


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "LevelSetFunction.h"

#include <boost/math/special_functions/sign.hpp>

#include "FractureProperty.h"

namespace
{
// Heaviside step function
inline double Heaviside(double v)
{
    return (v < 0.0) ? 0.0 : 1.0;
}

} // no named namespace

namespace ProcessLib
{
namespace LIE
{

double calculateLevelSetFunction(
        FractureProperty const& frac, double const* x_)
{
    Eigen::Map<Eigen::Vector3d const> x(x_, 3);
    return Heaviside(
                boost::math::sign(
                    frac.normal_vector.dot(x - frac.point_on_fracture)));
}

} // LIE
} // ProcessLib


// Copyright 2021 Jan Niklas Hasse <jhasse@bixense.com>
// For conditions of distribution and use, see copyright notice in LICENSE.txt
#include "Mat3.hpp"

#include "Vec2.hpp"
#include "screen.hpp"

#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/mat_operations3.hpp>
#include <boost/qvm/vec.hpp>

namespace jngl {
Mat3::Mat3(std::initializer_list<float> elements) {
	int row = 0;
	int column = 0;
	for (float element : elements) {
		data[column * 3 + row] = element;
		++column;
		if (column == 3) {
			++row;
			column = 0;
		}
	}
}

Mat3& Mat3::translate(const jngl::Vec2& v) {
	return *this *= boost::qvm::translation_mat(
	           boost::qvm::vec<double, 2>{ { v.x * getScaleFactor(), v.y * getScaleFactor() } });
}

Mat3& Mat3::scale(const float factor) {
	return scale(factor, factor);
}

Mat3& Mat3::scale(const float xfactor, const float yfactor) {
	return *this *= boost::qvm::diag_mat(boost::qvm::vec<float, 3>{ { xfactor, yfactor, 1 } });
}

Mat3& Mat3::rotate(const float radian) {
	boost::qvm::rotate_z(*this, radian);
	return *this;
}

} // namespace jngl


/*******************************************************************************
* Copyright 2020 Jose Manuel Fajardo
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

#pragma once

#include <iostream>
#include <algorithm>
#include <vector>
#include <math.h>
#include <Eigen/Dense>

#include <ros/ros.h>

#include <std_msgs/String.h>
#include <std_msgs/Float32.h>

#include <sensor_msgs/JointState.h>

#include <nav_msgs/Odometry.h>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>

#include <std_srvs/SetBool.h>

namespace mobile_odometry {

/*!
 * Class containing the Robotino Odometry class
 */
class MobileOdom {
public:
	/*!
	 * Constructor.
	 */
	MobileOdom(ros::NodeHandle& nodeHandle);

	/*!
	 * Destructor.
	 */
	virtual ~MobileOdom();

private:

	// Node handle
	ros::NodeHandle nodeHandle_;

	/*
	*	Subscribers
	*/
	ros::Subscriber joint_sts_sub_;

	/*
	*	Publisher
	*/
	ros::Publisher pub_odom;

	/*
	*  Service servers
	*/
	ros::ServiceServer service_reset_odom_;
	ros::ServiceServer service_reset_odom_cov_;

	/*!
	*	TF objects
	*/
	tf2_ros::TransformBroadcaster broadcaster_;

	/*!
	 * Parameters
	*/
	std::string output_rf;
	std::string input_rf;
	std::string robot_name;
	float init_x;
	float init_y;
	float init_theta;

	/*!
	* Variables
	*/

	geometry_msgs::TransformStamped odom_transform_;
	std::string joint_state_topic ;

	float phi_0_dot;
	float phi_1_dot;
	float phi_2_dot;

	float radius;
	float wheel_sep;

	int  frecuency_rate;
	bool broadcast_tf;

	std::string odometry_topic;
	nav_msgs::Odometry odom_msg;
	float scale;

	// Eigen Variables
	Eigen::MatrixXd J1;
	Eigen::MatrixXd J2;
	Eigen::MatrixXd Jacob;
	Eigen::Matrix3d Rot_mat;
	Eigen::VectorXd x_dot_local;
	Eigen::VectorXd x_dot_global;
	Eigen::VectorXd q_dot;

	/*
	Functions
	*/

	/*!
	* Reads and verifies the ROS parameters.
	* @return true if successful.
	*/
	bool readParameters();

	/*!
	* Callback function for the joint states of the robot
	* @param jointState the states of the three wheels
	*/
	void Joint_state_Callback_function(const sensor_msgs::JointState jointState);

	/*!
	* Function to refresh publisher and subscribers
	*/
	void spin();

	/*!
	* Service callback function to reset odometry
	* @param req the request of the service
	* @param res the reply of the service
	*/
	bool reset_odometry(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res);

	/*!
	* Service callback function to reset odometry covariance
	* @param req the request of the service
	* @param res the reply of the service
	*/
	bool reset_odom_cov(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res);

};

} /* namespace */


/***************************************************************************
 * Copyright 1998-2015 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxRender.                                       *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <stdexcept>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>

#include "slg/film/film.h"
#include "slg/film/imagepipeline/plugins/gammacorrection.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Gamma correction plugin
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::GammaCorrectionPlugin)

GammaCorrectionPlugin::GammaCorrectionPlugin(const float g, const u_int tableSize) {
	gamma = g;

	gammaTable.resize(tableSize, 0.f);
	float x = 0.f;
	const float dx = 1.f / tableSize;
	for (u_int i = 0; i < tableSize; ++i, x += dx)
		gammaTable[i] = powf(Clamp(x, 0.f, 1.f), 1.f / g);
}

ImagePipelinePlugin *GammaCorrectionPlugin::Copy() const {
	return new GammaCorrectionPlugin(gamma, gammaTable.size());
}

float GammaCorrectionPlugin::Radiance2PixelFloat(const float x) const {
	// Very slow !
	//return powf(Clamp(x, 0.f, 1.f), 1.f / 2.2f);

	const u_int tableSize = gammaTable.size();
	const int index = Clamp<int>(Floor2UInt(tableSize * Clamp(x, 0.f, 1.f)), 0, tableSize - 1);
	return gammaTable[index];
}

void GammaCorrectionPlugin::Apply(const Film &film, Spectrum *pixels, vector<bool> &pixelsMask) const {
	const u_int pixelCount = film.GetWidth() * film.GetHeight();

	for (u_int i = 0; i < pixelCount; ++i) {
		if (pixelsMask[i]) {
			pixels[i].c[0] = Radiance2PixelFloat(pixels[i].c[0]);
			pixels[i].c[1] = Radiance2PixelFloat(pixels[i].c[1]);
			pixels[i].c[2] = Radiance2PixelFloat(pixels[i].c[2]);
		}
	}
}


#include <vector>
#include <iostream>
#include <cstdlib>
#include <cassert>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <chrono>
#include <ctime>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/foreach.hpp>

using boost::property_tree::ptree;
using boost::property_tree::read_json;

template <typename T>
std::vector<T> as_vector(ptree const &pt, ptree::key_type const &key)
{
	std::vector<T> r;
	for (auto &item : pt.get_child(key))
		r.push_back(item.second.get_value<T>());
	return r;
}

template <typename T>
std::vector<std::vector<T>> asLayerVector(ptree const &pt, ptree::key_type const &key)
{
	std::vector<std::vector<T>> r;
	for (auto &item : pt.get_child(key))
	{
		std::vector<T> tmp;
		tmp.clear();
		for (auto &connectionWeight : item.second)
			tmp.push_back(connectionWeight.second.get_value<T>());
		r.push_back(tmp);
	}
	return r;
}

template <typename T>
void showVectorVals(std::string label, std::vector<T> &v)
{
	std::cout << label << " ";
	for (unsigned i = 0; i < v.size(); ++i)
	{
		std::cout << v[i] << " ";
	}
	std::cout << std::endl;
}

struct Connection
{
	double weight;
};

class Neuron;

typedef std::vector<Neuron> Layer;

// ****************** class Neuron ******************

class Neuron
{
  public:
	Neuron(unsigned numOutputs, unsigned myIndex);
	Neuron(std::vector<float> connectionWeights, unsigned myIndex);
	void setOutputVal(double val) { m_outputVal = val; }
	double getOutputVal(void) const { return m_outputVal; }
	void feedForward(const Layer &prevLayer, std::string);

  private:
	static double transferFunction(double x, std::string actFunc);
	// randomWeight: 0 - 1
	static double randomWeight(void) { return rand() / double(RAND_MAX); }
	double m_outputVal;
	std::vector<Connection> m_outputWeights;
	unsigned m_myIndex;
};

double Neuron::transferFunction(double x, std::string actFunc = "relu")
{
	if (actFunc == "relu")
	{
		return std::max(0.0, x);
	}
	else if (actFunc == "linear")
	{
		return x;
	}
}

void Neuron::feedForward(const Layer &prevLayer, std::string actFunc = "relu")
{
	double sum = 0.0;

	// Sum the previous layer's outputs (which are our inputs)
	// Include the bias node from the previous layer.

	for (unsigned n = 0; n < prevLayer.size(); ++n)
	{
		sum += prevLayer[n].getOutputVal() *
			   prevLayer[n].m_outputWeights[m_myIndex].weight;
	}

	m_outputVal = Neuron::transferFunction(sum, actFunc);
}

Neuron::Neuron(unsigned numOutputs, unsigned myIndex)
{
	for (unsigned c = 0; c < numOutputs; ++c)
	{
		m_outputWeights.push_back(Connection());
		m_outputWeights.back().weight = randomWeight();
	}

	m_myIndex = myIndex;
}

Neuron::Neuron(std::vector<float> connectionWeights, unsigned myIndex)
{
	for (auto &v : connectionWeights)
	{
		m_outputWeights.push_back(Connection());
		m_outputWeights.back().weight = v;
	}

	m_myIndex = myIndex;
}

// ****************** class Net ******************
class Net
{
  public:
	Net(const std::vector<std::vector<std::vector<float>>> &modelWeights);
	void feedForward(const std::vector<double> &inputVals);
	void getResults(std::vector<double> &resultVals) const;
	bool infer(std::vector<double> &data_in, std::vector<double> &out);

  private:
	std::vector<Layer> m_layers; //m_layers[layerNum][neuronNum]
};

void Net::getResults(std::vector<double> &resultVals) const
{
	resultVals.clear();

	for (unsigned n = 0; n < m_layers.back().size(); ++n)
	{
		resultVals.push_back(m_layers.back()[n].getOutputVal());
	}
}

void Net::feedForward(const std::vector<double> &inputVals)
{
	// Check the num of inputVals equal to neuronnum expect bias
	assert(inputVals.size() == m_layers[0].size() - 1);

	// Assign {latch} the input values into the input neurons
	for (unsigned i = 0; i < inputVals.size(); ++i)
	{
		m_layers[0][i].setOutputVal(inputVals[i]);
	}

	// Forward propagate
	for (unsigned layerNum = 1; layerNum < m_layers.size() - 1; ++layerNum)
	{
		Layer &prevLayer = m_layers[layerNum - 1];
		for (unsigned n = 0; n < m_layers[layerNum].size() - 1; ++n)
		{
			// std::cout << "n:" << n << '\n';
			m_layers[layerNum][n].feedForward(prevLayer, "relu");
		}
	}
	//output layer
	{
		unsigned linearOutputLayer = m_layers.size() - 1;
		Layer &prevLayer = m_layers[linearOutputLayer - 1];
		for (auto &n : m_layers[linearOutputLayer])
		{
			n.feedForward(prevLayer, "linear");
		}
	}
}

bool Net::infer(std::vector<double> &data_in, std::vector<double> &out)
{
	std::vector<double> inputVals, resultVals;
	int trainingPass = 0;
	auto t_start = std::chrono::high_resolution_clock::now();

	while (trainingPass < 1e6)
	{
		++trainingPass;
		// std::cout << "Pass:" << trainingPass << "\n";

		inputVals = data_in;

		this->feedForward(inputVals);

		// Collect the net's actual results:
		this->getResults(resultVals);
		// showVectorVals<double>("Outputs:", resultVals);

		// assert(targetVals.size() == topology.back());
	}
	auto t_end = std::chrono::high_resolution_clock::now();
	auto total = std::chrono::duration<float, std::milli>(t_end - t_start).count();
	std::cout << "totol time is " << total << "ms.\n";
	out = resultVals;
	return 0;
}

Net::Net(const std::vector<std::vector<std::vector<float>>> &modelWeight)
{
	unsigned numLayers = modelWeight.size();
	for (unsigned layerNum = 0; layerNum < numLayers; ++layerNum)
	{
		std::cout << "layer:" << layerNum << '\n';
		m_layers.push_back(Layer());

		std::vector<std::vector<float>> layerWeights = modelWeight[layerNum];
		for (unsigned neuronNum = 0; neuronNum < modelWeight[layerNum].size(); ++neuronNum)
		{
			m_layers.back().push_back(Neuron(layerWeights[neuronNum], neuronNum));
			// std::cout << "Mad a Neuron! Index:" << neuronNum << std::endl;
		}
		m_layers.back().back().setOutputVal(1.0);
	}
	//output layer
	std::cout << "layer:output\n";
	m_layers.push_back(Layer());
	for (unsigned neuronNum = 0; neuronNum < modelWeight[numLayers - 1][0].size(); ++neuronNum)
	{
		m_layers.back().push_back(Neuron(0, neuronNum));
		// std::cout << "Mad a Neuron! Index:" << neuronNum << std::endl;
	}
	m_layers.back().back().setOutputVal(1.0);

	assert(m_layers.size() == (modelWeight.size() + 1));
}

int main()
{
	std::ifstream myfile;
	myfile.open("data.json");
	// myfile.open("test.json");
	std::stringstream buffer;
	buffer << myfile.rdbuf();
	// std::cout << buffer.str() << '\n';

	ptree pt2;
	read_json(buffer, pt2);

	std::vector<std::vector<std::vector<float>>> modelWeights;
	std::vector<std::vector<float>> layer1, layer2, output;

	layer1 = asLayerVector<float>(pt2, "l1");
	layer2 = asLayerVector<float>(pt2, "l2");
	output = asLayerVector<float>(pt2, "output");

	modelWeights.push_back(layer1);
	modelWeights.push_back(layer2);
	modelWeights.push_back(output);

	Net myNet(modelWeights);

	std::vector<double> inputVals, resultVals;
	// int trainingPass = 0;
	// while (trainingPass < 1)
	// {
	// 	++trainingPass;
	// 	std::cout << "Pass:" << trainingPass << "\n";

	// 	inputVals = as_vector<double>(pt2, "in");
	// 	showVectorVals<double>(": Inputs :", inputVals);
	// 	myNet.feedForward(inputVals);

	// 	// Collect the net's actual results:
	// 	myNet.getResults(resultVals);
	// 	showVectorVals<double>("Outputs:", resultVals);

	// 	// assert(targetVals.size() == topology.back());
	// }
	inputVals = as_vector<double>(pt2, "in");
	showVectorVals<double>(": Inputs :", inputVals);
	myNet.infer(inputVals, resultVals);
	showVectorVals<double>("Outputs:", resultVals);

	std::cout << std::endl
			  << "Done" << std::endl;
}


#pragma once

#include <algorithm>
#include <atomic>
#include <limits>
#include <stack>
#include <thread>
#include <queue>

#include <Eigen/Dense>

namespace pcs {

template<uint16_t K, typename index_t = unsigned>
class KDTree {
public:
  const index_t nai_v = std::numeric_limits<index_t>::max();
public:
  KDTree(const std::vector<Eigen::Matrix<double, K, 1>> &vertices,
         int max_threads = 2 * std::thread::hardware_concurrency());

  std::pair<index_t, double>
  find_nn(Eigen::Matrix<double, K, 1> point,
          double max_dist = std::numeric_limits<double>::infinity()) const;

  std::vector<std::pair<index_t, double>>
  find_nns(Eigen::Matrix<double, K, 1> point, std::size_t n,
           double max_dist = std::numeric_limits<double>::infinity()) const;

private:
  const std::vector<Eigen::Matrix<double, K, 1>> &vertices;

  struct Node {
    typedef index_t ID;
    decltype(K) d;
    index_t first;
    index_t last;
    index_t vertex_id;
    Node::ID left;
    Node::ID right;
  };

  std::atomic<index_t> num_nodes;
  std::vector<Node> nodes;

  typename Node::ID CreateNode(decltype(K) d, index_t first, index_t last) {
    typename Node::ID node_id = num_nodes++;
    Node &node = nodes[node_id];
    node.first = first;
    node.last = last;
    node.left = nai_v;
    node.right = nai_v;
    node.vertex_id = nai_v;
    node.d = d;
    return node_id;
  }

  std::pair<typename Node::ID, typename Node::ID>
  ssplit(typename Node::ID node_id, std::vector<index_t> *indices);

  void split(typename Node::ID node_id, std::vector<index_t> *indices,
             std::atomic<int> *num_threads);
};

template<uint16_t K, typename IdxType>
KDTree<K, IdxType>::KDTree(const std::vector<Eigen::Matrix<double, K, 1>>&vertices,
                           int max_threads)
    : vertices(vertices), num_nodes(0) {

  std::size_t num_vertices = vertices.size();
  nodes.resize(num_vertices);

  std::vector<IdxType> indices(num_vertices);
  for (std::size_t i = 0; i < indices.size(); ++i) {
    indices[i] = i;
  }

  std::atomic<int> num_threads(max_threads);
  split(CreateNode(0, 0, num_vertices), &indices, &num_threads);
}

template<uint16_t K, typename IdxType>
void KDTree<K, IdxType>::split(typename Node::ID node_id,
                               std::vector<IdxType> *indices,
                               std::atomic<int> *num_threads) {
  typename Node::ID left, right;
  if ((*num_threads -= 1) >= 1) {
    std::tie(left, right) = ssplit(node_id, indices);
    if (left != nai_v && right != nai_v) {
      std::thread other(&KDTree::split, this, left, indices, num_threads);
      split(right, indices, num_threads);
      other.join();
    } else {
      if (left != nai_v)
        split(left, indices, num_threads);
      if (right != nai_v)
        split(right, indices, num_threads);
    }
  } else {
    std::deque<typename Node::ID> queue;
    queue.push_back(node_id);
    while (!queue.empty()) {
      typename Node::ID node_id = queue.front();
      queue.pop_front();

      std::tie(left, right) = ssplit(node_id, indices);
      if (left != nai_v)
        queue.push_back(left);
      if (right != nai_v)
        queue.push_back(right);
    }
  }
  *num_threads += 1;
}

template<uint16_t K, typename IdxType>
std::pair<typename KDTree<K, IdxType>::Node::ID, typename KDTree<K, IdxType>::Node::ID>
KDTree<K, IdxType>::ssplit(typename Node::ID node_id, std::vector<IdxType> *indices) {
  Node &node = nodes[node_id];
  decltype(K) d = node.d;
  std::sort(indices->data() + node.first, indices->data() + node.last,
            [this, d](IdxType a, IdxType b) -> bool {
              return vertices[a][d] < vertices[b][d];
            }
  );
  d = (d + 1) % K;
  IdxType mid = (node.last + node.first) / 2;
  node.vertex_id = indices->at(mid);
  if (mid - node.first > 0) {
    node.left = CreateNode(d, node.first, mid);
  }
  if (node.last - (mid + 1) > 0) {
    node.right = CreateNode(d, mid + 1, node.last);
  }
  return std::make_pair(node.left, node.right);
}

template<uint16_t K, typename IdxType>
std::pair<IdxType, double>
KDTree<K, IdxType>::find_nn(Eigen::Matrix<double, K, 1> point, double max_dist) const {
  return find_nns(point, 1, max_dist)[0];
}

template<uint16_t K, typename IdxType>
std::vector<std::pair<IdxType, double>>
KDTree<K, IdxType>::find_nns(Eigen::Matrix<double, K, 1> vertex, std::size_t n, double max_dist) const {

  std::pair<IdxType, double> nn = std::make_pair(nai_v, max_dist);
  std::vector<std::pair<IdxType, double>> nns(n, nn);

  std::stack<std::pair<typename Node::ID, bool> > s;
  s.emplace(0, true);
  while (!s.empty()) {
    typename Node::ID node_id;
    bool down;
    std::tie(node_id, down) = s.top();
    s.pop();

    if (node_id == nai_v)
      continue;

    Node const &node = nodes[node_id];

    double diff = vertex[node.d] - vertices[node.vertex_id][node.d];
    if (down) {
      double dist = (vertex - vertices[node.vertex_id]).norm();
      if (dist < max_dist) {
        nns.emplace_back(node.vertex_id, dist);
        std::sort(nns.begin(), nns.end(),
                  [](std::pair<IdxType, double> a, std::pair<IdxType, double> b) -> bool {
                    return a.second < b.second;
                  });
        nns.pop_back();
        max_dist = nns.back().second;
      }

      if (node.left == nai_v && node.right == nai_v)
        continue;

      s.emplace(node_id, false);
      if (diff < 0.0f) {
        s.emplace(node.left, true);
      } else {
        s.emplace(node.right, true);
      }
    } else {
      if (std::abs(diff) >= max_dist)
        continue;

      if (diff < 0.0f) {
        s.emplace(node.right, true);
      } else {
        s.emplace(node.left, true);
      }
    }
  }
  return nns;
}

}// namespace pcs


#include <catch.hpp>
#include <Eigen/Core>
#include <random>

#include "check_adjoint.h"
#include "test_utils.h"
#include "renderer_interpolation.cuh"

template<typename Tensor_t>
Tensor_t accessor4D(const VectorXr& tf, const int3& xyz)
{
	const int64_t sizes[] = {
		1, xyz.x, xyz.y, xyz.z
	};
	const int64_t strides[] = {
		0, xyz.y*xyz.z, xyz.z, 1
	};
	return Tensor_t(
		const_cast<real_t*>(tf.data()), sizes, strides);
}

template<bool hasVolumeDerivative>
void testAdjointInterpolation()
{
	typedef empty TmpStorage_t;
	typedef VectorXr Vector_t;

	std::default_random_engine rnd(42);
	std::uniform_real_distribution<real_t> distr(0.01, 0.99);
	static const int3 resolution = make_int3(2, 3, 4);
	static const int resolution_prod = resolution.x * resolution.y * resolution.z;
	static const real3 voxelSize = make_real3(1.0f) / make_real3(resolution);
	Vector_t baseVolume(resolution_prod);
	for (int j = 0; j < baseVolume.size(); ++j) baseVolume[j] = distr(rnd);

	auto forward = [&baseVolume](const Vector_t& x, TmpStorage_t* tmp) -> Vector_t
	{
		const real3 pos = fromEigen3(x.segment(0, 3));

		const Vector_t volume = hasVolumeDerivative
			? x.segment(3, resolution_prod)
			: baseVolume;
		const auto volume_acc = accessor4D<kernel::Tensor4Read>(volume, resolution);

		kernel::VolumeInterpolation<kernel::FilterTrilinear> volumeInterpolation;
		real_t density = volumeInterpolation.fetch(volume_acc, resolution, 0, pos);

		Vector_t result(1);
		result[0] = density;
		return result;
	};
	auto adjoint = [baseVolume](const Vector_t& x, const Vector_t& e, const Vector_t& g,
		Vector_t& z, const TmpStorage_t& tmp)
	{
		const real3 pos = fromEigen3(x.segment(0, 3));

		const Vector_t volume = hasVolumeDerivative
			? x.segment(3, resolution_prod)
			: baseVolume;
		const auto volume_acc = accessor4D<kernel::Tensor4Read>(volume, resolution);

		real_t density = e[0];
		real_t adj_density = g[0];
		real3 adj_pos;
		Vector_t adjVolume = Vector_t::Zero(resolution_prod);
		auto adjVolume_acc = accessor4D<kernel::BTensor4RW>(adjVolume, resolution);

		{
			kernel::VolumeInterpolation<kernel::FilterTrilinear, true, hasVolumeDerivative> volumeInterpolation(
				0, resolution, adjVolume_acc);
			volumeInterpolation.fetch(volume_acc, resolution, 0, pos);
			volumeInterpolation.adjoint(adj_density, adj_pos);
		}

		z.segment(0, 3) = toEigen(adj_pos);
		if (hasVolumeDerivative) z.segment(3, resolution_prod) = adjVolume;
	};

	int N = 20;
	for (int i = 0; i < N; ++i)
	{
		INFO("N=" << i);
		Vector_t x;
		if (hasVolumeDerivative) {
			x.resize(3 + resolution_prod);
			x.segment(3, resolution_prod) = baseVolume;
		}
		else {
			x.resize(3);
		}
		x[0] = distr(rnd) / voxelSize.x;
		x[1] = distr(rnd) / voxelSize.y;
		x[2] = distr(rnd) / voxelSize.z;
		checkAdjoint<Vector_t, TmpStorage_t>(x, forward, adjoint,
			1e-5, 1e-5, 1e-6);
	}
}

TEST_CASE("Adjoint-Interpolation-NoVolumeDerivatives", "[adjoint]")
{
	testAdjointInterpolation<false>();
}

TEST_CASE("Adjoint-Interpolation-withVolumeDerivatives", "[adjoint]")
{
	testAdjointInterpolation<true>();
}


//////////////////////////////////////////////////////////////////
// example93.cpp
//
// Copyright (c) 2015 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

// include headers to support safe integers
#include <boost/safe_numerics/cpp.hpp>
#include <boost/safe_numerics/exception.hpp>
#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/safe_integer_range.hpp>
#include <boost/safe_numerics/safe_integer_literal.hpp>

// use same type promotion as used by the pic compiler
// target compiler XC8 supports:
using pic16_promotion = boost::safe_numerics::cpp<
    8,  // char      8 bits
    16, // short     16 bits
    16, // int       16 bits
    16, // long      16 bits
    32  // long long 32 bits
>;

// ***************************
// 1. Specify exception policies so we will generate a
// compile time error whenever an operation MIGHT fail.

// ***************************
// generate runtime errors if operation could fail
using exception_policy = boost::safe_numerics::default_exception_policy;

// generate compile time errors if operation could fail
using trap_policy = boost::safe_numerics::loose_trap_policy;

// ***************************
// 2. Create a macro named literal an integral value
// that can be evaluated at compile time.
#define literal(n) make_safe_literal(n, pic16_promotion, void)

// For min speed of 2 mm / sec (24.8 format)
// sec / step = sec / 2 mm * 2 mm / rotation * rotation / 200 steps
#define C0      literal(5000 << 8)

// For max speed of 400 mm / sec
// sec / step = sec / 400 mm * 2 mm / rotation * rotation / 200 steps
#define C_MIN   literal(25 << 8)

static_assert(
    C0 < make_safe_literal(0xffffff, pic16_promotion,trap_policy),
    "Largest step too long"
);
static_assert(
    C_MIN > make_safe_literal(0, pic16_promotion,trap_policy),
    "Smallest step must be greater than zero"
);

// ***************************
// 3. Create special ranged types for the motor program
// These wiil guarantee that values are in the expected
// ranges and permit compile time determination of when
// exceptional conditions might occur.

using pic_register_t = boost::safe_numerics::safe<
    uint8_t,
    pic16_promotion,
    trap_policy // use for compiling and running tests
>;

// note: the maximum value of step_t would be:
// 50000 = 500 mm / 2 mm/rotation * 200 steps/rotation.
// But in one expression the value of number of steps * 4 is
// used.  To prevent introduction of error, permit this
// type to hold the larger value.
using step_t = boost::safe_numerics::safe_unsigned_range<
    0,
    200000,
    pic16_promotion,
    exception_policy
>;

// position
using position_t = boost::safe_numerics::safe_unsigned_range<
    0,
    50000, // 500 mm / 2 mm/rotation * 200 steps/rotation
    pic16_promotion,
    exception_policy
>;

// next end of step timer value in format 24.8
// where the .8 is the number of bits in the fractional part.
using ccpr_t = boost::safe_numerics::safe<
    uint32_t,
    pic16_promotion,
    exception_policy
>;

// pulse length in format 24.8
// note: this value is constrainted to be a positive value. But
// we still need to make it a signed type. We get an arithmetic
// error when moving to a negative step number.
using c_t = boost::safe_numerics::safe_unsigned_range<
    C_MIN,
    C0,
    pic16_promotion,
    exception_policy
>;

// 32 bit unsigned integer used for temporary purposes
using temp_t = boost::safe_numerics::safe_unsigned_range<
    0, 0xffffffff,
    pic16_promotion,
    exception_policy
>;

// index into phase table
// note: The legal values are 0-3.  So why must this be a signed
// type?  Turns out that expressions like phase_ix + d
// will convert both operands to unsigned.  This in turn will
// create an exception.  So leave it signed even though the
// value is greater than zero.
using phase_ix_t = boost::safe_numerics::safe_signed_range<
    0,
    3,
    pic16_promotion,
    trap_policy
>;

// settings for control value output
using phase_t = boost::safe_numerics::safe<
    uint16_t,
    pic16_promotion,
    trap_policy
>;

// direction of rotation
using direction_t = boost::safe_numerics::safe_signed_range<
    -1,
    +1,
    pic16_promotion,
    trap_policy
>;

// some number of microseconds
using microseconds = boost::safe_numerics::safe<
    uint32_t,
    pic16_promotion,
    trap_policy
>;

// ***************************
// emulate PIC features on the desktop

// filter out special keyword used only by XC8 compiler
#define __interrupt
// filter out XC8 enable/disable global interrupts
#define ei()
#define di()

// emulate PIC special registers
pic_register_t RCON;
pic_register_t INTCON;
pic_register_t CCP1IE;
pic_register_t CCP2IE;
pic_register_t PORTC;
pic_register_t TRISC;
pic_register_t T3CON;
pic_register_t T1CON;

pic_register_t CCPR2H;
pic_register_t CCPR2L;
pic_register_t CCPR1H;
pic_register_t CCPR1L;
pic_register_t CCP1CON;
pic_register_t CCP2CON;
pic_register_t TMR1H;
pic_register_t TMR1L;

// ***************************
// special checked type for bits - values restricted to 0 or 1
using safe_bit_t = boost::safe_numerics::safe_unsigned_range<
    0,
    1,
    pic16_promotion,
    trap_policy
>;

// create type used to map PIC bit names to
// correct bit in PIC register
template<typename T, std::int8_t N>
struct bit {
    T & m_word;
    constexpr explicit bit(T & rhs) :
        m_word(rhs)
    {}
    // special functions for assignment of literal
    constexpr bit & operator=(decltype(literal(1))){
        m_word |= literal(1 << N);
        return *this;
    }
    constexpr bit & operator=(decltype(literal(0))){
        m_word &= ~literal(1 << N);
        return *this;
    }
    // operator to convert to 0 or 1
    constexpr operator safe_bit_t () const {
        return m_word >> literal(N) & literal(1);
    }
};

// define bits for T1CON register
struct  {
    bit<pic_register_t, 7> RD16{T1CON};
    bit<pic_register_t, 5> T1CKPS1{T1CON};
    bit<pic_register_t, 4> T1CKPS0{T1CON};
    bit<pic_register_t, 3> T1OSCEN{T1CON};
    bit<pic_register_t, 2> T1SYNC{T1CON};
    bit<pic_register_t, 1> TMR1CS{T1CON};
    bit<pic_register_t, 0> TMR1ON{T1CON};
} T1CONbits;

// define bits for T1CON register
struct  {
    bit<pic_register_t, 7> GEI{INTCON};
    bit<pic_register_t, 5> PEIE{INTCON};
    bit<pic_register_t, 4> TMR0IE{INTCON};
    bit<pic_register_t, 3> RBIE{INTCON};
    bit<pic_register_t, 2> TMR0IF{INTCON};
    bit<pic_register_t, 1> INT0IF{INTCON};
    bit<pic_register_t, 0> RBIF{INTCON};
} INTCONbits;

#include "motor3.c"

#include <chrono>
#include <thread>

// round 24.8 format to microseconds
microseconds to_microseconds(ccpr_t t){
    return (t + literal(128)) / literal(256);
}

using result_t = uint8_t;
const result_t success = 1;
const result_t fail = 0;

// move motor to the indicated target position in steps
result_t test(position_t new_position){
    try {
        std::cout << "move motor to " << new_position << '\n';
        motor_run(new_position);
        std::cout
        << "step #" << ' '
        << "delay(us)(24.8)" << ' '
        << "delay(us)" << ' '
        << "CCPR" << ' '
        << "motor position" << '\n';
        while(busy()){
            std::this_thread::sleep_for(std::chrono::microseconds(to_microseconds(c)));
            c_t last_c = c;
            ccpr_t last_ccpr = ccpr;
            isr_motor_step();
            std::cout << i << ' '
            << last_c << ' '
            << to_microseconds(last_c) << ' '
            << std::hex << last_ccpr << std::dec << ' '
            << motor_position << '\n';
        };
    }
    catch(const std::exception & e){
        std::cout << e.what() << '\n';
        return fail;
    }
    return success;
}

int main(){
    std::cout << "start test\n";
    result_t result = success;
    try {
        initialize();
        // move motor to position 1000
        result &= test(literal(9000));
        // move to the left before zero position
        // fails to compile !
        // result &= ! test(-10);
        // move motor to position 200
        result &= test(literal(200));
        // move motor to position 200 again! Should result in no movement.
        result &= test(literal(200));
        // move motor to position 50000.
        result &= test(literal(50000));
        // move motor back to position 0.
        result &= test(literal(0));
    }
    catch(...){
        std::cout << "test interrupted\n";
        return EXIT_FAILURE;
    }
    std::cout << "end test\n";
    return result == success ? EXIT_SUCCESS : EXIT_FAILURE;
}


#include "ElasticRod.h"

#include <cmath>
#include <dlib/optimization.h>
#include "Utils.h"

typedef dlib::matrix<double> Hessian;

//====================================== MinimizationPImpl definition - minimize Energy with respect to twist angle theta =======================================

struct ElasticRod::MinimizationPImpl
{
public:
    MinimizationPImpl() { }
    ~MinimizationPImpl() { }

    double minimize(const ElasticRod *rod, ColumnVector& io_theta);

public:

    static void extractThetaVars(const ElasticRod* rod, const ColumnVector& theta, ColumnVector& o_thetaVars);
    static void constructTheta(const ElasticRod* rod, const ColumnVector& thetaVars, ColumnVector& o_theta);

public:

    struct evaluate
    {
        double operator() (const ColumnVector& theta) const;
        const ElasticRod* m_rod;
    };

    struct evaluateGradient
    {
        ColumnVector operator() (const ColumnVector& theta) const;
        const ElasticRod* m_rod;
    };

    struct evaluateHessian
    {
        Hessian operator() (const ColumnVector& theta) const;
        const ElasticRod* m_rod;
    };

    evaluate m_evaluate;
    evaluateGradient m_evaluateGradient;
    evaluateHessian m_evaluateHessian;
};

//====================================== ElasticRod implementation =======================================

ElasticRod::ElasticRod(const ElasticRodParams *params)
{
    assert( params != NULL );
    m_params = params;
    m_minimization = new MinimizationPImpl();
}

ElasticRod::~ElasticRod()
{
    delete m_minimization;
}

void ElasticRod::init(const std::vector<mg::Vec3D>& restpos,
                      const mg::Vec3D& u0,
                      const std::vector<mg::Vec3D>& pos,
                      const std::vector<mg::Vec3D>& vel,
                      const std::vector<mg::Real>& mass,
                      const ColumnVector& theta,
                      const std::set<unsigned> &isClamped)
{
    assert( pos.size() > 2 );
    assert( pos.size() == restpos.size() );
    assert( pos.size() == vel.size() );
    assert( pos.size() == mass.size() );
    assert( (unsigned)theta.size() == (pos.size() - 1) );

    m_u0 = u0;
    m_ppos = pos;
    m_pprepos = pos;
    m_pvel = vel;
    m_pmass = mass;
    m_theta = theta;
    m_isClamped = isClamped;

    m_edges.resize(m_ppos.size() - 1);
    m_kb.resize(m_edges.size());
    m_m1.resize(m_edges.size());
    m_m2.resize(m_edges.size());

    m_restEdgeL.resize(m_edges.size());
    m_restRegionL.resize(m_edges.size());
    m_restWprev.resize(m_edges.size());
    m_restWnext.resize(m_edges.size());

//    compute edges & lengths for rest shape
    computeEdges(restpos, m_edges);
    computeLengths(m_edges, m_restEdgeL, m_restRegionL);
//    compute kb and material frame (Bishop frame) for rest shape
    computeBishopFrame(m_u0, m_edges, m_kb, m_m1, m_m2);
//    precompute material curvature for rest shape
    computeMaterialCurvature(m_kb, m_m1, m_m2, m_restWprev, m_restWnext);

//    initialize current state
    updateCurrentState();

#ifdef DBUGG
//    store elasic force for debug purpose
    m_elasticForce.resize(m_ppos.size(), mg::Vec3D(0, 0, 0));
    accumulateInternalElasticForces(m_elasticForce);
#endif

}

void ElasticRod::getState(ElasticRodState& o_state) const
{
    o_state.m_ppos = m_ppos;
    o_state.m_pvel = m_pvel;
    o_state.m_u0 = m_u0;
}

void ElasticRod::setState(const ElasticRodState& state)
{
    m_ppos = state.m_ppos;
    m_pvel = state.m_pvel;
    m_u0 = state.m_u0;

    computeEdges(m_ppos, m_edges);
    updateCurrentState();
}

void ElasticRod::computeEdges(const std::vector<mg::Vec3D>& vertices,
                              std::vector<mg::Vec3D>& o_edges) const
{
    for (unsigned i = 0; i < vertices.size() - 1; ++i)
    {
        o_edges[i] = vertices[i + 1] - vertices[i];
        assert( std::isfinite(o_edges[i].length_squared()) );
    }
}

void ElasticRod::computeLengths(const std::vector<mg::Vec3D>& edges,
                                std::vector<mg::Real> &o_edgeL,
                                std::vector<mg::Real> &o_regionL) const
{
    for (unsigned i = 0; i < edges.size(); ++i)
    {
        o_edgeL[i] = edges[i].length();
        assert( std::isfinite(o_edgeL[i]) && o_edgeL[i] > mg::ERR );
    }

    o_regionL[0] = 0.0;
    for (unsigned i = 1; i < edges.size(); ++i)
    {
        o_regionL[i] = o_edgeL[i - 1] + o_edgeL[i];
    }
}

void ElasticRod::computeMaterialCurvature(const std::vector<mg::Vec3D>& kb,
                                        const std::vector<mg::Vec3D>& m1,
                                        const std::vector<mg::Vec3D>& m2,
                                        std::vector<mg::Vec2D>& o_Wprev,
                                        std::vector<mg::Vec2D>& o_Wnext) const
{
    o_Wprev[0].zero();
    o_Wnext[0].zero();
    for (unsigned i = 1; i < kb.size(); ++i)
    {
        computeW(kb[i], m1[i - 1], m2[i - 1], o_Wprev[i]);
        computeW(kb[i], m1[i], m2[i], o_Wnext[i]);
    }
}

void ElasticRod::computeW(const mg::Vec3D& kb, const mg::Vec3D& m1, const mg::Vec3D& m2, mg::Vec2D& o_wij) const
{
    o_wij.set( mg::dot( kb, m2 ), -mg::dot( kb, m1 ));
}

void ElasticRod::computeKB(const std::vector<mg::Vec3D>& edges,
                           std::vector<mg::Vec3D>& o_kb) const
{
    o_kb[0].zero();

    for (unsigned i = 1; i < edges.size(); ++i)
    {
///     NOTE: as metioned in the paper the following formula produces ||kb|| = 2 * tan(phi / 2),
///     where phi is the angle of rotation defined by the 2 edges
///     This holds only if ||edges[i]|| = ||rest_edges[i]||,
///     otherwise errors occur in consequent frame rotations and force calculations
///     in particular this assumption can be violated if the constraint enforcement DOES NOT GUARANTEE inextensibility,
///     which is the case with PBD used below
///     note that tan maps [-pi/2, pi/2] to [-inf, inf], so interestingly for arbitrary real number an arbitrary bounded angle can be extracted
///
        o_kb[i] = 2 * mg::cross( edges[i - 1], edges[i] ) /
                (m_restEdgeL[i - 1] * m_restEdgeL[i] + mg::dot( edges[i - 1], edges[i]) );
    }
}

void ElasticRod::extractSinAndCos(const double &magnitude,
                                  double &o_sinPhi, double &o_cosPhi) const
{
    o_cosPhi = mg::sqrt_safe(4.0 / (4.0 + magnitude));
    o_sinPhi = mg::sqrt_safe(magnitude / (4.0 + magnitude));
}

void ElasticRod::computeBishopFrame(const mg::Vec3D& u0,
                        const std::vector<mg::Vec3D>& edges,
                        std::vector<mg::Vec3D>& o_kb,
                        std::vector<mg::Vec3D>& o_u,
                        std::vector<mg::Vec3D>& o_v) const
{
    computeKB(edges, o_kb);

    o_u[0] = u0;
    o_v[0] = mg::cross(edges[0], o_u[0]);
    o_v[0].normalize();

    double magnitude;
    double sinPhi, cosPhi;
//    compute Bishop frame for current configuration by parallel transporting u0
    for (unsigned i = 1; i < edges.size(); ++i)
    {
//        here sinPhi and cosPhi are derived from the length of kb ||kb|| = 2 * tan( phi/2 )
        magnitude = mg::dot(o_kb[i], o_kb[i]);
        extractSinAndCos(magnitude, sinPhi, cosPhi);
        assert( cosPhi >= 0 && cosPhi <= 1 );

        if ( (1 - cosPhi) < mg::ERR )
        {
            o_u[i] = o_u[i - 1];
            o_v[i] = o_v[i - 1];
            continue;
        }

//        rotate frame u axis around kb
        mg::Quaternion q(cosPhi, sinPhi * mg::normalize(o_kb[i]));
        mg::Quaternion p(0, o_u[i - 1]);
        p = q * p * mg::conjugate(q);

        o_u[i].set(p[1], p[2], p[3]);
        o_u[i].normalize();
        o_v[i] = mg::cross(edges[i], o_u[i]);
        o_v[i].normalize();
    }
}

void ElasticRod::parallelTransportFrame(const mg::Vec3D& e0, const mg::Vec3D& e1,
                                        mg::Vec3D& io_u) const
{
    mg::Vec3D axis = 2 * mg::cross(e0, e1) /
            (e0.length() * e1.length() + mg::dot(e0, e1));

//    here sinPhi and cosPhi are derived from the length of axis ||axis|| = 2 * tan( phi/2 )
    double sinPhi, cosPhi;
    double magnitude = mg::dot(axis, axis);
    extractSinAndCos(magnitude, sinPhi, cosPhi);
    assert( cosPhi >= 0 && cosPhi <= 1 );

    if ( (1 - cosPhi) < mg::ERR )
    {
        io_u = mg::cross(e1, io_u);
        io_u = mg::normalize( mg::cross(io_u, e1) );
        return;
    }
    mg::Quaternion q(cosPhi, sinPhi * mg::normalize(axis));
    mg::Quaternion p(0, io_u);
    p = q * p * mg::conjugate(q);

    io_u.set(p[1], p[2], p[3]);
    io_u.normalize();
}

void ElasticRod::computeMaterialFrame(const ColumnVector &theta,
                                      std::vector<mg::Vec3D>& io_m1,
                                      std::vector<mg::Vec3D>& io_m2) const
{
    mg::Real sinQ, cosQ;
    mg::Vec3D m1, m2;
    for (unsigned i = 0; i < io_m1.size(); ++i)
    {
        cosQ = std::cos(theta(i));
        sinQ = std::sqrt(1 - cosQ * cosQ);

        m1 = cosQ * io_m1[i] + sinQ * io_m2[i];
        m2 = -sinQ * io_m1[i] + cosQ * io_m2[i];

        io_m1[i] = m1;
        io_m2[i] = m2;
    }
}

void ElasticRod::applyInternalConstraintsIteration()
{
    mg::Vec3D e;
    mg::Real l, l1, l2;
    for (unsigned i = 0; i < m_ppos.size() - 1; ++i)
    {
        bool clamped_i = m_isClamped.count(i);
        bool clamped_i1 = m_isClamped.count(i + 1);

        e = m_ppos[i + 1] - m_ppos[i];
//            approximate e.length() with first order accurate Taylor expansion of square root function in the neightbourhood of (restLength^2)
        l = 1 - 2 * m_restEdgeL[i] * m_restEdgeL[i] / (m_restEdgeL[i] * m_restEdgeL[i] +  mg::dot(e, e));

        if (clamped_i)
        {
            l1 = 0;
            l2 = -l;
        }
        else if (clamped_i1)
        {
            l1 = l;
            l2 = 0;
        }
        else
        {
            l1 = m_pmass[i + 1] / (m_pmass[i] + m_pmass[i + 1]) * l;
            l2 = -m_pmass[i] / (m_pmass[i] + m_pmass[i + 1]) * l;
        }

        m_ppos[i] += l1 * e;
        m_ppos[i + 1] += l2 * e;
    }
}

void ElasticRod::accumulateInternalElasticForces(std::vector<mg::Vec3D>& o_forces)
{
    std::vector<mg::Matrix3D> minusGKB(m_edges.size());
    std::vector<mg::Matrix3D> plusGKB(m_edges.size());
    std::vector<mg::Matrix3D> eqGKB(m_edges.size());
    computeGradientKB(m_kb, m_edges, minusGKB, plusGKB, eqGKB);

    std::vector<mg::Vec3D> minusGH(m_kb.size());
    std::vector<mg::Vec3D> plusGH(m_kb.size());
    std::vector<mg::Vec3D> eqGH(m_kb.size());
    computeGradientHolonomyTerms(m_kb, minusGH, plusGH, eqGH);

    mg::Vec2D wkj;
    mg::Matrix2D J;
    mg::matrix_rotation_2D(J, mg::Constants::pi_over_2());

//    compute dE/dQn
    unsigned n = m_edges.size() - 1;
    mg::Real dEdQn;
    computedEdQj(n, m_m1[n], m_m2[n], m_theta, J * m_params->m_B, dEdQn);

    mg::Matrix23D GW;
    mg::Vec3D GH, term;
    for (unsigned i = 0; i < m_ppos.size(); ++i)
    {
        if (m_isClamped.count(i))
        {
            continue;
        }

        for (unsigned k = std::max((int)i - 1, 1); k < m_edges.size(); ++k)
        {
            computeW(m_kb[k], m_m1[k - 1], m_m2[k - 1], wkj);
            computeGradientCurvature(i, k, k - 1,
                                     minusGKB, plusGKB, eqGKB,
                                     minusGH, plusGH, eqGH,
                                     wkj,
                                     J,
                                     GW);

//            o_forces[i] -= (mg::transpose(GW) * m_params->m_B * (wkj - m_restWprev[k])) / m_restRegionL[k];
//            assert( std::isfinite(o_forces[i].length_squared()) );
            term = (mg::transpose(GW) * m_params->m_B * (wkj - m_restWprev[k]));

            computeW(m_kb[k], m_m1[k], m_m2[k], wkj);
            computeGradientCurvature(i, k, k,
                                     minusGKB, plusGKB, eqGKB,
                                     minusGH, plusGH, eqGH,
                                     wkj,
                                     J,
                                     GW);
//            o_forces[i] -= (mg::transpose(GW) * m_params->m_B * (wkj - m_restWnext[k])) / m_restRegionL[k];
//            assert( std::isfinite(o_forces[i].length_squared()) );
            term += (mg::transpose(GW) * m_params->m_B * (wkj - m_restWnext[k]));
            o_forces[i] -= term / m_restRegionL[k];
            assert( std::isfinite(o_forces[i].length_squared()) );
        }

//    need to add  dE/dQn * gradient holonomy(GH) if we have clamped ends since not all twist angles minimize the energy
        if (m_isClamped.size())
        {
            computeGradientHolonomy(i, n, minusGH, plusGH, eqGH, GH);
            o_forces[i] += dEdQn * GH;
            assert( std::isfinite(o_forces[i].length_squared()) );
        }

//        need to limit the force otherwise when e[i] ~ -e[i - 1] ||kb|| goes to infinity => force goes to infinity
        if (o_forces[i].length_squared() > m_params->m_maxElasticForce * m_params->m_maxElasticForce)
        {
            o_forces[i].normalize();
            o_forces[i] *= m_params->m_maxElasticForce;
        }
    }

#ifdef DBUGG
    m_elasticForce = o_forces;
#endif
}

void ElasticRod::computeGradientKB(const std::vector<mg::Vec3D> &kb,
                                   const std::vector<mg::Vec3D> &edges,
                                   std::vector<mg::Matrix3D>& o_minusGKB,
                                   std::vector<mg::Matrix3D>& o_plusGKB,
                                   std::vector<mg::Matrix3D>& o_eqGKB) const
{
// Compute skew-symmetric matrix 3x3 [e], such that [e] * x = cross( e, x )
    std::vector<mg::Matrix3D> edgeMatrix(edges.size());
    for (unsigned i = 0; i < edges.size(); ++i)
    {
        mg::matrix_skew_symmetric(edgeMatrix[i], edges[i]);
    }

    o_minusGKB[0].zero();
    o_plusGKB[0].zero();
    o_eqGKB[0].zero();

    mg::Real scalarFactor;
    for (unsigned i = 1; i < edges.size(); ++i)
    {
        scalarFactor = (m_restEdgeL[i - 1] * m_restEdgeL[i] + mg::dot( edges[i - 1], edges[i] ));
        assert( std::isfinite(scalarFactor) && fabs(scalarFactor) > 0 );

        o_minusGKB[i] = (2.0 * edgeMatrix[i - 1] + mg::outer( kb[i], edges[i - 1] )) / scalarFactor;
        o_plusGKB[i] = (2.0 * edgeMatrix[i] - mg::outer( kb[i], edges[i] )) / scalarFactor;
        o_eqGKB[i] = -(o_plusGKB[i] + o_minusGKB[i]);

//        o_minusGKB[i] = (2.0 * edgeMatrix[i] + mg::outer( kb[i], edges[i] )) / scalarFactor;
//        o_plusGKB[i] = (2.0 * edgeMatrix[i - 1] - mg::outer( kb[i], edges[i - 1] )) / scalarFactor;
//        o_eqGKB[i] = -(o_minusGKB[i] + o_plusGKB[i]);
    }
}

void ElasticRod::computeGradientHolonomyTerms(const std::vector<mg::Vec3D> &kb,
                                       std::vector<mg::Vec3D>& o_minusGH,
                                       std::vector<mg::Vec3D>& o_plusGH,
                                       std::vector<mg::Vec3D>& o_eqGH) const
{
    o_minusGH[0].zero();
    o_plusGH[0].zero();
    o_eqGH[0].zero();

    for (unsigned i = 1; i < kb.size(); ++i)
    {
        o_minusGH[i] = 0.5 * kb[i] / m_restEdgeL[i - 1];
        o_plusGH[i]  = -0.5 * kb[i] / m_restEdgeL[i];
        o_eqGH[i] = -(o_minusGH[i] + o_plusGH[i]);

        assert( std::isfinite( o_minusGH[i].length_squared() ) );
        assert( std::isfinite( o_plusGH[i].length_squared() ) );
        assert( std::isfinite( o_eqGH[i].length_squared() ) );
    }
}

void ElasticRod::computeGradientCurvature(unsigned i, unsigned k, unsigned j,
                                        const std::vector<mg::Matrix3D>& minusGKB,
                                        const std::vector<mg::Matrix3D>& plusGKB,
                                        const std::vector<mg::Matrix3D>& eqGKB,
                                        const std::vector<mg::Vec3D>& minusGH,
                                        const std::vector<mg::Vec3D>& plusGH,
                                        const std::vector<mg::Vec3D>& eqGH,
                                        const mg::Vec2D &wkj,
                                        const mg::Matrix2D &J,
                                        mg::Matrix23D &o_GW) const
{
    assert( k >= (i - 1) && (j == k || j == (k - 1)) && j < m_m1.size() );

//    need to make o_GW zero 3x2 matrix
    o_GW.zero();
//    compute gradient KB(GKB) term
    if (k < i + 2)
    {
        o_GW(0,0) = m_m2[j][0];
        o_GW(0,1) = m_m2[j][1];
        o_GW(0,2) = m_m2[j][2];

        o_GW(1,0) = -m_m1[j][0];
        o_GW(1,1) = -m_m1[j][1];
        o_GW(1,2) = -m_m1[j][2];

        if (k == (i - 1))
        {
            o_GW = o_GW * plusGKB[k];
        } else if (k == i)
        {
            o_GW = o_GW * eqGKB[k];
        } else if (k == i + 1)
        {
            o_GW = o_GW * minusGKB[k];
        }
    }
//    compute gradient Holonomy(GH) term
    mg::Vec3D GH;
    computeGradientHolonomy(i, j, minusGH, plusGH, eqGH, GH);
    o_GW -= J * mg::outer(wkj, GH);
}

void ElasticRod::computeGradientHolonomy(unsigned i , unsigned j,
                                 const std::vector<mg::Vec3D>& minusGH,
                                 const std::vector<mg::Vec3D>& plusGH,
                                 const std::vector<mg::Vec3D>& eqGH,
                                 mg::Vec3D& o_GH) const
{
    o_GH.zero();

    if (j >= (i - 1) && i > 1 && (i - 1) < plusGH.size())
    {
        o_GH += plusGH[i - 1];
    }
    if (j >= i && i < eqGH.size())
    {
        o_GH += eqGH[i];
    }
    if (j >= (i + 1) && (i + 1) < minusGH.size())
    {
        o_GH += minusGH[i + 1];
    }
}

void ElasticRod::computeEnergy(const std::vector<mg::Vec3D>& m1,
                               const std::vector<mg::Vec3D>& m2,
                               const ColumnVector &theta,
                               mg::Real &o_E) const
{
    o_E = 0.0;
    mg::Vec2D wij;
    mg::Real mi;
    for (unsigned i = 1; i < m_edges.size(); ++i)
    {
//        bend energy term
        computeW(m_kb[i], m1[i - 1], m2[i - 1], wij);
        o_E += mg::dot(wij - m_restWprev[i], m_params->m_B * (wij - m_restWprev[i])) * 0.5 / m_restRegionL[i];

        computeW(m_kb[i], m1[i], m2[i], wij);
        o_E += mg::dot(wij - m_restWnext[i], m_params->m_B * (wij - m_restWnext[i])) * 0.5 / m_restRegionL[i];

//        twist energy term
        mi = (theta(i) - theta(i - 1));
        o_E += m_params->m_beta * mi * mi / m_restRegionL[i];
    }
}

void ElasticRod::computedEdQj(unsigned j,
                              const mg::Vec3D& m1j,
                              const mg::Vec3D& m2j,
                              const ColumnVector &theta,
                              const mg::Matrix2D &JB,
                              mg::Real &o_dEQj) const
{
    o_dEQj = 0.0;
    mg::Vec2D wij;
    mg::Real term;
//    compute first term dWj/dQj + 2 * beta * mj / lj
    if (j > 0)
    {
        computeW(m_kb[j], m1j, m2j, wij);

        term = mg::dot(wij, JB * (wij - m_restWnext[j]));
        term += 2 * m_params->m_beta * (theta(j) - theta(j - 1));
        term /= m_restRegionL[j];

        o_dEQj += term;
    }
//    compute second term dWj+1/dQj - 2 * beta * mj+1 / lj+1
    if (j < m_edges.size() - 1)
    {
        computeW(m_kb[j + 1], m1j, m2j, wij);

        term = mg::dot(wij, JB * (wij - m_restWprev[j + 1]));
        term -= 2 * m_params->m_beta * (theta(j + 1) - theta(j));
        term /= m_restRegionL[j + 1];

        o_dEQj += term;
    }
}

void ElasticRod::computeHessian(unsigned j,
                                const mg::Vec3D& m1j,
                                const mg::Vec3D& m2j,
                                const mg::Matrix2D& J,
                                mg::Real &o_Hjjm1,
                                mg::Real &o_Hjj,
                                mg::Real &o_Hjjp1) const
{
    o_Hjjm1 = o_Hjj = o_Hjjp1 = 0;

    mg::Vec2D wij;
    double hjj;
    if (j > 0)
    {
        o_Hjjm1 = -2 * m_params->m_beta / m_restRegionL[j];

        computeW(m_kb[j], m1j, m2j, wij);

        hjj = 2 * m_params->m_beta;
        hjj += mg::dot( wij, mg::transpose(J) * m_params->m_B * J * wij );
        hjj -= mg::dot( wij, m_params->m_B * (wij - m_restWnext[j]) );
        hjj /= m_restRegionL[j];

        o_Hjj = hjj;
    }
    if (j < m_edges.size() - 1)
    {
        o_Hjjp1 = -2 * m_params->m_beta / m_restRegionL[j + 1];

        computeW(m_kb[j + 1], m1j, m2j, wij);
        hjj = 2 * m_params->m_beta;
        hjj += mg::dot( wij, mg::transpose(J) * m_params->m_B * J * wij );
        hjj -= mg::dot( wij, m_params->m_B * (wij - m_restWprev[j + 1]) );
        hjj /= m_restRegionL[j + 1];

        o_Hjj += hjj;
    }
}

void ElasticRod::updateCurrentState()
{
//    parallel transport first frame in time
    mg::Vec3D e0 = m_edges[0];
    computeEdges(m_ppos, m_edges);
    mg::Vec3D e1 = m_edges[0];

    parallelTransportFrame(e0, e1, m_u0);
    computeBishopFrame(m_u0, m_edges, m_kb, m_m1, m_m2);

    double minE = 0;
    if (m_params->m_strategy != ElasticRodParams::NONE)
    {
        minE = m_minimization->minimize(this, m_theta);
    }
    computeMaterialFrame(m_theta, m_m1, m_m2);

#ifdef DBUGG
    mg::Real E;
    computeEnergy(m_m1, m_m2, m_theta, E);
    std::cout<< "Theta:" << m_theta << "\n";
    std::cout<< "Total Energy: " << E << " MIN Energy: " << minE << std::endl;
#endif
}

void ElasticRod::printself()
{
    for (std::vector<mg::Vec3D>::iterator it = m_ppos.begin();it!=m_ppos.end();it++)
    {
        std::cout << "(";
        for (int i=0;i<it->dimension;i++)
        {
            std::cout << it->data()[i]<<",";
        }
        std::cout << ")";
    }
    std::cout << std::endl;

}

//====================================== MinimizationPImpl implementation =======================================

void ElasticRod::MinimizationPImpl::extractThetaVars(const ElasticRod *rod, const ColumnVector& theta, ColumnVector& o_thetaVars)
{
    o_thetaVars.set_size(theta.size() - rod->m_isClamped.size());
    unsigned j = 0;
    for (unsigned i = 0; i < theta.size(); ++i)
    {
        if (rod->m_isClamped.count(i) || rod->m_isClamped.count(i + 1))
        {
            continue;
        }
        o_thetaVars(j) = theta(i);
        ++j;
    }
}

void ElasticRod::MinimizationPImpl::constructTheta(const ElasticRod *rod, const ColumnVector& thetaVars, ColumnVector& o_theta)
{

    unsigned j = 0;
    for (unsigned i = 0; i < o_theta.size(); ++i)
    {
        if (rod->m_isClamped.count(i) || rod->m_isClamped.count(i + 1))
        {
            continue;
        }
        o_theta(i) = thetaVars(j);
        ++j;
    }
}

double ElasticRod::MinimizationPImpl::minimize(const ElasticRod* rod, ColumnVector &io_theta)
{
    m_evaluate.m_rod = rod;
    m_evaluateGradient.m_rod = rod;
    m_evaluateHessian.m_rod = rod;

    ColumnVector thetaVars;
    extractThetaVars(rod, io_theta, thetaVars);

    double minE = -1;
    switch (rod->m_params->m_strategy) {
    case ElasticRodParams::BFGS_NUMERIC:
        minE = dlib::find_min(dlib::bfgs_search_strategy(),
                             dlib::objective_delta_stop_strategy(rod->m_params->m_tolerance, rod->m_params->m_maxIter),
                             m_evaluate,
                             dlib::derivative(m_evaluate),
                             thetaVars,
                             0.0);
        break;
    case ElasticRodParams::BFGS:
        minE = dlib::find_min(dlib::bfgs_search_strategy(),
                             dlib::objective_delta_stop_strategy(rod->m_params->m_tolerance, rod->m_params->m_maxIter),
                             m_evaluate,
                             m_evaluateGradient,
                             thetaVars,
                             0.0);
        break;
    case ElasticRodParams::NEWTON:
        minE = dlib::find_min(dlib::newton_search_strategy(m_evaluateHessian),
                             dlib::objective_delta_stop_strategy(rod->m_params->m_tolerance, rod->m_params->m_maxIter),
                             m_evaluate,
                             m_evaluateGradient,
                             thetaVars,
                             0.0);
        break;
    default:
        break;
    }

    constructTheta(rod, thetaVars, io_theta);

    return minE;
}

double ElasticRod::MinimizationPImpl::evaluate::operator ()(const ColumnVector& theta) const
{
//    TODO FIX: can I avoid copying ?????
//    keeping m1, m2 as members and copying the data gives ~ 1ms performance benefit
    std::vector<mg::Vec3D> m1 = m_rod->m_m1;
    std::vector<mg::Vec3D> m2 = m_rod->m_m2;
    ColumnVector theta_full = m_rod->m_theta;

    constructTheta(m_rod, theta, theta_full);
    m_rod->computeMaterialFrame(theta_full, m1, m2);

    mg::Real E;
    m_rod->computeEnergy(m1, m2, theta_full, E);
    return (double)E;
}

ColumnVector ElasticRod::MinimizationPImpl::evaluateGradient::operator ()(const ColumnVector &theta) const
{
    ColumnVector gradient(theta.size());
//    TODO FIX: can I avoid copying ?????
    std::vector<mg::Vec3D> m1 = m_rod->m_m1;
    std::vector<mg::Vec3D> m2 = m_rod->m_m2;
    ColumnVector theta_full = m_rod->m_theta;

    constructTheta(m_rod, theta, theta_full);
    m_rod->computeMaterialFrame(theta_full, m1, m2);

    mg::Matrix2D JB;
    mg::matrix_rotation_2D(JB, mg::Constants::pi_over_2());
    JB *= m_rod->m_params->m_B;

    mg::Real dEdQj;
    unsigned j = 0;
    for (unsigned i = 0; i < theta_full.size(); ++i)
    {
        if (m_rod->m_isClamped.count(i) || m_rod->m_isClamped.count(i + 1))
        {
            continue;
        }
        m_rod->computedEdQj(i, m1[i], m2[i], theta_full, JB, dEdQj);
        gradient(j) = dEdQj;
        ++j;
    }

    return gradient;
}

Hessian ElasticRod::MinimizationPImpl::evaluateHessian::operator ()(const ColumnVector& theta) const
{
    Hessian hessian(theta.size(), theta.size());
    hessian = dlib::zeros_matrix(hessian);
//    TODO FIX: can I avoid copying ?????
    std::vector<mg::Vec3D> m1 = m_rod->m_m1;
    std::vector<mg::Vec3D> m2 = m_rod->m_m2;
    ColumnVector theta_full = m_rod->m_theta;

    constructTheta(m_rod, theta, theta_full);
    m_rod->computeMaterialFrame(theta_full, m1, m2);

    mg::Matrix2D J;
    mg::matrix_rotation_2D(J, mg::Constants::pi_over_2());
    mg::Real Hjjm1, Hjj, Hjjp1;
    unsigned j = 0;
    for (unsigned i = 0; i < m_rod->m_theta.size(); ++i)
    {
        if (m_rod->m_isClamped.count(i) || m_rod->m_isClamped.count(i + 1))
        {
            continue;
        }

        m_rod->computeHessian(i, m1[i], m2[i], J, Hjjm1, Hjj, Hjjp1);

        if (j > 0)
        {
            hessian(j, j - 1) = Hjjm1;
        }
        hessian(j, j) = Hjj;
        if ((j + 1) < theta.size())
        {
            hessian(j, j + 1) = Hjjp1;
        }
        ++j;
    }

    return hessian;
}

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

//    mg::Real dEdQj;
//        for (unsigned j = std::max((int)(i) - 1, 0); j < m_edges.size(); ++j)
//        {
//            computedEdQj(j, J * m_B, dEdQj);
//            computeGradientHolonomySum(i, n, minusGH, plusGH, eqGH, GH);
//            o_forces[i] += dEdQj * GH;
//        }

//void ElasticRod::computeElasticForces(const std::vector<mg::Vec3D>& vertices, std::vector<mg::Vec3D>& o_forces)
//{
////    TODO: need to implement minimization of energy for twistAngles first

//    std::vector<mg::Matrix3D> minusGKB(3);
//    std::vector<mg::Matrix3D> eqGKB(3);
//    std::vector<mg::Matrix3D> plusGKB(3);
//    std::vector<mg::Vec3D> minusGH(3);
//    std::vector<mg::Vec3D> eqGH(3);
//    std::vector<mg::Vec3D> plusGH(3);
//    for (unsigned i = 0; i < minusGKB.size(); ++i)
//    {
//        minusGKB.zero();
//        eqGKB.zero();
//        plusGKB.zero();
//        minusGH.zero();
//        eqGH.zero();
//        plusGH.zero();
//    }

//    mg::Matrix23D gw;
//    for (unsigned i = 0; i < vertices.size(); ++i)
//    {
//        computeGKBandGH(i + 1, minusGKB, eqGKB, plusGKB, minusGH, eqGH, plusGH);

//        if (m_isClamped.count(i))
//        {
//            continue;
//        } ...

//void ElasticRod::computeGKBandGH(unsigned idx,
//                                std::vector<mg::Matrix3D> &o_minusGKB,
//                                std::vector<mg::Matrix3D> &o_eqGKB,
//                                std::vector<mg::Matrix3D> &o_plusGKB,
//                                std::vector<mg::Vec3D> &o_minusGH,
//                                std::vector<mg::Vec3D> &o_eqGH,
//                                std::vector<mg::Vec3D> &o_plusGH) const
//{
//    assert(idx > 0 && idx < m_edges.size());

//    for (unsigned i = 0; i < o_minusGKB.size() - 1; ++i)
//    {
//        o_minusGKB[i] = o_minusGKB[i + 1];
//        o_eqGKB[i] = o_eqGKB[i + 1];
//        o_plusGKB[i] = o_plusGKB[i + 1];

//        o_minusGH[i] = o_minusGH[i + 1];
//        o_eqGH[i] = o_eqGH[i + 1];
//        o_plusGH[i] = o_plusGH[i + 1];
//    }

//    mg::Real scalarFactor = (m_restEdgeL[idx - 1] * m_restEdgeL[idx] + mg::dot( m_edges[idx - 1], m_edges[idx] ));;
//    assert( std::isfinite(scalarFactor) && fabs(scalarFactor) > 0 );
//    mg::Matrix3D edgeMatrix;
//    mg::matrix_skew_symmetric(edgeMatrix, edges[idx]);

//    o_minusGKB[i] = (2.0 * edgeMatrix[i] + mg::outer( kb[i], edges[i] )) / scalarFactor;
//    o_plusGKB[i] = (2.0 * edgeMatrix[i - 1] - mg::outer( kb[i], edges[i - 1] )) / scalarFactor;
//    o_eqGKB[i] = -(o_minusGKB[i] + o_plusGKB[i]);

//}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <string>
#include <type_traits>
using namespace boost::hana;

int main() {

{

//! [concat]
using namespace literals;
BOOST_HANA_CONSTEXPR_CHECK(
    concat(make<Tuple>(1, '2'), make<Tuple>(3.3, 4_c)) == make<Tuple>(1, '2', 3.3, 4_c)
);
//! [concat]

}{

//! [empty]
BOOST_HANA_CONSTANT_CHECK(empty<Tuple>() == make<Tuple>());
BOOST_HANA_CONSTANT_CHECK(empty<Maybe>() == nothing);
//! [empty]

}{

//! [prepend]
BOOST_HANA_CONSTEXPR_CHECK(prepend(1, make<Tuple>()) == make<Tuple>(1));
BOOST_HANA_CONSTEXPR_CHECK(prepend(1, make<Tuple>('2', 3.3)) == make<Tuple>(1, '2', 3.3));
BOOST_HANA_CONSTEXPR_CHECK(
    prepend(1, prepend('2', prepend(3.3, make<Tuple>()))) == make<Tuple>(1, '2', 3.3)
);
//! [prepend]

}{

//! [append]
BOOST_HANA_CONSTEXPR_CHECK(append(make<Tuple>(), 1) == make<Tuple>(1));
BOOST_HANA_CONSTEXPR_CHECK(append(make<Tuple>(1, '2'), 3.3) == make<Tuple>(1, '2', 3.3));
BOOST_HANA_CONSTEXPR_CHECK(
    append(append(append(make<Tuple>(), 1), '2'), 3.3) == make<Tuple>(1, '2', 3.3)
);
//! [append]

}{

//! [filter]
BOOST_HANA_CONSTEXPR_CHECK(
    filter(make<Tuple>(1, 2.0, 3, 4.0), trait_<std::is_integral>) == make<Tuple>(1, 3)
);

BOOST_HANA_CONSTEXPR_CHECK(
    filter(just(3), trait_<std::is_integral>) == just(3)
);

BOOST_HANA_CONSTANT_CHECK(
    filter(just(3.0), trait_<std::is_integral>) == nothing
);
//! [filter]

}{

//! [cycle]
BOOST_HANA_CONSTEXPR_CHECK(
    cycle(size_t<2>, make<Tuple>('x', 'y', 'z')) == make<Tuple>('x', 'y', 'z', 'x', 'y', 'z')
);
//! [cycle]

}{

//! [repeat]
BOOST_HANA_CONSTEXPR_CHECK(repeat<Tuple>(size_t<2>, 'x') == make<Tuple>('x', 'x'));

// Of course, because Maybe can hold at most one element.
static_assert(repeat<Maybe>(size_t<2>, 'x') == just('x'), "");
//! [repeat]

}{

//! [prefix]
using namespace std::literals;
BOOST_HANA_RUNTIME_CHECK(
    prefix("my"s, make<Tuple>("dog"s, "car"s, "house"s)) ==
    make<Tuple>("my", "dog", "my", "car", "my", "house")
);
//! [prefix]

}{

//! [suffix]
BOOST_HANA_CONSTEXPR_CHECK(
    suffix(0, make<Tuple>(1, 2, 3, 4)) == make<Tuple>(1, 0, 2, 0, 3, 0, 4, 0)
);
//! [suffix]

}

}


/*
 * test_statinfos.cpp
 *
 *  Created on: 13 mai 2016
 *      Author: boubad
 */
///////////////////////////////////
#include <boost/test/unit_test.hpp>
///////////////////////////
#include "indivproviderfixture.h"
/////////////////////////
#include <statinfo.h>
#include <indivcluster.h>
//////////////////////
using namespace std;
using namespace info;
/////////////////////////////
BOOST_FIXTURE_TEST_SUITE(MiscTestSuite, IndivProviderFixture)
;
BOOST_AUTO_TEST_CASE(testStatInfo) {
	IIndivProvider *pProvider = m_provider.get();
	statinfos_map oRes;
	size_t n = info_global_compute_stats(pProvider, oRes);
	BOOST_CHECK(n == this->m_nbcols);
} //testStatInfo
BOOST_AUTO_TEST_CASE(testComputeDistances) {
	IIndivProvider *pProvider = m_provider.get();
	//
	size_t nc = 0;
	bool bRet = pProvider->indivs_count(nc);
	BOOST_CHECK(bRet);
	BOOST_CHECK(m_nbrows == nc);
	//
	IndivDistanceMap oDistances;
	info_global_compute_distances(pProvider, oDistances);
	//
	const ints_set &oSetIndivs = oDistances.indexes();
	BOOST_CHECK(oSetIndivs.size() == nc);
	//
	for (size_t i = 0; i < nc; ++i) {
		Indiv oInd1;
		bRet = pProvider->find_indiv_at(i, oInd1);
		BOOST_CHECK(bRet);
		IntType aIndex1 = oInd1.id();
		BOOST_CHECK(aIndex1 != 0);
		for (size_t j = 0; j < i; ++j) {
			Indiv oInd2;
			bRet = pProvider->find_indiv_at(j, oInd2);
			BOOST_CHECK(bRet);
			IntType aIndex2 = oInd2.id();
			BOOST_CHECK(aIndex2 != 0);
			double dRes = 0;
			bRet = oDistances.get(aIndex1, aIndex2, dRes);
			BOOST_CHECK(bRet);
			BOOST_CHECK(dRes > 0);
		} // j
	} // i
} //testComputeDistances

BOOST_AUTO_TEST_SUITE_END();


//==============================================================================
//         Copyright 2003 - 2014   LASMEA UMR 6602 CNRS/UBP
//         Copyright 2009 - 2014   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
// cover for functor fast_tand in scalar mode
#include <nt2/trigonometric/include/functions/fast_tand.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <cmath>
#include <iostream>
#include <nt2/sdk/unit/args.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/cover.hpp>
#include <vector>

extern "C" {extern long double cephes_tanl(long double);}

NT2_TEST_CASE_TPL(fast_tand_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::unit::args;
  const std::size_t NR = args("samples", NT2_NB_RANDOM_TEST);
  const double ulpd = args("ulpd", 2.5);

  const T min = args("min", T(-45));
  const T max = args("max", T(45));
  std::cout << "Argument samples #0 chosen in range: [" << min << ",  " << max << "]" << std::endl;
  NT2_CREATE_BUF(a0,T, NR, min, max);
    const long double long_deginrad = 0.017453292519943295769236907684886l;

  std::vector<T> ref(NR);
  for(std::size_t i=0; i!=NR; ++i)
    ref[i] = ::cephes_tanl(a0[i]*long_deginrad);

  NT2_COVER_ULP_EQUAL(nt2::tag::fast_tand_, ((T, a0)), ref, ulpd);

}