#ifndef IMAGE_REPROJECTION_PLUGINS_SPHERE_SURFACE_MODEL_HPP
#define IMAGE_REPROJECTION_PLUGINS_SPHERE_SURFACE_MODEL_HPP

#include <cmath>
#include <string>

#include <image_reprojection/surface_model.hpp>
#include <image_reprojection_plugins/SphereStamped.h>
#include <topic_tools/shape_shifter.h>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <opencv2/core/core.hpp>

namespace image_reprojection_plugins {

class SphereSurfaceModel : public image_reprojection::SurfaceModel {
public:
  SphereSurfaceModel() {}

  virtual ~SphereSurfaceModel() {}

  virtual void update(const topic_tools::ShapeShifter &surface) {
    const SphereStampedConstPtr sphere(surface.instantiate< SphereStamped >());
    CV_Assert(sphere);
    update(*sphere);
  }

  void update(const SphereStamped &sphere) {
    CV_Assert(sphere.radius > 0.);

    boost::unique_lock< boost::shared_mutex > write_lock(mutex_);

    frame_id_ = sphere.header.frame_id;
    center_ = cv::Vec3f(sphere.center.x, sphere.center.y, sphere.center.z);
    radius_ = sphere.radius;
  }

  virtual std::string getFrameId() const {
    boost::shared_lock< boost::shared_mutex > read_lock(mutex_);
    return frame_id_;
  }

private:
  virtual void onInit() {}

  virtual void onIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                              cv::Mat &dst, cv::Mat &mask) const {
    boost::shared_lock< boost::shared_mutex > read_lock(mutex_);
    multiraySphereIntersection(src_origin, src_direction, dst, mask);
  }

  void multiraySphereIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                                  cv::Mat &dst, cv::Mat &mask) const {
    dst.create(src_direction.size(), CV_32FC3);
    for (int x = 0; x < src_direction.size().width; ++x) {
      for (int y = 0; y < src_direction.size().height; ++y) {
        unsigned char &m(mask.at< unsigned char >(y, x));
        const cv::Vec3f &sd(src_direction.at< cv::Vec3f >(y, x));
        cv::Vec3f &d(dst.at< cv::Vec3f >(y, x));
        m = (m != 0 && raySphereIntersection(src_origin, sd, d)) ? 1 : 0;
      }
    }
  }

  bool raySphereIntersection(const cv::Vec3f &src_origin, const cv::Vec3f &src_direction,
                             cv::Vec3f &dst) const {
    // intersection point (x) can be described as
    //   x = p + t * d  (t >= 0)
    //   |x - c| = r
    // where p: ray origin, d: ray direction, c: center of sphere, r: radius of sphere
    //   |d|^2 * t^2 + 2 * dot(d, p - c) * t + |p - c|^2 - r^2 = 0

    // position of ray origin with respect to center of sphere
    const cv::Vec3f o(src_origin - center_);

    // coefficients
    const double a(src_direction.dot(src_direction));
    const double b(src_direction.dot(o));
    const double c(o.dot(o) - radius_ * radius_);
    const double D(b * b - a * c); // discriminant
    if (/* no intersection */ D < 0.) {
      return false;
    }

    // intersection point
    const double sD(std::sqrt(D));
    const double t0((-b + sD) / a), t1((-b - sD) / a);
    if (t0 >= 0. && t1 >= 0.) {
      dst = src_origin + std::min(t0, t1) * src_direction;
      return true;
    } else if (t0 >= 0. && t1 < 0.) {
      dst = src_origin + t0 * src_direction;
      return true;
    } else if (t0 < 0. && t1 >= 0.) {
      dst = src_origin + t1 * src_direction;
      return true;
    }

    return false;
  }

private:
  mutable boost::shared_mutex mutex_;
  std::string frame_id_;
  cv::Vec3f center_;
  double radius_;
};

} // namespace image_reprojection_plugins

#endif


// Copyright (c) 2014 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "core.h"
#include "main.h"
#include "uint256.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

/**
 * the maximum block reward at a given height for a block without fees
 */
uint64_t expectedMaxSubsidy(int height) {
    if(height < 100000) {
        return 1000000 * COIN;
    } else if (height < 150000) {
        return 500000 * COIN;
    } else if (height < 200000) {
        return 250000 * COIN;
    } else if (height < 300000) {
        return 125000 * COIN;
    } else if (height < 400000) {
        return  62500 * COIN;
    } else if (height < 500000) {
        return  31250 * COIN;
    } else if (height < 600000) {
        return  15625 * COIN;
    } else {
        return  10000 * COIN;
    }
}

/**
 * the minimum possible value for the maximum block reward at a given height
 * for a block without fees
 */
uint64_t expectedMinSubsidy(int height) {
    if(height < 100000) {
        return 0;
    } else if (height < 150000) {
        return 0;
    } else if (height < 200000) {
        return 250000 * COIN;
    } else if (height < 300000) {
        return 125000 * COIN;
    } else if (height < 400000) {
        return  62500 * COIN;
    } else if (height < 500000) {
        return  31250 * COIN;
    } else if (height < 600000) {
        return  15625 * COIN;
    } else {
        return  10000 * COIN;
    }
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    int nHeight = 0;
    int nStepSize= 1;
    uint256 nSum = 0;

    for (nHeight = 0; nHeight <= 600000; nHeight += nStepSize) {
        uint64_t nSubsidy = GetBlockValue(nHeight, 0, 0);
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK(nSubsidy >= expectedMinSubsidy(nHeight));
        BOOST_CHECK(nSubsidy <= expectedMaxSubsidy(nHeight));

        nSum += nSubsidy * nStepSize;
    }

    //test sum +- ~10billion
    uint256 upperlimit = uint256("95e14ec776380000"); //108 billion pete
    BOOST_CHECK(nSum <= upperlimit);

    uint256 lowerlimit = uint256("7a1fe16027700000"); //88 billion pete
    BOOST_CHECK(nSum >= lowerlimit);

    //test infinitely increasing block rewards
    for (; nHeight < 700000; nHeight += nStepSize) {
        uint64_t nSubsidy = GetBlockValue(nHeight, 0, 0);
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK(nSubsidy >= expectedMinSubsidy(nHeight));
        BOOST_CHECK(nSubsidy <= expectedMaxSubsidy(nHeight));

        nSum += nSubsidy * nStepSize;
    }

    //make sure that MAX_MONEY is not what some people think it is
    BOOST_CHECK(nSum > MAX_MONEY);

    //test subsidy in 1000 years
    nHeight = 1000 * 365 * 24 * 60;
    uint64_t nSubsidy = GetBlockValue(nHeight, 0, 0);
    BOOST_CHECK(MoneyRange(nSubsidy));
    BOOST_CHECK(nSubsidy >= expectedMinSubsidy(nHeight));
    BOOST_CHECK(nSubsidy <= expectedMaxSubsidy(nHeight));
}

BOOST_AUTO_TEST_CASE(GetMinFee_test)
{
    uint64_t value = 1000 * COIN;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 100, false, GMF_RELAY) == CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 100, false, GMF_SEND) == CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_RELAY) == CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_SEND) == CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_RELAY) == CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_SEND) == CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_RELAY) == 2 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_SEND) == 2 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_RELAY) == 3 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_SEND) == 3 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_RELAY) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_SEND) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_dust_test)
{
    uint64_t value = 1000;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_RELAY) == 2 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_SEND) == 2 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_RELAY) == 2 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_SEND) == 2 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_RELAY) == 3 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_SEND) == 3 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_RELAY) == 4 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_SEND) == 4 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_RELAY) == (2+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_SEND) == (2+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_manydust_test)
{
    uint64_t value = 1000;

    CTransaction tx;
    CTxOut txout1(1000 * COIN, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    for(int i=0; i<100; i++) {
        CTxOut txoutn(value, (CScript)vector<unsigned char>(24, 0));
        tx.vout.push_back(txoutn);
    }

    CTxOut txout101(1000 * COIN, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout101);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_RELAY) == 101 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_SEND) == 101 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_RELAY) == 101 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_SEND) == 101 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_RELAY) == 102 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_SEND) == 102 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_RELAY) == 103 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_SEND) == 103 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_RELAY) == (101+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_SEND) == (101+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_relayfree_test)
{
    uint64_t value = 1000 * COIN;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 100, true, GMF_RELAY) == 0);
    BOOST_CHECK(GetMinFee(tx, 1000, true, GMF_RELAY) == 0);
    BOOST_CHECK(GetMinFee(tx, 25999, true, GMF_RELAY) == 0);

    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_RELAY) > 0);
    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_RELAY) == GetMinFee(tx, 26000, false, GMF_RELAY));

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, true, GMF_RELAY) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_createNoFree_test)
{
    uint64_t value = 1000 * COIN;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 100, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 100, true, GMF_SEND) == GetMinFee(tx, 100, false, GMF_SEND));
    BOOST_CHECK(GetMinFee(tx, 1000, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 1000, true, GMF_SEND) == GetMinFee(tx, 1000, false, GMF_SEND));
    BOOST_CHECK(GetMinFee(tx, 25999, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 25999, true, GMF_SEND) == GetMinFee(tx, 25999, false, GMF_SEND));

    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_SEND) == GetMinFee(tx, 26000, false, GMF_SEND));

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, true, GMF_SEND) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 optimize toolbox - levenberg"

#include <iostream>
#include <nt2/include/functions/levenberg.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/norm.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/table.hpp>

template < class Tabout >
struct fpp
{
  template < class Tabin> inline
  Tabout operator()(const Tabin & x ) const
  {
    typedef typename Tabin::value_type value_type;
    Tabout r = (nt2::sqr((x-nt2::_(value_type(1), value_type(numel(x))))));
    return r;
  }
};

template<class Tabout, class Tabin >  Tabout f1(const Tabin & x )
{
    typedef typename Tabin::value_type value_type;
    Tabout r = (nt2::sqr(x)-value_type(3));
    return r;
}

NT2_TEST_CASE_TPL( levenberg_function_ptr, NT2_REAL_TYPES )
{
  using nt2::levenberg;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  typedef nt2::table<T> ltab_t;
  tab_t x0 = nt2::ones(nt2::of_size(1, 3), nt2::meta::as_<T>());
  ltab_t h = nt2::is_nez(nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>())*nt2::Half<T>());
  tab_t r = nt2::sqrt(T(3))*nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>());
  output<tab_t,T> res = levenberg(&f1<tab_t, tab_t>, x0, h);
  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";
  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Sqrteps<T>());
  NT2_TEST_LESSER_EQUAL(nt2::norm(res.covar), T(12.1));
}

NT2_TEST_CASE_TPL( levenberg_functor, NT2_REAL_TYPES )
{
  using nt2::levenberg;
  using nt2::options;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  typedef nt2::table<T> ltab_t;
  tab_t x0 = nt2::zeros(nt2::of_size(1, 3), nt2::meta::as_<T>());
  ltab_t h = nt2::is_nez(nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>())*nt2::Half<T>());
  tab_t r = nt2::_(T(1), T(3));
  output<tab_t,T> res = levenberg(fpp<tab_t>(), x0, h,
                                  options [ nt2::iterations_ = 100,
                                            nt2::tolerance::absolute_ = nt2::Eps<T>()
                                    ]);
  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Four<T>()*nt2::Sqrteps<T>());
  NT2_TEST_LESSER_EQUAL(nt2::norm(res.covar), T(10*nt2::numel(res.covar))*nt2::Eps<T>());
}


#include "export/mvproduct.h"

#include <Eigen/Core>
#include "geometrycentral/utilities/vector3.h"
#include "poly_curve_network.h"
#include "spatial/tpe_bvh.h"
#include "product/block_cluster_tree.h"

using namespace geometrycentral;

namespace LWS
{
PolyCurveNetwork *createCurveNetwork(std::vector<std::array<double, 3>> &positions, std::vector<std::array<size_t, 2>> &edges)
{
    std::vector<Vector3> vectors(positions.size());
    for (size_t i = 0; i < positions.size(); i++)
    {
        vectors[i] = Vector3{positions[i][0], positions[i][1], positions[i][2]};
    }
    return new PolyCurveNetwork(vectors, edges);
}

BVHNode3D *createBVHForEnergy(PolyCurveNetwork *curves)
{
    return CreateBVHFromCurve(curves);
}

BlockClusterTree *createBlockClusterTree(PolyCurveNetwork *curves, double sep, double alpha, double beta)
{
    BVHNode3D *edgeBVH = CreateEdgeBVHFromCurve(curves);
    BlockClusterTree *tree = new BlockClusterTree(curves, edgeBVH, sep, alpha, beta, 0);
    // This block cluster tree is only going to multiply the dense upper-left block, with no duplication of entries.
    tree->SetBlockTreeMode(BlockTreeMode::MatrixOnly);
    return tree;
}

void multiplyMetricWithVector(BlockClusterTree *tree, std::vector<double> &vec, std::vector<double> &output)
{
    // Copy input to an Eigen matrix
    Eigen::VectorXd in(vec.size());
    for (size_t i = 0; i < vec.size(); i++)
    {
        in(i) = vec[i];
    }
    // Set up an Eigen matrix as output
    Eigen::VectorXd out(vec.size());
    out.setZero();

    // Call the block cluster tree routine
    tree->Multiply(in, out);

    // Copy result to the output
    for (size_t i = 0; i < vec.size(); i++)
    {
        output[i] = out(i);
    }
}

double evaluateEnergy(PolyCurveNetwork *curve, BVHNode3D *root, double alpha, double beta)
{
    return SpatialTree::TPEnergyBH(curve, root, alpha, beta);
}

void evaluateGradient(PolyCurveNetwork *curve, BVHNode3D *root, std::vector<std::array<double, 3>> &out, double alpha, double beta)
{
    // Set up an Eigen matrix for the computation to use
    Eigen::MatrixXd grad(out.size(), 3);
    grad.setZero();

    // Use the BVH routine
    SpatialTree::TPEGradientBarnesHut(curve, root, grad, alpha, beta);

    // Copy result to the output
    for (size_t i = 0; i < out.size(); i++)
    {
        out[i][0] = grad(i, 0);
        out[i][1] = grad(i, 1);
        out[i][2] = grad(i, 2);
    }
}

} // namespace LWS


// Filename: matrix_free_pcg.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

struct poisson2D_diagonal_pc
{
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	for (std::size_t i= 0; i < size(x); i++)
	    y[i]= x[i] * 0.25;
    }

    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	for (std::size_t i= 0; i < size(x); i++)
	    y[i]= x[i] * 0.25;
    }
};

template <typename Vector>
itl::pc::solver<poisson2D_diagonal_pc, Vector, false>
inline solve(const poisson2D_diagonal_pc& P, const Vector& x)
{
    return itl::pc::solver<poisson2D_diagonal_pc, Vector, false>(P, x);
}

template <typename Vector>
itl::pc::solver<poisson2D_diagonal_pc, Vector, true>
inline adjoint_solve(const poisson2D_diagonal_pc& P, const Vector& x)
{
    return itl::pc::solver<poisson2D_diagonal_pc, Vector, true>(P, x);
}

int main(int, char**)
{
    // For a more realistic example set size to 1000 or larger
    const int size = 10, N = size * size;

    typedef mtl::mat::poisson2D_dirichlet  matrix_type;
    matrix_type                               A(size, size);
    poisson2D_diagonal_pc                     P;

    mtl::dense_vector<double>                 x(N, 1.0), b(N);

    b = A * x;
    x= 0;
    itl::cyclic_iteration<double>             iter(b, 100, 1.e-11, 0.0, 5);
    cg(A, x, b, P, P, iter);

    return 0;
}


#include <engine/Eigenmodes.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <SymEigsSolver.h>  // Also includes <MatOp/DenseSymMatProd.h>

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

namespace Engine
{
    namespace Eigenmodes
    {
        using Utility::Log_Level;
        using Utility::Log_Sender;

        void Check_Eigenmode_Parameters(std::shared_ptr<Data::Spin_System> system)
        {
            int nos = system->nos;
            auto& n_modes = system->ema_parameters->n_modes;
            if (n_modes > 2*nos-2)
            {
                n_modes = 2*nos-2;
                system->modes.resize(n_modes);
                system->eigenvalues.resize(n_modes);

                Log(Log_Level::Warning, Log_Sender::EMA, fmt::format("Number of eigenmodes declared in "
                    "EMA Parameters is too large. The number is set to {}", n_modes));
            }
            if (n_modes != system->modes.size())
                system->modes.resize(n_modes);

            // Initial check of selected_mode
            auto& n_mode_follow = system->ema_parameters->n_mode_follow;
            if (n_mode_follow > n_modes-1)
            {
                Log(Log_Level::Warning, Log_Sender::EMA, fmt::format("Eigenmode number {} is not "
                    "available. The largest eigenmode ({}) is used instead", n_mode_follow, n_modes-1));
                n_mode_follow = n_modes-1;
            }
        }

        void Calculate_Eigenmodes(std::shared_ptr<Data::Spin_System> system, int idx_img, int idx_chain)
        {
            int nos = system->nos;

            Check_Eigenmode_Parameters(system);

            auto& n_modes = system->ema_parameters->n_modes;

            // vectorfield mode(nos, Vector3{1, 0, 0});
            vectorfield spins_initial = *system->spins;

            Log( Log_Level::Info, Log_Sender::EMA, fmt::format("Started calculation of {} Eigenmodes ", n_modes ),
                idx_img, idx_chain );

            // Calculate the Eigenmodes
            vectorfield gradient(nos);
            MatrixX hessian(3*nos, 3*nos);

            // The gradient (unprojected)
            system->hamiltonian->Gradient(spins_initial, gradient);
            Vectormath::set_c_a(1, gradient, gradient, system->geometry->mask_unpinned);

            // The Hessian (unprojected)
            system->hamiltonian->Hessian(spins_initial, hessian);

            // Get the eigenspectrum
            MatrixX hessian_constrained = MatrixX::Zero(2*nos, 2*nos);
            MatrixX tangent_basis = MatrixX::Zero(3*nos, 2*nos);
            VectorX eigenvalues;
            MatrixX eigenvectors;
            bool successful = Eigenmodes::Hessian_Partial_Spectrum(system->ema_parameters, spins_initial, gradient, hessian,
                n_modes, tangent_basis, hessian_constrained, eigenvalues, eigenvectors);

            if (successful)
            {
                // get every mode and save it to system->modes
                for (int i=0; i<n_modes; i++)
                {
                    // Extract the minimum mode (transform evec_lowest_2N back to 3N)
                    VectorX evec_3N = tangent_basis * eigenvectors.col(i);

                    // dynamically allocate the system->modes
                    system->modes[i] = std::shared_ptr<vectorfield>(new vectorfield(nos, Vector3{1,0,0}));

                    // Set the modes
                    for (int j=0; j<nos; j++)
                        (*system->modes[i])[j] = {evec_3N[3*j], evec_3N[3*j+1], evec_3N[3*j+2]};

                    // get the eigenvalues
                    system->eigenvalues[i] = eigenvalues(i);
                }

                Log( Log_Level::Info, Log_Sender::All, fmt::format("Finished calculation of {} Eigenmodes ", n_modes ),
                    idx_img, idx_chain );

                int ev_print = std::min(n_modes, 100);
                Log( Log_Level::Info, Log_Sender::EMA, fmt::format("Eigenvalues: {}",
                    eigenvalues.head( ev_print ).transpose() ), idx_img, idx_chain );
            }
            else
            {
                //// TODO: What to do then?
                Log( Log_Level::Warning, Log_Sender::All, "Something went wrong in eigenmode calculation...",
                    idx_img, idx_chain );
            }
        }

        bool Hessian_Full_Spectrum(const std::shared_ptr<Data::Parameters_Method> parameters,
            const vectorfield & spins, const vectorfield & gradient, const MatrixX & hessian,
            MatrixX & tangent_basis, MatrixX & hessian_constrained, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            int nos = spins.size();

            // Calculate the final Hessian to use for the minimum mode
            // TODO: add option to choose different Hessian calculation
            hessian_constrained = MatrixX::Zero(2*nos, 2*nos);
            tangent_basis       = MatrixX::Zero(3*nos, 2*nos);
            Manifoldmath::hessian_bordered(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_projected(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_weingarten(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_spherical(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_covariant(spins, gradient, hessian, tangent_basis, hessian_constrained);

            // Create and initialize a Eigen solver. Note: the hessian matrix should be symmetric!
            Eigen::SelfAdjointEigenSolver<MatrixX> hessian_spectrum(hessian_constrained);

            // Extract real eigenvalues
            eigenvalues = hessian_spectrum.eigenvalues().real();
            // Retrieve the real eigenvectors
            eigenvectors = hessian_spectrum.eigenvectors().real();

            // Return whether the calculation was successful
            return true;
        }

        bool Hessian_Partial_Spectrum(const std::shared_ptr<Data::Parameters_Method> parameters,
            const vectorfield & spins, const vectorfield & gradient, const MatrixX & hessian, int n_modes,
            MatrixX & tangent_basis, MatrixX & hessian_constrained, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            int nos = spins.size();

            // Restrict number of calculated modes to [1,2N)
            n_modes = std::max(1, std::min(2*nos-2, n_modes));

            // If we have only one spin, we can only calculate the full spectrum
            if (n_modes == nos)
                return Hessian_Full_Spectrum(parameters, spins, gradient, hessian, tangent_basis, hessian_constrained, eigenvalues, eigenvectors);

            // Calculate the final Hessian to use for the minimum mode
            // TODO: add option to choose different Hessian calculation
            hessian_constrained = MatrixX::Zero(2*nos, 2*nos);
            tangent_basis       = MatrixX::Zero(3*nos, 2*nos);
            Manifoldmath::hessian_bordered(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_projected(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_weingarten(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_spherical(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_covariant(spins, gradient, hessian, tangent_basis, hessian_constrained);

            // Remove degrees of freedom of pinned spins
            #ifdef SPIRIT_ENABLE_PINNING
                for (int i=0; i<nos; ++i)
                {
                    // TODO: pinning is now in Data::Geometry
                    // if (!parameters->pinning->mask_unpinned[i])
                    // {
                    //     // Remove interaction block
                    //     for (int j=0; j<nos; ++j)
                    //     {
                    //         hessian_constrained.block<2,2>(2*i,2*j).setZero();
                    //         hessian_constrained.block<2,2>(2*j,2*i).setZero();
                    //     }
                    //     // Set diagonal matrix entries of pinned spins to a large value
                    //     hessian_constrained.block<2,2>(2*i,2*i).setZero();
                    //     hessian_constrained.block<2,2>(2*i,2*i).diagonal().setConstant(nos*1e5);
                    // }
                }
            #endif // SPIRIT_ENABLE_PINNING

            // Create the Spectra Matrix product operation
            Spectra::DenseSymMatProd<scalar> op(hessian_constrained);
            // Create and initialize a Spectra solver
            Spectra::SymEigsSolver< scalar, Spectra::SMALLEST_ALGE, Spectra::DenseSymMatProd<scalar> > hessian_spectrum(&op, n_modes, 2*nos);
            hessian_spectrum.init();

            // Compute the specified spectrum, sorted by smallest real eigenvalue
            int nconv = hessian_spectrum.compute(1000, 1e-10, int(Spectra::SMALLEST_ALGE));

            // Extract real eigenvalues
            eigenvalues = hessian_spectrum.eigenvalues().real();

            // Retrieve the real eigenvectors
            eigenvectors = hessian_spectrum.eigenvectors().real();

            // Return whether the calculation was successful
            return (hessian_spectrum.info() == Spectra::SUCCESSFUL) && (nconv > 0);
        }
    }
}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/not.hpp>

#include "matrix/comparable.hpp"
namespace hana = boost::hana;
using namespace cppcon;

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
        matrix(row(1, 2)),
        matrix(row(1, 2))
    ));
    BOOST_HANA_CONSTEXPR_CHECK(hana::not_(hana::equal(
        matrix(row(1, 2)),
        matrix(row(1, 5))
    )));

    BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
        matrix(row(1, 2),
               row(3, 4)),
        matrix(row(1, 2),
               row(3, 4))
    ));
    BOOST_HANA_CONSTEXPR_CHECK(hana::not_(hana::equal(
        matrix(row(1, 2),
               row(3, 4)),
        matrix(row(1, 2),
               row(0, 4))
    )));
    BOOST_HANA_CONSTEXPR_CHECK(hana::not_(hana::equal(
        matrix(row(1, 2),
               row(3, 4)),
        matrix(row(0, 2),
               row(3, 4))
    )));

    BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
        matrix(row(1),
               row(2)),
        matrix(row(3, 4),
               row(5, 6))
    )));
    BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
        matrix(row(1),
               row(2)),
        matrix(row(3, 4))
    )));
}


// proj-marker-transform.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <vector>
#include <iostream>
#include <time.h>

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Cholesky>

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include <math/func.h>
#include <math/matrix.h>
#include <import.h>

#include <math/SparseBlockSquareMatrix.h>

Eigen::VectorXf pcg( const SparseBlockSquareMatrix &A, const Eigen::VectorXf &b, int maxIters, float threshold = 1e-4 ) {
	Eigen::VectorXf M = A.diagonal();
	for ( int i = 0; i < M.rows(); i++ ) {
		if ( fabs( M[i] ) > threshold ) {
			M[i] = 1.0f / M[i];
		} else {
			M[i] = 1.0f;
		}
	}

	Eigen::VectorXf x = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf p = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf r = b; // b - A * x

	int m_numIters = 0;
	clock_t begin = clock();
	for ( int i = 0; i < maxIters; i++ ) {
		m_numIters++;

		Eigen::VectorXf Mr = M.array() * r.array();
		float rMr = r.dot( Mr );
		if ( rMr < threshold * A.size() ) {
		//	break;
		}
		if ( threshold <= 0 && rMr < 1e-6 ) {
			rMr = 0.0f;
		} else {
			rMr = 1.0f / rMr;
		}
		p += Mr * rMr;

		Eigen::VectorXf Ap = A * p;
		float pAp = p.dot( Ap );
		if ( threshold <= 0 && pAp < 1e-6 ) {
			pAp = 0.0f;
		} else {
			pAp = 1.0f / pAp;
		}
		x +=  p * pAp;
		r -= Ap * pAp;

		float rme = sqrt( r.dot( r ) / A.size() );
		if ( rme < 1e-6 ) {
			break;
		}
	}
	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
	printf( "pcg cpu (ms) : %.3f : %.3f : %d\n", time_spent * 1000, time_spent * 1000 / m_numIters, m_numIters );

	return x;
}

float icp_point_to_point_svd(
	const std::vector< Eigen::Vector3f > &src_points,
	const std::vector< Eigen::Vector3f > &dst_points,
	Eigen::Matrix3f &out_r, // rotation
	Eigen::Vector3f &out_t  // translation
) {
	if ( src_points.empty() ) {
		out_r.setIdentity();
		out_t.setZero();
		return 0.0f;
	}

	// E(src)
	Eigen::Vector3f src_mean = mean( src_points );

	// E(dst)
	Eigen::Vector3f dst_mean = mean( dst_points );

	// A = Sum( ( src - E(src) ) * ( dst - E(dst) )' )
	Eigen::Matrix3f A = Eigen::Matrix3f::Identity() * 1e-6f;
	for ( int i = 0; i < src_points.size(); i++ ) {
		A += ( src_points[i] - src_mean ) * ( dst_points[i] - dst_mean ).transpose();
	}

	// SVD : A = U * D * V'
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

	// R = V * U'
	out_r = svd.matrixV() * svd.matrixU().transpose();

	// T = E(dst) - R * E(src)
	out_t = dst_mean - out_r * src_mean;

	// rme
	return rme( src_points, dst_points, out_r, out_t );
}

void init_affine_transforms( int size, Eigen::VectorXf &x, const Eigen::Matrix3f &rot, const std::vector< Eigen::Vector3f > &src, const std::vector< Eigen::Vector3f > &dst ) {
	for ( int i = 0; i < size; i++ ) {
		x.block( 12 * i + 3 * 0, 0, 3, 1 ) = rot.col(0); Eigen::Vector3f( 1, 0, 0 );
		x.block( 12 * i + 3 * 1, 0, 3, 1 ) = rot.col(1); Eigen::Vector3f( 0, 1, 0 );
		x.block( 12 * i + 3 * 2, 0, 3, 1 ) = rot.col(2); Eigen::Vector3f( 0, 0, 1 );
		x.block( 12 * i + 3 * 3, 0, 3, 1 ) = dst[i] - src[i];
	}
}

void EvalRigidTerm( int size, Eigen::MatrixXf &JtJ, Eigen::VectorXf &Jtr, Eigen::VectorXf &x, std::vector<Eigen::Vector3f> &src, std::vector<Eigen::Vector3f> &dst, float &E ) {
	// E-rigid
	float w_rigid = 1.0f;
	{
		for ( int i = 0; i < size; i++ ) {
			Eigen::Vector3f M[4];
			M[0] = x.block( i * 12 + 0 * 3, 0, 3, 1 );
			M[1] = x.block( i * 12 + 1 * 3, 0, 3, 1 );
			M[2] = x.block( i * 12 + 2 * 3, 0, 3, 1 );
			M[3] = x.block( i * 12 + 3 * 3, 0, 3, 1 );

			// ck dot ck - 1
			for ( int k = 0; k < 3; k++ ) {
				float r = M[k].dot( M[k] ) - 1;

				Eigen::VectorXf J = Eigen::VectorXf::Zero( 12 );
				J.block( k * 3, 0, 3, 1 ) = M[k] + M[k];

				E += r * r * w_rigid;
				JtJ.block( i * 12, i * 12, 12, 12 ) += J * J.transpose() * w_rigid; // block (i, i)
				Jtr.block( i * 12, 0, 12, 1 ) += J * r * w_rigid; // block ( i )
			}

			// ck dot cj
			for ( int k = 0; k < 3; k++ ) {
				int j = ( k + 1 ) % 3;

				float r = M[k].dot( M[j] );

				E += r * r * w_rigid;

				Eigen::VectorXf J = Eigen::VectorXf::Zero( 12 );

				J.block( k * 3, 0, 3, 1 ) = M[j];
				J.block( j * 3, 0, 3, 1 ) = M[k];

				JtJ.block( i * 12, i * 12, 12, 12 ) += J * J.transpose() * w_rigid; // block (i, i)
				Jtr.block( i * 12, 0, 12, 1 ) += J * r * w_rigid; // block ( i )
			}
		}
	}
	printf( "E-rigid : %f\n", E );
}

void EvalSmoothTerm( int size, Eigen::MatrixXf &JtJ, Eigen::VectorXf &Jtr, Eigen::VectorXf &x, std::vector<Eigen::Vector3f> &src, std::vector<Eigen::Vector3f> &dst, float &out_E, const std::vector<std::vector<int>> &links ) {
	// E-smooth
	float E = 0;
	float w_smooth = 1.0f;
	{
		for ( int i = 0; i < size; i++ ) {
		for ( int k = 0; k < links[i].size(); k++ ) {
			int j = links[i][k];

			Eigen::Vector3f Mi[4], Mj[4];
			Mi[0] = x.block( i * 12 + 0 * 3, 0, 3, 1 );
			Mi[1] = x.block( i * 12 + 1 * 3, 0, 3, 1 );
			Mi[2] = x.block( i * 12 + 2 * 3, 0, 3, 1 );
			Mi[3] = x.block( i * 12 + 3 * 3, 0, 3, 1 );

			Mj[0] = x.block( j * 12 + 0 * 3, 0, 3, 1 );
			Mj[1] = x.block( j * 12 + 1 * 3, 0, 3, 1 );
			Mj[2] = x.block( j * 12 + 2 * 3, 0, 3, 1 );
			Mj[3] = x.block( j * 12 + 3 * 3, 0, 3, 1 );

			// Ri * ( gj - gi ) - ( gj - gi ) - ( tj - ti )

			Eigen::Vector3f dg = src[j] - src[i];

			Eigen::Vector3f dt = Mj[3] - Mi[3];

			Eigen::Vector3f r = ( dg[0] * Mi[0] + dg[1] * Mi[1] + dg[2] * Mi[2] ) - dg - dt;

			E += r.dot( r ) * w_smooth;

			Eigen::MatrixXf Ji = Eigen::MatrixXf::Zero( 3, 12 );
			Ji( 0, 0 + 3 * 0 ) = dg[0]; Ji( 0, 0 + 3 * 1 ) = dg[1]; Ji( 0, 0 + 3 * 2 ) = dg[2];
			Ji( 1, 1 + 3 * 0 ) = dg[0];	Ji( 1, 1 + 3 * 1 ) = dg[1];	Ji( 1, 1 + 3 * 2 ) = dg[2];
			Ji( 2, 2 + 3 * 0 ) = dg[0];	Ji( 2, 2 + 3 * 1 ) = dg[1];	Ji( 2, 2 + 3 * 2 ) = dg[2];

		//	std::cout << Ji <<std::endl;

			JtJ.block( i * 12, i * 12, 12, 12 ) += Ji.transpose() * Ji * w_smooth;
			Jtr.block( i * 12, 0, 12, 1 ) += Ji.transpose() * r * w_smooth;
		}}
	}
	printf( "E-smooth : %f\n", E );
	out_E += E;
}
bool read_text_file( const char *path, std::vector<std::vector<Eigen::Vector3f>> &poses ) {
	FILE *fp = fopen( path, "r" );
	if ( !fp ) {
		printf( "failed to read text file: %s\n", path );
		return false;
	}

	int frame = 0;
	while( fscanf( fp, "%d", &frame ) == 1 ) {
		frame -= 1;
		if ( frame != poses.size() ) {
			break;
		}
		printf( "read %d\n", frame );

		int subFrame = 0;
		fscanf( fp, "%d", &subFrame );

		poses.push_back( std::vector<Eigen::Vector3f>( 41 ) );
		for ( int i = 0; i < 41; i++ ) {
			float x, y, z;
			fscanf( fp, "%f", &x );
			fscanf( fp, "%f", &y );
			fscanf( fp, "%f", &z );
		//	printf( "%f %f %f\n", x, y, z);
			poses[frame][i][0] = x * 0.001f;
			poses[frame][i][1] = y * 0.001f;
			poses[frame][i][2] = z * 0.001f;
		}
	}

	fclose( fp );
	return true;
}

void test_gen_data( void ) {
	int size = 8;

	std::vector<Eigen::Vector3f> src( size ), dst( size );

	// gen transform
	Eigen::Vector3f euler;
	euler << 20, 50, -45;
	Eigen::Matrix3f rot = euler_to_mat( radians( euler ) );
	Eigen::Vector3f t;
	t << 30, 50, -10;
	std::cout << "Truth" << std::endl;
	std::cout << rot << std::endl;
	std::cout << t << std::endl;

	// gen data
	for ( int i = 0; i < size; i++ ) {
		Eigen::Vector3f euler;
		euler << 20, 50, -45;
		euler[1] += i * 5.0f;
		Eigen::Matrix3f rot = euler_to_mat( radians( euler ) );

		Eigen::Vector3f t;
		t << 30, 50, -10;

		src[i] = Eigen::Vector3f::Random();
		src[i][0] = fmod( src[i][0], 256.0f );
		src[i][1] = fmod( src[i][1], 256.0f );
		src[i][2] = fmod( src[i][2], 256.0f );
		dst[i] = rot * src[i] + t;
		//std::cout << "i << std::endl;
		//std::cout << src[i] << std::endl;
		//std::cout << dst[i] << std::endl;
	}

	// rigid transform
	Eigen::Matrix3f out_rot = Eigen::Matrix3f::Identity();
	Eigen::Vector3f out_t = Eigen::Vector3f::Zero();;
	{
		float rme = icp_point_to_point_svd( src, dst, out_rot, out_t );

		std::cout << "Result" << std::endl;
		std::cout << out_rot << std::endl;
		std::cout << out_t << std::endl;
		std::cout << "rme : " << rme << std::endl;
		std::cout << std::endl;
	}

	Eigen::MatrixXf JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-6f;
	Eigen::VectorXf Jtr = Eigen::VectorXf::Zero( 12 * size );
	Eigen::VectorXf x( 12 * size );
	float E = 0.0f;

	init_affine_transforms( size, x, out_rot, src, dst );

	for ( int i = 0; i < 5; i++ ) {
		JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-6f;
		Jtr = Eigen::VectorXf::Zero( 12 * size );
		E = 0.0f;

	//	EvalLinearSystem( size, JtJ, Jtr, x, src, dst, E );
		//std::cout << Jtr;
		x -= JtJ.fullPivHouseholderQr().solve( Jtr );
	}
}

class Timer {
public :
	clock_t begin, end;

	void Start( void ) {
		begin = clock();
	}

	void Pause( void ) {
		end = clock();
	}

	void Print( const char *id ) {
		double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
		printf( "time : %s : %f\n", id, time_spent );
	}
};

int _tmain(int argc, _TCHAR* argv[])
{
	std::vector<std::vector<Eigen::Vector3f>> poses;

	read_text_file( "New Session 0103.csv", poses );

	// rigid transform

	Timer timer;

	Eigen::Matrix3f out_rot = Eigen::Matrix3f::Identity();
	Eigen::Vector3f out_t = Eigen::Vector3f::Zero();
	{
		timer.Start();
		float rme = icp_point_to_point_svd( poses[10], poses[600], out_rot, out_t );
		timer.Pause();
		timer.Print( "ICP" );

		std::cout << "Result" << std::endl;
		std::cout << out_rot << std::endl;
		std::cout << out_t << std::endl;
		std::cout << "rme : " << rme << std::endl;
		std::cout << std::endl;
	}

	int size = poses[0].size();

	Eigen::MatrixXf JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-6f;
	Eigen::VectorXf Jtr = Eigen::VectorXf::Zero( 12 * size );
	Eigen::VectorXf x( 12 * size );
	float E = 0.0f;

	init_affine_transforms( size, x, out_rot, poses[0], poses[600] );

	std::vector<std::vector<int>> links( poses[0].size() );
	for ( int i = 0; i < poses[0].size(); i++ ) {
		for ( int j = i + 1; j < poses[0].size(); j++ ) {
			if ( ( poses[0][i] - poses[0][j] ).norm() < 0.25f ) {
				links[i].push_back( j );
				links[j].push_back( i );
			}
		}
	}

	for ( int i = 0; i < links.size(); i++ ) {
		printf( "%d : %d\n", i, links[i].size() );
	}

	for ( int i = 0; i < 7; i++ ) {
		JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-3f;
		Jtr = Eigen::VectorXf::Zero( 12 * size );
		E = 0.0f;

		timer.Start();
		EvalRigidTerm( size, JtJ, Jtr, x, poses[0], poses[300], E );
		EvalSmoothTerm( size, JtJ, Jtr, x, poses[0], poses[300], E, links );
		timer.Pause();
		timer.Print( "Jacobian" );

		//std::cout << Jtr;
		timer.Start();
		SparseBlockSquareMatrix B;
		B.createFromDenseMatrix( JtJ, 12 );

		x -= pcg( B, Jtr, 100, 1e-6 );

		//x -= JtJ.fullPivHouseholderQr().solve( Jtr );
		timer.Pause();
		timer.Print( "Solve" );
	}

	return 0;
}


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

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<double, stats<tag::weighted_moment<2> >, double> acc2;
    accumulator_set<double, stats<tag::weighted_moment<7> >, double> acc7;

    acc2(2.1, weight = 0.7);
    acc2(2.7, weight = 1.4);
    acc2(1.8, weight = 0.9);

    acc7(2.1, weight = 0.7);
    acc7(2.7, weight = 1.4);
    acc7(1.8, weight = 0.9);

    BOOST_CHECK_CLOSE(5.403, accumulators::weighted_moment<2>(acc2), 1e-5);
    BOOST_CHECK_CLOSE(548.54182, accumulators::weighted_moment<7>(acc7), 1e-5);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_moment test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#ifndef _mitkCLUtil_HXX
#define _mitkCLUtil_HXX

#include <mitkCLUtil.h>

#include <mitkImageAccessByItk.h>

#include <Eigen/Dense>
#include <itkImage.h>

// itk includes
#include <itkCheckerBoardImageFilter.h>
#include <itkShapedNeighborhoodIterator.h>
// Morphologic Operations
#include <itkBinaryBallStructuringElement.h>
#include <itkBinaryDilateImageFilter.h>
#include <itkBinaryErodeImageFilter.h>
#include <itkBinaryFillholeImageFilter.h>
#include <itkBinaryMorphologicalClosingImageFilter.h>
#include <itkGrayscaleErodeImageFilter.h>
#include <itkGrayscaleDilateImageFilter.h>
#include <itkGrayscaleFillholeImageFilter.h>

// Image Filter
#include <itkDiscreteGaussianImageFilter.h>

void mitk::CLUtil::ProbabilityMap(const mitk::Image::Pointer & image , double mean, double stddev, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkProbabilityMap, 3, mean, stddev, outimage);
}

void mitk::CLUtil::ErodeGrayscale(mitk::Image::Pointer & image , unsigned int radius, mitk::CLUtil::MorphologicalDimensions d, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkErodeGrayscale, 3, outimage, radius, d);
}

void mitk::CLUtil::DilateGrayscale(mitk::Image::Pointer & image, unsigned int radius, mitk::CLUtil::MorphologicalDimensions d, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkDilateGrayscale, 3, outimage, radius, d);
}

void mitk::CLUtil::FillHoleGrayscale(mitk::Image::Pointer & image, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_1(image, mitk::CLUtil::itkFillHoleGrayscale, 3, outimage);
}

void mitk::CLUtil::InsertLabel(mitk::Image::Pointer & image, mitk::Image::Pointer & maskImage, unsigned int label)
{
  AccessByItk_2(image, mitk::CLUtil::itkInsertLabel, maskImage, label);
}

void mitk::CLUtil::GrabLabel(mitk::Image::Pointer & image, mitk::Image::Pointer & outimage, unsigned int label)
{
  AccessFixedDimensionByItk_2(image, mitk::CLUtil::itkGrabLabel, 3, outimage, label);
}

void mitk::CLUtil::ConnectedComponentsImage(mitk::Image::Pointer & image, mitk::Image::Pointer& mask, mitk::Image::Pointer &outimage, unsigned int& num_components)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkConnectedComponentsImage,3, mask, outimage, num_components);
}

void mitk::CLUtil::MergeLabels(mitk::Image::Pointer & img, const std::map<unsigned int, unsigned int> & map)
{
  AccessByItk_1(img, mitk::CLUtil::itkMergeLabels, map);
}

void mitk::CLUtil::CountVoxel(mitk::Image::Pointer image, std::map<unsigned int, unsigned int> & map)
{
  AccessByItk_1(image, mitk::CLUtil::itkCountVoxel, map);
}

void mitk::CLUtil::CountVoxel(mitk::Image::Pointer image, unsigned int label, unsigned int & count)
{
  AccessByItk_2(image, mitk::CLUtil::itkCountVoxel, label, count);
}

void mitk::CLUtil::CountVoxel(mitk::Image::Pointer image, unsigned int & count)
{
  AccessByItk_1(image, mitk::CLUtil::itkCountVoxel, count);
}

void mitk::CLUtil::CreateCheckerboardMask(mitk::Image::Pointer image, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_1(image, mitk::CLUtil::itkCreateCheckerboardMask,3, outimage);
}

void mitk::CLUtil::LogicalAndImages(const mitk::Image::Pointer & image1, const mitk::Image::Pointer & image2, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_2(image1,itkLogicalAndImages, 3, image2, outimage);

}

void mitk::CLUtil::InterpolateCheckerboardPrediction(mitk::Image::Pointer checkerboard_prediction, mitk::Image::Pointer & checkerboard_mask, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_2(checkerboard_prediction, mitk::CLUtil::itkInterpolateCheckerboardPrediction,3, checkerboard_mask, outimage);
}

void mitk::CLUtil::GaussianFilter(mitk::Image::Pointer image, mitk::Image::Pointer & smoothed ,double sigma)
{
  AccessFixedDimensionByItk_2(image, mitk::CLUtil::itkGaussianFilter,3, smoothed, sigma);
}

void mitk::CLUtil::DilateBinary(mitk::Image::Pointer & sourceImage, mitk::Image::Pointer& resultImage, int factor , MorphologicalDimensions d)
{
  AccessFixedDimensionByItk_3(sourceImage, mitk::CLUtil::itkDilateBinary, 3, resultImage, factor, d);
}

void mitk::CLUtil::ErodeBinary(mitk::Image::Pointer & sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  AccessFixedDimensionByItk_3(sourceImage, mitk::CLUtil::itkErodeBinary, 3, resultImage, factor, d);
}

void mitk::CLUtil::ClosingBinary(mitk::Image::Pointer & sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  AccessFixedDimensionByItk_3(sourceImage, mitk::CLUtil::itkClosingBinary, 3, resultImage, factor, d);
}

template<typename TImageType>
void mitk::CLUtil::itkProbabilityMap(const TImageType * sourceImage, double mean, double std_dev, mitk::Image::Pointer& resultImage)
{
  itk::Image<double, 3>::Pointer itk_img = itk::Image<double, 3>::New();
  itk_img->SetRegions(sourceImage->GetLargestPossibleRegion());
  itk_img->SetOrigin(sourceImage->GetOrigin());
  itk_img->SetSpacing(sourceImage->GetSpacing());
  itk_img->SetDirection(sourceImage->GetDirection());
  itk_img->Allocate();

  itk::ImageRegionConstIterator<TImageType> it(sourceImage,sourceImage->GetLargestPossibleRegion());
  itk::ImageRegionIterator<itk::Image<double, 3> > outit(itk_img,itk_img->GetLargestPossibleRegion());

  while(!it.IsAtEnd())
  {
    double x = it.Value();

    double prob = (1.0/(std_dev*std::sqrt(2.0*M_PI))) * std::exp(-(((x-mean)*(x-mean))/(2.0*std_dev*std_dev)));
    outit.Set(prob);
    ++it;
    ++outit;
  }

  mitk::CastToMitkImage(itk_img, resultImage);
}

template< typename TImageType >
void mitk::CLUtil::itkInterpolateCheckerboardPrediction(TImageType * checkerboard_prediction, Image::Pointer &checkerboard_mask, mitk::Image::Pointer & outimage)
{
  typename TImageType::Pointer itk_checkerboard_mask;
  mitk::CastToItkImage(checkerboard_mask,itk_checkerboard_mask);

  typename TImageType::Pointer itk_outimage = TImageType::New();
  itk_outimage->SetRegions(checkerboard_prediction->GetLargestPossibleRegion());
  itk_outimage->SetDirection(checkerboard_prediction->GetDirection());
  itk_outimage->SetOrigin(checkerboard_prediction->GetOrigin());
  itk_outimage->SetSpacing(checkerboard_prediction->GetSpacing());
  itk_outimage->Allocate();
  itk_outimage->FillBuffer(0);

  //typedef typename itk::ShapedNeighborhoodIterator<TImageType>::SizeType SizeType;
  typedef itk::Size<3> SizeType;
  SizeType size;
  size.Fill(1);
  itk::ShapedNeighborhoodIterator<TImageType> iit(size,checkerboard_prediction,checkerboard_prediction->GetLargestPossibleRegion());
  itk::ShapedNeighborhoodIterator<TImageType> mit(size,itk_checkerboard_mask,itk_checkerboard_mask->GetLargestPossibleRegion());
  itk::ImageRegionIterator<TImageType> oit(itk_outimage,itk_outimage->GetLargestPossibleRegion());

  typedef typename itk::ShapedNeighborhoodIterator<TImageType>::OffsetType OffsetType;
  OffsetType offset;
  offset.Fill(0);
  offset[0] = 1;       // {1,0,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);
  offset[0] = -1;      // {-1,0,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);
  offset[0] = 0; offset[1] = 1; //{0,1,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);
  offset[1] = -1;      //{0,-1,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);

  //    iit.ActivateOffset({{0,0,1}});
  //    iit.ActivateOffset({{0,0,-1}});
  //    mit.ActivateOffset({{0,0,1}});
  //    mit.ActivateOffset({{0,0,-1}});

  while(!iit.IsAtEnd())
  {
    if(mit.GetCenterPixel() == 0)
    {
      typename TImageType::PixelType mean = 0;
      for (auto i = iit.Begin(); ! i.IsAtEnd(); i++)
      { mean += i.Get(); }

      //std::sort(list.begin(),list.end(),[](const typename TImageType::PixelType x,const typename TImageType::PixelType y){return x<=y;});

      oit.Set((mean+0.5)/6.0);
    }
    else
    {
      oit.Set(iit.GetCenterPixel());
    }
    ++iit;
    ++mit;
    ++oit;
  }

  mitk::CastToMitkImage(itk_outimage,outimage);
}

template< typename TImageType >
void mitk::CLUtil::itkCreateCheckerboardMask(TImageType * image, mitk::Image::Pointer & outimage)
{
  typename TImageType::Pointer zeroimg = TImageType::New();
  zeroimg->SetRegions(image->GetLargestPossibleRegion());
  zeroimg->SetDirection(image->GetDirection());
  zeroimg->SetOrigin(image->GetOrigin());
  zeroimg->SetSpacing(image->GetSpacing());

  zeroimg->Allocate();
  zeroimg->FillBuffer(0);

  typedef itk::CheckerBoardImageFilter<TImageType> FilterType;
  typename FilterType::Pointer filter = FilterType::New();
  filter->SetInput1(image);
  filter->SetInput2(zeroimg);
  typename FilterType::PatternArrayType pattern;
  pattern.SetElement(0,(image->GetLargestPossibleRegion().GetSize()[0]));
  pattern.SetElement(1,(image->GetLargestPossibleRegion().GetSize()[1]));
  pattern.SetElement(2,(image->GetLargestPossibleRegion().GetSize()[2]));
  filter->SetCheckerPattern(pattern);

  filter->Update();
  mitk::CastToMitkImage(filter->GetOutput(), outimage);
}

template <class TImageType>
void mitk::CLUtil::itkSumVoxelForLabel(TImageType* image, const mitk::Image::Pointer & source , typename TImageType::PixelType label, double & val )
{
  itk::Image<double,3>::Pointer itk_source;
  mitk::CastToItkImage(source,itk_source);

  itk::ImageRegionConstIterator<TImageType> inputIter(image, image->GetLargestPossibleRegion());
  itk::ImageRegionConstIterator< itk::Image<double,3> > sourceIter(itk_source, itk_source->GetLargestPossibleRegion());
  while(!inputIter.IsAtEnd())
  {
    if(inputIter.Value() == label) val += sourceIter.Value();
    ++inputIter;
    ++sourceIter;
  }
}

template <class TImageType>
void mitk::CLUtil::itkSqSumVoxelForLabel(TImageType* image, const mitk::Image::Pointer & source, typename TImageType::PixelType label, double & val )
{
  itk::Image<double,3>::Pointer itk_source;
  mitk::CastToItkImage(source,itk_source);

  itk::ImageRegionConstIterator<TImageType> inputIter(image, image->GetLargestPossibleRegion());
  itk::ImageRegionConstIterator< itk::Image<double,3> > sourceIter(itk_source, itk_source->GetLargestPossibleRegion());
  while(!inputIter.IsAtEnd())
  {
    if(inputIter.Value() == label) val += sourceIter.Value() * sourceIter.Value();
    ++inputIter;
    ++sourceIter;
  }
}

template<typename TStructuringElement>
void mitk::CLUtil::itkFitStructuringElement(TStructuringElement & se, MorphologicalDimensions d, int factor)
{
  typename TStructuringElement::SizeType size;
  size.Fill(factor);
  switch(d)
  {
  case(All):
  case(Axial):
    size.SetElement(2,0);
    break;
  case(Sagital):
    size.SetElement(0,0);
    break;
  case(Coronal):
    size.SetElement(1,0);
    break;
  }
  se.SetRadius(size);
  se.CreateStructuringElement();
}

template<typename TImageType>
void mitk::CLUtil::itkClosingBinary(TImageType * sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3> BallType;
  typedef itk::BinaryMorphologicalClosingImageFilter<TImageType, TImageType, BallType> FilterType;

  BallType strElem;
  itkFitStructuringElement(strElem,d,factor);

  typename FilterType::Pointer erodeFilter = FilterType::New();
  erodeFilter->SetKernel(strElem);
  erodeFilter->SetInput(sourceImage);
  erodeFilter->SetForegroundValue(1);
  erodeFilter->Update();

  mitk::CastToMitkImage(erodeFilter->GetOutput(), resultImage);

}

template<typename TImageType>
void mitk::CLUtil::itkDilateBinary(TImageType * sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3> BallType;
  typedef typename itk::BinaryDilateImageFilter<TImageType, TImageType, BallType> BallDilateFilterType;

  BallType strElem;
  itkFitStructuringElement(strElem,d,factor);

  typename BallDilateFilterType::Pointer erodeFilter = BallDilateFilterType::New();
  erodeFilter->SetKernel(strElem);
  erodeFilter->SetInput(sourceImage);
  erodeFilter->SetDilateValue(1);
  erodeFilter->Update();

  mitk::CastToMitkImage(erodeFilter->GetOutput(), resultImage);

}

template<typename TImageType>
void mitk::CLUtil::itkErodeBinary(TImageType * sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3> BallType;
  typedef typename itk::BinaryErodeImageFilter<TImageType, TImageType, BallType> BallErodeFilterType;

  BallType strElem;
  itkFitStructuringElement(strElem,d,factor);

  typename BallErodeFilterType::Pointer erodeFilter = BallErodeFilterType::New();
  erodeFilter->SetKernel(strElem);
  erodeFilter->SetInput(sourceImage);
  erodeFilter->SetErodeValue(1);
//  erodeFilter->UpdateLargestPossibleRegion();
  erodeFilter->Update();

  mitk::CastToMitkImage(erodeFilter->GetOutput(), resultImage);

}

///
/// \brief itkFillHolesBinary
/// \param sourceImage
/// \param resultImage
///
template<typename TPixel, unsigned int VDimension>
void mitk::CLUtil::itkFillHolesBinary(itk::Image<TPixel, VDimension>* sourceImage, mitk::Image::Pointer& resultImage)
{
  typedef itk::Image<TPixel, VDimension> ImageType;
  typedef typename itk::BinaryFillholeImageFilter<ImageType> FillHoleFilterType;

  typename FillHoleFilterType::Pointer fillHoleFilter = FillHoleFilterType::New();
  fillHoleFilter->SetInput(sourceImage);
  fillHoleFilter->SetForegroundValue(1);
  fillHoleFilter->Update();

  mitk::CastToMitkImage(fillHoleFilter->GetOutput(), resultImage);
}

///
/// \brief itkLogicalAndImages
/// \param image1 keep the values of image 1
/// \param image2
///
template<typename TImageType>
void mitk::CLUtil::itkLogicalAndImages(const TImageType * image1, const mitk::Image::Pointer & image2, mitk::Image::Pointer & outimage)
{

  typename TImageType::Pointer itk_outimage = TImageType::New();
  itk_outimage->SetRegions(image1->GetLargestPossibleRegion());
  itk_outimage->SetDirection(image1->GetDirection());
  itk_outimage->SetOrigin(image1->GetOrigin());
  itk_outimage->SetSpacing(image1->GetSpacing());

  itk_outimage->Allocate();
  itk_outimage->FillBuffer(0);

  typename TImageType::Pointer itk_image2;
  mitk::CastToItkImage(image2,itk_image2);

  itk::ImageRegionConstIterator<TImageType> it1(image1, image1->GetLargestPossibleRegion());
  itk::ImageRegionConstIterator<TImageType> it2(itk_image2, itk_image2->GetLargestPossibleRegion());
  itk::ImageRegionIterator<TImageType> oit(itk_outimage,itk_outimage->GetLargestPossibleRegion());

  while(!it1.IsAtEnd())
  {
    if(it1.Value() == 0 || it2.Value() == 0)
    {
      oit.Set(0);
    }else
      oit.Set(it1.Value());
    ++it1;
    ++it2;
    ++oit;
  }

  mitk::CastToMitkImage(itk_outimage, outimage);
}

///
/// \brief GaussianFilter
/// \param image
/// \param smoothed
/// \param sigma
///
template<class TImageType>
void mitk::CLUtil::itkGaussianFilter(TImageType * image, mitk::Image::Pointer & smoothed ,double sigma)
{
  typedef itk::DiscreteGaussianImageFilter<TImageType,TImageType> FilterType;
  typename FilterType::Pointer filter = FilterType::New();
  filter->SetInput(image);
  filter->SetVariance(sigma);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),smoothed);
}

template<class TImageType>
void mitk::CLUtil::itkErodeGrayscale(TImageType * image, mitk::Image::Pointer & outimage , unsigned int radius, mitk::CLUtil::MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3>          StructureElementType;
  typedef itk::GrayscaleErodeImageFilter<TImageType,TImageType,StructureElementType>    FilterType;

  StructureElementType ball;
  itkFitStructuringElement(ball,d, radius);

  typename FilterType::Pointer filter = FilterType::New();
  filter->SetKernel(ball);
  filter->SetInput(image);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),outimage);
}

template<class TImageType>
void mitk::CLUtil::itkDilateGrayscale(TImageType * image, mitk::Image::Pointer & outimage , unsigned int radius, mitk::CLUtil::MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3>          StructureElementType;
  typedef itk::GrayscaleDilateImageFilter<TImageType,TImageType,StructureElementType>    FilterType;

  StructureElementType ball;
  itkFitStructuringElement(ball,d, radius);

  typename FilterType::Pointer filter = FilterType::New();
  filter->SetKernel(ball);
  filter->SetInput(image);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),outimage);
}

template<class TImageType>
void mitk::CLUtil::itkFillHoleGrayscale(TImageType * image, mitk::Image::Pointer & outimage)
{
  typedef itk::GrayscaleFillholeImageFilter<TImageType,TImageType>    FilterType;

  typename FilterType::Pointer filter = FilterType::New();
  filter->SetInput(image);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),outimage);
}

#endif


#include "nanobench.h"
#include <Eigen/Dense>
#include <random>
#include <iostream>
#include <atomic>
#include <memory>
#include "../include/cematrix.hpp"
#include "../include/noexpr/matrix.hpp"
#include "../include/noexpr/operator.hpp"
#include "../include/vari/matrix.hpp"
#include "../include/vari/vari_expr.hpp"
#include "../include/crtp/matrix_expr.hpp"
#include "../include/crtp/operator_expr.hpp"
constexpr auto N = 8;
using scaler_t = int;
using fmatNN_crtp = ce::crtp::matrix_t<scaler_t, N, N>;
using fmatNN_noexpr = ce::noexpr::matrix<scaler_t, N, N>;
using fmatNN_vari = ce::vari::matrix_t<scaler_t, N, N>;

std::random_device rd;
ankerl::nanobench::Rng e2(rd());
std::uniform_int_distribution<scaler_t> dist(-5000, 5000);

template<typename mat_t>
auto rand_mat()
{
    std::array<scaler_t, N * N> ret{};
    for (auto &element : ret)
    {
        element = dist(e2);
    }
    mat_t ret_mat{};
    ret_mat.data = ret;
    return ret_mat;
}

void bench_noexpr(ankerl::nanobench::Bench *bench)
{
    const fmatNN_noexpr a = rand_mat<fmatNN_noexpr>();
    const fmatNN_noexpr b = rand_mat<fmatNN_noexpr>();
    const fmatNN_noexpr c = rand_mat<fmatNN_noexpr>();

    bench->run("noexpr", [&]() {
        fmatNN_noexpr res = (a * b + a * c);
        ankerl::nanobench::doNotOptimizeAway(res);
    });
}

void bench_crtp(ankerl::nanobench::Bench *bench)
{
    const fmatNN_crtp a = rand_mat<fmatNN_crtp>();
    const fmatNN_crtp b = rand_mat<fmatNN_crtp>();
    const fmatNN_crtp c = rand_mat<fmatNN_crtp>();

    bench->run("crtp", [&]() {
        fmatNN_crtp res = (a * b + a * c);
        ankerl::nanobench::doNotOptimizeAway(res);
    });
}
void bench_vari(ankerl::nanobench::Bench *bench)
{
    const fmatNN_vari a = rand_mat<fmatNN_vari>();
    const fmatNN_vari b = rand_mat<fmatNN_vari>();
    const fmatNN_vari c = rand_mat<fmatNN_vari>();

    bench->run("vari", [&]() {
        fmatNN_vari res = (a * b + a * c);
        ankerl::nanobench::doNotOptimizeAway(res);
    });
}
void bench_Eigen(ankerl::nanobench::Bench *bench)
{
    const Eigen::Matrix<scaler_t, N, N> a = Eigen::Matrix<scaler_t, N, N>::Random();
    const Eigen::Matrix<scaler_t, N, N> b = Eigen::Matrix<scaler_t, N, N>::Random();
    const Eigen::Matrix<scaler_t, N, N> c = Eigen::Matrix<scaler_t, N, N>::Random();

    bench->run("Eigen", [&]() {
        Eigen::Matrix<scaler_t, N, N> res = (a * b + a * c);
        ankerl::nanobench::doNotOptimizeAway(res);
    });
}

int main()
{

    ankerl::nanobench::Bench b;
    b.title("Expression templates benchmark").unit("expr").warmup(500).relative(true);
    b.performanceCounters(true);
    bench_Eigen(&b);
    bench_crtp(&b);
    bench_noexpr(&b);
    bench_vari(&b);
}


#include <Eigen/Dense>
#include <memory>
#include <iostream>
#include <limits>
#include "bpmfutils.h"
#include "sparsetensor.h"

using namespace Eigen;

std::pair<double,double> eval_rmse_tensor(
		SparseMode & sparseMode,
		const int Nepoch,
		Eigen::VectorXd & predictions,
		Eigen::VectorXd & predictions_var,
		std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples,
		double mean_value)
{
  auto& U = samples[0];

  const int nmodes = samples.size();
  const int num_latents = U->rows();

  const unsigned N = sparseMode.values.size();
  double se = 0.0, se_avg = 0.0;

  if (N == 0) {
    // No test data, returning NaN's
    return std::make_pair(std::numeric_limits<double>::quiet_NaN(),
                          std::numeric_limits<double>::quiet_NaN());
  }

  if (N != predictions.size()) {
    throw std::runtime_error("Ytest.size() and predictions.size() must be equal.");
  }
	if (sparseMode.row_ptr.size() - 1 != U->cols()) {
    throw std::runtime_error("U.cols() and sparseMode size must be equal.");
	}

#pragma omp parallel for schedule(dynamic, 2) reduction(+:se, se_avg)
  for (int n = 0; n < U->cols(); n++) {
    Eigen::VectorXd u = U->col(n);
    for (int j = sparseMode.row_ptr(n);
             j < sparseMode.row_ptr(n + 1);
             j++)
    {
      VectorXi idx = sparseMode.indices.row(j);
      double pred = mean_value;
      for (int d = 0; d < num_latents; d++) {
        double tmp = u(d);

        for (int m = 1; m < nmodes; m++) {
          tmp *= (*samples[m])(d, idx(m - 1));
        }
        pred += tmp;
      }

      double pred_avg;
      if (Nepoch == 0) {
        pred_avg = pred;
      } else {
        double delta = pred - predictions(j);
        pred_avg = (predictions(j) + delta / (Nepoch + 1));
        predictions_var(j) += delta * (pred - pred_avg);
      }
      se     += square(sparseMode.values(j) - pred);
      se_avg += square(sparseMode.values(j) - pred_avg);
      predictions(j) = pred_avg;
    }
  }
  const double rmse = sqrt(se / N);
  const double rmse_avg = sqrt(se_avg / N);
  return std::make_pair(rmse, rmse_avg);
}


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "../precomp.hpp"
#include "../usac.hpp"
#ifdef HAVE_EIGEN
#include <Eigen/Eigen>
#endif

namespace cv { namespace usac {
class HomographyMinimalSolver4ptsGEMImpl : public HomographyMinimalSolver4ptsGEM {
private:
    const Mat * points_mat;
    const float * const points;
public:
    explicit HomographyMinimalSolver4ptsGEMImpl (const Mat &points_) :
        points_mat(&points_), points ((float*) points_.data) {}

    int estimate (const std::vector<int>& sample, std::vector<Mat> &models) const override {
        int m = 8, n = 9;
        std::vector<double> A(72, 0);
        int cnt = 0;
        for (int i = 0; i < 4; i++) {
            const int smpl = 4*sample[i];
            const double x1 = points[smpl], y1 = points[smpl+1], x2 = points[smpl+2], y2 = points[smpl+3];

            A[cnt++] = -x1;
            A[cnt++] = -y1;
            A[cnt++] = -1;
            cnt += 3; // skip zeros
            A[cnt++] = x2*x1;
            A[cnt++] = x2*y1;
            A[cnt++] = x2;

            cnt += 3;
            A[cnt++] = -x1;
            A[cnt++] = -y1;
            A[cnt++] = -1;
            A[cnt++] = y2*x1;
            A[cnt++] = y2*y1;
            A[cnt++] = y2;
        }

        if (!Math::eliminateUpperTriangular(A, m, n))
            return 0;

        models = std::vector<Mat>{ Mat_<double>(3,3) };
        auto * h = (double *) models[0].data;
        h[8] = 1.;

        // start from the last row
        for (int i = m-1; i >= 0; i--) {
            double acc = 0;
            for (int j = i+1; j < n; j++)
                acc -= A[i*n+j]*h[j];

            h[i] = acc / A[i*n+i];
            // due to numerical errors return 0 solutions
            if (std::isnan(h[i]))
                return 0;
        }
        return 1;
    }

    int getMaxNumberOfSolutions () const override { return 1; }
    int getSampleSize() const override { return 4; }
    Ptr<MinimalSolver> clone () const override {
        return makePtr<HomographyMinimalSolver4ptsGEMImpl>(*points_mat);
    }
};
Ptr<HomographyMinimalSolver4ptsGEM> HomographyMinimalSolver4ptsGEM::create(const Mat &points_) {
    return makePtr<HomographyMinimalSolver4ptsGEMImpl>(points_);
}

class HomographyNonMinimalSolverImpl : public HomographyNonMinimalSolver {
private:
    const Mat * points_mat;
    const Ptr<NormTransform> normTr;
public:
    explicit HomographyNonMinimalSolverImpl (const Mat &points_) :
        points_mat(&points_), normTr (NormTransform::create(points_)) {}

    /*
     * Find Homography matrix using (weighted) non-minimal estimation.
     * Use Principal Component Analysis. Use normalized points.
     */
    int estimate (const std::vector<int> &sample, int sample_size, std::vector<Mat> &models,
            const std::vector<double> &weights) const override {
        if (sample_size < getMinimumRequiredSampleSize())
            return 0;

        Matx33d T1, T2;
        Mat norm_points_;
        normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2);

        /*
         * @norm_points is matrix 4 x inlier_size
         * @weights is vector of inliers_size
         * weights[i] is weight of i-th inlier
         */
        const auto * const norm_points = (float *) norm_points_.data;

        double a1[9] = {0, 0, -1, 0, 0, 0, 0, 0, 0},
               a2[9] = {0, 0, 0, 0, 0, -1, 0, 0, 0},
               AtA[81] = {0};

        if (weights.empty()) {
            for (int i = 0; i < sample_size; i++) {
                const int smpl = 4*i;
                const double x1 = norm_points[smpl  ], y1 = norm_points[smpl+1],
                             x2 = norm_points[smpl+2], y2 = norm_points[smpl+3];

                a1[0] = -x1;
                a1[1] = -y1;
                a1[6] = x2*x1;
                a1[7] = x2*y1;
                a1[8] = x2;

                a2[3] = -x1;
                a2[4] = -y1;
                a2[6] = y2*x1;
                a2[7] = y2*y1;
                a2[8] = y2;

                for (int j = 0; j < 9; j++)
                    for (int z = j; z < 9; z++)
                        AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z];
            }
        } else {
            for (int i = 0; i < sample_size; i++) {
                const int smpl = 4*i;
                const double weight = weights[i];
                const double x1 = norm_points[smpl  ], y1 = norm_points[smpl+1],
                             x2 = norm_points[smpl+2], y2 = norm_points[smpl+3];
                const double minus_weight_times_x1 = -weight * x1,
                             minus_weight_times_y1 = -weight * y1,
                                   weight_times_x2 =  weight * x2,
                                   weight_times_y2 =  weight * y2;

                a1[0] = minus_weight_times_x1;
                a1[1] = minus_weight_times_y1;
                a1[2] = -weight;
                a1[6] = weight_times_x2 * x1;
                a1[7] = weight_times_x2 * y1;
                a1[8] = weight_times_x2;

                a2[3] = minus_weight_times_x1;
                a2[4] = minus_weight_times_y1;
                a2[5] = -weight;
                a2[6] = weight_times_y2 * x1;
                a2[7] = weight_times_y2 * y1;
                a2[8] = weight_times_y2;

                for (int j = 0; j < 9; j++)
                    for (int z = j; z < 9; z++)
                        AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z];
            }
        }

        // copy symmetric part of covariance matrix
        for (int j = 1; j < 9; j++)
            for (int z = 0; z < j; z++)
                AtA[j*9+z] = AtA[z*9+j];

#ifdef HAVE_EIGEN
        Mat H = Mat_<double>(3,3);
        Eigen::HouseholderQR<Eigen::Matrix<double, 9, 9>> qr((Eigen::Matrix<double, 9, 9> (AtA)));
        const Eigen::Matrix<double, 9, 9> &Q = qr.householderQ();
        // extract the last nullspace
        Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)H.data) = Q.col(8);
#else
        Matx<double, 9, 9> Vt;
        Vec<double, 9> D;
        if (! eigen(Matx<double, 9, 9>(AtA), D, Vt)) return 0;
        Mat H = Mat_<double>(3, 3, Vt.val + 72/*=8*9*/);
#endif

        models = std::vector<Mat>{ T2.inv() * H * T1 };
        return 1;
    }

    int getMinimumRequiredSampleSize() const override { return 4; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<NonMinimalSolver> clone () const override {
        return makePtr<HomographyNonMinimalSolverImpl>(*points_mat);
    }
};
Ptr<HomographyNonMinimalSolver> HomographyNonMinimalSolver::create(const Mat &points_) {
    return makePtr<HomographyNonMinimalSolverImpl>(points_);
}

class AffineMinimalSolverImpl : public AffineMinimalSolver {
private:
    const Mat * points_mat;
    const float * const points;
public:
    explicit AffineMinimalSolverImpl (const Mat &points_) :
            points_mat(&points_), points((float *) points_.data) {}
    /*
        Affine transformation
        x1 y1 1 0  0  0   a   u1
        0  0  0 x1 y1 1   b   v1
        x2 y2 1 0  0  0   c   u2
        0  0  0 x2 y2 1 * d = v2
        x3 y3 1 0  0  0   e   u3
        0  0  0 x3 y3 1   f   v3
    */
    int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
        const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2];
        const auto
                x1 = points[smpl1], y1 = points[smpl1+1], u1 = points[smpl1+2], v1 = points[smpl1+3],
                x2 = points[smpl2], y2 = points[smpl2+1], u2 = points[smpl2+2], v2 = points[smpl2+3],
                x3 = points[smpl3], y3 = points[smpl3+1], u3 = points[smpl3+2], v3 = points[smpl3+3];

        // covers degeneracy test when all 3 points are collinear.
        // In this case denominator will be 0
        double denominator = x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2;
        if (fabs(denominator) < FLT_EPSILON) // check if denominator is zero
            return 0;
        denominator = 1. / denominator;

        double a =  (u1*y2 - u2*y1 - u1*y3 + u3*y1 + u2*y3 - u3*y2) * denominator;
        double b = -(u1*x2 - u2*x1 - u1*x3 + u3*x1 + u2*x3 - u3*x2) * denominator;
        double c = u1 - a * x1 - b * y1; // ax1 + by1 + c = u1
        double d =  (v1*y2 - v2*y1 - v1*y3 + v3*y1 + v2*y3 - v3*y2) * denominator;
        double e = -(v1*x2 - v2*x1 - v1*x3 + v3*x1 + v2*x3 - v3*x2) * denominator;
        double f = v1 - d * x1 - e * y1; // dx1 + ey1 + f = v1

        models[0] = Mat(Matx33d(a, b, c, d, e, f, 0, 0, 1));
        return 1;
    }
    int getSampleSize() const override { return 3; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<MinimalSolver> clone () const override {
        return makePtr<AffineMinimalSolverImpl>(*points_mat);
    }
};
Ptr<AffineMinimalSolver> AffineMinimalSolver::create(const Mat &points_) {
    return makePtr<AffineMinimalSolverImpl>(points_);
}

class AffineNonMinimalSolverImpl : public AffineNonMinimalSolver {
private:
    const Mat * points_mat;
    const float * const points;
    // const NormTransform<double> norm_transform;
public:
    explicit AffineNonMinimalSolverImpl (const Mat &points_) :
            points_mat(&points_), points((float*) points_.data)
    /*, norm_transform(points_)*/ {}

    int estimate (const std::vector<int> &sample, int sample_size, std::vector<Mat> &models,
                  const std::vector<double> &weights) const override {
        // surprisingly normalization of points does not improve the output model
        // Mat norm_points_, T1, T2;
        // norm_transform.getNormTransformation(norm_points_, sample, sample_size, T1, T2);
        // const auto * const n_pts = (double *) norm_points_.data;

        if (sample_size < getMinimumRequiredSampleSize())
            return 0;
        // do Least Squares
        // Ax = b   ->  A^T Ax = A^T b
        // x = (A^T A)^-1 A^T b
        double AtA[36] = {0}, Ab[6] = {0};
        double r1[6] = {0, 0, 1, 0, 0, 0}; // row 1 of A
        double r2[6] = {0, 0, 0, 0, 0, 1}; // row 2 of A

        if (weights.empty())
            for (int p = 0; p < sample_size; p++) {
                // if (weights != nullptr) weight = weights[sample[p]];

                const int smpl = 4*sample[p];
                const double x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
                // const double x1=n_pts[smpl], y1=n_pts[smpl+1], x2=n_pts[smpl+2], y2=n_pts[smpl+3];

                r1[0] = x1;
                r1[1] = y1;

                r2[3] = x1;
                r2[4] = y1;

                for (int j = 0; j < 6; j++) {
                    for (int z = j; z < 6; z++)
                        AtA[j * 6 + z] += r1[j] * r1[z] + r2[j] * r2[z];
                    Ab[j] += r1[j]*x2 + r2[j]*y2;
                }
            }
        else
            for (int p = 0; p < sample_size; p++) {
                const int smpl = 4*sample[p];
                const double weight = weights[p];
                const double weight_times_x1 = weight * points[smpl  ],
                             weight_times_y1 = weight * points[smpl+1],
                             weight_times_x2 = weight * points[smpl+2],
                             weight_times_y2 = weight * points[smpl+3];

                r1[0] = weight_times_x1;
                r1[1] = weight_times_y1;
                r1[2] = weight;

                r2[3] = weight_times_x1;
                r2[4] = weight_times_y1;
                r2[5] = weight;

                for (int j = 0; j < 6; j++) {
                    for (int z = j; z < 6; z++)
                        AtA[j * 6 + z] += r1[j] * r1[z] + r2[j] * r2[z];
                    Ab[j] += r1[j]*weight_times_x2 + r2[j]*weight_times_y2;
                }
            }

        // copy symmetric part
        for (int j = 1; j < 6; j++)
            for (int z = 0; z < j; z++)
                AtA[j*6+z] = AtA[z*6+j];

        Vec6d aff;
        if (!solve(Matx66d(AtA), Vec6d(Ab), aff))
            return 0;
        models[0] = Mat(Matx33d(aff(0), aff(1), aff(2),
                                aff(3), aff(4), aff(5),
                                0, 0, 1));

        // models[0] = T2.inv() * models[0] * T1;
        return 1;
    }

    int getMinimumRequiredSampleSize() const override { return 3; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<NonMinimalSolver> clone () const override {
        return makePtr<AffineNonMinimalSolverImpl>(*points_mat);
    }
};
Ptr<AffineNonMinimalSolver> AffineNonMinimalSolver::create(const Mat &points_) {
    return makePtr<AffineNonMinimalSolverImpl>(points_);
}
}}


#include <dlib/clustering.h>
#include <dlib/matrix.h>
#include <plot.h>

#include <experimental/filesystem>
#include <iostream>
#include <unordered_map>

using namespace dlib;
namespace fs = std::experimental::filesystem;
using SampleType = dlib::matrix<double, 1, 1>;
using Samples = std::vector<SampleType>;

const std::vector<std::string> data_names{"dataset0.csv", "dataset1.csv",
                                          "dataset2.csv", "dataset3.csv",
                                          "dataset4.csv", "dataset5.csv"};

const std::vector<std::string> colors{"black", "red",    "blue",  "green",
                                      "cyan",  "yellow", "brown", "magenta"};

using DataType = double;
using Coords = std::vector<DataType>;
using PointCoords = std::pair<Coords, Coords>;
using Clusters = std::unordered_map<size_t, PointCoords>;

void PlotClusters(const Clusters& clusters,
                  const std::string& name,
                  const std::string& file_name) {
  plotcpp::Plot plt;
  plt.SetTerminal("png");
  plt.SetOutput(file_name);
  plt.SetTitle(name);
  plt.SetXLabel("x");
  plt.SetYLabel("y");
  plt.SetAutoscale();
  plt.GnuplotCommand("set grid");

  auto draw_state = plt.StartDraw2D<Coords::const_iterator>();
  for (auto& cluster : clusters) {
    std::stringstream params;
    params << "lc rgb '" << colors[cluster.first] << "' pt 7";
    plt.AddDrawing(draw_state,
                   plotcpp::Points(
                       cluster.second.first.begin(), cluster.second.first.end(),
                       cluster.second.second.begin(),
                       std::to_string(cluster.first) + " cls", params.str()));
  }

  plt.EndDraw2D(draw_state);
  plt.Flush();
}

template <typename I>
void DoHierarhicalClustering(const I& inputs,
                             size_t num_clusters,
                             const std::string& name) {
  // agglomerative clustering algorithm
  matrix<double> dists(inputs.nr(), inputs.nr());
  for (long r = 0; r < dists.nr(); ++r) {
    for (long c = 0; c < dists.nc(); ++c) {
      dists(r, c) = length(subm(inputs, r, 0, 1, 2) - subm(inputs, c, 0, 1, 2));
    }
  }
  std::vector<unsigned long> clusters;
  bottom_up_cluster(dists, clusters, num_clusters);
  Clusters plot_clusters;
  for (long i = 0; i != inputs.nr(); i++) {
    auto cluser_idx = clusters[i];
    plot_clusters[cluser_idx].first.push_back(inputs(i, 0));
    plot_clusters[cluser_idx].second.push_back(inputs(i, 1));
  }

  PlotClusters(plot_clusters, "Agglomerative clustering", name + "-aggl.png");
}

template <typename I>
void DoGraphClustering(const I& inputs, const std::string& name) {
  // chinese whispers algorithm
  std::vector<sample_pair> edges;
  for (long i = 0; i < inputs.nr(); ++i) {
    for (long j = 0; j < inputs.nr(); ++j) {
      auto dist = length(subm(inputs, i, 0, 1, 2) - subm(inputs, j, 0, 1, 2));
      if (dist < 1)
        edges.push_back(sample_pair(i, j, dist));
    }
  }
  std::vector<unsigned long> clusters;
  const auto num_clusters = chinese_whispers(edges, clusters);
  std::cout << "Num clusters detected: " << num_clusters << std::endl;
  Clusters plot_clusters;
  for (long i = 0; i != inputs.nr(); i++) {
    auto cluser_idx = clusters[i];
    plot_clusters[cluser_idx].first.push_back(inputs(i, 0));
    plot_clusters[cluser_idx].second.push_back(inputs(i, 1));
  }

  PlotClusters(plot_clusters, "Graph clustering", name + "-graph.png");
}

template <typename I>
void DoGraphNewmanClustering(const I& inputs, const std::string& name) {
  std::vector<sample_pair> edges;
  for (long i = 0; i < inputs.nr(); ++i) {
    for (long j = 0; j < inputs.nr(); ++j) {
      auto dist = length(subm(inputs, i, 0, 1, 2) - subm(inputs, j, 0, 1, 2));
      if (dist < 0.5)
        edges.push_back(sample_pair(i, j, dist));
    }
  }
  remove_duplicate_edges(edges);

  std::vector<unsigned long> clusters;
  const auto num_clusters = newman_cluster(edges, clusters);
  std::cout << "Num clusters detected: " << num_clusters << std::endl;
  Clusters plot_clusters;
  for (long i = 0; i != inputs.nr(); i++) {
    auto cluser_idx = clusters[i];
    plot_clusters[cluser_idx].first.push_back(inputs(i, 0));
    plot_clusters[cluser_idx].second.push_back(inputs(i, 1));
  }

  PlotClusters(plot_clusters, "Graph Newman clustering",
               name + "-graph-newman.png");
}

template <typename I>
void DoKMeansClustering(const I& inputs,
                        size_t num_clusters,
                        const std::string& name) {
  typedef matrix<double, 2, 1> sample_type;
  typedef radial_basis_kernel<sample_type> kernel_type;
  kcentroid<kernel_type> kc(kernel_type(0.1), 0.01, 8);
  kkmeans<kernel_type> kmeans(kc);

  std::vector<sample_type> samples;
  samples.reserve(inputs.nr());
  for (long i = 0; i != inputs.nr(); i++) {
    samples.push_back(dlib::trans(dlib::subm(inputs, i, 0, 1, 2)));
  }

  std::vector<sample_type> initial_centers;
  pick_initial_centers(num_clusters, initial_centers, samples,
                       kmeans.get_kernel());

  kmeans.set_number_of_centers(num_clusters);
  kmeans.train(samples, initial_centers);

  std::vector<unsigned long> clusters;
  Clusters plot_clusters;
  for (long i = 0; i != inputs.nr(); i++) {
    auto cluser_idx = kmeans(samples[i]);
    plot_clusters[cluser_idx].first.push_back(inputs(i, 0));
    plot_clusters[cluser_idx].second.push_back(inputs(i, 1));
  }

  PlotClusters(plot_clusters, "K-Means", name + "-kmeans.png");
}

template <typename T>
struct knn_kernel {
  knn_kernel(const std::vector<T>& samples, unsigned long k)
      : samples_(&samples) {
    find_k_nearest_neighbors(
        samples, [](const T& a, const T& b) { return dlib::length(a - b); }, k,
        edges_);
    std::sort(edges_.begin(), edges_.end(), order_by_index<sample_pair>);
  }
  DataType operator()(const T& a, const T& b) const {
    auto idx1 = std::distance(samples_->begin(),
                              std::find(samples_->begin(), samples_->end(), a));
    auto idx2 = std::distance(samples_->begin(),
                              std::find(samples_->begin(), samples_->end(), b));
    sample_pair value{idx1, idx2};
    auto edge = std::lower_bound(edges_.begin(), edges_.end(), value,
                                 order_by_index<sample_pair>);
    if (edge != edges_.end() && !order_by_index<sample_pair>(value, *edge))
      return 1;
    else
      return 0;
  }
  std::vector<sample_pair> edges_;
  const std::vector<T>* samples_;
};

template <typename I>
void DoSpectralClustering(const I& inputs,
                          size_t num_clusters,
                          const std::string& name) {
  typedef matrix<double, 2, 1> sample_type;
  typedef knn_kernel<sample_type> kernel_type;

  std::vector<sample_type> samples;
  samples.reserve(inputs.nr());
  for (long i = 0; i != inputs.nr(); i++) {
    samples.push_back(dlib::trans(dlib::subm(inputs, i, 0, 1, 2)));
  }

  std::vector<unsigned long> clusters =
      spectral_cluster(kernel_type(samples, 15), samples, num_clusters);

  Clusters plot_clusters;
  for (long i = 0; i != inputs.nr(); i++) {
    auto cluser_idx = clusters[i];
    plot_clusters[cluser_idx].first.push_back(inputs(i, 0));
    plot_clusters[cluser_idx].second.push_back(inputs(i, 1));
  }

  PlotClusters(plot_clusters, "Spectral clustering", name + "-spectral.png");
}

int main(int argc, char** argv) {
  if (argc > 1) {
    auto base_dir = fs::path(argv[1]);
    for (auto& dataset : data_names) {
      auto dataset_name = base_dir / dataset;
      if (fs::exists(dataset_name)) {
        std::ifstream file(dataset_name);
        matrix<DataType> data;
        file >> data;

        auto inputs = dlib::subm(data, 0, 1, data.nr(), 2);
        auto labels = dlib::subm(data, 0, 3, data.nr(), 1);

        auto num_samples = inputs.nr();
        auto num_features = inputs.nc();
        std::size_t num_clusters =
            std::set<double>(labels.begin(), labels.end()).size();
        if (num_clusters < 2)
          num_clusters = 3;

        std::cout << dataset << "\n"
                  << "Num samples: " << num_samples
                  << " num features: " << num_features
                  << " num clusters: " << num_clusters << std::endl;

        // DoHierarhicalClustering(inputs, num_clusters, dataset);
        // DoGraphClustering(inputs, dataset);
        // DoKMeansClustering(inputs, num_clusters, dataset);
        // DoGraphNewmanClustering(inputs, dataset);
        DoSpectralClustering(inputs, num_clusters, dataset);
      } else {
        std::cerr << "Dataset file " << dataset_name << " missed\n";
      }
    }
  } else {
    std::cerr << "Please provider path to the datasets folder\n";
  }

  return 0;
}


/*********************************************************************
*  Copyright (c) 2017 Robert Bosch GmbH.
*  All rights reserved.
*
*  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 EKF_HPP
#define EKF_HPP

#include <iostream>

#include <Eigen/Dense>

#include "steering_functions/steering_functions.hpp"
#include "steering_functions/utilities/utilities.hpp"

using namespace std;
using namespace steer;

typedef Eigen::Matrix<double, 2, 2> Matrix2d;
typedef Eigen::Matrix<double, 3, 3> Matrix3d;
typedef Eigen::Matrix<double, 3, 2> Matrix32d;
typedef Eigen::Matrix<double, 2, 3> Matrix23d;

class EKF
{
public:
  /** Constructor */
  EKF();

  /** \brief Sets the parameters required by the EKF */
  void set_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise,
                      const Controller &_controller);

  /** \brief Converts a covariance given by a double array to an Eigen matrix */
  Matrix3d covariance_to_eigen(const double covariance[16]) const;

  /** \brief Converts a covariance given by an Eigen matrix to a double array */
  void eigen_to_covariance(const Matrix3d &covariance_eigen, double covariance[16]) const;

  /** \brief Computes the Jacobians of the motion equations with respect to the state and control */
  void get_motion_jacobi(const State &state, const Control &control, double integration_step, Matrix3d &F_x,
                         Matrix32d &F_u) const;

  /** \brief Computes the Jacobian of the observation equations with respect to the state */
  Matrix3d get_observation_jacobi() const;

  /** \brief Returns the motion covariance in control space */
  Matrix2d get_motion_covariance(const State &state, const Control &control, double integration_step) const;

  /** \brief Returns the observation covariance */
  Matrix3d get_observation_covariance() const;

  /** \brief Returns the gain of the controller */
  Matrix23d get_controller_gain(const Control &control) const;

  /** \brief Returns the rotation matrix from a global frame to a local frame */
  Matrix3d get_rotation_matrix(double angle) const;

  /** \brief Predicts the covariances based on the paper:
      Rapidly-exploring random belief trees for motion planning under uncertainty, A. Bry and N. Roy, IEEE ICRA 2011 */
  void predict(const State_With_Covariance &state, const Control &control, double integration_step,
               State_With_Covariance &state_pred) const;

  /** \brief Predicts the covariances */
  void update(const State_With_Covariance &state_pred, State_With_Covariance &state_corr) const;

  /** \brief Overload operator new for fixed-size vectorizable Eigen member variable */
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

private:
  /** \brief Motion noise */
  Motion_Noise motion_noise_;

  /** \brief Measurement noise */
  Measurement_Noise measurement_noise_;

  /** \brief Feedback controller */
  Controller controller_;

  /** \brief Identity matrix */
  Matrix3d I_;
};

#endif


#include <iostream>
#include <lwr/lwr.h>
#include <lwr/lwr_flann.h>
#include <armadillo>
#include <boost/lexical_cast.hpp>
#include <flann/io/hdf5.h>
#include <iostream>

void simple_ann_test(){
    int nn = 3;

    Matrix<float> dataset;
    Matrix<float> query;
    load_from_file(dataset, "dataset.hdf5","dataset");
    load_from_file(query, "dataset.hdf5","query");

    Matrix<int> indices(new int[query.rows*nn], query.rows, nn);
    Matrix<float> dists(new float[query.rows*nn], query.rows, nn);

    std::cout<< "dataset: (" << dataset.rows << " x " << dataset.cols << ")" << std::endl;
    std::cout<< "query:   (" << query.rows << " x " << query.cols << ")" << std::endl;
    std::cout<< "indices: (" << indices.rows << " x " << indices.cols << ")" << std::endl;

    // construct an randomized kd-tree index using 4 kd-trees
    Index<L2<float> > index(dataset, flann::KDTreeIndexParams(4));
    index.buildIndex();

    // do a knn search, using 128 checks
    index.knnSearch(query, indices, dists, nn, flann::SearchParams(128));

    flann::save_to_file(indices,"result.hdf5","result");

    delete[] dataset.ptr();
    delete[] query.ptr();
    delete[] indices.ptr();
    delete[] dists.ptr();
}

void simple_ann_test2(){

    int nn = 3;

    Matrix<double> dataset;
    Matrix<double> query;
    load_from_file(dataset, "dataset.hdf5","dataset");
    load_from_file(query, "dataset.hdf5","query");

    Matrix<int> indices(new int[query.rows*nn], query.rows, nn);
    Matrix<double> dists(new double[query.rows*nn], query.rows, nn);

    std::cout<< "dataset: (" << dataset.rows << " x " << dataset.cols << ")" << std::endl;
    std::cout<< "query:   (" << query.rows << " x " << query.cols << ")" << std::endl;
    std::cout<< "indices: (" << indices.rows << " x " << indices.cols << ")" << std::endl;

    // construct an randomized kd-tree index using 4 kd-trees
    Index<L2<double> > index(dataset, flann::KDTreeIndexParams(4));
    index.buildIndex();

    // do a knn search, using 128 checks
    index.knnSearch(query, indices, dists, nn, flann::SearchParams(128));

    flann::save_to_file(indices,"result.hdf5","result");

    delete[] dataset.ptr();
    delete[] query.ptr();
    delete[] indices.ptr();
    delete[] dists.ptr();

}

void simple_ann_test3(){

    int nn = 3;

    int dim        = 3;
    int numSamples = 9000;
    int numQuery   = 1000;
    arma::mat    X(dim,numSamples,arma::fill::randu);
    arma::mat    Xq(dim,numQuery,arma::fill::randu);
    X   = X * 100;
    Xq  = Xq * 100;

    double *ptr_X     = X.memptr();
    double *ptr_query = Xq.memptr();

    Matrix<double> dataset(ptr_X,X.n_cols,X.n_rows);
    Matrix<double> query(ptr_query,Xq.n_cols,Xq.n_rows);
    Matrix<int>     indices(new int[query.rows*nn], query.rows, nn);
    Matrix<double>  dists(new double[query.rows*nn], query.rows, nn);

    std::cout<< "dataset: (" << dataset.rows << " x " << dataset.cols << ")" << std::endl;
    std::cout<< "query:   (" << query.rows << " x " << query.cols << ")" << std::endl;
    std::cout<< "indices: (" << indices.rows << " x " << indices.cols << ")" << std::endl;

    // construct an randomized kd-tree index using 4 kd-trees
    Index<L2<double> > index(dataset, flann::KDTreeIndexParams(4));
    index.buildIndex();

    // do a knn search, using 128 checks
    index.knnSearch(query, indices, dists, nn, flann::SearchParams(128));

    std::cout<< "after KNN" << std::endl;

   // delete[] dataset.ptr();
  //  delete[] query.ptr();
  //  delete[] indices.ptr();
  //  delete[] dists.ptr();

}

void my_test(){

    int dim        = 2;
    int numSamples = 1000;
    int numQuery   = 10;
    arma::mat    X(dim,numSamples,arma::fill::randu);
    arma::mat    Xq(dim,numQuery,arma::fill::randu);
    arma::colvec  y(numSamples,arma::fill::randu);
    X   = X * 100;
    Xq  = Xq * 100;

    lwr::lwr_options lwr_opts;

    lwr_opts.D.resize(dim);
    for(int i = 0; i < dim;i++){
        lwr_opts.D[i] = 1*1;
    }

    lwr_opts.k_bias  = 1;
    lwr_opts.K       = 3;
    lwr_opts.bUseKDT = true;

    lwr::LWR Lwr(lwr_opts);
    Lwr.mtype = conversion::ROW_M;

    Lwr.print();

    double* ptr_y = new double[numQuery];

    for(std::size_t i=0; i < 100;i++){

        Lwr.set_X(X);
        Lwr.set_Y(y);

        Lwr.f(ptr_y,Xq);
        std::cout<< "i: " << i << std::endl;
        sleep(1);

    }

    //Lwr.lwr_flann.ann(Xq);
    std::cout<< "=== delete Lwr ===" << std::endl;

}

void f_flann_test(){

    int dim        = 2;
    int numSamples = 200;
    int numQuery   = 1;
    arma::mat     X(dim,numSamples,arma::fill::randu);
    arma::mat    Xq(dim,numQuery,arma::fill::randu);
    arma::colvec  y(numSamples,arma::fill::randu);

    X   = X  * 100;
    Xq  = Xq * 100;
    Xq(0,0) = 500;
    Xq(1,0) = 500;

    lwr::lwr_options lwr_opts;

    lwr_opts.D.resize(dim);
    for(int i = 0; i < dim;i++){
        lwr_opts.D[i] = 5*5;
    }

    lwr_opts.k_bias  = 1;
    lwr_opts.y_bias  = 0;
    lwr_opts.K       = 10;
    lwr_opts.bUseKDT = true;

    lwr::LWR Lwr(lwr_opts);
    Lwr.mtype = conversion::ROW_M;

    Lwr.set_X(X);
    Lwr.set_Y(y);

    double* ptr_y = new double[numQuery];

    Lwr.f(ptr_y,Xq);

    Lwr.lwr_flann.print();

}

void arma_test(){

    arma::mat A(3,3);

    A(0,0) = 25.0000; A(0,1) =  25.0000; A(0,2) =  -5.0000;
    A(1,0) = 25.0000; A(1,1) =  25.0000; A(1,2) =  -5.0000;
    A(2,0) = -5.0000; A(2,1) =  -5.0000; A(2,2) =    1.0000;

    A.print("A");

    pinv(A).print("pinv(A)");

}

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

    std::cout<< "=== lwr_run ===" << std::endl;
   /* std::cout<< " test 2  " << std::endl;
    simple_ann_test2();
    std::cout<< " test 3  " << std::endl;*/
   // simple_ann_test3();
//    std::cout<< " my_test  " << std::endl;
 //   my_test();

    my_test();

  return 0;
}