#include <cmath>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include "constants.hpp"
#include "fastmath.hpp"
#include "logger.hpp"
#include "nlopt/nlopt.h"
#include "shredder.hpp"

double shredder_opt_objective(unsigned int _n, const double* _x,
                              double* _grad, void* data)
{
    UNUSED(_n);

    Shredder* sampler = reinterpret_cast<Shredder*>(data);
    if (_grad) {
        double fx = sampler->f(_x[0], _grad[0]);
        _grad[0] = std::max<double>(std::min<double>(_grad[0], 1e4), -1e4);
        return fx;
    }
    else {
        double d;
        return sampler->f(_x[0], d);
    }
}

static void assert_finite(double x)
{
    if (!boost::math::isfinite(x)) {
        Logger::abort("%f found where finite value expected.", x);
    }
}

Shredder::Shredder(double lower_limit, double upper_limit, double tolerance)
    : lower_limit(lower_limit)
    , upper_limit(upper_limit)
    , tolerance(tolerance)
    , opt(NULL)
{
    opt = nlopt_create(NLOPT_LD_SLSQP, 1);
    nlopt_set_lower_bounds(opt, &lower_limit);
    nlopt_set_upper_bounds(opt, &upper_limit);
    nlopt_set_max_objective(opt, shredder_opt_objective,
            reinterpret_cast<void*>(this));
    nlopt_set_maxeval(opt, 20);

    nlopt_set_ftol_abs(opt, 1e-7);
    nlopt_set_xtol_abs(opt, &tolerance);
}

Shredder::~Shredder()
{
    nlopt_destroy(opt);
}

void Shredder::set_tolerance(double tolerance)
{
    this->tolerance = tolerance;
    nlopt_set_xtol_abs(opt, &this->tolerance);
}

double Shredder::sample(rng_t& rng, double x0)
{
    double d0;
    double lp0 = f(x0, d0);

    assert_finite(lp0);

    double slice_height = fastlog(
            std::max<double>(constants::zero_eps, random_uniform_01(rng))) + lp0;
    assert_finite(slice_height);

    x_min = find_slice_edge(x0, slice_height, lp0, d0, -1);
    x_max = find_slice_edge(x0, slice_height, lp0, d0,  1);

    double x = (x_max + x_min) / 2;
    while (x_max - x_min > tolerance) {
        x = x_min + (x_max - x_min) * random_uniform_01(rng);
        double d;
        double lp = f(x, d);

        if (lp >= slice_height) break;
        else if (x > x0) x_max = x;
        else             x_min = x;
    }

    return x;
}

double Shredder::optimize(double x0)
{
    // bounds may have changed
    nlopt_set_lower_bounds(opt, &lower_limit);
    nlopt_set_upper_bounds(opt, &upper_limit);

    x0 = std::max<double>(std::min<double>(x0, upper_limit), lower_limit);
    //x0 = (lower_limit + upper_limit) / 2;
    double maxf;
    nlopt_result result = nlopt_optimize(opt, &x0, &maxf);

    if (result < 0 && result != NLOPT_ROUNDOFF_LIMITED) {
        Logger::warn("Optimization failed with code %d", (int) result);
    }

    return std::max<double>(std::min<double>(x0, upper_limit), lower_limit);
}

double Shredder::find_slice_edge(double x0, double slice_height,
                                 double lp0, double d0, int direction)
{
    const double lp_eps = 1e-2;
    const double d_eps  = 1e-3;

    // if newton method iterations are not making progress, resort to bisection
    size_t newton_count = 0;

    double lp = lp0 - slice_height;
    double d = d0;
    double x = x0;
    double x_bound_lower, x_bound_upper;

    if (direction < 0) {
        x_bound_lower = lower_limit;
        x_bound_upper = x0;
        double fx = f(lower_limit, d);
        if (boost::math::isfinite(fx) && fx >= slice_height) {
            return lower_limit;
        }
    }
    else {
        x_bound_lower = x0;
        x_bound_upper = upper_limit;
        double fx = f(upper_limit, d);
        if (boost::math::isfinite(fx) && fx >= slice_height) {
            return upper_limit;
        }
    }

    while (fabs(lp) > lp_eps && fabs(x_bound_upper - x_bound_lower) > tolerance) {
        double x1 = x - lp / d;
        if (isnan(d) || d == 0.0 || fabs(d) < d_eps || !boost::math::isfinite(x1)) {
            x1 = (x_bound_lower + x_bound_upper) / 2;
        }

        // if we are very close to the boundry, and this iteration moves us past
        // the boundry, just give up.
        if (direction < 0 && fabs(x - lower_limit) <= tolerance && (x1 < x || lp > 0.0)) break;
        if (direction > 0 && fabs(x - upper_limit) <= tolerance && (x1 > x || lp > 0.0)) break;

        // if we are moving in the wrong direction (i.e. toward the other root),
        // use bisection to correct course.
        if (direction < 0) {
            if (lp > 0) x_bound_upper = x;
            else        x_bound_lower = x;
        }
        else {
            if (lp > 0) x_bound_lower = x;
            else        x_bound_upper = x;
        }

        bool bisect = newton_count >= constants::max_newton_steps ||
            x1 < x_bound_lower + tolerance || x1 > x_bound_upper - tolerance;

        // try using the gradient
        if (!bisect) {
            x = x1;
            lp = f(x, d) - slice_height;
            bisect = !boost::math::isfinite(lp) || !boost::math::isfinite(d);
        }

        // resort to binary search if we seem not to be making progress
        if (bisect) {
            size_t iteration_count = 0;
            while (true) {
                x = (x_bound_lower + x_bound_upper) / 2;
                lp = f(x, d) - slice_height;

                //if (boost::math::isinf(lp) || boost::math::isinf(d)) {
                if (!boost::math::isfinite(lp)) {
                    if (direction < 0) x_bound_lower = x;
                    else               x_bound_upper = x;
                }
                else break;

                if (++iteration_count > 50) {
                    Logger::abort("Slice sampler edge finding is not making progress.");
                }
            }
    }
    else ++newton_count;

        assert_finite(lp);
    }

    assert_finite(x);

    return x;
}

static double sq(double x)
{
    return x * x;
}

static double cb(double x)
{
    return x * x * x;
}

static double lbeta(double x, double y)
{
    return lgamma(x) + lgamma(y) - lgamma(x + y);
}

#if 0
double PoissonLogPdf::f(float lambda, unsigned int k)
{
    return k * fastlog(lambda) - lgammaf(k + 1) - lambda;
}

double PoissonLogPdf::df_dlambda(float lambda, unsigned int k)
{
    return (float) k / lambda - 1;
}
#endif

static const double NEG_LOG_2_PI_DIV_2 = -log(2 * M_PI)/2;

double NormalLogPdf::f(double mu, double sigma, const double* xs, size_t n)
{
    double part1 = n * (NEG_LOG_2_PI_DIV_2 - fastlog(sigma));
    double part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part2 += sq(xs[i] - mu) / (2 * sq(sigma));
    }

    return part1 - part2;
}

float NormalLogPdf::f(float mu, float sigma, const float* xs, size_t n)
{
    float part1 = n * (NEG_LOG_2_PI_DIV_2 - fastlog(sigma));
    float part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part2 += sq(xs[i] - mu) / (2 * sq(sigma));
    }

    return part1 - part2;
}

double NormalLogPdf::f(double mu, double sigma, const double x)
{
    double part1 = NEG_LOG_2_PI_DIV_2 - fastlog(sigma);
    double part2 = sq(x - mu) / (2 * sq(sigma));
    return part1 - part2;
}

double NormalLogPdf::df_dx(double mu, double sigma, const double x)
{
    return (mu - x) / sq(sigma);
}

double NormalLogPdf::df_dx(double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += mu - xs[i];
    }
    return part / sq(sigma);
}

double NormalLogPdf::df_dmu(double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += xs[i] - mu;
    }
    return part / sq(sigma);
}

float NormalLogPdf::df_dmu(float mu, float sigma, const float* xs, size_t n)
{
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += xs[i] - mu;
    }
    return part / sq(sigma);
}

double NormalLogPdf::df_dsigma(double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += sq(xs[i] - mu);
    }

    return part / cb(sigma) - n/sigma;
}

float NormalLogPdf::df_dsigma(float mu, float sigma, const float* xs, size_t n)
{
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += sq(xs[i] - mu);
    }

    return part / cb(sigma) - n/sigma;
}

double LogNormalLogPdf::f(double mu, double sigma, const double* xs, size_t n)
{
    double part1 = n * (NEG_LOG_2_PI_DIV_2 - fastlog(sigma));
    double part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        double logx = fastlog(xs[i]);
        part2 += sq(logx - mu) / (2 * sq(sigma)) + logx;
    }

    return part1 - part2;
}

double LogNormalLogPdf::df_dx(double mu, double sigma, double x)
{
    return (mu - fastlog(x)) / (x * sq(sigma)) - 1.0 / x;
}

double LogNormalLogPdf::df_dmu(double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += fastlog(xs[i]) - mu;
    }
    return part / sq(sigma);
}

double LogNormalLogPdf::df_dsigma(double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += sq(fastlog(xs[i]) - mu);
    }

    return part / cb(sigma) - n/sigma;
}

#if 0
double StudentsTLogPdf::f(double nu, double mu, double sigma, const double* xs, size_t n)
{
    double part1 =
        n * (lgamma((nu + 1) / 2) - lgamma(nu / 2) - fastlog(sqrt(nu * M_PI) * sigma));

    double part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part2 += log1p(sq((xs[i] - mu) / sigma) / nu);
    }

    return part1 - ((nu + 1) / 2) * part2;
}

float StudentsTLogPdf::f(float nu, float mu, float sigma, const float* xs, size_t n)
{
    float part1 =
        n * (lgammaf((nu + 1) / 2) - lgamma(nu / 2) - fastlog(sqrt(nu * M_PI) * sigma));

    float part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part2 += log1p(sq((xs[i] - mu) / sigma) / nu);
    }

    return part1 - ((nu + 1) / 2) * part2;
}

double StudentsTLogPdf::df_dx(double nu, double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (2 * (xs[i] - mu) / sq(sigma) / nu) / (1 + sq((xs[i] - mu) / sigma) / nu);
    }

    return -((nu + 1) / 2) * part;
}

double StudentsTLogPdf::df_dmu(double nu, double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (2 * (xs[i] - mu) / sq(sigma) / nu) / (1 + sq((xs[i] - mu) / sigma) / nu);
    }

    return ((nu + 1) / 2) * part;
}

float StudentsTLogPdf::df_dmu(float nu, float mu, float sigma, const float* xs, size_t n)
{
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (2 * (xs[i] - mu) / sq(sigma) / nu) / (1 + sq((xs[i] - mu) / sigma) / nu);
    }

    return ((nu + 1) / 2) * part;
}

double StudentsTLogPdf::df_dsigma(double nu, double mu, double sigma, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (2 * sq((xs[i] - mu) / sigma) / (nu * sigma)) /
                    (1 + sq((xs[i] - mu) / sigma) / nu);
    }

    return ((nu + 1) / 2) * part - n / sigma;
}
#endif

#if 0
double GammaLogPdf::f(double alpha, double beta, const double* xs, size_t n)
{
    double part1 = 0.0, part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part1 += fastlog(xs[i]);
        part2 += xs[i];
    }

    return
        n * (alpha * fastlog(beta) - lgamma(alpha)) +
        (alpha - 1) * part1 -
        beta * part2;
}

float GammaLogPdf::f(float alpha, float beta, const float* xs, size_t n)
{
    float part1 = 0.0, part2 = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part1 += fastlog(xs[i]);
        part2 += xs[i];
    }

    return
        n * (alpha * fastlog(beta) - lgammaf(alpha)) +
        (alpha - 1) * part1 -
        beta * part2;
}

double GammaLogPdf::df_dx(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (alpha - 1) / xs[i];
    }

    return part - n * beta;
}

double GammaLogPdf::df_dalpha(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += fastlog(xs[i]);
    }

    return n * (fastlog(beta) - boost::math::digamma(alpha)) + part;
}

double GammaLogPdf::df_dbeta(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += xs[i];
    }

    return (double) n * (alpha / beta) - part;
}

float GammaLogPdf::df_dbeta(float alpha, float beta, const float* xs, size_t n)
{
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += xs[i];
    }

    return (float) n * (alpha / beta) - part;
}
#endif

#if 0
double AltGammaLogPdf::f(double mean, double shape, const double* xs, size_t n)
{
    double scale =  mean / shape;
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (shape - 1.0) * fastlog(xs[i]) - xs[i] / scale;
    }

    double ans = -(double) n * (lgamma(shape) + shape * fastlog(scale)) + part;
    assert_finite(ans);
    return ans;
}

float AltGammaLogPdf::f(float mean, float shape, const float* xs, size_t n)
{
    float scale =  mean / shape;
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (shape - 1.0) * fastlog(xs[i]) - xs[i] / scale;
    }

    float ans = -(float) n * (lgammaf(shape) + shape * fastlog(scale)) + part;
    assert_finite(ans);
    return ans;
}

double AltGammaLogPdf::f(double mean, double shape, double x)
{
    double scale =  mean / shape;
    double part = (shape - 1.0) * fastlog(x) - x / scale;
    return -(lgamma(shape) + shape * fastlog(scale)) + part;
}

float AltGammaLogPdf::f(float mean, float shape, float x)
{
    float scale =  mean / shape;
    float part = (shape - 1.0) * fastlog(x) - x / scale;
    return -(lgammaf(shape) + shape * fastlog(scale)) + part;
}

double AltGammaLogPdf::df_dx(double mean, double shape, const double* xs, size_t n)
{
    double scale = mean / shape;
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (shape - 1.0) / xs[i];
    }

    return part - (double) n / scale;
}

double AltGammaLogPdf::df_dx(double mean, double shape, double x)
{
    double scale = mean / shape;
    double part = (shape - 1.0) / x;
    return part - 1.0 / scale;
}

double AltGammaLogPdf::df_dmean(double mean, double shape, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += xs[i];
    }
    part *= shape / sq(mean);
    return part - (double) n * shape / mean;
}

float AltGammaLogPdf::df_dmean(float mean, float shape, const float* xs, size_t n)
{
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += xs[i];
    }
    part *= shape / sq(mean);
    return part - (float) n * shape / mean;
}

double AltGammaLogPdf::df_dshape(double mean, double shape, const double* xs, size_t n)
{
    double scale = mean / shape;
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += fastlog(xs[i]) - xs[i] / mean;
    }

    return (double) n * (-boost::math::digamma(shape) + fastlog(scale) * (mean/sq(shape))) + part;
}

float AltGammaLogPdf::df_dshape(float mean, float shape, const float* xs, size_t n)
{
    float scale = mean / shape;
    float part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += fastlog(xs[i]) - xs[i] / mean;
    }

    return (float) n * (-boost::math::digamma(shape) + fastlog(scale) * (mean/sq(shape))) + part;
}

float AltGammaLogPdf::df_dshape(float mean, float shape, float x)
{
    float scale = mean / shape;
    float part = fastlog(x) - x / mean;
    return (-boost::math::digamma(shape) + fastlog(scale) * (mean/sq(shape))) + part;
}
#endif

double InvGammaLogPdf::f(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += (alpha + 1) * fastlog(xs[i]) + beta / xs[i];
    }

    return n * (alpha * fastlog(beta) - lgamma(alpha)) - part;
}

double InvGammaLogPdf::df_dx(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += beta / sq(xs[i]) - (alpha + 1) / xs[i];
    }

    return part;
}

double InvGammaLogPdf::df_dalpha(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += fastlog(xs[i]);
    }

    return n * (fastlog(beta) - boost::math::digamma(alpha)) - part;
}

double InvGammaLogPdf::df_dbeta(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += 1 / xs[i];
    }

    return n * (alpha / beta) - part;
}

double SqInvGammaLogPdf::f(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        double x = xs[i] * xs[i];
        part += (alpha + 1) * fastlog(x) + beta / x;
    }

    return n * (alpha * fastlog(beta) - lgamma(alpha)) - part;
}

double SqInvGammaLogPdf::df_dx(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        part += 2 * beta / cb(xs[i]) - (2 * alpha + 2) / xs[i];
    }

    return part;
}

double SqInvGammaLogPdf::df_dalpha(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        double x = xs[i] * xs[i];
        part += fastlog(x);
    }

    return n * (fastlog(beta) - boost::math::digamma(alpha)) - part;
}

double SqInvGammaLogPdf::df_dbeta(double alpha, double beta, const double* xs, size_t n)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        double x = xs[i] * xs[i];
        part += 1 / x;
    }

    return n * (alpha / beta) - part;
}

double BetaLogPdf::f(double alpha, double beta, double x)
{
    return (alpha - 1) * fastlog(x) + (beta - 1) * fastlog(1 - x) - lbeta(alpha, beta);
}

double BetaLogPdf::df_dx(double alpha, double beta, double x)
{
    return (alpha - 1) / x - (beta - 1) / (1 - x);
}

double BetaLogPdf::df_dgamma(double gamma, double c, double x)
{
    return c * (fastlog(x / (1 - x)) -
                boost::math::digamma(gamma * c) +
                boost::math::digamma((1 - gamma) * c));
}

double DirichletLogPdf::f(double alpha,
                          const boost::numeric::ublas::matrix<double>* mean,
                          const boost::numeric::ublas::matrix<double>* data,
                          size_t n, size_t m)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        for (size_t j = 0; j < m; ++j) {
            double am = alpha * (*mean)(i, j);
            part += (am - 1) * fastlog((*data)(i, j)) - lgamma(am);
        }
    }

    return n * lgamma(alpha) + part;
}

double DirichletLogPdf::df_dalpha(double alpha,
                                  const boost::numeric::ublas::matrix<double>* mean,
                                  const boost::numeric::ublas::matrix<double>* data,
                                  size_t n, size_t m)
{
    double part = 0.0;
    for (size_t i = 0; i < n; ++i) {
        for (size_t j = 0; j < m; ++j) {
            part += (*mean)(i, j) * (fastlog((*data)(i, j)) -
                                     boost::math::digamma(alpha * (*mean)(i, j)));
        }
    }

    return n * boost::math::digamma(alpha) + part;
}

double LogisticNormalLogPdf::f(double mu, double sigma, double x)
{
    return -fastlog(sigma) - fastlog(sqrt(2*M_PI)) -
           sq(fastlog(x / (1 - x)) - mu) / (2 * sq(sigma)) -
           fastlog(x) - fastlog(1-x);
}

double LogisticNormalLogPdf::df_dx(double mu, double sigma, double x)
{
    double y = fastlog(x / (1 - x));
    return (1/(1-x)) - (1/x) - (mu - y) / (sq(sigma) * (x - 1) * x);
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Seho Shin */

#include "ompl/base/spaces/ClothoidStateSpace.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Exception.h"
#include <queue>
#include <boost/math/constants/constants.hpp>
#include "ompl/tools/config/MagicConstants.h"
#include <cstring>

using namespace std;
using namespace ompl::base;

namespace
{
    const double twopi = 2. * boost::math::constants::pi<double>();
    const double pi = boost::math::constants::pi<double>();

	const double CLOTHOID_ZERO = -1e-9;

	//[0~2pi]
	double conv2pi(double x){
		x = fmod(x,twopi);
		if (x < 0)
			x += twopi;
		return x;
	}

	//[-pi,pi]
	double convpi(double x){
		x = fmod(x + pi,twopi);
		if (x < 0)
			x += twopi;
		return x - pi;
	}
	inline double mod2pi(double x)
    {
        if (x<0 && x>CLOTHOID_ZERO) return 0;
        return x - twopi * floor(x / twopi);
    }
};

ompl::base::State* ompl::base::ClothoidStateSpace::allocState() const
{
    StateType *state = new StateType();
    allocStateComponents(state);
    return state;
}

void ompl::base::ClothoidStateSpace::freeState(State *state) const
{
    CompoundStateSpace::freeState(state);
}

void ompl::base::ClothoidStateSpace::registerProjections()
{
    class ClothoidDefaultProjection : public ProjectionEvaluator
    {
    public:

        ClothoidDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
        {
        }

        virtual unsigned int getDimension() const
        {
            return 2;

        }

        virtual void defaultCellSizes()
        {
            cellSizes_.resize(2);
            bounds_ = space_->as<ClothoidStateSpace>()->getBounds();
            cellSizes_[0] = (bounds_.high[0] - bounds_.low[0]) / magic::PROJECTION_DIMENSION_SPLITS;
            cellSizes_[1] = (bounds_.high[1] - bounds_.low[1]) / magic::PROJECTION_DIMENSION_SPLITS;
            //cellSizes_[2] = (bounds_.high[2] - bounds_.low[2]) / magic::PROJECTION_DIMENSION_SPLITS;
        }

        virtual void project(const State *state, EuclideanProjection &projection) const
        {
            memcpy(&projection(0),
					state->as<ClothoidStateSpace::StateType>()->as<RealVectorStateSpace::StateType>(0)->values,
					2* sizeof(double));
        }
    };

    registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new
					ClothoidDefaultProjection(this))));
}
double ompl::base::ClothoidStateSpace::distance(const State *state1, const State *state2) const
{
	double W0 = weight_[0];
	double W1 = weight_[1];
	double W2 = weight_[2];

	const StateType *s1 = static_cast<const StateType*>(state1);
    const StateType *s2 = static_cast<const StateType*>(state2);
    double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
    double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();

	if( x1 == x2 && y1 == y2 && th1 == th2 )
		return 0.;

    if( !CheckRange(s1, s2) )
        return 99999.0;

    if (isReversable_)
	{
		ClothoidPath pathForward = clothoid(state1, state2);
		ClothoidPath pathReverse = clothoid(state2, state1);

		//When searching in the forward direction, it should be compared with the
		//	curvature of the first node of the candidate clothoid.
		double clothoid_k = pathForward.k_;
		double clothoid_l = pathForward.length();

		double costF =
			W0*clothoid_l + W1*fabs(s1->getK()-clothoid_k);

		//When searching in the reverse direction, it should be compared with
		//the curvature of the last node of the candidate clothoid.
		clothoid_k = pathReverse.k_+pathReverse.dk_*pathReverse.l_;
		clothoid_l = pathReverse.length();

		double costR =
			W2*(W0*clothoid_l + W1*fabs(s1->getK()-clothoid_k));

		return std::min(costF, costR);
	}
	else
	{
		ClothoidPath path = clothoid(state1, state2);
		return W0*path.length()+W1*fabs(s1->getK()-path.k_);
	}
}

void ompl::base::ClothoidStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    bool firstTime = true;
	Clothoid::ClothoidCurve cc;
    ClothoidPath path(cc);
    interpolate(from, to, t, firstTime, path, state);
}

void ompl::base::ClothoidStateSpace::interpolate(const State *from, const State *to, const double t,
    bool &firstTime, ClothoidPath &path, State *state) const
{
    if (firstTime)
    {
        if (t>=1.)
        {
            if (to != state)
                copyState(state, to);
            return;
        }
        if (t<=0.)
        {
            if (from != state)
                copyState(state, from);
            return;
        }

        path = clothoid(from, to);
        if (isReversable_) // should be true in vehicle case - commented by shinsh
        {
			double W0 = weight_[0];
			double W1 = weight_[1];
			double W2 = weight_[2];
            ClothoidPath path2(clothoid(to, from));

			const StateType *s1 = static_cast<const StateType*>(from);

			double costF =
				W0*path.length()+W1*fabs(s1->getK()-path.k_);

		    double rev_k = path2.k_+path2.dk_*path2.l_;
            double costR =
				(W0*path2.length()+W1*fabs(s1->getK()-rev_k))*W2;

			if (costR < costF)
            {
                path2.reverse_ = true;
                path = path2;
            }
        }
        firstTime = false;
    }
	//const StateType *s1 = static_cast<const StateType*>(from);
	//const StateType *s2 = static_cast<const StateType*>(to);
	//cout << s1->getX() << " " << s1->getY() << " " <<s1->getYaw()<< endl;
	//cout << s2->getX() << " " << s2->getY() << " " <<s2->getYaw()<< endl;
    interpolate(from, path, t, state);
}

void ompl::base::ClothoidStateSpace::interpolate(const State *from, const
		ClothoidPath &path, double t, State *state) const
{
    StateType *s = allocState()->as<StateType>();
	double seg = t * path.length();

	Clothoid::valueType theta;
	Clothoid::valueType kappa;
	Clothoid::valueType x;
	Clothoid::valueType y;

    if (path.reverse_)
	{
		seg = path.length()-seg;
		path.cc_.eval(seg, theta, kappa, x, y);
	   // state->as<StateType>()->setYaw(conv2pi(conv2pi(theta)-pi));
    }
    else
    {
		path.cc_.eval(seg, theta, kappa, x, y);
    }
    state->as<StateType>()->setX(x);
    state->as<StateType>()->setY(y);
    getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
    state->as<StateType>()->setYaw(theta);
    state->as<StateType>()->setK(kappa);
    freeState(s);
	//cout << x << " " << y << " " <<theta << endl;
	//getchar();
}

ompl::base::ClothoidStateSpace::ClothoidPath
ompl::base::ClothoidStateSpace::clothoid(const State *state1, const State *state2) const
{

	Clothoid::ClothoidCurve cc;

    const StateType *s1 = static_cast<const StateType*>(state1);
    const StateType *s2 = static_cast<const StateType*>(state2);
    double x1 = s1->getX(), y1 = s1->getY(), th1 = convpi(s1->getYaw());
    double x2 = s2->getX(), y2 = s2->getY(), th2 = convpi(s2->getYaw());

//	cout << x1 << " " << y1 << " " << th1 << " " << x2 <<" " << y2 << " " <<
//		th2 << endl;
	cc.setup_G1(x1, y1, th1, x2, y2, th2);

	ompl::base::ClothoidStateSpace::ClothoidPath path(cc);

    return path;
}

bool ompl::base::ClothoidStateSpace::CheckRange(const State *state1, const
        State *state2) const
{
     const StateType *s1 = static_cast<const StateType*>(state1);
    const StateType *s2 = static_cast<const StateType*>(state2);
    double x1 = s1->getX(), y1 = s1->getY();
    double x2 = s2->getX(), y2 = s2->getY();

    if( sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)) < 1.0 )
    {
        return false;
    }
    else
    {
        return true;
    }

}

void ompl::base::ClothoidMotionValidator::defaultSettings()
{
    stateSpace_ = dynamic_cast<ClothoidStateSpace*>(si_->getStateSpace().get());
    if (!stateSpace_)
        throw Exception("No state space for motion validator");
}

bool ompl::base::ClothoidMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
{
   /* assume motion starts in a valid configuration so s1 is valid */

    if( !stateSpace_->CheckRange(s1, s2) )
        return false;

    bool result = true, firstTime = true;
	Clothoid::ClothoidCurve cc;
    ClothoidStateSpace::ClothoidPath path(cc);
    int nd = stateSpace_->validSegmentCount(s1, s2);

    nd = 30;
    if (nd > 1)
    {
        /* temporary storage for the checked state */
        State *test = si_->allocState();

        for (int j = 1 ; j < nd ; ++j)
        {
            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
            if (!si_->isValid(test))
            {
                lastValid.second = (double)(j - 1) / (double)nd;
                if (lastValid.first)
                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
                result = false;
                break;
            }
        }
        si_->freeState(test);
    }

    if (result)
        if (!si_->isValid(s2))
        {
            lastValid.second = (double)(nd - 1) / (double)nd;
            if (lastValid.first)
                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
            result = false;
        }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}

bool ompl::base::ClothoidMotionValidator::checkMotion(const State *s1, const State *s2) const
{
    /* assume motion starts in a valid configuration so s1 is valid */
    if( !stateSpace_->CheckRange(s1, s2) )
        return false;

   // if (!si_->isValid(s2))
   //     return false;

    bool result = true, firstTime = true;
    Clothoid::ClothoidCurve cc;
    ClothoidStateSpace::ClothoidPath path(cc);
    int nd = stateSpace_->validSegmentCount(s1, s2);

    nd = 30;
    /* initialize the queue of test positions */
    std::queue< std::pair<int, int> > pos;
    if (nd >= 2)
    {
        pos.push(std::make_pair(1, nd - 1));

        /* temporary storage for the checked state */
        State *test = si_->allocState();

        /* repeatedly subdivide the path segment in the middle (and check the middle) */
        while (!pos.empty())
        {
            std::pair<int, int> x = pos.front();

            int mid = (x.first + x.second) / 2;
            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);

            if (!si_->isValid(test))
            {
                result = false;
                break;
            }

            pos.pop();

            if (x.first < mid)
                pos.push(std::make_pair(x.first, mid - 1));
            if (x.second > mid)
                pos.push(std::make_pair(mid + 1, x.second));
        }

        si_->freeState(test);
    }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}


#ifndef PYTHONIC_INCLUDE_NUMPY_CEIL_HPP
#define PYTHONIC_INCLUDE_NUMPY_CEIL_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/ceil.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> ceil(std::complex<T> const &val)
    {
      return std::ceil(val);
    }
    template <class T>
    auto ceil(T const &val) -> decltype(boost::simd::ceil(val))
    {
      return boost::simd::ceil(val);
    }
  }
#define NUMPY_NARY_FUNC_NAME ceil
#define NUMPY_NARY_FUNC_SYM wrapper::ceil
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


//  Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.
//  Copyright Toon Knapen

#ifndef BOOST_NUMERIC_BINDINGS_AMOS_AMOS_HPP
#define BOOST_NUMERIC_BINDINGS_AMOS_AMOS_HPP

#include <boost/numeric/bindings/amos/amos_overloads.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/vector_traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace amos {

  template < typename vector_type, typename value_type >
  int besi(const value_type&                                             z,   // std::complex< float > or std::complex< double >
           const typename traits::type_traits< value_type >::real_type   fnu, // float or double
           int                                                           kode,
           vector_type&                                                  cy,
           int&                                                          nz)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    int n = traits::vector_size( cy ) ;
    value_type * cy_ptr = traits::vector_storage( cy ) ;

    int error = 0 ;
    detail::besi( z, fnu, kode, n, cy_ptr, nz, error ) ;
    return error ;
  }

  template < typename vector_type, typename value_type >
  int besj(const value_type&                                            z,
           const typename traits::type_traits< value_type >::real_type  fnu,
           int                                                          kode,
           vector_type&                                                 cy,
           int&                                                         nz)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    int n = traits::vector_size( cy ) ;
    value_type * cy_ptr = traits::vector_storage( cy ) ;

    int error = 0 ;
    detail::besj( z, fnu, kode, n, cy_ptr, nz, error ) ;
    return error ;
  }

  template < typename vector_type, typename value_type >
  int besy(const value_type&                                           z,
           const typename traits::type_traits< value_type >::real_type fnu,
           int                                                         kode,
           vector_type&                                                cy,
           int&                                                        nz)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    int n = traits::vector_size( cy ) ;
    value_type * cy_ptr = traits::vector_storage( cy ) ;

    int error = 0 ;
    value_type * cwrk = new value_type[n];
    detail::besy( z, fnu, kode, n, cy_ptr, nz, cwrk, error ) ;
    delete[] cwrk ;
    return error ;
  }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_AMOS_AMOS_HPP


/*
* TS Elements
* Copyright 2015-2018 M. Newhouse
* Released under the MIT license.
*/

#pragma once

#include <cstdint>

#include "utility/vector2.hpp"
#include "utility/color.hpp"

#include <boost/container/small_vector.hpp>

namespace ts
{
  namespace world
  {
    class Car;
    class World;

    struct HandlingState
    {
      std::int8_t current_gear = 0;
      std::int8_t gear_shift_state = 0;
      double engine_rev_speed = 0.0;
      Vector2d net_force;

      struct WheelState
      {
        Vector2d pos;
        double speed;
        double slide_ratio;
        double terrain_roughness = 0.0;
        Colorb terrain_color;
      };

      boost::container::small_vector<WheelState, 4> wheel_states;
    };

    HandlingState update_car_state(Car& car, const World& world, double frame_duration);

    //HandlingState apply_physics_forces(Car& car, const TerrainMap& terrain_map,
    //                                   double frame_duration);
  }
}


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/ratio/ratio.hpp>

template <typename T, typename R>
struct S {
    T val;
};

boost::intmax_t func(S<int, boost::ratio<5,6> > const& s) {
    return s.val*3;
}

boost::intmax_t test() {
    return func(
            S<int, boost::ratio_add<
                boost::ratio<1,2>,
                boost::ratio<1,3>
            >
            //~ ::type
            >()
            );
}


#include "eucsearch.h"
#include "search.h"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <queue>

#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filter/gzip.hpp>

using namespace std;

void usage(const char* argv[])
{
	std::cerr << "Usage: " << argv[0]
		<<" db.record query.record <max number of results>"
		<< std::endl;
}
int eucsearch(const char* matrix,const char* queryMatrix,int n_results,std::ostream &ofs)
{

	int db_dim;
	std::vector<double> db_coord;
	std::vector<datatype> db_data;
	if (not read_file(matrix, db_dim, db_coord, db_data)) {
		std::cerr << "cannot process file " << matrix << std::endl;
	}

	int query_dim;
	std::vector<double> query_coord;
	std::vector<datatype> query_data;
	if (not read_file(queryMatrix, query_dim, query_coord, query_data)) {
		std::cerr << "cannot process file " << queryMatrix<< std::endl;
		return 1;
	}

	if (query_dim != db_dim) {
		std::cerr << "dimensions do not match: "
			<< query_dim << " != " << db_dim << std::endl;
		return 1;
	}

	for (size_t q_id = 0; q_id < query_data.size(); q_id ++) {
		std::cerr << "processing query " << q_id << "\r";
		std::vector<std::pair<double, datatype> > results;
		for (size_t db_id = 0; db_id < db_data.size(); db_id ++) {
			double d = distf(db_coord.begin() + db_id * db_dim,
				query_coord.begin() + q_id * query_dim, db_dim);
			std::pair<double, datatype> p(d, db_data[db_id]);
			results.push_back(p);
		}
		sort(results.begin(), results.end());

		if (n_results > results.size()) n_results = results.size();

		for (unsigned int i = 0; i < n_results; i ++)
			ofs << results[i].second << ":" << results[i].first << " ";
		ofs << std::endl;
	}

	return 0;
}
int eucsearch2file(const char* matrix,const char* queryMatrix,int n_results,char* outfile)
{
   std::fstream ofs;
   ofs.open(outfile,std::ios::out);
   return eucsearch(matrix,queryMatrix,n_results,ofs);
}
#ifndef NO_MAIN
int main(int argc, const char* argv[])
{
	if (argc != 4) {
		usage(argv);
		return 1;
	}

	int n_results = atoi(argv[3]);
	if (n_results <= 0) {
		usage(argv);
		return 1;
	}

   return eucsearch(argv[1],argv[2],n_results,cout);

}
#endif

// vim:tabstop=2:shiftwidth=2:smartindent


//-----------------------------------------------------------------------------
// Copyright (c) 2016 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _mitrax__uBLAS__convert__hpp_INCLUDED_
#define _mitrax__uBLAS__convert__hpp_INCLUDED_

#include <boost/numeric/ublas/matrix.hpp>

#include <mitrax/matrix.hpp>

namespace mitrax{

	namespace ublas = boost::numeric::ublas;

	template < typename T >
	constexpr auto convert(ublas::matrix< T > const& m){
		return make_matrix_fn(dim_pair(m.size1(), m.size2()), [&m](auto x, auto y){
			return m(x, y);
		});
	}

	template < typename M, col_t C, row_t R >
	constexpr auto convert(matrix< M, C, R > const& m){
		ublas::matrix< value_type_t< M > > res(size_t(m.cols()), size_t(m.rows()));

		for(size_t y = 0; y < m.rows(); ++y){
			for(size_t x = 0; x < m.cols(); ++x){
				res(x, y) = m(x, y);
			}
		}

		return res;
	}

}

#endif


/*
 *  pose.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2016-10-27
 *
 *  Class to store and make calculations on a robot arm,
 *  including the angle of the grippers.
 *
 */

#ifndef IROB_POSE_HPP_
#define IROB_POSE_HPP_

#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Transform.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/JointState.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <irob_msgs/ToolPose.h>
#include <irob_msgs/ToolPoseStamped.h>

namespace saf {

class Pose {

   public:

   	struct Distance {
    	double cartesian;
    	double angle;
    	double jaw;

    	Distance operator*=(double);
    	Distance operator/=(double);
    	Distance operator*(double) const;
    	Distance operator/(double) const;

    	friend std::ostream& operator<<(std::ostream&, const Distance&);
  	};

   	Eigen::Vector3d position;
   	Eigen::Quaternion<double> orientation;
   	double jaw;

   	Pose();
   	Pose(double, double, double, double, double, double, double, double);
   	Pose(const Pose&);
   	Pose(const irob_msgs::ToolPose&);
   	Pose(const irob_msgs::ToolPoseStamped&);
	Pose(const geometry_msgs::Pose&, double jaw);
	Pose(const geometry_msgs::PoseStamped&, double jaw);
   	Pose(const Eigen::Vector3d&, const Eigen::Quaternion<double>&, double);
    Pose(const geometry_msgs::Point&, const Eigen::Quaternion<double>&, double);

   	void swap(Pose&);
   	Pose operator=(const Pose&);

   	Pose operator+=(const Eigen::Vector3d&);
   	Pose operator-=(const Eigen::Vector3d&);

   	Pose operator+(const Eigen::Vector3d&) const;
   	Pose operator-(const Eigen::Vector3d&) const;

   	Pose interpolate(double, const Pose&) const;

   	Pose rotate(const Eigen::Matrix3d&) const;
   	Pose transform(const Eigen::Matrix3d&, const Eigen::Vector3d&, double = 1.0);
   	Pose invTransform(const Eigen::Matrix3d&, const Eigen::Vector3d&, double = 1.0);

    Pose transform(const geometry_msgs::Transform&, double = 1.0);
    Pose invTransform(const geometry_msgs::Transform&, double = 1.0);

   	bool isNaN() const;

   	Distance dist(const Pose&) const;
   	Distance dist(const Eigen::Vector3d&) const;
   	Distance dist(const Eigen::Quaternion<double>&) const;
   	Distance dist(double) const;

    irob_msgs::ToolPose toRosToolPose() const;
   	geometry_msgs::Pose toRosPose() const;
    sensor_msgs::JointState toRosJaw() const;

   	friend std::ostream& operator<<(std::ostream&, const Pose&);
	friend std::istream& operator>>(std::istream&, Pose&);
};
}
#endif


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/strong_ints.hpp>

#define JASEL_CONFIG_CONTRACT_VIOLATION_THROWS_V 1

#include <experimental/strong_bounded_int.hpp>
#include <sstream>

#include <boost/detail/lightweight_test.hpp>

namespace stdex = std::experimental;

using Frame = stdex::strong_bounded_int<class FrameTag, int, -1023, 1023>;
using FrameDC = stdex::strong_bounded_int<class FrameTag, int, -1023, 1023>;
using Slot = stdex::strong_bounded_int<class SlotTag, int, 0, 6>;

Frame add(Frame x, Frame y){
  return x+y;
}

int main()
{
  {
      //Frame fn1 = 1;     // error - explicit required
      Frame fn2{1};
      BOOST_TEST(fn2 == Frame{1});
      short s=1;
      Frame fn3 {s};
      BOOST_TEST(fn2 == Frame{1});

      //Frame fn4 {1.0};      // error narrowing conversion

      Slot sn1{1};
      (void)sn1;
      Frame fn5 {fn2};
      BOOST_TEST(fn5 == Frame{1});
      //Slot sn2{fn2}; // error

      //int i = fn2; // error

      auto fn6 = add(fn2, fn3);
      BOOST_TEST(fn6 == Frame{2});
      //auto fn7 = add(fn2, 1); // error
      BOOST_TEST(fn2 != fn6);
      BOOST_TEST(fn2 < fn6);

  }
  { // default constructor
    FrameDC oc;
    (void)oc;
    // not initialized
    //BOOST_TEST(oc == FrameDC{});
  }
  { // constructor from UT
    Frame oc{1};
    BOOST_TEST(oc == Frame{1});
  }
  { // constructor from implicitly convertible to UT
    short s=1;
    Frame oc{s};
    BOOST_TEST(oc == Frame{1});
  }
  { // constructor from invalid Int
    try
    {
      Slot oc{-1};
      BOOST_TEST(false);
    }
    catch (stdex::contract_failed& )
    {    }
    catch (... )
    {
        BOOST_TEST(false);
    }
  }
  { // copy constructor
    Frame oc1{1};
    Frame oc2{oc1};
    BOOST_TEST(oc1 == oc2);
  }
  { // assignment
    Frame oc1{1};
    Frame oc2;
    oc2 = oc1;
    BOOST_TEST(oc2 == oc1);
  }
#if defined COMPILE_ERROR
  { // assignment from UT
    Frame oc1{1};
    oc1 = 2; // error
  }
#endif

  { // operator+(x)
    Frame oc{1};
    Frame oc2 = +oc;
    BOOST_TEST(oc2 == Frame{1});
  }
  { // operator+(x,y)
    Frame oc1{1};
    Frame oc2{2};
    BOOST_TEST(oc1+oc2 == Frame{3});
  }
  { // operator+=(x)
    Frame oc1{1};
    Frame oc2{2};
    oc2 += oc1;
    BOOST_TEST(oc2 == Frame{3});
  }
  { // operator++()
    Frame oc1{1};
    auto oc2 = ++oc1;
    BOOST_TEST(oc1 == Frame{2});
    BOOST_TEST(oc2 == Frame{2});
  }
  { // operator++(int)
    Frame oc1{1};
    auto oc2 = oc1++;
    BOOST_TEST(oc1 == Frame{2});
    BOOST_TEST(oc2 == Frame{1});
  }

  { // operator-(x)
    Frame oc{1};
    Frame oc2 = -oc;
    BOOST_TEST(oc2 == Frame{-1});
  }
  { // operator-(x,y)
    Frame oc1{1};
    Frame oc2{2};
    BOOST_TEST(oc1-oc2 == Frame{-1});
  }
  { // operator-=(x)
    Frame oc1{1};
    Frame oc2{2};
    oc2 -= oc1;
    BOOST_TEST(oc2 == Frame{1});
  }
  { // operator--()
    Frame oc1{1};
    auto oc2 = --oc1;
    BOOST_TEST(oc1 == Frame{0});
    BOOST_TEST(oc2 == Frame{0});
  }
  { // operator--(int)
    Frame oc1{1};
    auto oc2 = oc1--;
    BOOST_TEST(oc1 == Frame{0});
    BOOST_TEST(oc2 == Frame{1});
  }

  { // operator*(x,y)
    Frame oc1{3};
    Frame oc2{2};
    BOOST_TEST(oc1*oc2 == Frame{6});
  }
  { // operator*=(x)
    Frame oc1{3};
    Frame oc2{2};
    oc2 *= oc1;
    BOOST_TEST(oc2 == Frame{6});
  }

  { // operator/(x,y)
    Frame oc1{6};
    Frame oc2{2};
    BOOST_TEST(oc1/oc2 == Frame{3});
  }
  { // operator/=(x)
    Frame oc1{6};
    Frame oc2{2};
    oc1 /= oc2;
    BOOST_TEST(oc1 == Frame{3});
  }

  { // operator%(x,y)
    Frame oc1{6};
    Frame oc2{5};
    BOOST_TEST(oc1%oc2 == Frame{1});
  }
  { // operator%=(x)
    Frame oc1{6};
    Frame oc2{5};
    oc1 %= oc2;
    BOOST_TEST(oc1 == Frame{1});
  }

  { // operator==(x,y)
    Frame oc1{2};
    Frame oc2{2};
    BOOST_TEST( oc1==oc2 );
  }
  { // operator!=(x,y)
    Frame oc1{2};
    Frame oc2{3};
    BOOST_TEST( oc1!=oc2 );
  }
  { // operator<(x,y)
    Frame oc1{2};
    Frame oc2{5};
    BOOST_TEST( oc1<oc2 );
  }
  { // operator<=(x,y)
    Frame oc1{2};
    Frame oc2{5};
    BOOST_TEST( oc1<=oc2 );
  }
  { // operator<=(x,y)
    Frame oc1{2};
    Frame oc2{2};
    BOOST_TEST( oc1<=oc2 );
  }
  { // operator>(x,y)
    Frame oc1{6};
    Frame oc2{5};
    BOOST_TEST( oc1>oc2 );
  }
  { // operator>=(x,y)
    Frame oc1{6};
    Frame oc2{5};
    BOOST_TEST( oc1>=oc2 );
  }
  { // operator<=(x,y)
    Frame oc1{2};
    Frame oc2{2};
    BOOST_TEST( oc1>=oc2 );
  }
  // swap
  {
    Frame oc1{1};
    Frame oc2{2};
    using std::swap;
    swap(oc1,oc2);
    BOOST_TEST(oc1.underlying()==2);
    BOOST_TEST(oc2.underlying()==1);
  }
  // operator <<
  {
    Frame oc{1};
      std::stringstream os;
      os << oc;
      BOOST_TEST_EQ(os.str(), "1");
  }
  // operator >>
  {
    Frame oc;
      std::stringstream s;
      s << 1;
      s >> oc;
      BOOST_TEST_EQ(oc, Frame{1});
  }
  // hash
  {
    Frame oc1{1};
    BOOST_TEST(std::hash<Frame>{}(oc1)==std::hash<int>{}(1));
  }
  {
    using Bounded = Slot;
    BOOST_TEST(stdex::ordinal::size<Bounded>() == 7);
    BOOST_TEST(stdex::ordinal::traits<Bounded>::pos(Bounded{0}) == 0);
    BOOST_TEST(stdex::ordinal::pos(Bounded{4}) == 4);
    BOOST_TEST(stdex::ordinal::pos(Bounded{1}) == 1);
    BOOST_TEST(stdex::ordinal::val<Bounded>(0) == Bounded{0});
    BOOST_TEST(stdex::ordinal::val<Bounded>(1) == Bounded{1});
    BOOST_TEST(stdex::ordinal::first<Bounded>() == Bounded{0});
    BOOST_TEST(stdex::ordinal::last<Bounded>() == Bounded{6});
    BOOST_TEST(stdex::ordinal::succ(Bounded{2}) == Bounded{3});
    BOOST_TEST(stdex::ordinal::pred(Bounded{3}) == Bounded{2});
  }
  return ::boost::report_errors();
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern feature raster
#include <numeric>
#include <boost/test/unit_test.hpp>
#include "fern/feature/core/raster.h"

BOOST_AUTO_TEST_CASE(raster)
{
    size_t const nr_rows = 5;
    size_t const nr_cols = 4;
    auto extents = fern::extents[nr_rows][nr_cols];

    double const cell_width = 4.0;
    double const cell_height = 5.0;
    double const west = 0.0;
    double const north = 0.0;
    using Raster = fern::Raster<int, 2>;
    Raster::Transformation transformation{{west, cell_width, north,
        cell_height}};
    Raster raster(extents, transformation);
    std::iota(raster.data(), raster.data() + raster.num_elements(), 0);

    BOOST_CHECK_EQUAL(raster[0][0], 0);
    BOOST_CHECK_EQUAL(raster[1][0], static_cast<int>(nr_cols));

    BOOST_CHECK_EQUAL(raster.transformation()[0], west);
    BOOST_CHECK_EQUAL(raster.transformation()[1], cell_width);
    BOOST_CHECK_EQUAL(raster.transformation()[2], north);
    BOOST_CHECK_EQUAL(raster.transformation()[3], cell_height);
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <memory>

#include "pyinterp/detail/math.hpp"
#include "pyinterp/eigen.hpp"

namespace pyinterp::detail::math {

/// Set of coordinates used for interpolation
class CoordsXY {
 public:
  /// Default constructor
  CoordsXY() = delete;

  /// Creates a new instance
  CoordsXY(const Eigen::Index x_size, const Eigen::Index y_size)
      : x_(new Eigen::VectorXd), y_(new Eigen::VectorXd) {
    auto nx = x_size << 1U;
    auto ny = y_size << 1U;
    x_->resize(nx);
    y_->resize(ny);
  }

  /// Creates a new instance from existing coordinates
  CoordsXY(std::shared_ptr<Eigen::VectorXd> x,
           std::shared_ptr<Eigen::VectorXd> y)
      : x_(std::move(x)), y_(std::move(y)) {}

  /// Default destructor
  virtual ~CoordsXY() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  CoordsXY(const CoordsXY &rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  CoordsXY(CoordsXY &&rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const CoordsXY &rhs) -> CoordsXY & = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(CoordsXY &&rhs) noexcept -> CoordsXY & = default;

  /// Get the half size of the window in abscissa.
  [[nodiscard]] inline auto nx() const noexcept -> Eigen::Index {
    return x_->size() >> 1;
  }

  /// Get the half size of the window in ordinate.
  [[nodiscard]] inline auto ny() const noexcept -> Eigen::Index {
    return y_->size() >> 1;
  }

  /// Get x-coordinates
  constexpr auto x() noexcept -> std::shared_ptr<Eigen::VectorXd> & {
    return x_;
  }

  /// Get x-coordinates
  [[nodiscard]] constexpr auto x() const noexcept
      -> const std::shared_ptr<Eigen::VectorXd> & {
    return x_;
  }

  /// Get y-coordinates
  constexpr auto y() noexcept -> std::shared_ptr<Eigen::VectorXd> & {
    return y_;
  }

  /// Get y-coordinates
  [[nodiscard]] constexpr auto y() const noexcept
      -> const std::shared_ptr<Eigen::VectorXd> & {
    return y_;
  }

  /// Get the ith x-axis.
  [[nodiscard]] inline auto x(const Eigen::Index ix) const -> double {
    return (*x_)(ix);
  }

  /// Get the ith y-axis.
  [[nodiscard]] inline auto y(const Eigen::Index jx) const -> double {
    return (*y_)(jx);
  }

  /// Set the ith x-axis.
  inline auto x(const Eigen::Index ix) -> double & { return (*x_)(ix); }

  /// Get the ith y-axis.
  inline auto y(const Eigen::Index jx) -> double & { return (*y_)(jx); }

  /// Normalizes the angle with respect to the first value of the X axis of this
  /// array.
  [[nodiscard]] inline auto normalize_angle(const double xi) const -> double {
    return math::normalize_angle(xi, (*x_)(0), 360.0);
  }

 private:
  std::shared_ptr<Eigen::VectorXd> x_{};
  std::shared_ptr<Eigen::VectorXd> y_{};
};

/// Set of coordinates/values used for interpolation
///  * q11 = (x1, y1)
///  * q12 = (x1, y2)
///  * .../...
///  * q1n = (x1, yn)
///  * q21 = (x2, y1)
///  * q22 = (x2, y2).
///  * .../...
///  * q2n = (x2, yn)
///  * .../...
///  * qnn = (xn, yn)
///
/// @code
/// Array2D({{x1, x2, ..., xn}, {y1, y2, ..., yn}},
///         {q11, q12, ..., q21, q22, ...., qnn})
/// @endcode
class Frame2D : public CoordsXY {
 public:
  /// Default constructor
  Frame2D() = delete;

  /// Creates a new Array
  Frame2D(const Eigen::Index x_size, const Eigen::Index y_size)
      : CoordsXY(x_size, y_size), q_(new Eigen::MatrixXd) {
    q_->resize(x()->size(), y()->size());
  }

  /// Creates a new Array from existing coordinates/values
  Frame2D(std::shared_ptr<Eigen::VectorXd> x,
          std::shared_ptr<Eigen::VectorXd> y,
          std::shared_ptr<Eigen::MatrixXd> q)
      : CoordsXY(std::move(x), std::move(y)), q_(std::move(q)) {}

  /// Default destructor
  ~Frame2D() override = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Frame2D(const Frame2D &rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Frame2D(Frame2D &&rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Frame2D &rhs) -> Frame2D & = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Frame2D &&rhs) noexcept -> Frame2D & = default;

  /// Get the values from the array for all x and y coordinates.
  constexpr auto q() noexcept -> std::shared_ptr<Eigen::MatrixXd> & {
    return q_;
  }

  /// Get the values from the array for all x and y coordinates.
  [[nodiscard]] constexpr auto q() const noexcept
      -> const std::shared_ptr<Eigen::MatrixXd> & {
    return q_;
  }

  /// Get the value at coordinate (ix, jx).
  [[nodiscard]] inline auto q(const Eigen::Index ix,
                              const Eigen::Index jx) const -> double {
    return (*q_)(ix, jx);
  }

  /// Get the value at coordinate (ix, jx).
  inline auto q(const Eigen::Index ix, const Eigen::Index jx) -> double & {
    return (*q_)(ix, jx);
  }

  /// Returns true if this instance does not contains at least one Not A Number
  /// (NaN).
  [[nodiscard]] inline auto is_valid() const -> bool { return !q_->hasNaN(); }

 private:
  std::shared_ptr<Eigen::MatrixXd> q_{};
};

/// Set of coordinates/values used for 3D-interpolation
///
/// @tparam Z-Axis type
template <typename T>
class Frame3D : public CoordsXY {
 public:
  /// Default constructor
  Frame3D() = delete;

  /// Creates a new instance
  Frame3D(const Eigen::Index x_size, const Eigen::Index y_size,
          const Eigen::Index z_size)
      : CoordsXY(x_size, y_size), z_() {
    auto nz = z_size << 1U;
    z_.resize(nz);
    q_.resize(nz);

    for (auto iz = 0U; iz < nz; ++iz) {
      q_(iz) = std::make_shared<Eigen::MatrixXd>(x()->size(), y()->size());
    }
  }

  /// Get the set of coordinates/values for the ith z-layer
  [[nodiscard]] auto frame_2d(const Eigen::Index iz) const -> Frame2D {
    return Frame2D(x(), y(), q_(iz));
  }

  /// Default destructor
  ~Frame3D() override = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Frame3D(const Frame3D &rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Frame3D(Frame3D &&rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Frame3D &rhs) -> Frame3D & = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Frame3D &&rhs) noexcept -> Frame3D & = default;

  /// Get the half size of the window in z.
  [[nodiscard]] inline auto nz() const noexcept -> Eigen::Index {
    return z_.size() >> 1;
  }

  /// Get z-coordinates
  constexpr auto z() noexcept -> Vector<T> & { return z_; }

  /// Get z-coordinates
  [[nodiscard]] constexpr auto z() const noexcept -> const Vector<T> & {
    return z_;
  }

  /// Get the ith z-axis.
  [[nodiscard]] inline auto z(const Eigen::Index ix) const -> T {
    return z_(ix);
  }

  /// Set the ith z-axis.
  inline auto z(const Eigen::Index ix) -> T & { return z_(ix); }

  /// Get the value at coordinate (ix, jx, kx).
  inline auto q(const Eigen::Index ix, const Eigen::Index jx,
                const Eigen::Index kx) -> double & {
    return (*q_(kx))(ix, jx);
  }

  /// Returns true if this instance does not contains at least one Not A Number
  /// (NaN).
  [[nodiscard]] inline auto is_valid() const -> bool {
    for (Eigen::Index kx = 0; kx < q_.size(); ++kx) {
      if ((*q_(kx)).hasNaN()) {
        return false;
      }
    }
    return true;
  }

 private:
  Vector<T> z_;
  Vector<std::shared_ptr<Eigen::MatrixXd>> q_;
};

/// Set of coordinates/values used for 4D-interpolation
///
/// @tparam Z-Axis type
template <typename T>
class Frame4D : public CoordsXY {
 public:
  /// Default constructor
  Frame4D() = delete;

  /// Creates a new instance
  Frame4D(const Eigen::Index x_size, const Eigen::Index y_size,
          const Eigen::Index z_size, const Eigen::Index u_size)
      : CoordsXY(x_size, y_size), z_() {
    auto nz = z_size << 1U;
    auto nu = u_size << 1U;
    z_.resize(nz);
    u_.resize(nu);
    q_.resize(nz, nu);

    for (auto iz = 0U; iz < nz; ++iz) {
      for (auto iu = 0U; iu < nu; ++iu) {
        q_(iz, iu) =
            std::make_shared<Eigen::MatrixXd>(x()->size(), y()->size());
      }
    }
  }

  /// Get the set of coordinates/values for the ith z-layer
  [[nodiscard]] auto frame_2d(const Eigen::Index iz,
                              const Eigen::Index iu) const -> Frame2D {
    return Frame2D(x(), y(), q_(iz, iu));
  }

  /// Default destructor
  ~Frame4D() override = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Frame4D(const Frame4D &rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Frame4D(Frame4D &&rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Frame4D &rhs) -> Frame4D & = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Frame4D &&rhs) noexcept -> Frame4D & = default;

  /// Get the half size of the window in z.
  [[nodiscard]] inline auto nz() const noexcept -> Eigen::Index {
    return z_.size() >> 1;
  }

  /// Get the half size of the window in u.
  [[nodiscard]] inline auto nu() const noexcept -> Eigen::Index {
    return u_.size() >> 1;
  }

  /// Get z-coordinates
  constexpr auto z() noexcept -> Vector<T> & { return z_; }

  /// Get u-coordinates
  constexpr auto u() noexcept -> Eigen::VectorXd & { return u_; }

  /// Get z-coordinates
  [[nodiscard]] inline auto z() const noexcept -> const Vector<T> & {
    return z_;
  }

  /// Get u-coordinates
  [[nodiscard]] inline auto u() const noexcept -> const Eigen::VectorXd & {
    return u_;
  }

  /// Get the ith z-axis.
  [[nodiscard]] inline auto z(const Eigen::Index ix) const -> T {
    return z_(ix);
  }

  /// Get the ith u-axis.
  [[nodiscard]] inline auto u(const Eigen::Index ix) const -> double {
    return u_(ix);
  }

  /// Set the ith z-axis.
  inline auto z(const Eigen::Index ix) -> T & { return z_(ix); }

  /// Set the ith u-axis.
  inline auto u(const Eigen::Index ix) -> double & { return u_(ix); }

  /// Get the value at coordinate (ix, jx, kx, lx).
  inline auto q(const Eigen::Index ix, const Eigen::Index jx,
                const Eigen::Index kx, const Eigen::Index lx) -> double & {
    return (*q_(kx, lx))(ix, jx);
  }

  /// Returns true if this instance does not contains at least one Not A Number
  /// (NaN).
  [[nodiscard]] inline auto is_valid() const -> bool {
    for (Eigen::Index kx = 0; kx < z_.size(); ++kx) {
      for (Eigen::Index lx = 0; lx < u_.size(); ++lx) {
        if ((*q_(kx, lx)).hasNaN()) {
          return false;
        }
      }
    }
    return true;
  }

 private:
  Vector<T> z_;
  Eigen::VectorXd u_;
  Matrix<std::shared_ptr<Eigen::MatrixXd>> q_;
};

}  // namespace pyinterp::detail::math


// -*- mode:c++; c-style:k&r; c-basic-offset:4; indent-tabs-mode: nil; -*-
// vi:set et cin sw=4 cino=>se0n0f0{0}0^0\:0=sl1g0hspst0+sc3C0/0(0u0U0w0m0:

#include <iostream>
#include <boost/random.hpp>
#include "random.hpp"

using namespace std;

random_gen::random_gen(uint32_t new_id, const uint32_t seed)
    : rng(seed), gen(rng), id(new_id) { }

uint32_t random_gen::get_id() const { return id; }

int random_gen::random_range(int max) {
    double uniform_random = gen();
    return static_cast<int>(max * uniform_random);
}

double random_gen::random_range_double(double max) {
    double uniform_random = gen();
    return max * uniform_random;
}


/* Copyright (c) 2016, Stefan.Eilemann@epfl.ch
 *
 * 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 Eyescale Software GmbH 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.
 */

#include <vmmlib/quaternion.hpp>

#define BOOST_TEST_MODULE quaternion
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(construction)
{
    const vmml::Quaternionf q1;
    BOOST_CHECK_EQUAL(q1, vmml::Quaternionf(0, 0, 0, 1));

    const vmml::Quaternionf q2(q1);
    BOOST_CHECK_EQUAL(q2, vmml::Quaternionf(0, 0, 0, 1));
    BOOST_CHECK_EQUAL(q2, q1);

    BOOST_CHECK_EQUAL(q1.x(), 0);
    BOOST_CHECK_EQUAL(q1.y(), 0);
    BOOST_CHECK_EQUAL(q1.z(), 0);
    BOOST_CHECK_EQUAL(q1.w(), 1);

    std::stringstream str;
    str << q1;
    BOOST_CHECK_EQUAL(str.str(),
                      "(          0          0          0          1 )");

    const vmml::Quaternionf q3(0.3f, vmml::Vector3f(.5f, .5f, 1.f));
    BOOST_CHECK_MESSAGE(q3.equals(vmml::Quaternionf(0.0610079f, 0.0610079f,
                                                    0.122016f, 0.988771f),
                                  0.00001f),
                        q3);
}

BOOST_AUTO_TEST_CASE(rotate)
{
    const vmml::Quaternionf q1(0.3f, vmml::Vector3f(.5f, .5f, 1.f));
    BOOST_CHECK_MESSAGE(
        (q1 * q1).equals(vmml::Quaternionf(0.6f, vmml::Vector3f(.5f, .5f, 1.f)),
                         0.00001f),
        vmml::Quaternionf(0.6f, vmml::Vector3f(.5f, .5f, 1.f)));

    const vmml::Vector3f z(0.0f, 0.0f, 1.0f);
    const vmml::Vector3f result = q1.rotate(z);
    BOOST_CHECK_CLOSE(result.x(), 0.135533f, 0.01f);
    BOOST_CHECK_CLOSE(result.y(), 0.-0.105758f, 0.01f);
    BOOST_CHECK_CLOSE(result.z(), 0.985112f, 0.01f);
}


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

#define BOOST_TEST_MODULE test_tsdf

//standard library
#include <climits>
//_DEBUG
#include <iostream>
#include "../src/console/pretty_printers.hpp"

//libraries
#include <boost/test/unit_test.hpp>
#include <boost/python.hpp>
#include <Eigen/Dense>

//test data
#include "data/test_data_tsdf.hpp"

//test targets
#include "../src/tsdf/generator.hpp"
#include "../src/math/typedefs.hpp"
#include "../src/math/almost_equal.hpp"
#include "../src/image_io/png_eigen.hpp"
#include "common.hpp"

namespace eig = Eigen;

BOOST_AUTO_TEST_CASE(test_tsdf_generation_no_interpolation_1) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag2_depth_00108.png");
	BOOST_REQUIRE(image_read);
	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;
	tsdf::Parameters2d parameters(
				0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				math::Vector2i(-8, 144), //offset
				math::Vector2i(16, 16), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::NONE
				);
	tsdf::Generator2d generator(parameters);
	eig::MatrixXf field = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);
	BOOST_REQUIRE(math::almost_equal_verbose(field, test_data::expected_tsdf_field01, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_no_interpolation_2) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag2_depth_00108.png");
	BOOST_REQUIRE(image_read);
	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;
	eig::Matrix4f camera_extrinsic_matrix = eig::Matrix4f::Identity();
	camera_extrinsic_matrix(2, 3) = 0.004;
	tsdf::Parameters2d parameters(
					0.001f, //depth unit ratio
					camera_intrinsic_matrix, //projection matrix
					0.05f, //near clipping distance
					math::Vector2i(-8, 144), //offset
					math::Vector2i(16, 16), //shape
					0.004f, //voxel size
					20, //narrow band width
					tsdf::FilteringMethod::NONE
					);
	tsdf::Generator2d generator(parameters);
	eig::MatrixXf field = generator.generate(depth_image, camera_extrinsic_matrix, 200);
	BOOST_REQUIRE(math::almost_equal_verbose(field, test_data::expected_tsdf_field02, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_EWA_2D_image_space_1) {

	math::MatrixXus depth_image = math::MatrixXus::Constant(3, 640, USHRT_MAX);
	depth_image.block(0, 399, 3, 18) = test_data::depth_image_region;

	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;

	tsdf::Parameters2d parameters(
			0.001f, //depth unit ratio
			camera_intrinsic_matrix, //projection matrix
			0.05f, //near clipping distance
			math::Vector2i(94, 804), //offset
			math::Vector2i(16, 16), //shape
			0.004f, //voxel size
			20, //narrow band width
			tsdf::FilteringMethod::EWA_IMAGE_SPACE,
			1.0f // gaussian covariance scale
			);
	tsdf::Generator2d generator(parameters);
	eig::MatrixXf field = generator.generate(depth_image, eig::Matrix4f::Identity(), 1);

	BOOST_REQUIRE(math::almost_equal_verbose(field, test_data::out_sdf_field, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_EWA_2D_image_space_2) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag2_depth_00108.png");
	BOOST_REQUIRE(image_read);
	bool test_full_image = false;

	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;
	math::Vector2i offset(-256, 0);
	int chunk_x_start = 210, chunk_y_start = 103;
	int chunk_size = 16;
	math::Vector2i offset_chunk_from_image(chunk_x_start, chunk_y_start);
	math::Vector2i offset_chunk = offset + offset_chunk_from_image;

	eig::MatrixXf field_chunk;
	if (test_full_image) {
		tsdf::Parameters2d parameters(
				0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				offset,
				math::Vector2i(512, 512), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::EWA_IMAGE_SPACE,
				1.0f // gaussian covariance scale
				);
		tsdf::Generator2d generator(parameters);
		eig::MatrixXf field = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);

		field_chunk = field.block(chunk_y_start, chunk_x_start, chunk_size, chunk_size);
	} else {
		tsdf::Parameters2d parameters(
				0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				offset_chunk,
				math::Vector2i(chunk_size, chunk_size), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::EWA_IMAGE_SPACE,
				1.0f // gaussian covariance scale
				);
		tsdf::Generator2d generator(parameters);
		field_chunk = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);
	}
	BOOST_REQUIRE(math::almost_equal_verbose(field_chunk, test_data::out_sdf_chunk, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_EWA_2D_voxel_space_inclusive_1) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag2_depth_00108.png");
	BOOST_REQUIRE(image_read);
	bool test_full_image = false;

	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;
	math::Vector2i offset(-256, 0);
	int chunk_x_start = 210, chunk_y_start = 103;
	int chunk_size = 16;
	math::Vector2i offset_chunk_from_image(chunk_x_start, chunk_y_start);
	math::Vector2i offset_chunk = offset + offset_chunk_from_image;

	eig::MatrixXf field_chunk;
	if (test_full_image) {
		tsdf::Parameters2d parameters(
				0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				offset_chunk,
				math::Vector2i(512, 512), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::EWA_VOXEL_SPACE_INCLUSIVE,
				1.0f // gaussian covariance scale
				);
		tsdf::Generator2d generator(parameters);
		eig::MatrixXf field = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);
		field_chunk = field.block(chunk_y_start, chunk_x_start, chunk_size, chunk_size);
	} else {
		tsdf::Parameters2d parameters(
				0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				offset_chunk,
				math::Vector2i(chunk_size, chunk_size), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::EWA_VOXEL_SPACE_INCLUSIVE,
				1.0f // gaussian covariance scale
				);
		tsdf::Generator2d generator(parameters);
		field_chunk = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);
	}

	BOOST_REQUIRE(math::almost_equal_verbose(field_chunk, test_data::expected_tsdf_EWA_voxel_space_inclusive_1, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_EWA_2D_voxel_space_inclusive_2) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag_depth_00064.png");
	BOOST_REQUIRE(image_read);
	bool test_full_image = false;

	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;
	math::Vector2i offset(-256, 480);
	int chunk_x_start = 24, chunk_y_start = 10;
	int chunk_size = 16;
	math::Vector2i offset_chunk_from_image(chunk_x_start, chunk_y_start);
	math::Vector2i offset_chunk = offset + offset_chunk_from_image;

	eig::MatrixXf field_chunk;
	if (test_full_image) {
		tsdf::Parameters2d parameters(0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				offset_chunk, //offset of scene from world origin
				math::Vector2i(512, 512), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::EWA_VOXEL_SPACE_INCLUSIVE,
				1.0f // gaussian covariance scale
				);
		tsdf::Generator2d generator(parameters);
		eig::MatrixXf field = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);
		field_chunk = field.block(chunk_y_start, chunk_x_start, chunk_size, chunk_size);
	} else {
		tsdf::Parameters2d parameters(
				0.001f, //depth unit ratio
				camera_intrinsic_matrix, //projection matrix
				0.05f, //near clipping distance
				offset_chunk,
				math::Vector2i(chunk_size, chunk_size), //shape
				0.004f, //voxel size
				20, //narrow band width
				tsdf::FilteringMethod::EWA_VOXEL_SPACE_INCLUSIVE,
				1.0f // gaussian covariance scale
				);
		tsdf::Generator2d generator(parameters);
		field_chunk = generator.generate(depth_image, eig::Matrix4f::Identity(), 200);
	}
	BOOST_REQUIRE(math::almost_equal_verbose(field_chunk, test_data::expected_tsdf_EWA_voxel_space_inclusive_2, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_EWA_3D_image_space_1) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag2_depth_00108.png");
	BOOST_REQUIRE(image_read);

	math::Vector3i offset(-46, -8, 105);
	math::Vector3i field_shape(16, 1, 16);
	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;

	tsdf::Parameters3d parameters(0.001f, //depth unit ratio
						camera_intrinsic_matrix, //projection matrix
						0.05f, //near clipping distance
						offset, //offset of scene from world origin
						field_shape, //dimensions of the voxel grid
						0.004f, //voxel size
						20, //narrow band width
						tsdf::FilteringMethod::EWA_IMAGE_SPACE,
						1.0f // gaussian covariance scale
						);
	tsdf::Generator3d generator(parameters);
	eig::Tensor<float, 3> field = generator.generate(depth_image, eig::Matrix4f::Identity());

	BOOST_REQUIRE(math::almost_equal_verbose(field, test_data::TSDF_slice01, 1e-6f));
}

BOOST_AUTO_TEST_CASE(test_tsdf_generation_EWA_3D_image_space_2) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag2_depth_00108.png");
	BOOST_REQUIRE(image_read);

	math::Vector3i offset(-46, -8, 105);
	math::Vector3i field_shape(16, 1, 16); //zigzag2-108
	eig::Matrix3f camera_intrinsic_matrix;
	camera_intrinsic_matrix <<
			700.0f, 0.0f, 320.0f,
			0.0f, 700.0f, 240.0f,
			0.0f, 0.0f, 1.0f;
	tsdf::Parameters3d parameters(0.001f, //depth unit ratio
							camera_intrinsic_matrix, //projection matrix
							0.05f, //near clipping distance
							offset, //offset of scene from world origin
							field_shape, //dimensions of the voxel grid
							0.004f, //voxel size
							20, //narrow band width
							tsdf::FilteringMethod::EWA_IMAGE_SPACE,
							0.5f // gaussian covariance scale
							);
	tsdf::Generator3d generator(parameters);
	eig::Tensor<float, 3> field = generator.generate(depth_image, eig::Matrix4f::Identity());
	BOOST_REQUIRE(math::almost_equal_verbose(field, test_data::TSDF_slice02, 1e-5f));
}


#pragma once
#include <Eigen/Dense>
#include <vector>
#include <opencv2/core.hpp>
#include "radar_utils.hpp"

/*!
   \brief Extract CFAR features from the polar radar data, one azimuth at a time (1D)
   \param fft_data Polar radar power readings
   \param window_size Length of window used to estimate local clutter power
   \param scale local clutter power is multiplied by scale
   \param guard_cells How many cells on the left and right side of the test cell are ignored before estimation
   \param min_range We ignore the range bins less than this
   \param targets [out] Matrix of feature locations (azimuth_bin, range_bin, 1) x N
*/
void cfar1d(cv::Mat fft_data, int window_size, float scale, int guard_cells, int min_range, Eigen::MatrixXd &targets);

/*!
   \brief Extract features from polar radar data using the method described in cen_icra18
   \param fft_data Polar radar power readings
   \param zq If y(i, j) > zq * sigma_q then it is considered a potential target point
   \param sigma_gauss std dev of the gaussian filter uesd to smooth the radar signal
   \param min_range We ignore the range bins less than this
   \param targets [out] Matrix of feature locations (azimuth_bin, range_bin, 1) x N
*/
double cen2018features(cv::Mat fft_data, float zq, int sigma_gauss, int min_range, Eigen::MatrixXd &targets);

/*!
   \brief Extract features from polar radar data using the method described in cen_icra19
   \param fft_data Polar radar power readings
   \param max_points Maximum number of targets points to be extracted from the radar image
   \param min_range We ignore the range bins less than this
   \param targets [out] Matrix of feature locations (azimuth_bin, range_bin, 1) x N
*/
double cen2019features(cv::Mat fft_data, int max_points, int min_range, Eigen::MatrixXd &targets);

double cen2019descriptors(std::vector<double> azimuths, cv::Size polar_dims, Eigen::MatrixXd polar_points,
    Eigen::MatrixXd cart_targets, float radar_resolution, float cart_resolution, int cart_pixel_width,
    cv::Mat &descriptors, int navtech_version = CTS350);


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


/*
 * Copyright (c) 2016-2018 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#include "exception_internal.hpp"
#include "utils/misc.hpp"
#include "utils/floating_point_comparison.hpp"

#include <pkmn/calculations/gender.hpp>
#include <pkmn/database/pokemon_entry.hpp>

#include <boost/assert.hpp>

namespace pkmn { namespace calculations {

    pkmn::e_gender gen2_pokemon_gender(
        pkmn::e_species species,
        int IV_attack
    )
    {
        pkmn::enforce_IV_bounds(
            "Attack",
            IV_attack,
            false
        );
        if((species == pkmn::e_species::NONE) || (species == pkmn::e_species::INVALID))
        {
            throw std::invalid_argument("Species cannot be none or invalid.");
        }

        pkmn::database::pokemon_entry entry(species, pkmn::e_game::CRYSTAL, "");
        float chance_male = entry.get_chance_male();
        float chance_female = entry.get_chance_female();

        pkmn::e_gender ret = pkmn::e_gender::NONE;

        if(pkmn::fp_compare_equal(chance_male, 0.0f) and
           pkmn::fp_compare_equal(chance_female, 0.0f)
        )
        {
            ret = pkmn::e_gender::GENDERLESS;
        }
        else if(pkmn::fp_compare_equal(chance_male, 1.0f))
        {
            ret = pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.875f))
        {
            ret = (IV_attack < 2) ? pkmn::e_gender::FEMALE
                                  : pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.75f))
        {
            ret = (IV_attack < 4) ? pkmn::e_gender::FEMALE
                                  : pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.5f))
        {
            ret = (IV_attack < 7) ? pkmn::e_gender::FEMALE
                                  : pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.25f))
        {
            ret = (IV_attack < 12) ? pkmn::e_gender::FEMALE
                                   : pkmn::e_gender::MALE;
        }
        else
        {
            ret = pkmn::e_gender::FEMALE;
        }

        BOOST_ASSERT(ret != pkmn::e_gender::NONE);

        return ret;
    }

    pkmn::e_gender modern_pokemon_gender(
        pkmn::e_species species,
        uint32_t personality
    )
    {
        if((species == pkmn::e_species::NONE) || (species == pkmn::e_species::INVALID))
        {
            throw std::invalid_argument("Species cannot be none or invalid.");
        }

        uint8_t truncated_pid = uint8_t(personality & 0xFF);

        pkmn::database::pokemon_entry entry(species, pkmn::e_game::OMEGA_RUBY, "");
        float chance_male = entry.get_chance_male();
        float chance_female = entry.get_chance_female();

        pkmn::e_gender ret = pkmn::e_gender::NONE;

        if(pkmn::fp_compare_equal(chance_male, 0.0f) and
           pkmn::fp_compare_equal(chance_female, 0.0f)
        )
        {
            ret = pkmn::e_gender::GENDERLESS;
        }
        else if(pkmn::fp_compare_equal(chance_male, 1.0f))
        {
            ret = pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.875f))
        {
            ret = (truncated_pid < 31) ? pkmn::e_gender::FEMALE
                                       : pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.75f))
        {
            ret = (truncated_pid < 64) ? pkmn::e_gender::FEMALE
                                       : pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.5f))
        {
            ret = (truncated_pid < 127) ? pkmn::e_gender::FEMALE
                                        : pkmn::e_gender::MALE;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.25f))
        {
            ret = (truncated_pid < 191) ? pkmn::e_gender::FEMALE
                                        : pkmn::e_gender::MALE;
        }
        else
        {
            ret = pkmn::e_gender::FEMALE;
        }

        BOOST_ASSERT(ret != pkmn::e_gender::NONE);

        return ret;
    }

}}


#ifndef SPLX_PARAMETRIC_CURVE_HPP
#define SPLX_PARAMETRIC_CURVE_HPP
#include <Eigen/Dense>
#include <cmath>
#include <vector>

namespace splx {

template <typename T, unsigned int DIM>
class ParametricCurve {
public:

  using VectorDIM = Eigen::Matrix<T, DIM, 1>;
  using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
  using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using Hyperplane = Eigen::Hyperplane<T, DIM>;
  using MatrixDIM = Eigen::Matrix<T, DIM, DIM, Eigen::RowMajor>;
  using Row = Eigen::Matrix<T, 1, Eigen::Dynamic>;
  using AlignedBox = Eigen::AlignedBox<T, DIM>;
  using ControlPoints = std::vector<VectorDIM, Eigen::aligned_allocator<VectorDIM>>;

  enum class CurveType {
    LINEAR,
    BSPLINE,
    BEZIER
  };

  CurveType type;

  ParametricCurve(CurveType t) : type(t) {
  }

  virtual ~ParametricCurve() {

  }

  /**
   * Evaluate k^{th} derivative of the curve at u
  */
  virtual VectorDIM eval(T u, unsigned int k) const = 0;

  /*
  * Returns true if the curve is on the negative side of the hyperplane
  * otherwise returns false
  */
  virtual bool onNegativeSide(const Hyperplane& hp) const = 0;

  /*
  * Returns true if the curve is in the non-positive side of the hyperplane
  */
  virtual bool onNonPositiveSide(const Hyperplane& hp) const = 0;

  /*
  * Get i^th control point
  */
  virtual VectorDIM& operator[](std::size_t i) = 0;
  virtual const VectorDIM& operator[](std::size_t i) const = 0;

  /*
  * Get number of control points
  */
  virtual std::size_t numControlPoints() const = 0;

  /*
    Append a new control points to the curve.
  */
  virtual void appendControlPoint(const VectorDIM& cpt) = 0;

  /*
    Remove i^th control point
  */
  virtual void removeControlPoint(std::size_t i) = 0;

  /*
  * Returns the max parameter of the curve, i.e. returns a where curve is defined
  * for u \in [0, a]
  */
  virtual T maxParameter() const = 0;

  /*
  * Sets the new max parameter
  */
  virtual void maxParameter(T nw) = 0;

};

}
#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================

/*
   IMPORTANT!!!
   ~~~~~~~~~~~~
   This example uses interfaces that have been deprecated and removed from
   Boost.Grpah. Someone needs to update it, as it does NOT compile.
*/

#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

int main()
{
    using namespace boost;
    GraphvizGraph g_dot;
    read_graphviz("figs/telephone-network.dot", g_dot);

    typedef adjacency_list< vecS, vecS, undirectedS, no_property,
        property< edge_weight_t, int > >
        Graph;
    Graph g(num_vertices(g_dot));
    property_map< GraphvizGraph, edge_attribute_t >::type edge_attr_map
        = get(edge_attribute, g_dot);
    graph_traits< GraphvizGraph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g_dot); ei != ei_end; ++ei)
    {
        int weight = lexical_cast< int >(edge_attr_map[*ei]["label"]);
        property< edge_weight_t, int > edge_property(weight);
        add_edge(source(*ei, g_dot), target(*ei, g_dot), edge_property, g);
    }

    std::vector< graph_traits< Graph >::edge_descriptor > mst;
    typedef std::vector< graph_traits< Graph >::edge_descriptor >::size_type
        size_type;
    kruskal_minimum_spanning_tree(g, std::back_inserter(mst));

    property_map< Graph, edge_weight_t >::type weight = get(edge_weight, g);
    int total_weight = 0;
    for (size_type e = 0; e < mst.size(); ++e)
        total_weight += get(weight, mst[e]);
    std::cout << "total weight: " << total_weight << std::endl;

    typedef graph_traits< Graph >::vertex_descriptor Vertex;
    for (size_type i = 0; i < mst.size(); ++i)
    {
        Vertex u = source(mst[i], g), v = target(mst[i], g);
        edge_attr_map[edge(u, v, g_dot).first]["color"] = "black";
    }
    std::ofstream out("figs/telephone-mst-kruskal.dot");
    graph_property< GraphvizGraph, graph_edge_attribute_t >::type&
        graph_edge_attr_map
        = get_property(g_dot, graph_edge_attribute);
    graph_edge_attr_map["color"] = "gray";
    graph_edge_attr_map["style"] = "bold";
    write_graphviz(out, g_dot);

    return EXIT_SUCCESS;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!
  @ingroup group-arithmetic
    Function object implementing dec capabilities

  Decrements a value by 1.

  @par semantic:
  For any given value @c x of type @c T:

  @code
  T r = dec(x);
  @endcode

  is equivalent to:

  @code
  T r =  x-T(1);
  @endcode

  @see  minusone, minus

  **/
  const boost::dispatch::functor<tag::dec_> dec = {};
} }
#endif

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

#endif


#include "pityor_mixing.h"

#include <google/protobuf/stubs/casts.h>

#include <Eigen/Dense>
#include <memory>
#include <stan/math/prim.hpp>
#include <vector>

#include "mixing_prior.pb.h"
#include "mixing_state.pb.h"
#include "src/hierarchies/abstract_hierarchy.h"

void PitYorMixing::update_state(
    const std::vector<std::shared_ptr<AbstractHierarchy>> &unique_values,
    const std::vector<unsigned int> &allocations) {
  auto priorcast = cast_prior();
  if (priorcast->has_fixed_values()) {
    return;
  } else {
    throw std::invalid_argument("Unrecognized mixing prior");
  }
}

double PitYorMixing::mass_existing_cluster(
    const unsigned int n, const unsigned int n_clust, const bool log,
    const bool propto, const std::shared_ptr<AbstractHierarchy> hier) const {
  double out;
  if (hier->get_card() == 0) {
    return log ? stan::math::NEGATIVE_INFTY : 0;
  }
  if (log) {
    out = std::log(hier->get_card() - state.discount);
    if (!propto) out -= std::log(n + state.strength);
  } else {
    out = hier->get_card() - state.discount;
    if (!propto) out /= n + state.strength;
  }
  return out;
}

double PitYorMixing::mass_new_cluster(const unsigned int n,
                                      const unsigned int n_clust,
                                      const bool log,
                                      const bool propto) const {
  double out;
  if (log) {
    out = std::log(state.strength + state.discount * n_clust);
    if (!propto) out -= std::log(n + state.strength);
  } else {
    out = state.strength + state.discount * n_clust;
    if (!propto) out /= n + state.strength;
  }
  return out;
}

void PitYorMixing::set_state_from_proto(
    const google::protobuf::Message &state_) {
  auto &statecast = downcast_state(state_);
  state.strength = statecast.py_state().strength();
  state.discount = statecast.py_state().discount();
}

std::shared_ptr<bayesmix::MixingState> PitYorMixing::get_state_proto() const {
  bayesmix::PYState state_;
  state_.set_strength(state.strength);
  state_.set_discount(state.discount);

  auto out = std::make_shared<bayesmix::MixingState>();
  out->mutable_py_state()->CopyFrom(state_);
  return out;
}

void PitYorMixing::initialize_state() {
  auto priorcast = cast_prior();
  if (priorcast->has_fixed_values()) {
    state.strength = priorcast->fixed_values().strength();
    state.discount = priorcast->fixed_values().discount();
    if (state.strength <= -state.discount) {
      throw std::invalid_argument("Mixing parameters are not valid");
    }
    if (state.discount < 0 or state.discount >= 1) {
      throw std::invalid_argument("Discount parameter must be in [0,1)");
    }

  } else {
    throw std::invalid_argument("Unrecognized mixing prior");
  }
}


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the square root of the greatest finite representable value

    @par Semantic:

    @code
    T r = Sqrtvalmax<T>();
    @endcode

    is similar to:

    @code
    T r =  sqrt(Valmax<T>();
    @endcode

    @return The Sqrtvalmax constant for the proper type
  **/
  template<typename T> T Sqrtvalmax();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant sqrtvalmax.

      @return The Sqrtvalmax constant for the proper type
    **/
    const boost::dispatch::functor<tag::sqrtvalmax_> sqrtvalmax = {};
  }
} }
#endif

#include <boost/simd/constant/definition/sqrtvalmax.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif