#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 "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 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// 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)
//=======================================================================
//
//This algorithm is described in "Network Flows: Theory, Algorithms, and Applications"
// by Ahuja, Magnanti, Orlin.

#ifndef BOOST_GRAPH_SUCCESSIVE_SHORTEST_PATH_HPP
#define BOOST_GRAPH_SUCCESSIVE_SHORTEST_PATH_HPP

#include <numeric>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/pending/relaxed_heap.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/detail/augment.hpp>

namespace boost {

namespace detail {

template <class Graph, class Weight, class Distance, class Reversed>
class MapReducedWeight :
    public put_get_helper<typename property_traits<Weight>::value_type, MapReducedWeight<Graph, Weight, Distance, Reversed> > {
    typedef graph_traits<Graph> gtraits;
public:
    typedef boost::readable_property_map_tag category;
    typedef typename property_traits<Weight>::value_type value_type;
    typedef value_type reference;
    typedef typename gtraits::edge_descriptor key_type;
    MapReducedWeight(const Graph & g, Weight w, Distance d, Reversed r) :
        g_(g), weight_(w), distance_(d), rev_(r) {}

    reference operator[](key_type v) const {
        return get(distance_, source(v, g_)) - get(distance_,target(v, g_)) + get(weight_, v);
    }
private:
    const Graph & g_;
    Weight weight_;
    Distance distance_;
    Reversed rev_;
};

template <class Graph, class Weight, class Distance, class Reversed>
MapReducedWeight<Graph, Weight, Distance, Reversed>
make_mapReducedWeight(const Graph & g, Weight w, Distance d, Reversed r)  {
    return MapReducedWeight<Graph, Weight, Distance, Reversed>(g, w, d, r);
}

}//detail

template <class Graph, class Capacity, class ResidualCapacity, class Reversed, class Pred, class Weight, class Distance, class Distance2, class VertexIndex>
void successive_shortest_path_nonnegative_weights(
        const Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance distance,
        Distance2 distance_prev) {
    filtered_graph<const Graph, is_residual_edge<ResidualCapacity> >
        gres = detail::residual_graph(g, residual_capacity);
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

    BGL_FORALL_EDGES_T(e, g, Graph) {
        put(residual_capacity, e, get(capacity, e));
    }

    BGL_FORALL_VERTICES_T(v, g, Graph) {
        put(distance_prev, v, 0);
    }

    while(true) {
        BGL_FORALL_VERTICES_T(v, g, Graph) {
            put(pred, v, edge_descriptor());
        }
        dijkstra_shortest_paths(gres, s,
                weight_map(detail::make_mapReducedWeight(gres, weight, distance_prev, rev)).
                distance_map(distance).
                vertex_index_map(index).
                visitor(make_dijkstra_visitor(record_edge_predecessors(pred, on_edge_relaxed()))));

        if(get(pred, t) == edge_descriptor()) {
            break;
        }

        BGL_FORALL_VERTICES_T(v, g, Graph) {
            put(distance_prev, v, get(distance_prev, v) + get(distance, v));
        }

        detail::augment(g, s, t, pred, residual_capacity, rev);
    }
}

//in this namespace argument dispatching tak place
namespace detail {

template <class Graph, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance, class Distance2, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch3(
        const Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance dist,
        Distance2 dist_pred) {
    successive_shortest_path_nonnegative_weights(g, s, t, capacity, residual_capacity, weight, rev, index, pred, dist, dist_pred);
}

//setting default distance map
template <class Graph, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch3(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance dist,
        param_not_found) {
    typedef typename property_traits<Weight>::value_type D;

    std::vector<D> d_map(num_vertices(g));

    successive_shortest_path_nonnegative_weights(g, s, t, capacity, residual_capacity, weight, rev, index, pred, dist,
                             make_iterator_property_map(d_map.begin(), index));
}

template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch2(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance dist,
        const bgl_named_params<P, T, R>& params) {
    successive_shortest_path_nonnegative_weights_dispatch3(g, s, t, capacity, residual_capacity, weight, rev, index, pred, dist, get_param(params, vertex_distance2));
}

//setting default distance map
template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch2(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        param_not_found,
        const bgl_named_params<P, T, R>& params) {
    typedef typename property_traits<Weight>::value_type D;

    std::vector<D> d_map(num_vertices(g));

    successive_shortest_path_nonnegative_weights_dispatch3(g, s, t, capacity, residual_capacity, weight, rev, index, pred,
            make_iterator_property_map(d_map.begin(), index),
            get_param(params, vertex_distance2));
}

template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch1(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        const bgl_named_params<P, T, R>& params) {
    successive_shortest_path_nonnegative_weights_dispatch2(g, s, t, capacity, residual_capacity, weight,  rev, index, pred,
                                get_param(params, vertex_distance), params);
}

//setting default predecessors map
template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch1(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        param_not_found,
        const bgl_named_params<P, T, R>& params) {
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    std::vector<edge_descriptor> pred_vec(num_vertices(g));

    successive_shortest_path_nonnegative_weights_dispatch2(g, s, t, capacity, residual_capacity, weight, rev, index,
            make_iterator_property_map(pred_vec.begin(), index),
            get_param(params, vertex_distance), params);
}

}//detail

template <class Graph, class P, class T, class R>
void successive_shortest_path_nonnegative_weights(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        const bgl_named_params<P, T, R>& params) {

    return detail::successive_shortest_path_nonnegative_weights_dispatch1(g, s, t,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
           get_param(params, vertex_predecessor),
           params);
}

template <class Graph>
void successive_shortest_path_nonnegative_weights(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t) {
    bgl_named_params<int, buffer_param_t> params(0);
    successive_shortest_path_nonnegative_weights(g, s, t, params);
}

}//boost
#endif /* BOOST_GRAPH_SUCCESSIVE_SHORTEST_PATH_HPP */


#pragma once

#include <armadillo>
#include <algorithm>
#include <random>
#include <cassert>

#include "net.hpp"

template <typename activation = Logistic, typename error = Squared_Error>
void train_online(FeedForward_Network<activation, error>& network,
    arma::Mat<float> inputs, arma::Mat<float> targets, float learning_rate) {
    for (int i = 0; i < targets.n_rows; ++i) {
      calculate_activation(network, inputs.row(i));
      backprop(network, targets.row(i), learning_rate);
    }
}

template <typename activation, typename error>
void train_batch(FeedForward_Network<activation, error>& network,
    arma::Mat<float> inputs, arma::Mat<float> targets, int batch_size, float learning_rate) {
    network.resize_activation(batch_size);

    int batches_in_train = targets.n_rows/batch_size - 1;
    for (int i = 0; i < batches_in_train; ++i) {
      arma::Mat<float> input_slice = inputs.rows(i*batch_size, (i+1) * batch_size-1);
      calculate_activation(network, input_slice);
      arma::Mat<float> target_slice = targets.rows(i*batch_size, (i+1) * batch_size-1);
      backprop(network, target_slice, learning_rate);
    }
}

//Randomize weights in a network.
template <typename activation, typename error>
void randomize(FeedForward_Network<activation, error>& network, float standard_deviation = 0.05) {
  std::default_random_engine generator;
  std::normal_distribution<float> distribution(0, standard_deviation);

  auto random_num = [&]() {return distribution(generator);};
  for (int i=0; i < network.weights.size(); ++i) {
    network.weights[i].imbue(random_num);
    //TODO figure out which one is right
    //avoid local nearby local maximum
    network.last_weights[i].imbue(random_num);
    //network.last_weights[i] = network.weights[i];
  }
}

template <typename arma_t, typename activation, typename error>
void backprop(FeedForward_Network<activation, error> &network,
    arma_t target, float learning_rate = 0.8f, float momentum = 0.8f) {
  //Calculate deltas

  //output delta first
  network.deltas.back() = error::error_dir(target, network.activations.back()) % activation::activation_dir(network.activations.back());

  //rest of the delta
  for (int i = network.deltas.size() - 2; i >= 0; --i) {
    network.deltas[i] = (network.deltas[i+1] * network.weights[i+1].t()) % activation::activation_dir(network.activations[i+1]);
  }

  //update weights
  for (int i=0; i < network.weights.size(); ++i) {
    auto & standard_piece = (1 - momentum) * learning_rate * (network.deltas[i].t() * network.activations[i]).t();
    auto & momentum_piece = momentum * (network.weights[i] - network.last_weights[i]);
    arma::Mat<float> delta_weights = standard_piece + momentum_piece;
    network.last_weights[i] = network.weights[i];
    network.weights[i] += delta_weights;
  }
}

template <typename arma_t, typename activation, typename error>
void calculate_activation(FeedForward_Network<activation, error>& network,
    arma_t input) {

  network.activations[0] = input;
  for(int i=1; i < network.activations.size(); ++i) {
    network.activations[i] = network.activations[i-1] * network.weights[i-1];
    network.activations[i] = activation::activation(network.activations[i]);
  }
}

//TODO remove this function??
template <typename activation, typename error>
arma::Mat<float> predict(FeedForward_Network<activation, error>& network,
    arma::Mat<float> input) {
  calculate_activation(network, input);
  return network.activations.back();
}

//Scoring function for classification.
inline double classify_percent_score(arma::Mat<float> result, arma::Mat<float> correct) {
  assert(result.n_cols == correct.n_cols);
  int num_correct = 0;
  for (int i=0; i < result.n_rows; ++i) {
    auto sort_vec = arma::sort_index(result.row(i), 1);
    if (correct.row(i)[sort_vec[0]] == 1) {
      num_correct += 1;
    }
  }
  return static_cast<float>(num_correct) / static_cast<float>(result.n_rows);
}

//Scoring function that calculates the difference in squares between two matrices.
inline float squared_diff(arma::Mat<float> result, arma::Mat<float> correct) {
  assert(result.n_cols == correct.n_cols);
  auto error_diff = correct - result;
  return arma::accu(error_diff % error_diff);
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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 <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

int main(int argc, char** argv)
{

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  graph g(11);
  add_edge(0,1,g);
  add_edge(2,3,g);
  add_edge(3,0,g);
  add_edge(3,4,g);
  add_edge(4,5,g);
  add_edge(5,3,g);
  add_edge(5,6,g);
  add_edge(6,7,g);
  add_edge(7,8,g);
  add_edge(8,5,g);
  add_edge(8,9,g);
  add_edge(0,10,g);

  //Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity; compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                     &embedding[0]
                                   )
      )
    std::cout << "Input graph is planar" << std::endl;
  else
    std::cout << "Input graph is not planar" << std::endl;

  typedef std::vector< graph_traits<graph>::edges_size_type >
    component_storage_t;
  typedef iterator_property_map
    < component_storage_t::iterator,
      property_map<graph, edge_index_t>::type
    >
    component_map_t;

  component_storage_t component_storage(num_edges(g));
  component_map_t component(component_storage.begin(), get(edge_index, g));

  std::cout << "Before calling make_biconnected_planar, the graph has "
            << biconnected_components(g, component)
            << " biconnected components" << std::endl;

  make_biconnected_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  // Re-size the storage for the biconnected components, since we
  // just added a few edges

  component_storage.resize(num_edges(g));
  component = component_map_t(component_storage.begin(), get(edge_index,g));

  std::cout << "After calling make_biconnected_planar, the graph has "
            << biconnected_components(g, component)
            << " biconnected components" << std::endl;

  if (boyer_myrvold_planarity_test(g))
    std::cout << "Also, the graph is still planar." << std::endl;
  else
    std::cout << "But the graph is not still planar." << std::endl;

  return 0;
}


/**
Good tutorial: http://www.informit.com/articles/article.aspx?p=673259
*/

#include <algorithm>
#include <fstream>
#include <iostream>
#include <set>
#include <vector>

#include <boost/config.hpp>
//-lboost_graph
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

int main() {

    /*
    #Graph

        The following class hierarchy exists:

            BidirectionalGraph -------- Incience ---------+
                                                          |
                                        Adjacency --------+
                                                          |
            VertexAndEdgeList ----+---- VertexList -------+---- Graph
                                  |                       |
                                  +---- EdgeList ---------+
                                                          |
                                        AdjacenyMatrix ---+
    */
    {
        /*
        #properties

            Properties are values associated to edges and vertices.
        */
        {
            /*
            There are a few predefined properties which you should use whenever possible
            as they are already used in many algorithms, but you can also define your own properties.

            Predefined properties include:

            - `edge_weight_t`. Used for most algorithms that have a single value associated to each
                edge such as Dijikstra.

            - `vertex_name_t`
            */
            {
                typedef boost::property<boost::vertex_name_t, std::string> VertexProperties;
                typedef boost::property<boost::edge_weight_t, int> EdgeProperties;
            }

            /*
            Multiple properties can be specified either by:

            - using a custom class as the property type. TODO is there any limitation to this?
            - chaining multile properties
            */
            {
            }

            /*
            The absense of a property is speficied by boost::no_property.
            */
            {
                typedef boost::no_property VertexProperties;
            }

        }

        typedef boost::property<boost::vertex_name_t, std::string> VertexProperties;
        typedef boost::property<boost::edge_weight_t, int> EdgeProperties;
        typedef boost::adjacency_list<
            // Data structure to represent the out edges for each vertex.
            // Possibilities:
            //
            // #vecS selects std::vector.
            // #listS selects std::list.
            // #slistS selects std::slist.
            // #setS selects std::set.
            // #multisetS selects std::multiset.
            // #hash_setS selects std::hash_set.
            //
            // `S` standas for Selector.
            boost::vecS,

            // Data structure to represent the vertex set.
            boost::vecS,

            // Directed type.
            // #bidirectionalS: directed graph with access to in and out edges
            // #directedS:      directed graph with access only to out-edges
            // #undirectedS:    undirected graph
            boost::bidirectionalS,

            // Optional.
            VertexProperties,

            // Optional.
            EdgeProperties
        > Graph;
        //typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
        //typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
        //typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;

        // Fix number of vertices, and add one edge at a time.
        int num_vertices = 3;
        Graph g(num_vertices);
        boost::add_edge(0, 1, g);
        boost::add_edge(1, 2, g);

        // Fix number of vertices, and add one edge array.
        {
            int num_vertices = 3;
            typedef std::pair<int, int> Edge;
            std::vector<Edge> edges{
                {0, 1},
                {1, 2},
            };
            Graph g(edges.data(), edges.data() + edges.size(), num_vertices);
        }

        // It is also possible to add vertices with #add_vertex.

        //#vertices
        {
            // Number of vertices.
            boost::graph_traits<Graph>::vertices_size_type num_vertices = boost::num_vertices(g);
            assert(num_vertices == 3u);

            //#vertices() Returns a begin() end() vertex iterator pair so we know where to stop.
            {
                typedef std::vector<boost::graph_traits<Graph>::vertex_descriptor> Vertices;
                Vertices vertices;
                vertices.reserve(num_vertices);
                //IndexMap
                auto index = boost::get(boost::vertex_index, g);
                //std::pair<vertex_iter, vertex_iter> vp
                for (auto vp = boost::vertices(g); vp.first != vp.second; ++vp.first) {
                    // Vertex
                    auto v = *vp.first;
                    vertices.push_back(index[v]);
                }
                assert((vertices == Vertices{0, 1, 2}));
            }

            // The iterator is a ranom access iterator.
            {
                auto index = boost::get(boost::vertex_index, g);
                auto it = boost::vertices(g).first;
                assert(index[it[2]] == 2);
                assert(index[it[1]] == 1);
            }
        }

        //#edges
        {
            // It seems that only AdjencyMatrix has a method to get an edge given two vertices:
            //edge(u, v, g)
        }
    }

    //#source is also a global function: <http://stackoverflow.com/questions/16114616/why-is-boost-graph-librarys-source-a-global-function>

    //#dijikstra
    std::cout << "#dijkstra" << std::endl;
    {
        typedef boost::adjacency_list<
            boost::listS,
            boost::vecS,
            boost::directedS,
            boost::no_property,
            boost::property<boost::edge_weight_t, int>
        > Graph;
        typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
        typedef boost::graph_traits<Graph>::edge_descriptor edge_descriptor;
        typedef std::pair<int, int> Edge;

        // Model inputs.
        const int num_nodes = 5;
        const int sorce = 0;
        std::vector<Edge> edges{
            {0, 2}, {1, 1}, {1, 3}, {1, 4}, {2, 1},
            {2, 3}, {3, 4}, {4, 0}, {4, 1}
        };
        std::vector<int> weights{
            1, 2, 1, 2, 7,
            3, 1, 1, 1
        };

        // Solve.
        Graph g(edges.data(), edges.data() + edges.size(), weights.data(), num_nodes);
        std::vector<vertex_descriptor> p(num_vertices(g));
        std::vector<int> d(num_vertices(g));
        vertex_descriptor s = vertex(sorce, g);
        dijkstra_shortest_paths(g, s,
            predecessor_map(boost::make_iterator_property_map(
                p.begin(),
                boost::get(boost::vertex_index, g)
            )).distance_map(boost::make_iterator_property_map(
                d.begin(),
                boost::get(boost::vertex_index, g)
            ))
        );

        // Print solution to stdout.
        std::cout << "node | distance from source | parent" << std::endl;
        boost::graph_traits<Graph>::vertex_iterator vi, vend;
        for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi)
            std::cout << *vi << " " << d[*vi] << " " << p[*vi] << std::endl;
        std::cout <<std::endl;

        // Generate a .dot graph file with shortest path highlighted.
        // To PNG with: dot -Tpng -o outfile.png input.dot
        boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, g);
        std::ofstream dot_file("dijkstra.dot");
        dot_file << "digraph D {\n"      << "  rankdir=LR\n"           << "  size=\"4,3\"\n"
                 << "  ratio=\"fill\"\n" << "  edge[style=\"bold\"]\n" << "  node[shape=\"circle\"]\n";
        boost::graph_traits <Graph>::edge_iterator ei, ei_end;
        for (std::tie(ei, ei_end) = boost::edges(g); ei != ei_end; ++ei) {
            edge_descriptor e = *ei;
            boost::graph_traits<Graph>::vertex_descriptor
                u = boost::source(e, g), v = boost::target(e, g);
            dot_file << u << " -> " << v << "[label=\"" << boost::get(weightmap, e) << "\"";
            if (p[v] == u)
                dot_file << ", color=\"black\"";
            else
                dot_file << ", color=\"grey\"";
            dot_file << "]";
        }
        dot_file << "}";

        // Construct forward path to a destination.
        int dest = 4;
        int cur = dest;
        std::vector<int> path;
        path.push_back(cur);
        while(cur != sorce) {
            cur = p[cur];
            path.push_back(cur);
        }
        std::reverse(path.begin(), path.end());
        // Print.
        std::cout << "Path to node " << std::to_string(dest) << ":" << std::endl;
        for(auto& node : path) {
            std::cout << node << std::endl;
        }
    }
}


// Copyright 2020 Advanced Remanufacturing and Technology Centre
// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team
//
// 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.

#ifndef PLANNING_FUNCTIONS_HPP_
#define PLANNING_FUNCTIONS_HPP_

#include <boost/filesystem.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <algorithm>
#include "cmath"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/image_encodings.hpp"
#include "yaml-cpp/yaml.h"

float PI = 3.14159265;
/**
 * Calculate length of two points in 2D space
 */
float length(int x1, int y1, int x2, int y2)
{
  return sqrt(pow(static_cast<float>(x1 - x2), 2) + pow(static_cast<float>(y1 - y2), 2));
}

/**
 * Checks if a point is in a circle.
 */
bool in_circle(float radius, std::vector<int> centre, std::vector<int> point)
{
  if (pow((point[0] - centre[0]), 2) + pow((point[1] - centre[1]), 2) - pow(radius, 2) < 0) {
    return true;
  } else {
    return false;
  }
}

/**
 * Quadratic equation solver
 */
std::vector<float> quadratic_equation(float a, float b, float c)
{
  std::vector<float> result;
  float discriminant = pow(b, 2) - (4 * a * c);
  if (discriminant < 0) {
    return result;
  } else if (discriminant == 0) {
    result.push_back(-b / (2 * a));
  } else {
    result.push_back(((-b + sqrt(discriminant)) / (2 * a)));
    result.push_back(((-b - sqrt(discriminant)) / (2 * a)));
  }
  return result;
}

/**
 * Get the intersecting coordinates between a circle and a line
 */
std::vector<std::vector<int>> circle_line_intersect(
  float gradient,
  float intersect,
  float radius,
  std::vector<float> circle_centre)
{
  std::vector<std::vector<int>> result;

  if (circle_centre.size() == 2 && radius > 0) {
    // Get the quadratic equation to determine the x coordinates of point
    float a = 1 + pow(gradient, 2);
    float b = 2 * ((gradient * (intersect - circle_centre[1])) - circle_centre[0]);
    float c = pow(circle_centre[0], 2) +
      pow((intersect - circle_centre[1]), 2) - pow(radius, 2);

    std::vector<float> x_coords = quadratic_equation(a, b, c);

    // Solve the equation to get the x coordinates
    if (static_cast<int>(x_coords.size()) == 2) {
      for (int i = 0; i < static_cast<int>(x_coords.size()); i++) {
        std::vector<int> temp_coords {static_cast<int>(round(x_coords[i])),
          static_cast<int>(round(gradient * x_coords[i] + intersect))};
        result.push_back(temp_coords);
      }
    } else {
      return result;
    }
  }

  return result;
}

/**
 * Get a bounding box based on coodinate centre and radius of box area.
 */

std::vector<std::vector<int>> get_border_corners(std::vector<int> centre, float radius)
{
  std::vector<std::vector<int>> result;
  if (centre.size() == 2 && radius > 0 && centre[0] >= 0 && centre[1] >= 0) {
    std::vector<int> top_left;
    std::vector<int> bottom_right;
    if (centre[0] - radius < 0) {
      top_left.push_back(0);
    } else {
      top_left.push_back(static_cast<int>(round(centre[0] - radius)));
    }
    if (centre[1] - radius < 0) {
      top_left.push_back(0);
    } else {
      top_left.push_back(static_cast<int>(round(centre[1] - radius)));
    }
    bottom_right = {static_cast<int>(round(centre[0] + radius)),
      static_cast<int>(round(centre[1] + radius))};
    result = {top_left, bottom_right};
  } else {
    return result;
  }
  return result;
}

/**
 * Make sure input is within 0 to Pi
 */

float keep_angle_in_bounds(float input)
{
  if (input < 0 || input >= PI) {
    if (input < 0) {
      while (input < 0) {
        input += PI;
      }
    }
    if (input >= PI) {
      while (input >= PI) {
        input -= PI;
      }
    }
  }
  return input;
}

#endif  // PLANNING_FUNCTIONS_HPP_


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

  @copyright 2016 NumScale SAS

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

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

 /*!
    @ingroup group-operator
    Function object implementing multiplies capabilities

    Perform the product of two parameters of the same type.

    Infix notation can be used with operator '*',

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    auto x = multiplies(a,b);
    @endcode

    or

    @code
    auto r = a*b;
    @endcode

    returns the product of @c a and @c b

    @see fma, fms, fnma, fnms

  **/
  Value multiplies(Value const & x, Value const& y);
} }
#endif

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

#endif


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

#ifndef MTL_FROBENIUS_NORM_INCLUDE
#define MTL_FROBENIUS_NORM_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/operation/max_of_sums.hpp>
#include <boost/numeric/mtl/operation/squared_abs.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

/// Frobenius norm, i.e. square root of sum of squares of all entries: \f$\sqrt{\sum_i \sum_j|a_{ij}|^2}\f$.
template <typename Matrix>
typename RealMagnitude<typename Collection<Matrix>::value_type>::type
inline frobenius_norm(const Matrix& matrix)
{
    vampir_trace<3010> tracer;
    using std::sqrt; using std::abs; using math::zero;
    namespace traits = mtl::traits;
    typename traits::const_value<Matrix>::type     value(matrix);

    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename RealMagnitude<value_type>::type  real_type;
    real_type ref, sum= zero(ref);

    typedef typename traits::range_generator<tag::major, Matrix>::type     cursor_type;
    typedef typename traits::range_generator<tag::nz, cursor_type>::type   icursor_type;

    for (cursor_type cursor = begin<tag::major>(matrix), cend = end<tag::major>(matrix); cursor != cend; ++cursor)
	for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
	    sum+= squared_abs(value(*icursor));
    return sqrt(sum);
}

}} // namespace mtl::matrix

#endif // MTL_FROBENIUS_NORM_INCLUDE


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

  Implementation of the original Hough transform for 2D line tracking.

  Reference papers:

  P.V.C. Hough, "Machine Analysis of Bubble Chamber Pictures",
  Proc. Int. Conf. High Energy Accelerators and Instrumentation, 1959.
  (U.S. Patent 3,069,654)

  Duda, R. O. and P. E. Hart, "Use of the Hough Transformation to Detect
  Lines and Curves in Pictures," Comm. ACM, Vol. 15, pp. 11–15, 1972.

  Copyright (c) 2013
  Dzmitry Hlindzich <hlindzich@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

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

#ifndef HOUGH_TRANSFORM_HPP_F8fEE77D_37A4_4DFB_83ED_CF832D40772D_
#define HOUGH_TRANSFORM_HPP_F8fEE77D_37A4_4DFB_83ED_CF832D40772D_

#include <cmath>
#include <utility>
#include <algorithm>
#include <vector>
#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include "bo/core/raw_image_2d.hpp"

namespace bo {
namespace recognition {

template <typename RealType>
class HoughTransform
{
public:

    typedef bo::RawImage2D<float> Image;
    // Pair (rho, theta) used in the transform.
    typedef std::pair<RealType, RealType> RhoTheta;
    typedef std::vector<RhoTheta> RhoThetas;
    // Pixel in the transformed image. First element corresponds to
    // theta, the second one to rho.
    typedef std::pair<std::size_t, std::size_t> HoughPixel;
    typedef std::vector<HoughPixel> HoughPixels;

    HoughTransform(const Image &image): input_image_(image),
        max_vote_(0)
    {
        std::size_t w = image.width();
        std::size_t h = image.height();

        BOOST_ASSERT(w > 0 && h > 0);

        max_rho_ = std::sqrt(w * w + h * h);
        min_rho_ = -w;

        max_theta_ = boost::math::constants::pi<RealType>();
        min_theta_ = RealType(0);
    }

    // Returns the Hough transform (in the rho-theta parametrization) of the points
    // from the input image whose values exceed the given threshold. Represents the
    // result as a 2D image of size (output_width * output_height).
    // The resulted image covers the following rectangle in the rho-theta space:
    // width = values of theta [0, Pi], height = values of rho [-w, sqrt(w^2 + h^2)],
    // where w and h are the width and the height of the input image correspondingly.
    Image compute(RealType threshold, std::size_t output_width, std::size_t output_height)
    {
        BOOST_ASSERT(output_height > 0 && output_width > 0);

        max_vote_ = 0;

        // Compute the output image resolution.
        theta_scaling_ = static_cast<RealType>(output_width) / (max_theta_ - min_theta_);
        rho_scaling_ = static_cast<RealType>(output_height - 1) / (max_rho_ - min_rho_);

        // Allocate the output image.
        bo::RawImage2D<RealType> hough(output_width, output_height, 0);

        std::size_t w = input_image_.width();
        std::size_t h = input_image_.height();

        // Compute sinusoids in the rho-theta space for each point of the input image
        // whose value is greater that the threshold.
        for (std::size_t i = 0; i < w; ++i)
            for (std::size_t j = 0; j < h; ++j)
            {
                if (input_image_(i,j) > threshold)
                {
                    // Accumulate the sinusoid.
                    for (std::size_t x = 0; x < output_width; ++x)
                    {
                        RealType theta = min_theta_ + x / theta_scaling_;
                        RealType rho = i * std::cos(theta) + j * std::sin(theta);

                        std::size_t y = round(rho_scaling_ * (rho - min_rho_));

                        RealType vote = ++hough(x, y);

                        if(max_vote_ < vote)
                        {
                            max_vote_ = vote;
                        }
                    }
                }
            }

        return hough;
    }

    // Returns the image with the lines reconstructed from the given Hough parameters.
    Image reconstruct_lines(const RhoThetas &parameters)
    {
        const RealType kEpsilon(0.001);

        // Allocate the output image.
        std::size_t w = input_image_.width();
        std::size_t h = input_image_.height();
        Image line_image(w, h, 0);

        for (typename RhoThetas::const_iterator it = parameters.begin();
             it != parameters.end(); ++it)
        {
            RealType rho = it->first;
            RealType theta = it->second;

            RealType sint = std::sin(theta);
            RealType cost = std::cos(theta);

            RealType t1, t2;

            // Find the min (t1) and max (t2) parametric values of the line
            // segment within the image.
            if (std::abs(sint) < kEpsilon)
            {
                t1 = 0;
                t2 = h;
            }
            else if (std::abs(cost) < kEpsilon)
            {
                t1 = -w;
                t2 = 0;
            }
            else
            {
                RealType tc = rho * cost / sint;
                t1 = t2 = tc;

                tc = -rho * sint / cost;
                t1 = std::min(t1, tc);
                t2 = std::max(t2, tc);

                tc = (rho * cost - w) / sint;
                t1 = std::min(t1, tc);
                t2 = std::max(t2, tc);

                tc = (h - rho * sint) / cost;
                t1 = std::min(t1, tc);
                t2 = std::max(t2, tc);
            }

            RealType delta_t = RealType(1);
            RealType t = t1;

            // Draw the segment of the line.
            while (t < t2)
            {
                std::size_t x = round(rho * cost - t * sint);
                std::size_t y = round(rho * sint + t * cost);

                if (x >= 0 && x < w && y >= 0 && y < h)
                {
                    line_image(x, y) = 1;
                }

                t += delta_t;
            }
        }

        return line_image;
    }

    // Returns the image with the lines reconstructed from the given subset of pixels
    // from the Hough image returned by compute() method.
    Image reconstruct_lines(const HoughPixels &pixels)
    {
        RhoThetas parameters;

        // Transform the pixels into rho-theta parameters.
        for (HoughPixels::const_iterator it = pixels.begin(); it != pixels.end(); ++it)
        {
            RealType rho = min_rho_ + it->second / rho_scaling_;
            RealType theta = min_theta_ + it->first / theta_scaling_;

            parameters.push_back(RhoTheta(rho, theta));
        }

        return reconstruct_lines(parameters);
    }

    // Returns the image with the lines detected in the input image. Only the points
    // whose values exceed the given threshold are considered. The number of the
    // detected lines is defined by quantity parameter. Only the lines with the
    // number of votes not less than quantity * (maximal vote in the accumulator)
    // are reconstructed.
    Image reconstruct_lines(RealType threshold, RealType quantity,
                            std::size_t accu_width = 1024,
                            std::size_t accu_height = 1024)
    {
        BOOST_ASSERT(quantity >= RealType(0) && quantity <= RealType(1));

        // Compute the accumulator.
        Image hough = compute(threshold, accu_width, accu_height);

        RealType accu_threshold = max_vote_ * quantity;

        RhoThetas detected;

        // Collect the best parameters from the accumulator.
        for (std::size_t i = 0; i < accu_width; ++i)
            for (std::size_t j = 0; j < accu_height; ++j)
            {
                if (hough(i, j) >= accu_threshold)
                {
                    RealType rho = min_rho_ + j / rho_scaling_;
                    RealType theta = min_theta_ + i / theta_scaling_;

                    detected.push_back(RhoTheta(rho, theta));
                }
            }

        return reconstruct_lines(detected);
    }

    RealType get_rho_scaling()
    {
        return rho_scaling_;
    }

    RealType get_theta_scaling()
    {
        return theta_scaling_;
    }

protected:

    inline std::size_t round(RealType x) const
    {
        return static_cast<std::size_t>(std::floor(x + 0.5));
    }

    Image input_image_;

    RealType max_rho_;
    RealType min_rho_;
    RealType max_theta_;
    RealType min_theta_;

    RealType theta_scaling_;
    RealType rho_scaling_;

    RealType max_vote_;
};

} // namespace recognition
} // namespace bo

#endif // HOUGH_TRANSFORM_HPP_F8fEE77D_37A4_4DFB_83ED_CF832D40772D_


#include <SmurffCpp/Types.h>
#include <Eigen/IterativeLinearSolvers>

#include <SmurffCpp/Utils/MatrixUtils.h>
#include <SmurffCpp/Utils/Error.h>
#include <SmurffCpp/Utils/counters.h>

#include <SmurffCpp/SideInfo/SparseSideInfo.h>
#include "linop.h"

namespace smurff {
namespace linop {
  class AtA;
} }

namespace Eigen {
namespace internal {
  // AtA looks-like a SparseMatrix, so let's inherits its traits:
  template<>
  struct traits<smurff::linop::AtA> :  public Eigen::internal::traits<smurff::SparseMatrix>
  {};
}
}

namespace smurff
{
namespace linop
{

// Example of a matrix-free wrapper from a user type to Eigen's compatible type
// For the sake of simplicity, this example simply wrap a Eigen::SparseMatrix.
class AtA : public Eigen::EigenBase<AtA>
{
public:
  // Required typedefs, constants, and method:
  typedef float_type Scalar;
  typedef float_type RealScalar;
  typedef int StorageIndex;
  enum
  {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = true
  };
  Index outerSize() const { return m_A.cols(); }
  Index innerSize() const { return m_A.cols(); }
  Index rows()      const { return m_A.cols(); }
  Index cols()      const { return m_A.cols(); }
  template <typename Rhs>
  Eigen::Product<AtA, Rhs, Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs> &x) const
  {
    return Eigen::Product<AtA, Rhs, Eigen::AliasFreeProduct>(*this, x.derived());
  }
  // Custom API:
  AtA(const SparseMatrix &A, const SparseMatrix &At, double reg) : m_A(A), m_At(At), m_reg(reg) {}

  const SparseMatrix &m_A;
  const SparseMatrix &m_At;
  double m_reg;
};

} // namespace linop
} // namespace smurff

// Implementation of AtA * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
namespace internal {
  template<typename Rhs>
  struct generic_product_impl<smurff::linop::AtA, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
  : generic_product_impl_base<smurff::linop::AtA,Rhs,generic_product_impl<smurff::linop::AtA,Rhs> >
  {
    typedef typename Product<smurff::linop::AtA,Rhs>::Scalar Scalar;
    template<typename Dest>
    static void scaleAndAddTo(Dest& dst, const smurff::linop::AtA& lhs, const Rhs& rhs, const Scalar& alpha)
    {
      // This method should implement "dst += alpha * lhs * rhs" inplace,
      dst += alpha * ((lhs.m_At * (lhs.m_A * rhs)) + lhs.m_reg * rhs);
    }
  };
}
}

namespace smurff
{
namespace linop
{

inline void makeSymmetric(Matrix &A)
{
  A = A.selfadjointView<Eigen::Lower>();
}

inline void AtA_mul_B(Matrix& out, const SparseSideInfo& A, double reg, const Matrix& B) {
  out.noalias() = (A.Ft * (A.F * B)) + reg * B;
}

//
//-- Solves the system (K' * K + reg * I) * X = B for X for m right-hand sides
//   K = d x n matrix
//   I = n x n identity
//   X = n x m matrix
//   B = n x m matrix
//
int solve_blockcg_1block(Matrix & X, const SparseSideInfo& K, double reg, Matrix & B, double tol, bool throw_on_cholesky_error) {
  // initialize
  const int nfeat = B.rows();
  const int nrhs  = B.cols();
  double tolsq = tol*tol;

  if (nfeat != K.cols()) {THROWERROR("B.rows() must equal K.cols()");}

  Vector norms(nrhs), inorms(nrhs);
  norms.setZero();
  inorms.setZero();
  #pragma omp parallel for schedule(static)
  for (int rhs = 0; rhs < nrhs; rhs++)
  {
    double sumsq = 0.0;
    for (int feat = 0; feat < nfeat; feat++)
    {
      sumsq += B(feat, rhs) * B(feat, rhs);
    }
    norms(rhs)  = std::sqrt(sumsq);
    inorms(rhs) = 1.0 / norms(rhs);
  }
  Matrix R(nfeat, nrhs);
  Matrix P(nfeat, nrhs);
  Matrix Ptmp(nfeat, nrhs);
  X.setZero();
  // normalize R and P:
  #pragma omp parallel for schedule(static) collapse(2)
  for (int feat = 0; feat < nfeat; feat++)
  {
    for (int rhs = 0; rhs < nrhs; rhs++)
    {
      R(feat, rhs) = B(feat, rhs) * inorms(rhs);
      P(feat, rhs) = R(feat, rhs);
    }
  }
  Matrix* RtR = new Matrix(nrhs, nrhs);
  Matrix* RtR2 = new Matrix(nrhs, nrhs);

  Matrix   KP(nfeat, nrhs);
  Matrix KPtP(nrhs, nrhs);
  Matrix A;
  Matrix Psi;

  //A_mul_At_combo(*RtR, R);
  *RtR = R.transpose() * R;
  makeSymmetric(*RtR);

  const int nblocks = (int)ceil(nfeat / 64.0);

  // CG iteration:
  int iter = 0;
  for (iter = 0; iter < 1000; iter++) {
    // KP = K * P
    ////double t1 = tick();
    AtA_mul_B(KP, K, reg, P);
    ////double t2 = tick();

    KPtP = KP.transpose() * P;
    auto chol_KPtP = KPtP.llt();
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_KPtP.info() != Eigen::NumericalIssue, "Cholesky Decomposition failed! (Numerical Issue)");
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_KPtP.info() != Eigen::InvalidInput, "Cholesky Decomposition failed! (Invalid Input)");
    A = chol_KPtP.solve(*RtR);
    ////double t3 = tick();

    #pragma omp parallel for schedule(guided)
    for (int block = 0; block < nblocks; block++)
    {
      int row = block * 64;
      int brows = std::min(64, nfeat - row);
      // X += A' * P
      X.block(row, 0, brows, nrhs).noalias() += P.block(row, 0, brows, nrhs) * A;
      // R -= A' * KP
      R.block(row, 0, brows, nrhs).noalias() -= KP.block(row, 0, brows, nrhs) * A;
    }
    ////double t4 = tick();

    // convergence check:
    //A_mul_At_combo(*RtR2, R);
    *RtR2 = R.transpose() * R;
    makeSymmetric(*RtR2);

    Vector d = RtR2->diagonal();
    // std::cout << "[ iter " << iter << "] " << std::scientific << d.transpose() << " (max: " << d.maxCoeff() << " > " << tolsq << ")" << std::endl;
    //std::cout << iter << ":" << std::scientific << d.transpose() << std::endl;
    if ( (d.array() < tolsq).all()) {
      break;
    }

    // Psi = (R R') \ R2 R2'
    auto chol_RtR = RtR->llt();
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_RtR.info() != Eigen::NumericalIssue, "Cholesky Decomposition failed! (Numerical Issue)");
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_RtR.info() != Eigen::InvalidInput, "Cholesky Decomposition failed! (Invalid Input)");
    Psi  = chol_RtR.solve(*RtR2);
    ////double t5 = tick();

    // P = R + Psi' * P (P and R are already transposed)
    #pragma omp parallel for schedule(guided)
    for (int block = 0; block < nblocks; block++)
    {
      int row = block * 64;
      int brows = std::min(64, nfeat - row);
      Matrix xtmp(brows, nrhs);
      xtmp = P.block(row, 0, brows, nrhs) * Psi;
      P.block(row, 0, brows, nrhs) = R.block(row, 0, brows, nrhs) + xtmp;
    }

    // R R' = R2 R2'
    std::swap(RtR, RtR2);
    ////double t6 = tick();
    ////double t_total = 0.01 * (t6-t1);
    ////printf("t2-t1 = %.3f, t3-t2 = %.3f, t4-t3 = %.3f, t5-t4 = %.3f, t6-t5 = %.3f\n", t2-t1, t3-t2, t4-t3, t5-t4, t6-t5);
    ////printf("t2-t1 = %.3f, t3-t2 = %.3f, t4-t3 = %.3f, t5-t4 = %.3f, t6-t5 = %.3f\n",
    ////  (t2-t1)/(t_total), (t3-t2)/(t_total), (t4-t3)/(t_total), (t5-t4)/(t_total), (t6-t5)/(t_total));
  }

  if (iter == 1000)
  {
    Vector d = RtR2->diagonal().cwiseSqrt();
    std::cerr << "warning: block_cg: could not find a solution in 1000 iterations; residual: ["
              << d.transpose() << " ].all() > " << tol << std::endl;
  }

  // unnormalizing X:
  #pragma omp parallel for schedule(static) collapse(2)
  for (int feat = 0; feat < nfeat; feat++)
  {
    for (int rhs = 0; rhs < nrhs; rhs++)
    {
      X(feat, rhs) *= norms(rhs);
    }
  }
  delete RtR;
  delete RtR2;
  return iter;
}

/** good values for solve_blockcg are blocksize=32 an excess=8 */
int solve_blockcg(Matrix & X, const SparseSideInfo& K, double reg, Matrix & B, double tol, const int blocksize, const int excess, bool throw_on_cholesky_error) {
  if (B.cols() <= excess + blocksize) {
    return solve_blockcg_1block(X, K, reg, B, tol, throw_on_cholesky_error);
  }
  // split B into blocks of size <blocksize> (+ excess if needed)
  Matrix Xblock, Bblock;
  int max_iter = 0;
  for (int i = 0; i < B.cols(); i += blocksize) {
    int ncols = blocksize;
    if (i + ncols + excess >= B.cols()) {
      ncols = B.cols() - i;
    }
    Bblock.resize(B.rows(), ncols);
    Xblock.resize(X.rows(), ncols);

    Bblock = B.block(0, i, B.rows(), ncols);
    int niter = solve_blockcg_1block(Xblock, K, reg, Bblock, tol, throw_on_cholesky_error);
    max_iter = std::max(niter, max_iter);
    X.block(0, i, X.rows(), ncols) = Xblock;
  }

  return max_iter;
}

int solve_blockcg_eigen(Matrix & X, const SparseSideInfo& K, double reg, Matrix & B, double tol, bool throw_on_cholesky_error)
{
   COUNTER("eigen_cg");
   linop::AtA A(K.F, K.Ft, reg);
   Eigen::ConjugateGradient<linop::AtA, Eigen::Lower | Eigen::Upper, Eigen::IdentityPreconditioner> cg;
   cg.setTolerance(tol);
   cg.compute(A);
   X = cg.solve(B);
   return cg.iterations();
}

}}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

#ifndef quantlib_tcopula_policy_hpp
#define quantlib_tcopula_policy_hpp

#include <ql/errors.hpp>
#include <ql/utilities/disposable.hpp>
#include <ql/experimental/math/convolvedstudentt.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/bind.hpp>
#include <vector>

namespace QuantLib {

    /*! \brief Student-T Latent Model's copula policy.

    Describes the copula of a set of normalized Student-T independent random
    factors to be fed into the latent variable model.
    The latent model requires the independent variables to be of unit variance
    so the policy expects the factors coefficients to be as usual and the T
    variables to be normalized, the normalization is performed by the policy.
    To normalize the random variables they are divided by the square root of
    the variance of each T (\f$ \frac{\nu}{\nu-2}\f$)
    */
    class TCopulaPolicy {
    public:
        /*! Stores the parameters defining the factors random variable
        T-distributions. As it is now the latent models are restricted to
        having the same distribution for all idiosyncratic factors, so only
        one parameter is needed for them.
        */
        typedef
            struct {
                std::vector<Integer> tOrders;
            } initTraits;

        /*! Delayed initialization of the distribution parameters and caches.
        To be called by the latent model. */
        /* \todo
        Explore other constructors, with different vector dimensions, defining
        simpler combinations (only one correlation, only one variable) might
        simplify memory.
        */
        explicit TCopulaPolicy(
            const std::vector<std::vector<Real> >& factorWeights =
                std::vector<std::vector<Real> >(),
            const initTraits& vals = initTraits());

        //! Number of independent random factors.
        Size numFactors() const {
            return latentVarsInverters_.size() + varianceFactors_.size() - 1;
        }

        //! returns a copy of the initialization arguments
        //... better to have a cache?
        initTraits getInitTraits() const {
            initTraits data;
            data.tOrders.resize(distributions_.size());
            for (Size i=0; i<distributions_.size(); ++i) {
                data.tOrders[i] = static_cast<Integer>(
                    distributions_[i].degrees_of_freedom());
            }
            return data;
        }
        const std::vector<Real>& varianceFactors() const {
            return varianceFactors_;
        }
        /*! Cumulative probability of a given latent variable.
            The iVariable parameter is the index of the requested variable.
        */
        Probability cumulativeY(Real val, Size iVariable) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(iVariable < latentVarsCumul_.size(),
                "Latent variable index out of bounds.");
    #endif
            return latentVarsCumul_[iVariable](val);
        }
        //! Cumulative probability of the idiosyncratic factors (all the same)
        Probability cumulativeZ(Real z) const {
            return boost::math::cdf(distributions_.back(), z /
                varianceFactors_.back());
        }
        /*! Probability density of a given realization of values of the systemic
          factors (remember they are independent).
          Intended to be used in numerical integration of an arbitrary function
          depending on those values.
        */
        Probability density(const std::vector<Real>& m) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(m.size() == distributions_.size()-1,
                "Incompatible sample and latent model sizes");
    #endif
            Real prodDensities = 1.;
            for(Size i=0; i<m.size(); i++)
                prodDensities *= boost::math::pdf(distributions_[i],
                    m[i] /varianceFactors_[i]) /varianceFactors_[i];
                 // accumulate lambda
            return prodDensities;
        }
        /*! Returns the inverse of the cumulative distribution of the (modelled)
          latent variable (as indexed by iVariable). Involves the convolution
          of the factors' distributions.
        */
        Real inverseCumulativeY(Probability p, Size iVariable) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(iVariable < latentVarsCumul_.size(),
                "Latent variable index out of bounds.");
    #endif
            return latentVarsInverters_[iVariable](p);
        }
        /*! Returns the inverse of the cumulative distribution of the
        idiosincratic factor. The LM here is limited to all idiosincratic
        factors following the same distribution.
        */
        Real inverseCumulativeZ(Probability p) const {
            return boost::math::quantile(distributions_.back(), p)
                * varianceFactors_.back();
        }
        /*! Returns the inverse of the cumulative distribution of the
          systemic factor iFactor.
        */
        Real inverseCumulativeDensity(Probability p, Size iFactor) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(iFactor < distributions_.size()-1,
                "Random factor variable index out of bounds.");
    #endif
            return boost::math::quantile(distributions_[iFactor], p)
                * varianceFactors_[iFactor];
        }
        //to use this (by default) version, the generator must be a uniform one.
        Disposable<std::vector<Real> >
            allFactorCumulInverter(const std::vector<Real>& probs) const;
    private:
        mutable std::vector<boost::math::students_t_distribution<> >
            distributions_;
        mutable std::vector<Real> varianceFactors_;
        mutable std::vector<CumulativeBehrensFisher> latentVarsCumul_;
        mutable std::vector<InverseCumulativeBehrensFisher>
            latentVarsInverters_;
    };

}

#endif


// Std includes
#include <vector>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "fl0w/kinematic.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;
using TypeFlow = fl0w::kinematic::Kinematic<TypeVector, TypeMatrix, TypeRef, std::vector>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") -> " << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") -> " << flow.getVorticity(x, t).transpose() << std::endl;
    std::cout << "flow.getAcceleration(" << x.transpose() << ", " << t << ") -> " << flow.getAcceleration(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

int main () {
    TypeFlow flow;
    flow.init();
    std::cout << "k " << (*flow.sK)[0] << std::endl;
    std::cout << "k " << (*flow.sK)[1] << std::endl;
    std::cout << "k " << (*flow.sK)[2] << std::endl;
    std::cout << "k " << (*flow.sK)[3] << std::endl;
    std::cout << "n0 " << (*flow.sK)[0].norm() << std::endl;
    std::cout << "nl " << (*flow.sK)[(*flow.sK).size()-1].norm() << std::endl;
    std::cout << "a " << flow.a[0] << std::endl;
    std::cout << "b " << flow.b[0] << std::endl;
    std::cout << "o " << flow.omega[0] << std::endl;
    TypeVector x;
    double t;
    // Init
    x << 0.0, 0.0, 0.0;
    t = 0.0;
    print(flow, x, t);
    //x << 10.0, -10.0, 40.0;
    //t = 10.0;
    //print(flow, x, t);
    //x << 1.0, -3.0, -1000.0;
    //t = 40.0;
    //print(flow, x, t);
}