#define BOOST_TEST_MODULE TS test
#include <boost/test/unit_test.hpp>
#include "Sum.h"
#include "Variable.h"
#include "System.h"

using namespace omnn::math;
using namespace boost::unit_test;

template<typename T>
T bits(T v){
    T bits = 0;
    while (v) {
        ++bits;
        v = v >> 1;
    }
    return bits;
}

BOOST_AUTO_TEST_CASE(TS_1d)
{
//    System s;
//    s.MakeTotalEqu(true);
    // y=f(x), where x is code of sequence
    Variable time, price;
    int points[] = {
        10,1,30,5
    };
    int sz = std::size(points);
    int encodeBits = bits(sz);
    int mask = (1 << encodeBits) - 1;

    auto sys = 0_v;

    // points math data
    Variable x;
    auto ExtractMove = [&](int i){
        auto targetId = x.And((i+1)*encodeBits,mask<<(i*encodeBits)).shr(i*encodeBits);
        return targetId;
    };
    // last move back to start
    auto statement = ExtractMove(sz-1).Equals(0);
//    s << statement;
    sys.logic_and(statement);
    // moves uniqueness
    for (int i=1; i<sz; ++i) {
        statement = ExtractMove(i).NotEquals(ExtractMove(i-1));
//        s << statement;
        sys.logic_and(statement);
    }
    // generate function of getting point value by its index
    auto ValueByIdx=[&](Valuable i){
        Variable id;
        static auto MathDataForm = [&](){
            auto data = 1_v;
            Variable val;
            for (int i=0; i<sz; ++i) {
                data.logic_or(id.Equals(i).LogicAnd(val.Equals(points[i])));
            }
            return data(val);
        };

        static auto data = MathDataForm();

        auto localData = data;
        localData.Eval(id, i);
        return localData;
    };
    // move len square
    auto MoveLenSq=[&](int i)
    {
        assert(i);
        auto prev = ExtractMove(i-1);
        auto target = ExtractMove(i);
        auto diff = ValueByIdx(prev)-ValueByIdx(target);
        return diff ^ 2;
    };
    auto SumSqLens = [&](){
        auto sum = 0_v;
        for (int i=1; i<sz; ++i)
            sum+=MoveLenSq(i);
        return sum;
    };
    auto sumSqLens=SumSqLens();
}

BOOST_AUTO_TEST_CASE(TS_2d
                     ,*disabled()
                     )
{
	Variable time, price;
    std::pair<Valuable,Valuable> points[] = {
		{0,0},
		{1,5},
		{3,8},
	};
	auto bits = 2;

	Variable xpath, ypath;
    Variable xmove[std::size(points)], ymove[std::size(points)];
//
//    auto unix = xpath.equals((xmove[0] << (2 * 2)) + (xmove[1] << 2) + xmove[2]);
//    auto uniy = ypath.equals((xmove[0] << (2 * 2)) + (xmove[1] << 2) + xmove[2]);

//    auto xext = unix(

IMPLEMENT
}


#include <iostream>
#include <stdio.h>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Core>

/*
 * A benchmark to test performance of calculating determinants of
 * 6x6 matrices with the Eigen linear algebra package.
 */

using namespace std;
using namespace Eigen;

int main(int argc, char * argv[])
{
  if (argc != 3 || strcmp(argv[1], "-h") == 0)
  {
    cout << "Usage: ./eigDeterm N_MATRIX(<=1000) ITERATIONS(~1000)"
      << endl;
    return 1;
  }

  srand(time(NULL));
  int MAT_DIM = 6;
  int N_MATRIX = atoi(argv[1]);
  int ITERATIONS = atoi(argv[2]);
  int i, j, k, count;
  double start, end, t_time = 0.0;

  printf("N_MATRIX: %d, ITERATIONS: %d\n", N_MATRIX, ITERATIONS);

  //Setting up array of matrices
  MatrixXd *m_list = new MatrixXd[N_MATRIX];

  //Initialise each matrix in m_list as random 6x6 matrices
  for (i=0; i<N_MATRIX; i++)
  {
    m_list[i] = MatrixXd::Random(MAT_DIM, MAT_DIM);
  }

  start = (double)clock() / (double) CLOCKS_PER_SEC;

  //Calculating determinants of matrices
  for(i=0; i<ITERATIONS; i++)
  {
    for(j=0; j<N_MATRIX; j++)
    {
    volatile double determinant;
    determinant = m_list[j].determinant();
    }
  }
  end = (double)clock() / (double) CLOCKS_PER_SEC;

  cout << "Total CPU time: " << end - start << endl;

  return 0;
}


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

    @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_FUNCTION_ASECD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ASECD_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse secant
    in degree: \f$(180/\pi) \arccos(1/x)\f$.

    @par Header <boost/simd/function/asecd.hpp>

    @see asec,  asecpi

    @par Example:

      @snippet asecd.cpp asecd

    @par Possible output:

      @snippet asecd.txt asecd

  **/
  IEEEValue asecd(IEEEValue const & x);
} }
#endif

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

#endif


#include "decomp.h"

// #include <Eigen/Eigenvalues>
#include <Eigen/SVD>

Cx5 LowRankKernels(Cx5 const &mIn, float const thresh)
{
  Index const kSz = mIn.dimension(0) * mIn.dimension(1) * mIn.dimension(2) * mIn.dimension(3);
  Index const nK = mIn.dimension(4);
  Eigen::Map<Eigen::MatrixXcf const> m(mIn.data(), kSz, nK);
  Log::Print(FMT_STRING("SVD Kernel Size {} Kernels {}"), kSz, nK);
  auto const svd = m.transpose().bdcSvd(Eigen::ComputeThinV);
  Eigen::ArrayXf const vals = svd.singularValues();
  Index const nRetain = (vals > (vals[0] * thresh)).cast<int>().sum();
  Log::Print(FMT_STRING("Retaining {} kernels"), nRetain);
  Cx5 out(mIn.dimension(0), mIn.dimension(1), mIn.dimension(2), mIn.dimension(3), nRetain);
  Eigen::Map<Eigen::MatrixXcf> lr(out.data(), kSz, nRetain);
  lr = svd.matrixV().leftCols(nRetain).conjugate();
  return out;
}

void PCA(Cx2 const &dataIn, Cx2 &vecIn, R1 &valIn)
{
  Eigen::Map<Eigen::MatrixXcf const> data(dataIn.data(), dataIn.dimension(0), dataIn.dimension(1));
  Eigen::Map<Eigen::MatrixXcf> vecs(vecIn.data(), vecIn.dimension(0), vecIn.dimension(1));
  Eigen::Map<Eigen::VectorXf> vals(valIn.data(), valIn.dimension(0));
  assert(vecs.rows() == data.rows());
  assert(vecs.cols() == data.rows());
  assert(vals.rows() == data.rows());
  auto const svd = data.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXcf const V = svd.matrixV();
  vecs = V;
  vals = svd.singularValues().array().sqrt();
}


#pragma once

/* Type lattice */

#include <crab/domains/lattice_domain.hpp>
#include <crab/support/os.hpp>
#include <crab/types/variable.hpp>

#include <boost/optional.hpp>
#include <string>

namespace crab {
namespace domains {
/**
 *  Lattice for variable types which is almost flat except regions
 *  that can be unknown. An unknown region unifies with any other
 *  region.
 *
 *  Num    = bool | int | real
 *  Scalar = Num | reference
 *
 *                 -------- top
 *                /         /   \
 *               /         /    region(unknown)--------
 *              /         /           |                |
 *            Scalar array(Num)    region(Scalar) region(array(Num))
 *               |          \      /                   |
 *               ---------- bottom----------------------
 *
 **/
class type_value: public lattice_domain_api<type_value> {

  boost::optional<variable_type> m_type;
  bool m_is_bottom;

  type_value(bool is_bottom);

public:

  type_value();

  type_value(variable_type ty);

  static type_value bottom();

  static type_value top();

  type_value make_top() const override;

  type_value make_bottom() const override;

  void set_to_top() override;

  void set_to_bottom() override;

  bool is_bottom() const override;

  bool is_top() const override;

  bool operator<=(const type_value &o) const override;

  bool operator==(const type_value &o) const;

  void operator|=(const type_value &o) override;

  type_value operator|(const type_value &o) const override;

  type_value operator||(const type_value &o) const override;

  template <typename Thresholds>
  type_value widening_thresholds(const type_value &o,
				 const Thresholds &ts /*unused*/) const;

  type_value operator&(const type_value &o) const override;

  type_value operator&&(const type_value &o) const override;

  variable_type get() const;

  void set(variable_type ty);

  void write(crab::crab_os &o) const override;

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

  std::string domain_name() const override;

};
} // namespace domains
} // namespace crab


#include <chrono>
#include <iomanip>
#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Dense>

using Evec = Eigen::VectorXd;
using Emat = Eigen::MatrixXd;
using EVec3 = Eigen::Vector3d;

// Assume A=(m,n), m>n
// U = (m,n), S = (n,n), VT = (n,n)
void testSVD(const Emat &U, const Evec &Sdiag, const Emat &VT, const Emat &A, const Evec &x, const Evec &b) {
    Emat S(U.cols(), VT.rows());
    S = Sdiag.asDiagonal();

    // step 1, test if USVT==A
    Emat Arecon = U * (S * VT);
    Emat Aerror = Arecon - A;
    printf("Aerror max min %g, %g\n", Aerror.maxCoeff(), Aerror.minCoeff());

    const double eps = std::numeric_limits<double>::epsilon();
    // step 2, test backward error
    Evec Sdiaginv = Sdiag;
    for (int i = 0; i < Sdiaginv.size(); i++) {
        Sdiaginv[i] = Sdiaginv[i] < Sdiag[0] * eps ? 0 : 1.0 / Sdiaginv[i];
    }

    Emat V = VT.transpose();
    for (int i = 0; i < Sdiaginv.size(); i++) {
        V.col(i) *= Sdiaginv[i];
    }

    Evec x2 = V * (U.transpose() * b);
    Evec b2 = A * x2;
    Evec xerror = x2 - x;
    Evec berror = b2 - b;
    printf("xerror max min %g, %g\n", xerror.maxCoeff(), xerror.minCoeff());
    printf("berror max min %g, %g\n", berror.maxCoeff(), berror.minCoeff());
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

inline double pot(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < 1e-12 ? 0 : 1 / rnorm;
}

int main(int argc, char **argv) {
    Eigen::initParallel();

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = 2 * atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // Aup for solving MEquiv
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointMCheck.size() / 3;
    Eigen::MatrixXd Aup(checkN, equivN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            Aup(k, l) = pot(Cpoint, Lpoint);
        }
    }

    Evec x(Aup.cols());
    x.setRandom();
    Evec b = Aup * x;

    // jacobi svd
    using std::cout;
    using std::endl;

    {
        cout << "JacobiSVD" << endl;
        Eigen::JacobiSVD<Emat> svd(Aup, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Emat U = svd.matrixU();
        Emat VT = svd.matrixV().transpose();
        Evec Svec = svd.singularValues();
        testSVD(U, Svec, VT, Aup, x, b);
    }
    {
        cout << "BDCSVD" << endl;
        Eigen::BDCSVD<Emat> svd(Aup, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Emat U = svd.matrixU();
        Emat VT = svd.matrixV().transpose();
        Evec Svec = svd.singularValues();
        testSVD(U, Svec, VT, Aup, x, b);
    }
    {
        cout << "HouseholderQR" << endl;
        Evec x2 = Aup.colPivHouseholderQr().solve(b);
        Evec b2 = Aup * x2;
        Evec xerror = x2 - x;
        Evec berror = b2 - b;
        printf("xerror max min %g, %g\n", xerror.maxCoeff(), xerror.minCoeff());
        printf("berror max min %g, %g\n", berror.maxCoeff(), berror.minCoeff());
    }

    return 0;
}


// Copyright 2015-2018 Hans Dembinski
//
// 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)

//[ guide_histogram_projection

#include <boost/histogram.hpp>
#include <cassert>
#include <iostream>
#include <sstream>

int main() {
  using namespace boost::histogram;
  using namespace literals; // enables _c suffix

  // make a 2d histogram
  auto h = make_histogram(axis::regular<>(3, -1.0, 1.0), axis::integer<>(0, 2));

  h(-0.9, 0);
  h(0.9, 1);
  h(0.1, 0);

  auto hr0 = algorithm::project(h, 0_c); // keep only first axis
  auto hr1 = algorithm::project(h, 1_c); // keep only second axis

  // reduce does not remove counts; returned histograms are summed over
  // the removed axes, so h, hr0, and hr1 have same number of total counts;
  // we compute the sum of counts with the sum algorithm
  assert(algorithm::sum(h) == 3 && algorithm::sum(hr0) == 3 && algorithm::sum(hr1) == 3);

  std::ostringstream os1;
  for (auto x : indexed(h))
    os1 << "(" << x.index(0) << ", " << x.index(1) << "): " << *x << "\n";
  std::cout << os1.str() << std::flush;
  assert(os1.str() == "(0, 0): 1\n"
                      "(1, 0): 1\n"
                      "(2, 0): 0\n"
                      "(0, 1): 0\n"
                      "(1, 1): 0\n"
                      "(2, 1): 1\n");

  std::ostringstream os2;
  for (auto x : indexed(hr0)) os2 << "(" << x.index(0) << ", -): " << *x << "\n";
  std::cout << os2.str() << std::flush;
  assert(os2.str() == "(0, -): 1\n"
                      "(1, -): 1\n"
                      "(2, -): 1\n");

  std::ostringstream os3;
  for (auto x : indexed(hr1)) os3 << "(- ," << x.index(0) << "): " << *x << "\n";
  std::cout << os3.str() << std::flush;
  assert(os3.str() == "(- ,0): 2\n"
                      "(- ,1): 1\n");
}

//]


#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Alpha_shape_face_base_2.h>
#include <CGAL/Alpha_shape_vertex_base_2.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif
#include <CGAL/point_generators_2.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/AlphaShapeGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>

// the two base classes
#include "ui_Alpha_shapes_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

typedef CGAL::Alpha_shape_vertex_base_2<K> Vb;
typedef CGAL::Alpha_shape_face_base_2<K>  Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;

typedef CGAL::Alpha_shape_2<Delaunay> Alpha_shape_2;

typedef Alpha_shape_2::Alpha_iterator Alpha_iterator;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Alpha_shapes_2
{
  Q_OBJECT

private:
  double alpha;
  std::vector<Point_2> points;
  Alpha_shape_2 as;
  QGraphicsScene scene;

  CGAL::Qt::AlphaShapeGraphicsItem<Alpha_shape_2> * agi;
  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

public:
  MainWindow();

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void alphaChanged(int i);

  void on_actionInsertRandomPoints_triggered();

  void on_actionLoadPoints_triggered();

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  void open(QString fileName);

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  // Add a GraphicItem for the alpha shape
  agi = new CGAL::Qt::AlphaShapeGraphicsItem<Alpha_shape_2>(&as);

  QObject::connect(this, SIGNAL(changed()),
                   agi, SLOT(modelChanged()));

  agi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setEdgesPen(QPen(Qt::lightGray, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setRegularEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setSingularEdgesPen(QPen(Qt::cyan, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setRegularFacesBrush(QBrush(Qt::cyan));
  scene.addItem(agi);

  //
  // Manual handling of actions
  //

  QObject::connect(this->alphaSlider, SIGNAL(valueChanged(int)),
                   this, SLOT(alphaChanged(int)));

  QObject::connect(this->alphaBox, SIGNAL(valueChanged(int)),
                   this, SLOT(alphaChanged(int)));

  QObject::connect(this->alphaSlider, SIGNAL(valueChanged(int)),
                   this->alphaBox, SLOT(setValue(int)));

  QObject::connect(this->alphaBox, SIGNAL(valueChanged(int)),
                   this->alphaSlider, SLOT(setValue(int)));

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 1, false); // inputs a list with one point
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  scene.installEventFilter(pi);
  //this->actionShowAlphaShape->setChecked(true);

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->transform().scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Alpha_shapes_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void
MainWindow::processInput(CGAL::Object o)
{
  std::list<Point_2> input;
  if(CGAL::assign(input, o)){
    if(input.size() == 1) {
      points.push_back(input.front());
      as.make_alpha_shape(points.begin(), points.end());
      as.set_alpha(alpha);
    }
    Q_EMIT( changed());
  }
}

void MainWindow::alphaChanged(int i)
{
  if (as.number_of_alphas() > 0){
    if(i < 100){
      int n = static_cast<int>((i * as.number_of_alphas())/ 100);
      if(n == 0) n++;
      alpha = as.get_nth_alpha(n);
      as.set_alpha(alpha);
    } else {
      Alpha_iterator alpha_end_it = as.alpha_end();
      alpha = (*(--alpha_end_it))+1;
      as.set_alpha(alpha);
    }
  } else {
    alpha = 0;
    as.set_alpha(0);
  }
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  as.clear();
  points.clear();
  Q_EMIT( changed());
}

void
MainWindow::on_actionInsertRandomPoints_triggered()
{
  QRectF rect = CGAL::Qt::viewportsBbox(&scene);
  CGAL::Qt::Converter<K> convert;
  Iso_rectangle_2 isor = convert(rect);
  CGAL::Random_points_in_iso_rectangle_2<Point_2> pg((isor.min)(), (isor.max)());
  bool ok = false;

  const int number_of_points =
    QInputDialog::getInt(this,
                             tr("Number of random points"),
                             tr("Enter number of random points"),
                             100,
                             0,
                             (std::numeric_limits<int>::max)(),
                             1,
                             &ok);

  if(!ok) {
    return;
  }

  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);

  points.reserve(points.size() + number_of_points);
  for(int i = 0; i < number_of_points; ++i){
    points.push_back(*pg++);
  }
  as.make_alpha_shape(points.begin(), points.end());
  as.set_alpha(alpha);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.wktk *.WKT);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  std::cerr << "open " << std::endl;
  std::cerr << qPrintable(fileName) << std::endl;
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));
  if(fileName.endsWith(".wkt",Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    CGAL::IO::read_multi_point_WKT(ifs, points);
#endif
  }
  else
  {
    K::Point_2 p;
    while(ifs >> p) {
      points.push_back(p);
    }
  }
  as.make_alpha_shape(points.begin(), points.end());
  as.set_alpha(alpha);
  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  actionRecenter->trigger();
  Q_EMIT( changed());

}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(agi->boundingRect());
  this->graphicsView->fitInView(agi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Alpha_shapes_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Alpha_shape_2 demo");

  // Import resources from libCGAL (Qt5).
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


#ifndef JOINT_HPP
#define JOINT_HPP
#include <armadillo>
#include "Math.hpp"
#include "Body.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>

class Joint : public boost::enable_shared_from_this<Joint>
{
public:
    Joint(unsigned int TypeIn, arma::vec piIn, arma::vec pjIn, arma::vec qiIn,
            arma::vec qjIn, BodyPtr i_In, BodyPtr j_In);
    ~Joint() {};
    void Build_C();
    void Build_Cq();
    void Build_GAMMA();
    void update();

    arma::mat get_Cqi();
    arma::mat get_Cqj();
    arma::vec get_GAMMA();
    arma::vec get_Pi();
    arma::vec get_Pj();
    arma::vec get_pi();
    arma::vec get_pj();
    arma::vec get_CONSTRAINT();
    BodyPtr get_body_i_ptr();
    BodyPtr get_body_j_ptr();

private:
    unsigned int Type;
    arma::vec pi;
    arma::vec pj;
    arma::vec qi;
    arma::vec qj;
    arma::mat Cqi;
    arma::mat Cqj;
    arma::vec GAMMA;
    arma::vec CONSTRAINT;
    arma::mat TBI_i;
    arma::mat TBI_j;
    arma::vec Pi;
    arma::vec Pj;
    arma::vec Qi;
    arma::vec Qj;
    arma::vec wi;
    arma::vec wj;
    arma::vec Si;
    arma::vec Sj;
    BodyPtr body_i_ptr;
    BodyPtr body_j_ptr;
};

typedef boost::shared_ptr<Joint> JointPtr;
#endif  //JOINT_HPP


#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "ceinms2/Types.h"
#include "ceinms2/NMSmodel.h"
#include "ceinms2/ElectromechanicalDelay.h"
#include "ceinms2/ExponentialActivation.h"

using namespace std;
using namespace Eigen;
using namespace ceinms;

int testMatrix() {
    MatrixXd m(2, 2);
    m(0, 0) = 3;
    m(1, 0) = 2.5;
    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    std::cout << "Here is the matrix m:\n" << m << std::endl;
    VectorXd v(2);
    v(0) = 4;
    v(1) = v(0) - 1;
    std::cout << "Here is the vector v:\n" << v << std::endl;
    return 0;
}

int main() {
    size_t N = 10, M = 10;
    using EMGMapping =  ceinms::MultiInputMultiOutput<Excitation, Excitation>;
    EMGMapping emgGenerator(N, M);

    auto f{ [N,M](const vector<Excitation> &in) {
        MatrixXd m{ MatrixXd::Random(M, N) };
        VectorXd v(N);
        for (int i(0); i < N; ++i) {
            v[i] = in[i];
        }
        MatrixXd x = m * v;
        std::vector<Excitation> out(x.data(), x.data() + x.size());
        return out;
    }};
    emgGenerator.setName("emgGenerator");
    emgGenerator.setFunction(f);
    emgGenerator.setInput(vector<Excitation>(N, 1.));
    emgGenerator.evaluate(0.01);
    for (auto &e : emgGenerator.getOutput())
        cout << e << endl;

    ElectromechanicalDelay delay({ 0.05 });
    delay.setName("mtu1");
    ExponentialActivation act;
    act.setName("mtu1");
    NMSmodel<EMGMapping, ElectromechanicalDelay, ExponentialActivation> model;
    model.addComponent(emgGenerator);
    model.addComponent(delay);
    model.addComponent(act);
    model.connect<EMGMapping, ElectromechanicalDelay>({ "emgGenerator", 0 }, "mtu1");
    model.connect<ElectromechanicalDelay, ExponentialActivation>();

    return 0;

}


#include <frovedis.hpp>
#include <frovedis/ml/glm/logistic_regression_with_sgd.hpp>
#include <frovedis/ml/glm/svm_with_sgd.hpp>
#include <boost/lexical_cast.hpp>

int main(int argc, char* argv[]){
  frovedis::use_frovedis use(argc, argv);

  auto samples = frovedis::make_crs_matrix_load<double>("./train.mat");
  auto label = frovedis::make_dvector_loadline("./train.label").
    map(+[](const std::string& s){return boost::lexical_cast<double>(s);});

  int num_iteration = 100;
  double alpha = 1.0;
  double minibatch_fraction = 1.0;
  frovedis::RegType rt = frovedis::ZERO;
  double regParam = 0.01;
  bool intercept = false;
  auto model = frovedis::logistic_regression_with_sgd::
    train(samples, label, num_iteration, alpha, minibatch_fraction, regParam,
          rt, intercept);

  /*
  double convTol = 0.001;
  frovedis::MatType mType = frovedis::CRS;
  auto model = frovedis::svm_with_sgd::
  train(samples, label, num_iteration, alpha, minibatch_fraction, regParam,
        rt, intercept, convTol, mType);
  */

  model.save("./model");

  frovedis::logistic_regression_model<double> lm;
  //frovedis::svm_model<double> lm;
  lm.load("./model");
  auto test = frovedis::make_crs_matrix_local_load<double>("./test.mat");
  auto result = lm.predict(test);
  for(auto i: result) std::cout << i << std::endl;
}


#include <stan/math/prim/mat.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>

TEST(MathMatrixPrimMat, chol2inv_exception) {
  using stan::math::chol2inv;

  stan::math::matrix_d m1(2, 3);

  // non-square
  m1 << 1, 2, 3, 4, 5, 6;
  EXPECT_THROW(chol2inv(m1), std::invalid_argument);

  stan::math::matrix_d m2(3, 3);

  // non-lower-triangular
  m2 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  EXPECT_THROW(chol2inv(m2), std::domain_error);
}

TEST(MathMatrixPrimMat, chol2inv) {
  using stan::math::chol2inv;
  using stan::math::cholesky_decompose;
  using stan::math::inverse_spd;
  using stan::math::matrix_d;
  using stan::math::wishart_rng;

  boost::random::mt19937 rng;
  matrix_d I(3, 3);
  I.setZero();
  I.diagonal().setOnes();
  matrix_d Y = wishart_rng(4.0, I, rng);
  matrix_d L = cholesky_decompose(Y);
  matrix_d Y_inv = inverse_spd(Y);
  matrix_d Y_inv2 = chol2inv(L);
  for (int j = 0; j < Y.cols(); j++)
    for (int i = 0; i < Y.rows(); i++)
      EXPECT_FLOAT_EQ(Y_inv(i, j), Y_inv2(i, j));
}

TEST(MathMatrixPrimMat, chol2inv01) {
  using stan::math::chol2inv;
  using stan::math::matrix_d;

  matrix_d Y(0, 0);
  matrix_d Y_inv2 = chol2inv(Y);
  EXPECT_EQ(0, Y_inv2.rows());
  EXPECT_EQ(0, Y_inv2.cols());

  matrix_d L(1, 1);
  L(0, 0) = 3.0;
  matrix_d inv2 = chol2inv(L);
  EXPECT_FLOAT_EQ(1 / 9.0, inv2(0, 0));
}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#ifndef SP_UTIL_RAND_HPP
#define	SP_UTIL_RAND_HPP

#include <random>
#include <memory>
#include <limits>
#include <set>
#include <boost/random.hpp>

#include "sp/config.hpp"
#include "hints.hpp"

SP_UTIL_NAMESPACE_BEGIN

/**
 * \brief Random utility
 *
 * Encapsulates common usage cases for uniform real distributions of float, double type,
 * and any integral type.
 */
template<typename Random_engine_type = boost::random::mt19937_64> //mt19937, consider default_random_engine
struct rand_util {

    using random_engine_type = Random_engine_type;
    using result_type = typename random_engine_type::result_type;

public:

    rand_util(uint seed = 0) : rng(), distFl(0, 1), distDl(0,1) {
        if (seed > 0) {
            rng.seed(seed);
        } else {
            rng.seed(std::random_device{}());
        }
    }

    sp_hot float rand_float() {
        return distFl(rng);
    }

    sp_hot double rand_double() {
        return distDl(rng);
    }

    /**
     * Generate a random integral in the range of [from, to)
     * \param from
     * \param to
     * \return
     */
    template<typename T>
    sp_hot T rand_integral(T from, T to) {
        if(std::numeric_limits<T>::digits10 > std::numeric_limits<float>::digits10) {
            return (to - from) * distDl(rng) + from;
        } else {
            return (to - from) * distFl(rng) + from;
        }
    }
        /**
     * Creates a completely randomized integer from
     * \return
     */
    sp_hot typename random_engine_type::result_type rand_integral() {
        return this->rng();
    }

    /**
     * Return the random engine being used
     */
    sp_hot random_engine_type& eng() {
        return this->rng;
    }

private:
    random_engine_type rng;
    boost::random::uniform_real_distribution<float> distFl;
    boost::random::uniform_real_distribution<double> distDl;
};

typedef std::shared_ptr<rand_util<>> rand_util_ptr;

inline rand_util_ptr new_rand() {
    return std::make_shared<rand_util<>>();
}

/**
 * Generated N unique numbers in the range [from, to)
 *
 * \param n
 * \param from
 * \param to
 * \return set of n unique numbers
 */
template<size_t N, typename result_type, typename Rand_util_type>
sp_hot std::array<result_type, N> n_unique(Rand_util_type& util, result_type from, result_type to) {
    std::set<result_type> s;
    //select n points
    for(result_type i = 0; i < N; ++i) {
        result_type rand_idx;
        do {
            rand_idx = util.rand_integral(from, to);
        } while(s.find(rand_idx) != s.end());
        s.insert(rand_idx);
    }
    std::array<result_type, N> arr;
    std::copy(s.begin(), s.end(), arr.begin());
    return arr;
}

SP_UTIL_NAMESPACE_END

#endif /* SP_UTIL_RAND_HPP */


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

#include <iostream>
#include <ctime>
#include <cassert>
#include <vector>
#include <algorithm>
#include <math.h>

double condRankP(unsigned int r, unsigned int c, double p, unsigned int q);
double nCkF(unsigned int n, unsigned int k);


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

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <smooth/so3.hpp>

// #include "smooth/compat/autodiff.hpp"
#include "smooth/feedback/ocp.hpp"

template<typename T>
using Vec = Eigen::VectorX<T>;

TEST(Ocp, Jacobians)
{
  // objective
  auto theta = []<typename T>(T tf, Vec<T> x0, Vec<T> xf, Vec<T> q) -> T {
    return (tf - 2) * (tf - 2) + x0.squaredNorm() + xf.squaredNorm() + q.sum();
  };

  // dynamics
  auto f = []<typename T>(T t, Vec<T> x, Vec<T> u) -> Vec<T> { return Vec<T>{{x.y() + t, u.x()}}; };

  // integrals
  auto g = []<typename T>(T t, Vec<T> x, Vec<T> u) -> Vec<T> {
    return Vec<T>{{t + x.squaredNorm() + u.squaredNorm()}};
  };

  // running constraint
  auto cr = []<typename T>(T t, Vec<T> x, Vec<T> u) -> Vec<T> {
    Vec<T> ret(4);
    ret << t, x, u;
    return ret;
  };

  // end constraint
  auto ce = []<typename T>(T tf, Vec<T> x0, Vec<T> xf, Vec<T> q) -> Vec<T> {
    Vec<T> ret(6);
    ret << tf, x0, xf, q;
    return ret;
  };

  const smooth::feedback::
    FlatOCP<decltype(theta), decltype(f), decltype(g), decltype(cr), decltype(ce)>
      ocp{
        .nx    = 2,
        .nu    = 1,
        .nq    = 1,
        .ncr   = 4,
        .nce   = 6,
        .theta = theta,
        .f     = f,
        .g     = g,
        .cr    = cr,
        .crl   = Vec<double>::Constant(4, -1),
        .cru   = Vec<double>::Constant(4, 1),
        .ce    = ce,
        .cel   = Vec<double>::Constant(6, -1),
        .ceu   = Vec<double>::Constant(6, 1),
      };

  ASSERT_TRUE(smooth::feedback::check_ocp(ocp));

  smooth::feedback::Mesh<5, 5> mesh;
  mesh.refine_ph(0, 8 * 5);

  auto nlp = smooth::feedback::ocp_to_nlp(ocp, mesh);

  Eigen::VectorXd x = Eigen::VectorXd::Constant(nlp.n, 1);

  const auto df_dx = nlp.df_dx(x);
  const auto dg_dx = nlp.dg_dx(x);

  const auto [fval, df_dx_num] =
    smooth::diff::dr<1, smooth::diff::Type::Numerical>(nlp.f, smooth::wrt(x));
  const auto [gval, dg_dx_num] =
    smooth::diff::dr<1, smooth::diff::Type::Numerical>(nlp.g, smooth::wrt(x));

  ASSERT_TRUE(Eigen::MatrixXd(df_dx).isApprox(df_dx_num, 1e-8));
  ASSERT_TRUE(Eigen::MatrixXd(dg_dx).isApprox(dg_dx_num, 1e-8));
}

TEST(OCP, Flatten)
{
  // objective
  auto theta = []<typename T>(T tf, smooth::SO3<T>, smooth::SO3<T>, Vec<T> q) -> T {
    return (tf - 2) * (tf - 2) + q.sum();
  };

  // dynamics
  auto f = []<typename T>(T, smooth::SO3<T> x, Eigen::Vector2<T> u) -> Vec<T> {
    return Vec<T>{{u.x(), -u.y(), 0.01 * x.log().x()}};
  };

  // integrals
  auto g = []<typename T>(T t, smooth::SO3<T> x, Eigen::Vector2<T> u) -> Vec<T> {
    return Vec<T>{{t + x.log().squaredNorm() + u.squaredNorm()}};
  };

  // running constraint
  auto cr = []<typename T>(T t, smooth::SO3<T>, Eigen::Vector2<T> u) -> Vec<T> {
    Vec<T> ret(3);
    ret << t, u;
    return ret;
  };

  // end constraint
  auto ce = []<typename T>(T tf, smooth::SO3<T> x0, smooth::SO3<T> xf, Vec<T>) -> Vec<T> {
    Vec<T> ret(7);
    ret << tf, x0.log(), xf.log();
    return ret;
  };

  const smooth::feedback::OCP<
    smooth::SO3d,
    Eigen::Vector2d,
    decltype(theta),
    decltype(f),
    decltype(g),
    decltype(cr),
    decltype(ce)>
    ocp{
      .nx    = 3,
      .nu    = 2,
      .nq    = 1,
      .ncr   = 3,
      .nce   = 7,
      .theta = theta,
      .f     = f,
      .g     = g,
      .cr    = cr,
      .crl   = Vec<double>::Constant(3, -1),
      .cru   = Vec<double>::Constant(3, 1),
      .ce    = ce,
      .cel   = Vec<double>::Constant(7, -1),
      .ceu   = Vec<double>::Constant(7, 1),
    };

  const auto xl_fun = []<typename T>(T) -> smooth::SO3<T> { return smooth::SO3<T>::Identity(); };

  const auto ul_fun = []<typename T>(T) -> Eigen::Vector2<T> { return Eigen::Vector2<T>::Zero(); };

  ASSERT_TRUE(smooth::feedback::check_ocp(ocp));

  const auto flat_ocp = smooth::feedback::flatten_ocp(ocp, xl_fun, ul_fun);

  ASSERT_TRUE(smooth::feedback::check_ocp(flat_ocp));
}


#include <iostream>
#include <cstdlib>
#include <cmath>
#include <string>
#include "spida/shape/shapeT.h"
#include "spida/helper/constants.h"
#include <boost/math/special_functions/airy.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace spida{

ShapeT::ShapeT(const GridT& grid,double A,double tp) :
            Shape(grid),
            m_t(grid.getT()),
            m_A(A),m_tp(tp),
            m_offset(0.0),
            m_chirp(0.0),
            m_slow_phase(0.0),
            m_omega0(0.0) {}

std::vector<dcmplx> ShapeT::shapeCV() const
{
    std::vector<dcmplx> v;
    shapeCV(v);
    return v;
}

std::vector<double> ShapeT::shapeRV() const
{
    std::vector<double> v;
    shapeRV(v);
    return v;
}

std::vector<dcmplx> ShapeT::envelope() const
{
    std::vector<dcmplx> v;
    envelope(v);
    return v;
}

void ShapeT::shapeCV(std::vector<dcmplx>& v) const
{
    v.clear();
    v.resize(m_t.size());
    for(auto i = 0; i < m_t.size(); i++)
        v[i] = shapeCV(m_t[i]);
}

void ShapeT::shapeRV(std::vector<double>& v) const
{
    v.clear();
    v.resize(m_t.size());
    for(auto i = 0; i < m_t.size(); i++)
        v[i] = shapeRV(m_t[i]);
}

void ShapeT::envelope(std::vector<dcmplx>& v) const
{
    v.clear();
    v.resize(m_t.size());
    for(auto i = 0; i < m_t.size(); i++)
        v[i] = computeEnvelope(m_t[i]);
}

dcmplx ShapeT::slowPhaseFactor(double t) const {
    return exp(-ii*m_chirp*pow((t-m_offset),2)+ii*m_slow_phase);
}

dcmplx ShapeT::fastPhaseFactor(double t) const {
    return exp(-ii*m_omega0*(t-m_offset));
}

double GaussT::compute(double t) const
{
    return exp(-pow((t-ShapeT::offset())/ShapeT::width(),2));
}

double SechT::compute(double t) const
{
    return (1.0/cosh((t-ShapeT::offset()\
                        )/ShapeT::width()));
}

double SuperGaussT::compute(double t) const {
    return exp(-pow((t-ShapeT::offset())/ShapeT::width(),2*m_M));
}

double AiryT::compute(double t) const
{
	double airy = boost::math::airy_ai<double>((t-ShapeT::offset())/ShapeT::width());
    double apodization = exp(-pow(m_apod*(t-ShapeT::offset())/ShapeT::width(),2));
    return airy*apodization;
}

BesselT::BesselT(const GridT& grid,double A,double tp,double apod) :
    ShapeT(grid,A,tp),
    m_apod(apod),
    m_j1(boost::math::cyl_bessel_j_zero<double>(0,1)) {}

double  BesselT::compute(double t) const
{
    double bessel = boost::math::cyl_bessel_j<double>(0,m_j1*fabs(t-ShapeT::offset())/ShapeT::width());
    double apodization = exp(-pow(m_apod*(t-ShapeT::offset())/ShapeT::width(),2));
    return bessel*apodization;
}

}


//
// Created by yu on 13.05.20.
//

#ifndef HW4_FASTEIGEN_HPP
#define HW4_FASTEIGEN_HPP
#include <Eigen/Eigenvalues>
Eigen::Vector3d ComputeEigenvector0(const Eigen::Matrix3d &A, double eval0);
Eigen::Vector3d ComputeEigenvector1(const Eigen::Matrix3d &A,
                                    const Eigen::Vector3d &evec0,
                                    double eval1);
Eigen::Vector3d FastEigen3x3(Eigen::Matrix3d &A);
#endif //HW4_FASTEIGEN_HPP


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::quaternion_type    Q;
typedef MT::real_type          T;

class ContactInfo
{
public:

  V m_point;
  V m_normal;
  T m_distance;

};

class MyCallback
  : public geometry::ContactsCallback<V>
{
public:

  std::vector<ContactInfo> m_contacts;

public:

  void operator()(
                  V const & point
                  , V const & normal
                  , typename V::real_type const & distance
                  )
  {
    ContactInfo info;

    info.m_point = point;
    info.m_normal = normal;
    info.m_distance = distance;

    m_contacts.push_back(info);
  }

};

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(contacts_sphere_tetrahedron_test)
{
  // Separation in vertex regions
  {
    V const center = V::make(2.1, 0.0, 0.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make(0.0, 2.1, 0.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make(0.0, 0.0, 2.1);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make(-1.0, -1.0, -1.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  // Separation in edge regions
  {
    V const center = V::make(0.5, -1.0, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make(-1.0, 0.5, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make(-1.0, -1.0, 0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make( 1.0, 1.0, -1.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make( 1.0, -1.0, 1.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make(-1.0, 1.0, 1.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  // Separation in face regions
  {
    V const center = V::make( 0.2, 0.2, -2.1);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make( 0.2, -2.1,  0.2);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make( -2.1, 0.2,  0.2);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  {
    V const center = V::make( 2.0, 2.0, 2.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(!test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 0u);
  }
  // Contacts in vertex regions
  {
    V const center = V::make(1.9, 0.0, 0.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(0),-1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.1, 0.01);
  }
  {
    V const center = V::make(1.9, 0.0, 0.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, true);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.1, 0.01);
  }

  {
    V const center = V::make(0.0, 1.9, 0.0);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make(0.0, 0.0, 1.9);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make(-0.5, -0.5, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  // Contact in edge regions
  {
    V const center = V::make(0.5, -0.5, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(0), 0.5, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(1), 0.707106769, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(2), 0.707106769, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.292893231, 0.01);

  }
  {
    V const center = V::make(0.5, -0.5, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, true);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(0), 0.5, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(1), -0.707106769, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(2), -0.707106769, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.292893231, 0.01);

  }
  {
    V const center = V::make(-0.5, 0.5, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make(-0.5, -0.5, 0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron,  callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make( 0.5, 0.5, -0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make( 0.5, -0.5, 0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make(-0.5, 0.5, 0.5);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  // Contact in face regions
  {
    V const center = V::make( 0.2, 0.2, -0.9);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(0), 0.2, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(1), 0.2, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(2), 1.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.1, 0.01);
  }
  {
    V const center = V::make( 0.2, 0.2, -0.9);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, true);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(0), 0.2, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(1), 0.2, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal(2),-1.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.1, 0.01);
  }
  {
    V const center = V::make( 0.2, -0.9,  0.2);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make( -0.9, 0.2,  0.2);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }
  {
    V const center = V::make( 0.7, 0.7, 0.7);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }

  // sphere inside
  {
    V const center = V::make( 0.3, 0.2, 0.1);
    T const radius = 1.0;

    V const p0 = V::make(0.0, 0.0, 0.0);
    V const p1 = V::make(1.0, 0.0, 0.0);
    V const p2 = V::make(0.0, 1.0, 0.0);
    V const p3 = V::make(0.0, 0.0, 1.0);

    geometry::Sphere<V>      const sphere      = geometry::make_sphere(center, radius);
    geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0, p1, p2 ,p3);

    MyCallback callback;

    bool const test = geometry::contacts_sphere_tetrahedron(sphere, tetrahedron, callback, false);

    BOOST_CHECK(test);

    BOOST_CHECK_EQUAL(callback.m_contacts.size(), 1u);
  }

}

BOOST_AUTO_TEST_SUITE_END();


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#include <glog/logging.h>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/math/special_functions/erf.hpp> // for erf/erfc.
#include "gtest/gtest.h"
#include "internal/approximations.hpp"

using namespace util_probability;

TEST(UtilNumeric, CDF) {

    double stddev = 1.0;
    double mean = 0.0;
    double sigma = 1.0;

    boost::math::normal norm(mean, stddev);

    double sigma_1_region = boost::math::cdf(norm, sigma) - boost::math::cdf(norm, -sigma);
    LOG_ASSERT(std::abs(sigma_1_region - 0.68) < 0.01);
    LOG_ASSERT(0.5 * boost::math::erfc(-sigma * M_SQRT1_2) == boost::math::cdf(norm, 1.0));

    double val = 0.5;
    auto boost_cdf = boost::math::cdf(norm, val);
    auto numeric_cdf = util_probability::norm_cdf(val, mean, sigma);
    LOG_ASSERT(std::abs(boost_cdf - numeric_cdf) < 1e-5);
}

int main(int argc, char** argv) {
    ::google::InitGoogleLogging(argv[0]);
    ::google::InstallFailureSignalHandler();
    ::testing::InitGoogleTest(&argc, argv);

    FLAGS_colorlogtostderr = true;
    FLAGS_logtostderr = true;

    return RUN_ALL_TESTS();
}


#ifndef ROMPC_UTILS_HPP
#define ROMPC_UTILS_HPP

/**
	@file rompc_utils.hpp
	Header file defining useful utilities for ROMPC controller.
*/

#include <memory>
#include <iostream>
#include <Eigen/Dense>
#include <qpOASES.hpp>

using Vec3 = Eigen::Vector3d;
using Vec4 = Eigen::Vector4d;
using Mat3 = Eigen::Matrix3d;
using VecX = Eigen::VectorXd;
using MatX = Eigen::MatrixXd;
using RowMajMat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                      Eigen::RowMajor>;
using ArrPtr = std::unique_ptr<qpOASES::real_t[]>;

namespace ROMPC_UTILS {

class Target {
public:
    Target();
    virtual void initialize(Vec3 p, double psi) {};
    virtual Vec3 get_pos(double t);
    virtual Vec3 get_vel(double t);
    virtual Vec4 get_att_quat(double t);
    virtual Vec3 get_att_euler(double t);
    virtual Vec3 get_att_aa(double t);
    virtual Vec3 get_om(double t);
    virtual ~Target() = default;
protected:
    Vec3 _p_r_i_I; // position relative to inertial in inertial coord
    Vec3 _v_r_I_R; // inertial velocity target frame coord.
    Vec4 _q_I_to_R; // quat rotation from inertial NED to target FRD frame
    Vec3 _om_R_I_R; // ang vel (p, q, r) of R w.r.t I frame, in R coordinates
    Vec3 _euler; // euler angles (roll, pitch, yaw)
    Vec3 _aa_I_to_R; // axis angle rotation from inertial NED to target FRD frame
};

/**
    @class SGF

    @brief Steady glideslope flight target. Velocity (u,v,w) is constant,
    body rates (p,q,r) are constant and zero, no side motion (v=0), roll angle
    is zero (phi=0), and constant descent rate defined by the glideslope
    angle gamma, which is negative when velocity is below local horizon.
    When gamma = 0 this is equivalent to steady level flight.
*/
class SGF : public Target {
public:
    SGF(double S, double gamma, double th);

    // Initialize position and yaw angle
    void initialize(Vec3 p, double psi) override;

    Vec3 get_pos(double t) override;

protected:
    Vec3 _v_r_I_I; // velocity as seen from I in inertial coord.
    double _S_xy; // speed in the inertial x-y plane
    double _th; // constant pitch angle
};

/**
    @class STF

    @brief Steady turning flight target. Velocity (u,v,w) is constant,
    body rates (p,q,r) are constant (but not zero), constant yaw rate
    \dot{\psi} (implicitly defined by constant speed S and turning radius
    R), constant altitude.
*/
class STF: public Target {
public:
    STF(Vec3 v, Vec3 om, double phi,  double th, double R);
    void compute_euler(double t);

    // Initialize position and yaw angle
    void initialize(Vec3 p, double psi) override;

    Vec3 get_pos(double t) override;
    Vec4 get_att_quat(double t) override;
    Vec3 get_att_euler(double t) override;
    Vec3 get_att_aa(double t) override;

protected:
    double _phi; // roll angle constant
    double _th; // pitch angle constant
    double _psi; // yaw angle/heading
    double _psi_dot; // yaw rate
    double _R; // turning radius
    Vec3 _p_c_i_I; // center of circle w.r.t inertial frame in I coord
};

/**
    @class OCP

    @brief ROMPC Optimal control problem with only control
    constraints.
*/
class OCP {
public:
    OCP(const std::string filepath, const double tmax);
    void solve(const VecX x0, Vec4& uopt);
    bool success();
    double solve_time();
    double get_dt();
    int get_N();

private:
    void eigen_to_qpoases(const MatX& M, ArrPtr& m);
    void set_x0(const VecX x0);

    int _nV; // number of vars in OCP
    int _nC; // number of constraints
    int _n; // state dimension
    MatX _G; // J = 1/2 U^T F U + x0^T G U
    MatX _E2; // s.t. E1 U <= ub = e + E2 x0
    VecX _e;

    ArrPtr _F;
    ArrPtr _E1;
    ArrPtr _ub;
    ArrPtr _g; // g = x0^T * G
    ArrPtr _U; // solution vector U = [u0, ..., u_N-1]

    double _dt; // discretization time of system
    int _N; // horizon

    qpOASES::QProblem _ocp; // ocp object
    double _tmax; // max amount of time to solve QP
    bool _success;
    double _solve_time;
};

void tangential_transf(const Vec3& aa, Mat3& T);

void om_to_aadot(const Vec3& aa, const Vec3& om, Vec3& aadot);

void aadot_to_om(const Vec3& aa, const Vec3& aadot, Vec3& om);

}

#endif // ROMPC_UTILS_HPP


#include "yavque/Utilities/pauli_operators.hpp"

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

namespace yavque
{
Eigen::SparseMatrix<double> pauli_x()
{
	std::vector<Eigen::Triplet<double>> t{{1, 0, 1.0}, {0, 1, 1.0}};
	Eigen::SparseMatrix<double> res(2, 2);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}
Eigen::SparseMatrix<cx_double> pauli_y()
{
	constexpr cx_double I(0., 1.);
	std::vector<Eigen::Triplet<cx_double>> t{{1, 0, I}, {0, 1, -I}};
	Eigen::SparseMatrix<cx_double> res(2, 2);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}

Eigen::SparseMatrix<double> pauli_z()
{
	std::vector<Eigen::Triplet<double>> t{{0, 0, 1.0}, {1, 1, -1.0}};
	Eigen::SparseMatrix<double> res(2, 2);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}

Eigen::SparseMatrix<double> pauli_xx()
{
	Eigen::SparseMatrix<double> res(4, 4);
	res.coeffRef(0, 3) = 1.0;
	res.coeffRef(1, 2) = 1.0;
	res.coeffRef(2, 1) = 1.0;
	res.coeffRef(3, 0) = 1.0;

	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> pauli_yy()
{
	Eigen::SparseMatrix<double> res(4, 4);
	res.coeffRef(0, 3) = -1.0;
	res.coeffRef(1, 2) = 1.0;
	res.coeffRef(2, 1) = 1.0;
	res.coeffRef(3, 0) = -1.0;

	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> pauli_xx_yy()
{
	std::vector<Eigen::Triplet<double>> t{{2, 1, 2.0}, {1, 2, 2.0}};
	Eigen::SparseMatrix<double> res(4, 4);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}

Eigen::SparseMatrix<double> pauli_zz()
{
	Eigen::SparseMatrix<double> res(4, 4);
	res.coeffRef(0, 0) = 1.0;
	res.coeffRef(1, 1) = -1.0;
	res.coeffRef(2, 2) = -1.0;
	res.coeffRef(3, 3) = 1.0;

	res.makeCompressed();
	return res;
}
} // namespace yavque


//
//  glasso.cpp
//
//  Python bindings of graphical lasso
//
//  Created by Kohei Miyaguchi on 2017/06/10.
//  Copyright © 2017年 Kohei Miyaguchi. All rights reserved.
//

#include <iostream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/functional.h>

namespace py = pybind11;
using namespace pybind11::literals;

struct GraphicalLassoResult {
    Eigen::MatrixXd Theta, Sigma;
    bool converged;
    GraphicalLassoResult(long m) : Theta{m, m}, Sigma{Eigen::MatrixXd::Identity(m, m)}, converged{false} {}
};

using GraphicalLasso = std::function<GraphicalLassoResult&(Eigen::MatrixXd const &, Eigen::MatrixXd const &)>;

double linesearch(const Eigen::MatrixXd& S, const Eigen::MatrixXd& W, const Eigen::MatrixXd& G, const Eigen::MatrixXd& Lambda, int iter_max=INT_MAX, bool verbose=false, double eps=1e-3);

GraphicalLasso graphicalLasso_stateful(long m, double tol, long iter_max, bool verbose, double eps) {
    using namespace Eigen;
    static const auto Zmm = MatrixXd::Zero(m, m);
    static const auto Ones = MatrixXd::Ones(m, m);
    MatrixXd W{m, m}, G{m, m};
    GraphicalLassoResult result(m);

    return [=](MatrixXd const &S, MatrixXd const &Lambda) mutable -> GraphicalLassoResult& {
        MatrixXd &Theta = result.Theta, &Sigma = result.Sigma;
        // Find feasible point of W assuming S and Sigma is SPD
        W = Sigma - S;
        MatrixXd shrinks = Lambda.array() / (W.cwiseAbs().array() + eps);
        double shrink = (Lambda.array() >= W.array()).select(shrinks, Ones).minCoeff();
        if (shrink < 1) {
            if (verbose) {
                printf("shrink rate: %f\n", shrink);
            }
            W *= shrink;
        }
        W.diagonal() = Lambda.diagonal();

        // Perform projected subgradient method over (S, Lambda, W, Theta) (not Sigma!)
        Theta = (S + W).inverse();
        double t = 0.0, gap = 2 * tol;
        for (int i = 0; i < iter_max; i++) {
            G = Theta;
            G.diagonal().setZero();
            G = (
                 ((W.array() >= Lambda.array()) * (G.array() > Zmm.array())) +
                 ((W.array() <= -Lambda.array()) * (G.array() < Zmm.array()))
                 ).select(Zmm, G);
            t = linesearch(S, W, G, Lambda, verbose);
            W = (W + t * G).cwiseMin(Lambda).cwiseMax(-Lambda);
            Theta = (S + W).inverse();
            S.cwiseProduct(Theta).sum();
            gap = S.cwiseProduct(Theta).sum() + Theta.cwiseAbs().cwiseProduct(Lambda).sum() - m;
            if (verbose) {
                printf("(glasso) step: %d, gap: %f\n", i, gap);
            }
            if (std::abs(gap / m) < tol) {
                result.converged = true;
                goto CONVERGED;
            }
        }
        result.converged = false;

    CONVERGED: // finish
        Theta = W.cwiseAbs().cwiseEqual(Lambda).select(Theta, Zmm);
        Sigma = S + W;
        return result;
    };
}

GraphicalLassoResult graphicalLasso(
        Eigen::MatrixXd const &S, Eigen::MatrixXd const &Lambda, Eigen::MatrixXd const &Sigma_init, Eigen::MatrixXd const &Theta_init,
        double tol, long iter_max, bool verbose, double eps) {
    using namespace Eigen;
    long m = S.rows();
    MatrixXd Zmm = MatrixXd::Zero(m, m);
    MatrixXd Ones = MatrixXd::Ones(m, m);
    MatrixXd W{m, m}, G{m, m};
    GraphicalLassoResult result(m);
    result.Theta = Theta_init;
    result.Sigma = Sigma_init;
    MatrixXd &Theta = result.Theta, &Sigma = result.Sigma;

    // Find feasible point of W assuming S and Sigma is SPD
    W = Sigma - S;
    MatrixXd shrinks = Lambda.array() / (W.cwiseAbs().array() + eps);
    double shrink = (Lambda.array() >= W.array()).select(shrinks, Ones).minCoeff();
    if (shrink < 1) {
        if (verbose) {
            printf("shrink rate: %f\n", shrink);
        }
        W *= shrink;
    }
    W.diagonal() = Lambda.diagonal();

    // Perform projected subgradient method over (S, Lambda, W, Theta) (not Sigma!)
    Theta = (S + W).inverse();
    double t = 0.0, gap = 2 * tol;
    for (int i = 0; i < iter_max; i++) {
        G = Theta;
        G.diagonal().setZero();
        G = (
             ((W.array() >= Lambda.array()) * (G.array() > Zmm.array())) +
             ((W.array() <= -Lambda.array()) * (G.array() < Zmm.array()))
             ).select(Zmm, G);
        t = linesearch(S, W, G, Lambda, verbose);
        W = (W + t * G).cwiseMin(Lambda).cwiseMax(-Lambda);
        Theta = (S + W).inverse();
        S.cwiseProduct(Theta).sum();
        gap = S.cwiseProduct(Theta).sum() + Theta.cwiseAbs().cwiseProduct(Lambda).sum() - m;
        if (verbose) {
            printf("(glasso) step: %d, gap: %f\n", i, gap);
        }
        if (std::abs(gap / m) < tol) {
            result.converged = true;
            goto CONVERGED;
        }
    }
    result.converged = false;

CONVERGED: // finish
    Theta = W.cwiseAbs().cwiseEqual(Lambda).select(Theta, Zmm);
    Sigma = S + W;
    return result;
}

void test_glasso() {
    using namespace Eigen;

    MatrixXd S(3, 3), K(3, 3);

    K << 3.0, 1.0, 0.0,
    1.0, 2.0, 0.5,
    0.0, 0.5, 1.0;
    S = K.inverse();

    MatrixXd Lambda = MatrixXd::Ones(3, 3) * 0.1;
    MatrixXd I = MatrixXd::Identity(3, 3);

    printf("start testing glasso:\n");
    auto result = graphicalLasso(S, Lambda, I, I, 1e-10, 100, true, 1e-5);

    MatrixXd Thetatrue(3, 3), Sigmatrue(3, 3);
    Thetatrue <<   2.04478, 0.343284,        0,
    0.343284,   1.3808, 0.262195,
    0, 0.262195, 0.835366;
    Sigmatrue <<   0.511765, -0.135294, 0.0424646,
    -0.135294,  0.805882, -0.252941,
    0.0424646, -0.252941,   1.27647;

    std::cout << "Thetaguess\n" << result.Theta << std::endl;
    std::cout << "Thetatrue\n" << Thetatrue << std::endl;
    std::cout << "Sigmaguess\n" << result.Sigma << std::endl;
    std::cout << "Sigmatrue\n" << Sigmatrue << std::endl;

    double abserror = (result.Theta - Thetatrue).cwiseAbs().sum() + (result.Sigma - Sigmatrue).cwiseAbs().sum();
    printf("absolute error: %f\n", abserror);
    assert(0.001 > abserror);
}

// subroutine of glasso
double linesearch(const Eigen::MatrixXd& S, const Eigen::MatrixXd& W, const Eigen::MatrixXd& G, const Eigen::MatrixXd& Lambda, int iter_max, bool verbose, double eps) {
    using namespace Eigen;
    double f0 = log((S + W).determinant());
    MatrixXd SWG = (S + W).inverse() * G;

    double nom = SWG.diagonal().sum();
    double denom = (SWG.array() * SWG.transpose().array()).sum();
    if (verbose) {
        printf("(linesearch) step:-, nom:%f denom:%f, f0:%f\n", nom, denom, f0);
    }
    if (std::abs(denom) <= 0.0) return 0.0;
    double t = nom / denom;
    if (t <= 0) return 0.0;

    for (int i = 0; 0.0 < t && i < iter_max; i++) {
        double f = log((S + (W + t * G).cwiseMin(Lambda).cwiseMax(-Lambda)).determinant());
        if (verbose) {
            printf("(linesearch) step:%d, t:%f f:%f, f0:%f\n", i, t, f, f0);
        }
        if (f >= f0) break;
        t *= 0.5;
    }
    return t;
}

PYBIND11_PLUGIN(glassobind) {
    py::module m("glassobind", "graphical lasso plugin");
    py::class_<GraphicalLassoResult>(m, "GraphicalLassoResult")
        .def(py::init<long>())
        .def_readonly("theta", &GraphicalLassoResult::Theta)
        .def_readonly("sigma", &GraphicalLassoResult::Sigma)
        .def_readonly("converged", &GraphicalLassoResult::converged);
    m.def("glasso", &graphicalLasso, "performs graphical lasso",
          "emp_cov"_a, "lambda"_a, "sigma_init"_a, "theta_init"_a, "tol"_a, "iter_max"_a, "verbose"_a, "eps"_a);
    m.def("test_glasso", &test_glasso, "test function");
    return m.ptr();
}


/*
 * adjointness.cpp
 *
 *  Created on: 22.07.2017
 *      Author: thies
 */

#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/SpaceTimeMesh.h>
#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>
#include <forward/DivRightHandSide.h>
#include <forward/DivRightHandSideAdjoint.h>
#include <gtest/gtest.h>
#include <norms/H1L2.h>
#include <norms/L2Coefficients.h>
#include <stddef.h>
#include <iostream>
#include <memory>
#include <vector>

namespace {

using namespace dealii;
using namespace wavepi::forward;
using namespace wavepi::base;
using namespace wavepi;

template<int dim>
class TestF: public Function<dim> {
public:
   double value(const Point<dim> &p, const unsigned int component = 0) const {
      Assert(component == 0, ExcIndexRange(component, 0, 1));

      Point<dim> pc = Point<dim>::unit_vector(0);
      pc *= 0.2;

      return this->get_time() * std::sin(p.distance(pc) * 2 * numbers::PI);
   }
};

template<int dim>
class TestG: public Function<dim> {
public:
   double value(const Point<dim> &p, const unsigned int component = 0) const {
      Assert(component == 0, ExcIndexRange(component, 0, 1));

      Point<dim> pc = Point<dim>::unit_vector(0);
      pc *= 0.5;

      return this->get_time() * std::sin(p.distance(pc) * 2 * numbers::PI);
   }
};

template<int dim>
void run_div_rhs_adjoint_test(int fe_order, int quad_order, int refines, int n_steps, double tol) {
   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, -1, 1);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   double t_start = 0.0, t_end = 2.0, dt = t_end / n_steps;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, FE_Q<dim>(fe_order),
         QGauss<dim>(quad_order), triangulation);

   deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
   deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;
   double err_avg = 0.0;
   double err_simple;

   for (size_t i = 0; i < 11; i++) {
      std::shared_ptr<DiscretizedFunction<dim>> f, g;

      if (i == 0) {
         // it is kind of important for this test that f and g are space dependent
         // (otherwise quantities will be very very small)
         TestF<dim> f_cont;
         f = std::make_shared<DiscretizedFunction<dim>>(mesh, f_cont);

         TestG<dim> g_cont;
         g = std::make_shared<DiscretizedFunction<dim>>(mesh, g_cont);
      } else {
         f = std::make_shared<DiscretizedFunction<dim>>(DiscretizedFunction<dim>::noise(mesh));

         // make it a bit smoother, random noise might be a bit too harsh
         f->set_norm(std::make_shared<norms::H1L2<dim>>(0.5));
         f->dot_transform_inverse();

         g = std::make_shared<DiscretizedFunction<dim>>(DiscretizedFunction<dim>::noise(mesh));

         // make it a bit smoother, random noise might be a bit too harsh
         g->set_norm(std::make_shared<norms::H1L2<dim>>(0.5));
         g->dot_transform_inverse();
      }

      f->set_norm(std::make_shared<norms::L2Coefficients<dim>>());
      *f *= 1.0 / f->norm();

      g->set_norm(std::make_shared<norms::L2Coefficients<dim>>());
      *g *= 1.0 / g->norm();

      auto zero = std::make_shared<DiscretizedFunction<dim>>(f->get_mesh());

      DivRightHandSide<dim> divrhs(f, zero, f);
      DivRightHandSideAdjoint<dim> divrhs_adj(f, g);

      DiscretizedFunction<dim> sol_f(mesh, std::make_shared<norms::L2Coefficients<dim>>());

      for (size_t i = 0; i < mesh->length(); i++) {
         divrhs.set_time(mesh->get_times()[i]);
         divrhs.create_right_hand_side(*mesh->get_dof_handler(i), mesh->get_quadrature(), sol_f[i]);
      }

      EXPECT_GT(sol_f.norm(), 0.0);

      auto adj_g = divrhs_adj.run_adjoint(mesh);
      adj_g.set_norm(std::make_shared<norms::L2Coefficients<dim>>());
      EXPECT_GT(adj_g.norm(), 0.0);

      double dot_solf_g = sol_f * (*g);
      double dot_f_adjg = (*f) * adj_g;
      double fg_err = std::abs(dot_solf_g - dot_f_adjg) / (std::abs(dot_solf_g) + 1e-300);

      if (i == 0) {
         // deallog << "simple f,g: " << std::scientific << "(Lf, g) = " << dot_solf_g << ", (f, L*g) = " << dot_f_adjg
         //         << std::endl;
         err_simple = fg_err;
         deallog << std::scientific << "        relative error for simple f,g = " << fg_err << std::endl;
      } else
         err_avg = ((i - 1) * err_avg + fg_err) / i;

      deallog << std::scientific << "(Lf, g) = " << dot_solf_g << ", (f, L*g) = " << dot_f_adjg << ", rel. error = "
            << fg_err << std::endl;

      // EXPECT_LT(zz_err, tol);
   }

   deallog << std::scientific << "average relative error for random f,g = " << err_avg << std::endl;
   EXPECT_LT(err_simple, tol);
   EXPECT_LT(err_avg, tol);
}
}  // namespace

TEST(DivRightHandSide, Adjoint1DFE1) {
   for (int i = 6; i < 7; i++)
      run_div_rhs_adjoint_test<1>(1, 3, 6, 1 << i, 1e-1);
}

TEST(DivRightHandSide, Adjoint1DFE2) {
   for (int i = 6; i < 7; i++)
      run_div_rhs_adjoint_test<1>(2, 6, 4, 1 << i, 1e-1);
}
TEST(DivRightHandSide, Adjoint2DFE1) {
   for (int i = 6; i < 7; i++)
      run_div_rhs_adjoint_test<2>(1, 3, 5, 1 << i, 1e-1);
}

TEST(DivRightHandSide, Adjoint3DFE1) {
   for (int i = 6; i < 7; i++)
      run_div_rhs_adjoint_test<3>(1, 3, 2, 1 << i, 1e-1);
}


#pragma once

#define EIGEN_HAS_CXX11_MATH 0
#include <Eigen/Dense>
#include <array>
#include "Geometry2d/Util.hpp"
#include "const-math.hpp"

/// Model parameters for a robot.  Used by the controls system.
class RobotModel {
public:
    /// Radius of omni-wheel (in meters)
    float WheelRadius;

    /// Distance from center of robot to center of wheel
    float WheelDist;

    /// Wheel angles (in radians) measured between +x axis and wheel axle
    std::array<float, 4> WheelAngles;

    /// wheelSpeeds = BotToWheel * V_bot
    Eigen::Matrix<float, 4, 3> BotToWheel;

    /// This should be called when any of the other parameters are changed
    void recalculateBotToWheel() {
        // See this paper for more info on how this matrix is derived:
        // http://people.idsia.ch/~foerster/2006/1/omnidrive_kiart_preprint.pdf

        // clang-format off
        BotToWheel <<
            sinf(WheelAngles[0]), cosf(WheelAngles[0]), -WheelDist,
            sinf(WheelAngles[1]), cosf(WheelAngles[1]), -WheelDist,
            sinf(WheelAngles[2]), cosf(WheelAngles[2]), -WheelDist,
            sinf(WheelAngles[3]), cosf(WheelAngles[3]), -WheelDist;
        BotToWheel /= WheelRadius;
        // clang-format on
    }

    /// (wheel rad/s desired) * DutyCycleMultiplier = duty cycle
    /// Note that this is an approximation, as the relationship isn't exactly
    /// linear
    float DutyCycleMultiplier;
};

/// Model parameters for 2015 robot.  See RobotModel.cpp for values.
extern const RobotModel RobotModel2015;


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file mcransac.hpp
///
/// \author Keenan Burnett
/// \brief Rigid and motion-compensated RANSAC implementations along with some auxilliary
///     SE(3) math functions.
//////////////////////////////////////////////////////////////////////////////////////////////
#pragma once
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <chrono>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <steam/steam.hpp>

namespace p = boost::python;
namespace np = boost::python::numpy;

/*!
   \brief Enforce orthogonality conditions on the given rotation matrix such that det(R) == 1 and R.tranpose() * R = I
   \param R The input rotation matrix either 2x2 or 3x3, will be overwritten with a slightly modified matrix to
   satisfy orthogonality conditions.
*/
void enforce_orthogonality(Eigen::MatrixXd &R);

/*!
   \brief Retrieve the rigid transformation that transforms points in p1 into points in p2.
   The output transform type (float or double) and size SE(2) vs. SE(3) depends on the size of the input points p1, p2.
   \param p1 A dim x N vector of points in either 2D (dim = 2) or 3D (dim = 3)
   \param p2 A dim x N vector of points in either 2D (dim = 2) or 3D (dim = 3)
   \param Tf [out] This matrix will be overwritten as the output transform
   \pre p1 and p2 are the same size. p1 and p2 are the matched feature point locations between two point clouds
   \post orthogonality is enforced on the rotation matrix.
*/
void get_rigid_transform(Eigen::MatrixXd p1, Eigen::MatrixXd p2, Eigen::MatrixXd &Tf);

/*!
   \brief Returns a random subset of indices, where 0 <= indices[i] <= max_index. indices are non-repeating.
*/
std::vector<int> random_subset(int max_index, int subset_size);

/*!
   \brief Returns the output of the carrot operator.
   For 3 x 1 input, carrot(x) * y is equivalent to cross_product(x, y)
   For 6 x 1 input, x = [rho, phi]^T. out = [carrot(phi), rho; 0 0 0 1]
   \param x Input vector which can be 3 x 1 or 6 x 1.
   \return If the input if 3 x 1, the output is 3 x 3, if the input is 6 x 1, the output is 4 x 4.
*/
Eigen::MatrixXd carrot(Eigen::VectorXd x);

/*!
   \brief Returns the output of the circledot operator. carrot(epsilon) * p == circledot(p) * epsilon,
   where epsilon is 6x1 and p is 4 x 1 homogeneous.
   p = [rhobar, eta]^T  circledot(p) = [eta * identity(3), -carrot(rhobar); 0 0 0 0 0 0]
   \param x Input is a 4 x 1 homogeneous 3D vector.
   \return returns the 4 x 6 output of circledot(x)
*/
Eigen::MatrixXd circledot(Eigen::VectorXd x);

/*!
   \brief This function converts from a lie vector to a 4 x 4 SE(3) transform.
   // Lie Vector xi = [rho, phi]^T (6 x 1) --> SE(3) T = [C, R; 0 0 0 1] (4 x 4)
   \param x Input vector is 6 x 1
   \return Output is 4 x SE(3) transform
*/
Eigen::Matrix4d se3ToSE3(Eigen::MatrixXd xi);

/*!
   \brief This function converts from an SE(3) transform into a lie vector
   // SE(3) T = [C, R; 0 0 0 1] (4 x 4) --> Lie Vector xi = [rho, phi]^T (6 x 1)
   \param T Input is a 4x4 SE(3) transform
   \return Output is 6x1 lie vector
*/
Eigen::VectorXd SE3tose3(Eigen::MatrixXd T);

/*!
   \brief Ensures that theta is within [0, 2 * pi)
*/
double wrapto2pi(double theta);

//* Ransac
/**
* \brief This class estimates a single rigid transform between two point clouds using RANSAC and singular value decomp
*/
class Ransac {
public:
    // p1, p2 need to be either (x, y) x N or (x, y, z) x N (must be in homogeneous coordinates)
    Ransac(const np::ndarray& p1_, const np::ndarray& p2_) {
        uint dim = p1_.shape(1);
        uint N = p1_.shape(0);
        assert(N == p2_.shape(0) && dim == p2_.shape(1) && dim >= 2);
        if (dim > 3)
            dim = 3;
        p1 = Eigen::MatrixXd::Zero(dim, N);
        p2 = Eigen::MatrixXd::Zero(dim, N);
        for (uint i = 0; i < dim; ++i) {
            for (uint j = 0; j < N; ++j) {
                p1(i, j) = double(p::extract<float>(p1_[j][i]));
                p2(i, j) = double(p::extract<float>(p2_[j][i]));
            }
        }
        T_best = Eigen::MatrixXd::Identity(dim + 1, dim + 1);
    }
    void setTolerance(double tolerance_) {tolerance = tolerance_;}
    void setInlierRatio(double inlier_ratio_) {inlier_ratio = inlier_ratio_;}
    void setMaxIterations(int iterations_) {iterations = iterations_;}
    void getTransform(Eigen::MatrixXd &Tf) {Tf = T_best;}

    /*!
       \brief Computes the transform that best aligns the two pointclouds such at T * p1 = p2
    */
    double computeModel();

    /*!
       \brief Retrieves the set of point pairs which are inliers given the current transform Tf.
    */
    void getInliers(Eigen::MatrixXd Tf, std::vector<int> &inliers);

private:
    Eigen::MatrixXd p1, p2;
    double tolerance = 0.35;
    double inlier_ratio = 0.9;
    int iterations = 100;
    Eigen::MatrixXd T_best;
};

//* MCRansac
/**
* \brief This class estimates the linear velocity and angular velocity of the sensor in the body-frame.
*
* Assuming constant velocity, the motion vector can be used to estimate the transform between any two pairs of points
* if the delta_t between those points issrand(t1_[i-1][0]); known.
*
* A single transform between the two pointclouds can also be retrieved.
*
* All operations are done in SE(3) even if the input is 2D. The output motion and transforms are in 3D.
*/
class MCRansac {
public:
    MCRansac(const np::ndarray& p1_, const np::ndarray& p2_, const np::ndarray& t1_, const np::ndarray& t2_) {
        assert(p1_.shape(0) == p2_.shape(0) && p1_.shape(1) == p2_.shape(1) && p1_.shape(0) >= p1_.shape(1));
        uint N = p1_.shape(0);
        uint dim = p1_.shape(1);
        if (dim > 3)
            dim = 3;
        p1bar = Eigen::MatrixXd::Zero(4, N);
        p2bar = Eigen::MatrixXd::Zero(4, N);
        p1bar.block(3, 0, 1, N) = Eigen::MatrixXd::Ones(1, N);
        p2bar.block(3, 0, 1, N) = Eigen::MatrixXd::Ones(1, N);
        for (uint i = 0; i < dim; ++i) {
            for (uint j = 0; j < N; ++j) {
                p1bar(i, j) = double(p::extract<float>(p1_[j][i]));
                p2bar(i, j) = double(p::extract<float>(p2_[j][i]));
            }
        }
        std::vector<int64_t> t1(N, 0);
        std::vector<int64_t> t2(N, 0);
        for (uint i = 0; i < N; ++i) {
            t1[i] = int64_t(p::extract<int64_t>(t1_[i]));
            t2[i] = int64_t(p::extract<int64_t>(t2_[i]));
        }
        R_pol << pow(0.25, 2), 0, 0, 0, 0, pow(0.0157, 2), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
        delta_ts = std::vector<double>(N, 0.0);
        for (uint i = 0; i < N; ++i) {
            int64_t delta_t = t2[i] - t1[i];
            delta_ts[i] = double(delta_t) / 1000000.0;
            if (delta_ts[i] > max_delta_t) {
                max_delta_t = delta_ts[i];
            }
            if (delta_ts[i] < min_delta_t) {
                min_delta_t = delta_ts[i];
            }
        }
        double delta_diff = (max_delta_t - min_delta_t) / (num_transforms - 1);
        for (int i = 0; i < num_transforms; ++i) {
            delta_vec.push_back(min_delta_t + i * delta_diff);
        }
    }
    void setTolerance(double tolerance_) {tolerance = tolerance_;}
    void setInlierRatio(double inlier_ratio_) {inlier_ratio = inlier_ratio_;}
    void setMaxIterations(int iterations_) {iterations = iterations_;}
    void setMaxGNIterations(int iterations_) {max_gn_iterations = iterations_;}
    void setConvergenceThreshold(double eps) {epsilon_converge = eps;}
    void correctForDoppler(bool doppler_) {doppler = doppler_;}
    void getTransform(double delta_t, Eigen::MatrixXd &Tf);
    void getMotion(Eigen::VectorXd &w) {w = w_best;}
    void setDopplerParameter(double beta_) {beta = beta_;}

    /*!
       \brief Computes the ego-motion vector that best aligns the two pointclouds
    */
    double computeModel();

    /*!
       \brief Retrieves the set of point pairs which are inliers given the current motion estimate.
    */
    void getInliers(Eigen::VectorXd wbar, std::vector<int> &inliers);

private:
    Eigen::MatrixXd p1bar, p2bar;
    std::vector<double> delta_ts;
    double tolerance = 0.1225;
    double inlier_ratio = 0.9;
    int iterations = 100;
    int max_gn_iterations = 10;
    double epsilon_converge = 0.0001;
    double error_converge = 0.01;
    int dim = 2;
    double beta = -0.049;  // beta = (f_t / (df / dt))
    double r_observable_sq = 0.0625;
    bool doppler = false;
    int num_transforms = 21;
    double max_delta_t = 0.0;
    double min_delta_t = 0.5;
    std::vector<double> delta_vec;
    Eigen::VectorXd w_best = Eigen::VectorXd::Zero(6);
    Eigen::Matrix4d R_pol = Eigen::Matrix4d::Identity();

    /*!
       \brief Given two sets of point pairs (p1small, p2small), this function computes the motion of the sensor
       (linear and angular velocity) in the body frame using nonlinear least squares.
       \pre It's very important that the delt_t_local is accurate. Note that each azimuth in the radar scan is time
       stamped, this should be used to get the more accurate time differences.
    */
    void get_motion_parameters(std::vector<int> subset, Eigen::VectorXd &wbar);

    /*!
       \brief Retrieve the number of inliers corresponding to body motion vector wbar. (6 x 1)
    */
    int getNumInliers(Eigen::VectorXd wbar);

    /*!
       \brief Given a body motion vector wbar (6 x 1), adjust the position of point p to account
       for the Doppler distortion which may be present in the data.
    */
    void dopplerCorrection(Eigen::VectorXd wbar, Eigen::VectorXd &p);
};

// Return the inverse of a 4x4 homogeneous transformation matrix
Eigen::Matrix4d get_inverse_tf(Eigen::Matrix4d T);