#pragma once

#include <Eigen/Dense>

namespace LSLOpt {

/**
 * @brief Vector type used.
 * @tparam Scalar The scalar type of the coefficients.
 *
 * This is a column vector (an `Eigen::Matrix`).
 */
template<typename Scalar>
using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

/**
 * @brief Matrix type used.
 * @tparam Scalar The scalar type of the coefficients.
 *
 * This is a matrix (an `Eigen::Matrix`).
 */
template<typename Scalar>
using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

/**
 * @brief Diagonal matrix type used.
 * @tparam Scalar The scalar type of the coefficients.
 *
 * This is a diagonal matrix (an `Eigen::DiagonalMatrix`).
 */
template<typename Scalar>
using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <dlib/optimization.h>
#include <iostream>
#include <limits>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/io.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/easy_mkl.hpp>
#include <mimkl/solvers/komd.hpp>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>

using dlib::mat;
using mimkl::data_structures::DataFrame;
using mimkl::data_structures::indexing_from_vector;
using mimkl::data_structures::range;
using mimkl::definitions::Indexing;

int main(int argc, char **argv)
{
    spdlog::set_level(spdlog::level::debug); // Set global log level to info
    auto console = spdlog::stdout_color_mt("console");

    const Index rows = 4; // 3 to reproduce error

    MATRIX(double) X(rows, 2);
    //	X << 1., 1., 3., 1., 1., 2.;
    X << 1., 1., 3., 1., 1., 4., 3., 2.;

    Eigen::SparseMatrix<double> L(2, 2);
    mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);
    MATRIX(double)
    K = mimkl::induction::induce_linear_kernel<MATRIX(double)>(X, X, L);
    COLUMN(double) Y(rows);
    //	Y << -1., 1., -1.;
    Y << -1., 1., -1., 1.;
    //	Eigen::Matrix<double, rows, 1> alphas;
    //	Eigen::Matrix<double, 3, 1> gamma_ref;

    std::cout << "X\n" << X << std::endl;
    std::cout << "dlib X\n" << mat(X) << std::endl;
    std::cout << "Y\n" << Y << std::endl;

    // test dlib assertions do not trigger? eg. with y.size()==1  TODO

    // KOMD
    mimkl::solvers::KOMD<double> some_dots(K, 0.2, 1e-8);
    some_dots.solve(Y);
    console->debug("get result :\n{}",
                   some_dots.get_result()); // gamma is private
    //  typedef Eigen::Map<EigenCol> MapEigenCol;
    COLUMN(double)
    gamma = some_dots.get_result(); // conversion of MapEigenCol to EigenCol
    console->info("gamma eigen (map to mat):\n{}", gamma);
    return EXIT_SUCCESS;
}


#ifndef SOLVE_SPARSE_LINEAR_SYSTEMS_HPP
#define SOLVE_SPARSE_LINEAR_SYSTEMS_HPP
#include "HeaderFile.h"

// http : //eigen.tuxfamily.org/dox/group__TopicSparseSystems.html
namespace Chapter3_SparseLinearAlgebra
{

//在Eigen中，当系数矩阵稀疏时，有几种方法可用于求解线性系统。
// 由于此类矩阵的特殊表示，因此应格外小心，以获得良好的性能。
// 有关Eigen中稀疏矩阵的详细介绍，请参见稀疏矩阵操作。
//此页面列出了Eigen中可用的稀疏求解器。还介绍了所有这些线性求解器共有的主要步骤。
//取决于矩阵的属性，所需的精度，最终用户能够调整这些步骤，以提高其代码的性能。
//请注意，并不需要深入了解这些步骤背后的内容：
//最后一部分提供了一个基准例程，可以轻松地使用它来深入了解所有可用求解器的性能。
namespace Section2_SolveSparseLinearSystems
{

void ListofSparseSolvers()
{
        //  Here SPD means symmetric positive definite SPD对应对称正定矩阵
        //Eigen当前提供了广泛的内置求解器，以及外部求解器库的包装器。下表中汇总了它们：

        // 1. 内置直接求解器: LLT LDLT需要对阵正定矩阵，LU需要方阵，QR适用于任意矩阵

        // Class	Solver kind	Matrix kind	Features related to performance	License        Notes

        // SimplicialLLT
        // #include<Eigen/SparseCholesky>	Direct LLt factorization	SPD	Fill-in reducing	LGPL    SimplicialLDLT is often preferable

        // SimplicialLDLT
        // #include<Eigen/SparseCholesky>	Direct LDLt factorization	SPD	Fill-in reducing	LGPL  Recommended for very sparse and not too large problems (e.g., 2D Poisson eq.)

        // SparseLU
        // #include<Eigen/SparseLU>	LU factorization	Square	Fill-in reducing, Leverage fast dense algebra	MPL2   optimized for small and large problems with irregular patterns

        // SparseQR
        // #include<Eigen/SparseQR>	QR factorization	Any, rectangular	Fill-in reducing	MPL2	recommended for least-square problems, has a basic rank-revealing feature

        // 2.内置迭代求解器

        //  Class	Solver kind	Matrix kind	Supported preconditioners, [default]	License  Notes

        // ConjugateGradient
        // #include<Eigen/IterativeLinearSolvers>	Classic iterative CG	SPD	IdentityPreconditioner, [DiagonalPreconditioner], IncompleteCholesky	MPL2
        // Recommended for large symmetric problems (e.g., 3D Poisson eq.)

        // LeastSquaresConjugateGradient
        // #include<Eigen/IterativeLinearSolvers>	CG for rectangular least-square problem	Rectangular	IdentityPreconditioner, [LeastSquareDiagonalPreconditioner]	MPL2
        // Solve for min |A'Ax-b|^2 without forming A'A

        // BiCGSTAB
        // #include<Eigen/IterativeLinearSolvers>	Iterative stabilized bi-conjugate gradient	Square	IdentityPreconditioner, [DiagonalPreconditioner], IncompleteLUT	MPL2	To speedup the convergence, try it with the IncompleteLUT preconditioner.

        //3.外部求解器
        //         Class	Module	Solver kind	Matrix kind	Features related to performance	Dependencies,License   Notes
        // PastixLLT
        // PastixLDLT
        // PastixLU	PaStiXSupport	Direct LLt, LDLt, LU factorizations	SPD
        // SPD
        // Square	Fill-in reducing, Leverage fast dense algebra, Multithreading	Requires the PaStiX package, CeCILL-C	optimized for tough problems and symmetric patterns
        // CholmodSupernodalLLT	CholmodSupport	Direct LLt factorization	SPD	Fill-in reducing, Leverage fast dense algebra	Requires the SuiteSparse package, GPL
        // UmfPackLU	UmfPackSupport	Direct LU factorization	Square	Fill-in reducing, Leverage fast dense algebra	Requires the SuiteSparse package, GPL
        // SuperLU	SuperLUSupport	Direct LU factorization	Square	Fill-in reducing, Leverage fast dense algebra	Requires the SuperLU library, (BSD-like)
        // SPQR	SPQRSupport	QR factorization	Any, rectangular	fill-in reducing, multithreaded, fast dense algebra	requires the SuiteSparse package, GPL	recommended for linear least-squares problems, has a rank-revealing feature
        // PardisoLLT
        // PardisoLDLT
        // PardisoLU	PardisoSupport	Direct LLt, LDLt, LU factorizations	SPD
        // SPD
        // Square	Fill-in reducing, Leverage fast dense algebra, Multithreading	Requires the Intel MKL package, Proprietary	optimized for tough problems patterns, see also using MKL with Eigen
}

void SparseSolverConcept()
{
        // 一个通用的例子
        // #include <Eigen/RequiredModuleName>
        // // ...
        // SparseMatrix<double> A;
        // // fill A
        // VectorXd b, x;
        // // fill b
        // // solve Ax = b
        // SolverClassName<SparseMatrix<double>> solver;
        // solver.compute(A);
        // if (solver.info() != Success)
        // {
        //         // decomposition failed
        //         return;
        // }
        // x = solver.solve(b);
        // if (solver.info() != Success)
        // {
        //         // solving failed
        //         return;
        // }
        // // solve for another right hand side:
        // x1 = solver.solve(b1);

        // 对于SPD求解器，第二个可选模板参数允许指定必须使用哪个三角形部分，例如：
        //```
        // #include <Eigen / IterativeLinearSolvers>
        // ConjugateGradient <SparseMatrix <double>，Eigen :: Upper >求解器;
        // x = Solver.compute（A）.solve（b）;

        // ```
        // 在上面的示例中，仅考虑输入矩阵A的上三角部分进行求解。对面的三角形可能为空或包含任意值。

        // 在必须解决具有相同稀疏模式的多个问题的情况下，可以将“计算”步骤分解如下：

        // SolverClassName<SparseMatrix<double>> 求解器;
        // Solver.analyzePattern（A）; //对于此步骤，不使用A的数值
        // resolver.factorize（A）;
        // x1 = Solver.solve（b1）;
        // x2 = Solver.solve（b2）;
        // ... A = ...; //修改A的非零值，非零模式必须保持不变
        // resolver.factorize（A）;
        // x1 = Solver.solve（b1）;
        // x2 = Solver.solve（b2）;
        // ...

        // 该compute（）方法等效于调用analyzerPattern（）和factorize（）。

        // 每个求解器都提供一些特定功能，例如行列式，对因子的访问，迭代控制等。有关更多详细信息，请参见相应类的文档。

        // 最后，大多数迭代求解器也可以在无矩阵的上下文中使用，请参见以下示例。
}

void TheComputeStep()
{
        // 在compute（）函数中，通常对矩阵进行因子分解：LLT用于自伴随矩阵，LDLT用于普通埃尔米特矩阵，LU用于非埃尔米特矩阵，QR用于矩形矩阵。
        //  这些是使用直接求解器的结果。对于这类求解器，将计算步骤进一步细分为analyzerPattern（）和factorize（）。

        // analyticsPattern（）的目标是对矩阵的非零元素进行重新排序，以使分解步骤创建的填充更少。
        //此步骤仅利用矩阵的结构。因此，该步骤的结果可用于矩阵结构相同的其他线性系统。
        //但是请注意，有时某些外部求解器（例如SuperLU）要求在此步骤中设置矩阵的值，以平衡矩阵的行和列。
        // 在这种情况下，此步骤的结果不应与其他矩阵一起使用。

        // Eigen在此步骤中提供了一组有限的方法来对矩阵进行重新排序，这些方法可以是内置的（COLAMD，AMD）或外部的（METIS）。
        // 这些方法在求解器的模板参数列表中设置：

        // DirectSolverClassName <SparseMatrix <double>，OrderingMethod <IndexType>> solver;
        // 有关可用方法和相关选项的列表，请参见OrderingMethods模块。

        // 在factorize（）中，计算系数矩阵的因数。每当矩阵值更改时，都应调用此步骤。但是，矩阵的结构模式不应在多个调用之间改变。

        // 对于迭代求解器，计算步骤用于最终设置预处理器。例如，使用ILUT预处理器，在此步骤中计算不完全因子L和U。
        // 请记住，基本上，预处理器的目标是通过解决系数矩阵具有更多聚类特征值的改进线性系统来加快迭代方法的收敛速度。
        //对于实际问题，迭代求解器应始终与前置条件一起使用。在Eigen中，只需将预处理器作为模板参数添加到迭代求解器对象中，即可选择预处理器。

        // IterativeSolverClassName <SparseMatrix <double>，PreconditionerName <SparseMatrix <double>>求解器;
        // 成员函数preconditioner（）返回对preconditioner的读写引用以直接与其交互。有关可用方法的列表，请参见迭代求解器模块和每个类的文档。
}

void TheSolveStep()
{
        //resolve()函数计算具有一个或多个右侧的线性系统的解。
        // X = Solver.solve（B）;
        // 在此，B可以是向量或矩阵，其中各列形成不同的右侧。可以多次调用solve（）函数，例如，当所有右侧都无法同时使用时。

        // x1 = Solver.solve（b1）;
        // //获得第二个右侧b2
        // x2 = Solver.solve（b2）;
        // // ...
        // 对于直接方法，解决方案以机器精度计算。
        // 有时，解决方案不必太精确。在这种情况下，迭代方法更合适，并且可以在使用setTolerance（）的求解步骤之前设置所需的精度。
        // 有关所有可用功能，请参阅“ 迭代求解器”模块的文档。
}

void BenchmarkRoutine()
{

        // 大多数时候，您需要知道的是解决系统所需的时间，并希望什么是最合适的求解器。
        // 在Eigen中，我们提供了可用于此目的的基准例程。这是非常容易使用。
        // 在构建目录中，导航到bench / spbench并通过键入make spbenchsolver来编译例程。
        // 使用–help选项运行它以获取所有可用选项的列表。基本上，要测试的矩阵应为MatrixMarket坐标格式，并且例程从Eigen中所有可用求解器返回统计信息。
        // 要以矩阵市场格式导出矩阵和右侧矢量，可以使用不受支持的SparseExtra模块：
        // #include <unsupported / Eigen / SparseExtra>
        // ...
        // Eigen :: saveMarket（A，“ filename.mtx”）;
        // Eigen :: saveMarket（A，“ filename_SPD.mtx”，Eigen :: Symmetric）; //如果A为对称正定
        // Eigen :: saveMarketVector（B，“ filename_b.mtx”）;
        // 下表提供了一些Eigen内置和外部求解器的XML统计信息示例。
        // 参见文档，这里不列出了
}
} // namespace Section2_SolveSparseLinearSystems

} // namespace Chapter3_SparseLinearAlgebra
#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * testTriangulation.cpp
 *
 *  Created on: July 30th, 2013
 *      Author: cbeall3
 */

#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

// Some common constants

static const boost::shared_ptr<Cal3_S2> sharedCal = //
    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);

// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);

// create second camera 1 meter to the right of first camera
static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);

// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);

// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);

//******************************************************************************
TEST( triangulation, twoPoses) {

  vector<Pose3> poses;
  vector<Point2> measurements;

  poses += pose1, pose2;
  measurements += z1, z2;

  bool optimize = true;
  double rank_tol = 1e-9;

  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
      sharedCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));

  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
      sharedCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
}

//******************************************************************************

TEST( triangulation, twoPosesBundler) {

  boost::shared_ptr<Cal3Bundler> bundlerCal = //
      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
  PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
  PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);
  Point2 z2 = camera2.project(landmark);

  vector<Pose3> poses;
  vector<Point2> measurements;

  poses += pose1, pose2;
  measurements += z1, z2;

  bool optimize = true;
  double rank_tol = 1e-9;

  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
      bundlerCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));

  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
      bundlerCal, measurements, rank_tol, optimize);
  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
}

//******************************************************************************
TEST( triangulation, fourPoses) {
  vector<Pose3> poses;
  vector<Point2> measurements;

  poses += pose1, pose2;
  measurements += z1, z2;

  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
      sharedCal, measurements);
  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));

  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> triangulated_landmark_noise = //
      triangulatePoint3(poses, sharedCal, measurements);
  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));

  // 3. Add a slightly rotated third camera above, again with measurement noise
  Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
  SimpleCamera camera3(pose3, *sharedCal);
  Point2 z3 = camera3.project(landmark);

  poses += pose3;
  measurements += z3 + Point2(0.1, -0.1);

  boost::optional<Point3> triangulated_3cameras = //
      triangulatePoint3(poses, sharedCal, measurements);
  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));

  // Again with nonlinear optimization
  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
      sharedCal, measurements, 1e-9, true);
  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));

  // 4. Test failure: Add a 4th camera facing the wrong way
  Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
  SimpleCamera camera4(pose4, *sharedCal);

#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);

  poses += pose4;
  measurements += Point2(400, 400);

  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
      TriangulationCheiralityException);
#endif
}

//******************************************************************************
TEST( triangulation, fourPoses_distinct_Ks) {
  Cal3_S2 K1(1500, 1200, 0, 640, 480);
  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  SimpleCamera camera1(pose1, K1);

  // create second camera 1 meter to the right of first camera
  Cal3_S2 K2(1600, 1300, 0, 650, 440);
  SimpleCamera camera2(pose2, K2);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);
  Point2 z2 = camera2.project(landmark);

  vector<SimpleCamera> cameras;
  vector<Point2> measurements;

  cameras += camera1, camera2;
  measurements += z1, z2;

  boost::optional<Point3> triangulated_landmark = //
      triangulatePoint3(cameras, measurements);
  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));

  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
  measurements.at(0) += Point2(0.1, 0.5);
  measurements.at(1) += Point2(-0.2, 0.3);

  boost::optional<Point3> triangulated_landmark_noise = //
      triangulatePoint3(cameras, measurements);
  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));

  // 3. Add a slightly rotated third camera above, again with measurement noise
  Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
  Cal3_S2 K3(700, 500, 0, 640, 480);
  SimpleCamera camera3(pose3, K3);
  Point2 z3 = camera3.project(landmark);

  cameras += camera3;
  measurements += z3 + Point2(0.1, -0.1);

  boost::optional<Point3> triangulated_3cameras = //
      triangulatePoint3(cameras, measurements);
  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));

  // Again with nonlinear optimization
  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras,
      measurements, 1e-9, true);
  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));

  // 4. Test failure: Add a 4th camera facing the wrong way
  Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
  Cal3_S2 K4(700, 500, 0, 640, 480);
  SimpleCamera camera4(pose4, K4);

#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);

  cameras += camera4;
  measurements += Point2(400, 400);
  CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
      TriangulationCheiralityException);
#endif
}

//******************************************************************************
TEST( triangulation, twoIdenticalPoses) {
  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  SimpleCamera camera1(pose1, *sharedCal);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 z1 = camera1.project(landmark);

  vector<Pose3> poses;
  vector<Point2> measurements;

  poses += pose1, pose1;
  measurements += z1, z1;

  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
      TriangulationUnderconstrainedException);
}

//******************************************************************************
/*
 TEST( triangulation, onePose) {
 // we expect this test to fail with a TriangulationUnderconstrainedException
 // because there's only one camera observation

 Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480);

 vector<Pose3> poses;
 vector<Point2> measurements;

 poses += Pose3();
 measurements += Point2();

 CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal),
 TriangulationUnderconstrainedException);
 }
 */

//******************************************************************************
TEST( triangulation, TriangulationFactor ) {
  // Create the factor with a measurement that is 3 pixels off in x
  Key pointKey(1);
  SharedNoiseModel model;
  typedef TriangulationFactor<> Factor;
  Factor factor(camera1, z1, model, pointKey);

  // Use the factor to calculate the Jacobians
  Matrix HActual;
  factor.evaluateError(landmark, HActual);

//  Matrix expectedH1 = numericalDerivative11<Pose3>(
//      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
//          boost::none, boost::none), pose1);
  // The expected Jacobian
  Matrix HExpected = numericalDerivative11<Point3>(
      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);

  // Verify the Jacobians are correct
  CHECK(assert_equal(HExpected, HActual, 1e-3));
}

//******************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//******************************************************************************


#include <mplot++/mplot++.h>
#include <Eigen/Dense>
#include <iostream>
#include <pybind11/eigen.h>
#include <pybind11/embed.h>

namespace ei = Eigen;
namespace py = pybind11;
using namespace py::literals;
namespace mp = mplotpp;

int
main()
{
  ei::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 << m << std::endl;

  ei::VectorXd v(2);
  v << 1, 2;

  py::scoped_interpreter guard;

  auto pym = py::cast(m);
  py::print("pym = ", pym);
  py::print("m = ", m);
  py::print("v = ", v);
  py::print("m*v = ", m * v);

  ei::ArrayXd x(4);
  x << 0, 1, 2, 3;
  ei::ArrayXd y = x.pow(2);

  std::cout << "x = " << x << std::endl;
  std::cout << "y = " << y << std::endl;

  py::module_ plt = py::module_::import("matplotlib.pyplot");

  auto [fig, ax] = mp::tuple<2>(plt.attr("subplots")());

  ax.attr("plot")(x, y, "r");
  plt.attr("show")();
}


/**
 * @file ConsensusCluster.hpp
 * @brief Implementation of consensus clustering using Armadillo.
 * @author Sriram P. Chockalingam <srirampc@gatech.edu>
 * @version 1.0
 * @date 2021-01-05
 *
 * Copyright 2021 Georgia Institute of Technology
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef DETAIL_CONSENSUSCLUSTER_HPP_
#define DETAIL_CONSENSUSCLUSTER_HPP_

#include "utils/Logging.hpp"

#include <armadillo>

template <typename MatType>
MatType
load_csv_mat(
  const char *file_name
)
{
  // load matrix from csv file
  MatType X;
  X.load(file_name, arma::csv_ascii);
  return X;
}

template <bool Norm, typename RandomIt, typename MatType>
double
clusterScore(const MatType&, const RandomIt, const RandomIt);

template <bool Norm, typename RandomIt>
double
clusterScore(
  const arma::mat& A,
  const RandomIt begin,
  const RandomIt end
)
{
  if (begin == end) {
    return 0.0;
  }
  arma::colvec v = arma::zeros<arma::colvec>(A.n_rows);
  for (auto it = begin; it != end; ++it) {
    v(*it) = 1.0;
  }
  auto w = A * v;
  auto score = arma::dot(v, w);
  auto denom = static_cast<double>(Norm ? pow(arma::norm(v), 2) :
                                          std::distance(begin, end));
  return score / denom;
}

template <bool Norm, typename RandomIt>
double
clusterScore(
  const arma::sp_mat& A,
  const RandomIt begin,
  const RandomIt end
)
{
  if (begin == end) {
    return 0.0;
  }
  arma::sp_mat v(A.n_rows, 1);
  for (auto it = begin; it != end; ++it) {
    v(*it, 0) = 1.0;
  }
  auto w = A * v;
  auto score = arma::dot(v, w);
  auto denom = static_cast<double>(Norm ? pow(arma::norm(v), 2) :
                                          std::distance(begin, end));
  return score / denom;
}

// Identify the cluster with best score by finding the top k
// vertices with the highest values of eigen vector that can maximize the
// cluster score
template <typename Var, typename MatType>
std::pair<std::vector<Var>, double>
bestCluster(
  const MatType& A,
  const arma::sp_mat&& v,
  const double tolerance
)
{
  LOG_MESSAGE_IF(v.n_cols != 1, error, "Perron vector should have one column");
  LOG_MESSAGE_IF(v.n_rows != A.n_rows, error, "Mismatch between the given row vector and matrix rows");
  LOG_MESSAGE_IF(v.n_rows != A.n_cols, error, "Mismatch between the given row vector and matrix columns");
  // Store non-zero values in sorted descending order
  std::set<std::pair<double, Var>, std::greater<std::pair<double, Var>>> vals;
  for (auto vit = v.begin_col(0); vit != v.end_col(0); ++vit) {
    // We only want to retain the elements which are greater than tolerance
    if (std::isgreater(*vit, tolerance)) {
      vals.insert(std::make_pair(*vit, vit.row()));
    }
  }
  // Now, copy all the eligible indices in the sorted order
  std::vector<Var> sortedIdx(vals.size());
  std::transform(vals.begin(), vals.end(), sortedIdx.begin(),
                 [] (const std::pair<Var, double>& vl)
                    { return vl.second; });

  auto maxScore = 0.0;
  auto numElements = 0u;
  // Find the top k vertices in the sorted order that maximzes the
  // cluster score
  const auto first = sortedIdx.cbegin();
  auto last = first + 1;
  for (auto k = 0u; k < sortedIdx.size(); ++k, ++last) {
    auto thisScore = clusterScore<true>(A, first, last);
    if (std::isgreaterequal(thisScore, maxScore)) {
      maxScore = thisScore;
      numElements = k + 1;
    }
  }
  LOG_MESSAGE_IF(numElements > 0, info, "Score: %g; Cluster Size: %u",
                                        clusterScore<false>(A, first, std::next(first, numElements)), numElements);
  sortedIdx.resize(numElements);
  return std::make_pair(sortedIdx, maxScore);
}

// Compute dominant eigen vector using the power method
template <typename MatType>
arma::sp_mat
perronVector(
  const MatType& A,
  const double tolerance,
  const uint32_t maxSteps
)
{
  // Initial vector : unit vector with 1.0 at vertex w. the maximum weight
  MatType rx = arma::sum(A, 1);
  arma::sp_mat v(rx.n_rows, 1);
  auto i = rx.index_max();
  v(i, 0) = 1.0;

  auto mu = 1.0;
  auto diff = 1.0;
  auto step = maxSteps;
  for (step = 0u; (step < maxSteps) && (diff > tolerance); ++step) {
    // Matrix - vector multiplication
    v = A * v;
    auto muNext = arma::norm(v);
    // normalize
    v = arma::normalise(v); // , p = 2);
    // Convergence parameter
    diff = std::abs(1.0 - muNext / mu);
    // Update mu
    mu = muNext;
  }
  LOG_MESSAGE(info, "Number of Steps / Max Steps: %u / %u", step, maxSteps);
  LOG_MESSAGE_IF(step == maxSteps, info, "Maximum number of steps reached with error = %g", diff);
  return v;
}

template <typename Var, typename MatType>
MatType
getSubmatrix(const MatType&, const std::vector<Var>&);

template <typename Var>
arma::mat
getSubmatrix(
  const arma::mat& Ain,
  const std::vector<Var>& indices
)
{
  // TODO : find a better way ?
  arma::mat Aout(indices.size(), indices.size());
  for (auto ix = 0u; ix < Aout.n_rows; ++ix) {
    for (auto jx = 0u; jx < Aout.n_cols; ++jx) {
      Aout(ix, jx) = Ain(indices[ix], indices[jx]);
    }
  }
  return Aout;
}

template <typename Var>
arma::sp_mat
getSubmatrix(
  const arma::sp_mat& Ain,
  const std::vector<Var>& indices
)
{
  // TODO : find a better way ?
  const auto nvertices = Ain.n_rows;
  arma::uvec map_idx(nvertices);
  map_idx.fill(nvertices);
  for (uint32_t ix = 0; ix < indices.size(); ++ix) {
    map_idx[indices[ix]] = ix;
  }
  uint32_t submat_size = 0;
  for (auto it = Ain.begin(); it != Ain.end(); ++it) {
    if (map_idx[it.row()] < nvertices && map_idx[it.col()] < nvertices) {
      ++submat_size;
    }
  }
  arma::umat locations(2, submat_size, arma::fill::zeros);
  arma::vec values(submat_size, arma::fill::zeros);
  uint32_t idx = 0;
  for (auto it = Ain.begin(); it != Ain.end(); ++it) {
    if ((map_idx[it.row()] < nvertices) && (map_idx[it.col()] < nvertices)) {
      locations(0, idx) = map_idx[it.row()];
      locations(1, idx) = map_idx[it.col()];
      values(idx) = *it; ++idx;
    }
  }
  return arma::sp_mat(locations, values, indices.size(), indices.size());
}

// Lemon tree cluster tightening algorithm
template <typename Var, typename MatType>
std::multimap<Var, Var>
perronCluster(
  MatType&& A,
  const double tolerance,
  const uint32_t maxSteps,
  const uint32_t minClustSize,
  const double minClustScore
)
{
  // set diagonal to ones as done by Lemon Tree
  A.diag().ones();
  // (to keep track of the original vertex id when we construct sub matrix)
  std::vector<Var> vertexMapping(A.n_rows);
  for (Var i = 0; i < A.n_rows; ++i) {
    vertexMapping[i] = i;
  }
  std::multimap<Var, Var> vertexClusters;
  Var clusterId = 0;
  auto numRemaining = A.n_rows;
  while (numRemaining >= minClustSize) {
    // Compute PF vector and best cluster
    auto v = perronVector(A, tolerance,  maxSteps);
    auto best = bestCluster<Var>(A, std::move(v), tolerance);
    auto& clusterElements = best.first;
    numRemaining = A.n_rows - clusterElements.size();
    // Identify remaining rows/cols
    std::vector<Var> currElements(A.n_rows);
    for (Var i = 0; i < A.n_rows; ++i) {
      currElements[i] = i;
    }
    std::sort(clusterElements.begin(), clusterElements.end());
    std::vector<Var> remainingElements(numRemaining);
    // XXX: We can possibly do better here
    std::set_difference(currElements.begin(), currElements.end(),
                        clusterElements.begin(), clusterElements.end(),
                        remainingElements.begin());
    A = getSubmatrix(A, remainingElements);
    if ((clusterElements.size() >= minClustSize) &&
        (best.second >= minClustScore)) {
      // Update the cluster ids of the clusters
      for (const auto ce : clusterElements) {
        auto vid = vertexMapping[ce];
        vertexClusters.emplace(clusterId, vid);
      }
      ++clusterId;
    }
    // Resize matrix and update mapping
    vertexMapping.resize(numRemaining);
    for (auto i = 0u; i < remainingElements.size(); ++i) {
      auto vid = vertexMapping[remainingElements[i]];
      vertexMapping[i] = vid;
    }
  }

  // XXX: Assign unique ids to the rest of the vertices ?
  return vertexClusters;
}

template <typename MatType, typename Set, typename Var>
void
fillCoclusteringWeights(
  MatType& C,
  const std::list<std::list<Set>>&& sampledClusters,
  const Var n,
  const double minWeight
)
{
  std::list<std::unordered_map<Var, uint32_t>> varClusterMaps;
  for (const auto& varClusters : sampledClusters) {
    std::unordered_map<Var, uint32_t> thisMap;
    auto c = 0u;
    for (auto cit = varClusters.begin(); cit != varClusters.end(); ++cit, ++c) {
      for (const auto var : *cit) {
        thisMap[var] = c;
      }
    }
    varClusterMaps.push_back(thisMap);
  }
  for (auto u = 0u; u < n; ++u) {
    for (auto v = u + 1; v < n; ++v) {
      auto cooccurrence = 0u;
      for (const auto& varCluster : varClusterMaps) {
        if (varCluster.at(u) == varCluster.at(v)) {
          ++cooccurrence;
        }
      }
      auto weight = static_cast<double>(cooccurrence) / sampledClusters.size();
      if (std::isgreater(weight, minWeight)) {
        C(u, v) = weight;
        C(v, u) = weight;
      }
    }
  }
}

template <typename Var, typename Set>
std::multimap<Var, Var>
consensusCluster(
  const std::list<std::list<Set>>&& sampledClusters,
  const Var numVars,
  const double minWeight,
  const double tolerance,
  const uint32_t maxSteps,
  const uint32_t minClustSize,
  const double minClustScore,
  const bool sparse = true
)
{
  if (sparse) {
    arma::sp_mat C(numVars, numVars);
    fillCoclusteringWeights(C, std::move(sampledClusters), numVars, minWeight);
    return perronCluster<Var>(std::move(C), tolerance, maxSteps, minClustSize, minClustScore);
  }
  else {
    arma::mat C(numVars, numVars, arma::fill::zeros);
    fillCoclusteringWeights(C, std::move(sampledClusters), numVars, minWeight);
    return perronCluster<Var>(std::move(C), tolerance, maxSteps, minClustSize, minClustScore);
  }
}

#endif // DETAIL_CONSENSUSCLUSTER_HPP_


//
// Copyright (c) 2019-2020 CNRS INRIA
//

#include <boost/python.hpp>

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/utils/namespace.hpp"
#include "pinocchio/math/rpy.hpp"

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobian_overload, rpy::computeRpyJacobian, 1, 2)
    BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianInverse_overload, rpy::computeRpyJacobianInverse, 1, 2)
    BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianTimeDerivative_overload, rpy::computeRpyJacobianTimeDerivative, 2, 3)

    Eigen::Matrix3d rotate(const std::string & axis, const double ang)
    {
      if(axis.length() != 1U)
          throw std::invalid_argument(std::string("Invalid axis: ").append(axis));
      Eigen::Vector3d u;
      u.setZero();
      const char axis_ = axis[0];
      switch(axis_)
      {
        case 'x': u[0] = 1.; break;
        case 'y': u[1] = 1.; break;
        case 'z': u[2] = 1.; break;
        default: throw std::invalid_argument(std::string("Invalid axis: ").append(1U,axis_));
      }

      return Eigen::AngleAxisd(ang, u).matrix();
    }

    void exposeRpy()
    {
      using namespace Eigen;
      using namespace pinocchio::rpy;

      {
        // using the rpy scope
        bp::scope current_scope = getOrCreatePythonNamespace("rpy");

        bp::def("rpyToMatrix",
                static_cast<Matrix3d (*)(const double&, const double&, const double&)>(&rpyToMatrix),
                bp::args("roll", "pitch", "yaw"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("rpyToMatrix",
                static_cast<Matrix3d (*)(const MatrixBase<Vector3d>&)>(&rpyToMatrix),
                bp::arg("rpy"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("matrixToRpy",
                &matrixToRpy<Matrix3d>,
                bp::arg("R"),
                "Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a."
                " The angles are guaranteed to be in the ranges: r in [-pi,pi],"
                " p in[-pi/2,pi/2], y in [-pi,pi]");

        bp::def("rotate",
                &rotate,
                bp::args("axis", "ang"),
                "Rotation matrix corresponding to a rotation about x, y or z"
                " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis");

        bp::def("computeRpyJacobian",
                &computeRpyJacobian<Vector3d>,
                computeRpyJacobian_overload(
                    bp::args("rpy","reference_frame"),
                    "Compute the Jacobian of the Roll-Pitch-Yaw conversion"
                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
                    " and reference frame F (either LOCAL or WORLD),"
                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
                    " where omega_F is the angular velocity expressed in frame F"
                    " and J_F is the Jacobian computed with reference frame F"
                    "\nParameters:\n"
                    "\trpy Roll-Pitch-Yaw vector"
                    "\treference_frame  Reference frame in which the angular velocity is expressed."
                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                )
        );

        bp::def("computeRpyJacobianInverse",
                &computeRpyJacobianInverse<Vector3d>,
                computeRpyJacobianInverse_overload(
                    bp::args("rpy","reference_frame"),
                    "Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion"
                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
                    " and reference frame F (either LOCAL or WORLD),"
                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
                    " where omega_F is the angular velocity expressed in frame F"
                    " and J_F is the Jacobian computed with reference frame F"
                    "\nParameters:\n"
                    "\trpy Roll-Pitch-Yaw vector"
                    "\treference_frame  Reference frame in which the angular velocity is expressed."
                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                )
        );

        bp::def("computeRpyJacobianTimeDerivative",
                &computeRpyJacobianTimeDerivative<Vector3d, Vector3d>,
                computeRpyJacobianTimeDerivative_overload(
                    bp::args("rpy", "rpydot", "reference_frame"),
                    "Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion"
                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
                    " and reference frame F (either LOCAL or WORLD),"
                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
                    " where omega_F is the angular velocity expressed in frame F"
                    " and J_F is the Jacobian computed with reference frame F"
                    "\nParameters:\n"
                    "\trpy Roll-Pitch-Yaw vector"
                    "\treference_frame  Reference frame in which the angular velocity is expressed."
                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                )
        );
      }

    }

  } // namespace python
} // namespace pinocchio


/**
 * @file UpdateMatricesTest.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the BSD 3-Clause License
 * @date 2020
 */

// Catch2
#include <catch2/catch.hpp>

// OsqpEigen
#include <OsqpEigen/OsqpEigen.h>

// eigen
#include <Eigen/Dense>

#include <cmath>
#include <iostream>
#include <fstream>

// colors
#define ANSI_TXT_GRN "\033[0;32m"
#define ANSI_TXT_MGT "\033[0;35m" //Magenta
#define ANSI_TXT_DFT "\033[0;0m" //Console default
#define GTEST_BOX "[     cout ] "
#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX //You could add the Default
#define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT

#define T 0.1

void setDynamicsMatrices(Eigen::Matrix<double, 2, 2> &a, Eigen::Matrix<double, 2, 1> &b,
                         Eigen::Matrix<double, 1, 2> &c,
                         double t)
{

    double omega = 0.1132;
    double alpha = 0.5 * sin(2 * M_PI * omega * t);
    double beta = 2 - 1 * sin(2 * M_PI * omega * t);

    a << alpha, 1,
        0, alpha;

    b << 0,
        1;

    c << beta, 0;
}

void setWeightMatrices(Eigen::DiagonalMatrix<double, 1> &Q, Eigen::DiagonalMatrix<double, 1> &R)
{
    Q.diagonal() << 10;
    R.diagonal() << 1;
}

void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 1> &Q, const Eigen::DiagonalMatrix<double, 1> &R, int mpcWindow,
                        int k, Eigen::SparseMatrix<double> &hessianMatrix)
{

    Eigen::Matrix<double, 2, 2> a;
    Eigen::Matrix<double, 2, 1> b;
    Eigen::Matrix<double, 1, 2> c;

    hessianMatrix.resize(2*(mpcWindow+1) + 1 * mpcWindow, 2*(mpcWindow+1) + 1 * mpcWindow);

    //populate hessian matrix
    for(int i = 0; i < 2 * (mpcWindow+1) + 1 * mpcWindow; i++){
        double t = (k + i) * T;
        setDynamicsMatrices(a, b, c, t);
        if(i < 2 * (mpcWindow + 1)){
            // here the structure of the matrix c is used!
            int pos=i%2;
            float value = c(pos) * Q.diagonal()[0] * c(pos);
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
        else{
            float value = R.diagonal()[0];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
    }
}

void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 1> &Q, const Eigen::Matrix<double, 1, 1> &yRef, int mpcWindow,
                         int k, Eigen::VectorXd &gradient)
{

    Eigen::Matrix<double, 2, 2> a;
    Eigen::Matrix<double, 2, 1> b;
    Eigen::Matrix<double, 1, 2> c;

    Eigen::Matrix<double,1,1> Qy_ref;
    Qy_ref = Q * (-yRef);

    // populate the gradient vector
    gradient = Eigen::VectorXd::Zero(2*(mpcWindow+1) + 1 *mpcWindow, 1);
    for(int i = 0; i<2*(mpcWindow+1); i++){
        double t = (k + i) * T;
        setDynamicsMatrices(a, b, c, t);

        int pos=i%2;
        float value = Qy_ref(0,0) * c(pos);
        gradient(i,0) = value;
    }
}

void castMPCToQPConstraintMatrix(int mpcWindow, int k, Eigen::SparseMatrix<double> &constraintMatrix)
{
    constraintMatrix.resize(2*(mpcWindow+1), 2*(mpcWindow+1) + 1 * mpcWindow);

    // populate linear constraint matrix
    for(int i = 0; i<2*(mpcWindow+1); i++){
        constraintMatrix.insert(i,i) = -1;
    }

    Eigen::Matrix<double, 2, 2> a;
    Eigen::Matrix<double, 2, 1> b;
    Eigen::Matrix<double, 1, 2> c;

    for(int i = 0; i < mpcWindow; i++){
        double t = (k + i) * T;
        setDynamicsMatrices(a, b, c, t);
        for(int j = 0; j<2; j++)
            for(int k = 0; k<2; k++){
                float value = a(j,k);
                if(value != 0){
                    constraintMatrix.insert(2 * (i+1) + j, 2 * i + k) = value;
                }
            }
    }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j < 2; j++)
            for(int k = 0; k < 1; k++){
                // b is constant
                float value = b(j,k);
                if(value != 0){
                    constraintMatrix.insert(2*(i+1)+j, 1*i+k+2*(mpcWindow + 1)) = value;
                }
            }
}

void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 2, 1> &x0,
                                  int mpcWindow,
                                  Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
    // evaluate the lower and the upper equality vectors
    lowerBound = Eigen::MatrixXd::Zero(2*(mpcWindow+1),1 );
    lowerBound.block(0,0,2,1) = -x0;
    upperBound = lowerBound;
}

bool updateHessianMatrix(OsqpEigen::Solver &solver,
                         const Eigen::DiagonalMatrix<double, 1> &Q, const Eigen::DiagonalMatrix<double, 1> &R,
                         int mpcWindow, int k)
{
    Eigen::SparseMatrix<double> hessianMatrix;
    castMPCToQPHessian(Q, R, mpcWindow, k, hessianMatrix);

    if(!solver.updateHessianMatrix(hessianMatrix))
        return false;

    return true;
}

bool updateLinearConstraintsMatrix(OsqpEigen::Solver &solver,
                                   int mpcWindow, int k)
{
    Eigen::SparseMatrix<double> constraintMatrix;
    castMPCToQPConstraintMatrix(mpcWindow, k, constraintMatrix);

    if(!solver.updateLinearConstraintsMatrix(constraintMatrix))
        return false;

    return true;
}

void updateConstraintVectors(const Eigen::Matrix<double, 2, 1> &x0,
                             Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
    lowerBound.block(0,0,2,1) = -x0;
    upperBound.block(0,0,2,1) = -x0;
}

TEST_CASE("MPCTest Update matrices")
{
    // open the ofstream
    std::ofstream dataStream;
    dataStream.open ("output.txt");

    // set the preview window
    int mpcWindow = 100;

    // allocate the dynamics matrices
    Eigen::Matrix<double, 2, 2> a;
    Eigen::Matrix<double, 2, 1> b;
    Eigen::Matrix<double, 1, 2> c;

    // allocate the weight matrices
    Eigen::DiagonalMatrix<double, 1> Q;
    Eigen::DiagonalMatrix<double, 1> R;

    // allocate the initial and the reference state space
    Eigen::Matrix<double, 2, 1> x0;
    Eigen::Matrix<double, 1, 1> yRef;
    Eigen::Matrix<double, 1, 1> y;

    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian;
    Eigen::VectorXd gradient;
    Eigen::SparseMatrix<double> linearMatrix;
    Eigen::VectorXd lowerBound;
    Eigen::VectorXd upperBound;

    // set the initial and the desired states
    x0 << 0, 0;
    yRef << 1;

    // set MPC problem quantities
    setWeightMatrices(Q, R);

    // cast the MPC problem as QP problem
    castMPCToQPHessian(Q, R, mpcWindow, 0, hessian);
    castMPCToQPGradient(Q, yRef, mpcWindow, 0, gradient);
    castMPCToQPConstraintMatrix(mpcWindow, 0, linearMatrix);
    castMPCToQPConstraintVectors(x0, mpcWindow, lowerBound, upperBound);

    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(2 * (mpcWindow + 1) + 1 * mpcWindow);
    solver.data()->setNumberOfConstraints(2 * (mpcWindow + 1));
    REQUIRE(solver.data()->setHessianMatrix(hessian));
    REQUIRE(solver.data()->setGradient(gradient));
    REQUIRE(solver.data()->setLinearConstraintsMatrix(linearMatrix));
    REQUIRE(solver.data()->setLowerBound(lowerBound));
    REQUIRE(solver.data()->setUpperBound(upperBound));

    // instantiate the solver
    REQUIRE(solver.initSolver());

    // controller input and QPSolution vector
    Eigen::VectorXd ctr;
    Eigen::VectorXd QPSolution;

    // number of iteration steps
    int numberOfSteps = 50;

    // profiling quantities
    clock_t startTime, endTime;
    double avarageTime = 0;

    for (int i = 0; i < numberOfSteps; i++){
        startTime = clock();

        setDynamicsMatrices(a, b, c, i * T);

        // update the constraint bound
        REQUIRE(updateHessianMatrix(solver, Q, R, mpcWindow, i));
        REQUIRE(updateLinearConstraintsMatrix(solver, mpcWindow, i));

        castMPCToQPGradient(Q, yRef, mpcWindow, i, gradient);
        REQUIRE(solver.updateGradient(gradient));

        updateConstraintVectors(x0, lowerBound, upperBound);
        REQUIRE(solver.updateBounds(lowerBound, upperBound));

        // solve the QP problem
        REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError);

        // get the controller input
        QPSolution = solver.getSolution();
        ctr = QPSolution.block(2 * (mpcWindow + 1), 0, 1, 1);

        // save data into file
        auto x0Data = x0.data();
        for(int j = 0; j < 2; j++)
            dataStream << x0Data[j] << " ";
        dataStream << std::endl;

        // propagate the model
        x0 = a * x0 + b * ctr;
        y = c * x0;

        endTime = clock();

        avarageTime += static_cast<double>(endTime - startTime) / CLOCKS_PER_SEC;
      }

    // close the stream
    dataStream.close();

    std::cout << COUT_GTEST_MGT << "Avarage time = " << avarageTime / numberOfSteps
              << " seconds." << ANSI_TXT_DFT << std::endl;
}


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include "ActivationFunctions.h"
#include <boost/algorithm/string.hpp>

// -------------------------------------------------------------------------
template< class _TScl >
const std::string& ActivationFunctions::Function< _TScl >::
name( ) const
{
  return( this->m_N );
}

// -------------------------------------------------------------------------
template< class _TScl >
const typename ActivationFunctions::Function< _TScl >::TScalar&
ActivationFunctions::Function< _TScl >::
threshold( ) const
{
  return( this->m_T );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Function< _TScl >::
TMatrix ActivationFunctions::Function< _TScl >::
t( const TMatrix& z ) const
{
  return(
    TMatrix( ( z.array( ) >= this->m_T ).template cast< TScalar >( ) )
    );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ArcTan< _TScl >::
TMatrix ActivationFunctions::ArcTan< _TScl >::
f( const TMatrix& z ) const
{
  return( Eigen::atan( z.array( ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ArcTan< _TScl >::
TMatrix ActivationFunctions::ArcTan< _TScl >::
d( const TMatrix& z ) const
{
  return( TScalar( 1 ) / ( Eigen::pow( z.array( ), 2 ) + TScalar( 1 ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::BinaryStep< _TScl >::
TMatrix ActivationFunctions::BinaryStep< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r = TMatrix::Ones( z.rows( ), z.cols( ) );
  r.array( ) *= ( z.array( ) >= 0 ).template cast< TScalar >( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::BinaryStep< _TScl >::
TMatrix ActivationFunctions::BinaryStep< _TScl >::
d( const TMatrix& z ) const
{
  return( TMatrix::Zero( z.rows( ), z.cols( ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ELU< _TScl >::
TMatrix ActivationFunctions::ELU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) =
    ( c * z.array( ) ) +
    (
      ( ( TScalar( 1 ) - c ) * this->m_Alpha ) *
      ( Eigen::exp( z.array( ) ) - TScalar( 1 ) )
      );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ELU< _TScl >::
TMatrix ActivationFunctions::ELU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix e = this->f( z );
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) < 0 ).template cast< TScalar >( );
  r.array( ) = ( c * ( e.array( ) + this->m_Alpha ) ) + ( TScalar( 1 ) - c );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Identity< _TScl >::
TMatrix ActivationFunctions::Identity< _TScl >::
f( const TMatrix& z ) const
{
  return( z );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Identity< _TScl >::
TMatrix ActivationFunctions::Identity< _TScl >::
d( const TMatrix& z ) const
{
  return( TMatrix::Ones( z.rows( ), z.cols( ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::LeakyReLU< _TScl >::
TMatrix ActivationFunctions::LeakyReLU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) =
    ( c * z.array( ) ) +
    ( ( TScalar( 1 ) - c ) * TScalar( 1e-2 ) ) * z.array( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::LeakyReLU< _TScl >::
TMatrix ActivationFunctions::LeakyReLU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) = c + ( ( TScalar( 1 ) - c ) * TScalar( 1e-2 ) );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Logistic< _TScl >::
TMatrix ActivationFunctions::Logistic< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  r.array( ) = TScalar( 1 ) / ( TScalar( 1 ) + Eigen::exp( -z.array( ) ) );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Logistic< _TScl >::
TMatrix ActivationFunctions::Logistic< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix e = this->f( z );
  TMatrix r( z.rows( ), z.cols( ) );
  r.array( ) = e.array( ) * ( TScalar( 1 ) - e.array( ) );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::OutTanh< _TScl >::
TMatrix ActivationFunctions::OutTanh< _TScl >::
f( const TMatrix& z ) const
{
  return( ( z.array( ).tanh( ) + TScalar( 1 ) ) / TScalar( 2 ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::OutTanh< _TScl >::
TMatrix ActivationFunctions::OutTanh< _TScl >::
d( const TMatrix& z ) const
{
  return( ( TScalar( 1 ) - z.array( ).tanh( ).square( ) ) / TScalar( 2 ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::RandomizedReLU< _TScl >::
TMatrix ActivationFunctions::RandomizedReLU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) =
    ( c * z.array( ) ) +
    ( ( TScalar( 1 ) - c ) * this->m_Alpha ) * z.array( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::RandomizedReLU< _TScl >::
TMatrix ActivationFunctions::RandomizedReLU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) = c + ( ( TScalar( 1 ) - c ) * this->m_Alpha );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ReLU< _TScl >::
TMatrix ActivationFunctions::ReLU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r = z;
  r.array( ) *= ( z.array( ) >= 0 ).template cast< TScalar >( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ReLU< _TScl >::
TMatrix ActivationFunctions::ReLU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix r = TMatrix::Ones( z.rows( ), z.cols( ) );
  r.array( ) *= ( z.array( ) >= 0 ).template cast< TScalar >( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::SoftPlus< _TScl >::
TMatrix ActivationFunctions::SoftPlus< _TScl >::
f( const TMatrix& z ) const
{
  return( Eigen::log( Eigen::exp( z.array( ) ) + TScalar( 1 ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::SoftPlus< _TScl >::
TMatrix ActivationFunctions::SoftPlus< _TScl >::
d( const TMatrix& z ) const
{
  return( TScalar( 1 ) / ( TScalar( 1 ) + Eigen::exp( -z.array( ) ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Tanh< _TScl >::
TMatrix ActivationFunctions::Tanh< _TScl >::
f( const TMatrix& z ) const
{
  return( z.array( ).tanh( ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Tanh< _TScl >::
TMatrix ActivationFunctions::Tanh< _TScl >::
d( const TMatrix& z ) const
{
  return( TScalar( 1 ) - this->f( z ).array( ).square( ) );
}

// Instances
#define _PUJ_ML_ActivationFunction_Instances( _n_, _t_ )        \
  template class ActivationFunctions::_n_< _t_ >

#define PUJ_ML_ActivationFunction_Instances( _n_ )             \
  _PUJ_ML_ActivationFunction_Instances( _n_, float );          \
  _PUJ_ML_ActivationFunction_Instances( _n_, double );         \
  _PUJ_ML_ActivationFunction_Instances( _n_, long double )

PUJ_ML_ActivationFunction_Instances( Function );
PUJ_ML_ActivationFunction_Instances( ArcTan );
PUJ_ML_ActivationFunction_Instances( BinaryStep );
PUJ_ML_ActivationFunction_Instances( ELU );
PUJ_ML_ActivationFunction_Instances( Identity );
PUJ_ML_ActivationFunction_Instances( LeakyReLU );
PUJ_ML_ActivationFunction_Instances( Logistic );
PUJ_ML_ActivationFunction_Instances( OutTanh );
PUJ_ML_ActivationFunction_Instances( RandomizedReLU );
PUJ_ML_ActivationFunction_Instances( ReLU );
PUJ_ML_ActivationFunction_Instances( SoftPlus );
PUJ_ML_ActivationFunction_Instances( Tanh );

// -------------------------------------------------------------------------
template< class _TScl >
ActivationFunctions::Factory< _TScl >::
Factory( )
{
  this->reg_cre( "arctan", &ActivationFunctions::ArcTan< _TScl >::create );
  this->reg_cre( "binarystep", &ActivationFunctions::BinaryStep< _TScl >::create );
  this->reg_cre( "elu", &ActivationFunctions::ELU< _TScl >::create );
  this->reg_cre( "identity", &ActivationFunctions::Identity< _TScl >::create );
  this->reg_cre( "leakyrelu", &ActivationFunctions::LeakyReLU< _TScl >::create );
  this->reg_cre( "logistic", &ActivationFunctions::Logistic< _TScl >::create );
  this->reg_cre( "outtanh", &ActivationFunctions::OutTanh< _TScl >::create );
  this->reg_cre( "randomizedrelu", &ActivationFunctions::RandomizedReLU< _TScl >::create );
  this->reg_cre( "relu", &ActivationFunctions::ReLU< _TScl >::create );
  this->reg_cre( "softplus", &ActivationFunctions::SoftPlus< _TScl >::create );
  this->reg_cre( "tanh", &ActivationFunctions::Tanh< _TScl >::create );
}

// -------------------------------------------------------------------------
template< class _TScl >
ActivationFunctions::Factory< _TScl >::
Factory( const Self& other )
{
}

// -------------------------------------------------------------------------
template< class _TScl >
ActivationFunctions::Factory< _TScl >::
~Factory( )
{
  this->m_FactoryMap.clear( );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Factory< _TScl >::
Self& ActivationFunctions::Factory< _TScl >::
operator=( const Self& other )
{
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Factory< _TScl >::
Self* ActivationFunctions::Factory< _TScl >::
get( )
{
  static Self instance;
  return( &instance );
}

// -------------------------------------------------------------------------
template< class _TScl >
void ActivationFunctions::Factory< _TScl >::
reg_cre( const std::string& n, TCreator c )
{
  this->m_FactoryMap[ n ] = c;
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Factory< _TScl >::
TFunction* ActivationFunctions::Factory< _TScl >::
create( const std::string& n ) const
{
  typename TMap::const_iterator i =
    this->m_FactoryMap.find( boost::algorithm::to_lower_copy( n ) );
  if( i != this->m_FactoryMap.end( ) )
    return( i->second( ) );
  else
    return( nullptr );
}

// -------------------------------------------------------------------------
PUJ_ML_ActivationFunction_Instances( Factory );

// eof - $RCSfile$


/*!
 * @file
 * calculating value of pi by MC trials
 *
 * command to run:
 * mpirung -n 2 ./bin/pi 100000
 *
 * The command line argument is the total number of trials.
 *
 * benchmarks at the bottom
 * */
#include <random>
#include <stdexcept>

#include <boost/mpi.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/filters.hpp>
#include <ezl/algorithms/io.hpp>
#include <ezl/algorithms/reduces.hpp>

// to facilitate random number generation
template <class T>
struct RandReal {
public:
  RandReal(T min, T max) : dis{min, max} {
    std::random_device rd;
    gen.seed(rd());
  }
  double operator () () {
    return dis(gen);
  }
private:
  std::mt19937 gen;
  std::uniform_real_distribution<T> dis;
};

void valueOfPi(int argc, char* argv[]) {
  if (argc < 2) {
    std::cout<<"Please provide number of MC trials as argument\n";
    return;
  }
  auto trials = std::stoll(argv[1]);

  RandReal<double> rand01{0.0,1.0};

  ezl::rise(ezl::kick(trials).split())
    .map([&rand01] {
      auto x = rand01();
      auto y = rand01();
      return x*x + y*y;
    })
    .filter(ezl::lt(1.))
    .reduce(ezl::count(), 0LL).inprocess()
    .reduce(ezl::sum(), 0LL)
    .map([trials](long long res) {
      return (4.0 * res / trials);
    }).colsTransform().dump("", "pi in " + std::to_string(trials) + " trials:")
    .run();
}

int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    valueOfPi(argc, argv);
  } catch (const std::exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}

/*!
 * The benchmarks are using rand() function for random.
 * benchmark results: i7(hdd); input: 4 x 10^9; units: secs
 *  *nprocs* | 1   | 2   | 4    |
 *  ---      |---  |---  |---   |
 *  *time(s)*| 111 | 56  | 39   |
 *
 * benchmark results: Linux(nfs-3); input: variable; units: secs
 *  *nprocs* | 1x12      | 2x12      | 4x12      | 8x12      |  16x12   |
 *  ---      |---        |---        |---        | ---       |          |
 *  *trials* | 1/8x10^11 | 1/4x10^11 | 1/2x10^11 | 1x10^11   |  2x10^11 |
 *  *time(s)*| 48        | 55        | 58        | 57.5      |  59      |
 */


#include <vtkImageData.h>
#include <vtkDICOMImageReader.h>
#include <vtkImageReader.h>
#include <vtkImageGaussianSmooth.h>
#include <vtkDemandDrivenPipeline.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>

#include <CGAL/Gray_image_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include <CGAL/Image_3.h>
#include <CGAL/read_vtk_image_data.h>

#include <boost/lexical_cast.hpp>

typedef short Image_word_type;

// Domain
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Gray_image_mesh_domain_3<CGAL::Image_3,K,
                                       Image_word_type,
                                       std::binder1st< std::less<Image_word_type> > > Mesh_domain;

// Triangulation
typedef CGAL::Mesh_triangulation_3<Mesh_domain>::type Tr;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

// Criteria
typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;

// To avoid verbose function and named parameters call
using namespace CGAL::parameters;

int main(int argc, char* argv[])
{
  // Loads image
  if(argc == 1){
    std::cerr << "Usage:  " << argv[0] << " <directory with dicom data> iso_level=1  facet_size=1  facet_distance=0.1  cell_size=1\n";
    return 0;
  }

  Image_word_type iso = (argc>2)? boost::lexical_cast<Image_word_type>(argv[2]): 1;
  double fs = (argc>3)? boost::lexical_cast<double>(argv[3]): 1;
  double fd = (argc>4)? boost::lexical_cast<double>(argv[4]): 0.1;
  double cs = (argc>5)? boost::lexical_cast<double>(argv[5]): 1;

  vtkDICOMImageReader*dicom_reader = vtkDICOMImageReader::New();
  dicom_reader->SetDirectoryName(argv[1]);

  vtkDemandDrivenPipeline*executive =
    vtkDemandDrivenPipeline::SafeDownCast(dicom_reader->GetExecutive());
  if (executive)
    {
      executive->SetReleaseDataFlag(0, 0); // where 0 is the port index
    }

  vtkImageGaussianSmooth* smoother = vtkImageGaussianSmooth::New();
  smoother->SetStandardDeviations(1., 1., 1.);
  smoother->SetInputConnection(dicom_reader->GetOutputPort());
  smoother->Update();
  vtkImageData* vtk_image = smoother->GetOutput();
  vtk_image->Print(std::cerr);

  CGAL::Image_3 image = CGAL::read_vtk_image_data(vtk_image);
  if(image.image() == 0){
    std::cerr << "could not create a CGAL::Image_3 from the vtk image\n";
    return 0;
  }
  // Domain
  Mesh_domain domain(image, std::bind1st(std::less<Image_word_type>(), iso), 0);

  // Mesh criteria
  Mesh_criteria criteria(facet_angle=30, facet_size=fs, facet_distance=fd,
                         cell_radius_edge_ratio=3, cell_size=cs);

  // Meshing
  C3t3 c3t3 = CGAL::make_mesh_3<C3t3>(domain, criteria);

  // Output
  std::ofstream medit_file("out.mesh");
  c3t3.output_to_medit(medit_file);

  return 0;
}


#include "software/sensor_fusion/filter/ball_filter.h"

#include <Eigen/Dense>
#include <algorithm>
#include <limits>
#include <vector>

#include "shared/constants.h"
#include "software/geom/algorithms/closest_point.h"
#include "software/geom/algorithms/contains.h"
#include "software/math/math_functions.h"

BallFilter::BallFilter() : ball_detection_buffer(MAX_BUFFER_SIZE) {}

std::optional<Ball> BallFilter::estimateBallState(
    const std::vector<BallDetection> &new_ball_detections, const Rectangle &filter_area)
{
    addNewDetectionsToBuffer(new_ball_detections, filter_area);
    return estimateBallStateFromBuffer(ball_detection_buffer);
}

void BallFilter::addNewDetectionsToBuffer(std::vector<BallDetection> new_ball_detections,
                                          const Rectangle &filter_area)
{
    // Sort the detections in increasing order before processing. This places the oldest
    // detections (with the smallest timestamp) at the front of the buffer, and the most
    // recent detections (largest timestamp) at the end of the buffer.
    std::sort(new_ball_detections.begin(), new_ball_detections.end());

    for (const auto &detection : new_ball_detections)
    {
        // Remove any detections outside the filter area
        if (!contains(filter_area, detection.position))
        {
            continue;
        }

        if (!ball_detection_buffer.empty())
        {
            // Use the smallest timestamp to minimize time_diffs of 0
            auto detection_with_smallest_timestamp = *std::min_element(
                ball_detection_buffer.begin(), ball_detection_buffer.end());
            Duration time_diff =
                detection.timestamp - detection_with_smallest_timestamp.timestamp;

            // Ignore any data from the past, and any data that is as old as the oldest
            // data in the buffer since it provides no additional value. This also
            // prevents division by 0 when calculating the estimated velocity
            if (time_diff.toSeconds() <= 0)
            {
                continue;
            }

            // We determine if the detection is noise based on how far it is from a ball
            // detection in the buffer. From this, we can calculate how fast the ball
            // must have moved to reach the new detection position. If this estimated
            // velocity is too far above the maximum allowed velocity, then there is a
            // good chance the detection is just noise and not the real ball. In this
            // case, we ignore the new "noise" data
            double detection_distance =
                (detection.position - detection_with_smallest_timestamp.position)
                    .length();
            double estimated_detection_velocity_magnitude =
                detection_distance / time_diff.toSeconds();

            // Make the maximum acceptable velocity a bit larger than the strict limits
            // according to the game rules to account for measurement error, and to be a
            // bit on the safe side. We don't want to risk discarding real data.
            double maximum_acceptable_velocity_magnitude =
                BALL_MAX_SPEED_METERS_PER_SECOND + MAX_ACCEPTABLE_BALL_SPEED_BUFFER;
            if (estimated_detection_velocity_magnitude >
                maximum_acceptable_velocity_magnitude)
            {
                // If we determine the data to be noise, remove an entry from the buffer.
                // This way if we have messed up and now the ball is too far away for the
                // buffer to track, the buffer will rapidly shrink and start tracking the
                // ball at its new location once the buffer is empty.
                // We sort the vector in decreasing order first so that we can always
                // ensure any elements that are ejected from the end of the buffer are the
                // oldest data
                std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend());
                ball_detection_buffer.pop_back();
            }
            else
            {
                // We sort the vector in decreasing order first so that we can always
                // ensure any elements that are ejected from the end of the buffer are the
                // oldest data
                std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend());
                ball_detection_buffer.push_front(detection);
            }
        }
        else
        {
            // If there is no data in the buffer, we always add the new data
            ball_detection_buffer.push_front(detection);
        }
    }
}

std::optional<Ball> BallFilter::estimateBallStateFromBuffer(
    boost::circular_buffer<BallDetection> ball_detections)
{
    // Sort the detections in decreasing order before processing. This places the most
    // recent detections (with the largest timestamp) at the front of the buffer, and the
    // oldest detections (smallest timestamp) at the end of the buffer
    std::sort(ball_detections.rbegin(), ball_detections.rend());

    if (ball_detections.empty())
    {
        return std::nullopt;
    }
    else if (ball_detections.size() == 1)
    {
        // If there is only 1 entry in the buffer, we can't fit a regression line
        // or calculate a velocity so we do our best with just the position
        BallState ball_state(ball_detections.front().position, Vector(0, 0),
                             ball_detections.front().distance_from_ground);
        Ball ball(ball_state, ball_detections.front().timestamp);
        return ball;
    }

    std::optional<size_t> adjusted_buffer_size = getAdjustedBufferSize(ball_detections);
    if (!adjusted_buffer_size)
    {
        return std::nullopt;
    }
    ball_detections.resize(*adjusted_buffer_size);

    auto regression_line = calculateLineOfBestFit(ball_detections);

    Point filtered_position = estimateBallPosition(ball_detections, regression_line);
    auto estimated_velocity = estimateBallVelocity(ball_detections, regression_line);
    if (!estimated_velocity)
    {
        return std::nullopt;
    }

    BallState ball_state(filtered_position, estimated_velocity->average_velocity,
                         ball_detections.front().distance_from_ground);
    return Ball(ball_state, ball_detections.front().timestamp);
}

std::optional<size_t> BallFilter::getAdjustedBufferSize(
    boost::circular_buffer<BallDetection> ball_detections)
{
    // Sort the detections in decreasing order before processing. This places the most
    // recent detections (with the largest timestamp) at the front of the buffer, and the
    // oldest detections (smallest timestamp) at the end of the buffer
    std::sort(ball_detections.rbegin(), ball_detections.rend());

    double buffer_size_velocity_magnitude_diff =
        MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE;

    unsigned int max_buffer_size =
        std::min(MAX_BUFFER_SIZE, static_cast<unsigned int>(ball_detections.size()));
    unsigned int min_buffer_size =
        std::min(MIN_BUFFER_SIZE, static_cast<unsigned int>(ball_detections.size()));
    double buffer_size_diff = max_buffer_size - min_buffer_size;

    std::optional<BallVelocityEstimate> velocity_estimate =
        estimateBallVelocity(ball_detections);
    if (!velocity_estimate)
    {
        return std::nullopt;
    }
    // Use the average of the min and max velocity magnitudes in the buffer. We use this
    // rather than the average so we can quickly respond to drastic changes in the ball
    // velocity, such as when the ball goes from being stationary to moving quickly (like
    // when it's kicked). If the buffer is large, then it will take more time for the mean
    // speed to increase enough to start shrinking the buffer. However, the average of the
    // min and max values will immediately increase if the ball starts moving, so the
    // buffer can start shrinking more quickly and increase the filter response time to
    // these sorts of changes.
    double min_max_magnitude_average = velocity_estimate->min_max_magnitude_average;

    // Between the min and max velocity magnitudes, we linearly scale the size of the
    // buffer
    double linear_offset =
        MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE + (buffer_size_velocity_magnitude_diff / 2);
    double linear_scaling_factor = linear(min_max_magnitude_average, linear_offset,
                                          buffer_size_velocity_magnitude_diff);
    int buffer_size =
        max_buffer_size -
        static_cast<unsigned int>(std::floor(linear_scaling_factor * buffer_size_diff));

    return static_cast<size_t>(buffer_size);
}

Line BallFilter::calculateLineOfBestFit(
    boost::circular_buffer<BallDetection> ball_detections)
{
    if (ball_detections.size() < 2)
    {
        throw std::invalid_argument("At least 2 elements required for linear regression");
    }

    auto x_vs_y_regression = calculateLinearRegression(ball_detections);

    // Linear regression cannot fit a vertical line. To get around this, we fit two lines,
    // one with x and y swapped, so any vertical line becomes horizontal. Then we take the
    // line of the two that fit the best.
    boost::circular_buffer<BallDetection> swapped_ball_detections = ball_detections;
    for (auto &detection : swapped_ball_detections)
    {
        detection.position = Point(detection.position.y(), detection.position.x());
    }
    auto y_vs_x_regression = calculateLinearRegression(swapped_ball_detections);
    // Because we swapped the coordinates of the input, we have to swap the coordinates of
    // the output to get back to our expected coordinate space
    y_vs_x_regression.regression_line.swapXY();

    // We use the regression from above with the least error
    if (x_vs_y_regression.regression_error < y_vs_x_regression.regression_error)
    {
        return x_vs_y_regression.regression_line;
    }
    else
    {
        return y_vs_x_regression.regression_line;
    }
}

BallFilter::LinearRegressionResults BallFilter::calculateLinearRegression(
    boost::circular_buffer<BallDetection> ball_detections)
{
    if (ball_detections.size() < 2)
    {
        throw std::invalid_argument("At least 2 elements required for linear regression");
    }

    // Sort the detections in increasing order before processing. This places the oldest
    // detections (smallest timestamp) at the front of the buffer, and the most recent
    // detections (with the largest timestamp) at the end of the buffer
    std::sort(ball_detections.begin(), ball_detections.end());

    // Construct matrix A and vector b for linear regression. The first column of A
    // contains the bias variable, and the second column contains the x coordinates of the
    // ball. Vector b contains the y coordinates of the ball.
    Eigen::MatrixXf A(ball_detections.size(), 2);
    Eigen::VectorXf b(ball_detections.size());
    for (unsigned i = 0; i < ball_detections.size(); i++)
    {
        // This extra column of 1's is the bias variable, so that we can regress with a
        // y-intercept
        A(i, 0) = 1.0;
        A(i, 1) = static_cast<float>(ball_detections.at(i).position.x());

        b(i) = static_cast<float>(ball_detections.at(i).position.y());
    }

    // Perform linear regression to find the line of best fit through the ball positions.
    // This is solving the formula Ax = b, where x is the vector we want to solve for.
    Eigen::Vector2f regression_vector =
        A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    // How to calculate the error is from
    // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
    double regression_error = std::numeric_limits<double>::max();

    if ((A * regression_vector - b).norm() == 0 && b.norm() == 0)
    {
        regression_error = 0;
    }
    if (b.norm() != 0)
    {
        regression_error =
            (A * regression_vector - b).norm() / (b.norm());  // norm() is L2 norm
    }

    // Find 2 points on the regression line that we solved for, and use this to construct
    // our own Line class
    Eigen::Vector2f p1_vec(1, 0);
    Point p1(0, p1_vec.dot(regression_vector));
    Eigen::Vector2f p2_vec(1, 1);
    Point p2(1, p2_vec.dot(regression_vector));
    Line regression_line = Line(p1, p2);

    LinearRegressionResults results({regression_line, regression_error});

    return results;
}

Point BallFilter::estimateBallPosition(
    boost::circular_buffer<BallDetection> ball_detections, const Line &regression_line)
{
    if (ball_detections.empty())
    {
        throw std::invalid_argument(
            "Non-empty buffer required to estimate ball position");
    }

    // Take the position of the most recent ball position and project it onto the line of
    // best fit. We do this because we assume the ball must be travelling along its
    // velocity vector (the line), and this allows us to return more stable position
    // values since the line of best fit is less likely to fluctuate compared to the raw
    // position of a ball detection
    BallDetection latest_ball_detection = ball_detections.front();
    return closestPoint(latest_ball_detection.position, regression_line);
}

std::optional<BallFilter::BallVelocityEstimate> BallFilter::estimateBallVelocity(
    boost::circular_buffer<BallDetection> ball_detections,
    const std::optional<Line> &ball_regression_line)
{
    // Sort the detections in increasing order before processing. This places the oldest
    // detections (smallest timestamp) at the front of the buffer, and the most recent
    // detections (with the largest timestamp) at the end of the buffer
    std::sort(ball_detections.begin(), ball_detections.end());

    std::vector<Vector> ball_velocities;
    std::vector<double> ball_velocity_magnitudes;
    for (unsigned i = 1; i < ball_detections.size(); i++)
    {
        for (unsigned j = i; j < ball_detections.size(); j++)
        {
            BallDetection previous_detection = ball_detections.at(i - 1);
            BallDetection current_detection  = ball_detections.at(j);

            Duration time_diff =
                current_detection.timestamp - previous_detection.timestamp;
            // Avoid division by 0. If we have adjacent detections with the same timestamp
            // the velocity cannot be calculated
            if (time_diff.toSeconds() == 0)
            {
                continue;
            }

            // Project the detection positions onto the regression line if it was provided
            Point current_position;
            Point previous_position;
            if (ball_regression_line)
            {
                current_position  = closestPoint(current_detection.position,
                                                ball_regression_line.value());
                previous_position = closestPoint(previous_detection.position,
                                                 ball_regression_line.value());
            }
            else
            {
                current_position  = current_detection.position;
                previous_position = previous_detection.position;
            }
            Vector velocity_vector    = current_position - previous_position;
            double velocity_magnitude = velocity_vector.length() / time_diff.toSeconds();
            Vector velocity           = velocity_vector.normalize(velocity_magnitude);

            ball_velocity_magnitudes.emplace_back(velocity_magnitude);
            ball_velocities.emplace_back(velocity);
        }
    }

    if (ball_velocities.empty() || ball_velocity_magnitudes.empty())
    {
        return std::nullopt;
    }

    double velocity_magnitude_sum = 0;
    for (const auto &velocity_magnitude : ball_velocity_magnitudes)
    {
        velocity_magnitude_sum += velocity_magnitude;
    }
    double average_velocity_magnitude =
        velocity_magnitude_sum / static_cast<double>(ball_velocity_magnitudes.size());
    double velocity_magnitude_max = *std::max_element(ball_velocity_magnitudes.begin(),
                                                      ball_velocity_magnitudes.end());
    double velocity_magnitude_min = *std::min_element(ball_velocity_magnitudes.begin(),
                                                      ball_velocity_magnitudes.end());
    double min_max_average = (velocity_magnitude_min + velocity_magnitude_max) / 2.0;

    Vector velocity_vector_sum = Vector(0, 0);
    for (const auto &velocity : ball_velocities)
    {
        velocity_vector_sum += velocity;
    }
    Vector average_velocity = velocity_vector_sum.normalize(average_velocity_magnitude);

    BallVelocityEstimate velocity_data(
        {average_velocity, average_velocity_magnitude, min_max_average});

    return velocity_data;
}


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <cmath>
#include <cstddef>
#include <iostream>

namespace gil = boost::gil;

int main()
{
    std::ptrdiff_t size = 32;
    gil::gray16_image_t input_image(size, size);
    auto input_view = gil::view(input_image);

    // fill secondary diagonal with ones
    // do note that origin is located at upper left,
    // not bottom left as in usual plots
    for (std::ptrdiff_t i = 0; i < size; ++i)
    {
        input_view(i, size - i - 1) = 1;
    }

    // print vertically flipped for better understanding of origin location
    for (std::ptrdiff_t y = size - 1; y >= 0; --y)
    {
        for (std::ptrdiff_t x = 0; x < size; ++x)
        {
            std::cout << input_view(x, y)[0] << ' ';
        }
        std::cout << '\n';
    }

    double minimum_theta_step = std::atan(1.0 / size);
    // this is the expected theta
    double _45_degrees = gil::detail::pi / 4;
    double _5_degrees = gil::detail::pi / 36;
    std::size_t step_count = 5;
    auto theta_parameter =
        gil::make_theta_parameter(_45_degrees, _5_degrees, input_view.dimensions());
    auto expected_radius = static_cast<std::ptrdiff_t>(std::round(std::cos(_45_degrees) * size));
    auto radius_parameter =
        gil::hough_parameter<std::ptrdiff_t>::from_step_size(expected_radius, 7, 1);
    gil::gray32_image_t accumulator_array_image(theta_parameter.step_count,
                                                radius_parameter.step_count);
    auto accumulator_array = gil::view(accumulator_array_image);
    gil::hough_line_transform(input_view, accumulator_array, theta_parameter, radius_parameter);
    std::cout << "expecting maximum at theta=" << _45_degrees << " and radius=" << expected_radius
              << '\n';
    for (std::size_t theta_index = 0; theta_index < theta_parameter.step_count; ++theta_index)
    {
        for (std::size_t radius_index = 0; radius_index < radius_parameter.step_count;
             ++radius_index)
        {
            double current_theta =
                theta_parameter.start_point + theta_index * theta_parameter.step_size;
            std::ptrdiff_t current_radius =
                radius_parameter.start_point + radius_parameter.step_size * radius_index;
            std::cout << "theta: " << current_theta << " radius: " << current_radius
                      << " accumulated value: " << accumulator_array(theta_index, radius_index)[0]
                      << '\n';
        }
    }
}


#ifndef _DUAL_FCC_ISOSURFACE_H_
#define _DUAL_FCC_ISOSURFACE_H_

#include <sisl/sisl.hpp>
#include <sisl/sparse_array.hpp>
#include <sisl/utility/ply_writer.hpp>

#include <Eigen/Dense>

#include <tuple>
#include <unordered_map>
#include <map>

namespace sisl{
namespace utility{
using namespace std;

template<class T>
class dualfcc_isosurface{
public:
	struct cell_vertex
	{
		cell_vertex(){}
		std::vector<vector3<T> > touching;
		vector3<T> vertex;
		int vertexId;
	};

	dualfcc_isosurface() : face_hash_table(1000,1000,1000, {}){
		this->faceList.clear();
	}

	template<class L, class I, class O>
	void contour(
			L *f,
			const O &isoValue,
			const I &scalingParameter,
			sisl::vector3<I> origin,
			sisl::vector3<I> boundary ){

		I dh = scalingParameter;
		int res = int(1./(dh));

		// Go over every lattice point
		#pragma omp parallel for
		for(unsigned int i = 2; i < res-4; i+=2){
			/*
			 * We keep this local to each worker, so we only have to delve into
			 * a critical section at the end of each loop
			 */
			std::vector<std::vector<vector3<int>>> localFaceList;

			for(unsigned int j = 2; j < res - 2; j++)
				for(unsigned int k = 2; k < res - 2; k++) {
					int ii = i + ((j&1) ^ (k&1));
					int jj = j;
					int kk = k;

					O value = f->f(dh*ii, dh*jj, dh*kk) - isoValue;

					// For each face in the minimal amount of faces of
					// the polyhedron
					for(auto idx : minimal_face_set) {
						auto polyhedron_vertex = polyhedron_vertices[idx];
						int x = polyhedron_vertex.i + ii,
							y = polyhedron_vertex.j + jj,
							z = polyhedron_vertex.k + kk;
						O next_value = f->f(dh*x, dh*y, dh*z) - isoValue;
						I zero_solution = 0.5;

						vector3<T> pv, n;

						// No sign change?
						if((next_value > 0 && value > 0) || (next_value <0 && value < 0))
							continue; // Whatever

						// Find the sign change.
						zero_solution = ((value - 0)/(value - next_value));
						pv = n = vector3<T>(x,y,z) - vector3<T>(ii,jj,kk);
						pv = vector3<T>(ii,jj,kk) + pv * zero_solution;
						n = n * (value > next_value ? -1 : 1);

						// Lookup all the dual points that touch this vertex
						std::vector<int> adj = adj_index[idx - 1];
						std::vector<std::vector<int>> luf = triangle_lookup[idx - 1];

						// Push all the faces into our local face list.
						for(auto triangle : luf) {
							vector3<int>
									hash1 = center_hash_offsets[triangle[0]] + vector3<int>(ii*2, jj*2, kk*2),
									hash2 = center_hash_offsets[triangle[1]] + vector3<int>(ii*2, jj*2, kk*2),
									hash3 = center_hash_offsets[triangle[2]] + vector3<int>(ii*2, jj*2, kk*2);

							vector3<int> t = (hash2 - hash1)%(hash3 - hash1);
							vector3<T> dir(t.i, t.j, t.k);
							if(dir * n > 0) localFaceList.push_back((std::vector<vector3<int>>){hash1, hash2, hash3});
							else localFaceList.push_back((std::vector<vector3<int>>){hash3, hash2, hash1});
						}

						// Mark the hashed dual vertex as having seen this primal vertex
						for(auto jdx : adj) {
							vector3<int> hash = center_hash_offsets[jdx] + vector3<int>(ii*2, jj*2, kk*2);

							#pragma omp critical (hash_bash_bcc)
							{
								face_hash_table(hash.i, hash.j, hash.k).touching.push_back({pv.i, pv.j, pv.k});
							}
						}

					}
				}
				// Merge the faces back in to global face list
				#pragma omp critical (lizst_cyst)
				{
					faceList.reserve(faceList.size() + localFaceList.size());
					faceList.insert(faceList.end(), localFaceList.begin(), localFaceList.end());
				}
		}
		processVertices<L,I,O>(f, dh);
		processFaces();
	}

	bool writeSurface(const std::string &out) const {
		return output_mesh.writePly(out);
	}

private:
	template<class L, class I, class O>
	void processVertices(L *f, const I &dh){
		// Calculate the vertex for each cell
		for (auto it = face_hash_table.siteMap.begin(); it != face_hash_table.siteMap.end(); ++it) {
			auto hash = it->first;
			auto vcache = it->second;

			std::vector<vector3<T>> normals;
			for(auto v : vcache.touching)
				normals.push_back(f->grad_f(v*dh).normalize());

			auto pavg = optimize_for_feature(vcache.touching, normals) * dh;
			auto normal = f->grad_f(pavg).normalize();

			face_hash_table.siteMap[hash].vertexId = output_mesh.addVertex({pavg, normal});
		}
	}

	void processFaces(){
		/* Build the final face list */
		for(auto face : faceList) {
			std::vector<int> index_face;
			for(auto hash : face) {
				index_face.push_back(face_hash_table(hash.i, hash.j, hash.k).vertexId);
			}
			output_mesh.addPolygon(index_face);
		}
	}

	std::vector<std::vector<vector3<int>>> faceList;
	sisl::sparse_array3<cell_vertex> face_hash_table;
	utility::ply_writer<T> output_mesh;

	const std::vector<vector3<int>> polyhedron_vertices = {
		{0,0,0}, {-1,0,1}, {0,1,1}, {1,0,1}, {0,-1,1},
		{-1,-1,0}, {1,-1,0}, {1,1,0}, {-1,1,0}, {0,-1,-1},
		{1,0,-1}, {0,1,-1},  {-1,0,-1}
	};

	const std::vector<vector3<int>> center_hash_offsets = {
		{ 2, 0, 0}, {-2, 0, 0}, { 0, 2, 0}, { 0,-2, 0},
		{ 0, 0, 2}, { 0, 0,-2}, { 1, 1, 1}, { 1, 1,-1},
		{ 1,-1, 1}, { 1,-1,-1}, {-1, 1, 1}, {-1, 1,-1},
		{-1,-1, 1}, {-1,-1,-1},
	};

	const std::vector<std::vector<int>> adj_index = {
		{4, 10, 12, 1}, {4, 10, 6, 2}, {4, 6, 8, 0}, {4, 8, 12, 3},
		{3, 13, 12, 1}, {0, 8, 9, 3}, {2,6,7,0}, {1,10,11,2},
		{3,9,13, 5}, {0,7,9,5}, {2,11,7, 5}, {1,13,11,5}
	};

	const std::vector<std::vector<std::vector<int>>> triangle_lookup = {
		{{4, 10, 12}, {10, 12, 1}}, {{4, 10, 6}, {10, 6, 2}},
		{{4, 6, 8}, {6, 8, 0}}, {{4, 8, 12}, {8, 12, 3}},
		{{3, 13, 12}, {8, 12, 3}}, {{0, 8, 9}, {8, 9, 3}},
		{{2,6,7}, {6,7,0}}, {{1,10,11}, {10,11,2}},
		{{3,9,13}, {9,13,5}}, {{0,7,9}, {7,9,5}},
		{{2,11,7}, {11,7,5}}, {{1,13,11}, {13,11,5}}
	};

	const std::vector<int> minimal_face_set = {1 , 2, 3, 4, 6, 7, 8};

	vector3<T> optimize_for_feature(
			const std::vector<vector3<T>> &points,
			const std::vector<vector3<T>> &normals,
			const T &threshold = 0.1,
			const bool &optimize = true) {
		using namespace Eigen;
		typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> EMatrix;
		vector3<T> center(0,0,0);
		EMatrix A(points.size(), 3), b(points.size(), 1);
		unsigned int i = 0;

		// Calculate the center and setup the matix
		for(auto v : points) {
			center += v;

			A(i, 0) = v.i;
			A(i, 1) = v.j;
			A(i, 2) = v.k;

			b(i, 0) = points[i] * normals[i];
			i++;
		}

		center = center * (1./(T(points.size())));

		return center;
	}

};
};
};

#endif // _DUAL_CC_ISOSURFACE_H_


#include <frovedis.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>
#include "../../rmse.hpp"

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating blockcyclic matrix from file
    auto bm1 = make_blockcyclic_matrix_load<float> ("./sample_4x4");
    auto v1 = make_blockcyclic_matrix_load<float> ("./sample_4x1");
    blockcyclic_matrix<float> v2(4,1); // output

    // v2 = bm1 * v1
    gemv<float> (bm1,v1,v2);
    v2.save("./out");

    double tol = 0.01;
    std::vector<float> e_out = {1.0, 32.0, 5.0, 4.0};
    auto m = make_rowmajor_matrix_local_load<float> ("./out");
    //for(auto &i: m.val) cout << i << " "; cout << endl;
    BOOST_CHECK (calc_rms_err<float> (m.val, e_out) < tol);
    system("rm -f ./out");
}


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

#include "Fraction.h"

using namespace omnn::math;
using namespace boost::unit_test_framework;

std::string l(const omnn::math::Valuable& v)
{
    std::stringstream ss;
    ss << v;
    return ss.str();
}

BOOST_AUTO_TEST_CASE(Fraction_tests)
{
    const auto a = 1_v / 2;
	auto c = 3_v / 1;
    auto b = a * 4;
	auto d = 2_v / 4;

    BOOST_TEST(a*b==1);
	BOOST_TEST((c += b) == 5);
	BOOST_TEST((c *= a) == 5_v / 2);
	BOOST_TEST((c /= a) == 5);
	BOOST_TEST((c--) == 5);
	BOOST_TEST((c++) == 4);
	BOOST_TEST(c > a);
	BOOST_TEST(a == d);
	BOOST_TEST(a - d == 0);

    Valuable _ = 3.1_v;
    BOOST_TEST(_.IsFraction());

    _ = (1_v/2)^2_v;
    BOOST_TEST(_ == 1_v/4);

    Variable v1, v2;
    _ = 1_v / (1_v / v1);
    BOOST_TEST(_ == v1);

    BOOST_TEST((2040_v*v1/(-2_v*v1))==-1020);

    _ = (2040_v/v1) / ((-1_v/v1)*v2);
    _.optimize();
    BOOST_TEST(_ == -2040_v/v2);

    BOOST_TEST((Fraction{1,-2}).operator<(0));

    _ = 1_v^(1_v/2);
    auto eq = _ == (1_v^(1_v/2));
    BOOST_TEST(eq);
    BOOST_TEST((_.IsMultival() == Valuable::YesNoMaybe::Yes));
    _ /= 1_v^(1_v/2);
    BOOST_TEST((_.IsMultival() == Valuable::YesNoMaybe::Yes));
    eq = _ == (1_v^(1_v/2));
    BOOST_TEST(eq);
}

BOOST_AUTO_TEST_CASE(Fraction_with_sum_tests
                     ,*disabled()
                     )
{
    auto _ = 841_v/64;
    _ ^= 1_v/2;
    auto a = (573440_v*(((841_v/64))^((1_v/2))) + 2115584)/262144;
    a.optimize();

    for (int i=38; i --> 1; ) {
        Valuable sh(1<<i);
        auto multi = 1_v^(1_v/sh);
        _ = multi;
        _ /= _;
        BOOST_TEST(_ == multi);
    }
}