// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution using on the DE/best/1 strategy.
// Uses two deviations from the standard DE algorithm:
// a) temporal locality introduced in
// https://www.researchgate.net/publication/309179699_Differential_evolution_for_protein_folding_optimization_based_on_a_three-dimensional_AB_off-lattice_model
// b) reinitialization of individuals based on their age.
// requires https://github.com/imneme/pcg-cpp
// To be used to further optimize a given solution. Initial population is created using a normal distribition
// with mean=init and sdev=sigma (normalized over the bounds, defined separately for each variable).

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"
#include "call_java.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef double (*callback_type)(int, double[]);

namespace l_differential_evolution {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);
static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static vec constant(int n, double val) {
    return vec::Constant(n, val);
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dx, dy, [&]() {
        return distr_01(rs);
    });
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static double normreal(double mean, double sdev, pcg64 &rs) {
    return gauss_01(rs) * sdev + mean;
}

static vec normalVec(const vec &mean, const vec &sdev, int dim, pcg64 &rs) {
    vec nv = Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return gauss_01(rs);
    });
    return (nv.array() * sdev.array()).matrix() + mean;
}

static int index_min(vec &v) {
    double minv = DBL_MAX;
    int mi = -1;
    for (int i = 0; i < v.size(); i++) {
        if (v[i] < minv) {
            mi = i;
            minv = v[i];
        }
    }
    return mi;
}

struct IndexVal {
    int index;
    double val;
};

// wrapper around the fittness function, scales according to boundaries

class Fitness {

public:

    Fitness(CallJava *func_, int dim_, const vec &lower_limit,
            const vec &upper_limit, const vec &guess_, const vec &sigma_,
            pcg64 &rs_) {
        func = func_;
        dim = dim_;
        lower = lower_limit;
        upper = upper_limit;
        // initial guess for the arguments of the fitness function
        guess = guess_;
        xmean = vec(guess);
        rs = rs_;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
        else
            scale = constant(dim, 1.0);
        invScale = scale.cwiseInverse();
        maxSigma = 0.25 * scale;
        // individual sigma values - initial search volume. inputSigma determines
        // the initial coordinate wise standard deviations for the search.
        if (sigma_.size() == 1)
            sigma0 =
                    0.5
                            * (scale.array()
                                    * (vec::Constant(dim, sigma_[0])).array()).matrix();
        else
            sigma0 = 0.5 * (scale.array() * sigma_.array()).matrix();
        sigma = vec(sigma0);
    }

    void updateSigma(const vec &X) {
        vec delta = (xmean - X).cwiseAbs() * 0.5;
        sigma = delta.cwiseMin(maxSigma);
        xmean = X;
    }

    vec normX() {
        return distr_01(rs) < 0.5 ?
                getClosestFeasible(normalVec(xmean, sigma0, dim, rs)) :
                getClosestFeasible(normalVec(xmean, sigma, dim, rs));
    }

    double normXi(int i) {
        double nx;
        if (distr_01(rs) < 0.5) {
            do {
                nx = normreal(xmean[i], sigma0[i], rs);
            } while (!feasible(i, nx));
        } else {
            do {
                nx = normreal(xmean[i], sigma[i], rs);
            } while (!feasible(i, nx));
        }
        return nx;
    }

    bool feasible(int i, double x) {
        return lower.size() == 0 || (x >= lower[i] && x <= upper[i]);
    }

    vec sample() {
        if (lower.size() > 0) {
            vec rv = uniformVec(dim, rs);
            return (rv.array() * scale.array()).matrix() + lower;
        } else
            return normX();
    }

    double sample_i(int i) {
        if (lower.size() > 0)
            return lower[i] + scale[i] * distr_01(rs);
        else
            return normXi(i);
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0) {
            return X.cwiseMin(upper).cwiseMax(lower);
        }
        return X;
    }

    double eval(const vec &X) {
        int n = X.size();
        double parg[n];
        for (int i = 0; i < n; i++)
            parg[i] = X(i);
        double res = func->evalJava1(n, parg);
        evaluationCounter++;
        return res;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

    vec guess;

private:
    CallJava *func;
    int dim;
    vec lower;
    vec upper;
    vec xmean;
    vec sigma0;
    vec sigma;
    vec maxSigma;
    pcg64 rs;
    long evaluationCounter;
    vec scale;
    vec invScale;
};

class LDeOptimizer {

public:

    LDeOptimizer(long runid_, Fitness *fitfun_, int dim_, pcg64 *rs_,
            int popsize_, int maxEvaluations_, double keep_,
            double stopfitness_, double F_, double CR_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 15 * dim;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 50000;
        // keep best young after each iteration.
        keep = keep_ > 0 ? keep_ : 30;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F0 = F_ > 0 ? F_ : 0.5;
        CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        bestY = DBL_MAX;
        // stop criteria
        stop = 0;
        rs = rs_;
        init();
    }

    ~LDeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    double rnd02() {
        double rnd = distr_01(*rs);
        return rnd * rnd;
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------
        for (iterations = 1; fitfun->getEvaluations() < maxEvaluations;
                iterations++) {

            double CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            double F = iterations % 2 == 0 ? 0.5 * F0 : F0;

            for (int p = 0; p < popsize; p++) {
                vec xp = popX.col(p);
                vec xb = popX.col(bestI);

                int r1, r2;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p || r1 == bestI);
                do {
                    r2 = rndInt(popsize);
                } while (r2 == p || r2 == bestI || r2 == r1);
                vec x1 = popX.col(r1);
                vec x2 = popX.col(r2);
                int r = rndInt(dim);
                vec x = vec(xp);
                for (int j = 0; j < dim; j++) {
                    if (j == r || rnd01() < CR) {
                        x[j] = xb[j] + F * (x1[j] - x2[j]);
                        if (!fitfun->feasible(j, x[j]))
                            x[j] = fitfun->normXi(j);
                    }
                }
                double y = fitfun->eval(x);
                if (isfinite(y) && y < popY[p]) {
                    // temporal locality
                    vec x2 = fitfun->getClosestFeasible(xb + ((x - xp) * 0.5));
                    double y2 = fitfun->eval(x2);
                    if (isfinite(y2) && y2 < y) {
                        y = y2;
                        x = x2;
                    }
                    popX.col(p) = x;
                    popY(p) = y;
                    popIter[p] = iterations;
                    if (y < popY[bestI]) {
                        bestI = p;
                        if (y < bestY) {
                            fitfun->updateSigma(x);
                            bestY = y;
                            bestX = x;
                            if (isfinite(stopfitness) && bestY < stopfitness) {
                                stop = 1;
                                return;
                            }
                        }
                    }
                } else {
                    // reinitialize individual
                    if (keep * rnd01() < iterations - popIter[p]) {
                        popX.col(p) = fitfun->normX();
                        popY[p] = fitfun->eval(popX.col(p)); // compute fitness
                    }
                }
            }
        }
    }

    void init() {
        popX = mat(dim, popsize);
        popY = vec(popsize);
        for (int p = 0; p < popsize; p++) {
            popX.col(p) = fitfun->guess;
            popY[p] = DBL_MAX; // compute fitness
        }
        bestI = 0;
        bestX = popX.col(bestI);
        popIter = zeros(popsize);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int bestI;
    int stop;
    double F0;
    double CR0;
    pcg64 *rs;
    mat popX;
    vec popY;
    vec popIter;
};

// see https://cvstuff.wordpress.com/2014/11/27/wraping-c-code-with-python-ctypes-memory-and-pointers/

}

using namespace l_differential_evolution;

/*
 * Class:     fcmaes_core_Jni
 * Method:    optimizeLDE
 * Signature: (Lfcmaes/core/Fitness;[D[D[D[DIDIDDDJI)I
 */
JNIEXPORT jint JNICALL Java_fcmaes_core_Jni_optimizeLDE(JNIEnv *env, jclass cls,
        jobject func, jdoubleArray jlower, jdoubleArray jupper,
        jdoubleArray jinit, jdoubleArray jsigma, jint maxEvals,
        jdouble stopfitness, jint popsize, jdouble keep, jdouble F, jdouble CR,
        jlong seed, jint runid) {

    double *init = env->GetDoubleArrayElements(jinit, JNI_FALSE);
    double *lower = env->GetDoubleArrayElements(jlower, JNI_FALSE);
    double *upper = env->GetDoubleArrayElements(jupper, JNI_FALSE);
    double *sigma = env->GetDoubleArrayElements(jsigma, JNI_FALSE);
    int dim = env->GetArrayLength(jinit);

    vec guess(dim), lower_limit(dim), upper_limit(dim), inputSigma(dim);
    bool useLimit = false;
    for (int i = 0; i < dim; i++) {
        guess[i] = init[i];
        inputSigma[i] = sigma[i];
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    pcg64 *rs = new pcg64(seed);
    CallJava callJava(func, env);
    Fitness fitfun(&callJava, dim, lower_limit, upper_limit, guess, inputSigma,
            *rs);
    LDeOptimizer opt(runid, &fitfun, dim, rs, popsize, maxEvals, keep,
            stopfitness, F, CR);
    try {
        opt.doOptimize();
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();

        for (int i = 0; i < dim; i++)
            init[i] = bestX[i];

        env->SetDoubleArrayRegion(jinit, 0, dim, (jdouble*) init);
        env->ReleaseDoubleArrayElements(jinit, init, 0);
        env->ReleaseDoubleArrayElements(jupper, upper, 0);
        env->ReleaseDoubleArrayElements(jlower, lower, 0);
        env->ReleaseDoubleArrayElements(jsigma, sigma, 0);
        return fitfun.getEvaluations();

    } catch (std::exception &e) {
        cout << e.what() << endl;
        return fitfun.getEvaluations();
    }
    return 0;
}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <numeric>
#include <unordered_map>
#include <vector>

#include <misc3d/common/normal_estimation.h>
#include <misc3d/logging.h>
#include <misc3d/utils.h>

namespace misc3d {
namespace common {

void VectorToPointer(const std::vector<Eigen::Vector3d> &data, double *ptr) {
    const size_t num = data.size();
#pragma omp parallel for
    for (int i = 0; i < num; i++) {
        ptr[3 * i] = data[i](0);
        ptr[3 * i + 1] = data[i](1);
        ptr[3 * i + 2] = data[i](2);
    }
}

void PointerToVector(const double *ptr, int num,
                     std::vector<Eigen::Vector3d> &data) {
    data.resize(num);
#pragma omp parallel for
    for (int i = 0; i < num; i++) {
        Eigen::Vector3d d(ptr[i * 3], ptr[i * 3 + 1], ptr[i * 3 + 2]);
        data[i] = d;
    }
}

template <typename T0, typename T1>
void SumDense(const T0 *data, const bool *mask, const unsigned int w,
              const unsigned int h, const unsigned int k, T1 *dst) {
    const size_t range_rows[2] = {k, h - k}, range_cols[2] = {k + 1, w - k},
                 double_k = 2 * k;
#pragma omp parallel for
    for (int r = (int)range_rows[0]; r < (int)range_rows[1]; r++) {
        size_t idx_r = (size_t)r * w;
        T1 *ptr = dst + idx_r + k;
        *ptr = 0;
        for (size_t r0 = (size_t)r - k, idx_r0; r0 <= (size_t)r + k; r0++) {
            idx_r0 = r0 * w;
            for (size_t c0 = 0; c0 <= double_k; c0++) {
                *ptr += data[idx_r0 + c0];
            }
        }
        ptr++;
        for (size_t c = range_cols[0]; c < range_cols[1]; c++, ptr++) {
            *ptr = *(ptr - 1);
            for (size_t r0 = (size_t)r - k, c0 = c - k - 1, c1 = c + k;
                 r0 <= (size_t)r + k; r0++) {
                *ptr += *(data + r0 * w + c1) - *(data + r0 * w + c0);
            }
        }
    }
}

void CalcNormalsFromPointMap(const double *xyzs, const unsigned int w,
                             const unsigned int h, const unsigned int k,
                             double *normals, const double view_point[3]) {
    size_t expand_w = w + 2 * k, expand_h = h + 2 * k,
           expand_wh = expand_w * expand_h;
    const size_t range_rows[2] = {k, expand_h - k},
                 range_cols[2] = {k, expand_w - k};
    bool *mask = (bool *)calloc(expand_wh, sizeof(bool));
    double *buffer0 = (double *)calloc(expand_wh * 9, sizeof(double));
    double *x = buffer0;
    double *y = x + expand_wh;
    double *z = y + expand_wh;
    double *xx = z + expand_wh;
    double *xy = xx + expand_wh;
    double *xz = xy + expand_wh;
    double *yy = xz + expand_wh;
    double *yz = yy + expand_wh;
    double *zz = yz + expand_wh;

#pragma omp parallel for
    for (int r = (int)range_rows[0]; r < (int)range_rows[1]; r++) {
        size_t idx_r = (size_t)r * expand_w, idx;
        const double *xyz = xyzs + ((size_t)r - k) * w * 3;
        for (size_t c = range_cols[0]; c < range_cols[1]; c++, xyz += 3) {
            idx = idx_r + c;
            if (xyz[2] == xyz[2]) {
                mask[idx] = true;
                x[idx] = xyz[0];
                y[idx] = xyz[1];
                z[idx] = xyz[2];
                xx[idx] = xyz[0] * xyz[0];
                xy[idx] = xyz[0] * xyz[1];
                xz[idx] = xyz[0] * xyz[2];
                yy[idx] = xyz[1] * xyz[1];
                yz[idx] = xyz[1] * xyz[2];
                zz[idx] = xyz[2] * xyz[2];
            }
        }
    }

    size_t *neighbor_nums = new size_t[expand_wh];
    double *buffer1 = (double *)malloc(expand_wh * 9 * sizeof(double));
    double *sum_x = buffer1;
    double *sum_y = sum_x + expand_wh;
    double *sum_z = sum_y + expand_wh;
    double *sum_xx = sum_z + expand_wh;
    double *sum_xy = sum_xx + expand_wh;
    double *sum_xz = sum_xy + expand_wh;
    double *sum_yy = sum_xz + expand_wh;
    double *sum_yz = sum_yy + expand_wh;
    double *sum_zz = sum_yz + expand_wh;

    SumDense(x, mask, (unsigned int)expand_w, (unsigned int)expand_h, k, sum_x);
    SumDense(y, mask, (unsigned int)expand_w, (unsigned int)expand_h, k, sum_y);
    SumDense(z, mask, (unsigned int)expand_w, (unsigned int)expand_h, k, sum_z);
    SumDense(xx, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             sum_xx);
    SumDense(xy, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             sum_xy);
    SumDense(xz, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             sum_xz);
    SumDense(yy, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             sum_yy);
    SumDense(yz, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             sum_yz);
    SumDense(zz, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             sum_zz);
    SumDense(mask, mask, (unsigned int)expand_w, (unsigned int)expand_h, k,
             neighbor_nums);

#pragma omp parallel for
    for (int r = (int)range_rows[0]; r < (int)range_rows[1]; r++) {
        size_t idx_r = (size_t)r * expand_w, idx;
        for (size_t c = range_cols[0]; c < range_cols[1]; c++) {
            idx = idx_r + c;
            if (!mask[idx])
                continue;
            double scale = 1. / neighbor_nums[idx];
            double hat_x = sum_x[idx] * scale, hat_y = sum_y[idx] * scale,
                   hat_z = sum_z[idx] * scale;
            double covariance[3][3] = {sum_xx[idx] * scale - hat_x * hat_x,
                                       sum_xy[idx] * scale - hat_x * hat_y,
                                       sum_xz[idx] * scale - hat_x * hat_z,
                                       0,
                                       sum_yy[idx] * scale - hat_y * hat_y,
                                       sum_yz[idx] * scale - hat_y * hat_z,
                                       0,
                                       0,
                                       sum_zz[idx] * scale - hat_z * hat_z};
            covariance[1][0] = covariance[0][1];
            covariance[2][0] = covariance[0][2];
            covariance[2][1] = covariance[1][2];

            Eigen::Matrix<double, 3, 3, Eigen::RowMajor | Eigen::DontAlign>
                covariance_matrix(covariance[0]);
            Eigen::SelfAdjointEigenSolver<
                Eigen::Matrix<double, 3, 3, Eigen::RowMajor | Eigen::DontAlign>>
                solver;
            solver.compute(covariance_matrix, Eigen::ComputeEigenvectors);
            Eigen::Vector3d v0 = solver.eigenvectors().col(0);
            double *temp_n = v0.data();
            if ((view_point[0] - x[idx]) * temp_n[0] +
                    (view_point[1] - y[idx]) * temp_n[1] +
                    (view_point[2] - z[idx]) * temp_n[2] <
                0) {
                temp_n[0] *= -1;
                temp_n[1] *= -1;
                temp_n[2] *= -1;
            }
            memcpy(normals + (((size_t)r - k) * w + c - k) * 3, v0.data(),
                   sizeof(double) * 3);
        }
    }

    delete[] neighbor_nums;
    free(buffer0);
    free(buffer1);
    free(mask);
}

void EstimateNormalsFromMap(const PointCloudPtr &pc,
                            const std::tuple<int, int> shape, int k,
                            const std::array<double, 3> &view_point) {
    const size_t num = pc->points_.size();
    const int w = std::get<0>(shape);
    const int h = std::get<1>(shape);

    if (num != w * h) {
        misc3d::LogError(
            "The point cloud size is not equal to given point map size.");
        return;
    }

    double *normals_ptr = new double[num * 3];
    double *points_ptr = new double[num * 3];
    VectorToPointer(pc->points_, points_ptr);
    CalcNormalsFromPointMap(points_ptr, w, h, k, normals_ptr,
                            view_point.data());

    // assign normals
    std::vector<Eigen::Vector3d> normals;
    PointerToVector(normals_ptr, num, normals);
    pc->normals_ = normals;

    delete[] normals_ptr;
    delete[] points_ptr;
}

}  // namespace common
}  // namespace misc3d


#include "config.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polygon_soup_item.h"
#include "Scene_surface_mesh_item.h"
#include <CGAL/Three/Scene_group_item.h>

#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Scene_group_item.h>

#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Fuzzy_sphere.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>

#include <CGAL/linear_least_squares_fitting_3.h>

#include <CGAL/Random.h>
#include <CGAL/Real_timer.h>

#include <CGAL/Shape_detection.h>
#include <CGAL/Regularization.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Alpha_shape_face_base_2.h>
#include <CGAL/Alpha_shape_vertex_base_2.h>

#include <CGAL/structure_point_set.h>

#include <QObject>
#include <QAction>
#include <QMainWindow>
#include <QApplication>
#include <QtPlugin>
#include <QMessageBox>

#include <boost/iterator/function_output_iterator.hpp>

#include "run_with_qprogressdialog.h"

#include "ui_Point_set_shape_detection_plugin.h"

template <typename Shape_detection>
struct Detect_shapes_functor
  : public Functor_with_signal_callback
{
  Shape_detection& shape_detection;
  typename Shape_detection::Parameters& op;

  Detect_shapes_functor (Shape_detection& shape_detection,
                         typename Shape_detection::Parameters& op)
    : shape_detection (shape_detection), op (op)
  { }

  void operator()()
  {
    shape_detection.detect(op, *(this->callback()));
  }
};

struct build_from_pair
{
  Point_set& m_pts;

  build_from_pair (Point_set& pts) : m_pts (pts) { }

  void operator() (const std::pair<Point_set::Point, Point_set::Vector>& pair)
  {
    m_pts.insert (pair.first, pair.second);
  }

};

class Point_set_demo_point_set_shape_detection_dialog : public QDialog, public Ui::PointSetShapeDetectionDialog
{
  Q_OBJECT
public:
  Point_set_demo_point_set_shape_detection_dialog(QWidget * /*parent*/ = nullptr)
  {
    setupUi(this);
    m_normal_tolerance_field->setMaximum(1.0);
    m_probability_field->setRange(0.00001, 1.0);
    m_epsilon_field->setMinimum(0.000001);
    m_normal_tolerance_field->setMinimum(0.01);
    m_cluster_epsilon_field->setMinimum(0.000001);
  }

  bool region_growing() const { return m_region_growing->isChecked(); }
  double cluster_epsilon() const { return m_cluster_epsilon_field->value(); }
  double epsilon() const { return m_epsilon_field->value(); }
  unsigned int min_points() const { return m_min_pts_field->value(); }
  double normal_tolerance() const { return m_normal_tolerance_field->value(); }
  double search_probability() const { return m_probability_field->value(); }
  double gridCellSize() const { return 1.0; }
  bool detect_plane() const { return planeCB->isChecked(); }
  bool detect_sphere() const { return sphereCB->isChecked(); }
  bool detect_cylinder() const { return cylinderCB->isChecked(); }
  bool detect_torus() const { return torusCB->isChecked(); }
  bool detect_cone() const { return coneCB->isChecked(); }
  bool add_property() const { return m_add_property->isChecked(); }
  bool generate_colored_point_set() const { return m_one_colored_point_set->isChecked(); }
  bool generate_subset() const { return m_point_subsets->isChecked(); }
  bool generate_alpha() const { return m_alpha_shapes->isChecked(); }
  bool regularize() const { return m_regularize->isChecked(); }
  bool generate_structured() const { return m_generate_structured->isChecked(); }
};

typedef CGAL::Exact_predicates_inexact_constructions_kernel Epic_kernel;
typedef Epic_kernel::Point_3 Point;
using namespace CGAL::Three;
class Polyhedron_demo_point_set_shape_detection_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
    Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
    Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

  QAction* actionDetect;
  QAction* actionEstimateParameters;
  QAction* actionDetectShapesSM;

  typedef Point_set_3<Kernel>::Point_map PointPMap;
  typedef Point_set_3<Kernel>::Vector_map NormalPMap;

  typedef CGAL::Shape_detection::Efficient_RANSAC_traits<Epic_kernel, Point_set, PointPMap, NormalPMap> Traits;

public:
  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface*) {
    scene = scene_interface;
    mw = mainWindow;
    actionDetect = new QAction(tr("Point Set Shape Detection"), mainWindow);
    actionDetect->setObjectName("actionDetect");
    actionEstimateParameters = new QAction(tr("Point Set Shape Detection (parameter estimation)"), mainWindow);
    actionEstimateParameters->setObjectName("actionEstimateParameters");
    actionDetectShapesSM = new QAction(tr("Surface Mesh Shape Detection"), mainWindow);
    actionDetectShapesSM->setObjectName("actionDetectShapesSM");
    autoConnectActions();
  }

  bool applicable(QAction* action) const {

    Scene_points_with_normal_item* item =
      qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    Scene_surface_mesh_item* sm_item =
      qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));

    if (action->objectName() == "actionDetectShapesSM") {
      if (sm_item)
        return true;
      else return false;
    }
    if (item && item->has_normals()) return true;
    return false;
  }

  QList<QAction*> actions() const {
    return QList<QAction*>() << actionDetect << actionEstimateParameters << actionDetectShapesSM;
  }

  public Q_SLOTS:
    void on_actionDetect_triggered();
    void on_actionEstimateParameters_triggered();
    void on_actionDetectShapesSM_triggered();

private:

  typedef Kernel::Plane_3 Plane_3;
  typedef Kernel::Point_3 Point_3;
  typedef Kernel::Vector_3 Vector_3;

  // RANSAC can handle all types of shapes
  template <typename Traits, typename Shape>
  void add_shape (CGAL::Shape_detection::Efficient_RANSAC<Traits>& ransac,
                  const Shape&)
  {
    ransac.template add_shape_factory<Shape>();
  }

  void detect_shapes_with_region_growing_sm (
    Scene_surface_mesh_item* sm_item,
    Point_set_demo_point_set_shape_detection_dialog& dialog) {

    using Face_range = typename SMesh::Face_range;

    using Neighbor_query =
    CGAL::Shape_detection::Polygon_mesh::One_ring_neighbor_query<SMesh>;
    using Region_type =
    CGAL::Shape_detection::Polygon_mesh::Least_squares_plane_fit_region<Kernel, SMesh>;

    using Vertex_to_point_map = typename Region_type::Vertex_to_point_map;
    using Region_growing = CGAL::Shape_detection::Region_growing<Face_range, Neighbor_query, Region_type>;

    CGAL::Random rand(static_cast<unsigned int>(time(nullptr)));
    const SMesh& mesh = *(sm_item->polyhedron());
    scene->setSelectedItem(-1);
    const Face_range face_range = faces(mesh);

    // Set parameters.
    const double max_distance_to_plane =
    dialog.epsilon();
    const double max_accepted_angle =
    dialog.normal_tolerance();
    const std::size_t min_region_size =
    dialog.min_points();

    // Region growing.
    Neighbor_query neighbor_query(mesh);
    const Vertex_to_point_map vertex_to_point_map(get(CGAL::vertex_point, mesh));
    Region_type region_type(
      mesh,
      max_distance_to_plane, max_accepted_angle, min_region_size,
      vertex_to_point_map);

    Region_growing region_growing(
      face_range, neighbor_query, region_type);

    std::vector< std::vector<std::size_t> > regions;
    region_growing.detect(std::back_inserter(regions));

    std::cerr << "* " << regions.size() <<
    " regions have been found"
    << std::endl;

    // Output result as a new colored item.
    Scene_surface_mesh_item *colored_item = new Scene_surface_mesh_item;
    colored_item->setName(QString("%1 (region growing)").arg(sm_item->name()));
    SMesh& fg = *(colored_item->polyhedron());

    fg = mesh;
    const Face_range fr = faces(fg);

    colored_item->setItemIsMulticolor(true);
    colored_item->computeItemColorVectorAutomatically(false);
    auto& color_vector = colored_item->color_vector();
    color_vector.clear();

    for (std::size_t i = 0; i < regions.size(); ++i) {
      for (const std::size_t idx : regions[i]) {
        const auto fit = fr.begin() + idx;
        fg.property_map<face_descriptor, int>("f:patch_id").first[*fit] =
        static_cast<int>(i);
      }
      CGAL::Random rnd(static_cast<unsigned int>(i));
      color_vector.push_back(QColor(
        64 + rnd.get_int(0, 192),
        64 + rnd.get_int(0, 192),
        64 + rnd.get_int(0, 192)));
    }
    if(color_vector.empty())
    {
      for(const auto& f : faces(fg))
      {
        fg.property_map<face_descriptor, int>("f:patch_id").first[f] =
            static_cast<int>(0);
      }
      CGAL::Random rnd(static_cast<unsigned int>(0));
      color_vector.push_back(QColor(
        64 + rnd.get_int(0, 192),
        64 + rnd.get_int(0, 192),
        64 + rnd.get_int(0, 192)));
    }
    colored_item->invalidateOpenGLBuffers();
    scene->addItem(colored_item);
  }

  void detect_shapes_with_region_growing (
    Scene_points_with_normal_item* item,
    Point_set_demo_point_set_shape_detection_dialog& dialog) {

    using Point_map = typename Point_set::Point_map;
    using Normal_map = typename Point_set::Vector_map;

    using Neighbor_query =
    CGAL::Shape_detection::Point_set::Sphere_neighbor_query<Kernel, Point_set, Point_map>;
    using Region_type =
    CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<Kernel, Point_set, Point_map, Normal_map>;
    using Region_growing =
    CGAL::Shape_detection::Region_growing<Point_set, Neighbor_query, Region_type>;

    // Set parameters.
    const double search_sphere_radius =
    dialog.cluster_epsilon();
    const double max_distance_to_plane =
    dialog.epsilon();
    const double max_accepted_angle =
    dialog.normal_tolerance();
    const std::size_t min_region_size =
    dialog.min_points();

    // Get a point set.
    CGAL::Random rand(static_cast<unsigned int>(time(nullptr)));
    Point_set* points = item->point_set();

    scene->setSelectedItem(-1);
    Scene_points_with_normal_item *colored_item =
    new Scene_points_with_normal_item;

    colored_item->setName(QString("%1 (region growing)").arg(item->name()));
    if (dialog.generate_colored_point_set()) {

      colored_item->point_set()->template add_property_map<unsigned char>("r", 128);
      colored_item->point_set()->template add_property_map<unsigned char>("g", 128);
      colored_item->point_set()->template add_property_map<unsigned char>("b", 128);
      colored_item->point_set()->check_colors();
      scene->addItem(colored_item);
    }
    std::string& comments = item->comments();

    Point_set::Property_map<int> shape_id;
    if (dialog.add_property()) {
      bool added = false;
      boost::tie(shape_id, added) = points->template add_property_map<int> ("shape", -1);
      if (!added) {
        for (auto it = points->begin(); it != points->end(); ++ it)
          shape_id[*it] = -1;
      }

      // Remove previously detected shapes from comments.
      std::string new_comment;

      std::istringstream stream(comments);
      std::string line;
      while (getline(stream, line)) {
        std::string tag;
        std::stringstream iss(line);

        if (iss >> tag && tag == "shape")
          continue;
        new_comment += line + "\n";
      }
      comments = new_comment;
      comments += "shape -1 no assigned shape\n";
    }
    QApplication::setOverrideCursor(Qt::BusyCursor);

    // Region growing set up.
    Neighbor_query neighbor_query(
      *points,
      search_sphere_radius,
      points->point_map());

    Region_type region_type(
      *points,
      max_distance_to_plane, max_accepted_angle, min_region_size,
      points->point_map(), points->normal_map());

    Region_growing region_growing(
      *points, neighbor_query, region_type);

    std::vector<Scene_group_item *> groups;
    groups.resize(1);
    if (dialog.detect_plane()){
      groups[0] = new Scene_group_item("Planes");
      groups[0]->setRenderingMode(Points);
    }

    // The actual shape detection.
    CGAL::Real_timer t;
    t.start();
    std::vector< std::vector<std::size_t> > regions;
    region_growing.detect(std::back_inserter(regions));
    t.stop();

    std::cout << regions.size() <<
      " shapes found in " << t.time() << " second(s)" << std::endl;

    std::vector<Plane_3> planes;
    CGAL::Shape_detection::internal::create_planes_from_points(
      *points, points->point_map(), regions, planes);

    if (dialog.regularize()) {

      std::cerr << "Regularization of planes... " << std::endl;
      CGAL::regularize_planes(
        *points,
        points->point_map(),
        planes,
        CGAL::Identity_property_map<Plane_3>(),
        CGAL::Shape_detection::RG::Point_to_shape_index_map(*points, regions),
        true, true, true, true,
        max_accepted_angle,
        max_distance_to_plane);

      std::cerr << "done" << std::endl;
    }
    std::map<Point_3, QColor> color_map;

    int index = 0;
    for (const auto& plane : planes) {

      if (dialog.add_property()) {
        std::ostringstream oss;
        oss << "shape " << index;
        oss << " plane " << plane << std::endl;
        comments += oss.str();
      }
      Scene_points_with_normal_item *point_item =
      new Scene_points_with_normal_item;

      for (const std::size_t idx : regions[index]) {
        point_item->point_set()->insert(points->point(*(points->begin() + idx)));
        if (dialog.add_property())
          shape_id[*(points->begin() + idx)] = index;
      }

      unsigned char r, g, b;
      r = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      g = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      b = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      point_item->setRgbColor(r, g, b);

      std::size_t nb_colored_pts = 0;
      if (dialog.generate_colored_point_set()) {
        for(std::size_t idx : regions[index]) {
          auto it = colored_item->point_set()->insert(
            points->point(*(points->begin() + idx)));
          ++nb_colored_pts;
          colored_item->point_set()->set_color(*it, r, g, b);
        }
        colored_item->invalidateOpenGLBuffers();
      }

      // Providing a useful name consisting of the order of detection,
      // name of type and number of inliers.
      std::stringstream ss;
      ss << item->name().toStdString() << "_plane_";

      Vector_3 plane_normal = plane.orthogonal_vector();
      const double normal_length = CGAL::sqrt(plane_normal.squared_length());
      CGAL_precondition(normal_length > 0.0);
      plane_normal /= normal_length;

      Kernel::Point_3 ref = CGAL::ORIGIN + plane_normal;
      if (color_map.find(ref) == color_map.end()) {

        ref = CGAL::ORIGIN + (-1.0) * plane_normal;
        if (color_map.find(ref) == color_map.end())
          color_map[ref] = point_item->color();
        else
          point_item->setColor(color_map[ref]);

      } else point_item->setColor(color_map[ref]);

      if (dialog.generate_colored_point_set()) {
        for (std::size_t i = 0; i < nb_colored_pts; ++i) {
          colored_item->point_set()->set_color(
            *(colored_item->point_set()->end() - 1 - i),
            color_map[ref].red(),
            color_map[ref].green(),
            color_map[ref].blue());
        }
      }
      ss << "(" << ref << ")_";

      if (dialog.generate_alpha()) {
        // If plane, build alpha shape
        Scene_surface_mesh_item* sm_item = nullptr;
        sm_item = new Scene_surface_mesh_item;

        using Plane = CGAL::Shape_detection::RG::Plane<Kernel>;
        boost::shared_ptr<Plane> rg_plane(new Plane(*points, points->point_map(), regions[index], plane));
        build_alpha_shape(
          *(point_item->point_set()), rg_plane,
          sm_item, search_sphere_radius);

        if (sm_item){
          sm_item->setColor(point_item->color ());
          sm_item->setName(QString("%1%2_alpha_shape").arg(QString::fromStdString(ss.str()))
          .arg(QString::number(regions[index].size())));
          sm_item->setRenderingMode(Flat);
          sm_item->invalidateOpenGLBuffers();
          scene->addItem(sm_item);
          if (scene->item_id(groups[0]) == -1)
            scene->addItem(groups[0]);
          scene->changeGroup(sm_item, groups[0]);
        }
      }
      ss << regions[index].size();

      point_item->setName(QString::fromStdString(ss.str()));
      point_item->setRenderingMode(item->renderingMode());

      if (dialog.generate_subset()){
        point_item->invalidateOpenGLBuffers();
        scene->addItem(point_item);
        point_item->point_set()->add_normal_map();

        // Set normals for point_item to the plane's normal.
        for (auto it = point_item->point_set()->begin();
        it != point_item->point_set()->end(); ++it)
          point_item->point_set()->normal(*it) = plane_normal;

        if (scene->item_id(groups[0]) == -1)
          scene->addItem(groups[0]);
        point_item->invalidateOpenGLBuffers();
        scene->changeGroup(point_item, groups[0]);
      }
      else delete point_item;
      ++index;
    }

    Q_FOREACH(Scene_group_item* group, groups)
      if(group && group->getChildren().empty())
        delete group;

    if (dialog.generate_structured()) {
      std::cerr << "Structuring point set... ";

      Scene_points_with_normal_item *pts_full = new Scene_points_with_normal_item;
      pts_full->point_set()->add_normal_map();

      CGAL::structure_point_set(
        *points,
        planes,
        boost::make_function_output_iterator(build_from_pair((*(pts_full->point_set())))),
        search_sphere_radius,
        points->parameters().
        plane_map(CGAL::Identity_property_map<Plane_3>()).
        plane_index_map(CGAL::Shape_detection::RG::Point_to_shape_index_map(*points, regions)));

      if (pts_full->point_set()->empty())
        delete pts_full;
      else {
        pts_full->point_set()->unselect_all();
        pts_full->setName(tr("%1 (structured)").arg(item->name()));
        pts_full->setRenderingMode(PointsPlusNormals);
        pts_full->setColor(Qt::blue);
        pts_full->invalidateOpenGLBuffers();
        scene->addItem(pts_full);
      }
      std::cerr << "done" << std::endl;
    }

    // Updates scene.
    scene->itemChanged(index);
    QApplication::restoreOverrideCursor();
    item->setVisible(false);
  }

  void detect_shapes_with_ransac (Scene_points_with_normal_item* item,
                      Point_set_demo_point_set_shape_detection_dialog& dialog)
  {
    typedef Point_set::Point_map PointPMap;
    typedef Point_set::Vector_map NormalPMap;

    typedef CGAL::Shape_detection::Efficient_RANSAC_traits<Epic_kernel, Point_set, PointPMap, NormalPMap> Traits;
    typedef CGAL::Shape_detection::Efficient_RANSAC<Traits> Ransac;

    Ransac::Parameters op;
    op.probability = dialog.search_probability();       // probability to miss the largest primitive on each iteration.
    op.min_points = dialog.min_points();          // Only extract shapes with a minimum number of points.
    op.epsilon = dialog.epsilon();          // maximum euclidean distance between point and shape.
    op.cluster_epsilon = dialog.cluster_epsilon();    // maximum euclidean distance between points to be clustered.
    op.normal_threshold = std::cos(CGAL_PI * dialog.normal_tolerance() / 180.);   // normal_threshold < dot(surface_normal, point_normal);

    CGAL::Random rand(static_cast<unsigned int>(time(nullptr)));
    // Gets point set
    Point_set* points = item->point_set();

    Scene_points_with_normal_item::Bbox bb = item->bbox();

    double diam = CGAL::sqrt((bb.xmax()-bb.xmin())*(bb.xmax()-bb.xmin()) + (bb.ymax()-bb.ymin())*(bb.ymax()-bb.ymin()) + (bb.zmax()-bb.zmin())*(bb.zmax()-bb.zmin()));

    scene->setSelectedItem(-1);
    Scene_points_with_normal_item *colored_item
      = new Scene_points_with_normal_item;
    colored_item->setName (QString("%1 (ransac)").arg(item->name()));
    if (dialog.generate_colored_point_set())
    {
      colored_item->point_set()->template add_property_map<unsigned char>("r", 128);
      colored_item->point_set()->template add_property_map<unsigned char>("g", 128);
      colored_item->point_set()->template add_property_map<unsigned char>("b", 128);
      colored_item->point_set()->check_colors();
      scene->addItem(colored_item);
    }

    std::string& comments = item->comments();

    Point_set::Property_map<int> shape_id;
    if (dialog.add_property())
    {
      bool added = false;
      boost::tie (shape_id, added) = points->template add_property_map<int> ("shape", -1);
      if (!added)
      {
        for (Point_set::iterator it = points->begin(); it != points->end(); ++ it)
          shape_id[*it] = -1;
      }

      // Remove previously detected shapes from comments
      std::string new_comment;

      std::istringstream stream (comments);
      std::string line;
      while (getline(stream, line))
      {
        std::stringstream iss (line);
        std::string tag;
        if (iss >> tag && tag == "shape")
          continue;
        new_comment += line + "\n";
      }
      comments = new_comment;
      comments += "shape -1 no assigned shape\n";
    }

    QApplication::setOverrideCursor(Qt::BusyCursor);

    Ransac ransac;
    ransac.set_input(*points, points->point_map(), points->normal_map());

    std::vector<Scene_group_item *> groups;
    groups.resize(5);
    // Shapes to be searched for are registered by using the template Shape_factory
    if(dialog.detect_plane()){
      groups[0] = new Scene_group_item("Planes");
      groups[0]->setRenderingMode(Points);
      add_shape<Traits> (ransac, CGAL::Shape_detection::Plane<Traits>());
    }
    if(dialog.detect_cylinder()){
      groups[1] = new Scene_group_item("Cylinders");
      groups[1]->setRenderingMode(Points);
      add_shape<Traits> (ransac, CGAL::Shape_detection::Cylinder<Traits>());
    }
    if(dialog.detect_torus()){
      groups[2] = new Scene_group_item("Torus");
      groups[2]->setRenderingMode(Points);
      add_shape<Traits> (ransac, CGAL::Shape_detection::Torus<Traits>());
    }
    if(dialog.detect_cone()){
      groups[3] = new Scene_group_item("Cones");
      groups[3]->setRenderingMode(Points);
      add_shape<Traits> (ransac, CGAL::Shape_detection::Cone<Traits>());
    }
    if(dialog.detect_sphere()){
      groups[4] = new Scene_group_item("Spheres");
      groups[4]->setRenderingMode(Points);
      add_shape<Traits> (ransac, CGAL::Shape_detection::Sphere<Traits>());
    }

    // The actual shape detection.
    CGAL::Real_timer t;
    t.start();
    Detect_shapes_functor<Ransac> functor (ransac, op);
    run_with_qprogressdialog<CGAL::Sequential_tag> (functor, "Detecting shapes...", mw);
    t.stop();

    std::cout << ransac.shapes().size() << " shapes found in "
              << t.time() << " second(s)" << std::endl;

    if (dialog.regularize ())
      {
        std::cerr << "Regularization of planes... " << std::endl;
        typename Ransac::Plane_range planes = ransac.planes();
        CGAL::regularize_planes (*points,
                                 points->point_map(),
                                 planes,
                                 CGAL::Shape_detection::Plane_map<Traits>(),
                                 CGAL::Shape_detection::Point_to_shape_index_map<Traits>(*points, planes),
                                 true, true, true, true,
                                 op.normal_threshold, op.epsilon);

        std::cerr << "done" << std::endl;
      }

    std::map<Kernel::Point_3, QColor> color_map;

    int index = 0;
    for(boost::shared_ptr<typename Ransac::Shape> shape : ransac.shapes())
    {
      CGAL::Shape_detection::Cylinder<Traits> *cyl;
      cyl = dynamic_cast<CGAL::Shape_detection::Cylinder<Traits> *>(shape.get());
      if (cyl != nullptr){
        if(cyl->radius() > diam){
          continue;
        }
      }

      if (dialog.add_property())
      {
        std::ostringstream oss;
        oss << "shape " << index;
        if (CGAL::Shape_detection::Plane<Traits>* s
            = dynamic_cast<CGAL::Shape_detection::Plane<Traits> *>(shape.get()))
          oss << " plane " << Kernel::Plane_3(*s) << std::endl;
        else if (CGAL::Shape_detection::Cylinder<Traits>* s
            = dynamic_cast<CGAL::Shape_detection::Cylinder<Traits> *>(shape.get()))
          oss << " cylinder axis = [" << s->axis() << "] radius = " << s->radius() << std::endl;
        else if (CGAL::Shape_detection::Cone<Traits>* s
            = dynamic_cast<CGAL::Shape_detection::Cone<Traits> *>(shape.get()))
          oss << " cone apex = [" << s->apex() << "] axis = [" << s->axis()
              << "] angle = " << s->angle() << std::endl;
        else if (CGAL::Shape_detection::Torus<Traits>* s
            = dynamic_cast<CGAL::Shape_detection::Torus<Traits> *>(shape.get()))
          oss << " torus center = [" << s->center() << "] axis = [" << s->axis()
              << "] R = " << s->major_radius() << " r = " << s->minor_radius() << std::endl;
        else if (CGAL::Shape_detection::Sphere<Traits>* s
            = dynamic_cast<CGAL::Shape_detection::Sphere<Traits> *>(shape.get()))
          oss << " sphere center = [" << s->center() << "] radius = " << s->radius() << std::endl;

        comments += oss.str();
      }

      Scene_points_with_normal_item *point_item = new Scene_points_with_normal_item;

      for(std::size_t i : shape->indices_of_assigned_points())
      {
        point_item->point_set()->insert(points->point(*(points->begin()+i)));
        if (dialog.add_property())
          shape_id[*(points->begin()+i)] = index;
      }

      unsigned char r, g, b;

      r = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      g = static_cast<unsigned char>(64 + rand.get_int(0, 192));
      b = static_cast<unsigned char>(64 + rand.get_int(0, 192));

      point_item->setRgbColor(r, g, b);

      std::size_t nb_colored_pts = 0;
      if (dialog.generate_colored_point_set())
      {
        for(std::size_t i : shape->indices_of_assigned_points())
        {
          Point_set::iterator it = colored_item->point_set()->insert(points->point(*(points->begin()+i)));
          ++ nb_colored_pts;
          colored_item->point_set()->set_color(*it, r, g, b);
        }
        colored_item->invalidateOpenGLBuffers();
      }

      // Providing a useful name consisting of the order of detection, name of type and number of inliers
      std::stringstream ss;
      if (dynamic_cast<CGAL::Shape_detection::Cylinder<Traits> *>(shape.get())){
        CGAL::Shape_detection::Cylinder<Traits> * cyl
          = dynamic_cast<CGAL::Shape_detection::Cylinder<Traits> *>(shape.get());
        ss << item->name().toStdString() << "_cylinder_" << cyl->radius() << "_";
      }
      else if (dynamic_cast<CGAL::Shape_detection::Plane<Traits> *>(shape.get()))
        {
          ss << item->name().toStdString() << "_plane_";

          boost::shared_ptr<CGAL::Shape_detection::Plane<Traits> > pshape
            = boost::dynamic_pointer_cast<CGAL::Shape_detection::Plane<Traits> > (shape);

          Kernel::Point_3 ref = CGAL::ORIGIN + pshape->plane_normal ();

          if (color_map.find (ref) == color_map.end ())
            {
              ref = CGAL::ORIGIN + (-1.) * pshape->plane_normal ();
              if (color_map.find (ref) == color_map.end ())
                color_map[ref] = point_item->color ();
              else
                point_item->setColor (color_map[ref]);
            }
          else
            point_item->setColor (color_map[ref]);

          if (dialog.generate_colored_point_set())
          {
            for (std::size_t i = 0; i < nb_colored_pts; ++ i)
            {
              colored_item->point_set()->set_color(*(colored_item->point_set()->end() - 1 - i), color_map[ref].red(),
                                                   color_map[ref].green(),
                                                   color_map[ref].blue());
            }
          }

          ss << "(" << ref << ")_";

          if (dialog.generate_alpha ())
            {
              // If plane, build alpha shape
              Scene_surface_mesh_item* sm_item = nullptr;
                sm_item = new Scene_surface_mesh_item;

              build_alpha_shape (*(point_item->point_set()), pshape,
                                 sm_item, dialog.cluster_epsilon());

              if(sm_item){
                sm_item->setColor(point_item->color ());
                sm_item->setName(QString("%1%2_alpha_shape").arg(QString::fromStdString(ss.str()))
                                   .arg (QString::number (shape->indices_of_assigned_points().size())));
                sm_item->setRenderingMode (Flat);
                sm_item->invalidateOpenGLBuffers();
                scene->addItem(sm_item);
                if(scene->item_id(groups[0]) == -1)
                  scene->addItem(groups[0]);
                scene->changeGroup(sm_item, groups[0]);
              }
            }
        }
      else if (dynamic_cast<CGAL::Shape_detection::Cone<Traits> *>(shape.get()))
        ss << item->name().toStdString() << "_cone_";
      else if (dynamic_cast<CGAL::Shape_detection::Torus<Traits> *>(shape.get()))
        ss << item->name().toStdString() << "_torus_";
      else if (dynamic_cast<CGAL::Shape_detection::Sphere<Traits> *>(shape.get()))
        ss << item->name().toStdString() << "_sphere_";
      ss << shape->indices_of_assigned_points().size();

      //names[i] = ss.str(
      point_item->setName(QString::fromStdString(ss.str()));
      point_item->setRenderingMode(item->renderingMode());

      if (dialog.generate_subset()){
        point_item->invalidateOpenGLBuffers();
        scene->addItem(point_item);
        if (dynamic_cast<CGAL::Shape_detection::Cylinder<Traits> *>(shape.get()))
        {
          if(scene->item_id(groups[1]) == -1)
             scene->addItem(groups[1]);
          scene->changeGroup(point_item, groups[1]);
        }
        else if (dynamic_cast<CGAL::Shape_detection::Plane<Traits> *>(shape.get()))
        {
          point_item->point_set()->add_normal_map();
          CGAL::Shape_detection::Plane<Traits> * plane = dynamic_cast<CGAL::Shape_detection::Plane<Traits> *>(shape.get());
          //set normals for point_item to the plane's normal
          for(Point_set::iterator it = point_item->point_set()->begin(); it != point_item->point_set()->end(); ++it)
            point_item->point_set()->normal(*it) = plane->plane_normal();

          if(scene->item_id(groups[0]) == -1)
            scene->addItem(groups[0]);

          point_item->invalidateOpenGLBuffers();
          scene->changeGroup(point_item, groups[0]);
        }
        else if (dynamic_cast<CGAL::Shape_detection::Cone<Traits> *>(shape.get()))
        {
          if(scene->item_id(groups[3]) == -1)
             scene->addItem(groups[3]);
          scene->changeGroup(point_item, groups[3]);
        }
        else if (dynamic_cast<CGAL::Shape_detection::Torus<Traits> *>(shape.get()))
        {
          if(scene->item_id(groups[2]) == -1)
             scene->addItem(groups[2]);
          scene->changeGroup(point_item, groups[2]);
        }
        else if (dynamic_cast<CGAL::Shape_detection::Sphere<Traits> *>(shape.get()))
        {
          if(scene->item_id(groups[4]) == -1)
             scene->addItem(groups[4]);
          scene->changeGroup(point_item, groups[4]);
        }
      }
      else
        delete point_item;

      ++index;
    }
    Q_FOREACH(Scene_group_item* group, groups)
      if(group && group->getChildren().empty())
        delete group;

    if (dialog.generate_structured ())
      {
        std::cerr << "Structuring point set... ";

        Scene_points_with_normal_item *pts_full = new Scene_points_with_normal_item;
        pts_full->point_set()->add_normal_map();

        typename Ransac::Plane_range planes = ransac.planes();
        CGAL::structure_point_set (*points,
                                   planes,
                                   boost::make_function_output_iterator (build_from_pair ((*(pts_full->point_set())))),
                                   op.cluster_epsilon,
                                   points->parameters().
                                   plane_map(CGAL::Shape_detection::Plane_map<Traits>()).
                                   plane_index_map(CGAL::Shape_detection::Point_to_shape_index_map<Traits>(*points, planes)));

        if (pts_full->point_set ()->empty ())
          delete pts_full;
        else
          {
            pts_full->point_set ()->unselect_all();
            pts_full->setName(tr("%1 (structured)").arg(item->name()));
            pts_full->setRenderingMode(PointsPlusNormals);
            pts_full->setColor(Qt::blue);
            pts_full->invalidateOpenGLBuffers();
            scene->addItem (pts_full);
          }
        std::cerr << "done" << std::endl;
      }

    // Updates scene
    scene->itemChanged(index);

    QApplication::restoreOverrideCursor();

    item->setVisible(false);
  }

  Kernel::Point_2 to_2d (const Point_3& centroid,
                         const Vector_3& base1,
                         const Vector_3& base2,
                         const Point_3& query)
  {
    Vector_3 v (centroid, query);
    return Kernel::Point_2 (v * base1, v * base2);
  }

  Point_3 to_3d (const Point_3& centroid,
                 const Vector_3& base1,
                 const Vector_3& base2,
                 const Kernel::Point_2& query)
  {
    return centroid + query.x() * base1 + query.y() * base2;
  }

    template<typename Plane>
    void build_alpha_shape (Point_set& points, boost::shared_ptr<Plane> plane,
                          Scene_surface_mesh_item* sm_item, double epsilon);

}; // end Polyhedron_demo_point_set_shape_detection_plugin

void Polyhedron_demo_point_set_shape_detection_plugin::on_actionDetectShapesSM_triggered() {

  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_surface_mesh_item* sm_item =
  qobject_cast<Scene_surface_mesh_item*>(scene->item(index));

  if(sm_item) {

    // Get a surface mesh.
    SMesh* mesh = sm_item->polyhedron();
    if(mesh == nullptr) return;

    Point_set_demo_point_set_shape_detection_dialog dialog;

    dialog.ransac->setEnabled(false);
    dialog.m_regularize->setEnabled(false);
    dialog.m_generate_structured->setEnabled(false);
    dialog.label_4->setEnabled(false);
    dialog.m_cluster_epsilon_field->setEnabled(false);
    dialog.groupBox_3->setEnabled(false);
    //todo: check default values
    dialog.m_epsilon_field->setValue(0.01*sm_item->diagonalBbox());
    std::size_t nb_faces = mesh->number_of_faces();
    dialog.m_min_pts_field->setValue((std::max)(static_cast<int>(0.01*nb_faces), 1));
    if(!dialog.exec()) return;

    if(dialog.min_points() > static_cast<unsigned int>(nb_faces))
      dialog.m_min_pts_field->setValue(static_cast<unsigned int>(nb_faces));
    QApplication::setOverrideCursor(Qt::WaitCursor);
    if (dialog.region_growing()) {
      detect_shapes_with_region_growing_sm(sm_item, dialog);
    }

    dialog.ransac->setEnabled(true);
    dialog.m_regularize->setEnabled(true);
    dialog.m_generate_structured->setEnabled(true);
    dialog.label_4->setEnabled(true);
    dialog.m_cluster_epsilon_field->setEnabled(true);
    dialog.groupBox_3->setEnabled(true);

    // Update scene.
    scene->itemChanged(index);
    QApplication::restoreOverrideCursor();
    sm_item->setVisible(false);
  }
}

void Polyhedron_demo_point_set_shape_detection_plugin::on_actionDetect_triggered() {

  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_points_with_normal_item* item =
    qobject_cast<Scene_points_with_normal_item*>(scene->item(index));

  if(item)
    {
      // Gets point set
      Point_set* points = item->point_set();

      if(points == nullptr)
        return;

      //Epic_kernel::FT diag = sqrt(((points->bounding_box().max)() - (points->bounding_box().min)()).squared_length());

      // Gets options
      Point_set_demo_point_set_shape_detection_dialog dialog;
      if(!dialog.exec())
        return;
      if(dialog.min_points() > static_cast<unsigned int>(points->size()))
        dialog.m_min_pts_field->setValue(static_cast<unsigned int>(points->size()));

      QApplication::setOverrideCursor(Qt::WaitCursor);
      if (dialog.region_growing())
      {
        detect_shapes_with_region_growing(item, dialog);
      }
      else
      {
        detect_shapes_with_ransac(item, dialog);
      }

      // Updates scene
      scene->itemChanged(index);

      QApplication::restoreOverrideCursor();

      item->setVisible(false);
    }
}

template<typename Plane>
void Polyhedron_demo_point_set_shape_detection_plugin::build_alpha_shape
(Point_set& points,  boost::shared_ptr<Plane> plane, Scene_surface_mesh_item* sm_item, double epsilon)
{
  typedef Kernel::Point_2  Point_2;
  typedef CGAL::Alpha_shape_vertex_base_2<Kernel> Vb;
  typedef CGAL::Alpha_shape_face_base_2<Kernel>  Fb;
  typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
  typedef CGAL::Delaunay_triangulation_2<Kernel,Tds> Triangulation_2;
  typedef CGAL::Alpha_shape_2<Triangulation_2>  Alpha_shape_2;

  std::vector<Point_2> projections;
  projections.reserve (points.size ());

  for (Point_set::const_iterator it = points.begin(); it != points.end(); ++ it)
    projections.push_back (plane->to_2d (points.point(*it)));

  Alpha_shape_2 ashape (projections.begin (), projections.end (), epsilon);

  std::map<Alpha_shape_2::Vertex_handle, std::size_t> map_v2i;

  Scene_polygon_soup_item *soup_item = new Scene_polygon_soup_item;

  soup_item->init_polygon_soup(points.size(), ashape.number_of_faces ());
  std::size_t current_index = 0;

  for (Alpha_shape_2::Finite_faces_iterator it = ashape.finite_faces_begin ();
       it != ashape.finite_faces_end (); ++ it)
    {
      if (ashape.classify (it) != Alpha_shape_2::INTERIOR)
        continue;

      for (int i = 0; i < 3; ++ i)
        {
          if (map_v2i.find (it->vertex (i)) == map_v2i.end ())
            {
              map_v2i.insert (std::make_pair (it->vertex (i), current_index ++));
              Point p = plane->to_3d (it->vertex (i)->point ());
              soup_item->new_vertex (p.x (), p.y (), p.z ());
            }
        }
      soup_item->new_triangle (map_v2i[it->vertex (0)],
                               map_v2i[it->vertex (1)],
                               map_v2i[it->vertex (2)]);
    }

  soup_item->orient();
  if(sm_item){
    soup_item->exportAsSurfaceMesh (sm_item->polyhedron());
  }

  if (soup_item->isEmpty ())
    {
      std::cerr << "POLYGON SOUP EMPTY" << std::endl;
      for (std::size_t i = 0; i < projections.size (); ++ i)
        std::cerr << projections[i] << std::endl;

    }

  delete soup_item;
}

void Polyhedron_demo_point_set_shape_detection_plugin::on_actionEstimateParameters_triggered() {

  CGAL::Random rand(static_cast<unsigned int>(time(nullptr)));
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_points_with_normal_item* item =
    qobject_cast<Scene_points_with_normal_item*>(scene->item(index));

  if(item)
    {
      // Gets point set
      Point_set* points = item->point_set();

      if(points == nullptr)
        return;

      if (points->nb_selected_points() == 0)
        {
          QMessageBox::information(nullptr,
                                   tr("Warning"),
                                   tr("Selection is empty.\nTo estimate parameters, please select a planar section."));
          return;
        }

      QApplication::setOverrideCursor(Qt::WaitCursor);

      typedef CGAL::Search_traits_3<Kernel> SearchTraits_3;
      typedef CGAL::Search_traits_adapter <Point_set::Index,
                                           Point_set::Point_map, SearchTraits_3> Search_traits;
      typedef CGAL::Orthogonal_k_neighbor_search<Search_traits> Neighbor_search;
      typedef Neighbor_search::Tree Tree;
      typedef Neighbor_search::Distance Distance;

      // build kdtree
      Tree tree(points->first_selected(),
                points->end(),
                Tree::Splitter(),
                Search_traits (points->point_map())
                );
      Distance tr_dist(points->point_map());

      Plane_3 plane;
      CGAL::linear_least_squares_fitting_3(boost::make_transform_iterator
                                           (points->first_selected(),
                                            CGAL::Property_map_to_unary_function<Point_set::Point_map>
                                            (points->point_map())),
                                           boost::make_transform_iterator
                                           (points->end(),
                                            CGAL::Property_map_to_unary_function<Point_set::Point_map>
                                            (points->point_map())),
                                           plane,
                                           CGAL::Dimension_tag<0>());

      std::vector<double> epsilon, dispersion, cluster_epsilon;

      Vector_3 norm = plane.orthogonal_vector();
      norm = norm / std::sqrt (norm * norm);
      for (Point_set::iterator it = points->first_selected(); it != points->end(); ++ it)
        {
          double dist = CGAL::squared_distance (plane, points->point(*it));
          epsilon.push_back(dist);

          double disp = std::fabs (norm * points->normal(*it));
          dispersion.push_back (disp);

          Neighbor_search search(tree, points->point(*it), 2, 0, true, tr_dist);
          Neighbor_search::iterator nit = search.begin();
          ++ nit;
          double eps = nit->second;
          cluster_epsilon.push_back(eps);
        }

      std::sort (epsilon.begin(), epsilon.end());
      std::sort (dispersion.begin(), dispersion.end());
      std::sort (cluster_epsilon.begin(), cluster_epsilon.end());

      QApplication::restoreOverrideCursor();

      QMessageBox::information(nullptr,
                               tr("Estimated Parameters"),
                               tr("Epsilon = [%1 ; %2 ; %3 ; %4 ; %5]\nNormal Tolerance = [%6 ; %7 ; %8 ; %9 ; %10]\nMinimum Number of Points = %11\nConnectivity Epsilon = [%12 ; %13 ; %14 ; %15 ; %16]")
                               .arg(std::sqrt(epsilon.front()))
                               .arg(std::sqrt(epsilon[epsilon.size() / 10]))
                               .arg(std::sqrt(epsilon[epsilon.size() / 2]))
                               .arg(std::sqrt(epsilon[9 * epsilon.size() / 10]))
                               .arg(std::sqrt(epsilon.back()))
                               .arg(dispersion.back())
                               .arg(dispersion[9 * dispersion.size() / 10])
                               .arg(dispersion[dispersion.size() / 2])
                               .arg(dispersion[dispersion.size() / 10])
                               .arg(dispersion.front())
                               .arg(points->nb_selected_points())
                               .arg(std::sqrt(cluster_epsilon.front()))
                               .arg(std::sqrt(cluster_epsilon[cluster_epsilon.size() / 10]))
                               .arg(std::sqrt(cluster_epsilon[cluster_epsilon.size() / 2]))
                               .arg(std::sqrt(cluster_epsilon[9 * cluster_epsilon.size() / 10]))
                               .arg(std::sqrt(cluster_epsilon.back())));
    }
}

#include <QtPlugin>

#include "Point_set_shape_detection_plugin.moc"


/*
Copyright (c) 2017 InversePalindrome
InPal - CalculusPanel.cpp
InversePalindrome.com
*/

#include "CalculusPanel.hpp"

#include <wx/sizer.h>
#include <wx/button.h>
#include <wx/stattext.h>

#include <boost/format.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/replace.hpp>

CalculusPanel::CalculusPanel(wxWindow* parent, MathDataDefault* mathData) :
    wxPanel(parent, wxID_ANY),
    mathData(mathData),
    derivativeChoice(),
    derivativeEntry(new wxTextCtrl(this, wxID_ANY)),
    xPositionEntry(new wxTextCtrl(this, wxID_ANY)),
    derivativeSolution(new wxTextCtrl(this, wxID_ANY, "", wxDefaultPosition, wxDefaultSize, wxTE_READONLY)),
    integralEntry(new wxTextCtrl(this, wxID_ANY)),
    initialXEntry(new wxTextCtrl(this, wxID_ANY)),
    finalXEntry(new wxTextCtrl(this, wxID_ANY)),
    integralSolution(new wxTextCtrl(this, wxID_ANY, "", wxDefaultPosition, wxDefaultSize, wxTE_READONLY))
{
    SetBackgroundColour(wxColor(128u, 128u, 128u));

    auto* topSizer = new wxBoxSizer(wxVERTICAL);
    auto* derivativeSizer = new wxBoxSizer(wxHORIZONTAL);
    auto* derivativeButtonSizer = new wxBoxSizer(wxHORIZONTAL);
    auto* integralSizer = new wxBoxSizer(wxHORIZONTAL);
    auto* integralButtonSizer = new wxBoxSizer(wxHORIZONTAL);

    auto* derivativeText = new wxStaticText(this, wxID_ANY, "Derivative");
    auto* derivativeFunctionText = new wxStaticText(this, wxID_ANY, "Function");
    auto* xPositionText = new wxStaticText(this, wxID_ANY, "Position(X):");

    auto* integralText = new wxStaticText(this, wxID_ANY, "Integral");
    auto* integralFunctionText = new wxStaticText(this, wxID_ANY, "Function");
    auto* integralInitialXText = new wxStaticText(this, wxID_ANY, "Starting Position(X):");
    auto* integralFinalXText = new wxStaticText(this, wxID_ANY, "Final Position(X):");

    auto& font = wxFont(8u, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);

    derivativeText->SetFont(font);
    xPositionText->SetFont(font);
    derivativeFunctionText->SetFont(font);
    integralText->SetFont(font);
    integralFunctionText->SetFont(font);
    integralInitialXText->SetFont(font);
    integralFinalXText->SetFont(font);

    wxArrayString derivativeChoices;
    derivativeChoices.Add("First Derivative");
    derivativeChoices.Add("Second Derivative");
    derivativeChoices.Add("Third Derivative");

    derivativeChoice = new wxChoice(this, wxID_ANY, wxDefaultPosition, wxDefaultSize, derivativeChoices);
    derivativeChoice->SetStringSelection("First Derivative");

    auto* solveDerivativeButton = new wxButton(this, wxID_ANY, "Solve");
    auto* solveIntegralButton = new wxButton(this, wxID_ANY, "Solve");

    auto* clearDerivativeButton = new wxButton(this, wxID_ANY, "Clear");
    auto* clearIntegralButton = new wxButton(this, wxID_ANY, "Clear");

    derivativeSizer->Add(derivativeChoice, 0u, wxALL, 10u);
    derivativeSizer->Add(xPositionText, 0u, wxALL, 10u);
    derivativeSizer->Add(xPositionEntry, 0u, wxALL, 10u);

    derivativeButtonSizer->Add(solveDerivativeButton, 0u, wxALL, 5u);
    derivativeButtonSizer->Add(clearDerivativeButton, 0u, wxALL, 5u);

    integralSizer->Add(integralInitialXText, 0u, wxALL, 10u);
    integralSizer->Add(initialXEntry, 0u, wxALL, 10u);
    integralSizer->Add(integralFinalXText, 0u, wxALL, 10u);
    integralSizer->Add(finalXEntry, 0u, wxALL, 10u);

    integralButtonSizer->Add(solveIntegralButton, 0u, wxALL, 5u);
    integralButtonSizer->Add(clearIntegralButton, 0u, wxALL, 5u);

    topSizer->AddSpacer(55u);
    topSizer->Add(derivativeText, 0u, wxALL, 5u);
    topSizer->Add(derivativeSolution, 0u, wxEXPAND | wxALL, 5u);
    topSizer->Add(derivativeFunctionText, 0u, wxEXPAND | wxALL, 5u);
    topSizer->Add(derivativeEntry, 0u, wxEXPAND | wxALL, 5u);
    topSizer->Add(derivativeSizer, 0u, wxALIGN_CENTER | wxALL, 5u);
    topSizer->Add(derivativeButtonSizer, 0u, wxALIGN_CENTER | wxALL, 5u);
    topSizer->AddSpacer(30u);
    topSizer->Add(integralText, 0u, wxALL, 5u);
    topSizer->Add(integralSolution, 0u, wxEXPAND | wxALL, 5u);
    topSizer->Add(integralFunctionText, 0u, wxEXPAND | wxALL, 5u);
    topSizer->Add(integralEntry, 0u, wxEXPAND | wxALL, 5u);
    topSizer->Add(integralSizer, 0u, wxALIGN_CENTER | wxALL, 5u);
    topSizer->Add(integralButtonSizer, 0u, wxALIGN_CENTER | wxALL, 5u);

    topSizer->Fit(this);
    topSizer->SetSizeHints(this);

    SetSizer(topSizer);

    solveDerivativeButton->Bind(wxEVT_LEFT_DOWN, &CalculusPanel::OnSolveDerivative, this);
    solveIntegralButton->Bind(wxEVT_LEFT_DOWN, &CalculusPanel::OnSolveIntegral, this);

    clearDerivativeButton->Bind(wxEVT_LEFT_DOWN, &CalculusPanel::OnClearDerivative, this);
    clearIntegralButton->Bind(wxEVT_LEFT_DOWN, &CalculusPanel::OnClearIntegral, this);
}

void CalculusPanel::OnSolveDerivative(wxMouseEvent& event)
{
    auto& functionEquation = this->derivativeEntry->GetValue().ToStdString();

    boost::replace_all(functionEquation, "x", "derivativeVariable");

    this->mathData->mathSolver.setTask(functionEquation);

    long double derivativeVariable;
    try
    {
        derivativeVariable = std::stold(this->xPositionEntry->GetValue().ToStdString());
    }
    catch (const std::invalid_argument & e)
    {
        derivativeVariable = 0;
    }

    this->mathData->mathSolver.addVariable("derivativeVariable", derivativeVariable);

    if (this->mathData->mathSolver.solve())
    {
        long double derivative;

        if (this->derivativeChoice->GetStringSelection() == "First Derivative")
        {
            derivative = this->mathData->mathSolver.getDerivative(derivativeVariable);
        }
        else if (this->derivativeChoice->GetStringSelection() == "Second Derivative")
        {
            derivative = this->mathData->mathSolver.getSecondDerivative(derivativeVariable);
        }
        else if (this->derivativeChoice->GetStringSelection() == "Third Derivative")
        {
            derivative = this->mathData->mathSolver.getThirdDerivative(derivativeVariable);
        }

        auto& result = boost::str(boost::format("%.18f") %
            derivative);

        boost::trim_right_if(result, boost::is_any_of("0"));
        boost::trim_right_if(result, boost::is_any_of("."));

        this->derivativeSolution->SetValue(result);
    }
}

void CalculusPanel::OnSolveIntegral(wxMouseEvent& event)
{
    auto& functionEquation = this->integralEntry->GetValue().ToStdString();

    boost::replace_all(functionEquation, "x", "integralVariable");

    this->mathData->mathSolver.setTask(functionEquation);

    long double integralVariable = 0.0;
    long double initialX;
    long double finalX;

    this->mathData->mathSolver.addVariable("integralVariable", integralVariable);

    try
    {
        initialX = std::stold(this->initialXEntry->GetValue().ToStdString());
        finalX = std::stold(this->finalXEntry->GetValue().ToStdString());
    }
    catch (const std::invalid_argument & e)
    {
        initialX = 0.0;
        finalX = 1.0;
    }

    if (this->mathData->mathSolver.solve())
    {
        auto& result = boost::str(boost::format("%.18f") %
            this->mathData->mathSolver.getIntegral(integralVariable, initialX, finalX));
        boost::trim_right_if(result, boost::is_any_of("0"));
        boost::trim_right_if(result, boost::is_any_of("."));

        this->integralSolution->SetValue(result);
    }
}

void CalculusPanel::OnClearDerivative(wxMouseEvent& event)
{
    this->derivativeSolution->Clear();
    this->derivativeEntry->Clear();
    this->xPositionEntry->Clear();
}

void CalculusPanel::OnClearIntegral(wxMouseEvent& event)
{
    this->integralSolution->Clear();
    this->integralEntry->Clear();
    this->initialXEntry->Clear();
    this->finalXEntry->Clear();
}


/**
 * \file dcs/math/random/uniform_real_adaptor.hpp
 *
 * \brief Adaptor for generating random numbers in [0,1] from a base generator.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_RANDOM_UNIFORM_REAL_ADAPTOR_HPP
#define DCS_MATH_RANDOM_UNIFORM_REAL_ADAPTOR_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost library version >= 1.15"
#endif // DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION

#include <boost/random/uniform_real_distribution.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <dcs/type_traits/remove_reference.hpp>
#include <dcs/util/holder.hpp>
#include <limits>

namespace dcs { namespace math { namespace random {

/**
 * \brief Adaptor for generating random numbers uniformly distributed in a real
 *  interval from a base random number generator.
 *
 * \tparam BaseRandomGeneratorT The base random number generator type.
 * \tparam RealT The result type.
 *
 * Wraps a random number generator of type \a BaseGeneratorT for generating
 * random numbers uniformly distributed in real interval.
 * Random numbers are generated according the algorithm provided by
 * \a BaseRandomGeneratorT.
 *
 * \todo Make it a derived class of base_generator.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename BaseRandomGeneratorT, typename RealT>
class uniform_real_adaptor: public base_generator<RealT>
{
	private: typedef base_generator<RealT> base_type;
    public: typedef BaseRandomGeneratorT base_engine_type;
    public: typedef typename ::dcs::type_traits::remove_reference<BaseRandomGeneratorT>::type::result_type input_type;
    public: typedef typename base_type::result_type result_type;
    public: typedef typename base_type::ulonglong_type ulonglong_type;
    private: typedef ::boost::random::uniform_real_distribution<result_type> adaptor_impl_type;

    public: explicit uniform_real_adaptor(result_type min_arg = 0.0, result_type max_arg = 1.0)
        : base_type(),
		  rng_(),
		  impl_(min_arg, max_arg)
//		  min_(min_arg),
//		  max_(max_arg)
    {
		// empty
    }

	/// Seed constructor.
    public: uniform_real_adaptor(input_type seed, result_type min_arg, result_type max_arg)
        : base_type(),
		  rng_(seed),
		  impl_(min_arg, max_arg)
//		  min_(min_arg),
//		  max_(max_arg)
    {
		// empty
    }

	/// Seed constructor.
    public: uniform_real_adaptor(input_type seed)
        : base_type(),
		  rng_(seed),
		  impl_()
    {
//		min_ = static_cast<result_type>((rng_.min)());
//		max_ = static_cast<result_type>((rng_.max)());
    }

	/// Copy constructor.
    public: uniform_real_adaptor(uniform_real_adaptor const& that)
        : base_type(that),
		  rng_(that.rng_),
		  impl_(that.impl_)
//		  min_(that.min_),
//		  max_(that.max_)
    {
		// empty
    }

	/// A Constructor: copy the base generator.
	public: uniform_real_adaptor(base_engine_type rng, result_type min_arg, result_type max_arg)
		: base_type(),
		  rng_(rng),
		  impl_(min_arg, max_arg)
//		  min_(min_arg),
//		  max_(max_arg)
	{
		// empty
	}

	/// A Constructor: copy the base generator.
	public: uniform_real_adaptor(base_engine_type rng)
		: base_type(),
		  rng_(rng),
		  impl_()
//		  min_((rng.min)()),
//		  max_((rng.max)())
	{
		// empty
	}

// Support of reference-to-refernce qualifier is only available from C++0x.
#if __cplusplus >= 201103L

	/// A constructor: move the base generator.
    //public: uniform_real_adaptor(base_engine_type&& rng)
    public: uniform_real_adaptor(base_engine_type& rng, result_type min_arg, result_type max_arg)
        : base_type(),
		  rng_(rng),
		  impl_(min_arg, max_arg)
//		  min_(min_arg),
//		  max_(max_arg)
    {
		// empty
    }

	/// A constructor: move the base generator.
    //public: uniform_real_adaptor(base_engine_type&& rng)
    public: uniform_real_adaptor(base_engine_type& rng)
        : base_type(),
		  rng_(rng),
		  impl_()
//		  min_((rng.min)()),
//		  max_((rng.max)())
    {
		// empty
    }

#else

	/// A constructor: move the base generator.
    public: uniform_real_adaptor(::dcs::util::holder<base_engine_type>& rng, result_type min_arg, result_type max_arg)
        : base_type(),
		  rng_(rng),
		  impl_(min_arg, max_arg)
//		  min_(min_arg),
//		  max_(max_arg)
    {
		// empty
    }

	/// A constructor: move the base generator.
    public: uniform_real_adaptor(::dcs::util::holder<base_engine_type>& rng)
        : base_type(),
		  rng_(rng),
		  impl_()
//		  min_((rng.min)()),
//		  max_((rng.max)())
    {
		// empty
    }

#endif

	public: base_engine_type base() const
	{
		return rng_;
	}

    private: result_type do_generate()
    {
//		result_type factor;
//		result_type result;
//
//		do
//		{
//			factor = (max_-min_)
//					 / (
//							static_cast<result_type>(
//							(rng_.max)()-(rng_.min)()
//						)
//						+ static_cast<result_type>(
//							::std::numeric_limits<input_type>::is_integer
//							? 1
//							: 0
//						)
//			);
//			result = static_cast<result_type>(rng_()-(rng_.min)()) * factor + min_;
//		}
//		while (result >= max_);
//
//		return result;

		return impl_(rng_);
    }

	private: void do_seed()
	{
		this->rng_.seed();
	}

	private: void do_seed(result_type s)
	{
		rng_.seed(s);
	}

	private: void do_discard(ulonglong_type z)
	{
		while (z--)
		{
			rng_();
		}
	}

	private: result_type do_min() const
	{
		return impl_.min();
	}

	private: result_type do_max() const
	{
		return impl_.max();
	}

    private: base_engine_type rng_;
	private: adaptor_impl_type impl_;
};

// Template specialization for avoid auto-wrapping (i.e., when the base
// generator is itself a uniform_real_adaptor)
template <typename RandomNumberGeneratorT, typename RealT>
class uniform_real_adaptor< uniform_real_adaptor<RandomNumberGeneratorT,RealT>, RealT >: public uniform_real_adaptor<RandomNumberGeneratorT,RealT>
{
    public: typedef RandomNumberGeneratorT base_engine_type;
    public: typedef typename ::dcs::type_traits::remove_reference<RandomNumberGeneratorT>::type::result_type input_type;
    public: typedef RealT result_type;
};

}}} // Namespace dcs::math::random

#endif // DCS_MATH_RANDOM_UNIFORM_REAL_ADAPTOR_HPP


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Implements the "Walking Control Based on Step Timing Adaptation" foot
 * trajectory QP. The pdf can be found in https://arxiv.org/abs/1704.01271, and
 * in the `doc/` folder in this repository.
 */

#pragma once

#include <eigen-quadprog/QuadProg.h>
#include <Eigen/Eigen>

namespace reactive_planners
{
/**
 * @brief
 */
class PolynomialEndEffectorTrajectory
{
    /*
     * Private methods
     */
public:
    /** @brief Constructor. */
    PolynomialEndEffectorTrajectory();

    /** @brief Destructor. */
    ~PolynomialEndEffectorTrajectory();

    bool compute(const Eigen::Ref<const Eigen::Vector3d> &start_pose,
                 const Eigen::Ref<const Eigen::Vector3d> &current_pose,
                 const Eigen::Ref<const Eigen::Vector3d> &current_velocity,
                 const Eigen::Ref<const Eigen::Vector3d> &current_acceleration,
                 const Eigen::Ref<const Eigen::Vector3d> &target_pose,
                 const double &start_time,
                 const double &current_time,
                 const double &end_time);

    void get_next_state(const double &next_time,
                        Eigen::Ref<Eigen::Vector3d> next_pose,
                        Eigen::Ref<Eigen::Vector3d> next_velocity,
                        Eigen::Ref<Eigen::Vector3d> next_acceleration);

    /** @brief Display the matrices of the Problem. */
    void print_solver() const;

    /** @brief Convert the inner data to a string format. */
    std::string to_string() const;

    /*
     * Getters
     */

    /** @brief Get the height of the flying foot. */
    double get_mid_air_height()
    {
        return mid_air_height_;
    }

    /** @brief Get the last end time taken into account during the foot
     * trajectory computation. */
    double get_last_end_time_taken_into_account()
    {
        return last_end_time_seen_;
    }

    /*
     * Setters
     */

    /** @brief Set the height of the flying foot.
     *
     * @param mid_air_height
     */
    void set_mid_air_height(double mid_air_height)
    {
        mid_air_height_ = mid_air_height;
    }
    /** @brief Set the costs of x, y, z axes, and hessian regularization.
     *
     * @param cost_x
     * @param cost_y
     * @param cost_z
     * @param hess_regularization
     */
    void set_costs(double cost_x,
                   double cost_y,
                   double cost_z,
                   double hess_regularization)
    {
        cost_x_ = cost_x;
        cost_y_ = cost_y;
        cost_z_ = cost_z;
        Q_regul_ =
            Eigen::MatrixXd::Identity(nb_var_, nb_var_) * hess_regularization;
    }
    /*
     * Private methods
     */
private:
    /**
     * @brief Compute the time vector: \f$ [1, t, ..., t^{ORDER}] \f$.
     *
     * @param time
     * @param time_vec
     */
    void t_vec(const double &time, Eigen::VectorXd &time_vec)
    {
        time_vec(0) = 1.0;
        for (int i = 1; i < time_vec.size(); ++i)
        {
            time_vec(i) = std::pow(time, i);
        }
    }

    /**
     * @brief Compute the time vector first derivative.
     *
     * \f[ t_vec =  [0, 1, 2*t, ..., ORDER * t^{ORDER-1}] \f]
     *
     * @param time
     * @param time_vec
     */
    void dt_vec(const double &time, Eigen::VectorXd &time_vec)
    {
        time_vec(0) = 0.0;
        time_vec(1) = 1.0;
        for (int i = 2; i < time_vec.size(); ++i)
        {
            double id = i;
            time_vec(i) = id * std::pow(time, i - 1);
        }
    }

    /**
     * @brief Compute the time vector second derivative:
     *
     * \f[ t_vec =  [0, 0, 2, 3*2*t..., ORDER * (ORDER-1) * t^{ORDER-2}] \f]
     *
     * @param time
     * @param time_vec
     */
    void ddt_vec(const double &time, Eigen::VectorXd &time_vec)
    {
        time_vec(0) = 0.0;
        time_vec(1) = 0.0;
        time_vec(2) = 2.0;
        for (int i = 3; i < time_vec.size(); ++i)
        {
            double id = i;
            time_vec(i) = id * (id - 1.0) * std::pow(time, i - 2);
        }
    }

    /*
     * Attributes
     */
private:
    /*
     * Constant problem parameters.
     */

    /** @brief Flying foot apex to be reach mid-air. */
    double mid_air_height_;

    /** @brief Number of the polynome coefficient for the trajectory on the
     * X-axis. */
    int nb_var_x_;

    /** @brief Number of the polynome coefficient for the trajectory on the
     * Y-axis. */
    int nb_var_y_;

    /** @brief Number of the polynome coefficient for the trajectory on the
     * Z-axis. */
    int nb_var_z_;

    /*
     * Variable problem parameters.
     */

    /** @brief Time vector used to compute X(t). */
    Eigen::VectorXd time_vec_x_;

    /** @brief Time vector used to compute Y(t). */
    Eigen::VectorXd time_vec_y_;

    /** @brief Time vector used to compute Z(t). */
    Eigen::VectorXd time_vec_z_;

    /** @brief Initial position before the motion. */
    Eigen::Vector3d start_pose_;

    /** @brief Current position. */
    Eigen::Vector3d current_pose_;

    /** @brief Previous computed position from the QP. */
    Eigen::Vector3d previous_solution_pose_;

    /** @brief Current velocity. */
    Eigen::Vector3d current_velocity_;

    /** @brief Current acceleration. */
    Eigen::Vector3d current_acceleration_;

    /** @brief Target pose after the motion. */
    Eigen::Vector3d target_pose_;

    /** @brief Initial time. */
    double start_time_;

    /** @brief Current time. */
    double current_time_;

    /** @brief Final time and the end of the motion. */
    double end_time_;

    /** @brief Last end time register when we computed the QP. */
    double last_end_time_seen_;

    /*
     * QP variables
     */

    /** @brief Number of variabes in the optimization problem. */
    int nb_var_;

    /** @brief Number of equality constraints in the optimization problem. */
    int nb_eq_;

    /** @brief Number of inequality in the optimization problem. */
    int nb_ineq_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_;

    /** @brief Solution of the optimization problem.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd x_opt_;

    /** @brief Lower Bound on the solution of the optimization problem.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd x_opt_lb_;

    /** @brief Upper Bound on the solution of the optimization problem.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd x_opt_ub_;

    /** @brief Quadratic term of the quadratic cost.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd Q_;

    /** @brief Quadratic term added to the quadratic cost in order regularize
     * the system.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd Q_regul_;

    /** @brief Cost weights for the X-axis. */
    double cost_x_;

    /** @brief Cost weights for the Y-axis. */
    double cost_y_;

    /** @brief Cost weights for the Z-axis. */
    double cost_z_;

    /** @brief Linear term of the quadratic cost.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd q_;

    /** @brief Linear equality matrix.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd A_eq_;

    /** @brief Linear equality vector.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd B_eq_;

    /** @brief Linear inequality matrix.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd A_ineq_;

    /** @brief Linear inequality vector.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd B_ineq_;
};

}  // namespace reactive_planners


/*
 * PACDEncrypter.cpp
 *
 *  Created on: 25 Jun 2018
 *      Author: scsjd
 */

#include "PolyACDEncrypter.h"

#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <jsoncpp/json/json.h>
#include <string>
#include <sstream>

/*
 * \param lambda the bit length of k1
 * \param mu the size of the polynomial coefficients in bits
 * \param the degree of the polynomial k2
 */
PolyACDEncrypter::PolyACDEncrypter(int lambda, int mu, int d) {
	this->mu = mu;
	RandomLen(k1,lambda);
	k2.SetLength(d+1);
	for(int i =0; i < d; i++){
		NTL::ZZ tmp;
		RandomBits(tmp,mu);
		SetCoeff(k2,i,tmp);
	}
	SetCoeff(k2,d,1);
}

PolyACDEncrypter::PolyACDEncrypter(int mu, std::string& secrets) {
	this->mu = mu;
	Json::Value root;   // will contains the root value after parsing.
	Json::Reader reader;
	bool parsingSuccessful = reader.parse(secrets,root);
	if (parsingSuccessful){
		std::istringstream k1buf(root["k1"].asString());
		k1buf >> k1;
		std::istringstream k2buf(root["k2"].asString());
		k2buf >> k2;
	}
}

PolyACDEncrypter::~PolyACDEncrypter() {
}

/*
 * Converts the key k1 (a large integer) to a string and k2 (a polynomial) to an array of strings, then writes JSON string
 */
std::string PolyACDEncrypter::writeSecretsToJSON(){
	Json::Value root;
	std::ostringstream k1buf;
	k1buf << k1;
	root["k1"]= k1buf.str();
	std::ostringstream k2buf;
	k2buf << k2;
	root["k2"]= k2buf.str();
	Json::FastWriter writer;
	return writer.write(root);
}

NTL::ZZX PolyACDEncrypter::encrypt(NTL::ZZX& plaintext){
	NTL::ZZX p = k2*plaintext;
	long degree = deg(p);
	NTL::ZZ m_n = LeadCoeff(plaintext);
	NTL::ZZX p2;
	p2.SetLength(degree-1);
	for (int i = 0; i < degree-1; i++){
		SetCoeff(p2,i,NTL::coeff(p,i));
	}
	SetCoeff(p2,degree-1,p[degree-1]+k1*m_n);
	NTL::ZZX r;
	long d = deg(k2);
	r.SetLength(d-1);
	for(int i =0; i < d; i++){
		NTL::ZZ tmp;
		RandomBits(tmp,mu);
		SetCoeff(r,i,tmp);
	}
	return p2+r;
}

NTL::ZZX PolyACDEncrypter::encrypt(NTL::vec_ZZ& plaintext){
	NTL::ZZX ptext = to_ZZX(plaintext);
	return encrypt(ptext);
}

std::pair<NTL::ZZ,NTL::ZZX> PolyACDEncrypter::getKey(){
	return (std::pair<NTL::ZZ,NTL::ZZX>(k1,k2));
}


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

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2005, 2006 StatPro Italia srl
 Copyright (C) 2015 Peter Caspers

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

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

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

/*! \file timegrid.hpp
    \brief discrete time grid
*/

#ifndef quantlib_time_grid_hpp
#define quantlib_time_grid_hpp

#include <ql/errors.hpp>
#include <ql/math/comparison.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <vector>
#include <numeric>

namespace QuantLib {

    //! time grid class
    class TimeGrid {
      public:
        //! \name Constructors
        //@{
        TimeGrid() {}
        //! Regularly spaced time-grid
        TimeGrid(Time end, Size steps, Time start = 0.0);
        //! Time grid with mandatory time points
        /*! Mandatory points are guaranteed to belong to the grid.
            No additional points are added.
        */
        template <class Iterator>
        TimeGrid(Iterator begin, Iterator end)
            : mandatoryTimes_(begin, end) {
            initialize(begin, end, 0.0);
        }
        template <class Iterator, class T>
        TimeGrid(Iterator begin, Iterator end, T start,
                 typename boost::enable_if<boost::is_float<T> >::type* = 0)
            : mandatoryTimes_(begin, end) {
            initialize(begin, end, start);
        }
        //! Time grid with mandatory time points
        /*! Mandatory points are guaranteed to belong to the grid.
            Additional points are then added with regular spacing
            between pairs of mandatory times in order to reach the
            desired number of steps.
        */
        template <class Iterator, class T>
        TimeGrid(Iterator begin, Iterator end, T steps, Time start = 0.0,
                 typename boost::enable_if<boost::is_integral<T> >::type* = 0)
            : mandatoryTimes_(begin, end) {
            initialize(begin, end, steps, start);
        }
        //@}
        //! \name Time grid interface
        //@{
        //! returns the index i such that grid[i] = t
        Size index(Time t) const;
        //! returns the index i such that grid[i] is closest to t
        Size closestIndex(Time t) const;
        //! returns the time on the grid closest to the given t
        Time closestTime(Time t) const {
            return times_[closestIndex(t)];
        }
        const std::vector<Time>& mandatoryTimes() const {
            return mandatoryTimes_;
        }
        Time dt(Size i) const { return dt_[i]; }
        //@}
        //! \name sequence interface
        //@{
        typedef std::vector<Time>::const_iterator const_iterator;
        typedef std::vector<Time>::const_reverse_iterator
                                          const_reverse_iterator;

        Time operator[](Size i) const { return times_[i]; }
        Time at(Size i) const { return times_.at(i); }
        Size size() const { return times_.size(); }
        bool empty() const { return times_.empty(); }
        const_iterator begin() const { return times_.begin(); }
        const_iterator end() const { return times_.end(); }
        const_reverse_iterator rbegin() const { return times_.rbegin(); }
        const_reverse_iterator rend() const { return times_.rend(); }
        Time front() const { return times_.front(); }
        Time back() const { return times_.back(); }
        //@}
      private:
        template <class Iterator>
        void initialize(Iterator begin, Iterator end, Time start);
        template <class Iterator>
        void initialize(Iterator begin, Iterator end, Size steps, Time start);
        std::vector<Time> times_;
        std::vector<Time> dt_;
        std::vector<Time> mandatoryTimes_;
    };

    template <class Iterator>
    void TimeGrid::initialize(Iterator begin, Iterator end, Time start) {
        std::sort(mandatoryTimes_.begin(), mandatoryTimes_.end());
        QL_REQUIRE(mandatoryTimes_.front() >= start,
                   "times (" << mandatoryTimes_.front() << ") less than start ("
                             << start << ") not allowed");
        std::vector<Time>::iterator e =
            std::unique(mandatoryTimes_.begin(), mandatoryTimes_.end(),
                        std::ptr_fun(close_enough));
        mandatoryTimes_.resize(e - mandatoryTimes_.begin());

        if (mandatoryTimes_[0] > start)
            times_.push_back(start);

        times_.insert(times_.end(), mandatoryTimes_.begin(),
                      mandatoryTimes_.end());

        std::adjacent_difference(times_.begin() + 1, times_.end(),
                                 std::back_inserter(dt_));
    }

    template <class Iterator>
    void TimeGrid::initialize(Iterator begin, Iterator end, Size steps, Time start) {
        std::sort(mandatoryTimes_.begin(), mandatoryTimes_.end());
        QL_REQUIRE(mandatoryTimes_.front() >= start,
                   "times less than start (" << start << ") not allowed");
        std::vector<Time>::iterator e =
            std::unique(mandatoryTimes_.begin(), mandatoryTimes_.end(),
                        std::ptr_fun(close_enough));
        mandatoryTimes_.resize(e - mandatoryTimes_.begin());

        Time last = mandatoryTimes_.back();
        Time dtMax;
        // The resulting timegrid have points at times listed in the input
        // list. Between these points, there are inner-points which are
        // regularly spaced.
        if (steps == 0) {
            std::vector<Time> diff;
            std::adjacent_difference(mandatoryTimes_.begin(),
                                     mandatoryTimes_.end(),
                                     std::back_inserter(diff));
            if (diff.front() == 0.0)
                diff.erase(diff.begin());
            dtMax = *(std::min_element(diff.begin(), diff.end()));
        } else {
            dtMax = (last - start) / steps;
        }

        Time periodBegin = start;
        times_.push_back(periodBegin);
        for (std::vector<Time>::const_iterator t = mandatoryTimes_.begin();
             t < mandatoryTimes_.end(); t++) {
            Time periodEnd = *t;
            if (periodEnd != start) {
                // the nearest integer
                Size nSteps = Size((periodEnd - periodBegin) / dtMax + 0.5);
                // at least one time step!
                nSteps = (nSteps != 0 ? nSteps : 1);
                Time dt = (periodEnd - periodBegin) / nSteps;
                times_.reserve(times_.size() + nSteps);
                for (Size n = 1; n <= nSteps; ++n)
                    times_.push_back(periodBegin + n * dt);
            }
            periodBegin = periodEnd;
        }

        std::adjacent_difference(times_.begin() + 1, times_.end(),
                                 std::back_inserter(dt_));
    }

} // namespace QuantLib

#endif


// Copyright 2013-2020 Lawrence Livermore National Security, LLC and other
// HavoqGT Project Developers. See the top-level LICENSE file for details.
//
// SPDX-License-Identifier: MIT

#include <math.h>
#include <stdint.h>
#include <assert.h>
#include <utility>
#include <boost/random.hpp>

namespace havoqgt { namespace detail {

template <typename DUMMY=uint64_t>
class preferential_attachment_helper {
public:
  typedef uint64_t                  vertex_descriptor;
  typedef typename std::pair<uint64_t,uint64_t> edge_type;

  preferential_attachment_helper(uint64_t k, uint64_t m, double beta, uint64_t rng_seed=5489)
    : m_rng(rng_seed)
  {
    m_k = k;
    m_num_edges = m;
    m_koffset = m_k*(m_k+1)/2;
    m_ptr_mask =  ~(std::numeric_limits<vertex_descriptor>::max() >> 1);
    m_alpha = (beta / double(m_k) + double(1)) / (beta / double(m_k) + 2);
    //std::cout << "beta = " << beta << ", alpha = " << m_alpha << std::endl;
  }

  edge_type gen_edge(uint64_t _edge_index) {
    edge_type to_return;
    to_return.first = calc_source(_edge_index);
    if(_edge_index >= m_koffset)  {
      //
      // Generate random edge_list location based on beta model
      boost::random::uniform_01<boost::random::mt19937> rand_prob(m_rng);
      boost::random::uniform_int_distribution<uint64_t> uid(0,_edge_index-1);
      uint64_t rand = uid(m_rng) * 2;
      if(rand_prob() > m_alpha) {
        ++rand;
      }
      if(rand % 2 == 0) { //this is a source vertex, we can calc!
        to_return.second = calc_source(rand/2);
      } else {
        uint64_t edge_rand = rand/2;
        if(edge_rand < m_koffset) {  //this is an early edge we can calc!
          to_return.second = calc_target(edge_rand);
        } else {
          to_return.second = make_pointer(edge_rand);
        }
      }
    } else {
      to_return.second = calc_target(_edge_index);
    }
    return to_return;
  }

  uint64_t calc_source(uint64_t i) {
    uint64_t to_return;
    if(i+1>m_koffset) {
      to_return = ((i-m_koffset)/m_k)+m_k+1;
    } else {
      to_return = (uint64_t) floor(double(-0.5f) +
                  sqrt(double(0.25f)+double(2)*double(i))+1);
    }
    return to_return;
  }

  uint64_t calc_target(uint64_t i) {
    uint64_t to_return;
    if(i+1>m_koffset) {
      assert(false);
      to_return = i;
    } else {
      double tmp = double(-0.5f) + sqrt(double(0.25f)+double(2)*double(i))+1;
      to_return = (uint64_t) ((tmp - floor(tmp)) * floor(tmp));
    }
    return to_return;
  }

  vertex_descriptor make_pointer(vertex_descriptor i) {
    return i | m_ptr_mask;
  }

  bool is_pointer(vertex_descriptor i) {
    return (i & m_ptr_mask) > 0;
  }

  //dereferences point
  uint64_t value_of_pointer(vertex_descriptor i, uint64_t _num_partitions=1) {
    i = i & ~m_ptr_mask;
    uint64_t my_partition = i%_num_partitions;
    uint64_t my_partition_offset = i/_num_partitions;
    uint64_t edges_per_partition = m_num_edges / _num_partitions;
    return my_partition * edges_per_partition + my_partition_offset;
  }

private:
  boost::random::mt19937 m_rng;
  uint64_t               m_k;
  uint64_t               m_koffset;
  vertex_descriptor      m_ptr_mask;
  uint64_t               m_num_edges;
  double                 m_alpha;
};

}} //end namespace havoqgt::detail