#include <queue>
#include <limits>
#include <cmath>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <Eigen/Dense>
#include <iostream>

namespace py = pybind11;

const float INF = std::numeric_limits<float>::infinity();

// represents a single pixel
class Node {
  public:
    int idx; // index in the flattened grid
    float cost; // cost of traversing this pixel
    int path_length; // the length of the path to reach this node

    Node(int i, float c, int path_length) : idx(i), cost(c), path_length(path_length) {}
};

// the top of the priority queue is the greatest element by default,
// but we want the smallest, so flip the sign
bool operator<(const Node &n1, const Node &n2) {
  return n1.cost > n2.cost;
}

// See for various grid heuristics:
// http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7
// L_\inf norm (diagonal distance)
inline float linf_norm(int i0, int j0, int i1, int j1) {
  return std::max(std::abs(i0 - i1), std::abs(j0 - j1));
}

// L_1 norm (manhattan distance)
inline float l1_norm(int i0, int j0, int i1, int j1) {
  return std::abs(i0 - i1) + std::abs(j0 - j1);
}

inline float euclidean_distance(int i0, int j0, int i1, int j1) {
    float xd =(float)(i0 - i1);
    float yd =(float)(j0 - j1);
    float dist = std::sqrt(xd*xd+yd*yd);
  return dist;
}

// weights:        flattened h x w grid of costs
// h, w:           height and width of grid
// start, goal:    index of start/goal in flattened grid
// diag_ok:        if true, allows diagonal moves (8-conn.)
// paths (output): for each node, stores previous node in path
Eigen::MatrixXi astar(Eigen::RowVectorXf op_map, int height, int width, int start, int goal, bool allow_diagonal) {
  int h = height;
  int w = width;
  int diag_ok = int(allow_diagonal);

  float* weights = op_map.data();
  int* paths = new int[h * w];
  int path_length = -1;

  Node start_node(start, 0., 1);

  float* costs = new float[h * w];
  for (int i = 0; i < h * w; ++i)
    costs[i] = INF;
  costs[start] = 0.;

  std::priority_queue<Node> nodes_to_visit;
  nodes_to_visit.push(start_node);

  int* nbrs = new int[8];

  while (!nodes_to_visit.empty()) {
    // .top() doesn't actually remove the node
    Node cur = nodes_to_visit.top();

    if (cur.idx == goal) {
      path_length = cur.path_length;
      break;
    }

    nodes_to_visit.pop();

    int row = cur.idx / w;
    int col = cur.idx % w;
    // check bounds and find up to eight neighbors: top to bottom, left to right
    nbrs[0] = (diag_ok && row > 0 && col > 0)          ? cur.idx - w - 1   : -1;
    nbrs[1] = (row > 0)                                ? cur.idx - w       : -1;
    nbrs[2] = (diag_ok && row > 0 && col + 1 < w)      ? cur.idx - w + 1   : -1;
    nbrs[3] = (col > 0)                                ? cur.idx - 1       : -1;
    nbrs[4] = (col + 1 < w)                            ? cur.idx + 1       : -1;
    nbrs[5] = (diag_ok && row + 1 < h && col > 0)      ? cur.idx + w - 1   : -1;
    nbrs[6] = (row + 1 < h)                            ? cur.idx + w       : -1;
    nbrs[7] = (diag_ok && row + 1 < h && col + 1 < w ) ? cur.idx + w + 1   : -1;

    float heuristic_cost;
    for (int i = 0; i < 8; ++i) {
      if (nbrs[i] >= 0) {
        // the sum of the cost so far and the cost of this move
        float new_cost = costs[cur.idx] + weights[nbrs[i]];
        if (new_cost < costs[nbrs[i]]) {
          // estimate the cost to the goal based on legal moves
          heuristic_cost = euclidean_distance(nbrs[i] / w, nbrs[i] % w,
                                       goal    / w, goal    % w);

          // paths with lower expected cost are explored first
          float priority = new_cost + heuristic_cost;
          nodes_to_visit.push(Node(nbrs[i], priority, cur.path_length + 1));

          costs[nbrs[i]] = new_cost;
          paths[nbrs[i]] = cur.idx;
        }
      }
    }
  }

  Eigen::MatrixXi path_return(2, path_length);

  int idx = goal;
  for (int i = path_length - 1; i >= 0; --i) {
      int point_x = idx / w;
      int point_y = idx % w;

      path_return(0, i) = point_x;
      path_return(1, i) = point_y;

      idx = paths[idx];
  }

  delete[] costs;
  delete[] nbrs;
  delete[] paths;

  return path_return;
}

PYBIND11_PLUGIN(astar) {
  py::module m("astar", "astar");
  m.def("astar", &astar, "astar");
  return m.ptr();
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <cmath>
using std::log;
using std::sqrt;
using std::pow;

#include <fstream>
using std::ofstream;

#include <streambuf>
using std::streambuf;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::Col;
using arma::uword;
using arma::abs;
using arma::eps;
using arma::exp;
using arma::exp2;
using arma::exp10;
using arma::trunc_exp;
using arma::log;
using arma::log2;
using arma::log10;
using arma::trunc_log;
using arma::sqrt;
using arma::square;
using arma::floor;
using arma::ceil;
using arma::round;
using arma::sign;
using arma::sin;
using arma::asin;
using arma::sinh;
using arma::asinh;
using arma::cos;
using arma::acos;
using arma::cosh;
using arma::acosh;
using arma::tan;
using arma::atan;
using arma::tanh;
using arma::atanh;
using arma::cond;
using arma::rank;
using arma::diagvec;
using arma::min;
using arma::max;
using arma::prod;
using arma::mean;
using arma::median;
using arma::stddev;
using arma::var;
using arma::cor;
using arma::cov;
using arma::cumsum;
using arma::fliplr;
using arma::flipud;
using arma::hist;
using arma::sort;
using arma::trans;
using arma::unique;
using arma::vectorise;
using arma::lu;
using arma::pinv;
using arma::princomp;
using arma::qr;
using arma::qr_econ;
using arma::svd;
using arma::svd_econ;
using arma::accu;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedGenMat : public Expected {
    public:
      ExpectedGenMat() {
        cout << "Compute ExpectedGenMat(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            expectedArmaAbs();
            expectedArmaEps();
            expectedArmaExp();
            expectedArmaExp2();
            expectedArmaExp10();
            expectedArmaTrunc_exp();
            expectedArmaLog();
            expectedArmaLog2();
            expectedArmaLog10();
            expectedArmaTrunc_log();
            expectedArmaSqrt();
            expectedArmaSquare();
            expectedArmaFloor();
            expectedArmaCeil();
            expectedArmaRound();
            expectedArmaSign();
            expectedArmaSin();
            expectedArmaAsin();
            expectedArmaSinh();
            expectedArmaAsinh();
            expectedArmaCos();
            expectedArmaAcos();
            expectedArmaCosh();
            expectedArmaAcosh();
            expectedArmaTan();
            expectedArmaAtan();
            expectedArmaTanh();
            expectedArmaAtanh();
            expectedArmaCond();
            expectedArmaRank();
            expectedArmaDiagvec();
            expectedArmaMin();
            expectedArmaMax();
            expectedArmaProd();
            expectedArmaMean();
            expectedArmaMedian();
            expectedArmaStddev();
            expectedArmaVar();
            expectedArmaCor();
            expectedArmaCov();
            expectedArmaCumsum();
            expectedArmaFliplr();
            expectedArmaFlipud();
            expectedArmaHist();
            expectedArmaSort();
            expectedArmaTrans();
            expectedArmaUnique();
            expectedArmaVectorise();
            expectedArmaLu();
            expectedArmaPinv();
            expectedArmaPrincomp();
            expectedArmaQr();
            expectedArmaQr_econ();
            expectedArmaSvd();
            expectedArmaSvd_econ();
            expectedArmaNegate();
            expectedArmaReciprocal();
            expectedArmaAccu();
            expectedMatMin();
            expectedMatMax();
            expectedMatSize();
            expectedMatIs_finite();
            expectedMatT();
            expectedMatDiag();
            expectedMatIs_square();
            expectedMatIs_vec();
            expectedMatIs_colvec();
            expectedMatIs_rowvec();
            expectedMatPrint();
            expectedMatRaw_print();
            expectedMat();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;

      void expectedArmaAbs() {
        cout << "- Compute expectedArmaAbs() ... ";
        save<double>("Arma.abs", abs(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaEps() {
        cout << "- Compute expectedArmaAbs() ... ";
        save<double>("Arma.eps", eps(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaExp() {
        cout << "- Compute expectedArmaExp() ... ";
        save<double>("Arma.exp", exp(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaExp2() {
        cout << "- Compute expectedArmaExp2() ... ";
        save<double>("Arma.exp2", exp2(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaExp10() {
        cout << "- Compute expectedArmaExp10() ... ";
        save<double>("Arma.exp10", exp10(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaTrunc_exp() {
        cout << "- Compute expectedArmaTrunc_exp() ... ";
        save<double>("Arma.trunc_exp", trunc_exp(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaLog() {
        cout << "- Compute expectedArmaLog() ... ";
        save<double>("Arma.log", log(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaLog2() {
        cout << "- Compute expectedArmaLog2() ... ";
        save<double>("Arma.log2", log2(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaLog10() {
        cout << "- Compute expectedArmaLog10() ... ";
        save<double>("Arma.log10", log10(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaTrunc_log() {
        cout << "- Compute expectedArmaTrunc_log() ... ";
        save<double>("Arma.trunc_log", trunc_log(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaSqrt() {
        cout << "- Compute expectedArmaSqrt() ... ";
        save<double>("Arma.sqrt", sqrt(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaSquare() {
        cout << "- Compute expectedArmaSquare() ... ";
        save<double>("Arma.square", square(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaFloor() {
        cout << "- Compute expectedArmaFloor() ... ";
        save<double>("Arma.floor", floor(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCeil() {
        cout << "- Compute expectedArmaCeil() ... ";
        save<double>("Arma.ceil", ceil(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaRound() {
        cout << "- Compute expectedArmaRound() ... ";
        save<double>("Arma.round", round(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaSign() {
        cout << "- Compute expectedArmaSign() ... ";
        save<double>("Arma.sign", sign(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaSin() {
        cout << "- Compute expectedArmaSin() ... ";
        save<double>("Arma.sin", sin(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaAsin() {
        cout << "- Compute expectedArmaAsin() ... ";
        save<double>("Arma.asin", asin(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaSinh() {
        cout << "- Compute expectedArmaSinh() ... ";
        save<double>("Arma.sinh", sinh(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaAsinh() {
        cout << "- Compute expectedArmaAsinh() ... ";
        save<double>("Arma.asinh", asinh(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCos() {
        cout << "- Compute expectedArmaCos() ... ";
        save<double>("Arma.cos", cos(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaAcos() {
        cout << "- Compute expectedArmaAcos() ... ";
        save<double>("Arma.acos", acos(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCosh() {
        cout << "- Compute expectedArmaCosh() ... ";
        save<double>("Arma.cosh", cosh(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaAcosh() {
        cout << "- Compute expectedArmaAcosh() ... ";

        /*
         * acosh behaves buggy on some systems, with acosh(inf) = nan instead of inf
         */
        //save<double>("Arma.acosh", acosh(_genMat));

        Mat<double> expected = _genMat;
        expected.transform([](double value) {
          return log(value + sqrt(pow(value, 2) - 1));
        });
        save<double>("Arma.acosh", expected);

        cout << "done." << endl;
      }

      void expectedArmaTan() {
        cout << "- Compute expectedArmaTan() ... ";
        save<double>("Arma.tan", tan(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaAtan() {
        cout << "- Compute expectedArmaAtan() ... ";
        save<double>("Arma.atan", atan(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaTanh() {
        cout << "- Compute expectedArmaTanh() ... ";
        save<double>("Arma.tanh", tanh(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaAtanh() {
        cout << "- Compute expectedArmaAtanh() ... ";
        save<double>("Arma.atanh", atanh(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCond() {
        cout << "- Compute expectedArmaCond() ... ";
        save<double>("Arma.cond", Mat<double>({cond(_genMat)}));
        cout << "done." << endl;
      }

      void expectedArmaRank() {
        cout << "- Compute expectedArmaRank() ... ";
        save<double>("Arma.rank", Mat<double>({static_cast<double>(rank(_genMat))}));
        cout << "done." << endl;
      }

      void expectedArmaDiagvec() {
        cout << "- Compute expectedArmaDiagvec() ... ";
        save<double>("Arma.diagvec", diagvec(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaMin() {
        cout << "- Compute expectedArmaMin() ... ";
        save<double>("Arma.min", min(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaMax() {
        cout << "- Compute expectedArmaMax() ... ";
        save<double>("Arma.max", max(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaProd() {
        cout << "- Compute expectedArmaProd() ... ";
        save<double>("Arma.prod", prod(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaMean() {
        cout << "- Compute expectedArmaMean() ... ";
        save<double>("Arma.mean", mean(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaMedian() {
        cout << "- Compute expectedArmaMedian() ... ";
        save<double>("Arma.median", median(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaStddev() {
        cout << "- Compute expectedArmaStddev() ... ";
        save<double>("Arma.stddev", stddev(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaVar() {
        cout << "- Compute expectedArmaVar() ... ";
        save<double>("Arma.var", var(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCor() {
        cout << "- Compute expectedArmaCor() ... ";
        save<double>("Arma.cor", cor(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCov() {
        cout << "- Compute expectedArmaCov() ... ";
        save<double>("Arma.cov", cov(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaCumsum() {
        cout << "- Compute expectedArmaCumsum() ... ";
        save<double>("Arma.cumsum", cumsum(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaFliplr() {
        cout << "- Compute expectedArmaFliplr() ... ";
        save<double>("Arma.fliplr", fliplr(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaFlipud() {
        cout << "- Compute expectedArmaFlipud() ... ";
        save<double>("Arma.flipud", flipud(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaHist() {
        cout << "- Compute expectedArmaHist() ... ";
        save<uword>("Arma.hist", hist(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaSort() {
        if(!_genMat.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaSort() ... ";
        save<double>("Arma.sort", sort(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaTrans() {
        cout << "- Compute expectedArmaTrans() ... ";
        save<double>("Arma.trans", trans(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaUnique() {
        cout << "- Compute expectedArmaUnique() ... ";
        save<double>("Arma.unique", unique(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaVectorise() {
        cout << "- Compute expectedArmaVectorise() ... ";
        save<double>("Arma.vectorise", vectorise(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaLu() {
        cout << "- Compute expectedArmaLu() ... ";

        Mat<double> L, U;

        if(lu(L, U, _genMat)) {
          save<double>("Arma.lu", Mat<double>({1}));
        } else {
          save<double>("Arma.lu", Mat<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedArmaPinv() {
        cout << "- Compute expectedArmaPinv() ... ";
        save<double>("Arma.pinv", pinv(_genMat));
        cout << "done." << endl;
      }

      void expectedArmaPrincomp() {
        cout << "- Compute expectedArmaPrincomp() ... ";

        Mat<double> coeff, score;
        Col<double> latent, tsquared;

        princomp(coeff, score, latent, tsquared, _genMat);

        save<double>("Arma.princompLatent", Mat<double>(latent));

        cout << "done." << endl;

      }

      void expectedArmaQr() {
        cout << "- Compute expectedArmaQr() ... ";

        Mat<double> Q, R;

        qr(Q, R, _genMat);

        save<double>("Arma.qrQ", Q);
        save<double>("Arma.qrR", R);

        cout << "done." << endl;
      }

      void expectedArmaQr_econ() {
        cout << "- Compute expectedArmaQr_econ() ... ";

        Mat<double> Q, R;

        qr_econ(Q, R, _genMat);

        save<double>("Arma.qr_econQ", Q);
        save<double>("Arma.qr_econR", R);

        cout << "done." << endl;
      }

      void expectedArmaSvd() {
        cout << "- Compute expectedArmaSvd() ... ";

        Mat<double> U, V;
        Col<double> s;

        svd(U, s, V, _genMat);
        save<double>("Arma.svd", Mat<double>(s));

        cout << "done." << endl;
      }

      void expectedArmaSvd_econ() {
        cout << "- Compute expectedArmaSvd_econ() ... ";

        Mat<double> U, V;
        Col<double> s;

        svd_econ(U, s, V, _genMat);
        save<double>("Arma.svd_econ", Mat<double>(s));

        cout << "done." << endl;
      }

      void expectedArmaNegate() {
        cout << "- Compute expectedArmaNegate() ... ";
        save<double>("Arma.negate", -_genMat);
        cout << "done." << endl;
      }

      void expectedArmaReciprocal() {
        cout << "- Compute expectedArmaReciprocal() ... ";
        save<double>("Arma.reciprocal", 1/_genMat);
        cout << "done." << endl;
      }

      void expectedArmaAccu() {
        cout << "- Compute expectedArmaAccu() ... ";
        save<double>("Arma.accu", Mat<double>({accu(_genMat)}));
        cout << "done." << endl;
      }

      void expectedMatMin() {
        cout << "- Compute expectedMatMinB() ... ";

        double value;
        uword row;
        uword column;

        value = _genMat.min(row, column);

        save<double>("Mat.minValue", Mat<double>({value}));
        save<double>("Mat.minRow", Mat<double>({static_cast<double>(row)}));
        save<double>("Mat.minColumn", Mat<double>({static_cast<double>(column)}));
        save<double>("Mat.minIndex", Mat<double>({static_cast<double>(row + column * _genMat.n_rows)}));

        cout << "done." << endl;
      }

      void expectedMatMax() {
        cout << "- Compute expectedMatMaxB() ... ";

        double value;
        uword row;
        uword column;

        value = _genMat.max(row, column);

        save<double>("Mat.maxValue", Mat<double>({value}));
        save<double>("Mat.maxRow", Mat<double>({static_cast<double>(row)}));
        save<double>("Mat.maxColumn", Mat<double>({static_cast<double>(column)}));
        save<double>("Mat.maxIndex", Mat<double>({static_cast<double>(row + column * _genMat.n_rows)}));

        cout << "done." << endl;
      }

      void expectedMatSize() {
        cout << "- Compute expectedMatSize() ... ";
        save<double>("Mat.size", Mat<double>({static_cast<double>(_genMat.size())}));
        cout << "done." << endl;
      }

      void expectedMatIs_finite() {
        cout << "- Compute expectedMatIs_finite() ... ";

        if(_genMat.is_finite()) {
          save<double>("Mat.is_finite", Mat<double>({1}));
        } else {
          save<double>("Mat.is_finite", Mat<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedMatT() {
        cout << "- Compute expectedMatT() ... ";
        save<double>("Mat.t", _genMat.t());
        cout << "done." << endl;
      }

      void expectedMatDiag() {
        cout << "- Compute expectedMatDiag() ... ";
        save<double>("Mat.diag", _genMat.diag());
        cout << "done." << endl;
      }

      void expectedMatIs_square() {
        cout << "- Compute expectedMatIs_square() ... ";

        if(_genMat.is_square()) {
          save<double>("Mat.is_square", Mat<double>({1}));
        } else {
          save<double>("Mat.is_square", Mat<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedMatIs_vec() {
        cout << "- Compute expectedMatIs_vec() ... ";

        if(_genMat.is_vec()) {
          save<double>("Mat.is_vec", Mat<double>({1}));
        } else {
          save<double>("Mat.is_vec", Mat<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedMatIs_colvec() {
        cout << "- Compute expectedMatIs_colvec() ... ";

        if(_genMat.is_colvec()) {
          save<double>("Mat.is_colvec", Mat<double>({1}));
        } else {
          save<double>("Mat.is_colvec", Mat<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedMatIs_rowvec() {
        cout << "- Compute expectedMatIs_rowvec() ... ";

        if(_genMat.is_rowvec()) {
          save<double>("Mat.is_rowvec", Mat<double>({1}));
        } else {
          save<double>("Mat.is_rowvec", Mat<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedMatPrint() {
        cout << "- Compute expectedMatPrint() ... ";

        ofstream expected(_filepath + "Mat.print(" + _fileSuffix + ").txt");
        streambuf* previousBuffer = cout.rdbuf(expected.rdbuf());

        _genMat.print();

        cout.rdbuf(previousBuffer);

        cout << "done." << endl;
      }

      void expectedMatRaw_print() {
        cout << "- Compute expectedMatRaw_print() ... ";

        ofstream expected(_filepath + "Mat.raw_print(" + _fileSuffix + ").txt");
        streambuf* previousBuffer = cout.rdbuf(expected.rdbuf());

        _genMat.raw_print();

        cout.rdbuf(previousBuffer);

        cout << "done." << endl;
      }

      void expectedMat() {
        cout << "- Compute expectedMat() ... ";
        save<double>("Mat", _genMat);
        cout << "done." << endl;
      }
  };
}


#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace ub = boost::numeric::ublas;

struct SimpleHarmonic {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = - x(0);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(2);
		x(0) = 0.;
		x(1) = 1.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 100.;
	}
};

struct Lorenz {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(3);

		y(0) = 10. * ( x(1) - x(0) );
		y(1) = 28. * x(0) - x(1) - x(0) * x(2);
		y(2) = (-8./3.) * x(2) + x(0) * x(1);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(3);
		x(0) = 15.;
		x(1) = 15.;
		x(2) = 36.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};

struct VdP {
	double mu;

	VdP(double mu = 0.25) : mu(mu) {}

	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = mu * (1. - x(0)*x(0))*x(1) - x(0);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(2);
		x(0) = 1.;
		x(1) = 1.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};

struct Nobi {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = x(0) - x(0)*x(0)*x(0);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(2);
		x(0) = 0.;
		x(1) = 4.;
		// x(0) = kv::interval<T>(-0.05, 0.05);
		// x(1) = kv::interval<T>(3.95, 4.05);
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = "3.3";
	}
};

struct QuadTest1 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(1);

		y(0) = t * t;

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(1);
		x(0) = 0.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};

struct QuadTest2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(1);

		y(0) = 0.;

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(1);
		x(0) = 0.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/half.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( refine_rsqrt_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a0, T x) const BOOST_NOEXCEPT
    {
      // Newton-Raphson
      return fma( fnms(a0, sqr(x), One<T>()), x*Half<T>(), x);
    }
  };
} } }

#endif


// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include "utils/multinomialCoefficients.hpp"

BOOST_AUTO_TEST_CASE( factorial_0 ) { BOOST_CHECK_EQUAL( 1, factorial( 0 ) ); }

BOOST_AUTO_TEST_CASE( factorial_1 ) { BOOST_CHECK_EQUAL( 1, factorial( 1 ) ); }

BOOST_AUTO_TEST_CASE( factorial_2 ) { BOOST_CHECK_EQUAL( 2, factorial( 2 ) ); }

BOOST_AUTO_TEST_CASE( factorial_3 ) { BOOST_CHECK_EQUAL( 6, factorial( 3 ) ); }


//            Copyright Daniel Trebbien 2010.
// Distributed under the Boost Software License, Version 1.0.
//   (See accompanying file LICENSE_1_0.txt or the copy at
//         http://www.boost.org/LICENSE_1_0.txt)

#include <cassert>
#include <cstddef>
#include <cstdlib>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/typeof/typeof.hpp>

struct edge_t
{
    unsigned long first;
    unsigned long second;
};

// A graphic of the min-cut is available at
// <http://www.boost.org/doc/libs/release/libs/graph/doc/stoer_wagner_imgs/stoer_wagner.cpp.gif>
int main()
{
    using namespace std;

    typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS,
        boost::no_property, boost::property< boost::edge_weight_t, int > >
        undirected_graph;
    typedef boost::property_map< undirected_graph, boost::edge_weight_t >::type
        weight_map_type;
    typedef boost::property_traits< weight_map_type >::value_type weight_type;

    // define the 16 edges of the graph. {3, 4} means an undirected edge between
    // vertices 3 and 4.
    edge_t edges[] = { { 3, 4 }, { 3, 6 }, { 3, 5 }, { 0, 4 }, { 0, 1 },
        { 0, 6 }, { 0, 7 }, { 0, 5 }, { 0, 2 }, { 4, 1 }, { 1, 6 }, { 1, 5 },
        { 6, 7 }, { 7, 5 }, { 5, 2 }, { 3, 4 } };

    // for each of the 16 edges, define the associated edge weight. ws[i] is the
    // weight for the edge that is described by edges[i].
    weight_type ws[] = { 0, 3, 1, 3, 1, 2, 6, 1, 8, 1, 1, 80, 2, 1, 1, 4 };

    // construct the graph object. 8 is the number of vertices, which are
    // numbered from 0 through 7, and 16 is the number of edges.
    undirected_graph g(edges, edges + 16, ws, 8, 16);

    // define a property map, `parities`, that will store a boolean value for
    // each vertex. Vertices that have the same parity after
    // `stoer_wagner_min_cut` runs are on the same side of the min-cut.
    BOOST_AUTO(parities,
        boost::make_one_bit_color_map(
            num_vertices(g), get(boost::vertex_index, g)));

    // run the Stoer-Wagner algorithm to obtain the min-cut weight. `parities`
    // is also filled in.
    int w = boost::stoer_wagner_min_cut(
        g, get(boost::edge_weight, g), boost::parity_map(parities));

    cout << "The min-cut weight of G is " << w << ".\n" << endl;
    assert(w == 7);

    cout << "One set of vertices consists of:" << endl;
    size_t i;
    for (i = 0; i < num_vertices(g); ++i)
    {
        if (get(parities, i))
            cout << i << endl;
    }
    cout << endl;

    cout << "The other set of vertices consists of:" << endl;
    for (i = 0; i < num_vertices(g); ++i)
    {
        if (!get(parities, i))
            cout << i << endl;
    }
    cout << endl;

    return EXIT_SUCCESS;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2016 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <algorithms/test_perimeter.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

template <typename P>
void test_all()
{
    // Simple
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,3 4,4 3,0 0))",
                                          5 + sqrt(2.0) + 5);
    // Non-simple
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,3 4,4 3,0 3,0 0))",
                                          5 + sqrt(2.0) + 4 + 3);
    // With holes
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,3 4,4 3,0 0),\
                                                   (2 2,3 4,3 3,2 2))",
                                          5 + sqrt(2.0) + 5 +
                                          sqrt(5.0) + 1 + sqrt(2.0));
    // Repeated points
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,3 4,3 4,3 4,4 3,4 3,\
                                                    4 3,4 3,4 3,4 3,0 3,0 0))",
                                          5 + sqrt(2.0) + 4 + 3);
    // Multipolygon
    test_geometry<bg::model::multi_polygon<bg::model::polygon<P> > >
    (
        "MULTIPOLYGON(((0 0,3 4,4 3,0 0)), ((0 0,3 4,4 3,0 3,0 0)))",
        5 + sqrt(2.0) + 5 + 5 + sqrt(2.0) + 4 + 3
    );

    // Geometries with perimeter zero
    test_geometry<P>("POINT(0 0)", 0);
    test_geometry<bg::model::linestring<P> >("LINESTRING(0 0,3 4,4 3)", 0);
}

template <typename P>
void test_empty_input()
{
    test_empty_input(bg::model::polygon<P>());
    test_empty_input(bg::model::multi_polygon<bg::model::polygon<P> >());
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

    // test_empty_input<bg::model::d2::point_xy<int> >();

    return 0;
}


//  Copyright (c) 2014 Anton Bikineev
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  Computes test data for the derivatives of the
//  various bessel functions. Results of derivatives
//  are generated by the relations between the derivatives
//  and Bessel functions, which actual implementation
//  doesn't use. Results are printed to ~ 50 digits.
//
#include <fstream>

#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/prg_exec_monitor.hpp>

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

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;
using namespace boost::multiprecision;

template <class T>
T bessel_j_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_j(v, x) - boost::math::cyl_bessel_j(v+1, x);
}

template <class T>
T bessel_y_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_neumann(v, x) - boost::math::cyl_neumann(v+1, x);
}

template <class T>
T bessel_i_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_i(v, x) + boost::math::cyl_bessel_i(v+1, x);
}

template <class T>
T bessel_k_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_k(v, x) - boost::math::cyl_bessel_k(v+1, x);
}

template <class T>
T sph_bessel_j_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_bessel(1, x);
   return boost::math::sph_bessel(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_bessel(itrunc(v), x);
}

template <class T>
T sph_bessel_y_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_neumann(1, x);
   return boost::math::sph_neumann(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_neumann(itrunc(v), x);
}

enum
{
   func_J = 0,
   func_Y,
   func_I,
   func_K,
   func_j,
   func_y
};

int cpp_main(int argc, char*argv [])
{
   typedef number<mpfr_float_backend<200> > bignum;

   parameter_info<bignum> arg1, arg2;
   test_data<bignum> data;

   int functype = 0;
   std::string letter = "J";

   if(argc == 2)
   {
      if(std::strcmp(argv[1], "--Y") == 0)
      {
         functype = func_Y;
         letter = "Y";
      }
      else if(std::strcmp(argv[1], "--I") == 0)
      {
         functype = func_I;
         letter = "I";
      }
      else if(std::strcmp(argv[1], "--K") == 0)
      {
         functype = func_K;
         letter = "K";
      }
      else if(std::strcmp(argv[1], "--j") == 0)
      {
         functype = func_j;
         letter = "j";
      }
      else if(std::strcmp(argv[1], "--y") == 0)
      {
         functype = func_y;
         letter = "y";
      }
      else
         assert(0);
   }

   bool cont;
   std::string line;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the Bessel " << letter << " function derivative\n\n";
   do{
      if(0 == get_user_parameter_info(arg1, "a"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "b"))
         return 1;

      bignum (*fp)(bignum, bignum) = 0;
      if(functype == func_J)
         fp = bessel_j_derivative_bare;
      else if(functype == func_I)
         fp = bessel_i_derivative_bare;
      else if(functype == func_K)
         fp = bessel_k_derivative_bare;
      else if(functype == func_Y)
         fp = bessel_y_derivative_bare;
      else if(functype == func_j)
         fp = sph_bessel_j_derivative_bare;
      else if(functype == func_y)
         fp = sph_bessel_y_derivative_bare;
      else
         assert(0);

      data.insert(fp, arg2, arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=bessel_j_derivative_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "bessel_j_derivative_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific << std::setprecision(50);
   write_code(ofs, data, line.c_str());

   return 0;
}


#pragma once
#include <Eigen/Dense>
using namespace Eigen;

class perceptron
{
public:
	perceptron();
	~perceptron() {};

	void fit(MatrixXf& X_train, VectorXi& y_train);
	void predict(MatrixXf& X_test, VectorXi& y_test);

	int num_epochs; //number of iterations through the dataset
	float   theta0; //hyperplane offset
	VectorXf theta; //hyperplane coeffs

};


#include <iostream>

#include <Eigen/Dense>

#include "polytope.hpp"

using namespace integrator_chains;

int main()
{
    Eigen::Vector4d bounds;
    bounds << 0, 1,
              0, 1;
    LabeledPolytope *square = LabeledPolytope::box( bounds, "square" );

    bounds << 0.5, 1.5,
              0.2, 2;
    Polytope *rect1 = Polytope::box( bounds );
    Polytope rect2 = *rect1 & *square;

    std::cout << *square << std::endl;
    std::cout << *rect1 << std::endl;
    std::cout << rect2 << std::endl;

    Eigen::Vector2d X;
    X << 0.3, 0.5;
    do {
        std::cout << "The vector (" << X(0) << ", " << X(1) << ")";
        if (square->is_in( X )) {
            std::cout << " is in ";
        } else {
            std::cout << " is not in ";
        }
        std::cout << "[0,1]^2" << std::endl;
        X(0) += 0.4;
    } while (X(0) < 1.5);

    delete square;
    return 0;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/WhiteNoiseAcceleration.h>

#include <cmath>
#include <utility>

#include <Eigen/Cholesky>

using namespace bfl;
using namespace Eigen;

WhiteNoiseAcceleration::WhiteNoiseAcceleration
(
    double T,
    double tilde_q,
    unsigned int seed
) noexcept :
    generator_(std::mt19937_64(seed)),
    distribution_(std::normal_distribution<double>(0.0, 1.0)),
    T_(T),
    tilde_q_(tilde_q),
    gauss_rnd_sample_([&] { return (distribution_)(generator_); })
{
    F_ << 1.0,  T_, 0.0, 0.0,
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 1.0,  T_,
          0.0, 0.0, 0.0, 1.0;

    double q11 = 1.0/3.0 * std::pow(T_, 3.0);
    double q2  = 1.0/2.0 * std::pow(T_, 2.0);
    Q_ << q11,  q2, 0.0, 0.0,
           q2,  T_, 0.0, 0.0,
          0.0, 0.0, q11,  q2,
          0.0, 0.0,  q2,  T_;
    Q_ *= tilde_q;

    LDLT<Matrix4d> chol_ldlt(Q_);
    sqrt_Q_ = (chol_ldlt.transpositionsP() * Matrix4d::Identity()).transpose() * chol_ldlt.matrixL() * chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();
}

WhiteNoiseAcceleration::WhiteNoiseAcceleration(double T, double tilde_q) noexcept :
    WhiteNoiseAcceleration(T, tilde_q, 1)
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration() noexcept :
    WhiteNoiseAcceleration(1.0, 1.0, 1)
{ }

WhiteNoiseAcceleration::~WhiteNoiseAcceleration() noexcept
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(const WhiteNoiseAcceleration& wna) :
    generator_(wna.generator_),
    distribution_(wna.distribution_),
    T_(wna.T_),
    F_(wna.F_),
    Q_(wna.Q_),
    tilde_q_(wna.tilde_q_),
    sqrt_Q_(wna.sqrt_Q_),
    gauss_rnd_sample_(wna.gauss_rnd_sample_)
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(WhiteNoiseAcceleration&& wna) noexcept :
    generator_(std::move(wna.generator_)),
    distribution_(std::move(wna.distribution_)),
    T_(wna.T_),
    F_(std::move(wna.F_)),
    Q_(std::move(wna.Q_)),
    tilde_q_(wna.tilde_q_),
    sqrt_Q_(std::move(wna.sqrt_Q_)),
    gauss_rnd_sample_(std::move(wna.gauss_rnd_sample_))
{
    wna.T_       = 0.0;
    wna.tilde_q_ = 0.0;
}

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(const WhiteNoiseAcceleration& wna)
{
    WhiteNoiseAcceleration tmp(wna);
    *this = std::move(tmp);

    return *this;
}

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(WhiteNoiseAcceleration&& wna) noexcept
{
    T_       = wna.T_;
    F_       = std::move(wna.F_);
    Q_       = std::move(wna.Q_);
    tilde_q_ = wna.tilde_q_;

    sqrt_Q_           = std::move(wna.sqrt_Q_);
    generator_        = std::move(wna.generator_);
    distribution_     = std::move(wna.distribution_);
    gauss_rnd_sample_ = std::move(wna.gauss_rnd_sample_);

    wna.T_       = 0.0;
    wna.tilde_q_ = 0.0;

    return *this;
}

MatrixXd WhiteNoiseAcceleration::getNoiseSample(const std::size_t num)
{
    MatrixXd rand_vectors(4, num);
    for (int i = 0; i < rand_vectors.size(); i++)
        *(rand_vectors.data() + i) = gauss_rnd_sample_();

    return sqrt_Q_ * rand_vectors;
}

MatrixXd WhiteNoiseAcceleration::getNoiseCovarianceMatrix()
{
    return Q_;
}

MatrixXd WhiteNoiseAcceleration::getStateTransitionMatrix()
{
    return F_;
}

VectorXd WhiteNoiseAcceleration::getTransitionProbability(const Ref<const MatrixXd>& prev_states, Ref<MatrixXd> cur_states)
{
    VectorXd probabilities(prev_states.cols());
    MatrixXd differences = cur_states - prev_states;

    std::size_t size = differences.rows();
    for (std::size_t i = 0; i < prev_states.cols(); i++)
    {
        probabilities(i) = (-0.5 * static_cast<double>(size) * log(2.0 * M_PI) + -0.5 * log(Q_.determinant()) -0.5 * (differences.col(i).transpose() * Q_.inverse() * differences.col(i)).array()).exp().coeff(0);
    }

    return probabilities;
}

std::pair<std::size_t, std::size_t> WhiteNoiseAcceleration::getOutputSize() const
{
    return std::make_pair(4, 0);
}


/* test_cauchy.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/cauchy_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/cauchy.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::cauchy_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME cauchy
#define BOOST_MATH_DISTRIBUTION boost::math::cauchy
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME median
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(-n, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME sigma
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.001, n)

#include "test_real_distribution.ipp"


#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <scitbx/math/basic_statistics.h>

#include <boost/python/class.hpp>
#include <vector>

namespace scitbx { namespace af { namespace boost_python {

  template <typename FloatType>
  struct median_functor_wrapper
  {
    typedef math::median_functor wt;

    static
    FloatType
    call(
      wt& O,
      af::const_ref<FloatType> const& data)
    {
      std::vector<FloatType> buffer(data.begin(), data.end());
      return O(af::make_ref(buffer));
    }

    static
    math::median_statistics<FloatType>
    dispersion(
      wt& O,
      af::const_ref<FloatType> const &data)
    {
      std::vector<FloatType> buffer(data.begin(), data.end());
      return O.dispersion(af::make_ref(buffer));
    }

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt>(name, no_init)
        .def(init<>())
        .def(init<wt::random_number_engine_t::result_type>(arg("seed")))
        .def("__call__", call, arg("data"))
        .def("dispersion", dispersion, arg("data"))
      ;
    }
  };

  template <typename FloatType>
  struct median_statistics_wrapper
  {
    typedef math::median_statistics<FloatType> wt;

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt>(name, no_init)
        .def_readonly("median", &wt::median)
        .def_readonly("median_absolute_deviation",
                      &wt::median_absolute_deviation)
      ;
    }
  };

  void wrap_flex_median_statistics() {
    median_statistics_wrapper<double>::wrap("median_statistics");
    median_functor_wrapper<double>::wrap("median_functor");
  }

}}}


#include <armadillo>

using namespace std;

class LinearRegression{
    public:
    int number_of_variables;
    arma::mat weights;

    LinearRegression(int num);

    void train(arma::mat& X_train, arma::mat& y_train, float alpha, int epochs);

    arma::mat predict(arma::mat& X_predict);
};


#include <iostream>

#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

void usage(char *progname) {
	cout << "This program returns the factors p and q of "
		"n if p - q < n^(1/4)."
		<< endl;
	cout << "Usage: " << progname << " n" << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 2) { usage(argv[0]); return 3; }

	ZZ n, a, b, p, q;
	n = conv<ZZ>(argv[1]);

	// p = floor(SqrRoot(n))
	SqrRoot(a, n);
	// q = p*p - n
	sqr(b, a);
	b -= n;

	while(b < 0 || sqr(SqrRoot(b)) != b) {
		a++;
		sqr(b, a);
		b -= n;
	}

	p = a - SqrRoot(b);
	q = n / p;

	cout << p << endl << q << endl;
}


#pragma once

#include "../../Primitives/SignalTraits.hpp"
#include "../../Utility/Numbers.hpp"

#include <Eigen/Dense>
#include <Eigen/QR>

namespace dspbb::fir {

namespace impl {

	template <class T>
	auto CoefficientMatrix(size_t filterLength, size_t gridSize) {
		Eigen::Matrix<T, Eigen::Dynamic, 1> col;
		Eigen::Matrix<T, 1, Eigen::Dynamic> row;

		col.resize(gridSize);
		row.resize(filterLength);

		for (size_t i = 0; i < gridSize; ++i) {
			col(i) = T(i);
		}
		col *= T(1) / T(gridSize - 1) * pi_v<T>;

		for (size_t i = 0; i < filterLength; ++i) {
			row(i) = T(i);
		}

		Eigen::MatrixX<T> coefficientMatrix = col * row;

		for (size_t row = 0; row < gridSize; ++row) {
			coefficientMatrix(row, 0) = T(1);
			for (size_t col = 1; col < filterLength; ++col) {
				coefficientMatrix(row, col) = 2 * std::cos(coefficientMatrix(row, col));
			}
		}

		return coefficientMatrix;
	}

	template <class T, class Func>
	auto WeightMatrix(size_t gridSize, const Func& weight) {
		Eigen::DiagonalMatrix<T, Eigen::Dynamic> weightMatrix;
		weightMatrix.resize(gridSize);
		for (size_t i = 0; i < gridSize; ++i) {
			weightMatrix.diagonal()(i) = weight(T(i) / T(gridSize - 1));
		}
		return weightMatrix;
	}

	template <class T, class Func>
	auto ResponseVector(size_t gridSize, const Func& response) {
		Eigen::Matrix<T, Eigen::Dynamic, 1> responseVector;
		responseVector.resize(gridSize);
		for (size_t i = 0; i < gridSize; ++i) {
			responseVector(i) = T(i);
		}
		responseVector *= T(1) / T(gridSize - 1);
		for (size_t i = 0; i < gridSize; ++i) {
			responseVector(i) = response(responseVector(i));
		}
		return responseVector;
	}

} // namespace impl

template <class SignalR, class ResponseFunc, class WeightFunc, std::enable_if_t<is_mutable_signal_v<SignalR>, int> = 0>
void KernelLeastSquares(SignalR&& coefficients, ResponseFunc responseFunc, WeightFunc weightFunc, size_t gridSize = 0) {
	using R = typename std::decay_t<SignalR>::value_type;
	using T = remove_complex_t<R>;

	const size_t filterLength = (coefficients.Size() + 1) / 2;
	gridSize = gridSize == 0 ? 4 * filterLength : std::min(filterLength, gridSize);

	const auto coefficientMatrix = impl::CoefficientMatrix<T>(filterLength, gridSize);
	const auto weightMatrix = impl::WeightMatrix<T>(gridSize, weightFunc);
	const auto responseVector = impl::ResponseVector<T>(gridSize, responseFunc);

	Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixX<T>> decomp{ weightMatrix * coefficientMatrix };
	const Eigen::VectorX<T> halfFilter = decomp.solve(weightMatrix * responseVector);
	for (size_t i = 0; i < filterLength; ++i) {
		coefficients[i] = halfFilter(filterLength - i - 1);
		coefficients[i + filterLength - 1] = halfFilter[i];
	}
}

} // namespace dspbb::fir


#include <cmath>
#include <iomanip>
#include <iostream>

#include <sodium/sodium.h>
#include <boost/numeric/odeint.hpp>
#include <catch/catch.hpp>

#include "../include/Plant.hpp"
#include "../include/control-frp.hpp"
#include "../include/pid.hpp"
#include "../include/util/util-frp.hpp"
#include "../include/util/util.hpp"

#include "calculations/analytical_solutions.cpp"

#ifdef PLOT
#include "../include/plotting/gnuplot-iostream.h"
#include "../include/plotting/plot-helpers.hpp"
#endif  // PLOT

namespace ode = boost::numeric::odeint;
using CState = PIDState<>;
using sim::PState;

constexpr double dt = 0.001;  // seconds.
constexpr auto dts = util::double_to_duration(dt);
const auto now = chrono::steady_clock::now();

constexpr double mass = 1.;
constexpr double damp = 10. / mass;
constexpr double spring = 20. / mass;
constexpr double staticForce = 1. / mass;
constexpr double simTime = 2;  // seconds
const sim::Plant plant(staticForce, damp, spring);

ode::runge_kutta4<PState> stepper;

inline SignalPt<double> to_error(const SignalPt<PState>& a, const PState& b) {
  return {a.time, a.value[0] - b[0]};
}

TEST_CASE(
    "Given system and controller parameters, simulation should reproduce "
    "analytically computed step responses to within a margin of error. See "
    "src/calculations for details. Simulations performed using FRP.") {
  const CState u0 = {now, 0., 0., 0.};
  // Setpoint to x = 1, for step response.
  const sodium::cell<PState> setPoint({1., 0., 0.});

  SECTION("Test A (Proportional Control) FRP.") {
    constexpr double Kp = 300.;
    constexpr double Ki = 0.;
    constexpr double Kd = 0.;

    const sodium::stream_sink<SignalPt<PState>> sPlantState;

    auto cControl = ctrl::control_frp(
        pid_algebra(Kp, Ki, Kd), &to_error, setPoint,
        static_cast<sodium::stream<SignalPt<PState>>>(sPlantState), u0);

    auto [sPlantState_unlisten, plantRecord] = util::make_listener(sPlantState);
    {
      PState x = {0., 0., 0.};
      sPlantState.send({now, x});
      for (int k = 1; k < simTime / dt; ++k) {
        x[2] = cControl.sample().ctrlVal;
        stepper.do_step(plant, x, 0, dt);
        sPlantState.send({now + k * dts, x});
      }
    }
    sPlantState_unlisten();

    {
      constexpr double margin = 0.03;
      auto simulatedPositions =
          util::fmap([](auto x) { return x.value[0]; }, *plantRecord);
      auto theoreticalPositions = util::fmap(
          [](auto x) {
            return analyt::test_A(util::unchrono_sec(x.time - now));
          },
          *plantRecord);

#ifdef PLOT

      const auto testData = util::fmap(
          [](const auto& x) {
            return std::make_pair(util::unchrono_sec(x.time - now), x.value[0]);
          },
          *plantRecord);

      plot_with_tube("Test A, (Kp, Ki, Kd) = (300, 0, 0).", testData,
                     &analyt::test_A, margin);

#endif  // PLOT

      REQUIRE(util::compareVectors(simulatedPositions, theoreticalPositions,
                                   margin));
    }
  }

  SECTION("Test B (Proportional-Derivative Control) anamorphism.") {
    constexpr double Kp = 300.;
    constexpr double Ki = 0.;
    constexpr double Kd = 10.;

    const sodium::stream_sink<SignalPt<PState>> sPlantState;

    auto cControl = ctrl::control_frp(
        pid_algebra(Kp, Ki, Kd), &to_error, setPoint,
        static_cast<sodium::stream<SignalPt<PState>>>(sPlantState), u0);

    auto [sPlantState_unlisten, plantRecord] = util::make_listener(sPlantState);
    {
      PState x = {0., 0., 0.};
      sPlantState.send({now, x});
      for (int k = 1; k < simTime / dt; ++k) {
        x[2] = cControl.sample().ctrlVal;
        stepper.do_step(plant, x, 0, dt);
        sPlantState.send({now + k * dts, x});
      }
    }
    sPlantState_unlisten();

    {
      constexpr double margin = 0.03;
      auto simulatedPositions =
          util::fmap([](auto x) { return x.value[0]; }, *plantRecord);
      auto theoreticalPositions = util::fmap(
          [](auto x) {
            return analyt::test_B(util::unchrono_sec(x.time - now));
          },
          *plantRecord);

#ifdef PLOT
      const auto testData = util::fmap(
          [](const auto& x) {
            return std::make_pair(util::unchrono_sec(x.time - now), x.value[0]);
          },
          *plantRecord);

      plot_with_tube("Test B, (Kp, Ki, Kd) = (300, 0, 10).", testData,
                     &analyt::test_B, margin);
#endif  // PLOT

      REQUIRE(util::compareVectors(simulatedPositions, theoreticalPositions,
                                   margin));
    }
  }

  SECTION("Test C (Proportional-Integral Control) anamorphism.") {
    constexpr double Kp = 30.;
    constexpr double Ki = 70.;
    constexpr double Kd = 0.;

    // Setpoint to x = 1, for step response.
    const sodium::cell<PState> setPoint({1., 0., 0.});
    const sodium::stream_sink<SignalPt<PState>> sPlantState;

    const CState u0 = {now - dts, 0., 0., 0.};
    auto cControl = ctrl::control_frp(
        pid_algebra(Kp, Ki, Kd), &to_error, setPoint,
        static_cast<sodium::stream<SignalPt<PState>>>(sPlantState), u0);

    auto [sPlantState_unlisten, plantRecord] = util::make_listener(sPlantState);
    {
      PState x = {0., 0., 0.};
      sPlantState.send({now, x});
      for (int k = 1; k < simTime / dt; ++k) {
        x[2] = cControl.sample().ctrlVal;
        stepper.do_step(plant, x, 0, dt);
        sPlantState.send({now + k * dts, x});
      }
    }
    sPlantState_unlisten();

    {
      constexpr double margin = 0.03;

      auto simulatedPositions =
          util::fmap([](auto x) { return x.value[0]; }, *plantRecord);
      auto theoreticalPositions = util::fmap(
          [](auto x) {
            return analyt::test_C(util::unchrono_sec(x.time - now));
          },
          *plantRecord);

#ifdef PLOT
      const auto testData = util::fmap(
          [](const auto& x) {
            return std::make_pair(util::unchrono_sec(x.time - now), x.value[0]);
          },
          *plantRecord);

      plot_with_tube("Test C, (Kp, Ki, Kd) = (30, 70, 0).", testData,
                     &analyt::test_C, margin);
#endif  // PLOT

      REQUIRE(util::compareVectors(simulatedPositions, theoreticalPositions,
                                   margin));
    }
  }

  SECTION("Test D (Proportional-Integral-Derivative Control) anamorphism.") {
    constexpr double Kp = 350.;
    constexpr double Ki = 300.;
    constexpr double Kd = 50.;

    // Setpoint to x = 1, for step response.
    const sodium::cell<PState> setPoint({1., 0., 0.});
    const sodium::stream_sink<SignalPt<PState>> sPlantState;

    const CState u0 = {now - dts, 0., 0., 0.};
    auto cControl = ctrl::control_frp(
        pid_algebra(Kp, Ki, Kd), &to_error, setPoint,
        static_cast<sodium::stream<SignalPt<PState>>>(sPlantState), u0);

    auto [sPlantState_unlisten, plantRecord] = util::make_listener(sPlantState);
    {
      PState x = {0., 0., 0.};
      sPlantState.send({now, x});
      for (int k = 1; k < simTime / dt; ++k) {
        x[2] = cControl.sample().ctrlVal;
        stepper.do_step(plant, x, 0, dt);
        sPlantState.send({now + k * dts, x});
      }
    }
    sPlantState_unlisten();

    {
      constexpr double margin = 0.07;

      auto simulatedPositions =
          util::fmap([](auto x) { return x.value[0]; }, *plantRecord);
      auto theoreticalPositions = util::fmap(
          [](auto x) {
            return analyt::test_D(util::unchrono_sec(x.time - now));
          },
          *plantRecord);

#ifdef PLOT
      const auto testData = util::fmap(
          [](const auto& x) {
            return std::make_pair(util::unchrono_sec(x.time - now), x.value[0]);
          },
          *plantRecord);

      plot_with_tube("Test B, (Kp, Ki, Kd) = (350, 300, 50).", testData,
                     &analyt::test_D, margin);
#endif  // PLOT

      REQUIRE(util::compareVectors(simulatedPositions, theoreticalPositions,
                                   margin));
    }
  }
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Siargey Kachanovich
 *
 *    Copyright (C) 2019 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "random_orthogonal_matrix_function"
#include <boost/test/unit_test.hpp>
#include <gudhi/Unitary_tests_utils.h>

#include <gudhi/Functions/random_orthogonal_matrix.h>

#include <string>

#include <random>
#include <cstdlib>

using namespace Gudhi::coxeter_triangulation;

// this test is separated as it requires CGAL
BOOST_AUTO_TEST_CASE(random_orthogonal_matrix_function) {
  // random orthogonal matrix
  Eigen::MatrixXd matrix = random_orthogonal_matrix(5);
  Eigen::MatrixXd id_matrix = matrix.transpose() * matrix;
  for (std::size_t i = 0; i < 5; ++i)
    for (std::size_t j = 0; j < 5; ++j)
      if (i == j)
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(id_matrix(i, j), 1.0, 1e-10);
      else
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(id_matrix(i, j), 0.0, 1e-10);
}


#pragma once
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "typedefs.hpp"

namespace yavque
{
Eigen::SparseMatrix<double> pauli_x();
Eigen::SparseMatrix<cx_double> pauli_y();
Eigen::SparseMatrix<double> pauli_z();
Eigen::SparseMatrix<double> pauli_xx();
Eigen::SparseMatrix<double> pauli_yy();
Eigen::SparseMatrix<double> pauli_zz();
} // namespace yavque


#pragma once

/* Classical constant propagation domain */

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/interval.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/support/stats.hpp>

#include <boost/optional.hpp>

namespace crab {
namespace domains {
/**
 *  Each variable is mapped to an element in this lattice:
 *
 *            top
 *             |
 * ...,-3,-2,-1,0,1,2,3,...
 *             |
 *           bottom
 *
 * top means that it might not be a constant value
 **/
template <typename Number> class constant {
  boost::optional<Number> m_constant;
  bool m_is_bottom;
  using constant_t = constant<Number>;
  constant(bool is_bottom) : m_constant(boost::none), m_is_bottom(is_bottom) {}
public:

  constant(Number c) : m_constant(c), m_is_bottom(false) {}

  static constant_t bottom() { return constant(true); }

  static constant_t top() { return constant(false); }

  static constant_t zero() { return constant(Number(0));}

  bool is_bottom() const { return m_is_bottom; }

  bool is_top() const { return (!is_bottom() && !m_constant); }

  bool is_constant() const { return m_constant != boost::none; }

  Number get_constant() const {
    assert(is_constant());
    return *m_constant;
  }

  /** Begin Lattice Operations **/
  bool operator<=(const constant_t &o) const {
    if (is_bottom() || o.is_top()) {
      return true;
    } else if (o.is_bottom() || is_top()) {
      return false;
    } else {
      assert(is_constant());
      assert(o.is_constant());
      return get_constant() == o.get_constant();
    }
  }

  bool operator==(const constant_t &o) const {
    return (m_is_bottom == o.m_is_bottom && m_constant == o.m_constant);
  }

  constant_t operator|(const constant_t &o) const {
    if (is_bottom() || o.is_top())
      return o;
    else if (is_top() || o.is_bottom())
      return *this;
    else {
      assert(is_constant());
      assert(o.is_constant());
      if (get_constant() == o.get_constant()) {
        return *this;
      } else {
        return constant_t::top();
      }
    }
  }

  constant_t operator||(const constant_t &o) const { return *this | o; }

  template <typename Thresholds>
  constant_t widening_thresholds(const constant_t &o,
                                 const Thresholds &ts /*unused*/) const {
    return *this | o;
  }

  constant_t operator&(const constant_t &o) const {
    if (is_bottom() || o.is_top())
      return *this;
    else if (is_top() || o.is_bottom()) {
      return o;
    } else {
      assert(is_constant());
      assert(o.is_constant());
      if (get_constant() == o.get_constant()) {
        return *this;
      } else {
        return constant_t::bottom();
      }
    }
  }

  constant_t operator&&(const constant_t &o) const { return *this & o; }
  /** End  Lattice Operations **/

  /** Begin arithmetic operations **/
  constant_t Add(const constant_t &o) const {
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() + o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t Sub(const constant_t &o) const {
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() - o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t Mul(const constant_t &o) const {
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() * o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t SDiv(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() / o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t SRem(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() % o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t UDiv(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    return constant_t::top();
  }
  constant_t URem(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    return constant_t::top();
  }
  /** End arithmetic operations **/

  /** Begin bitwise operations **/
  // These operations depend on the type of Number
  constant_t BitwiseAnd(const constant_t &o) const;
  constant_t BitwiseOr(const constant_t &o) const;
  constant_t BitwiseXor(const constant_t &o) const;
  constant_t BitwiseShl(const constant_t &o) const;
  constant_t BitwiseLShr(const constant_t &o) const;
  constant_t BitwiseAShr(const constant_t &o) const;
  /** End bitwise operations **/

  void write(crab::crab_os &o) const {
    if (is_bottom()) {
      o << "_|_";
    } else if (is_top()) {
      o << "top";
    } else {
      assert(is_constant());
      o << get_constant();
    }
  }

  friend inline crab_os &operator<<(crab_os &o, const constant_t &c) {
    c.write(o);
    return o;
  }

};

namespace constant_details {
using z_constant_t = constant<ikos::z_number>;
using q_constant_t = constant<ikos::q_number>;
} // end namespace constant_details

template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseAnd(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    return constant_details::z_constant_t(get_constant() & o.get_constant());
  } else {
    return constant_details::z_constant_t::top();
  }
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseOr(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    return constant_details::z_constant_t(get_constant() | o.get_constant());
  } else {
    return constant_details::z_constant_t::top();
  }
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseXor(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    return constant_details::z_constant_t(get_constant() ^ o.get_constant());
  } else {
    return constant_details::z_constant_t::top();
  }
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseShl(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    if (o.get_constant() >= ikos::z_number(0)) {
      return constant_details::z_constant_t(get_constant() << o.get_constant());
    }
  }
  return constant_details::z_constant_t::top();
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseLShr(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    // if get_contant() is non-negative then LShr = AShr.
    if (get_constant() >= ikos::z_number(0)) {
      if (o.get_constant() >= ikos::z_number(0)) {
	return constant_details::z_constant_t(get_constant() >> o.get_constant());
      }
    }
  }
  return constant_details::z_constant_t::top();
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseAShr(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    if (o.get_constant() >= ikos::z_number(0)) {
      return constant_details::z_constant_t(get_constant() >> o.get_constant());
    }
  }
  return constant_details::z_constant_t::top();
}

template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseAnd(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseOr(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseXor(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseShl(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseLShr(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseAShr(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}

template <typename Number, typename VariableName>
class constant_domain final : public crab::domains::abstract_domain_api<
                                  constant_domain<Number, VariableName>> {
public:
  using constant_domain_t = constant_domain<Number, VariableName>;
  using abstract_domain_t =
      crab::domains::abstract_domain_api<constant_domain_t>;
  using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
  using typename abstract_domain_t::interval_t;
  using typename abstract_domain_t::linear_constraint_system_t;
  using typename abstract_domain_t::linear_constraint_t;
  using typename abstract_domain_t::linear_expression_t;
  using typename abstract_domain_t::reference_constraint_t;
  using typename abstract_domain_t::variable_or_constant_t;
  using typename abstract_domain_t::variable_t;
  using typename abstract_domain_t::variable_vector_t;
  using typename abstract_domain_t::variable_or_constant_vector_t;
  using constant_t = constant<Number>;
  using number_t = Number;
  using varname_t = VariableName;

private:
  using interval_domain_t = ikos::interval_domain<number_t, varname_t>;
  using separate_domain_t = ikos::separate_domain<variable_t, constant_t>;

private:
  separate_domain_t m_env;

  constant_domain(separate_domain_t &&env) : m_env(std::move(env)) {}

  constant_t eval(const linear_expression_t &expr) const {
    assert(!is_bottom());
    constant_t r(expr.constant());
    for (auto const&kv : expr) {
      constant_t c(kv.first);
      r  = r.Add(c.Mul(m_env[kv.second]));
      if (r.is_top()) {
	break;
      }
    }
    return r;
  }

  constant_t compute_residual(const linear_constraint_t &cst, const variable_t &pivot) const {
    constant_t residual(cst.constant());
    for (auto const&kv: cst) {
      constant_t c(kv.first);
      const variable_t &v = kv.second;
      if (!(v == pivot)) {
	residual = residual.Sub(c.Mul(m_env[v]));
	if (residual.is_top()) {
	  break;
	}
      }
    }
    return residual;
  }

  void propagate(const linear_constraint_t &cst){
    if (is_bottom()) {
      return;
    }

    if (cst.is_inequality() || cst.is_strict_inequality() || cst.is_disequation()) {
      constant_t e = eval(cst.expression());
      if (e.is_constant()) {
	if (cst.is_inequality()) {
	  if (!(e.get_constant() <= number_t(0))) {
	    set_to_bottom();
	    return;
	  }
	} else if (cst.is_disequation()) {
	  if (!(e.get_constant() != number_t(0))) {
	    set_to_bottom();
	    return;
	  }
	} else if (cst.is_strict_inequality()) {
	  if (!(e.get_constant() < number_t(0))) {
	    set_to_bottom();
	    return;
	  }
	}
      }
    } else if (cst.is_equality()) {
      for (auto kv : cst) {
	number_t c = kv.first;
	const variable_t &pivot = kv.second;
	constant_t new_c = compute_residual(cst, pivot).SDiv(c);
	if (!new_c.is_top()) {
	  m_env.set(pivot, m_env[pivot] & new_c);
	}
      }
    }
  }

  void solve_constraints(const linear_constraint_system_t &csts) {
    for (auto const &c : csts) {
      if (is_bottom()) {
	return;
      }
      if (c.is_inequality() && c.is_unsigned()) {
	// we don't handle unsigned constraints
	continue;
      }
      if (c.is_tautology()) {
	continue;
      }
      if (c.is_contradiction()) {
	set_to_bottom();
	return;
      }
      propagate(c);
    }
  }

public:
  constant_domain_t make_top() const override {
    return constant_domain_t(separate_domain_t::top());
  }

  constant_domain_t make_bottom() const override {
    return constant_domain_t(separate_domain_t::bottom());
  }

  void set_to_top() override {
    constant_domain abs(separate_domain_t::top());
    std::swap(*this, abs);
  }

  void set_to_bottom() override {
    constant_domain abs(separate_domain_t::bottom());
    std::swap(*this, abs);
  }

  constant_domain() : m_env(separate_domain_t::top()) {}

  constant_domain(const constant_domain_t &e) : m_env(e.m_env) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
  }

  constant_domain(constant_domain_t &&e) : m_env(std::move(e.m_env)) {}

  constant_domain_t &operator=(const constant_domain_t &o) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
    if (this != &o) {
      m_env = o.m_env;
    }
    return *this;
  }

  constant_domain_t &operator=(constant_domain_t &&o) {
    if (this != &o) {
      m_env = std::move(o.m_env);
    }
    return *this;
  }

  constant_t get_constant(const variable_t &v) const {
    return m_env[v];
  }

  void set_constant(const variable_t &v, constant_t c) {
    m_env.set(v, c);
  }

  bool is_bottom() const override { return m_env.is_bottom(); }

  bool is_top() const override { return m_env.is_top(); }

  bool operator<=(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.leq");
    crab::ScopedCrabStats __st__(domain_name() + ".leq");
    return (m_env <= o.m_env);
  }

  void operator|=(const constant_domain_t &o) override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    CRAB_LOG("constant-domain",
             crab::outs() << "Join " << m_env << " and " << o.m_env << "\n";);
    m_env = m_env | o.m_env;
    CRAB_LOG("constant-domain", crab::outs() << "Res=" << m_env << "\n";);
  }

  constant_domain_t operator|(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    return (m_env | o.m_env);
  }

  constant_domain_t operator&(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.meet");
    crab::ScopedCrabStats __st__(domain_name() + ".meet");
    return (m_env & o.m_env);
  }

  constant_domain_t operator||(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return (m_env || o.m_env);
  }

  constant_domain_t widening_thresholds(
      const constant_domain_t &o,
      const crab::iterators::thresholds<number_t> &ts) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return m_env.widening_thresholds(o.m_env, ts);
  }

  constant_domain_t operator&&(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.narrowing");
    crab::ScopedCrabStats __st__(domain_name() + ".narrowing");
    return (m_env && o.m_env);
  }

  void operator-=(const variable_t &v) override {
    crab::CrabStats::count(domain_name() + ".count.forget");
    crab::ScopedCrabStats __st__(domain_name() + ".forget");
    m_env -= v;
  }

  interval_t operator[](const variable_t &v) override {
    constant_t c = m_env[v];
    if (c.is_bottom()) {
      return interval_t::bottom();
    } else if (c.is_top()) {
      return interval_t::top();
    } else {
      assert(c.is_constant());
      return interval_t(c.get_constant());
    }
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    crab::CrabStats::count(domain_name() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(domain_name() + ".add_constraints");
    solve_constraints(csts);
  }

  void assign(const variable_t &x, const linear_expression_t &e) override {
    crab::CrabStats::count(domain_name() + ".count.assign");
    crab::ScopedCrabStats __st__(domain_name() + ".assign");
    if (boost::optional<variable_t> v = e.get_variable()) {
      m_env.set(x, m_env[(*v)]);
    } else {
      m_env.set(x, eval(e));
    }
  }

  void apply(crab::domains::arith_operation_t op, const variable_t &x,
             const variable_t &y, const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc = m_env[z];
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_ADDITION:
	xc = yc.Add(zc);
	break;
      case crab::domains::OP_SUBTRACTION:
	xc = yc.Sub(zc);
	break;
      case crab::domains::OP_MULTIPLICATION:
	xc = yc.Mul(zc);
	break;
      case crab::domains::OP_SDIV:
	xc = yc.SDiv(zc);
	break;
      case crab::domains::OP_SREM:
	xc = yc.SRem(zc);
	break;
      case crab::domains::OP_UDIV:
	xc = yc.UDiv(zc);
	break;
      case crab::domains::OP_UREM:
	xc = yc.URem(zc);
	break;
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }
  }

  void apply(crab::domains::arith_operation_t op, const variable_t &x,
             const variable_t &y, number_t k) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc(k);
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_ADDITION:
	xc = yc.Add(zc);
	break;
      case crab::domains::OP_SUBTRACTION:
	xc = yc.Sub(zc);
	break;
      case crab::domains::OP_MULTIPLICATION:
	xc = yc.Mul(zc);
	break;
      case crab::domains::OP_SDIV:
	xc = yc.SDiv(zc);
	break;
      case crab::domains::OP_SREM:
	xc = yc.SRem(zc);
	break;
      case crab::domains::OP_UDIV:
	xc = yc.UDiv(zc);
	break;
      case crab::domains::OP_UREM:
	xc = yc.URem(zc);
	break;
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }

  }

  // intrinsics operations
  void intrinsic(std::string name,
		 const variable_or_constant_vector_t &inputs,
                 const variable_vector_t &outputs) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }

  void backward_intrinsic(std::string name,
			  const variable_or_constant_vector_t &inputs,
                          const variable_vector_t &outputs,
                          const constant_domain_t &invariant) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }

  // backward arithmetic operations
  void backward_assign(const variable_t &x, const linear_expression_t &e,
                       const constant_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_assign");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_assign");
    // TODO
  }

  void backward_apply(crab::domains::arith_operation_t op, const variable_t &x,
                      const variable_t &y, number_t z,
                      const constant_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    // TODO
  }

  void backward_apply(crab::domains::arith_operation_t op, const variable_t &x,
                      const variable_t &y, const variable_t &z,
                      const constant_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    // TODO
  }

  // cast operations
  void apply(crab::domains::int_conv_operation_t /*op*/, const variable_t &dst,
             const variable_t &src) override {
    // ignore the widths
    assign(dst, src);
  }

  // bitwise operations
  void apply(crab::domains::bitwise_operation_t op, const variable_t &x,
             const variable_t &y, const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc = m_env[z];
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_AND:
	xc = yc.BitwiseAnd(zc);
	break;
      case crab::domains::OP_OR:
	xc = yc.BitwiseOr(zc);
	break;
      case crab::domains::OP_XOR:
	xc = yc.BitwiseXor(zc);
	break;
      case crab::domains::OP_SHL:
	xc = yc.BitwiseShl(zc);
	break;
      case crab::domains::OP_LSHR:
	xc = yc.BitwiseLShr(zc);
	break;
      case crab::domains::OP_ASHR: {
	xc = yc.BitwiseAShr(zc);
	break;
      }
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }
  }

  void apply(crab::domains::bitwise_operation_t op, const variable_t &x,
             const variable_t &y, number_t k) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc(k);
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_AND:
	xc = yc.BitwiseAnd(zc);
	break;
      case crab::domains::OP_OR:
	xc = yc.BitwiseOr(zc);
	break;
      case crab::domains::OP_XOR:
	xc = yc.BitwiseXor(zc);
	break;
      case crab::domains::OP_SHL:
	xc = yc.BitwiseShl(zc);
	break;
      case crab::domains::OP_LSHR:
	xc = yc.BitwiseLShr(zc);
	break;
      case crab::domains::OP_ASHR: {
	xc = yc.BitwiseAShr(zc);
	break;
      }
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }

  }

  virtual void select(const variable_t &lhs, const linear_constraint_t &cond,
                      const linear_expression_t &e1,
                      const linear_expression_t &e2) override {
    crab::CrabStats::count(domain_name() + ".count.select");
    crab::ScopedCrabStats __st__(domain_name() + ".select");

    if (!is_bottom()) {
      constant_domain_t inv1(*this);
      inv1 += cond;
      if (inv1.is_bottom()) {
        assign(lhs, e2);
        return;
      }
      constant_domain_t inv2(*this);
      inv2 += cond.negate();
      if (inv2.is_bottom()) {
        assign(lhs, e1);
        return;
      }
      m_env.set(lhs, eval(e1) | eval(e2));
    }
  }

  /// constant_domain implements only standard abstract operations of
  /// a numerical domain so it is intended to be used as a leaf domain
  /// in the hierarchy of domains.
  BOOL_OPERATIONS_NOT_IMPLEMENTED(constant_domain_t)
  ARRAY_OPERATIONS_NOT_IMPLEMENTED(constant_domain_t)
  REGION_AND_REFERENCE_OPERATIONS_NOT_IMPLEMENTED(constant_domain_t)

  void forget(const variable_vector_t &variables) override {
    if (is_bottom() || is_top()) {
      return;
    }
    for (auto const &var : variables) {
      this->operator-=(var);
    }
  }

  void project(const variable_vector_t &variables) override {
    crab::CrabStats::count(domain_name() + ".count.project");
    crab::ScopedCrabStats __st__(domain_name() + ".project");

    m_env.project(variables);
  }

  void rename(const variable_vector_t &from,
              const variable_vector_t &to) override {
    crab::CrabStats::count(domain_name() + ".count.rename");
    crab::ScopedCrabStats __st__(domain_name() + ".rename");

    m_env.rename(from, to);
  }

  void expand(const variable_t &x, const variable_t &new_x) override {
    crab::CrabStats::count(domain_name() + ".count.expand");
    crab::ScopedCrabStats __st__(domain_name() + ".expand");

    if (is_bottom() || is_top()) {
      return;
    }

    m_env.set(new_x, m_env[x]);
  }

  void normalize() override {}

  void minimize() override {}

  void write(crab::crab_os &o) const override {
    crab::CrabStats::count(domain_name() + ".count.write");
    crab::ScopedCrabStats __st__(domain_name() + ".write");

    m_env.write(o);
  }

  linear_constraint_system_t to_linear_constraint_system() const override {
    crab::CrabStats::count(domain_name() +
                           ".count.to_linear_constraint_system");
    crab::ScopedCrabStats __st__(domain_name() +
                                 ".to_linear_constraint_system");

    linear_constraint_system_t csts;

    if (this->is_bottom()) {
      csts += linear_constraint_t::get_false();
      return csts;
    }

    for (auto it = m_env.begin(); it != m_env.end(); ++it) {
      const variable_t &v = it->first;
      const constant_t &c = it->second;
      if (c.is_constant()) {
        csts += linear_constraint_t(v == c.get_constant());
      }
    }
    return csts;
  }

  disjunctive_linear_constraint_system_t
  to_disjunctive_linear_constraint_system() const override {
    auto lin_csts = to_linear_constraint_system();
    if (lin_csts.is_false()) {
      return disjunctive_linear_constraint_system_t(true /*is_false*/);
    } else if (lin_csts.is_true()) {
      return disjunctive_linear_constraint_system_t(false /*is_false*/);
    } else {
      return disjunctive_linear_constraint_system_t(lin_csts);
    }
  }

  std::string domain_name() const override { return "ConstantDomain"; }

}; // class constant_domain
} // namespace domains
} // namespace crab

namespace crab {
namespace domains {
template <typename Number, typename VariableName>
struct abstract_domain_traits<constant_domain<Number, VariableName>> {
  using number_t = Number;
  using varname_t = VariableName;
};

} // namespace domains
} // namespace crab


// Compile with:
// clang++ -o demoEllipsoids2D demoEllipsoids2D.cpp -L../build/ -I ../include/ -l diamonds -stdlib=libc++ -std=c++11 -Wno-deprecated-register
//

#include <ctime>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <unordered_set>
#include <Eigen/Core>
#include "File.h"
#include "EuclideanMetric.h"
#include "KmeansClusterer.h"
#include "Ellipsoid.h"
#include "PrincipalComponentProjector.h"

using namespace std;
using namespace Eigen;

int main()
{
    // ------ IDENTIFY CLUSTERS FROM INPUT SAMPLE ------
    // Open the input file and read the data (synthetic sampling of a 2D parameter space)

    ifstream inputFile;
    File::openInputFile(inputFile, "kmeans_testsample2D.txt");
    unsigned long Nrows;
    int Ncols;

    File::sniffFile(inputFile, Nrows, Ncols);
    ArrayXXd data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    ArrayXXd sample = data.transpose();
    inputFile.close();

    // Set up the clusterer using a Euclidean metric

    EuclideanMetric myMetric;
    int minNclusters = 2;
    int maxNclusters = 10;
    int Ntrials = 20;
    double relTolerance = 0.01;

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = false;

    KmeansClusterer clusterer(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // Do the clustering, and get for each point the index of the cluster it belongs to

    int optimalNclusters;
    vector<int> clusterIndices(Nrows);
    vector<int> clusterSizes;

    optimalNclusters = clusterer.cluster(sample, clusterIndices, clusterSizes);
    int Nclusters = optimalNclusters;

    // Output the results

    cerr << "Input number of clusters: 5" << endl;
    cerr << "Optimal number of clusters: " << optimalNclusters << endl;

    // ------ Compute Ellipsoids ------

    int Ndimensions = Ncols;
    assert(sample.cols() == clusterIndices.size());
    assert(sample.cols() >= Ndimensions + 1);            // At least Ndimensions + 1 points are required.

    // The enlargement fraction (it is the fraction by which each axis of an ellipsoid is enlarged)

    double enlargementFraction = 3.0;

    // Compute "sorted indices" such that clusterIndices[sortedindices[k]] <= clusterIndices[sortedIndices[k+1]]

    vector<int> sortedIndices = Functions::argsort(clusterIndices);

    // beginIndex will take values such that the indices for one particular cluster (# n) will be in
    // sortedIndex[beginIndex, ..., beginIndex + clusterSize[n] - 1]

    int beginIndex = 0;

    // Clear whatever was in the ellipsoids collection

    vector<Ellipsoid> ellipsoids;
    ellipsoids.clear();

    // Create an Ellipsoid for each cluster (provided it's large enough)

    for (int i = 0; i < Nclusters; i++)
    {
        // Skip cluster if number of points is not large enough

        if (clusterSizes[i] < Ndimensions + 1)
        {
            // Move the beginIndex up to the next cluster

            beginIndex += clusterSizes[i];

            // Continue with the next cluster

            continue;
        }
        else
        {
            // The cluster is indeed large enough to compute an Ellipsoid.

            // Copy those points that belong to the current cluster in a separate Array
            // This is because Ellipsoid needs a contiguous array of points.

            ArrayXXd sampleOfOneCluster(Ndimensions, clusterSizes[i]);

            for (int n = 0; n < clusterSizes[i]; ++n)
            {
                sampleOfOneCluster.col(n) = sample.col(sortedIndices[beginIndex+n]);
            }

            // Move the beginIndex up to the next cluster

            beginIndex += clusterSizes[i];

            // Add ellipsoid at the end of our vector

            ellipsoids.push_back(Ellipsoid(sampleOfOneCluster, enlargementFraction));
        }
    }

    int Nellipsoids = ellipsoids.size();
    cerr << "Nellispids: " << Nellipsoids << endl;

    // Find which ellipsoids are overlapping and which are not

    vector<unordered_set<int>> overlappingEllipsoidsIndices;

    // Remove whatever was in the container before

    overlappingEllipsoidsIndices.clear();

    // Make sure that the indices container has the right size

    overlappingEllipsoidsIndices.resize(ellipsoids.size());

    // If Ellipsoid i overlaps with ellipsoid j, than of course ellipsoid j also overlaps with i.
    // The indices are kept in an unordered_set<> which automatically takes care
    // that there are no duplicates.

    bool ellipsoidMatrixDecompositionIsSuccessful;

    for (int i = 0; i < Nellipsoids-1; ++i)
    {
        for (int j = i+1; j < Nellipsoids; ++j)
        {
            if (ellipsoids[i].overlapsWith(ellipsoids[j], ellipsoidMatrixDecompositionIsSuccessful))
            {
                overlappingEllipsoidsIndices[i].insert(j);
                overlappingEllipsoidsIndices[j].insert(i);
            }
        }
    }

    mt19937 engine;
    clock_t clockticks = clock();
    engine.seed(clockticks);
    uniform_real_distribution<> uniform(0.0, 1.0);

    // Get the hyper-volume for each of the ellipsoids and normalize it
    // to the sum of the hyper-volumes over all the ellipsoids

    vector<double> normalizedHyperVolumes(Nellipsoids);

    for (int n=0; n < Nellipsoids; ++n)
    {
        normalizedHyperVolumes[n] = ellipsoids[n].getHyperVolume();
    }

    double sumOfHyperVolumes = accumulate(normalizedHyperVolumes.begin(), normalizedHyperVolumes.end(), 0.0, plus<double>());

    ArrayXd centerCoordinate(2);

    cout << "Nrmalized Hyper-Volumes" << endl;
    for (int n = 0; n < Nellipsoids; ++n)
    {
        normalizedHyperVolumes[n] /= sumOfHyperVolumes;
        centerCoordinate = ellipsoids[n].getCenterCoordinates();
        cerr << "Ellipsoid #" << n << "   " << normalizedHyperVolumes[n] << endl;
        cerr << "Center Coordinates: " << centerCoordinate.transpose() << endl;
        cerr << endl;
    }

    // Pick an ellipsoid with a probability according to its normalized hyper-volume
    // First generate a uniform random number between 0 and 1

    double uniformNumber = uniform(engine);

    // Select the ellipsoid that makes the cumulative hyper-volume greater than this random
    // number. Those ellipsoids with a larger hyper-volume will have a greater probability to
    // be chosen.

    double cumulativeHyperVolume = normalizedHyperVolumes[0];
    int indexOfSelectedEllipsoid = 0;

    while (cumulativeHyperVolume < uniformNumber)
    {
        indexOfSelectedEllipsoid++;
        cumulativeHyperVolume += normalizedHyperVolumes[indexOfSelectedEllipsoid];
    }

    cerr << "Selected Ellipsoid #: " << indexOfSelectedEllipsoid << endl;
    centerCoordinate = ellipsoids[indexOfSelectedEllipsoid].getCenterCoordinates();
    cerr << "Center Coordinates: " << centerCoordinate.transpose() << endl;
    cerr << endl;

    // ------ Draw points from the Ellipsoid ------

    int Npoints = 10000;
    ArrayXXd sampleOfDrawnPoints(Npoints,Ndimensions);
    ArrayXd drawnPoint(Ndimensions);

    for (int i=0; i < Npoints; ++i)
    {
        bool newPointIsFound = false;

        while (newPointIsFound == false)
        {
            // Draw a new point inside the ellipsoid

            ellipsoids[indexOfSelectedEllipsoid].drawPoint(drawnPoint);

            // Check if the new point is also in other ellipsoids. If the point happens to be
            // in N overlapping ellipsoids, then accept it only with a probability 1/N. If we
            // wouldn't do this, the overlapping regions in the ellipsoids would be oversampled.

            if (!overlappingEllipsoidsIndices[indexOfSelectedEllipsoid].empty())
            {
                // There are overlaps, so count the number of ellipsoids to which the new
                // point belongs

                int NenclosingEllipsoids = 1;

                for (auto index = overlappingEllipsoidsIndices[indexOfSelectedEllipsoid].begin();
                          index != overlappingEllipsoidsIndices[indexOfSelectedEllipsoid].end();
                        ++index)
                {
                    if (ellipsoids[*index].containsPoint(drawnPoint))
                    {
                        NenclosingEllipsoids = static_cast<int>(DBL_MAX);
                        //NenclosingEllipsoids++;
                    }
                }

                // Only accept the new point with a probability = 1/NenclosingEllipsoids.
                // If it's not accepted, go immediately back to the beginning of the while loop,
                // and draw a new point inside the ellipsoid.

                uniformNumber = uniform(engine);
                newPointIsFound = (uniformNumber < 1./NenclosingEllipsoids);
            }
            else
            {
                // There are no ellipsoids overlapping with the selected one, so the point
                // is automatically accepted

                newPointIsFound = true;
            }
        }

        sampleOfDrawnPoints.row(i) = drawnPoint.transpose();
    }

    ofstream outputFile;
    File::openOutputFile(outputFile,"ellipsoidSample2D.txt");
    File::arrayXXdToFile(outputFile, sampleOfDrawnPoints);

    return EXIT_SUCCESS;
}


#pragma once

#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @return The distance between p0 and p1.
template <typename DerivedP0, typename DerivedP1>
inline auto point_point_distance(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1)
{
    return (p1 - p0).squaredNorm();
}

/// @brief Compute the gradient of the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @param[out] grad The computed gradient.
template <typename DerivedP0, typename DerivedP1, typename DerivedGrad>
inline void point_point_distance_gradient(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p0.size() == p1.size());
    grad.resize(p0.size() + p1.size());
    grad.head(p0.size()) = 2.0 * (p0 - p1);
    grad.tail(p1.size()) = -grad.head(p0.size());
}

/// @brief Compute the hessian of the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @param[out] hess The computed hessian.
template <typename DerivedP0, typename DerivedP1, typename DerivedHess>
inline void point_point_distance_hessian(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = p0.size();
    assert(p1.size() == dim);

    hess.resize(2 * dim, 2 * dim);

    hess.setZero();
    hess.diagonal().setConstant(2.0);
    for (int i = 0; i < dim; i++) {
        hess(i, i + dim) = hess(i + dim, i) = -2;
    }
}

} // namespace ipc


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <typeinfo>
#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace std;
    using mtl::lazy; using mtl::io::tout;
    typedef mtl::dense_vector<double> vt;

    vt v(60);
    iota(v);
    tout << "v is " << v << endl;

    mtl::mat::identity2D I(60);
    vt w1(I * v);
    tout << "I * v is " << w1 << endl;

    if (one_norm(vt(w1 - v)) > 0.001) throw "Wrong result with square identity";

    w1-= I * v;
    tout << "w1-= I * v is " << w1 << endl;
    if (one_norm(w1) > 0.001) throw "Wrong result";

    w1+= I * v;
    tout << "w1+= I * v is " << w1 << endl;
    if (one_norm(vt(w1 - v)) > 0.001) throw "Wrong result with square identity";

    vt w2( w1 - I * v );
    double alpha;
    (lazy(w2)= I * v) || (lazy(alpha)= lazy_dot(w2, v));

    vt w3(30), w4(90);
    mtl::mat::identity2D I3(30, 60), I4(90, 60);

    w3= I3 * v;
    tout << "I3 * v is " << w3 << endl;
    if (one_norm(vt(w3 - v[mtl::irange(30)])) > 0.001) throw "Wrong result with broad identity";

    w4= I4 * v;
    tout << "I4 * v is " << w4 << endl;
    if (one_norm(vt(w4[mtl::irange(60)] - v)) > 0.001) throw "Wrong result with long identity";
    if (one_norm(w4[mtl::irange(60, 90)]) > 0.001) throw "Wrong result with long identity";

#if 0
    mtl::dense2D<double> A(60,60);
    A=2;
    A+= A*I;
    std::cout<< "A=\n" << A << "\n";
#endif
    return 0;
}