// Copyright 2014-2015 SDL plc
// 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.
/** \file

    compute inside, outside weights for states in hg. inside(st)*outside(st) =
    sum of weights of all trees (start/axioms)->st->final

    TODO: speed up inside-then-outside for hg; currently results in examining in
    arcs 3 times and out arcs 2 times; could do better with 1 time each (by not
    using StatesTraversal)

    TODO: also allow cyclic hg
*/

#ifndef HYP__HYPERGRAPH_OUTSIDE_ALGORITHM_HPP
#define HYP__HYPERGRAPH_OUTSIDE_ALGORITHM_HPP
#pragma once

#include <sdl/Hypergraph/IHypergraph.hpp>
#include <sdl/Hypergraph/MutableHypergraph.hpp>
#include <sdl/Hypergraph/StatesTraversal.hpp>
#include <sdl/Hypergraph/Weight.hpp>
#include <sdl/Hypergraph/WeightUtil.hpp>
#include <sdl/Util/Add.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <cassert>
#include <vector>

namespace sdl {
namespace Hypergraph {

/**
   sets sum the outside score (with final as the root) for s, assuming the
   outside scores of all the heads of outarcs of sid are already known (and all
   the inside scores, of course). assumes commutative Weight

   insideScores needs to be generated for all states, not just the nonlexical
   ones (i.e. enable includingLeaves)
*/
template <class Arc>
void outsideFromInside(StateId stateId, IHypergraph<Arc> const& hg,
                       boost::ptr_vector<typename Arc::Weight> const& insideScores,
                       boost::ptr_vector<typename Arc::Weight> const& outsideScores,
                       typename Arc::Weight& sum, StateId final, bool haveInsideForAxiom = true) {
  SDL_TRACE(Hypergraph.OutsideAlgorithm, "Computing outside score for " << stateId << ", sum=" << sum);
  typedef typename Arc::Weight Weight;
  if (stateId == final)
    sum = Weight::one();
  else {
    for (ArcId aid : hg.outArcIds(stateId)) {
      Arc const& arc = *hg.outArc(stateId, aid);
      SDL_TRACE(Hypergraph.OutsideAlgorithm, " Found out arc: " << arc);
      Weight prod(times(arc.weight(), outsideScores[arc.head()]));
      // normally we say that arc weight comes after tails, but outside is only
      // meaningful for commutative anyway (granted, you could instead have a
      // pair for each state: (L, R) where L are the left-outside and R are the
      // right-outside)
      StateIdContainer const& tails = arc.tails();
      unsigned nself = 0;
      for (StateIdContainer::const_iterator i = tails.begin(), e = tails.end(); i != e; ++i) {
        StateId const tail = *i;
        if (tail == stateId && !nself)
          ++nself;
        else if (haveInsideForAxiom || !hg.isAxiom(tail)) {
          assert(tail < insideScores.size());
          timesBy(insideScores[tail], prod);
        }
      }
      SDL_TRACE(Hypergraph.OutsideAlgorithm, " prod=" << prod << " (add #self=" << nself << " times)");
      while (nself--) plusBy(prod, sum);
    }
  }
  SDL_TRACE(Hypergraph.OutsideAlgorithm, " sum now " << sum);
}

/**
   A states visitor that computes the distance to each particular state
   that it's called with
*/
template <class Arc>
struct ComputeOutsideScoreStatesVisitor : public IStatesVisitor {

  typedef typename Arc::Weight Weight;

  ComputeOutsideScoreStatesVisitor(IHypergraph<Arc> const& hg, boost::ptr_vector<Weight> const& insideScores,
                                   boost::ptr_vector<Weight>* outsideScores, bool haveInsideForAxiom)
      : hg_(hg)
      , final(hg.final())
      , insideScores_(insideScores)
      , outsideScores_(outsideScores)
      , kZero(Weight::zero())
      , haveInsideForAxiom_(haveInsideForAxiom) {}

  /**
     Computes the outside score of a particular state.
  */
  void visit(StateId stateId) {
    assert(outsideScores_);
    Weight& outside = Util::atExpandPtr(*outsideScores_, stateId, kZero);
    outsideFromInside(stateId, hg_, insideScores_, *outsideScores_, outside, final, haveInsideForAxiom_);
    SDL_TRACE(Hypergraph.InsideAlgorithm, "Stored outside distance: " << (*outsideScores_)[stateId]
                                                                      << " to state " << stateId);
  }

  Weight const kZero;
  IHypergraph<Arc> const& hg_;
  StateId final;
  boost::ptr_vector<Weight> const& insideScores_;
  boost::ptr_vector<Weight>* outsideScores_;
  bool haveInsideForAxiom_;
};

/**
   Runs the outside algorithm. Assumes there are no cycles.

   \param outsideScore The resulting vector that will be filled

   \param insideScores A vector of inside scores (unused if HG is an
   FSM)
*/
template <class Arc>
void outsideAlgorithm(IHypergraph<Arc> const& hg, boost::ptr_vector<typename Arc::Weight> const& insideScores,
                      boost::ptr_vector<typename Arc::Weight>* outsideScores, bool haveInsideForAxiom = true) {

  // Traverse states in reverse topsorted order (i.e., starting from
  // FINAL root), and compute outsideScore for each state:
  ComputeOutsideScoreStatesVisitor<Arc> outsideScoreComputer(hg, insideScores, outsideScores, haveInsideForAxiom);
  ReverseTopsortStatesTraversal<Arc>(hg, &outsideScoreComputer);
}

}}

#endif


/*
*
* AUTHOR: Milan De Cauwer
*
*/

#include <boost/math/distributions/weibull.hpp>
#include <boost/program_options.hpp> // Reading from the CLI
#include <boost/random.hpp> // Aiming for the weibull dist in particular
#include <boost/random/random_device.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/math/special_functions/round.hpp>
#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <utility>
#include <functional>
#include <sstream>
#include <time.h>
#include <list>
#include <unordered_map>
#include <any>
#include <vector>

// Defining Strings for the id naming convention
#define ID_PRB "1BPP"
#define ID_SEP "_"
#define ID_EXT ".dat"

#define MSG_ALPHA " * Warning : The probability of having an item of size 0 is above "
#define MSG_ZERO  " * A 0 value was produced !"
#define MSG_HZERO " 0 values were produced !"

// Defining programm constants
#define ALPHA 0.0001


/*
 * Profile.cpp
 *
 *  Created on: Apr 16, 2011
 *      Author: tbabb
 */

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

#include <boost/iterator/counting_iterator.hpp>

#include <geomc/function/Dual.h>
#include <geomc/linalg/Vec.h>
#include <geomc/linalg/Matrix.h>
#include <geomc/random/RandomTools.h>
#include <geomc/random/MTRand.h>
#include <geomc/random/LCRand.h>
#include <geomc/function/Path.h>
#include <geomc/function/PerlinNoise.h>
#include <geomc/function/Raster.h>
#include <geomc/function/SphericalHarmonics.h>
#include <geomc/shape/BinLatticePartition.h>
#include <geomc/shape/Trace.h>
#include <geomc/shape/Oriented.h>
#include <geomc/shape/Intersect.h>
#include <geomc/shape/Frustum.h>

#include "RandomBattery.h"

#define NUM_PROFILE_CASES 1024;

using namespace geom;
using namespace std;

template <typename T, index_t N>
void fill_unit_vec_array(typename PointType<T,N>::point_t *dst, index_t n) {
    Sampler<T> rntools = Sampler<T>();

    for (index_t i = 0; i < n; i++) {
        dst[i] = rntools.template unit<N>();
    }
}

template <index_t N> void fill_unit_raw_vec_array(double dst[][N], index_t n) {
    Sampler<double> rntools = Sampler<double>();
    Vec<double,N> v;

    for (index_t i = 0; i < n; i++) {
        v = rntools.template unit<N>();
        for (index_t axis = 0; axis < N; axis++) {
            dst[i][axis] = v[axis];
        }
    }
}

template <typename T, index_t N> void fill_range_vec_array(Vec<T, N> *dst, index_t n, Vec<T,N> lo, Vec<T,N> hi) {
    Sampler<double> rntools = Sampler<double>();

    for (index_t i = 0; i < n; i++) {
        dst[i] = rntools.box(lo, hi);
    }
}

template <typename T, index_t N> void fill_range_vec_array(Vec<Dual<T>, N> *dst, index_t n, Vec<Dual<T>,N> lo, Vec<Dual<T>,N> hi) {
    Sampler< Dual<T> > rntools = Sampler< Dual<T> >();

    for (index_t i = 0; i < n; i++) {
        dst[i] = rntools.box(lo, hi);
    }
}

double profile_vec_cross(index_t iters) {
    const index_t n = NUM_PROFILE_CASES;
    Vec3d v;
    Vec3d *vecs_src = new Vec3d[n];
    Vec3d *vecs_dst = new Vec3d[n];

    fill_unit_vec_array<double,3>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].cross(vecs_src[(idx+1) % n]);
        idx += 1;
    }
    clock_t end = clock();

    delete [] vecs_src;
    delete [] vecs_dst;
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_add(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src_1 = new Vec<double,N>[n];
    Vec<double, N> *vecs_src_2 = new Vec<double,N>[n];
    Vec<double, N> *vecs_dst   = new Vec<double,N>[n];

    fill_unit_vec_array<double,N>(vecs_src_1, n);
    fill_unit_vec_array<double,N>(vecs_src_2, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx] = vecs_src_1[idx] + (vecs_src_2[idx]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();

    delete [] vecs_src_1;
    delete [] vecs_src_2;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_dot(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src_1 = new Vec<double,N>[n];
    Vec<double, N> *vecs_src_2 = new Vec<double,N>[n];
    double *vecs_dst = new double[n];

    fill_unit_vec_array<double,N>(vecs_src_1, n);
    fill_unit_vec_array<double,N>(vecs_src_2, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx] = vecs_src_1[idx].dot(vecs_src_2[idx]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();
    vecs_dst[0] += 1;

    delete [] vecs_src_1;
    delete [] vecs_src_2;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_norm(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src = new Vec<double,N>[n];
    double *vecs_dst = new double[n];

    fill_unit_vec_array<double,N>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].mag();
        idx += 1;
    }
    clock_t end = clock();
    vecs_dst[0] += 1;

    delete [] vecs_src;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_norm2(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src = new Vec<double,N>[n];
    double *vecs_dst = new double[n];

    fill_unit_vec_array<double,N>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].mag2();
        idx += 1;
    }
    clock_t end = clock();
    vecs_dst[0] += 1; //shenanigans to prevent compiler from optimizing to a null loop.

    delete [] vecs_src;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_vec_hash(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    Vec<double, N> *vecs_src = new Vec<double,N>[n];
    index_t *vecs_dst= new index_t[n];

    fill_unit_vec_array<double,N>(vecs_src, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        vecs_dst[idx % n] = vecs_src[idx % n].hashcode();
        idx += 1;
    }
    clock_t end = clock();
    vecs_dst[0] += 1;

    delete [] vecs_src;
    delete [] vecs_dst;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

// XXX: TODO: HANDLE THIS

template <index_t N> double profile_raw_vec_add(index_t iters) {
    index_t n = NUM_PROFILE_CASES;
    double vecs_src1[n][N];
    double vecs_src2[n][N];
    double vecs_dst[n][N];

    fill_unit_raw_vec_array(vecs_src1, n);
    fill_unit_raw_vec_array(vecs_src2, n);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        for (index_t axis = 0; axis < N; axis++) {
            vecs_dst[idx][axis] = vecs_src1[idx][axis] + vecs_src2[idx][axis];
        }
        idx = (idx + 1) % n;
    }
    clock_t end = clock();
    vecs_dst[0][0] += 1;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <index_t N> double profile_perlin(index_t iters) {
    typedef typename PointType<double,N>::point_t point_t;
    index_t n = NUM_PROFILE_CASES;
    point_t range = point_t(100000);
    point_t *vecs_src = new point_t[n];
    PerlinNoise<double,N> perlin;
    double *dest_vals = new double[n];

    fill_range_vec_array<double,N>(vecs_src, n, -range, range);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        dest_vals[idx] = perlin.eval(vecs_src[i % n]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();
    dest_vals[0] += 1;

    delete [] vecs_src;
    delete [] dest_vals;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_perlin_grad(index_t iters) {
    typedef Dual<T> dual;
    typedef typename PointType<dual,N>::point_t point_t;

    index_t n = NUM_PROFILE_CASES;
    point_t range = point_t(10000);
    point_t *vecs_src = new point_t[n];
    PerlinNoise<dual,N> perlin;
    dual *dest_vals = new dual[n];

    fill_range_vec_array<T,N>(vecs_src, n, -range, range);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        dest_vals[idx] = perlin.eval(vecs_src[i%n]);
        idx = (idx + 1) % n;
    }
    clock_t end = clock();

    delete [] vecs_src;
    delete [] dest_vals;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_perlin_hard_grad(index_t iters) {
    typedef typename PointType<T,N>::point_t point_t;

    index_t n = NUM_PROFILE_CASES;
    point_t range = point_t(10000);
    point_t *vecs_src = new point_t[n];
    PerlinNoise<T,N> perlin;
    point_t *dest_vals = new point_t[n];

    fill_range_vec_array<T,N>(vecs_src, n, -range, range);

    index_t idx = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        dest_vals[idx] = perlin.gradient(vecs_src[n]).second;
        idx = (idx + 1) % n;
    }
    clock_t end = clock();

    delete [] vecs_src;
    delete [] dest_vals;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T>
double profile_rayTriangleTest(index_t iters) {
    Sampler<T> rvs;
    static const index_t N = 3;
    index_t n = std::sqrt(iters);
    Ray<T,N>* rays = new Ray<T,N>[n];
    Vec<T,N>* pts  = new Vec<T,N>[n*N];

    // generate n random rays; pts.
    for (index_t i = 0; i < n; i++) {
        rays[i].origin    = rvs.template unit<N>(2.0);
        rays[i].direction = rvs.template unit<N>();
        for (index_t j = 0; j < N; ++j) {
            pts[i*N + j] = rvs.template unit<N>();
        }
    }
    index_t hits = 0;

    clock_t start = clock();
    for (index_t i = 0; i < n; i++) {
        Vec<T,N>* verts = pts + N * i;
        for (index_t j = 0; j < n; ++j) {
            hits += trace_tri(verts[0], verts[1], verts[2], rays[j], HIT_FRONT).hit;
        }
    }
    clock_t end = clock();

    delete [] rays;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N>
double profile_raySimplexTest(index_t iters) {
    Sampler<T> rvs;
    index_t n = std::sqrt(iters);
    Ray<T,N>* rays = new Ray<T,N>[n];
    Vec<T,N>* pts  = new Vec<T,N>[n*N];

    // generate n random rays; pts.
    for (index_t i = 0; i < n; i++) {
        rays[i].origin    = rvs.template unit<N>(2.0);
        rays[i].direction = rvs.template unit<N>();
        for (index_t j = 0; j < N; ++j) {
            pts[i*N + j] = rvs.template unit<N>();
        }
    }
    index_t hits = 0;
    Vec<T,N-1> srf_coords;
    T s;

    clock_t start = clock();
    for (index_t i = 0; i < n; i++) {
        Vec<T,N>* verts = pts + N * i;
        for (index_t j = 0; j < n; ++j) {
            hits += trace_simplex(verts, rays[j], &srf_coords, &s);
        }
    }
    clock_t end = clock();

    delete [] rays;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t Bands> double profile_sh(index_t iters) {
    SphericalHarmonics<T,Bands> sh;
    index_t n = std::min(iters, (index_t)1000000);
    Vec<T,3> *vs = new Vec<T,3>[n];
    fill_unit_vec_array<T,3>(vs, n);

    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        Vec<T,3> v = vs[i % n];
        sh.eval(v);
    }
    clock_t end = clock();

    delete [] vs;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t Bands> double profile_sh_buffer(index_t iters) {
    SphericalHarmonics<T,Bands> sh;
    SphericalHarmonics<T,Bands> buf;
    index_t n = std::min(iters, (index_t)1000000);
    Vec<T,3> *vs = new Vec<T,3>[n];
    fill_unit_vec_array<T,3>(vs, n);
    T k = 0;

    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        Vec<T,3> v = vs[i % n];
        spherical_harmonic_coeff(&buf, v.z, std::atan2(v.y, v.x));
        k = sh.dot(buf);
    }
    clock_t end = clock();

    delete [] vs;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t Bands>
void test_sh_methods(index_t iters) {
    SphericalHarmonics<T,Bands> sh;
    SphericalHarmonics<T,Bands> buf;
    index_t n = std::min(iters, (index_t)1000000);
    Vec<T,3> *vs = new Vec<T,3>[n];
    fill_unit_vec_array<T,3>(vs, n);

    std::fill(sh.coeffs.get(), sh.coeffs.get() + sh.size(), 1);

    T diff = 0;
    T a_avg = 0;
    T b_avg = 0;

    for (index_t i = 0; i < iters; i++) {
        Vec<T,3> v = vs[i % n];
        spherical_harmonic_coeff(&buf, v.z, std::atan2(v.y, v.x));
        T a = sh.dot(buf);
        T b = sh.eval(v);
        a_avg += a;
        b_avg += b;
        diff  += std::abs(a-b);
    }

    std::cout << "avg sh diff: " << diff / iters << std::endl;
    std::cout << "avg buf,std: " << a_avg << "," << b_avg << std::endl;

    delete [] vs;
}

template <typename T, index_t N> double profile_path(index_t iters) {
    Path<T,N> p;
    Sampler<T> rvs;
    int n_knots = 50;
    for (index_t i = 0; i < n_knots; i++) {
        p.knots.push_back(Ray<T,N>(rvs.template box<N>(), rvs.template solidball<N>()));
    }
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        p.eval(getRandom()->rand(0,n_knots));
    }
    clock_t end = clock();
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N, index_t Channels, Interpolation Interp>
double profile_raster(index_t iters) {
    static const index_t n = 128;
    Vec<index_t,N> dim(n);
    Raster<T,T,N,Channels> image(dim);
    Vec<T,N> *coords = new Vec<T,N>[n];
    Sampler<T> smp;

    // fill with random junk
    // also make some random coords to use
    for (index_t i = 0; i < n; i++) {
        coords[i] = smp.box(Vec<T,N>::zeros, (Vec<T,N>)dim);
        for (index_t j = 0; j < n; j++) {
            image.set(Vec<index_t,N>(i,j), smp.template unit<Channels>());
        }
    }

    // sample in a bunch of random places.
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        image.template sample<EDGE_CLAMP,Interp>(coords[i % n]);
    }
    clock_t end = clock();

    delete [] coords;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T> double profile_rotCtr(index_t iters) {
    Sampler<T> rvs;
    AffineTransform<T,3> xf;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        T angle = rvs.rng->template rand<T>(0, 2*M_PI);
        Vec<T,3> axis = rvs.template unit<3>();
        Vec<T,3> ctr  = rvs.box(Vec<T,3>(-10,-10,-10), Vec<T,3>(10,10,10));
        xf = rotation<T>(axis, ctr, angle);
    }
    clock_t end = clock();
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename M> double profile_mtxCopy(index_t iters) {
    index_t rows = 5;
    index_t cols = 5;
    M m(rows,cols);
    Random *rng = getRandom();
    typedef typename M::elem_t T;
    T *ary = new T[rows*cols+1];
    for (index_t i = 0; i < rows*cols+1; i++) {
        ary[i] = rng->rand(1.0);
    }
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        std::copy(m.begin(), m.end(), ary);
    }
    clock_t end = clock();

    delete [] ary;
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename M> double profile_mtxRegionCopy(index_t iters) {
    index_t rows = 5;
    index_t cols = 5;
    M m(rows,cols);
    Random *rng = getRandom();
    typedef typename M::elem_t T;
    T *ary = new T[rows*cols+1];
    for (index_t i = 0; i < rows*cols+1; i++) {
        ary[i] = rng->rand(1.0);
    }
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++) {
        MatrixRegion region(Vec<index_t,2>(1,1), Vec<index_t,2>(rows-1, cols-1));
        std::copy(m.region_begin(region), m.region_end(region), ary);
    }
    clock_t end = clock();

    delete [] ary;
    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_mtxInverse(index_t iters) {
    SimpleMatrix<T,N,N> mtx[2];
    Random *rng = getRandom();
    for (T *p = mtx[0].begin(); p != mtx[0].end(); p++) {
        *p = rng->rand(1.0);
    }
    index_t x = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++, x = !x) {
        inv(&mtx[!x], mtx[x]);
    }
    clock_t end = clock();

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_mtxInverseLU(index_t iters) {
    SimpleMatrix<T,N,N> mtx[2];
    Random *rng = getRandom();
    for (T *p = mtx[0].begin(); p != mtx[0].end(); p++) {
        *p = rng->rand(1.0);
    }
    index_t x = 0;
    clock_t start = clock();
    for (index_t i = 0; i < iters; i++, x = !x) {
        PLUDecomposition<T,N,N> plu = plu_decompose(mtx[x]);
        plu.inverse(&mtx[!x]);
    }
    clock_t end = clock();

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N>
void randomOrient(AffineTransform<T,N> *xf) {
    Sampler<T> smp;
    Vec<T,N> basis[N];
    for (index_t i = 0; i < N; i++) {
        basis[i] = smp.template unit<N>();
    }
    orthonormalize(basis,N);
    SimpleMatrix<T,N,N> rot(basis[0].begin());
    *xf = transformation(rot);
}

template <typename T>
void randomOrient(AffineTransform<T,3> *xf) {
    Sampler<T> smp;
    Quat<T> q = smp.template unit<4>();
    *xf = rotation(q);
}

template <typename T>
void randomOrient(AffineTransform<T,2> *xf) {
    T angle =  M_PI * getRandom()->rand<T>(-1,1);
    *xf = rotation(angle);
}

template <typename T, index_t N>
void randomBox(OrientedRect<T,N> *r) {
    Sampler<T> smp;

    randomOrient(&r->xf);
    r->xf *= scale(Vec<T,N>(getRandom()->rand<T>(2,8)));
    r->xf *= translation(smp.template unit<N>());
    Vec<T,N> b0 = smp.box(Vec<T,N>(-1), Vec<T,N>(1));
    Vec<T,N> b1 = smp.box(Vec<T,N>(-1), Vec<T,N>(1));
    r->shape = Rect<T,N>::spanning_corners(b0,b1);
}

template <typename T, index_t N>
void getCorners(const ViewFrustum<T,N>& f, Vec<T,N> p[1 << N]) {
    typedef PointType<T,N-1> Pt;
    const Frustum< Rect<T,N-1> >& f_i = f.shape;
    typename Pt::point_t extreme[2] = { f_i.base.lo, f_i.base.hi };
    Rect<T,1> h_clip = f_i.clipped_height();

    for (index_t k = 0; k < 2; k++) {
        // for lower and upper extents
        T h = k == 0 ? h_clip.lo : h_clip.hi;
        for (index_t c = 0; c < (1 << (N-1)); c++) {
            // for each corner of the base rect
            Vec<T,N> pt;
            for (index_t i = 0; i < N-1; i++) {
                // for each coordinate of the corner point
                pt[i] = Pt::iterator(extreme[(c & (1 << i)) != 0])[i];
            }
            pt[N-1] = 1;
            pt *= h;
            pt  = f.xf * pt;
            p[c + ((k > 0) ? (1 <<(N-1)) : 0)] = pt;
        }
    }
}

template <typename T, index_t N>
void getCorners(const OrientedRect<T,N>& r, Vec<T,N> p[1 << N]) {
    Vec<T,N> extreme[2] = { r.shape.lo, r.shape.hi };

    for (index_t i = 0; i < (1 << N); ++i) {
        Vec<T,N> v;
        for (index_t axis = 0; axis < N; ++axis) {
            v[axis] = extreme[((i >> axis) & 1)][axis];
        }
        p[i] = r.xf * v;
    }
}

template <typename T>
void randomFrustum(ViewFrustum<T,2>* f) {
    Sampler<T> smp;
    f->shape.height = Rect<T,1>::spanning_corners(
                        getRandom()->rand<T>(-5,5),
                        getRandom()->rand<T>(-5,5));
    f->shape.base = Rect<T,1>::spanning_corners(
                        getRandom()->rand<T>(-5),
                        getRandom()->rand<T>( 5));
    randomOrient(&f->xf);
    f->xf *= scale(Vec<T,2>(getRandom()->rand(2,8)));
    f->xf *= translation(smp.template unit<2>());
}

template <typename T, index_t N>
void randomFrustum(ViewFrustum<T,N>* f) {
    Sampler<T> smp;
    f->shape.height = Rect<T,1>::spanning_corners(
                        getRandom()->rand<T>(-5,5),
                        getRandom()->rand<T>(-5,5));
    Vec<T,N-1> b0 = smp.box(Vec<T,N-1>(-5), Vec<T,N-1>(5));
    Vec<T,N-1> b1 = smp.box(Vec<T,N-1>(-5), Vec<T,N-1>(5));
    f->shape.base = Rect<T,N-1>::spanning_corners(b0, b1);
    randomOrient(&f->xf);
    f->xf *= scale(Vec<T,N>(getRandom()->rand(2,8)));
    f->xf *= translation(smp.template unit<N>());
}

template <typename T, index_t N> double profile_gjkIntersect(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    const index_t n_corners = 1 << N;
    //Vec<T,N> *rects = new Vec<T,N>[n*n_corners];
    OrientedRect<T,N> *rects = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        //OrientedRect<T,N> r;
        randomBox(rects+i);
        //r.getCorners(rects + n_corners*i);
    }

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    clock_t start = clock();
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        //b = b ^ gjk_intersect(rects + n_corners*i0, n_corners, rects + n_corners*i1, n_corners, &d);
        b = b ^ gjk_intersect(rects[i0], rects[i1], &d);
    }
    clock_t end = clock();
    delete [] rects;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_OBB_intersect(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    OrientedRect<T,N> *boxes = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        randomBox(boxes + i);
    }

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    clock_t start = clock();
    bool SAT = false;
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        SAT = SAT ^ boxes[i0].intersects(boxes[i1]);
    }
    clock_t end = clock();

    delete [] boxes;

    return (end-start) / (double)CLOCKS_PER_SEC;
}

template <typename T, index_t N> double profile_OBB_bounds(index_t iters) {
    index_t n = std::min(iters, (index_t)100000);
    OrientedRect<T,N> *boxes = new OrientedRect<T,N>[n];
    for (index_t i = 0; i < n; i++) randomBox(boxes + i);

    clock_t s = 0;
    Vec<T,N> z;

    for (index_t k = 0; k < iters; k += n) {
        index_t nn = std::min(iters - k, n);

        Rect<T,N> r;

        clock_t start = clock();
        for (index_t j = 0; j < nn; j++) {
            r  = boxes[j].bounds();
            z += r.hi;
        }
        clock_t end = clock();

        s += end - start;
    }

    delete [] boxes;
    return s / (double)CLOCKS_PER_SEC;;
}

//#define EMIT_GJK_ERRS
//#define EMIT_GJK_ALL

// this is tautological for N > 3. OBBs use GJK directly!
template <typename T, index_t N> index_t test_gjkIntersect(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    OrientedRect<T,N>* boxes = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        randomBox(boxes + i);
    }
    const index_t n_corners = 1 << N;

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    index_t failures = 0;
    index_t positive = 0;
    index_t negative = 0;
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        Vec<T,N> b0[n_corners];
        Vec<T,N> b1[n_corners];
        getCorners(boxes[i0], b0);
        getCorners(boxes[i1], b1);
        bool print = false;
#ifdef EMIT_GJK_ALL
        print = true;
#endif
        bool gjk = gjk_intersect(b0, n_corners, b1, n_corners, &d); //, print);
        bool SAT = boxes[i0].intersects(boxes[i1]);
        if (gjk != SAT) {
#ifdef EMIT_GJK_ERRS
            gjk_intersect(b0, n_corners, b1, n_corners, &d); //, true);
            std::cout << ".\n";
#endif
            failures++;
        }
#ifdef EMIT_GJK_ALL
        std::cout << ".\n";
#endif
        if (SAT) positive++;
        else negative++;
    }

    delete [] boxes;

#if !defined(EMIT_GJK_ERRS) and !defined(EMIT_GJK_ALL)
    std::cout << "gjk " << N << "D failures: " << failures;
    std::cout << " (" << positive << " overlapped " << negative << " disjoint)" << std::endl;
#endif

    return failures;
}

template <typename T, index_t N> T test_gjkNearest(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    OrientedRect<T,N>* boxes = new OrientedRect<T,N>[n];
    bool b = true;
    for (index_t i = 0; i < n; i++) {
        randomBox(boxes + i);
    }

    Vec<T,N> d;
    index_t i0 = 0;
    index_t i1 = 0;
    T olap_sum = 0;
    T disj_sum = 0;
    index_t olap_n = 0;
    index_t disj_n = 0;
    for (index_t j = 0; j < iters; j++) {
        i0 = (i0 + 1) % n;
        if (i0 == 0) i1 = (i1 + 1) % n;
        OrientedRect<T,N>& b0 = boxes[i0];
        OrientedRect<T,N>& b1 = boxes[i1];

        Vec<T,N> axi;
        bool olap = minimal_separation_axis(b0, b1, &axi);
        Vec<T,N> contact = b1.convexSupport(axi) + axi;
        T dist = (b0.convexSupport(-axi) - contact).mag();

        // tally error
        if (olap) {
            olap_sum += dist;
            olap_n++;
        } else {
            disj_sum += dist;
            disj_n++;
        }
    }

    delete [] boxes;

    std::cout << "gjk " << N << "D nearest axis:" << std::endl;
    std::cout << "  overlap mean error:  " << (olap_sum / olap_n) << std::endl;
    std::cout << "  disjoint mean error: " << (disj_sum / disj_n) << std::endl;
    return (olap_sum + disj_sum) / iters;
}

template <typename T, index_t N> index_t test_frustumSupport(index_t iters) {
    const index_t n = (index_t)std::ceil(std::sqrt(iters));
    const index_t n_corners = 1 << N;

    Sampler<T> smp;
    T err = 0;

    index_t failures = 0;
    for (index_t j = 0; j < iters/100; j++) {
        ViewFrustum<T,N> f;
        randomFrustum(&f);
        Vec<T,N> pts[n_corners];
        getCorners(f, pts);
        for (index_t k = 0; k < 100; k++) {
            Vec<T,N> d = smp.template unit<N>();
            Vec<T,N> p0 = f.convexSupport(d);
            Vec<T,N> p1 = pts[0];
            T dot0 = p1.dot(d);
            // brute force find support point
            for (index_t c = 1; c < n_corners; c++) {
                T dot1 = pts[c].dot(d);
                if (dot1 > dot0) {
                    dot0 = dot1;
                    p1 = pts[c];
                }
            }
            if (p0 != p1) {
                err += p1.dist(p0);
                failures++;
            }
        }
    }

    std::cout << "frustum support " << N << "D failures: " << failures;
    std::cout << " (" << (100 * failures / (double)iters) << "%)";
    std::cout << " average mismatch: " << (err / (double)iters) << std::endl;

    return failures;
}

template <typename T, index_t N> void test_mtxInverse(index_t iters) {
    SimpleMatrix<T,N,N> mtx[2];
    Random *rng = getRandom();
    for (T *p = mtx[0].begin(); p != mtx[0].end(); p++) {
        *p = rng->rand(1.0);
    }
    std::cout << mtx[0];
    index_t x = 0;
    for (index_t i = 0; i < iters; i++, x = !x) {
        inv(&mtx[!x], mtx[x]);
        std::cout << mtx[!x];
    }
}

template <typename M> void test_mtxRegionCopy(index_t rows, index_t cols) {
    M m(rows,cols);
    typedef typename M::elem_t T;
    const index_t subn = (rows-2)*(cols-2);
    T *v = new T[subn];

    for (index_t i = 0; i < subn; i++) {
        v[i] = 11*i;
    }

    std::fill(m.begin(), m.end(), 1);
    std::copy(v, v+subn, m.region_begin(1,1,rows-1, cols-1));
    cout << m;
}

template <typename T, index_t N> void test_OOBB() {
    OrientedRect<T,N> r1;
    OrientedRect<T,N> r2;
    cout << "box1 intersects box2: " << r1.intersects(r2) << endl;
}

template <typename T, index_t M, index_t N> void test_mtxArithmetic(index_t rows, index_t cols) {
    SimpleMatrix<T,M,N> a(rows,cols);
    SimpleMatrix<T,M,N> b(rows,cols);
    SimpleMatrix<T,M,N == 0 ? 0 : N+1> bogus(rows,cols+1);

    cout << "addTest:" << endl;
    cout << a + b << endl;
    cout << a * 3 << endl;
    cout << 3 * a << endl;
    //correctly does not compile:
    //cout << a + bogus << endl;
}

template <index_t N> void test_permuteMatrix() {
    PermutationMatrix<N> p;
    PermutationMatrix<N> p1;
    Vec<double,N> v;
    Random *rng = getRandom();
    for (index_t i = 0; i < N; i++) {
        v[i] = i;
    }
    for (index_t i = 0; i < N*2; i++) {
        p.swap_rows(rng->rand(N), rng->rand(N));
        p1.swap_rows(rng->rand(N), rng->rand(N));
    }
    SimpleMatrix<double,N,N> m;
    SimpleMatrix<double,N,N> minv;
    SimpleMatrix<double,N,N> m_p1;
    std::copy(p.begin(), p.end(),  m.begin());
    std::copy(p1.begin(), p1.end(),  m_p1.begin());
    inv(&minv, p);
    bool correct = (p * p1) == (m * m_p1);
    if (!correct) throw "permutation matrix failure";
    cout << "v: " << v << endl;
    cout << "P: " << endl << p << endl;
    cout << "P*v: " << (p * v) << endl;
    cout << "M: " << endl << m << endl;
    cout << "M*v: " << (m * v) << endl;
    cout << "P * P1: " << (p * p1) << endl;
    cout << "P * P1 correct? " << correct << endl;
    cout << "P^-1: " << endl << minv << endl;
}

template <index_t M, index_t N> void test_simpleMatrix() {
    SimpleMatrix<double, M, N> m;
    SimpleMatrix<double, DYNAMIC_DIM, DYNAMIC_DIM> q(M,N);
    m[0][1] = 5;
    q[0][1] = 8;
    cout << m << endl;
    cout << q << endl;
}

void profile(std::string name, double (*fnc)(index_t), index_t iterations) {
    double t = fnc(iterations);
    std::cout << name << " (" << iterations << " iters): " << t << " secs; " << iterations/t << " ops/sec"  << std::endl;
}

void test_matrix_asplode() {
    SimpleMatrix<double,4,4> mtx;
    SimpleMatrix<double,2,5> other;
    // correctly, won't compile:
    // mtx * other;
}

void test_dual() {
    typedef Dual<double> duald;
    cout << "sin(pi): " << sin(duald(PI,1)) << endl;
    cout << "cos(pi): " << cos(duald(PI,1)) << endl;
    cout << "tan(pi): " << tan(duald(PI,1)) << endl;
    cout << "2x + 1, @x=3: " << (duald(3,1) * 2 + 1) << endl;
    cout << "exp(1): " << exp(duald(1,1)) << endl;
    cout << "pow(x,x) @x=2: " << pow(duald(2,1), duald(2,1)) << endl;
    cout << "pow(x=2,2): " << pow(duald(2,1), 2) << endl;
    cout << endl;
}

using geom::operator<<;

#if defined(EMIT_GJK_ERRS) or defined(EMIT_GJK_ALL)
int main(int argc, char** argv) {
    test_gjkIntersect<double,3>(100000);
    return 0;
}
#else

int main(int argc, char** argv) {
    Vec2d a2d;
    Vec3d a3d;
    Vec4d a4d;

    //test_gjkNearest<double,3>(1000);

    index_t iters = 10000000;

    assert(&a2d.get(0) == &a2d.x && &a2d.get(1) == &a2d.y);
    assert(&a3d.get(0) == &a3d.x && &a3d.get(1) == &a3d.y && &a3d.get(2) == &a3d.z);
    assert(&a4d.get(0) == &a4d.x && &a4d.get(1) == &a4d.y && &a4d.get(2) == &a4d.z && &a4d.get(3) == &a4d.w);

    test_permuteMatrix<5>();
    test_simpleMatrix<4,3>();
    test_mtxArithmetic<double,4,3>(4,3);
    test_dual();
    test_OOBB<double,3>();
    test_gjkIntersect<double,2>(1000000);
    test_gjkIntersect<double,3>(1000000);
    // test_gjkNearest<double,2>(1000000);
    // test_gjkNearest<double,3>(1000);

#ifdef ENABLE_FRUSTUM
    test_frustumSupport<double,2>(1000000);
    test_frustumSupport<double,3>(1000000);
    test_frustumSupport<double,4>(1000000);
    std::cout << std::endl;
#endif

    profile("3d cross product", profile_vec_cross, iters);
    std::cout << std::endl;

    profile("2d add", profile_vec_add<2>, iters);
    profile("3d add", profile_vec_add<3>, iters);
    profile("4d add", profile_vec_add<4>, iters);
    profile("8d add", profile_vec_add<8>, iters);
    std::cout << std::endl;

    profile("2d raw add", profile_raw_vec_add<2>, iters);
    profile("3d raw add", profile_raw_vec_add<3>, iters);
    profile("4d raw add", profile_raw_vec_add<4>, iters);
    profile("8d raw add", profile_raw_vec_add<8>, iters);
    std::cout << std::endl;

    profile("2d norm", profile_vec_norm<2>, iters);
    profile("3d norm", profile_vec_norm<3>, iters);
    profile("4d norm", profile_vec_norm<4>, iters);
    profile("8d norm", profile_vec_norm<8>, iters);
    std::cout << std::endl;

    profile("2d norm2", profile_vec_norm2<2>, iters);
    profile("3d norm2", profile_vec_norm2<3>, iters);
    profile("4d norm2", profile_vec_norm2<4>, iters);
    profile("8d norm2", profile_vec_norm2<8>, iters);
    std::cout << std::endl;

    //profile("1d perlin", profile_perlin<1>, iters/100);
    profile("2d perlin", profile_perlin<2>, iters/100);
    profile("3d perlin", profile_perlin<3>, iters/100);
    profile("4d perlin", profile_perlin<4>, iters/100);
    profile("5d perlin", profile_perlin<5>, iters/100);
    std::cout << std::endl;

    //profile("1d perlin dual", profile_perlin_grad<double,1>, iters/100);
    profile("2d perlin dual", profile_perlin_grad<double,2>, iters/100);
    profile("3d perlin dual", profile_perlin_grad<double,3>, iters/100);
    profile("4d perlin dual", profile_perlin_grad<double,4>, iters/100);
    std::cout << std::endl;

    //profile("1d perlin grad", profile_perlin_hard_grad<double,1>, iters/100);
    profile("2d perlin grad", profile_perlin_hard_grad<double,2>, iters/100);
    profile("3d perlin grad", profile_perlin_hard_grad<double,3>, iters/100);
    profile("4d perlin grad", profile_perlin_hard_grad<double,4>, iters/100);
    std::cout << std::endl;

    profile("2d hash", profile_vec_hash<2>, iters);
    profile("3d hash", profile_vec_hash<3>, iters);
    profile("4d hash", profile_vec_hash<4>, iters);
    std::cout << std::endl;

    profile("2d dot", profile_vec_dot<2>, iters);
    profile("3d dot", profile_vec_dot<3>, iters);
    profile("4d dot", profile_vec_dot<4>, iters);
    profile("8d dot", profile_vec_dot<8>, iters);
    std::cout << std::endl;

    profile("ray3f-triangle hit",     profile_rayTriangleTest<float>,       iters/10);
    profile("ray3d-triangle hit",     profile_rayTriangleTest<double>,      iters/10);
    profile("ray3d-simplex hit",      profile_raySimplexTest<double,3>,     iters/10);
    profile("ray4d-simplex hit",      profile_raySimplexTest<double,4>,     iters/10);
    profile("ray5d-simplex hit",      profile_raySimplexTest<double,5>,     iters/10);

    std::cout << std::endl;

    profile("2d path", profile_path<double, 2>, iters);
    profile("3d path", profile_path<double, 3>, iters);
    profile("4d path", profile_path<double, 4>, iters);
    profile("2f path", profile_path<float, 2>,  iters);
    profile("3f path", profile_path<float, 3>,  iters);
    profile("4f path", profile_path<float, 4>,  iters);
    std::cout << std::endl;

    profile("OBB 2f bounds", profile_OBB_bounds<float,2>,  iters / 10);
    profile("OBB 3f bounds", profile_OBB_bounds<float,3>,  iters / 10);
    profile("OBB 4f bounds", profile_OBB_bounds<float,4>,  iters / 10);
    profile("OBB 2d bounds", profile_OBB_bounds<double,2>, iters / 10);
    profile("OBB 3d bounds", profile_OBB_bounds<double,3>, iters / 10);
    profile("OBB 4d bounds", profile_OBB_bounds<double,3>, iters / 10);
    std::cout << std::endl;

    profile("sh  3f band", profile_sh<float, 3>,  iters/10);
    profile("sh  8f band", profile_sh<float, 8>,  iters/50);
    profile("sh 16f band", profile_sh<float, 16>, iters/100);
    profile("sh  3d band", profile_sh<double, 3>,  iters/10);
    profile("sh  8d band", profile_sh<double, 8>,  iters/50);
    profile("sh 16d band", profile_sh<double, 16>, iters/100);
    std::cout << std::endl;

    profile("2f->3f linear img sample", profile_raster<float, 2, 3, INTERP_LINEAR>, iters/100);
    profile("3f->3f linear img sample", profile_raster<float, 3, 3, INTERP_LINEAR>, iters/100);
    profile("2f->3f cubic img sample",  profile_raster<float, 2, 3, INTERP_CUBIC>,  iters/100);
    profile("3f->3f cubic img sample",  profile_raster<float, 3, 3, INTERP_CUBIC>,  iters/100);
    std::cout << std::endl;

    profile("5x5 mtxf region copy",    profile_mtxRegionCopy<SimpleMatrix<float,0,0> >,    iters);
    profile("5x5 mtxd region copy",    profile_mtxRegionCopy<SimpleMatrix<double,0,0> >,   iters);
    profile("5x5 diagf region copy",   profile_mtxRegionCopy<DiagMatrix<float,5,5> >, iters);
    std::cout << std::endl;

    profile("5x5 mtxf copy",    profile_mtxCopy<SimpleMatrix<float,0,0> >, iters/100);
    profile("5x5 mtxd copy",    profile_mtxCopy<SimpleMatrix<double,0,0> >, iters/100);
    profile("5x5 diagf copy",   profile_mtxCopy<DiagMatrix<float,5,5> >, iters/100);   // xxx: memory error in here?!
    std::cout << std::endl;

    profile("2x2 mtxf inv", profile_mtxInverse<float,2>,  iters/10);
    profile("3x3 mtxf inv", profile_mtxInverse<float,3>,  iters/10);
    profile("4x4 mtxf inv", profile_mtxInverse<float,4>,  iters/10);
    profile("2x2 mtxd inv", profile_mtxInverse<double,2>, iters/10);
    profile("3x3 mtxd inv", profile_mtxInverse<double,3>, iters/10);
    profile("4x4 mtxd inv", profile_mtxInverse<double,4>, iters/10);
    profile("8x8 mtxd inv", profile_mtxInverse<double,8>, iters/10);
    std::cout << std::endl;

    profile("rotationd matrix", profile_rotCtr<double>, iters/10);
    profile("rotationf matrix", profile_rotCtr<float>, iters/10);
    std::cout << std::endl;

    profile("gjk float 2d",  profile_gjkIntersect<float,2>,  1000000);
    profile("gjk double 2d", profile_gjkIntersect<double,2>, 1000000);
    profile("gjk float 3d",  profile_gjkIntersect<float,3>,  1000000);
    profile("gjk double 3d", profile_gjkIntersect<double,3>, 1000000);
    profile("gjk float 4d",  profile_gjkIntersect<float,4>,  100000);
    profile("gjk double 4d", profile_gjkIntersect<double,4>, 100000);
    profile("gjk double 5d", profile_gjkIntersect<double,5>, 100000);
    std::cout << std::endl;

    profile("SAT 2D OBB double", profile_OBB_intersect<double,2>, 1000000);
    profile("SAT 3D OBB double", profile_OBB_intersect<double,3>, 1000000);
    std::cout << std::endl;

    std::cout << "sizeof vec2f: " << sizeof(Vec<float,2>) << std::endl;
    std::cout << "sizeof vec3f: " << sizeof(Vec<float,3>) << std::endl;
    std::cout << "sizeof vec4f: " << sizeof(Vec<float,4>) << std::endl;
    std::cout << "sizeof vec2d: " << sizeof(Vec<double,2>) << std::endl;
    std::cout << "sizeof vec3d: " << sizeof(Vec<double,3>) << std::endl;
    std::cout << "sizeof vec4d: " << sizeof(Vec<double,4>) << std::endl;

    try {
        test_matrix_asplode();
    } catch (GeomException &ex) {
        std::cout << ex.what() << std::endl << std::endl;
    }

    /*
    std::cout << "Mersenne Twister" << std::endl << "================================" << std::endl;
    RandomBattery bat = RandomBattery(new MTRand());
    bat.runAll(iters);
    std::cout << std::endl;

    std::cout << "Linear Congruential" << std::endl << "================================" << std::endl;
    bat = RandomBattery(new LCRand());
    bat.runAll(iters);
    std::cout << std::endl;
    */

    std::cout << "done." << std::endl;

    return 0;
}

#endif


#ifndef POTENTIAL_HPP_INCLUDED
#define POTENTIAL_HPP_INCLUDED

/*! \file potential.hpp
    \brief Defines a class that manages n-dimensional Potentials (including derivatives).
    \ingroup common
*/

#include "vector.hpp"
#include <vector>
#include <map>
#include <boost/noncopyable.hpp>
#include <boost/optional.hpp>
#include "global.hpp"
#include "dynamic_grid.hpp"
#include "multiindex.hpp"

/*! \class Potential
    \brief Class for managing potentials and derivatives.
    \details This class manages an n-dimensional potential along with its derivatives. It manages their
            grids for saving the raw data, contains metainformation as to the parameters of the Potential
            and adds a support definition to define the physical scale of the potential data.
    \attention This class is not optimized for speed. If you need to use data from a protential in a tight loop,
            get the data grid once and then only access that grid.
    \ingroup common
*/
class Potential final
{
    struct IndexType
    {
        IndexType(std::string n, std::vector<int> d) : name(std::move(n)), derivations(std::move(d)) {}
        std::string name;
        std::vector<int> derivations;
        bool operator<(const IndexType& other) const
        {
            if(name < other.name)
                return true;
            else if(name == other.name)
                return std::lexicographical_compare(derivations.begin(), derivations.end(), other.derivations.begin(), other.derivations.end());
            return false;
        }
    };
public:
    // commonly used typedefs
    /// type that is saved inside the potential
    typedef double value_type;
    /// type of the grid that is used to save the potential and its derivatives.
    typedef DynamicGrid<value_type> grid_type;

    // --------------------------------------------------
    //        c/d tors
    // --------------------------------------------------
    /*!    \brief square potential ctor.
        \details This constructor creates a potential that is equivalent in all dimensions, i.e.
                extents and support are the same for all dimensions.
        \param size size of the potential
        \param dimension dimension of the potential
        \param support support (in physical domain) of the potential
        \param strength Strength of the potential. Usually assumed to mean standard deviation of the potential.
    */
    Potential(std::size_t dimension, double support, std::size_t size, double strength=1);

    /*! \brief Constructs a Potential object with defined extents, support and max_order.
        \param extents extents of the potential. This is the number of elements in each dimension.
        \param support This defines the size of the Potential in physical domain.
        \param strength Strength of the potential. Usually assumed to mean standard deviation of the potential.
    */
    Potential(std::vector<std::size_t> extents, std::vector<double> support, double strength=1);

    /// user defined move constructor, because extents in defined as a const member.
    Potential( Potential&& ) noexcept;

    /// no copying
    Potential( const Potential& ) = delete;
    /// no copying
    Potential& operator=(const Potential&) = delete;

    // --------------------------------------------------
    //               info functions
    // --------------------------------------------------

    /// sets the creation info
    void setCreationInfo( std::size_t seed, std::size_t version, double corrlength);

    /// returns the size of the potential
    /// defined inline because it might be called from hot part of code
    const std::vector<std::size_t>& getExtents() const { return mExtents; };

    /// gets the real size of the domain the potential is defined on. This is [0, getSupport)^N
    const std::vector<double>& getSupport() const;

    /// get seed used to create this potential
    std::size_t getSeed() const;

    /// get potgen version
    std::size_t getPotgenVersion() const;

    /// gets potential dimension
    std::size_t getDimension() const;

    /// get the correlation length used to generate this potential
    /// returns -1 if no valid correlation length
    double getCorrelationLength() const;

    // --------------------------------------------------
    //            read access potential data
    // --------------------------------------------------

    /// returns a grid with the potential data
    const grid_type& getPotential(const std::string& name = "potential") const;

    /// returns a grid with given derivative, checking that \p deriv is in fact valid.
    const grid_type& getDerivative( std::vector<int> deriv, const std::string& name = "potential" ) const;

    /// returns a grid with the data of the index'th derivative
    template<class T>
    const grid_type& getDerivative( const T& index, const std::string& name = "potential" ) const
    {
        std::vector<int> index_vec(index.begin(), index.end());
        return getDerivative(index_vec, name);
    }

    /// checks whether a certain derivative is saved inside this potential
    bool hasDerivative( const MultiIndex& index, const std::string& name = "potential") const;

    /// checks whether all derivatives of a certain order are present
    bool hasDerivativesOfOrder( std::size_t order, const std::string& name = "potential" ) const;

    // --------------------------------------------------
    //            write access potential data
    // --------------------------------------------------

    /// set potential \sa setDerivative
    const grid_type& setPotential( grid_type data, const std::string& name = "potential" );

    /// sets the n'th derivative. implemented as a move operation to prevent copies of
    /// really big vectors.
    /// \return the newly created derivative grid
    const grid_type& setDerivative( const MultiIndex& index, grid_type data, const std::string& name = "potential" );

    const grid_type& setDerivative( std::vector<int> deriv, grid_type data, const std::string& name = "potential" );

    // --------------------------------------------------
    //               transformation
    // --------------------------------------------------

    /// multiplies all derivatives and the potential itself by a constant \p factor
    /// use empty /p name to scale all potentials simultaneously.
    void scalePotential( double factor, const std::string& name = "" );

    /// changes the size of the potential. This scaling of the definition domain affects
    /// the value of derivatives.
    /// \attention  This only changes derivatives based on the changing length scale. If
    ///                the corresponding physical quantity has to be scaled too, use
    ///                scalePotential.
    void setSupport( const std::vector<double>&  supp, const std::string& name = "potential");

    // --------------------------------------------------
    //               strength
    // --------------------------------------------------
    /// gets the strength prefactor of the potential.
    double getStrength() const;

    /// sets the strength of the potential
    void setStrength(double new_strength);

    // --------------------------------------------------
    //               serialization
    // --------------------------------------------------

    /// writes this potential to a file
    void writeToFile( std::fstream& file ) const;

    /// creates a potential object by reading from a file
    static Potential readFromFile( std::fstream& file );

private:
    // helper functions
    /// gets the total order of the derivative index
    std::size_t getOrder( const MultiIndex& dindex ) const;

    // general data
    /// potentials dimension
    const std::size_t mDimension;
    /// seed used to generate this potential
    std::size_t mSeed;
    /// size of the potential
    const std::vector<std::size_t> mExtents;
    /// support size
    std::vector<double> mSupport;
    /// potential strength
    double mStrength = 1;
    /// correlation length
    double mCorrelationLength = -1;
    /// version of the potgen function
    std::size_t mPotgenVersion;
    /// potential and derivatives grids
    std::map<IndexType, grid_type> mData;

};

#endif // POTENTIAL_HPP_INCLUDED


#include <boost/test/unit_test.hpp>

#include <secp256k1/include/Scalar.h>
#include <secp256k1/include/GroupElement.h>

BOOST_AUTO_TEST_SUITE(sigma_primitive_types)

BOOST_AUTO_TEST_CASE(scalar_test)
{
    // Create a large scalar value.
    secp_primitives::Scalar s(123456789);
    s *= s;
    s *= s;
    s *= s;
    s *= s;
    s *= s;
    secp_primitives::Scalar s2(s);

    // Make sure that copy constructor works correctly.
    BOOST_CHECK(s == s2);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef SAMPLE_CONSENSUS_HPP
#define SAMPLE_CONSENSUS_HPP

/// PROJECT
#include <csapex/model/node.h>
#include <csapex/model/node_modifier.h>

#include <csapex/msg/generic_value_message.hpp>
#include <csapex/msg/generic_vector_message.hpp>
#include <csapex/msg/io.h>

#include <csapex_point_cloud/msg/indices_message.h>
#include <csapex_point_cloud/msg/model_message.h>
#include <csapex_point_cloud/msg/normals_message.h>
#include <csapex_point_cloud/msg/point_cloud_message.h>

#include <csapex/param/parameter_factory.h>
#include <csapex/utility/register_apex_plugin.h>

#include <csapex_math/param/factory.h>

#include <pcl/point_representation.h>

#include <boost/mpl/for_each.hpp>

#include <Eigen/Core>

#include <csapex_sample_consensus/csapex_sample_consensus.hpp>

namespace csapex
{
using namespace connection_types;

class EIGEN_ALIGN16 SampleConsensus : public csapex::Node
{
public:
    SampleConsensus() = default;

    void setupParameters(Parameterizable& parameters) override
    {
        parameters.addParameter(param::factory::declareRange("model search distance", 0.0, 10.0, 0.1, 0.001), sac_parameters_.model_search_distance);
        parameters.addParameter(param::factory::declareRange("model validation ratio", 0.0, 1.0, 0.0, 0.01), sac_parameters_.model_validation_ratio);
        parameters.addParameter(param::factory::declareBool("terminate on mean model distance", false), sac_parameters_.use_mean_model_distance);
        parameters.addConditionalParameter(param::factory::declareRange("maximum mean model distance", 0.0, 10.0, 0.05, 0.01), [this]() { return sac_parameters_.use_mean_model_distance; },
                                           sac_parameters_.mean_model_distance);
        //        parameters.addParameter(param::factory::declareBool("optimize
        //        model coefficients", false),
        //                                ransac_parameters_.optimize_model_coefficients);
        parameters.addParameter(param::factory::declareRange("point skip", 0, 10, 0, 1), point_skip_);
        parameters.addParameter(param::factory::declareRange("maximum iterations", 1, 100000, 100, 1), sac_parameters_.maximum_iterations);

        parameters.addParameter(param::factory::declareRange("minimum model cloud size", 1, 100000, 1000, 1), minimum_model_cloud_size_);

        parameters.addParameter(param::factory::declareBool("find multiple models", false), fit_multiple_models_);
        parameters.addConditionalParameter(param::factory::declareRange("minimum residual cloud size", 0, 1000, 100, 1), [this]() { return fit_multiple_models_; }, minimum_residual_cloud_size_);
        parameters.addConditionalParameter(param::factory::declareRange("maximum model count", -1, 100, 5, 1), [this]() { return fit_multiple_models_; }, maximum_model_count_);

        std::map<std::string, int> model_types = { { "PLANE", PLANE }, { "NORMAL_PLANE", NORMAL_PLANE }, { "PARALLEL_NORMAL_PLANE", PARALLEL_NORMAL_PLANE } };

        parameters.addParameter(param::factory::declareParameterSet("model type", model_types, (int)PLANE), model_type_);

        /// actually, this can be done better ! - temporary
        auto uses_axis = [this]() { return model_type_ == PARALLEL_NORMAL_PLANE; };

        auto uses_normals = [this]() { return model_type_ == PARALLEL_NORMAL_PLANE || NORMAL_PLANE; };

        parameters.addConditionalParameter(param::factory::declareValue("axis/x", 0.0), uses_axis, axis_(0));
        parameters.addConditionalParameter(param::factory::declareValue("axis/y", 0.0), uses_axis, axis_(1));
        parameters.addConditionalParameter(param::factory::declareValue("axis/z", 1.0), uses_axis, axis_(2));

        parameters.addConditionalParameter(param::factory::declareValue("normal/distance_weight", 0.0), uses_normals, normal_distance_weight_);

        parameters.addConditionalParameter(param::factory::declareAngle("normal/angle_distance", 0.0), uses_axis, angle_eps_);

        parameters.addParameter(param::factory::declareBool("keep invalid as outlier", true), keep_invalid_as_outlier_);
    }

    virtual void setup(csapex::NodeModifier& node_modifier) override
    {
        in_cloud_ = node_modifier.addInput<PointCloudMessage>("PointCloud");
        in_indices_ = node_modifier.addOptionalInput<PointIndicesMessage>("Indices");  // optional input
        in_normals_ = node_modifier.addOptionalInput<NormalsMessage>("Normals");       // optional input

        out_models_ = node_modifier.addOutput<GenericVectorMessage, ModelMessage>("Models");
        out_inlier_indices_ = node_modifier.addOutput<GenericVectorMessage, pcl::PointIndices>("Model Points");
        out_outlier_indices_ = node_modifier.addOutput<GenericVectorMessage, pcl::PointIndices>("Rejected Points");
    }

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
    enum ModelType
    {
        PLANE,
        NORMAL_PLANE,
        PARALLEL_NORMAL_PLANE
    };

    int model_type_;

    csapex_sample_consensus::Parameters sac_parameters_;

    int point_skip_;
    int termination_criteria_;
    int minimum_fit_size_;
    int minimum_model_cloud_size_;

    bool fit_multiple_models_;
    int minimum_residual_cloud_size_;
    int maximum_model_count_;

    double normal_distance_weight_;
    Eigen::Vector3d axis_;
    double angle_eps_;

    bool keep_invalid_as_outlier_;

    Input* in_cloud_;
    Input* in_indices_;
    Input* in_normals_;
    Output* out_models_;
    Output* out_inlier_indices_;
    Output* out_outlier_indices_;

    template <typename PointT>
    typename csapex_sample_consensus::models::Model<PointT>::Ptr getModel(typename pcl::PointCloud<PointT>::ConstPtr& cloud)
    {
        typename csapex_sample_consensus::models::Model<PointT>::Ptr model;

        switch ((ModelType)model_type_) {
            case PLANE:
                model.reset(new csapex_sample_consensus::models::Plane<PointT>(cloud));
                break;
            case NORMAL_PLANE: {
                pcl::PointCloud<pcl::Normal>::ConstPtr normals;
                getNormals(normals);
                if (!normals)
                    throw std::runtime_error("The selected setting requires normals!");

                model.reset(new csapex_sample_consensus::models::NormalPlane<PointT, pcl::Normal>(cloud, normals, normal_distance_weight_));

            } break;
            case PARALLEL_NORMAL_PLANE: {
                pcl::PointCloud<pcl::Normal>::ConstPtr normals;
                getNormals(normals);
                if (!normals)
                    throw std::runtime_error("The selected setting requires normals!");

                auto* pnp_model = new csapex_sample_consensus::models::ParallelNormalPlane<PointT, pcl::Normal>(cloud, normals, normal_distance_weight_);
                pnp_model->setAxis(pcl::Normal(axis_(0), axis_(1), axis_(2)), angle_eps_);
                model.reset(pnp_model);
            } break;
            default:
                throw std::runtime_error("[SampleConsensus]: Unknown model type!");
        }

        return model;
    }

    void getNormals(pcl::PointCloud<pcl::Normal>::ConstPtr& normals)
    {
        if (msg::hasMessage(in_normals_)) {
            NormalsMessage::ConstPtr in_normals = msg::getMessage<NormalsMessage>(in_normals_);
            normals = in_normals->value;
        }
    }

    template <typename PointT>
    void getIndices(typename pcl::PointCloud<PointT>::ConstPtr& cloud, std::vector<int>& indices)
    {
        const static pcl::DefaultPointRepresentation<PointT> pr;
        const std::size_t size = cloud->size();
        indices.reserve(size);

        const std::size_t step = 1 + point_skip_;
        for (std::size_t i = 0; i < size; i += step) {
            if (pr.isValid(cloud->at(i))) {
                indices.emplace_back(i);
            }
        }
    }

    template <typename PointT>
    void getIndices(typename pcl::PointCloud<PointT>::ConstPtr& cloud, std::vector<int>& valids, std::vector<int>& invalids)
    {
        const static pcl::DefaultPointRepresentation<PointT> pr;
        const std::size_t size = cloud->size();
        valids.reserve(size);
        invalids.reserve(size);

        const std::size_t step = 1 + point_skip_;
        for (std::size_t i = 0; i < size; i += step) {
            if (pr.isValid(cloud->at(i))) {
                valids.emplace_back(i);
            } else {
                invalids.emplace_back(i);
            }
        }
    }

    void getInidicesFromInput(std::vector<int>& indices)
    {
        if (msg::hasMessage(in_indices_)) {
            PointIndicesMessage::ConstPtr in_indices_msg = msg::getMessage<PointIndicesMessage>(in_indices_);
            const std::vector<int>& in_indices = in_indices_msg->value->indices;
            if (point_skip_ > 0) {
                const std::size_t step = 1 + point_skip_;
                const std::size_t size = in_indices.size();
                indices.reserve(size);
                for (std::size_t i = 0; i < size; i += step) {
                    indices.emplace_back(in_indices[i]);
                }
            } else {
                indices = in_indices;
            }
        }
    }
};
}  // namespace csapex

#endif  // SAMPLE_CONSENSUS_HPP


#ifndef TMC_UNIT_TESTS
#define BOOST_TEST_MODULE lib_classifier_creator_train_tmc
#endif
#include <boost/test/included/unit_test.hpp>

#include "feature_names.h"
#include "classifier_creator_train_tmc.h"

BOOST_AUTO_TEST_SUITE( classifier_creator_train_tmc )

BOOST_AUTO_TEST_CASE( basics )
{
    const std::string names_file("data/4layers_36points/4layers_36points.names");
    const std::string data_file("data/4layers_36points/4layers_36points.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_TEST_MESSAGE("Resulting sample: " << *pSample);
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 36);

    std::shared_ptr<ClassifierCreatorTrain> pTC = std::make_shared<ClassifierCreatorTrainTmc>();
    (*pTC).init(pSample).train();
    (*pTC).init(pSample).train();

    std::shared_ptr<Classifier> pClTmc = pTC->get_classifier();

    std::vector<double> p;  std::pair<double, double> conf;
    p = {10,2}; conf = {1.0/6.0,1.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), conf.first);
    p = {16,1}; conf = {1.0/3.0,1.0/3.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), conf.first);
    p = {7,10}; conf = {2.0/3.0,2.0/3.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), conf.first);
    p = {11,10}; conf = {5.0/6.0,5.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), conf.first);
    p = {4,6}; conf = {0,1.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), conf.second);
    p = {7,13}; conf = {5.0/6.0,1}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), conf.first);
    p = {14.5,0.5}; conf = {0,1.0/3.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8)/(double)(50+10+16+8));
    p = {-1,16}; conf = {0,5.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8+18+45)/(double)(50+10+16+8+9+18+9+45));
    p = {-1,18}; conf = {0,1}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8+18+45)/(double)(50+10+16+8+9+18+9+45));
    p = {12.5,1.5}; conf = {1.0/6.0,1.0/3.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8)/(double)(50+10+16+8));
    p = {4,8}; conf = {1.0/6.0,2.0/3.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8+18)/(double)(50+10+16+8+9+18));
    p = {3,13}; conf = {1.0/6.0,5.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8+18+45)/(double)(50+10+16+8+9+18+9+45));
    p = {1,18};  conf = {1.0/6.0,1}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (10+8+18+45)/(double)(50+10+16+8+9+18+9+45));
    p = {15,5.5}; conf = {1.0/3.0,5.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (8+18+45)/(double)(16+8+9+18+9+45));
    p = {18,1}; conf = {1.0/3.0,1}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (8+18+45)/(double)(16+8+9+18+9+45));
    p = {8,12}; conf = {2.0/3.0,5.0/6.0}; BOOST_CHECK(pClTmc->confidence_interval(p) == conf);
    BOOST_CHECK_EQUAL(pClTmc->confidence(p), (18+45)/(double)(9+18+9+45));
}

BOOST_AUTO_TEST_SUITE_END()


#include <bsplines/BSplinePose.hpp>
#include <sm/assert_macros.hpp>
// boost::tie
#include <boost/tuple/tuple.hpp>
#include <sm/kinematics/transformations.hpp>

using namespace sm::kinematics;
using namespace bsplines;

BSplinePose::BSplinePose(int splineOrder, const RotationalKinematics::Ptr& rotationalKinematics)
    : BSpline(splineOrder), rotation_(rotationalKinematics) {}

BSplinePose::~BSplinePose() {}

Eigen::Matrix4d BSplinePose::transformation(double tk) const { return curveValueToTransformation(eval(tk)); }

Eigen::Matrix4d BSplinePose::transformationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                       Eigen::VectorXi* coefficientIndices) const {
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::MatrixXd JT;
    Eigen::Matrix4d T = curveValueToTransformationAndJacobian(p, &JT);

    if (J) {
        *J = JT * JS;
    }

    return T;
}

Eigen::Matrix3d BSplinePose::orientationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                    Eigen::VectorXi* coefficientIndices) const {
    Eigen::Matrix3d C;
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::Matrix3d S;
    C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);

    Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3, 6);
    JO.block(0, 3, 3, 3) = S;
    if (J) {
        *J = JO * JS;
    }

    return C;
}

Eigen::Matrix3d BSplinePose::inverseOrientationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                           Eigen::VectorXi* coefficientIndices) const {
    Eigen::Matrix3d C;
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::Matrix3d S;
    C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S).transpose();

    Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3, 6);
    JO.block(0, 3, 3, 3) = S;
    if (J) {
        *J = -C * JO * JS;
    }

    return C;
}

Eigen::Matrix4d BSplinePose::inverseTransformationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                              Eigen::VectorXi* coefficientIndices) const {
    // std::cout << __FUNCTION__ << "()\n";
    // ASRL_THROW(std::runtime_error,"Not Implemented");
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::MatrixXd JT;
    Eigen::Matrix4d T = curveValueToTransformationAndJacobian(p, &JT);
    // Invert the transformation.
    T.topLeftCorner<3, 3>().transposeInPlace();
    T.topRightCorner<3, 1>() = (-T.topLeftCorner<3, 3>() * T.topRightCorner<3, 1>()).eval();

    if (J) {
        // The "box times" is the linearized transformation way of inverting the jacobian.
        *J = -sm::kinematics::boxTimes(T) * JT * JS;
    }

    if (coefficientIndices) {
        *coefficientIndices = localCoefficientVectorIndices(tk);
    }

    return T;
}

Eigen::Matrix4d BSplinePose::inverseTransformation(double tk) const {
    Eigen::Matrix4d T = curveValueToTransformation(eval(tk));
    T.topLeftCorner<3, 3>().transposeInPlace();
    T.topRightCorner<3, 1>() = (-T.topLeftCorner<3, 3>() * T.topRightCorner<3, 1>()).eval();
    return T;
}

Eigen::Vector4d BSplinePose::transformVectorAndJacobian(double tk, const Eigen::Vector4d& v_tk, Eigen::MatrixXd* J,
                                                        Eigen::VectorXi* coefficientIndices) const {
    Eigen::MatrixXd JT;
    Eigen::Matrix4d T_n_vk = transformationAndJacobian(tk, &JT, coefficientIndices);
    Eigen::Vector4d v_n = T_n_vk * v_tk;

    if (J) {
        *J = sm::kinematics::boxMinus(v_n) * JT;
    }

    return v_n;
}

// Position at certain time. p = x[:3]
Eigen::Vector3d BSplinePose::position(double tk) const { return eval(tk).head<3>(); }

// Orientation at certain time, phi = x[3:], R = Exp(phi)
Eigen::Matrix3d BSplinePose::orientation(double tk) const {
    return rotation_->parametersToRotationMatrix(eval(tk).tail<3>());
}

Eigen::Matrix3d BSplinePose::inverseOrientation(double tk) const {
    return rotation_->parametersToRotationMatrix(eval(tk).tail<3>()).transpose();
}

// v_W = dp/dt
Eigen::Vector3d BSplinePose::linearVelocity(double tk) const { return evalD(tk, 1).head<3>(); }

// v_B = R_BW * v_W = R_WB^T * dp/dt
Eigen::Vector3d BSplinePose::linearVelocityBodyFrame(double tk) const {
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
    return C_wb.transpose() * evalD(tk, 1).head<3>();
}

// a_W = dp^2/dt^2
Eigen::Vector3d BSplinePose::linearAcceleration(double tk) const { return evalD(tk, 2).head<3>(); }

// a_B = R_BW * a_W = R_WB^T * dp^2/dt^2
Eigen::Vector3d BSplinePose::linearAccelerationBodyFrame(double tk) const {
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
    return C_wb.transpose() * evalD(tk, 2).head<3>();
}

Eigen::Vector3d BSplinePose::linearAccelerationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                           Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d a = evalDAndJacobian(tk, 2, J, coefficientIndices).head<3>();
    if (J) {
        J->conservativeResize(3, J->cols());
    }
    return a;
}

// \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
Eigen::Vector3d BSplinePose::angularVelocity(double tk) const {
    Eigen::Vector3d omega;
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::VectorXd v = evalD(tk, 1);

    // \omega = S(\bar \theta) \dot \theta
    omega = -rotation_->parametersToSMatrix(r.tail<3>()) * v.tail<3>();
    return omega;
}

// \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularVelocityBodyFrame(double tk) const {
    Eigen::Vector3d omega;
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::VectorXd v = evalD(tk, 1);
    Eigen::Matrix3d S;
    Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

    // \omega = S(\bar \theta) \dot \theta
    omega = -C_w_b.transpose() * S * v.tail<3>();
    return omega;
}

// \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularVelocityBodyFrameAndJacobian(double tk, Eigen::MatrixXd* J,
                                                                 Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, NULL).tail<3>();
    pdot = evalDAndJacobian(tk, 1, &Jpdot, coefficientIndices).tail<3>();

    Eigen::MatrixXd Jr;
    Eigen::Matrix3d C_w_b = inverseOrientationAndJacobian(tk, &Jr, NULL);

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    // std::cout << "Jpdot\n" << Jpdot << std::endl;

    Eigen::Matrix<double, 3, 6> Jo;
    omega = -C_w_b * rotation_->angularVelocityAndJacobian(p, pdot, &Jo);
    Jo = (-C_w_b * Jo).eval();
    // std::cout << "Jo:\n" << Jo << std::endl;
    if (J) {
        *J = Jo * Jpdot + sm::kinematics::crossMx(omega) * Jr;
    }

    return omega;
}

// \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
Eigen::Vector3d BSplinePose::angularVelocityAndJacobian(double tk, Eigen::MatrixXd* J,
                                                        Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, nullptr).tail<3>();
    pdot = evalDAndJacobian(tk, 1, &Jpdot, coefficientIndices).tail<3>();

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    // std::cout << "Jpdot\n" << Jpdot << std::endl;

    Eigen::Matrix<double, 3, 6> Jo;
    // FixMe by CC: seems like lost the minus "-"?
    omega = rotation_->angularVelocityAndJacobian(p, pdot, &Jo);

    // std::cout << "Jo:\n" << Jo << std::endl;
    if (J) {
        *J = Jo * Jpdot;
    }

    return omega;
}

// \omega_dot_b_{w,b} (angular acceleration of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularAccelerationBodyFrame(double tk) const {
    Eigen::Vector3d omega;
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::VectorXd v = evalD(tk, 2);
    Eigen::Matrix3d S;
    Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

    // \omega = S(\bar \theta) \dot \theta
    omega = -C_w_b.transpose() * S * v.tail<3>();
    return omega;
}

// \omega_dot_b_{w,b} (angular acceleration of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularAccelerationBodyFrameAndJacobian(double tk, Eigen::MatrixXd* J,
                                                                     Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, NULL).tail<3>();
    pdot = evalDAndJacobian(tk, 2, &Jpdot, coefficientIndices).tail<3>();

    Eigen::MatrixXd Jr;
    Eigen::Matrix3d C_w_b = inverseOrientationAndJacobian(tk, &Jr, NULL);

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    Eigen::Matrix<double, 3, 6> Jo;
    omega = -C_w_b * rotation_->angularVelocityAndJacobian(p, pdot, &Jo);
    Jo = (-C_w_b * Jo).eval();
    if (J) {
        *J = Jo * Jpdot + sm::kinematics::crossMx(omega) * Jr;
    }

    return omega;
}

// \omega_dot_w_{b,w} (angular acceleration of the body frame as seen from the world frame, expressed in the world
// frame)
Eigen::Vector3d BSplinePose::angularAccelerationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                            Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, NULL).tail<3>();
    pdot = evalDAndJacobian(tk, 2, &Jpdot, coefficientIndices).tail<3>();

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    Eigen::Matrix<double, 3, 6> Jo;
    // FixMe by CC: seems like lost the minus "-"?
    omega = rotation_->angularVelocityAndJacobian(p, pdot, &Jo);
    if (J) {
        *J = Jo * Jpdot;
    }

    return omega;
}

void BSplinePose::initPoseSpline(double t0, double t1, const Eigen::Matrix4d& T_n_t0, const Eigen::Matrix4d& T_n_t1) {
    Eigen::VectorXd v0 = transformationToCurveValue(T_n_t0);
    Eigen::VectorXd v1 = transformationToCurveValue(T_n_t1);

    initSpline(t0, t1, v0, v1);
}

void BSplinePose::initPoseSpline2(const Eigen::VectorXd& times, const Eigen::Matrix<double, 6, Eigen::Dynamic>& poses,
                                  int numSegments, double lambda) {
    initSpline2(times, poses, numSegments, lambda);
}

void BSplinePose::initPoseSpline3(const Eigen::VectorXd& times, const Eigen::Matrix<double, 6, Eigen::Dynamic>& poses,
                                  int numSegments, double lambda) {
    initSpline3(times, poses, numSegments, lambda);
}

void BSplinePose::initPoseSplineSparse(const Eigen::VectorXd& times,
                                       const Eigen::Matrix<double, 6, Eigen::Dynamic>& poses, int numSegments,
                                       double lambda) {
    initSplineSparse(times, poses, numSegments, lambda);
}

void BSplinePose::initPoseSplineSparseKnots(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints,
                                            const Eigen::VectorXd& knots, double lambda) {
    initSplineSparseKnots(times, interpolationPoints, knots, lambda);
}

void BSplinePose::addPoseSegment(double tk, const Eigen::Matrix4d& T_n_tk) {
    Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

    addCurveSegment(tk, vk);
}

void BSplinePose::addPoseSegment2(double tk, const Eigen::Matrix4d& T_n_tk, double lambda) {
    Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

    addCurveSegment2(tk, vk, lambda);
}

Eigen::Matrix4d BSplinePose::curveValueToTransformation(const Eigen::VectorXd& c) const {
    SM_ASSERT_EQ_DBG(Exception, c.size(), 6, "The curve value is an unexpected size!");
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.topLeftCorner<3, 3>() = rotation_->parametersToRotationMatrix(c.tail<3>());
    T.topRightCorner<3, 1>() = c.head<3>();

    return T;
}

Eigen::VectorXd BSplinePose::transformationToCurveValue(const Eigen::Matrix4d& T) const {
    Eigen::VectorXd c(6);
    c.head<3>() = T.topRightCorner<3, 1>();
    c.tail<3>() = rotation_->rotationMatrixToParameters(T.topLeftCorner<3, 3>());

    return c;
}

RotationalKinematics::Ptr BSplinePose::rotation() const { return rotation_; }

Eigen::Matrix4d BSplinePose::curveValueToTransformationAndJacobian(const Eigen::VectorXd& p, Eigen::MatrixXd* J) const {
    SM_ASSERT_EQ_DBG(Exception, p.size(), 6, "The curve value is an unexpected size!");
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d S;
    T.topLeftCorner<3, 3>() = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);
    T.topRightCorner<3, 1>() = p.head<3>();

    if (J) {
        *J = Eigen::MatrixXd::Identity(6, 6);
        J->topRightCorner<3, 3>() = -crossMx(p.head<3>()) * S;
        J->bottomRightCorner<3, 3>() = S;
    }

    return T;
}


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

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

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

using namespace std;
using namespace Eigen;
using namespace KDL;

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

	ros::init(argc, argv, "collision_avoidance");	// ROS node
	ros::NodeHandle nh;

	string relative_path_file_in;
	string file_name_in;
	string relative_path_file_out;
	string file_name_out;

	nh.param<std::string>("file_name_in", relative_path_file_in, "" );
	nh.param<std::string>("file_name_out", relative_path_file_out, "" );

	////////////////////////////////////// load the data_base /////////////////////////////////////
	std::string path = ros::package::getPath("grasp_learning");

	file_name_in = path + relative_path_file_in;
	ifstream file_in(file_name_in);

	std::cout << "file: " << file_name_in.c_str() << " is " << (file_in.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_in.is_open())
	return 0;

  	file_name_out = path + relative_path_file_out;

  	ofstream f_output; //output file
	std::string name = "box_discr_angle.csv";
  	f_output.open( file_name_out + name, ofstream::app);

  	////////////////////////////////////////////////////////////////////////////
	///////////////////// take a first pose ////////////////////////////////////
  	std::string line;
  	getline( file_in, line, '\n' );

	std::vector<double> values_inline;
    std::istringstream iss_line(line);
    for(std::string value; getline(iss_line, value, ',' ); )
    	values_inline.push_back(stod(value));
    //////////////////////////////////////////////////////////////////////////////////

   double x_box = values_inline[0];
   double y_box = values_inline[1];
   double z_box = values_inline[2];

   double x = values_inline[3];
   double y = values_inline[4];
   double z = values_inline[5];

   double qx = values_inline[6];
   double qy = values_inline[7];
   double qz = values_inline[8];
   double qw = values_inline[9];

   	KDL::Vector t_point(x,y,z);
    KDL::Rotation R_point= Rotation::Quaternion(qx,qy,qz,qw);

    KDL::Frame T_point(R_point,t_point);

    KDL::Vector t(x,y,z);
    KDL::Vector t_app(x,y,z);

    for(int i = 0 ; i < 360; i++)
    {

    	R_point.DoRotZ(i);

    	double qx_ = 0;
   		double qy_ = 0;
   		double qz_ = 0;
   		double qw_ = 0;

   		R_point.GetQuaternion(qx_,qy_,qz_,qw_);

    	f_output << x_box << ',' << y_box << ',' << z_box << ',' << t.x() << ',' << t.y() << ',' << t.z() << ',' << qx_ << ',' << qy_ << ',' << qz_ << ',' << qw_ << endl;

    }

}


//  (C) Copyright Gennadiy Rozental 2011-2015.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
//! @file
//! @brief tests random dataset
// ***************************************************************************

// Boost.Test
#include <boost/test/unit_test.hpp>
#include <boost/test/data/monomorphic/generators/random.hpp>
#include <boost/test/data/monomorphic/zip.hpp>
#include <boost/test/data/monomorphic/array.hpp>
#include <boost/test/data/monomorphic/grid.hpp>
namespace data=boost::unit_test::data;

#include "datasets-test.hpp"

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test_default )
{
    BOOST_TEST( data::random().size() == data::BOOST_TEST_DS_INFINITE_SIZE );

    auto ds = data::random();

    BOOST_CHECK_THROW( data::for_each_sample( ds, [](double ) {}), std::logic_error );

    invocation_count ic;

    ic.m_value = 0;
    data::for_each_sample( ds, ic, 10 );
    BOOST_TEST( ic.m_value == 10 );

    ic.m_value = 0;
    int arr[] = {1,2,3,4,5};
    data::for_each_sample( ds^arr, ic );
    BOOST_TEST( ic.m_value == 5 );

    BOOST_CHECK_THROW( ds * arr, std::logic_error );
    BOOST_CHECK_THROW( arr * ds, std::logic_error );
}

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test_uniform_range )
{
    auto ds1 = data::random(1,5);

    data::for_each_sample( ds1, [](int s) {
        BOOST_TEST(s>=1);
        BOOST_TEST(s<=5);
    }, 10);

    auto ds2 = data::random(1.,2.);

    data::for_each_sample( ds2, [](double s) {
        BOOST_TEST(s>=1.);
        BOOST_TEST(s<=2.);
    }, 100);
}

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test_parameterized_init )
{
    auto ds1 = data::random(data::distribution = std::normal_distribution<>(5.,2));
    typedef decltype(ds1) DS1;

    BOOST_TEST(( std::is_same<DS1::generator_type::distr_type,
                                std::normal_distribution<>>::value ));
    BOOST_TEST(( std::is_same<DS1::generator_type::data_type,double>::value ));
    BOOST_TEST(( std::is_same<DS1::generator_type::engine_type,
                                std::default_random_engine>::value ));

    auto ds2 = data::random(data::distribution = std::discrete_distribution<>());
    typedef decltype(ds2) DS2;

    BOOST_TEST(( std::is_same<DS2::generator_type::distr_type,
                                std::discrete_distribution<>>::value ));
    BOOST_TEST(( std::is_same<DS2::generator_type::data_type,int>::value ));
    BOOST_TEST(( std::is_same<DS2::generator_type::engine_type,
                                std::default_random_engine>::value ));

    auto ds3 = data::random(data::engine = std::minstd_rand());
    typedef decltype(ds3) DS3;

    BOOST_TEST(( std::is_same<DS3::generator_type::distr_type,
                                std::uniform_real_distribution<>>::value ));
    BOOST_TEST(( std::is_same<DS3::generator_type::data_type,double>::value ));
    BOOST_TEST(( std::is_same<DS3::generator_type::engine_type,
                                std::minstd_rand>::value ));

    auto ds4 = data::random(data::seed = 100UL);
    typedef decltype(ds4) DS4;

    BOOST_TEST(( std::is_same<DS4::generator_type::distr_type,
                                std::uniform_real_distribution<>>::value ));
    BOOST_TEST(( std::is_same<DS4::generator_type::data_type,double>::value ));
    BOOST_TEST(( std::is_same<DS4::generator_type::engine_type,
                                std::default_random_engine>::value ));

    auto ds5 = data::random(data::seed = 100UL);

    std::list<double> vals;
    data::for_each_sample( ds4, [&vals](double s) {
        vals.push_back( s );
    }, 10);
    data::for_each_sample( ds5, [&vals](double s) {
        BOOST_TEST( vals.front() == s );
        vals.pop_front();
    }, 10);

    auto ds6 = data::random(( data::engine = std::minstd_rand(),
                              data::distribution = std::normal_distribution<>(5.,2),
                              data::seed = 20UL ));
    typedef decltype(ds6) DS6;

    BOOST_TEST(( std::is_same<DS6::generator_type::distr_type,
                                std::normal_distribution<>>::value ));
    BOOST_TEST(( std::is_same<DS6::generator_type::data_type,double>::value ));
    BOOST_TEST(( std::is_same<DS6::generator_type::engine_type,
                                std::minstd_rand>::value ));
}

//____________________________________________________________________________//

// EOF


#include "optimizer.hpp"

#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

void init_optimizer(py::module& main)
{
    auto m  = main.def_submodule("optimizers");
    m.doc() = R"doc(
        Optimizers
        -----------------------
    )doc";

    py::class_<SgdOptimizer>(m, "SgdOptimizer")
        .def(py::init<Real>(), py::arg("learning_rate") = 0.1);

    py::class_<AdamOptimizer, SgdOptimizer>(m, "AdamOptimizer")
        .def(py::init<std::size_t, Real, Real, Real, Real>(),
             py::arg("n_parameters"),
             py::arg("alpha")   = 0.001,
             py::arg("beta1")   = 0.9,
             py::arg("beta2")   = 0.999,
             py::arg("epsilon") = 1e-8);
}


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file portfolio/builders/commodityforward.hpp
    \brief Engine builder for commodity forward
    \ingroup builders
*/

#pragma once

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/cachingenginebuilder.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <qle/pricingengines/discountingcommodityforwardengine.hpp>

namespace ore {
namespace data {

//! Engine builder for commodity forward
/*! Pricing engines are cached by currency
    \ingroup builders
 */
class CommodityForwardEngineBuilder
    : public CachingPricingEngineBuilder<std::string, const QuantLib::Currency&> {
public:
    CommodityForwardEngineBuilder()
        : CachingEngineBuilder("DiscountedCashflows", "DiscountingCommodityForwardEngine", {"CommodityForward"}) {}

protected:
    virtual std::string keyImpl(const QuantLib::Currency& ccy) override {
        return ccy.code();
    }

    virtual boost::shared_ptr<QuantLib::PricingEngine> engineImpl(const QuantLib::Currency& ccy) override {
        return boost::make_shared<QuantExt::DiscountingCommodityForwardEngine>(
            market_->discountCurve(ccy.code(), configuration(MarketContext::pricing)));
    }
};

} // namespace data
} // namespace ore