#include "MeshTriangle.h"
#include "triangle_triangle_intersection.h"
#include "find_all_intersecting_pairs_using_AABBTrees.h"
#include "Object.h"
#include "AABBTree.h"
#include "warnings.h"
#include "tictoc.h"
#include "visualize_aabbtree.h"
#include <igl/read_triangle_mesh.h>
#include <Eigen/Core>
#include <iostream>
#include <iomanip> // std::setw
#include <memory> // std::shared_ptr

int main(int argc, char * argv[])
{
  /////////////////////////////////////////////////////////////////////////////
  // TRIANGLE MESH INTERSECTION DETECTION
  /////////////////////////////////////////////////////////////////////////////
  std::cout<<"# Triangle Mesh Intersection Detection"<<std::endl;
  // Read in a triangle mesh
  Eigen::MatrixXd VA;
  Eigen::MatrixXi FA;
  igl::read_triangle_mesh(argc>1?argv[1]:"../data/knight.obj",VA,FA);
  std::cout<<"  |VA| "<<VA.rows()<<"  "<<std::endl;
  std::cout<<"  |FA| "<<FA.rows()<<"  "<<std::endl<<std::endl;
  Eigen::MatrixXd VB;
  Eigen::MatrixXi FB;
  igl::read_triangle_mesh(argc>2?argv[2]:"../data/cheburashka.obj",VB,FB);
  std::cout<<"  |VB| "<<VB.rows()<<"  "<<std::endl;
  std::cout<<"  |FB| "<<FB.rows()<<"  "<<std::endl<<std::endl;

  tic(); // Start the clock!
  std::vector<std::pair<int, int> > bf_pairs;
  // Brute force
  for(int fa = 0;fa<FA.rows();fa++)
  {
    for(int fb = 0;fb<FB.rows();fb++)
    {
      if(triangle_triangle_intersection(
          VA.row(FA(fa,0)),
          VA.row(FA(fa,1)),
          VA.row(FA(fa,2)),
          VB.row(FB(fb,0)),
          VB.row(FB(fb,1)),
          VB.row(FB(fb,2))))
      {
        bf_pairs.emplace_back(fa,fb);
      }
    }
  }
  std::cout<<"  | Method      | Time in seconds |"<<std::endl;
  std::cout<<"  |:------------|----------------:|"<<std::endl;
  std::cout<<"  | brute force | " << FLOAT15 << toc() << " |"<<std::endl;

  // Build trees for each
  tic();
  std::vector<std::pair<int, int> > tree_pairs;
  const auto triangle_tree = [](
      const Eigen::MatrixXd & V,
      const Eigen::MatrixXi & F)->std::shared_ptr<AABBTree>
  {
    // Because we use shared_ptrs it's OK that this list is destroyed
    std::vector<std::shared_ptr<Object> > triangles;
    triangles.reserve(F.rows());
    // Create a box for each triangle
    for(int f = 0;f<F.rows();f++)
    {
      triangles.emplace_back( std::make_shared<MeshTriangle>(V,F,f) );
    }
    return std::make_shared<AABBTree>(triangles);
  };
  std::shared_ptr<AABBTree> rootA = triangle_tree(VA,FA);
  std::shared_ptr<AABBTree> rootB = triangle_tree(VB,FB);
  std::cout<<"  | build trees | " << FLOAT15 << toc() << " |"<<std::endl;

  tic();
  std::vector<std::pair<std::shared_ptr<Object>,std::shared_ptr<Object> > >
    leaf_pairs;
  // Broad phase
  find_all_intersecting_pairs_using_AABBTrees(rootA,rootB,leaf_pairs);
  for(const auto & pair : leaf_pairs)
  {
    std::shared_ptr<MeshTriangle> triangleA =
      std::static_pointer_cast<MeshTriangle>(pair.first);
    std::shared_ptr<MeshTriangle> triangleB =
      std::static_pointer_cast<MeshTriangle>(pair.second);
    const int fa = triangleA->f;
    const int fb = triangleB->f;
    // Narrow phase
    if(triangle_triangle_intersection(
        VA.row(FA(fa,0)),
        VA.row(FA(fa,1)),
        VA.row(FA(fa,2)),
        VB.row(FB(fb,0)),
        VB.row(FB(fb,1)),
        VB.row(FB(fb,2))))
    {
      tree_pairs.emplace_back(fa,fb);
    }
  }
  std::cout<<"  | use trees   | " << FLOAT15 << toc() << " |"<<std::endl;
  std::cout<<std::endl;

  // See if lists match
  diff_and_warn(  bf_pairs,"brute force",tree_pairs,"tree");
  diff_and_warn(tree_pairs,"tree",       bf_pairs,  "brute force");

  visualize_aabbtree(VA,FA,VB,FB,leaf_pairs);
}


#pragma once

#include <array>
#include <Eigen/Core>

#include <Discregrid/common.hpp>

namespace Discregrid
{

enum class NearestEntity
{
	VN0, VN1, VN2, EN0, EN1, EN2, FN
};

Real point_triangle_sqdistance(Vector3r const& point,
	std::array<Vector3r const*, 3> const& triangle,
	Vector3r* nearest_point = nullptr,
	NearestEntity* ne = nullptr);

}


#include <glm/models/links/odds.hpp>

#include <armadillo>

using namespace arma;

odds_link::odds_link()
    : glm_link::glm_link( "odds" )
{
}

vec
odds_link::init_beta(const mat &X, const vec &y) const
{
    vec mu = (y + 0.5) / 2.0;
    vec eta = mu / ( 1.0 - mu );

    /* Sometimes the initialization behaves badly for the odds-additive
     * scale by taking a too large step in the first iteration. Always
     * shrink the initial betas some to address this.
    */
    return pinv( X ) * eta / 4.0;
}

vec
odds_link::mu(const arma::vec &eta) const
{
    return eta / ( 1 + eta );
}

vec
odds_link::eta(const arma::vec &mu) const
{
    return mu / ( 1 - mu );
}

vec
odds_link::mu_eta(const arma::vec &mu) const
{
    return 1 / ( (mu - 1) % (mu - 1) );
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE COMPUTE

#include <boost/test/unit_test.hpp>

#include "api/umo.hpp"

#include <cmath>
#include <stdexcept>
#include <vector>

#define STRINGIFY(x) #x
#define TOSTRING(x) STRINGIFY(x)

using namespace umo;

const double eps = 0.1;

BOOST_AUTO_TEST_CASE(Square1) {
    Model model;
    FloatExpression x = model.floatVar();
    minimize(x*x + (-2)*x + 1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Square2) {
    Model model;
    FloatExpression x = model.floatVar();
    minimize( (x + (-1)) * (x + (-1)) );
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Square3) {
    Model model;
    FloatExpression x = model.floatVar();
    minimize(x*x + 2*x + 1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), -1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Square4) {
    Model model;
    FloatExpression x = model.floatVar();
    minimize( (x + 1) * (x + 1) );
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), -1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LowerBound) {
    Model model;
    FloatExpression x = model.floatVar(2.0, umo::inf());
    minimize(x*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(UpperBound) {
    Model model;
    FloatExpression x = model.floatVar(-umo::inf(), -2.0);
    minimize(x*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), -2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(FixedVar) {
    Model model;
    FloatExpression x = model.floatVar(2.0, 2.0);
    minimize(x*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Exp) {
    Model model;
    FloatExpression x = model.floatVar();
    minimize(umo::exp(x) + (-1)*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Log) {
    Model model;
    FloatExpression x = model.floatVar(1.0e-5, 1.0e5);
    maximize(umo::log(x) + (-1)*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Cos1) {
    Model model;
    FloatExpression x = model.floatVar(0, 1);
    minimize(umo::cos(x));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Cos2) {
    Model model;
    FloatExpression x = model.floatVar(0, 1);
    maximize(umo::cos(x));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Sin1) {
    Model model;
    FloatExpression x = model.floatVar(0, 1);
    minimize(umo::sin(x));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Sin2) {
    Model model;
    FloatExpression x = model.floatVar(0, 1);
    maximize(umo::sin(x));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Tan1) {
    Model model;
    FloatExpression x = model.floatVar(0, 1);
    minimize(umo::tan(x));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Tan2) {
    Model model;
    FloatExpression x = model.floatVar(0, 1);
    maximize(umo::tan(x));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 1, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Bool1) {
    Model model;
    BoolExpression x = model.boolVar();
    minimize(x*x + (-1.6)*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Bool2) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    maximize(x1 - x2 + x3 - x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue());
    BOOST_CHECK(!x2.getValue());
    BOOST_CHECK(x3.getValue());
    BOOST_CHECK(!x4.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Int1) {
    Model model;
    IntExpression x = model.intVar();
    minimize(x*x + (-3.8)*x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(x.getValue(), 2);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Bounds1) {
    Model model;
    BoolExpression b1 = model.boolVar();
    BoolExpression b2 = model.boolVar();
    IntExpression i1 = model.intVar(-9, 9);
    IntExpression i2 = model.intVar(-8, 8);
    IntExpression i3 = model.intVar(-7, 7);
    IntExpression i4 = model.intVar(-6, 6);
    FloatExpression f1 = model.floatVar(-5, 5);
    FloatExpression f2 = model.floatVar(-4, 4);
    FloatExpression f3 = model.floatVar(-3, 3);
    FloatExpression f4 = model.floatVar(-2, 2);
    maximize(i1 + i2 + i3 + i4 + f1 + f2 + f3 + f4 + b1 + b2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(i1.getValue(), 9);
    BOOST_CHECK_EQUAL(i2.getValue(), 8);
    BOOST_CHECK_EQUAL(i3.getValue(), 7);
    BOOST_CHECK_EQUAL(i4.getValue(), 6);
    BOOST_CHECK_CLOSE(f1.getValue(), 5.0, eps);
    BOOST_CHECK_CLOSE(f2.getValue(), 4.0, eps);
    BOOST_CHECK_CLOSE(f3.getValue(), 3.0, eps);
    BOOST_CHECK_CLOSE(f4.getValue(), 2.0, eps);
    BOOST_CHECK(b1.getValue());
    BOOST_CHECK(b2.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Bounds2) {
    Model model;
    BoolExpression b1 = model.boolVar();
    BoolExpression b2 = model.boolVar();
    IntExpression i1 = model.intVar(-9, 9);
    IntExpression i2 = model.intVar(-8, 8);
    IntExpression i3 = model.intVar(-7, 7);
    IntExpression i4 = model.intVar(-6, 6);
    FloatExpression f1 = model.floatVar(-5, 5);
    FloatExpression f2 = model.floatVar(-4, 4);
    FloatExpression f3 = model.floatVar(-3, 3);
    FloatExpression f4 = model.floatVar(-2, 2);
    maximize(i1 + i2 - i3 - i4 + f1 + f2 - f3 - f4 + b1 - b2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(i1.getValue(), 9);
    BOOST_CHECK_EQUAL(i2.getValue(), 8);
    BOOST_CHECK_EQUAL(i3.getValue(), -7);
    BOOST_CHECK_EQUAL(i4.getValue(), -6);
    BOOST_CHECK_CLOSE(f1.getValue(), 5.0, eps);
    BOOST_CHECK_CLOSE(f2.getValue(), 4.0, eps);
    BOOST_CHECK_CLOSE(f3.getValue(), -3.0, eps);
    BOOST_CHECK_CLOSE(f4.getValue(), -2.0, eps);
    BOOST_CHECK(b1.getValue());
    BOOST_CHECK(!b2.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Bounds3) {
    Model model;
    BoolExpression b1 = model.boolVar();
    BoolExpression b2 = model.boolVar();
    IntExpression i1 = model.intVar(-9, 9);
    IntExpression i2 = model.intVar(-8, 8);
    IntExpression i3 = model.intVar(-7, 7);
    IntExpression i4 = model.intVar(-6, 6);
    FloatExpression f1 = model.floatVar(-5, 5);
    FloatExpression f2 = model.floatVar(-4, 4);
    FloatExpression f3 = model.floatVar(-3, 3);
    FloatExpression f4 = model.floatVar(-2, 2);
    minimize(i1 + i2 + i3 + i4 + f1 + f2 + f3 + f4 + b1 + b2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(i1.getValue(), -9);
    BOOST_CHECK_EQUAL(i2.getValue(), -8);
    BOOST_CHECK_EQUAL(i3.getValue(), -7);
    BOOST_CHECK_EQUAL(i4.getValue(), -6);
    BOOST_CHECK_CLOSE(f1.getValue(), -5.0, eps);
    BOOST_CHECK_CLOSE(f2.getValue(), -4.0, eps);
    BOOST_CHECK_CLOSE(f3.getValue(), -3.0, eps);
    BOOST_CHECK_CLOSE(f4.getValue(), -2.0, eps);
    BOOST_CHECK(!b1.getValue());
    BOOST_CHECK(!b2.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Mixed1) {
    Model model;
    BoolExpression x1 = model.boolVar();
    IntExpression x2 = model.intVar(0, 10);
    FloatExpression x3 = model.floatVar(0, 20);
    maximize(x1 + x2 + x3);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue());
    BOOST_CHECK_EQUAL(x2.getValue(), 10);
    BOOST_CHECK_CLOSE(x3.getValue(), 20, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Mixed2) {
    Model model;
    FloatExpression x1 = model.floatVar(0, 20);
    IntExpression x2 = model.intVar(0, 10);
    BoolExpression x3 = model.boolVar();
    maximize(x1 + x2 + x3);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 20, eps);
    BOOST_CHECK_EQUAL(x2.getValue(), 10);
    BOOST_CHECK(x3.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Mixed3) {
    Model model;
    FloatExpression x1 = model.floatVar(0, 20);
    IntExpression x2 = model.intVar(0, 10);
    BoolExpression x3 = model.boolVar();
    FloatExpression x4 = model.floatVar(-5, 40);
    IntExpression x5 = model.intVar(-10, 30);
    BoolExpression x6 = model.boolVar();
    maximize(x1 + x2 + x3 - x4 - x5 -x6);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 20, eps);
    BOOST_CHECK_EQUAL(x2.getValue(), 10);
    BOOST_CHECK(x3.getValue());
    BOOST_CHECK_CLOSE(x4.getValue(), -5, eps);
    BOOST_CHECK_EQUAL(x5.getValue(), -10);
    BOOST_CHECK(!x6.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Empty) {
    Model model;
    BoolExpression x1 = model.boolVar();
    IntExpression x2 = model.intVar();
    FloatExpression x3 = model.floatVar();
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Linear1) {
    Model model;
    FloatExpression x1 = model.floatVar(0.0, umo::unbounded());
    FloatExpression x2 = model.floatVar(0.0, umo::unbounded());
    maximize(x1);
    linearConstraint(umo::unbounded(), 1.0, {x1, x2});
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 1.0, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 0.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Linear2) {
    Model model;
    FloatExpression x1 = model.floatVar(0.0, umo::unbounded());
    FloatExpression x2 = model.floatVar(0.0, umo::unbounded());
    maximize(x2);
    linearConstraint(umo::unbounded(), 1.0, {x1, x2});
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 0.0, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 1.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Linear3) {
    Model model;
    FloatExpression x1 = model.floatVar(umo::unbounded(), 1.0);
    FloatExpression x2 = model.floatVar(umo::unbounded(), 1.0);
    maximize(x2);
    linearConstraint(umo::unbounded(), 1.0, {x1, x2});
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 0.0, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 1.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Linear4) {
    Model model;
    FloatExpression x1 = model.floatVar(umo::unbounded(), 1.0);
    FloatExpression x2 = model.floatVar(umo::unbounded(), 1.0);
    maximize(x1 + x2);
    linearConstraint(umo::unbounded(), 1.0, {x1, x2}, {1.0, 2.0});
    linearConstraint(umo::unbounded(), 1.0, {x1, x2}, {2.0, 1.0});
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 1.0/3.0, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 1.0/3.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(Linear5) {
    Model model;
    FloatExpression x1 = model.floatVar();
    FloatExpression x2 = model.floatVar();
    maximize(x2);
    linearConstraint(umo::unbounded(), 1.0, {x1, x2});
    linearConstraint(umo::unbounded(), 0.0, {x1}, {-1.0});
    linearConstraint(umo::unbounded(), 0.0, {x2}, {-1.0});
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 0.0, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 1.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}


#include <ctime>
#include <iostream>
#include <limits>
#include <iomanip>
using namespace std;

#define BOOST_TEST_MAIN
// #define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestMisc
#include <boost/test/unit_test.hpp>

//#include <boost/chrono/system_clocks.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/random.hpp>

#include <fftscarf.h>

int g_nb_tests = 100000;

BOOST_AUTO_TEST_CASE( test_ispow2 )
{
    // Test power of 2 up to 2^30
    int p=2;
    for(int l=1; l<30; ++l, p*=2)
        BOOST_CHECK(fftscarf::isPow2(p));

    // Test non-power of 2
    boost::random::mt19937 rng((const uint32_t)std::time(0));
    boost::random::uniform_int_distribution<int> intrnd(1,std::numeric_limits<int>::max());
    for(int n=0; n<g_nb_tests; ++n){
        p = intrnd(rng);
        while(p%2) p /= 2; // Remove potential powers of 2
        if(p>1)
            BOOST_CHECK(!fftscarf::isPow2(p)); // The reminder should be non-power of 2^a
    }
}

BOOST_AUTO_TEST_CASE( test_ispow235 )
{
    // Test power of 2^a * 3^b * 5^c
    boost::random::mt19937 rng((const uint32_t)std::time(0));
    boost::random::uniform_int_distribution<int> powsrnd(1,30);
    int p;
    for(int n=0; n<g_nb_tests; ++n){
        p = std::pow(2, powsrnd(rng)) * std::pow(3, powsrnd(rng)) * std::pow(5, powsrnd(rng));
        BOOST_CHECK(fftscarf::isPow235(p));
    }

    // Test non-power of 2^a * 3^b * 5^c
    boost::random::uniform_int_distribution<int> intrnd(1,std::numeric_limits<int>::max());
    for(int n=0; n<g_nb_tests; ++n){
        p = intrnd(rng);
        while(p%2==0) p /= 2; // Remove potential powers of 2
        while(p%3==0) p /= 3; // Remove potential powers of 3
        while(p%5==0) p /= 5; // Remove potential powers of 5
        if(p>1)
            BOOST_CHECK(!fftscarf::isPow235(p)); // The reminder should be non-power of 2^a * 3^b * 5^c
    }
}

// Test wrapq -----------------------------------------------------------------

template<typename FloatType>
inline FloatType refwrap(FloatType value){
    return std::arg(std::complex<FloatType>(std::cos(value),std::sin(value)));
}

template<typename ValueType>
void check_wrap(ValueType value){
    long double err = refwrap(value) - fftscarf::wrapq(value);
    if(!(std::abs(err)<100*fftscarf::eps<ValueType>())){
        std::cout << std::setprecision(std::numeric_limits<ValueType>::digits10+2);
        std::cout << "value=" << value << " refwrap=" << refwrap(value) << " wrap=" << fftscarf::wrapq(value) << " err=" << err << " eps=" << fftscarf::eps<ValueType>() << std::endl;
    }
    BOOST_CHECK(std::abs(err)<100*fftscarf::eps<ValueType>());
}

template<typename ValueType>
void check_multi_wrap(){

    std::cout << "Test fftscarf::wrapq for type size: " << sizeof(ValueType) << std::endl;

    boost::mt19937 rnd_engine((uint32_t)std::time(0));
    boost::random::uniform_real_distribution<ValueType> phirnd(0.0, 2*fftscarf::pi); // For random phase

    check_wrap<ValueType>(0.0);
    check_wrap<ValueType>(fftscarf::pi/2);
    check_wrap<ValueType>(fftscarf::pi);
    check_wrap<ValueType>(-fftscarf::pi/2);
    check_wrap<ValueType>(-fftscarf::pi);
    check_wrap<ValueType>(2*fftscarf::pi);
    for(int N=-16; N<=16; ++N)
        check_wrap<ValueType>(phirnd(rnd_engine) + N*2*fftscarf::pi);

    // Check speed
    int Nmax = 1000000;
    volatile ValueType res = 0.0; // volatile to avoid simplification because res is not used
    boost::posix_time::ptime tstart;
    boost::posix_time::ptime tend;

    tstart = boost::posix_time::microsec_clock::local_time();
    for(int N=-Nmax; N<=Nmax; ++N)
        res = fftscarf::wrap(phirnd(rnd_engine) + N*2*fftscarf::pi);
    tend  = boost::posix_time::microsec_clock::local_time();
    long double dur_wrap = (tend-tstart).total_milliseconds();
    std::cout << "wrap time:" << dur_wrap << "ms" << std::endl;

    tstart = boost::posix_time::microsec_clock::local_time();
    for(int N=-Nmax; N<=Nmax; ++N)
        res = fftscarf::wrapq(phirnd(rnd_engine) + N*2*fftscarf::pi);
    tend  = boost::posix_time::microsec_clock::local_time();
    long double dur_wrapq = (tend-tstart).total_milliseconds();
    std::cout << "wrapq time:" << dur_wrapq << "ms" << std::endl;

    long double acc = dur_wrap/dur_wrapq;
    std::cout << "speed up ratio:" << acc << std::endl;
    BOOST_CHECK(acc>2.0); // Should be at least twice faster
}

BOOST_AUTO_TEST_CASE( test_wrap_single )
{
    check_multi_wrap<float>();
}
BOOST_AUTO_TEST_CASE( test_wrap_double )
{
    check_multi_wrap<double>();
}
BOOST_AUTO_TEST_CASE( test_wrap_long_double )
{
    check_multi_wrap<long double>();
}


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

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math{

template<typename Scalar, typename NestedContainer>
void cwise_nested_sum(Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& summed,
		const Eigen::Matrix<NestedContainer, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);

template<typename Scalar, typename NestedContainer>
void cwise_nested_sum(Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& summed,
		const Eigen::Tensor<NestedContainer, 3, Eigen::ColMajor>& field);

template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_square(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);

template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor>
cwise_square(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

template<typename Scalar>
inline
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_abs(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field){
	return field.array().abs();
}

template<typename Scalar>
inline
Eigen::Tensor<Scalar, 3, Eigen::ColMajor>
cwise_abs(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field){
	return field.abs();
}

}  // namespace math


#include "gtest/gtest.h"
#include <Eigen/Dense>
#include <autoppl/mcmc/hmc/hamiltonian.hpp>

namespace ppl {
namespace mcmc {

struct hamiltonian_fixture : ::testing::Test
{
protected:
};

////////////////////////////////////////////////////////////
// hamiltonian TESTS
////////////////////////////////////////////////////////////

TEST_F(hamiltonian_fixture, hamiltonian_sanity)
{
    double potential = -3.41;
    Eigen::VectorXd momentum(5);
    momentum.setZero();
    momentum += Eigen::VectorXd::LinSpaced(momentum.size(), 1, momentum.size());
    double kinetic = -0.5 * momentum.squaredNorm();
    double actual = hamiltonian(potential, kinetic);
    double expected = -3.41 - 0.5 * 55;
    EXPECT_DOUBLE_EQ(actual, expected);
}

} // namespace mcmc
} // namespace ppl


// ----------------------------------------------------------------------------
// FILENAME: tylortest.cpp
//
// DESCRIPTION:
//    This file contians the function that used for save the running time of
//    tylor expansion of polynomial shift with original one
//
// AUTHOR: Xinlong Yi
//
// ----------------------------------------------------------------------------

#include "poly.h"
#include <boost/numeric/interval/utility_fwd.hpp>
#include <chrono>
#include <time.h>

static const int kTYLORDEGREE = 6;
static const int digit = 2; // number of digit after point
static const int digit_control = std::pow(10, digit); // controler of digit
static const double max_root = 1000;
// static const double max_root = std::pow(2, kTYLORDEGREE);

/**
 * Get random double in range min to max
 */
double rand_double(double min, double max) {
  double f = (double)rand() / RAND_MAX;
  f = min + f * (max - min);
  f = std::ceil(f * digit_control) / digit_control;
  return f;
}

/**
 * Replace "x" in polynomial with "x+h"
 * Applied Taylor Expansion to this.
 * p(x+h) = p(h) + p'(h)x + 1/2*p''(h)x^2 ... 1/(n!) * p^n(h)*x^n
 * ref: https://math.stackexchange.com/questions/694565/polynomial-shift
 *
 * @tparam n :Maximum degree of poly
 * @param poly :Polynomial
 * @param h :Number add to x
 * @return :Polynomial that replace x in poly to x+h
 */
template <int n> Poly<n> Tylor(const Poly<n> &poly, interval h) {
  if (h.lower() <= 0.0 && h.upper() >= 0.0)
    return poly;

  Poly<n> ret, tmp(poly);
  ret[0] = tmp.ValueAt(h);
  double divisor(1.0);
  for (int index = 1; index <= poly.get_degree(); ++index) {
    divisor *= index;
    tmp.Derivative_();
    ret[index] = (tmp.ValueAt(h) / divisor);
  }

  ret.set_degree();
  return ret;
}

/**
 * Replace "x" in polynomial with "x+h"
 * With original implementation. Start with x+h, then multiply them
 *
 * @tparam n :Maximum degree of poly
 * @param poly :Polynomial
 * @param h :Number add to x
 * @return :Polynomial that replace x in poly to x+h
 */
template <int n> Poly<n> Original(const Poly<n> &poly, interval h) {
  double tmp[2] = {boost::numeric::median(h), 1};
  Poly<n> tmp_poly(tmp, 2), ret, multiplier(tmp, 2);
  ret[0] = poly[0];

  /* TODO :  */
  for (int i = 1; i <= poly.get_degree(); i++) {
    ret += (poly[i] * multiplier);

    for (int j = multiplier.get_degree(); j >= 0; j--) {
      multiplier[j + 1] = multiplier[j] + h * multiplier[j + 1];
    }
    multiplier[0] *= h;
    multiplier.set_degree(multiplier.get_degree() + 1);
  }

  ret.set_degree(poly.get_degree());

  return ret;
}

int main() {
  srand(time(NULL));

  double *coeffs = new double[kTYLORDEGREE];
  for (size_t i = 0; i < kTYLORDEGREE; i++) {
    coeffs[i] = rand_double(-kTYLORDEGREE, kTYLORDEGREE);
  }

  interval h(rand_double(-max_root, max_root));

  Poly<kTYLORDEGREE> test_poly(coeffs, kTYLORDEGREE);
  std::cout << "orig " << test_poly << std::endl;

  // Tylor
  auto tylor_start = std::chrono::high_resolution_clock::now();
  auto r1 = Tylor(test_poly, h);
  auto tylor_end = std::chrono::high_resolution_clock::now();

  //    Original
  auto ori_start = std::chrono::high_resolution_clock::now();
  auto r2 = Original(test_poly, h);
  auto ori_end = std::chrono::high_resolution_clock::now();

  //     Time
  auto tylor_duration = std::chrono::duration_cast<std::chrono::nanoseconds>(
      tylor_end - tylor_start);
  auto ori_duration =
      std::chrono::duration_cast<std::chrono::nanoseconds>(ori_end - ori_start);

  std::cout << "Tylor expansion method takes " << tylor_duration.count()
            << " ns for " << kTYLORDEGREE - 1 << " degree" << std::endl;

  std::cout << "Original method takes " << ori_duration.count() << " ns for "
            << kTYLORDEGREE - 1 << " degree" << std::endl;

  std::cout << r1 << std::endl;
  std::cout << r2 << std::endl;

  interval a = 1.0, b = 3.0;
  interval tmp = a - b;
  std::cout << boost::numeric::median(tmp) << "[" << boost::numeric::width(tmp)
            << "]" << std::endl;

  return 0;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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 RW_MATH_VectorND_HPP
#define RW_MATH_VectorND_HPP

/**
 * @file VectorND.hpp
 */
#if !defined(SWIG)
#include <rw/common/InputArchive.hpp>
#include <rw/common/OutputArchive.hpp>
#include <rw/common/Serializable.hpp>
#include <rw/core/macros.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A N-Dimensional Vector
     *
     */
    template< size_t N, class T = double > class VectorND : public rw::common::Serializable
    {
      public:
        //! The type of the internal Eigen Vector
        typedef Eigen::Matrix< T, N, 1 > EigenVectorND;

        //! Value type.
        typedef T value_type;

        /**
         * @brief Creates a N-dimensional VectorND
         */
        VectorND ()
        {
            if (N <= 0) {
                RW_THROW ("Vector to small, N must be larger than 0");
            }
            _vec = EigenVectorND (N);
        }

        /**
         * @brief Construct a Vector from N arguments
         * @param args [in] a list of arguments
         */
        template< typename... ARGS > VectorND (T arg0, ARGS... args)
        {
            if (N <= 0u) {
                RW_THROW ("Vector to small, N must be larger than 0");
            }
            size_t i = 1;
            ParamExpansion (i, args...);

            _vec[--i] = arg0;
        }

        /**
         * @brief construct vector from std::vector
         * @param vec [in] the vector to construct from
         */
        VectorND (const std::vector< T >& vec)
        {
            if (N <= 0u) {
                RW_THROW ("Vector to small, N must be larger than 0");
            }
            else if (vec.size () != N) {
                RW_THROW ("Wrong Size vector matrix: N of size:" << N << " and vector of size: "
                                                                 << vec.size () << "given");
            }
            for (size_t i = 0; i < N; i++) {
                _vec[i] = vec[i];
            }
        }

        /**
         * @brief Creates a 3D VectorND from Eigen type.
         *
         * @param v [in] an Eigen vector.
         */
        template< class R > VectorND (const Eigen::MatrixBase< R >& v)
        {
            if (v.cols () != 1 || v.rows () != N)
                RW_THROW ("Unable to initialize VectorND with " << v.rows () << " x " << v.cols ()
                                                                << " matrix");
            _vec = v;
        }

        /**
         * @brief The dimension of the VectorND (i.e. 3).
         * This method is provided to help support generic algorithms using
           size() and operator[].
         */
        size_t size () const { return N; }

        // ###################################################
        // #                 Math Operations                 #
        // ###################################################

        // ########## Eigen Operations

        /**
         * @brief element wise multiplication.
         * @param rhs [in] the vector being multiplied with
         * @return the resulting VectorND
         */
        template< class R > VectorND< N, T > elemMultiply (const Eigen::MatrixBase< R >& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] *= rhs[i];
            }
            return ret;
        }

        /**
         * @brief element wise division.
         * @param lhs [in] vector
         * @param rhs [in] vector
         * @return the resulting VectorND
         */
        template< class R > VectorND< N, T > elemDivide (const Eigen::MatrixBase< R >& rhs) const
        {
            VectorND< N, T > ret;
            for (size_t i = 0; i < N; i++) {
                ret[i] = (*this)[i] / rhs[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > VectorND< N, T > operator- (const Eigen::MatrixBase< R >& rhs) const
        {
            return VectorND< N, T > (_vec - rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend VectorND< N, T > operator- (const Eigen::MatrixBase< R >& lhs,
                                           const VectorND< N, T >& rhs)
        {
            return VectorND< N, T > (lhs - rhs.e ());
        }

        /**
         * @brief Vector addition.
         */
        template< class R > VectorND< N, T > operator+ (const Eigen::MatrixBase< R >& rhs) const
        {
            return VectorND< N, T > (_vec + rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend VectorND< N, T > operator+ (const Eigen::MatrixBase< R >& lhs,
                                           const VectorND< N, T >& rhs)
        {
            return VectorND< N, T > (lhs + rhs.e ());
        }

        // ########## VectorND Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        VectorND< N, T > elemDivide (const VectorND< N, T >& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] /= rhs._vec[i];
            }
            return ret;
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        VectorND< N, T > elemMultiply (const VectorND< N, T >& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] *= rhs._vec[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        VectorND< N, T > operator- (const VectorND< N, T >& rhs) const
        {
            return VectorND< N, T > (_vec - rhs._vec);
        }

        /**
         * @brief Vector addition.
         */
        VectorND< N, T > operator+ (const VectorND< N, T >& rhs) const
        {
            return VectorND< N, T > (_vec + rhs._vec);
        }

        /**
         * @brief Unary minus.
         * @brief negative version
         */
        VectorND< N, T > operator- () const { return VectorND< N, T > (_vec * (-1)); }

        // ########## Scalar Operations

        /**
         * @brief Scalar division.
         * @param rhs [in] the scalar to devide with
         * @return result of devision
         */
        VectorND< N, T > operator/ (T rhs) const { return VectorND< N, T > (_vec / rhs); }

#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar division.
         * @param lhs [in] the scalar to devide with
         * @param rhs [out] the vector beind devided
         * @return result of devision
         */
        friend VectorND< N, T > operator/ (T lhs, const VectorND< N, T >& rhs)
        {
            VectorND< N, T > ret = rhs;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] = lhs / ret._vec[i];
            }
            return ret;
        }
#endif

        /**
         * @brief Scalar multiplication.
         * @param rhs [in] the scalar to multiply with
         * @return the product
         */
        VectorND< N, T > operator* (T rhs) const { return VectorND< N, T > (_vec * rhs); }

#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication.
         * @param lhs [in] the scalar to multiply with
         * @param rhs [in] the Vector to be multiplied
         * @return the product
         */
        friend VectorND< N, T > operator* (T lhs, const VectorND< N, T >& rhs)
        {
            return VectorND< N, T > (lhs * rhs._vec);
        }
#endif

        /**
         * @brief Scalar subtraction.
         */
        VectorND< N, T > elemSubtract (const T& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] = ret._vec[i] - rhs;
            }
            return ret;
        }

        /**
         * @brief Scalar addition.
         */
        VectorND< N, T > elemAdd (const T& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] = ret._vec[i] + rhs;
            }
            return ret;
        }

        // ########### Math Functions

        /**
         * @brief Returns the Euclidean norm (2-norm) of the VectorND
         * @return the norm
         */
        T norm2 () const { return _vec.norm (); }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the VectorND
         * @return the norm
         */
        T norm1 () const { return _vec.template lpNorm< 1 > (); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the VectorND
         * @return the norm
         */
        T normInf () const { return _vec.template lpNorm< Eigen::Infinity > (); }

        /**
         * @brief calculate the dot product
         * @param vec [in] the vecor to be dotted
         * @return the dot product
         */
        double dot (const VectorND< N, T >& vec) const { return _vec.dot (vec._vec); }

        /**
         * @brief normalize vector to get length 1
         * @return the normalized Vector
         */
        VectorND< N, T > normalize ()
        {
            T length = norm2 ();
            if (length != 0)
                return (*this) / length;
            else
                return VectorND< N, T > ();
        }

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################
#if !defined(SWIG)
        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator() (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator() (size_t i) { return _vec[i]; }

        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator[] (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator[] (size_t i) { return _vec[i]; }
#else
        ARRAYOPERATOR (T);
#endif
        /**
         * @brief Accessor for the internal Eigen VectorND.
         */
        EigenVectorND& e () { return _vec; }

        /**
           @brief Accessor for the internal Eigen VectorND.
         */
        const EigenVectorND& e () const { return _vec; }
#if !defined(SWIG)
        /**
         * @brief Streaming operator.
         * @param out [in/out] the stream to continue
         * @param v [in] the vector to stream
         * @param reference to \b out
         */
        friend std::ostream& operator<< (std::ostream& out, const VectorND< N, T >& v)
        {
            out << "Vector" << N << "D(";
            for (size_t i = 0; i < N - 1; i++) {
                out << v[i] << ", ";
            }
            out << v[N - 1] << ")";
            return out;
        }
#else
#define VECTORND(num, type) rw::math::VectorND< num, type >
        TOSTRING (VECTORND (N, T));
#undef VECTORND
#endif

        /**
         * @brief converts the vector to a std:vector
         * @return a std::vector
         */
        std::vector< T > toStdVector () const
        {
            std::vector< T > ret;
            for (size_t i = 0; i < N; i++) {
                ret.push_back (_vec[i]);
            }
            return ret;
        }

        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief Scalar multiplication.
         */
        VectorND< N, T >& operator*= (double s)
        {
            _vec *= s;
            return *this;
        }

        /**
         * @brief Scalar division.
         */
        VectorND< N, T >& operator/= (double s)
        {
            _vec /= s;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        VectorND< N, T >& operator+= (const VectorND< N, T >& v)
        {
            _vec += v._vec;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        VectorND< N, T >& operator-= (const VectorND< N, T >& v)
        {
            _vec -= v._vec;
            return *this;
        }

        /**
         * @brief copy a vector from eigen type
         * @param r [in] an Eigen Vector
         */
        template< class R > VectorND< N, T >& operator= (const Eigen::MatrixBase< R >& r)
        {
            _vec = r;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        template< class R > VectorND< N, T >& operator+= (const Eigen::MatrixBase< R >& r)
        {
            _vec += r;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > VectorND< N, T >& operator-= (const Eigen::MatrixBase< R >& r)
        {
            _vec -= r;
            return *this;
        }

        // ###################################################
        // #                    Comparetors                  #
        // ###################################################
        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R > bool operator== (const Eigen::MatrixBase< R >& rhs) const
        {
            return this->_vec == rhs;
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R >
        friend bool operator== (const Eigen::MatrixBase< R >& lhs, const VectorND< N, T >& rhs)
        {
            return lhs == rhs._vec;
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param ths [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R > bool operator!= (const Eigen::MatrixBase< R >& rhs) const
        {
            return !(*this == rhs);
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R >
        friend bool operator!= (const Eigen::MatrixBase< R >& lhs, const VectorND< N, T >& rhs)
        {
            return !(lhs == rhs);
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const VectorND< N, T >& rhs) const { return this->_vec == rhs._vec; }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param rhs [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        bool operator!= (const VectorND< N, T >& rhs) const { return !(*this == rhs); }

        // ###################################################
        // #                      OTHER                      #
        // ###################################################
#if !defined(SWIG)
        //! @copydoc rw::common::Serializable::write
        void write (rw::common::OutputArchive& oarchive, const std::string& id) const
        {
            oarchive.write (this->toStdVector (), id, "VectorND");
        }

        //! @copydoc rw::common::Serializable::read
        void read (rw::common::InputArchive& iarchive, const std::string& id)
        {
            std::vector< T > result (N, 0);
            iarchive.read (result, id, "VectorND");
            *this = VectorND< N, T > (result);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVectorND () const { return this->e (); }

        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVectorND& () { return this->e (); }
#endif
        /**
         * @brief Get zero-initialized vector.
         * @return vector.
         */
        static VectorND< N, T > zero () { return Eigen::Matrix< T, N, 1 >::Zero (); }

      private:
        void ParamExpansion (size_t& i)
        {
            if (i > N) {
                RW_THROW ("Vector to big, argc(" << i << ") != N(" << N << ")");
            }
            else if (i < N) {
                RW_THROW ("Vector to small, argc(" << i << ") != N(" << N << ")");
            }
        }

        template< typename R > void ParamExpansion (size_t& i, R arg)
        {
            ParamExpansion (++i);
            _vec[--i] = T (arg);
        }

        template< typename R, typename... ARGS >
        void ParamExpansion (size_t& i, R arg, ARGS... args)
        {
            ParamExpansion (++i, args...);

            _vec[--i] = T (arg);
        }

        EigenVectorND _vec;
    };

    /**
     * @brief Calculates the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D VectorND cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    const VectorND< ND, T > cross (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2)
    {
        return v1.e ().cross (v2.e ());
        // return cross(v1.e(),v2.e());
    }

    /**
     * @brief Calculates the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param dst [out] the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D VectorND cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    void cross (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2, VectorND< ND, T >& dst)
    {
        dst = v1.e ().cross (v2.e ());
        // dst = cross(v1.m(),v2.m());
    }

    /**
     * @brief Calculates the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T > T dot (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2)
    {
        return v1.e ().dot (v2.e ());
    }

    /**
     * @brief Returns the normalized VectorND \f$\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} \f$.
     * In case \f$ \|mathbf{v}\| = 0\f$ the zero VectorND is returned.
     * @param v [in] \f$ \mathbf{v} \f$ which should be normalized
     * @return the normalized VectorND \f$ \mathbf{n} \f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T > const VectorND< ND, T > normalize (const VectorND< ND, T >& v)
    {
        // Create a copy
        VectorND< ND, T > res (v);
        res.e ().normalize ();
        return res;
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$ with n
     * determining the sign.
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param n [in] @f$ \mathbf{n} @f$
     *
     * @return the angle
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    double angle (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2,
                  const VectorND< ND, T >& n)
    {
        const VectorND< ND, T > nv1 = normalize (v1);
        const VectorND< ND, T > nv2 = normalize (v2);
        const VectorND< ND, T > nn  = normalize (n);
        return atan2 (dot (nn, cross (nv1, nv2)), dot (nv1, nv2));
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the angle
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    double angle (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2)
    {
        VectorND< ND, T > n = cross (v1, v2);
        return angle (v1, v2, n);
    }

    /**
     * @brief Casts VectorND<N,T> to VectorND<Q>
     * @param v [in] VectorND with type T
     * @return VectorND with type Q
     *
     * @relates VectorND
     */
    template< class Q, size_t ND, class T >
    const VectorND< ND, Q > cast (const VectorND< ND, T >& v)
    {
        VectorND< ND, Q > ret;

        for (size_t i = 0; i < v.size (); i++) {
            ret[i] = static_cast< Q > (v[i]);
        }
        return ret;
    }

    template< class T > using Vector6D = VectorND< 6, T >;

#if !defined(SWIG)
    extern template class rw::math::VectorND< 6, double >;
    extern template class rw::math::VectorND< 6, float >;
    extern template class rw::math::VectorND< 5, double >;
    extern template class rw::math::VectorND< 5, float >;
    extern template class rw::math::VectorND< 4, double >;
    extern template class rw::math::VectorND< 4, float >;
    extern template class rw::math::VectorND< 3, double >;
    extern template class rw::math::VectorND< 3, float >;
    extern template class rw::math::VectorND< 2, double >;
    extern template class rw::math::VectorND< 2, float >;
#else
#define VECTORND(num, type) rw::math::VectorND< num, type >;
    SWIG_DECLARE_TEMPLATE (Vector6Dd, VECTORND (6, double));
    SWIG_DECLARE_TEMPLATE (Vector6Df, VECTORND (6, float));
    SWIG_DECLARE_TEMPLATE (Vector5Dd, VECTORND (5, double));
    SWIG_DECLARE_TEMPLATE (Vector5Df, VECTORND (5, float));
    SWIG_DECLARE_TEMPLATE (Vector4Dd, VECTORND (4, double));
    SWIG_DECLARE_TEMPLATE (Vector4Df, VECTORND (4, float));
#undef VECTORND
#endif

    /**@}*/
}}    // namespace rw::math

#endif    // end include guard


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

/**
 * @file Optimal control problem on SE2.
 */

#ifndef OCP_SE2_HPP_
#define OCP_SE2_HPP_

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <smooth/bundle.hpp>
#include <smooth/derivatives.hpp>
#include <smooth/feedback/ocp.hpp>
#include <smooth/feedback/utils/sparse.hpp>
#include <smooth/se2.hpp>

template<typename T>
using X = smooth::Bundle<smooth::SE2<T>, Eigen::Vector2<T>>;

template<typename T>
using U = Eigen::Vector2<T>;

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

/// @brief Objective function
struct SE2Theta
{
  template<typename T>
  T operator()(T tf, const X<T> &, const X<T> &, const Vec<T, 1> & q) const
  {
    return tf + q.x();
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(1, 12);
    ret.coeffRef(0, 0)  = 1;
    ret.coeffRef(0, 11) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(12, 12);
    return ret;
  }
};

/// @brief Dynamics
struct SE2Dyn
{
  template<typename T>
  smooth::Tangent<X<T>> operator()(T, const X<T> & x, const U<T> & u) const
  {
    smooth::Tangent<X<T>> ret;
    ret(0) = x.template part<1>().x();
    ret(1) = T(0);
    ret(2) = x.template part<1>().y();
    ret(3) = u.x();
    ret(4) = u.y();
    return ret;
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(5, 8);
    ret.coeffRef(0, 4) = 1;
    ret.coeffRef(2, 5) = 1;
    ret.coeffRef(3, 6) = 1;
    ret.coeffRef(4, 7) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(8, 5 * 8);
    return ret;
  }
};

/// @brief Target trajectory
const auto xdes = []<typename T>(T t) -> X<T> {
  const Eigen::Vector3<T> vel{1., 0., 0.5};

  X<T> ret;
  ret.template part<0>()     = smooth::SE2<T>::exp(t * vel);
  ret.template part<1>().x() = vel.x();
  ret.template part<1>().y() = vel.z();
  return ret;
};

/// @brief Integrals
struct SE2Integral
{
  template<typename T>
  Vec<T, 1> operator()(T t, const X<T> & x, const U<T> & u) const
  {
    return 0.5 * Vec<T, 1>{(x - xdes(t)).squaredNorm() + u.squaredNorm()};
  }

  Eigen::SparseMatrix<double> jacobian(double t, const X<double> & x, const U<double> & u) const
  {
    const auto a = x - xdes(t);

    Eigen::SparseMatrix<double> ret(1, 8);
    ret.coeffRef(0, 0) = -(a.transpose() * smooth::dl_expinv<X<double>>(a))
                            .dot(Eigen::Vector<double, 5>{1., 0., 0.5, 0, 0});
    smooth::feedback::block_add(ret, 0, 1, smooth::dr_rminus_squarednorm<X<double>>(a));
    ret.coeffRef(0, 6) = u.x();
    ret.coeffRef(0, 7) = u.y();
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double t, const X<double> & x, const U<double> &) const
  {
    const auto H = smooth::d2r_rminus_squarednorm<X<double>>(x - xdes(t));

    Eigen::SparseMatrix<double> ret(8, 8);
    /// @todo don't have derivatives w.r.t. t
    smooth::feedback::block_add(ret, 1, 1, H);
    ret.coeffRef(6, 6) = 1;
    ret.coeffRef(7, 7) = 1;
    return ret;
  }
};

/// @brief Running constraints
struct SE2Cr
{
  template<typename T>
  Vec<T, 2> operator()(T, const X<T> &, const U<T> & u) const
  {
    return u;
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(2, 8);
    ret.coeffRef(0, 6) = 1;
    ret.coeffRef(1, 7) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(8, 2 * 8);
    return ret;
  }
};

/// @brief End constraints
struct SE2Ce
{
  template<typename T>
  Vec<T, 6> operator()(T tf, const X<T> & x0, const X<T> &, const Vec<T, 1> &) const
  {
    Vec<T, 6> ret;
    ret << tf, x0.log();
    return ret;
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> & x0, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(6, 12);
    ret.coeffRef(0, 0) = 1;
    smooth::feedback::block_add(ret, 1, 1, smooth::dr_expinv<X<double>>(x0.log()));
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> & x0, const X<double> &, const Vec<double, 1> &) const
  {
    const auto d2_logx0 = smooth::d2r_rminus<X<double>>(x0.log());

    Eigen::SparseMatrix<double> ret(12, 6 * 12);
    for (auto i = 0u; i < 5; ++i) {
      smooth::feedback::block_add(ret, 1, 12 * (1 + i) + 1, d2_logx0.block(0, i * 5, 5, 5));
    }

    return ret;
  }
};

using OcpSE2 =
  smooth::feedback::OCP<X<double>, U<double>, SE2Theta, SE2Dyn, SE2Integral, SE2Cr, SE2Ce>;

inline const OcpSE2 ocp_se2{
  .theta = SE2Theta{},
  .f     = SE2Dyn{},
  .g     = SE2Integral{},
  .cr    = SE2Cr{},
  .crl   = Vec<double, 2>{{-1, -1}},
  .cru   = Vec<double, 2>{{1, 1}},
  .ce    = SE2Ce{},
  .cel   = Vec<double, 6>{{5, 0, 0, 0, 1, 0}},
  .ceu   = Vec<double, 6>{{5, 0, 0, 0, 1, 0}},
};

#endif  // OCP_SE2_HPP_


#include "q_matrix_tools.h"
#include "rhab/basic_iteration.h"
#include <iomanip>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

// Boost Algorithm
#include <boost/algorithm/minmax_element.hpp>

extern "C"
{
#include <cblas.h>
}

#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multimin.h>

typedef boost::tuple<const std::vector<size_t>*,
                     const std::vector<double>*,
                     const std::vector<double>*
                     > params_t;

/**
 * Apply symmetry condition.
 *
 * We can only use pairs of T(i->j) and T(j->i) for the equation system.
 */
void apply_symmetry_conditions(matrix_int_t &imat, std::vector<size_t> &coords) {
  for (size_t i = 0; i < imat.size1(); i++) {
    for (size_t j = 0; j < imat.size2(); ++j) {
      if (i==j) {
        continue;
      }
      if (imat(i,j) > 0 && imat(j,i) > 0) {
        coords.push_back(i);
        coords.push_back(j);
        continue;
      }
      imat(i,j) = imat(j,i) = 0;
    }
  }
}

double variance_function(const gsl_vector * x, void * params) {
  params_t* p = (params_t*)params;
  const std::vector<size_t>* coords  = boost::get<0>(*p);
  const std::vector<double>* sigma   = boost::get<1>(*p);
  const std::vector<double>* precalc = boost::get<2>(*p);

  double err = 0;

  for (size_t k = 0; k < coords->size(); k+=2) {
    const size_t &i = (*coords)[k];
    const size_t &j = (*coords)[k+1];

    double xi(1.0), xj(1.0);
    if (i < x->size) {
      xi = gsl_vector_get(x, i);
    }
    if (j < x->size) {
      xj = gsl_vector_get(x, j);
    }

    double tmp = xi - xj + (*precalc)[k/2];
    err += tmp*tmp / (*sigma)[k/2];
  }

  return err;
}

bool rhab::calculate_dos_minimization(matrix_int_t imat, matrix_double_t &dmat, vector_double_t &dos) {
  // Histogram
  vector_int_t hist(imat.size1());

  // coordinates of entries in the matrix stored one after another i1,j1,i2,j2,...
  std::vector<size_t> coords;
  std::vector<double> sigma;
  std::vector<double> precalc;

  apply_symmetry_conditions(imat, coords);

  // if we have too few entries left in the matrix abort
  if (coords.size()/2 < dos.size()) {
    return false;
  }

  normalize_q(imat, dmat);

  for (size_t i = 0; i < imat.size1(); i++) {
    hist[i] = sum(row(imat,i));
  }

  // we keep the state with the highest energy fixed
  // and thus do minimization only on N-1 states
  size_t num_states = dos.size()-1;

  // Precalculate variables
  sigma.resize(coords.size()/2);
  precalc.resize(coords.size()/2);

  for (size_t k = 0; k < coords.size(); k+=2) {
    const size_t &i = coords[k];
    const size_t &j = coords[k+1];

    precalc[k/2] = log(dmat(i,j) / dmat(j,i));
    sigma[k/2]   = 1./imat(i,j) + 1./hist[i] + 1./imat(j,i) + 1./hist[j];
  }

  // setup the minimization function and fminimizer
  gsl_vector *ss, *x;
  gsl_multimin_function minex_func;

  params_t params(&coords, &sigma, &precalc);

  minex_func.n = num_states;
  minex_func.f = variance_function;
  minex_func.params = &params;

  // Starting point
  x = gsl_vector_alloc(num_states);
  gsl_vector_set_all(x,  1.0);

  // Set initial step sizes to 1
  ss = gsl_vector_alloc(num_states);
  gsl_vector_set_all(ss, 1.0);

  gsl_multimin_fminimizer *s = NULL;
  s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex2, num_states);
  gsl_multimin_fminimizer_set(s, &minex_func, x, ss);
  int status;
  size_t iter = 0;
  double size;

  do {
    iter++;
    status = gsl_multimin_fminimizer_iterate(s);

    if (status) {
      break;
    }

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, 1e-6);
    //std::cout << iter << " " << size << " " << status << std::endl;
  } while(status == GSL_CONTINUE /* && iter < 1000000*/);

  std::cout << iter << " " << size;

  for (size_t i = 0; i < s->x->size; i++) {
    dos[i] = gsl_vector_get(s->x, i);
  }
  dos[dos.size()-1] = 1;

  for (size_t i = 0; i < dos.size(); i++) {
    std::cout << " " << dos[i];
  }
  std::cout << std::endl;

  gsl_multimin_fminimizer_free(s);
  gsl_vector_free(x);
  gsl_vector_free(ss);

  return true;
}

bool rhab::calculate_dos_leastsquares(matrix_int_t imat, matrix_double_t &dmat, vector_double_t &dos) {
  vector_int_t hist(imat.size1());

  // coordinates of entries in the matrix stored one after another i1,j1,i2,j2,...
  std::vector<size_t> coords;
  std::vector<size_t> pivot(imat.size1());
  size_t cnt_nnz = 0;

  apply_symmetry_conditions(imat, coords);

  if (coords.size()/2 < dos.size()) {
    return false;
  }

  normalize_q(imat, dmat);

  for (size_t i = 0; i < imat.size1(); i++) {
    hist[cnt_nnz] = sum(row(imat,i));
    if (hist[cnt_nnz] > 0) {
      pivot[i] = cnt_nnz;
      cnt_nnz ++;
    }
  }

  int xn = coords.size()/2;

  gsl_matrix *X, *cov;
  gsl_vector *y, *c;
  double chisq;

  X = gsl_matrix_calloc(xn, cnt_nnz);
  y = gsl_vector_calloc(xn);
  c = gsl_vector_calloc(cnt_nnz);
  cov = gsl_matrix_calloc(cnt_nnz, cnt_nnz);

  for (int k = 0; k < 2*xn; k+=2) {
    const size_t &i = coords[k];
    const size_t &j = coords[k+1];
    gsl_matrix_set(X, k/2, pivot[i],  1.0/sqrt(1./hist[pivot[i]] + 1./hist[pivot[j]] + 1./imat(j,i) + 1./imat(i,j)));
    gsl_matrix_set(X, k/2, pivot[j], -1.0/sqrt(1./hist[pivot[i]] + 1./hist[pivot[j]] + 1./imat(j,i) + 1./imat(i,j)));
    gsl_vector_set(y, k/2, -log( dmat(i,j) / dmat(j,i) )/sqrt(1./hist[pivot[i]] + 1./hist[pivot[j]] + 1./imat(j,i) + 1./imat(i,j)));
  }

  {
    gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc(xn, cnt_nnz);
    gsl_multifit_linear(X,y,c,cov,&chisq, work);
    gsl_multifit_linear_free(work);
  }

  for (size_t i = 0; i < dos.size(); i++) {
    dos[i] = gsl_vector_get(c, pivot[i]);
  }

  gsl_matrix_free(X);
  gsl_vector_free(y);
  gsl_vector_free(c);
  gsl_matrix_free(cov);

  return true;
}

void rhab::normalize_q(const matrix_int_t & Q, matrix_double_t & Qd) {
  using namespace boost::numeric::ublas;
  for (size_t i = 0; i < Q.size1(); i++) {
    double s = sum(row(Q,i));
    if (s == 0) {
      row(Qd,i) *= 0;
    } else {
      row(Qd,i) = row(Q,i)/s;
    }
  }
}

void rhab::normalize(vector_double_t &vec) {
  vec /= sum(vec);
}

void rhab::normalize_from_log(vector_double_t &vec) {
  double sub(0), norm(0);
  std::pair<vector_double_t::iterator, vector_double_t::iterator> mm =
    boost::minmax_element(vec.begin(), vec.end());
  sub = (*(mm.second) + *(mm.first))/2;
  for (size_t i = 0; i < vec.size(); i++) {
    norm += exp(vec[i] - sub);
  }
  for (size_t i = 0; i < vec.size(); i++) {
    vec[i] = exp(vec[i] - sub) / norm;
  }
}

bool rhab::calculate_dos_gth(matrix_double_t & inner_mat, vector_double_t &dos) {
  namespace ublas = boost::numeric::ublas;
  std::size_t inner_rows(inner_mat.size1());
  std::size_t inner_cols(inner_mat.size1());
  // we assume small matrix and try GTH method
  // do GTH LU decomposition
  for (std::size_t i = inner_rows-1; i > 0; --i) {
    double s = ublas::norm_1(ublas::subrange(ublas::row(inner_mat,i), 0, i));
    if (s != 0) {
      inner_mat(i,i) = -s;
      for (std::size_t j = 0; j < i; ++j) {
        inner_mat(j,i) /= s;
      }
    }
    for (std::size_t k = 0; k < i; ++k) {
      for (std::size_t j = 0; j < i; ++j) {
        inner_mat(k,j) += inner_mat(k,i)*inner_mat(i,j);
      }
    }
  }
  // now just do modified eqn. 33 of M. Fenwick - J. Chem. Phys. 125, 144905
  std::fill(dos.begin(), dos.end(), 0.0);//dos.clear();
  dos[0] = 1;
  for (std::size_t i = 1; i < inner_rows; ++i) {
    for (std::size_t j = 0; j < i; ++j) {
      if (inner_mat(j,i) > 0) {
        dos[i] += exp(dos[j] + log(inner_mat(j,i)));
      }
    }
    dos[i] = log(dos[i]);
  }
  for (std::size_t ei = 0; ei < inner_cols; ++ei) {
    dos(ei) = exp(dos(ei));
  }

  // eqn. 32 of M. Fenwick - J. Chem. Phys. 125, 144905
  /*
  for (size_t i = 1; i < inner_rows; ++i) {
    for (size_t j = 0; j < i; ++j) {
      dos[i] += dos[j] * inner_mat(j,i);
    }
  }
  */

  normalize(dos);
  return ( std::count_if(dos.begin(), dos.end(), boost::math::isnan<double>) == 0 );
}

bool rhab::calculate_dos_power(const matrix_double_t &imat, vector_double_t &t1) {
  namespace ublas = boost::numeric::ublas;
  matrix_double_t mat(ublas::trans(imat));
  //vector_double_t t1(mat.size1());
  vector_double_t t2(mat.size1());
  vector_double_t t3(mat.size1());
  std::fill(t1.begin(), t1.end(), 1.0/mat.size1());
  size_t max_iter = 10000;
  double lambda, residual, dist;

  rhab::basic_iteration<vector_double_t::iterator> iter(max_iter, 1e-8);
  do {
    ++iter;
    cblas_dgemv(CblasRowMajor, CblasNoTrans, mat.size1(), mat.size2(), 1, &(mat.data()[0]), mat.size1(), &(t1.data()[0]), 1, 0.0, &(t2.data()[0]), 1);
    //ublas::axpy_prod(mat, t1, t2, true);
    t2 /= ublas::norm_1(t2);
    ++iter;
    cblas_dgemv(CblasRowMajor, CblasNoTrans, mat.size1(), mat.size2(), 1, &(mat.data()[0]), mat.size1(), &(t2.data()[0]), 1, 0.0, &(t1.data()[0]), 1);
    //ublas::axpy_prod(mat, t2, t1, true);
    t1 /= ublas::norm_1(t1);
  } while(!iter.converged(t2.begin(), t2.end(), t1.begin(), dist));

  size_t cnt_zero = std::count_if(t2.begin(), t2.end(), std::bind2nd(std::equal_to<double>(), 0.0));
  bool enought_non_zero   = ( cnt_zero < 0.7 * t2.size() );
  bool all_entries_finite = ( std::count_if(t2.begin(), t2.end(),
        boost::math::isfinite<double>) == (long)t2.size() );
  return ( enought_non_zero && all_entries_finite );
}

double rhab::calculate_error(const vector_double_t &exact,
                             const vector_double_t &dos,
                             error_mat_t* error_per_bin,
                             const size_t& index, bool normalize) {
  if (dos.size() == 0 || exact.size() == 0) {
    std::cerr << "exact or calculated density of states vector has zero length!" << std::endl;
    return -1;
  }

  // vector to calculate the error
  vector_double_t err(dos.size());
  std::fill(err.begin(), err.end(), 0.0);

  double sum = 0.0;
  size_t cnt = 0;
  double norm = 0;

  if (normalize) {
    /*
     * Density of states provided in dos is actually \f$\ln(\Omega)\f$.
     * Find the largest value, then subtract it from dos[i]
     * and sum exp(dos[i] - max) to calculate the norm.
     * Then divide the every exp(dos[i] - max) by the norm
     * and subtract the exact value, i.e. exact[i].
     * Calculate the absolute value of it and divide by exact[i].
     *
     * exact[i] is assumed to be positive
     *
     * @todo: calculate both error in density of states and entropy,
     *        i.e. additionally dos[i]-log(exact[i])/log(exact[i])
     */

    // create a copy
    vector_double_t d(dos);
    // and find median
    vector_double_t::iterator middle = d.begin()+(d.end()-d.begin())/2;
    std::nth_element(d.begin(), middle, d.end());
    double sub  = *middle;

    // calculate the norm
    for (size_t i = 0; i < dos.size(); i++) {
      // Be careful here! The least squares and minimization algorithms
      // tend to calculate very large values for states that have never been
      // visited. Having a very large max value results in the exp(foo-max)
      // to become 0
      if ((exact[i]) > 0) {
        norm += exp(dos[i]-sub);
#ifdef DEBUG
        if (!boost::math::isfinite(norm) || !boost::math::isfinite(dos[i])
            || !boost::math::isfinite(exp(dos[i]-sub))) {
          std::cerr << __FILE__ << ":" << __LINE__ << " "
                    << norm << " " << dos[i] << " " << exp(dos[i]-sub) << std::endl;
        }
#endif
      }
    }

    // yeah, I know that one should not compare doubles by equal
    if (norm == 0) {
      norm = 1;
    }

    for (size_t i = 0; i < dos.size(); i++) {
      // Be careful here and do not devide by 0
      if ((exact[i]) > 0) {
        err[i] = fabs( (exp(dos[i]-sub)/norm - exact[i]) / exact[i] );
        sum += err[i];
        cnt ++;
        (*error_per_bin)(index, i)(err[i]);
#ifdef DEBUG
        if (!boost::math::isfinite(sum) || !boost::math::isfinite(exact[i])
            || !boost::math::isfinite(err[i])) {
          std::cerr << __FILE__ << ":" << __LINE__ << " "
                    << sum << " " << norm << " "
                    << dos[i] <<  " " << exact[i] << std::endl;
        }
#endif
      }
    }
  } else {
    for (size_t i = 0; i < dos.size(); i++) {
      if (exact[i] > 0) {
        norm += dos[i];
      }
    }
    for (size_t i = 0; i < dos.size(); i++) {
      // Be careful here and do not divide by 0
      if (exact[i] > 0) {
        err[i] = fabs( (dos[i]/norm - exact[i]) / exact[i] );
        sum += err[i];
        cnt ++;
        (*error_per_bin)(index, i)(err[i]);
      }
#ifdef DEBUG
      if (!boost::math::isfinite(sum) || !boost::math::isfinite(dos[i])
          || !boost::math::isfinite(exact[i])
          || !boost::math::isfinite(err[i])) {
        std::cerr << __FILE__ << ":" << __LINE__ << " "
                  << sum << " " << dos[i] << " " << exact[i] << std::endl;
      }
#endif
    }
  }

  return (sum / cnt);
}

double rhab::calculate_error_q_matrix(const matrix_double_t &Qex, const matrix_double_t &Q) {
  double value(0.0);
  size_t cnt(0);
  for (size_t i = 0; i < Qex.data().size(); i++) {
    if (Qex.data()[i] > 0) {
      double tmp = fabs(Qex.data()[i] - Q.data()[i])/Qex.data()[i];
      value += tmp;
      cnt ++;
    }
  }
  return value / cnt;
}

boost::tuple<double, double, double, bool, bool, bool, double>
rhab::calculate_error_q(const vector_double_t &exact,
                        const matrix_double_t &Qexact,
                        const matrix_int_t &Q, matrix_double_t &Qd,
                        error_mat_tuple_t error_matrices,
                        vector_double_t &dos_lsq,
                        vector_double_t &dos_gth,
                        vector_double_t &dos_pow,
                        const size_t& index) {
  std::fill(dos_lsq.begin(), dos_lsq.end(), 0.0);
  std::fill(dos_gth.begin(), dos_gth.end(), 0.0);
  std::fill(dos_pow.begin(), dos_pow.end(), 0.0);

  // Least Squares
  bool lq = calculate_dos_leastsquares(Q, Qd, dos_lsq);
  //bool lq = calculate_dos_minimization(Q, Qd, dos);
  double error_lsq = calculate_error(exact, dos_lsq, error_matrices.get<0>(), index, true);
  if (!boost::math::isfinite(error_lsq)) {
    lq = false;
  }

  // Least Squares uses Qd as workspace only, so compute Qd
  normalize_q(Q, Qd);

  // calculate error in Q matrix
  double q_error = 0;
  if (Qexact.size1() > 0) {
    q_error = rhab::calculate_error_q_matrix(Qexact, Qd);
  }

  // GTH method
  bool gth = calculate_dos_gth(Qd, dos_gth);
  double error_gth = calculate_error(exact, dos_gth, error_matrices.get<1>(), index, false);

  // GTH Method modifies Qd, so recompute
  normalize_q(Q, Qd);

  // Power method
  bool pow = calculate_dos_power(Qd, dos_pow);
  double error_pow = calculate_error(exact, dos_pow, error_matrices.get<2>(), index, false);

  return boost::make_tuple(error_lsq, error_gth, error_pow,
                           lq,        gth,       pow,
                           q_error);
}

boost::tuple<double, double, double, bool,  bool,   bool>
rhab::calculate_error_q_lj(const vector_double_t &exact,
                           const matrix_int_t &Q, matrix_double_t &Qd,
                           error_mat_tuple_t error_matrices,
                           vector_double_t &dos_lsq,
                           vector_double_t &dos_gth,
                           vector_double_t &dos_pow,
                           const size_t& index) {
  std::fill(dos_lsq.begin(), dos_lsq.end(), 0.0);
  std::fill(dos_gth.begin(), dos_gth.end(), 0.0);
  std::fill(dos_pow.begin(), dos_pow.end(), 0.0);

  // Least Squares
  bool lq = calculate_dos_leastsquares(Q, Qd, dos_lsq);
  //bool lq = calculate_dos_minimization(Q, Qd, dos_lsq);
  double error_lsq = calculate_error(exact, dos_lsq, error_matrices.get<0>(), index, true);
  if (!boost::math::isfinite(error_lsq)) {
    lq = false;
  }

  // Least Squares uses Qd as workspace only, so compute Qd
  normalize_q(Q, Qd);

  // GTH method
  bool gth = calculate_dos_gth(Qd, dos_gth);
  double error_gth = calculate_error(exact, dos_gth, error_matrices.get<1>(), index, false);

  // GTH Method modifies Qd, so recompute
  normalize_q(Q, Qd);

  // Power method
  bool pow = calculate_dos_power(Qd, dos_pow);
  double error_pow = calculate_error(exact, dos_pow, error_matrices.get<2>(), index, false);

  return boost::make_tuple(error_lsq, error_gth, error_pow,
                           lq,        gth,       pow);
}

/* vim: set ts=2 sw=2 sts=2 tw=0 expandtab :*/


// Copyright (C) 2017-2018 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
// Based on https://github.com/akrzemi1/explicit/blob/master/test/test_explicit.cpp

// <experimental/numerics/v1/numbers/double_wide_types.hpp>

//#define JASEL_CONFIG_CONTRACT_LEVEL_MASK 0x111
#define JASEL_CONFIG_CONTRACT_VIOLATION_THROWS_V 1

#include <iostream>
#include <experimental/numerics/v1/numbers/overflow_detection_arithmetic.hpp>
#include <string>

#include <boost/detail/lightweight_test.hpp>

namespace stdex = std::experimental;
namespace nmx = std::experimental::numerics;

#if __cplusplus >= 201402L

static_assert(nmx::overflow_cvt<short>(1) == std::make_pair(false, short(1)), "error");
static_assert(nmx::overflow_cvt<short>(100000) == std::make_pair(true, short(0)), "error");
static_assert(nmx::check_overflow_cvt<short>(100000) == true, "error");
static_assert(nmx::overflow_add(2,2) == std::make_pair(false, 4), "error");
static_assert(nmx::overflow_sub(2,2) == std::make_pair(false, 0), "error");
static_assert(nmx::overflow_mul(2,2) == std::make_pair(false, 4), "error");
static_assert(nmx::overflow_div(2,2) == std::make_pair(false, 1), "error");

#endif

int main()
{
    {
        int i=0;
        short s;
        BOOST_TEST_EQ(false, nmx::overflow_cvt(&s, i));
        BOOST_TEST_EQ(s, 0);
    }
    {
        int i=100000;
        short s;
        BOOST_TEST_EQ(true, nmx::overflow_cvt(&s, i));
    }
    {
        int i=1;
        int r;
        BOOST_TEST_EQ(false, nmx::overflow_neg(&r, i));
        BOOST_TEST_EQ(r, -1);
    }
    {
        signed char i=-128;
        signed char r;
        BOOST_TEST_EQ(true, nmx::overflow_neg(&r, i));
    }
    {
        unsigned r;
        unsigned x = 1;
        BOOST_TEST_EQ(false, nmx::overflow_lsh(&r, x, 2));
        BOOST_TEST_EQ(r, 4);
    }
    {
        unsigned r;
        unsigned x = 1;
        BOOST_TEST_EQ(true, nmx::overflow_lsh(&r, x, -1));
    }
    {
        unsigned r;
        unsigned x = 1;
        BOOST_TEST_EQ(true, nmx::overflow_lsh(&r, x, std::numeric_limits<unsigned>::digits));
    }
    {
        unsigned r;
        unsigned x = 1;
        unsigned y = 1;
        BOOST_TEST_EQ(false, nmx::overflow_add(&r, x, y));
        BOOST_TEST_EQ(r, 2);
    }
    {
        unsigned char r;
        unsigned char x = 128;
        unsigned char y = 128;
        BOOST_TEST_EQ(true, nmx::overflow_add(&r, x, y));
    }
    {
        std::uint64_t r;
        BOOST_TEST_EQ(true, nmx::overflow_add(&r, std::numeric_limits<std::uint64_t>::max(), std::uint64_t(1)));
    }
    {
        signed char r;
        signed char x = -64;
        signed char y = -65;
        BOOST_TEST_EQ(true, nmx::overflow_add(&r, x, y));
    }
    {
        unsigned r;
        unsigned x = 1;
        unsigned y = 1;
        BOOST_TEST_EQ(false, nmx::overflow_sub(&r, x, y));
        BOOST_TEST_EQ(r, 0);
    }
    {
        unsigned char r;
        unsigned char x = 1;
        unsigned char y = 2;
        BOOST_TEST_EQ(true, nmx::overflow_sub(&r, x, y));
    }
    {
        signed char r;
        signed char x = -64;
        signed char y = 65;
        BOOST_TEST_EQ(true, nmx::overflow_sub(&r, x, y));
    }
    {
        unsigned r;
        unsigned x = 2;
        unsigned y = 3;
        BOOST_TEST_EQ(false, nmx::overflow_mul(&r, x, y));
        BOOST_TEST_EQ(r, 6);
    }
    {
        unsigned char r;
        unsigned char x = 2;
        unsigned char y = 128;
        BOOST_TEST_EQ(true, nmx::overflow_mul(&r, x, y));
    }
    {
        signed char r;
        signed char x = -65;
        signed char y = 2;
        BOOST_TEST_EQ(true, nmx::overflow_mul(&r, x, y));
    }
    {
        unsigned r;
        unsigned x = 4;
        unsigned y = 2;
        BOOST_TEST_EQ(false, nmx::overflow_div(&r, x, y));
        BOOST_TEST_EQ(r, 2);
    }
    {
        unsigned char r;
        unsigned char x = 2;
        unsigned char y = 0;
        BOOST_TEST_EQ(true, nmx::overflow_div(&r, x, y));
    }
    {
        unsigned char r;
        unsigned char x = 2;
        unsigned char y = 0;
        BOOST_TEST_EQ(true, nmx::overflow_div(&r, x, y));
    }
    return boost::report_errors();
}


/**
 * Metrics class definition
 */

#pragma once

#include <Eigen/Dense>

namespace DeepLearningFramework {
/**
 * Metrics class
 *
 * accuracy: count of good predictions / number of predictions
 */
class Metrics {
public:
  Metrics() = delete;
  ~Metrics() = delete;

  /**
   * accuracy static method
   *
   * accuracy: count of good predictions / number of predictions
   *
   * @param[out] accuracy accuracy in range [0.f, 1.f]
   * @param[in] labels one-hot encoded labels in format [N, 2]
   * @param[in] features prediction in format [N, 2]
   */
  static void accuracy(float &accuracy, const Eigen::MatrixXf &labels,
                       const Eigen::MatrixXf &features);
};
}; // namespace DeepLearningFramework


#define BOOST_TEST_MODULE literals
#include <boost/test/included/unit_test.hpp>
#include "exprtest.hpp"

using namespace boost::math::constants;

EXPRTEST(literal1, "1.234"  ,  1.234)
EXPRTEST(literal2, "4.2e2"  ,  420)
EXPRTEST(literal3, "5e-01"  ,  0.5)
EXPRTEST(literal4, "-3"     , -3)
EXPRTEST(literal5, "pi"     ,  pi<double>())
EXPRTEST(literal6, "epsilon",  std::numeric_limits<double>::epsilon())
EXPRTEST(literal7, "phi"    ,  phi<double>())
EXPRTEST(literal8, "e"      ,  e<double>())


/**
*cpp file of assignment 4 Unit-Test
*Authors Alexey Titov and Shir Bentabou
*Version 1.0
**/
//libraries
#include <boost/test/minimal.hpp>
#include "CircularInt.hpp"
using namespace std;
//main
int test_main( int, char *[ ] ) // note the name!
{
    CircularInt hour {1, 12};
    CircularInt tmp=hour;
    //before operator
    BOOST_CHECK( hour.getPos()==1);
    //operator '+='
    hour += 4;
    BOOST_CHECK(hour.getPos()==5);
    //operators '+=' and '++'
    (hour+=2)++;
    BOOST_CHECK(hour.getPos()==8);
    //operator '+='
    hour += 18;
    BOOST_REQUIRE(hour.getPos()==2);
    //operator '-'
    -hour;
    BOOST_REQUIRE(hour.getPos()==10);
    //operators '=' and int '+' CircularInt
    hour= 1-hour;
    BOOST_REQUIRE(hour.getPos()==3);            //BOOST_REQUIRE(hour.getPos()==11);
    //operator CircularInt '+' CircularInt
    tmp=hour+hour;
    if( tmp.getPos()== 3 )
         BOOST_FAIL( "Ouch..." );
	// cout << hour+hour << endl;                // 10 (11 hours after 11)
    hour += 8;
    //operator '*='
    hour *= 2;
    if (!(hour.getPos()== 10))
        BOOST_FAIL( "Ouch..." );
    //operator '/'
    tmp=hour/2;
    if (!(tmp.getPos()==5 || tmp.getPos()==11))  // TWO OPTIONS: 11 (since 11*2=10) or 5 (since 5*2=10 too).
        BOOST_FAIL( "Ouch..." );
	return 0;                                    // #returns error code
}


/* Copyright 2017 The sfcpp Authors. 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.
==============================================================================*/

#pragma once

#include <sfc/CurveSpecification.hpp>

#include <Eigen/Dense>

#include <cstddef>
#include <memory>
#include <vector>

namespace sfcpp {
namespace sfc {

/**
 * This class provides a simpler means to specify curves that are based on
 * k^d-trees. An object of this class can be converted to a CurveSpecification
 * object.
 */
struct KDCurveSpecification {
  // Number of cubes per dimension in the subdivision step
  size_t k;
  // Number of dimensions
  size_t d;
  // A lookup table representing the child state function S^c
  std::vector<std::vector<size_t>> grammar;

  // A vector providing the order of subcubes for each state (subcubes are
  // enumerated
  // row-major), for example, in the 3D Hilbert curve, childOrdering[0] might be
  // {0, 1, 3, 2, 6, 7, 5, 4}.
  std::vector<std::vector<size_t>> childOrdering;

  KDCurveSpecification(size_t k, size_t d, size_t numGrammarElements);

  // KDCurveSpecification(size_t k, size_t d, std::initializer_list<std::string>
  // stringGrammar);

  /**
   * Returns a global model of the specified curve.
   */
  std::shared_ptr<CurveSpecification> getCurveSpecification() const;

  /**
   * Returns a local model of the specified curve. Only works for curves where
   * this is possible!
   */
  std::shared_ptr<CurveSpecification> getLocalCurveSpecification() const;

  static Eigen::MatrixXd generateUnitCube(size_t dim);

  /**
   * @param k number of cubes per "row", must be odd and >= 3.
   */
  static std::shared_ptr<KDCurveSpecification> getPeanoCurveSpecification(
      size_t d, size_t k = 3);

  static std::shared_ptr<KDCurveSpecification> getHilbertCurveSpecification(
      size_t d);

  /**
   * @param k number of cubes per "row", can be anything >= 2.
   * A big value of k can be used to emulate row-major layout.
   */
  static std::shared_ptr<KDCurveSpecification> getMortonCurveSpecification(
      size_t d, size_t k = 2);
};

} /* namespace sfc */
} /* namespace sfcpp */


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_TRIANGULATION_HPP
#define PIC_COMPUTER_VISION_TRIANGULATION_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../image.hpp"

#include "../util/math.hpp"

#include "../util/eigen_util.hpp"

#include "../computer_vision/nelder_mead_opt_triangulation.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief triangulationLonguetHiggins computes triangulation using Longuet-Higgins equations.
 * @param point_0 is the point from the first view that matches point_1
 * @param point_1 is the point from the second view that matches point_0
 * @param R is the rotation matrix between the two views.
 * @param t is the translation matrix between the two views.
 * @return
 */
PIC_INLINE Eigen::Vector3d triangulationLonguetHiggins(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1, Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    Eigen::Vector3d ret;

    Eigen::Vector3d r_0 = Eigen::Vector3d(R(0, 0), R(0, 1), R(0, 2));
    Eigen::Vector3d r_2 = Eigen::Vector3d(R(2, 0), R(2, 1), R(2, 2));

    Eigen::Vector3d tmp = r_0 - point_1[0] * r_2;

    ret[2] = tmp.dot(t) / tmp.dot(point_0);

    ret[0] = point_0[0] * ret[2];
    ret[1] = point_0[1] * ret[2];

    return ret;
}

/**
 * @brief triangulationHartl
 * Sturm
 * @param point_0
 * @param point_1
 * @param R
 * @param t
 * @return
 */
PIC_INLINE Eigen::Vector4d triangulationHartleySturm(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1,
                                          Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, int maxIter = 100)
{
    Eigen::Vector4d M0_row[3], M1_row[3];

    for(int i = 0; i < 3; i++) {
        M0_row[i] = Eigen::Vector4d(M0(i, 0), M0(i, 1), M0(i, 2), M0(i, 3));
        M1_row[i] = Eigen::Vector4d(M1(i, 0), M1(i, 1), M1(i, 2), M1(i, 3));
    }

    Eigen::Vector4d x;
    double weight0 = 1.0;
    double weight0_prev = 1.0;

    double weight1 = 1.0;
    double weight1_prev = 1.0;

    int j = 0;
    while(j < maxIter) {
        Eigen::Vector4d A0 = (M0_row[0] - point_0[0] * M0_row[2]) / weight0;
        Eigen::Vector4d A1 = (M0_row[1] - point_0[1] * M0_row[2]) / weight0;

        Eigen::Vector4d A2 = (M1_row[0] - point_1[0] * M1_row[2]) / weight1;
        Eigen::Vector4d A3 = (M1_row[1] - point_1[1] * M1_row[2]) / weight1;

        Eigen::MatrixXd A(4, 4);
        for(int i = 0; i < 4; i++) {
            A(0, i) = A0[i];
            A(1, i) = A1[i];
            A(2, i) = A2[i];
            A(3, i) = A3[i];
        }

        Eigen::JacobiSVD< Eigen::MatrixXd > svdA(A, Eigen::ComputeFullV);
        Eigen::MatrixXd V = svdA.matrixV();
        int n = int(V.cols()) - 1;

        x[0] = V(0, n);
        x[1] = V(1, n);
        x[2] = V(2, n);
        x[3] = V(3, n);
        x /= x[3];

        weight0_prev = weight0;
        weight1_prev = weight1;

        weight0 = x.dot(M0_row[2]);
        weight1 = x.dot(M1_row[2]);

        double d0 = weight0_prev - weight0;
        double d1 = weight1_prev - weight1;
        double err = sqrt(d0 * d0 + d1 * d1);

        if(err < 1e-12){
            break;
        }

        j++;
    }

    #ifdef PIC_DEBUG
        printf("triangulationHartleySturm's Iterations: %d\n",j);
    #endif

    return x;
}

/**
 * @brief triangulationPoints
 * @param M0
 * @param M1
 * @param m0f
 * @param m1f
 * @param points_3d
 * @param colors
 * @param bColor
 */
PIC_INLINE void triangulationPoints(Eigen::Matrix34d &M0,
                                    Eigen::Matrix34d &M1,
                                    std::vector< Eigen::Vector2f > &m0f,
                                    std::vector< Eigen::Vector2f > &m1f,
                                    std::vector< Eigen::Vector3d > &points_3d,
                                    std::vector< unsigned char > &colors,
                                    Image *img0 = NULL,
                                    Image *img1 = NULL,
                                    bool bColor = false
                                  )
{
    if(m0f.size() != m1f.size()) {
        return;
    }

    NelderMeadOptTriangulation nmTri(M0, M1);
    for(unsigned int i = 0; i < m0f.size(); i++) {
        //normalized coordinates
        Eigen::Vector3d p0 = Eigen::Vector3d(m0f[i][0], m0f[i][1], 1.0);
        Eigen::Vector3d p1 = Eigen::Vector3d(m1f[i][0], m1f[i][1], 1.0);

        //triangulation
        Eigen::Vector4d point = triangulationHartleySturm(p0, p1, M0, M1);

        //non-linear refinement
        nmTri.update(m0f[i], m1f[i]);
        double tmpp[] = {point[0], point[1], point[2]};
        double out[3];
        nmTri.run(tmpp, 3, 1e-9f, 10000, &out[0]);

        //output
        points_3d.push_back(Eigen::Vector3d(out[0], out[1], out[2]));

        if(bColor) {
            float *color0 = (*img0)(int(m0f[i][0]), int(m0f[i][1]));
            float *color1 = (*img1)(int(m1f[i][0]), int(m1f[i][1]));

            for(int j = 0; j < img0->channels; j++) {
                float c_mean = (color0[j] + color1[j]) * 0.5f;
                c_mean = CLAMPi(c_mean, 0.0f, 1.0f);
                unsigned char c = int(c_mean * 255.0f);
                colors.push_back(c);
            }
        }
    }
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_TRIANGULATION_HPP


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

/*
 Copyright (C) 2014 Fabien Le Floc'h

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

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

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

/*! \file analytichestonengine.hpp
    \brief analytic Heston expansion engine
*/

#ifndef quantlib_heston_expansion_engine_hpp
#define quantlib_heston_expansion_engine_hpp

#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/models/equity/hestonmodel.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    //! Heston-model engine for European options based on analytic expansions
    /*! References:

        M Forde, A Jacquier, R Lee, The small-time smile and term
        structure of implied volatility under the Heston model
        SIAM Journal on Financial Mathematics, 2012 - SIAM

        M Lorig, S Pagliarani, A Pascucci, Explicit implied vols for
        multifactor local-stochastic vol models
        arXiv preprint arXiv:1306.5447v3, 2014 - arxiv.org

        \ingroup vanillaengines
    */
    class HestonExpansionEngine
        : public GenericModelEngine<HestonModel,
                                    VanillaOption::arguments,
                                    VanillaOption::results> {
      public:
        enum HestonExpansionFormula { LPP2, LPP3, Forde };

        HestonExpansionEngine(const boost::shared_ptr<HestonModel>& model,
                              HestonExpansionFormula formula);

        void calculate() const;

      private:
        const HestonExpansionFormula formula_;
    };

    /*! Interface to represent some Heston expansion formula.
        During calibration, it would typically be initialized once per
        implied volatility surface slice, then calls for each surface
        strike to impliedVolatility(strike, forward) would be
        performed.
    */
    class HestonExpansion {
      public:
        virtual ~HestonExpansion() {}
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const = 0;
    };

    /*! Lorig Pagliarani Pascucci expansion of order-2 for the Heston model.
        During calibration, it can be initialized once per expiry, and
        called many times with different strikes.  The formula is also
        available in the Mathematica notebook from the authors at
        http://explicitsolutions.wordpress.com/
    */
    class LPP2HestonExpansion : public HestonExpansion {
      public:
        LPP2HestonExpansion(const Real kappa, const Real theta,
                            const Real sigma, const Real v0,
                            const Real rho, const Real term);
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const;
      private:
        Real coeffs[3];
        Real ekt, e2kt, e3kt, e4kt;
        Real z0(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z1(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z2(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
    };

    /*! Lorig Pagliarani Pascucci expansion of order-3 for the Heston model.
        During calibration, it can be initialized once per expiry, and
        called many times with different strikes.  The formula is also
        available in the Mathematica notebook from the authors at
        http://explicitsolutions.wordpress.com/
    */
    class LPP3HestonExpansion : public HestonExpansion{
      public:
        LPP3HestonExpansion(const Real kappa, const Real theta,
                            const Real sigma, const Real v0,
                            const Real rho, const Real term);
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const;
      private:
        Real coeffs[4];
        Real ekt, e2kt, e3kt, e4kt;
        Real z0(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z1(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z2(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
        Real z3(Real t, Real kappa, Real theta,
                Real delta, Real y, Real rho) const;
    };

    /*! Small-time expansion from
        "The small-time smile and term structure of implied volatility
        under the Heston model" M Forde, A Jacquier, R Lee - SIAM
        Journal on Financial Mathematics, 2012 - SIAM
    */
    class FordeHestonExpansion : public HestonExpansion {
      public:
        FordeHestonExpansion(const Real kappa, const Real theta,
                             const Real sigma, const Real v0,
                             const Real rho, const Real term);
        virtual Real impliedVolatility(const Real strike,
                                       const Real forward) const;
      private:
        Real coeffs[5];
    };

}

#endif


#include <algorithm>
#include <iostream>
#include <fstream>
#include <numeric>
#include <vector>
#include <NTL/ZZ.h>

NTL::ZZ mortal_fib(uintmax_t n, uintmax_t m)
{
    std::vector<NTL::ZZ> rabbits(m);
    rabbits.at(0) = 1;
    for (decltype(n) i = 0; i < n - 1; ++i) {
        auto tmp = rabbits.at(0), total = NTL::to_ZZ(0);
        for (decltype(m) j = 1; j < m; ++j) {
            total += rabbits.at(j);
            std::swap(rabbits.at(j), tmp);
        }
        rabbits.at(0) = total;
    }
    return std::accumulate(std::begin(rabbits),
            std::end(rabbits),
            NTL::to_ZZ(0));
}

int main()
{
    std::ifstream f("data/rosalind_fibd.txt");
    uintmax_t n, m;
    f >> n;
    f >> m;
    f.close();
    std::cout << mortal_fib(n, m) << std::endl;
    return EXIT_SUCCESS;
}


#pragma once

#include <boost/math/constants/constants.hpp>
#include <cmath>

#include "GNAObject.hh"

class GaussianPeakWithBackground: public GNAObject,
                                  public TransformationBind<GaussianPeakWithBackground> {
public:
  GaussianPeakWithBackground() {
    variable_(&m_b, "BackgroundRate");
    variable_(&m_mu, "Mu");
    variable_(&m_E0, "E0");
    variable_(&m_w, "Width");

    transformation_("rate")
      .input("E")
      .output("rate")
      .types(Atypes::pass<0,0>)
      .func(&GaussianPeakWithBackground::calcRate)
      ;
  }

  void calcRate(Args args, Rets rets) {
    const double pi = boost::math::constants::pi<double>();
    const auto &E = args[0].arr;
    rets[0].arr = m_b + m_mu*(1./std::sqrt(2*pi*m_w))*(-(E-m_E0).square()/(2*m_w*m_w)).exp();
  }
protected:
  variable<double> m_b, m_mu, m_E0, m_w;
};