#include <ceres/gradient_checker.h>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include "wave/wave_test.hpp"
#include "wave/utils/math.hpp"
#include "wave/geometry_og/transformation.hpp"
#include "wave/optimization/ceres/odom_gp/constant_velocity.hpp"
#include "wave/optimization/ceres/local_params/SE3Parameterization.hpp"

namespace {
/// Helper functions
Eigen::Matrix<double, 12, 12> calculateCVIntegrand(const wave::Mat6 &Qc,
                                                   const double &delta_T,
                                                   const wave::Vec6 &velocity) {
    Eigen::Matrix<double, 12, 12> retval;
    auto jacobian = wave::Transformation<>::SE3LeftJacobian(delta_T * velocity);
    retval.block<6, 6>(0, 0) = delta_T * delta_T * jacobian * Qc * jacobian.transpose();
    retval.block<6, 6>(6, 0) = delta_T * jacobian.transpose();
    retval.block<6, 6>(0, 6) = delta_T * jacobian;
    retval.block<6, 6>(6, 6) = Qc;
    return retval;
}

void calculateTransitionMatrix(const double &delta_T,
                               const wave::Vec6 &velocity,
                               Eigen::Matrix<double, 12, 12> &transition_matrix) {
    transition_matrix.setIdentity();
    transition_matrix.block<6, 6>(0, 0) = wave::Transformation<>::expMapAdjoint(delta_T * velocity);
    transition_matrix.block<6, 6>(0, 6) = delta_T * wave::Transformation<>::SE3LeftJacobian(delta_T * velocity);
}

Eigen::Matrix<double, 12, 12> integrateCovariance(const wave::Mat6 &Qc,
                                                  const double &delta_T,
                                                  const wave::Vec6 &velocity,
                                                  const int &steps) {
    Eigen::Matrix<double, 12, 12> Qtotal;
    Qtotal.setZero();
    double step_size = delta_T / (double) steps;
    Eigen::Matrix<double, 12, 12> Qincremental;
    double step_delta_T;
    for (int i = 0; i <= steps; i++) {
        step_delta_T = delta_T - ((double) i) * step_size;
        Qincremental = calculateCVIntegrand(Qc, step_delta_T, velocity);
        if (i == 0 || i == steps) {
            Qtotal.noalias() += 0.5 * Qincremental;
        } else {
            Qtotal.noalias() += Qincremental;
        }
    }
    Qtotal = Qtotal * step_size;
    return Qtotal;
}
}

namespace wave {

/**
 * Due to hack to workaround Ceres local parameterization for speedup
 * the built-in gradient checker can't be used
 */
TEST(ConstantVelocity, Jacobians) {
    Transformation<Eigen::Matrix<double, 3, 4>> start, end;
    Mat4 t_matrix;
    t_matrix << 0.936293363584199, -0.275095847318244, 0.218350663146334, 1, 0.289629477625516, 0.956425085849232,
      -0.036957013524625, 2, -0.198669330795061, 0.097843395007256, 0.975170327201816, 3, 0, 0, 0, 1;

    start.setFromMatrix(t_matrix);
    end.setFromMatrix(t_matrix);

    const double delta_T = 0.1;
    Vec6 start_vel, end_vel;

    start_vel << -0, 0, 0.3, 20, 5, -0.1;
    end_vel = start_vel;

    end.manifoldPlus(delta_T * start_vel);

    // Calculate transition matrix and weight matrix for residual
    Mat6 Qc = Mat6::Identity();
    Eigen::Matrix<double, 12, 12> covariance, weight;
    covariance = integrateCovariance(Qc, delta_T, start_vel, 20);

    weight = Eigen::Matrix<double, 12, 12>::Identity();

    ceres::CostFunction *cost_function = new ConstantVelocityPrior(weight, delta_T);

    const double **parameters;
    parameters = new const double *[4];
    parameters[0] = start.storage.data();
    parameters[1] = end.storage.data();
    parameters[2] = start_vel.data();
    parameters[3] = end_vel.data();

    double *residuals;
    residuals = new double[12];

    double **jacobians;
    jacobians = new double *[4];
    jacobians[0] = new double[144];
    jacobians[1] = new double[144];
    jacobians[2] = new double[72];
    jacobians[3] = new double[72];

    cost_function->Evaluate(parameters, residuals, jacobians);

    Eigen::Matrix<double, 12, 1> residual_vec;
    residual_vec = Eigen::Map<Eigen::Matrix<double, 12, 1>>(residuals);

    EXPECT_NEAR(residual_vec.norm(), 0, 1e-6);

    Eigen::Map<Eigen::Matrix<double, 12, 12, Eigen::RowMajor>> Jr_Ti(jacobians[0]);
    Eigen::Map<Eigen::Matrix<double, 12, 12, Eigen::RowMajor>> Jr_Tip1(jacobians[1]);
    Eigen::Map<Eigen::Matrix<double, 12, 6, Eigen::RowMajor>> Jr_Wi(jacobians[2]);
    Eigen::Map<Eigen::Matrix<double, 12, 6, Eigen::RowMajor>> Jr_Wip1(jacobians[3]);

    const double step_size = 1.4901e-07;
    const double inv_step = 1.0 / step_size;
    Vec6 delta;
    delta.setZero();

    Transformation<> Tk_perturbed, Tkp1_perturbed;
    Vec6 vel_k_perturbed, vel_kp1_perturbed;

    Tk_perturbed = (start);
    Tkp1_perturbed = (end);
    vel_k_perturbed = start_vel;
    vel_kp1_perturbed = end_vel;

    parameters[0] = Tk_perturbed.storage.data();
    parameters[1] = Tkp1_perturbed.storage.data();
    parameters[2] = vel_k_perturbed.data();
    parameters[3] = vel_kp1_perturbed.data();

    Eigen::Matrix<double, 12, 1> diff, result;

    std::vector<Eigen::Matrix<double, 12, 6>> an_jacs, num_jacs;
    num_jacs.resize(4);
    an_jacs.resize(4);
    an_jacs.at(0) = Jr_Ti.block<12, 6>(0, 0);
    an_jacs.at(1) = Jr_Tip1.block<12, 6>(0, 0);
    an_jacs.at(2) = Jr_Wi;
    an_jacs.at(3) = Jr_Wip1;

    for (uint32_t i = 0; i < 6; i++) {
        delta(i) = step_size;
        // First parameter
        Tk_perturbed.manifoldPlus(delta);
        cost_function->Evaluate(parameters, result.data(), nullptr);
        diff = result - residual_vec;
        num_jacs.at(0).block<12, 1>(0, i) = inv_step * diff;
        Tk_perturbed = (start);
        // Second parameter
        Tkp1_perturbed.manifoldPlus(delta);
        cost_function->Evaluate(parameters, result.data(), nullptr);
        diff = result - residual_vec;
        num_jacs.at(1).block<12, 1>(0, i) = inv_step * diff;
        Tkp1_perturbed = (end);
        // Third parameter
        vel_k_perturbed = vel_k_perturbed + delta;
        cost_function->Evaluate(parameters, result.data(), nullptr);
        diff = result - residual_vec;
        num_jacs.at(2).block<12, 1>(0, i) = inv_step * diff;
        vel_k_perturbed = start_vel;
        // Fourth parameter
        vel_kp1_perturbed = vel_kp1_perturbed + delta;
        cost_function->Evaluate(parameters, result.data(), nullptr);
        diff = result - residual_vec;
        num_jacs.at(3).block<12, 1>(0, i) = inv_step * diff;
        vel_kp1_perturbed = end_vel;

        delta.setZero();
    }

    for (uint32_t i = 0; i < 4; i++) {
        double err = (num_jacs.at(i) - an_jacs.at(i)).norm();
        EXPECT_NEAR(err, 0.0, 1e-6);
        if (err > 1e-6) {
            std::cout << "Failed on index " << i << std::endl
                      << "Numerical: " << std::endl
                      << num_jacs.at(i) << std::endl
                      << "Analytical:" << std::endl
                      << an_jacs.at(i) << std::endl
                      << std::endl;
        }
    }
}

}


/*
 * @file sa_test.cpp
 * @auther Zhihao Lou
 *
 * Test file for SA (simulated annealing).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/sa/sa.hpp>
#include <mlpack/core/optimizers/sa/exponential_schedule.hpp>
#include <mlpack/core/optimizers/problems/generalized_rosenbrock_function.hpp>
#include <mlpack/core/optimizers/problems/rosenbrock_function.hpp>
#include <mlpack/core/optimizers/problems/rastrigin_function.hpp>

#include <mlpack/core/metrics/ip_metric.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <mlpack/core/metrics/mahalanobis_distance.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;
using namespace mlpack::optimization::test;
using namespace mlpack::metric;

BOOST_AUTO_TEST_SUITE(SATest);

// The Generalized-Rosenbrock function is a simple function to optimize.
BOOST_AUTO_TEST_CASE(GeneralizedRosenbrockTest)
{
  size_t dim = 10;
  GeneralizedRosenbrockFunction f(dim);

  double iteration = 0;
  double result = DBL_MAX;
  arma::mat coordinates;
  while (result > 1e-6)
  {
    ExponentialSchedule schedule;
    // The convergence is very sensitive to the choices of maxMove and initMove.
    SA<ExponentialSchedule> sa(schedule, 1000000, 1000., 1000, 100, 1e-10, 3,
        1.5, 0.5, 0.3);
    coordinates = f.GetInitialPoint();
    result = sa.Optimize(f, coordinates);
    ++iteration;

    BOOST_REQUIRE_LT(iteration, 4); // No more than three tries.
  }

  // 0.1% tolerance for each coordinate.
  BOOST_REQUIRE_SMALL(result, 1e-6);
  for (size_t j = 0; j < dim; ++j)
      BOOST_REQUIRE_CLOSE(coordinates[j], (double) 1.0, 0.1);
}

// The Rosenbrock function is a simple function to optimize.
BOOST_AUTO_TEST_CASE(RosenbrockTest)
{
  RosenbrockFunction f;
  ExponentialSchedule schedule;
  // The convergence is very sensitive to the choices of maxMove and initMove.
  SA<> sa(schedule, 1000000, 1000., 1000, 100, 1e-11, 3, 1.5, 0.3, 0.3);
  arma::mat coordinates = f.GetInitialPoint();

  const double result = sa.Optimize(f, coordinates);

  BOOST_REQUIRE_SMALL(result, 1e-5);
  BOOST_REQUIRE_CLOSE(coordinates[0], 1.0, 1e-2);
  BOOST_REQUIRE_CLOSE(coordinates[1], 1.0, 1e-2);
}

/**
 * The Rastrigrin function, a (not very) simple nonconvex function. It has very
 * many local minima, so finding the true global minimum is difficult.
 */
BOOST_AUTO_TEST_CASE(RastrigrinFunctionTest)
{
  // Simulated annealing isn't guaranteed to converge (except in very specific
  // situations).  If this works 1 of 4 times, I'm fine with that.  All I want
  // to know is that this implementation will escape from local minima.
  size_t successes = 0;

  for (size_t trial = 0; trial < 4; ++trial)
  {
    RastriginFunction f(2);
    ExponentialSchedule schedule;
    // The convergence is very sensitive to the choices of maxMove and initMove.
    // SA<> sa(schedule, 2000000, 100, 50, 1000, 1e-12, 2, 2.0, 0.5, 0.1);
    SA<> sa(schedule, 2000000, 100, 50, 1000, 1e-12, 2, 2.0, 0.5, 0.1);
    arma::mat coordinates = f.GetInitialPoint();

    const double result = sa.Optimize(f, coordinates);

    if ((std::abs(result) < 1e-3) &&
        (std::abs(coordinates[0]) < 1e-3) &&
        (std::abs(coordinates[1]) < 1e-3))
    {
      ++successes;
      break; // No need to continue.
    }
  }

  BOOST_REQUIRE_GE(successes, 1);
}

BOOST_AUTO_TEST_SUITE_END();


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

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

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

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

 /*!

    @ingroup group-hyperbolic
    Function object implementing acoth capabilities

    Returns the hyperbolic cotangent argument \f$\frac12\log\frac{x^2+1}{x^2-1}\f$

    @par Semantic:

    For every parameter of floating type T

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

    is similar to:

    @code
    T r = Half<T>()*log(oneplus(sqr(x))/minusone(sqr(x)));
    @endcode

    @see log, Half

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

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

#endif


// Copyright (C) 2016  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/optimization/elastic_net.h>
#include "tester.h"
#include <dlib/svm.h>
#include <dlib/rand.h>
#include <dlib/string.h>
#include <vector>
#include <sstream>
#include <ctime>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.elastic_net");

// ----------------------------------------------------------------------------------------

    matrix<double,0,1> basic_elastic_net(
        const matrix<double>& X,
        const matrix<double,0,1>& Y,
        double ridge_lambda,
        double lasso_budget,
        double eps
    )
    {
        DLIB_CASSERT(X.nc() == Y.nr(),"");

        typedef matrix<double,0,1> sample_type;
        typedef linear_kernel<sample_type> kernel_type;

        svm_c_linear_dcd_trainer<kernel_type> trainer;
        trainer.solve_svm_l2_problem(true);
        const double C = 1/(2*ridge_lambda);
        trainer.set_c(C);
        trainer.set_epsilon(eps);
        trainer.enable_shrinking(true);
        trainer.include_bias(false);

        std::vector<sample_type> samples;
        std::vector<double> labels;
        for (long r = 0; r < X.nr(); ++r)
        {
            sample_type temp = trans(rowm(X,r));

            const double xmul = (1/lasso_budget);
            samples.push_back(temp - xmul*Y);
            labels.push_back(+1);
            samples.push_back(temp + xmul*Y);
            labels.push_back(-1);
        }

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;
        auto df = trainer.train(samples, labels, state);
        auto&& alpha = state.get_alpha();

        matrix<double,0,1> betas(alpha.size()/2);
        for (long i = 0; i < betas.size(); ++i)
            betas(i) = lasso_budget*(alpha[2*i] - alpha[2*i+1]);
        betas /= sum(mat(alpha));
        return betas;
    }

// ----------------------------------------------------------------------------------------

    class test_elastic_net : public tester
    {
    public:
        test_elastic_net (
        ) :
            tester (
                "test_elastic_net",
                "Run tests on the elastic_net object.",
                0
            )
        {
        }

        void perform_test (
        )
        {
            matrix<double> w = {1,2,0,4, 0,0,0,0,0, 6, 7,8,0, 9, 0};

            matrix<double> X = randm(w.size(),1000);
            matrix<double> Y = trans(X)*w;
            Y += 0.1*(randm(Y.nr(), Y.nc())-0.5);

            double ridge_lambda = 0.1;
            double lasso_budget = sum(abs(w));
            double eps = 0.0000001;

            dlib::elastic_net solver(X*trans(X),X*Y);
            solver.set_epsilon(eps);

            matrix<double,0,1> results;
            matrix<double,0,1> results2;
            for (double s = 1.2; s > 0.10; s *= 0.9)
            {
                print_spinner();
                dlog << LINFO << "s: "<< s;
                // make sure the two solvers agree.
                results = basic_elastic_net(X, Y, ridge_lambda, lasso_budget*s, eps);
                results2 = solver(ridge_lambda, lasso_budget*s);
                dlog << LINFO << "error: "<< max(abs(results - results2));
                DLIB_TEST(max(abs(results - results2) < 1e-3));
            }
        }
    } a;

// ----------------------------------------------------------------------------------------

}


#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <pangolin/pangolin.h>
#include <string>
#include <unistd.h>
#include <vector>

using namespace std;
using namespace Eigen;

// Load the images
string left_file = "./left.png";
string right_file = "./right.png";

// Method in pangolin to plot the grayscale pointcloud
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {

  // Intrinsics
  double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
  // Baseline
  double b = 0.573;

  // Read the images as grayscales and get the disparity map
  cv::Mat left = cv::imread(left_file, 0);
  cv::Mat right = cv::imread(right_file, 0);
  // Semi global block matching to get the disparity maps
  // Very parameter dependent, read the paper to understand
  cv::Ptr<cv::StereoSGBM> sgbm =
      cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
  cv::Mat disparity_sgbm, disparity;
  sgbm->compute(left, right, disparity_sgbm);
  disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

  // Vector to store the pointcloud
  vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

  // Cycle through th pixels
  for (int v = 0; v < left.rows; v++)
    for (int u = 0; u < left.cols; u++) {
      // Check if the disparity (which is in pixels) is valid
      if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

      // Initialize the position an the grayscale value
      Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);

      // Steps to get pointcloud:
      // - Get the depth using the fx (in pixels), the disparity (in pixels) and
      // the baseline (in meters)
      // Get the normalized coordinates which are basically projections on plane z=1
      // Multiply the x,y coordinates with the depth and th z coordinate is the depth
      double depth = fx * b / (disparity.at<float>(v, u));
      double x = (u - cx) / fx;
      double y = (v - cy) / fy;
      point[0] = x * depth;
      point[1] = y * depth;
      point[2] = depth;

      pointcloud.push_back(point);
    }

  cv::imshow("disparity", disparity / 96.0);
  cv::waitKey(0);
  // Show the pointcloud in pangolin
  showPointCloud(pointcloud);
  return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

  if (pointcloud.empty()) {
    cerr << "Point cloud is empty!" << endl;
    return;
  }

  pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));

  pangolin::View &d_cam =
      pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
          .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glPointSize(2);
    glBegin(GL_POINTS);
    for (auto &p : pointcloud) {
      glColor3f(p[3], p[3], p[3]);
      glVertex3d(p[0], p[1], p[2]);
    }
    glEnd();
    pangolin::FinishFrame();
    usleep(5000);  // sleep 5 ms
  }
  return;
}


#ifndef NDSS_MATRIX_HPP
#define NDSS_MATRIX_HPP
#include "Vector.hpp"

#include <vector>
#include <iostream>
#ifdef USE_EIGEN
#include <eigen3/Eigen/Dense>
#endif
#include <NTL/ZZX.h>
class EncryptedArray;
namespace MDL {
template<typename T>
class Matrix : public std::vector<Vector<T> > {
public:
    Matrix(int rows = 0, int cols = 0)
        : std::vector<Vector<T> >(rows, Vector<T>(cols)) {}

    size_t rows() const;
    size_t cols() const;

    Matrix<T>  dot(const Matrix<T>& oth) const;
    Vector<T>  dot(const Vector<T>& oth) const;
#ifdef USE_EIGEN
    double        maxEigenValue() const;
    Matrix<double>inverse() const;
    Eigen::MatrixXd to_Eigen_matrix_format() const;
    void from_Eigen_matrix(const Eigen::MatrixXd& mat);
#endif
    std::vector<NTL::ZZX> encode(const EncryptedArray &ea) const;

    Matrix& operator*=(const T &val);

    Matrix& operator+=(const Matrix<T> &oth);

    Matrix& operator-=(const Matrix<T> &oth);

    void random(const T &domain);

    Matrix<double> reduce(const double factor) const;

    Matrix<long> div(long factor) const;

    Vector<T> vector() const;

    template<typename U>
    friend std::ostream& operator<<(std::ostream& os,
                                    Matrix<U>   & obj);
    /// @create a submatrix from row r1, to row r2 and from column c1 to column c2.
    Matrix<T> submatrix(long r1, long r2, long c1 = 0, long c2 = 0) const;

    Matrix<T> transpose() const;
};

Matrix<long> eye(long dimension);

template<typename T>
Matrix<T> covariance(const Vector<T> &a, const Vector<T> &b);

} // namespace MDL
#endif // ifndef NDSS_MATRIX_HPP


#include "libs/experiments/ROC.h"
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <math.h>
#include <utility>
#include <tuple>
#include <iostream>
#include <iterator>
#include  <stdexcept>

namespace exprs
{

value_type accuracy(value_type threshold, const std::vector<int>& true_val,
        const data_array& pred)
{
    static const value_type eps = 1.0e-10;
    int a  = 0,b = 0,c = 0, d = 0;
    for (int item = 0; item < (int)true_val.size(); item++) {
        if (true_val[item] == 1) {
            if (pred[item] >= threshold) {
                a++;
            } else {
                b++;
            }
        } else {
            if (pred[item] >= threshold) {
                c++;
            } else {
                d++;
            }
        }
    }
    return( ((value_type)(a+d)) / (((value_type)(a+b+c+d)) + eps) );
}

int partition(int p, int r, std::vector<int>& true_val, data_array& pred)
{

    value_type x = pred[p];
    if (p >= (int)pred.size() || p < 0) {
        throw std::runtime_error{"cannot partition lower limit - out of range between " + std::to_string(pred.size()) +
            " and " + std::to_string(p)};
    }
    if (r <= 0 || r >= (int)pred.size()) {
       throw std::runtime_error{"cannot partition upper limit - out of range between " + std::to_string(r) +
                " and " + std::to_string(pred.size())};
    }
    int i = p - 1;
    int j = r + 1;
    while (true) {
        do j--; while (j > -1 && pred[j] > x);
        if (j < 0) {
            throw std::runtime_error{"invalid value given no value less than " + std::to_string(x) + " found"};
        }
        do i++; while (i < (int)pred.size() && pred[i] < x );
        if (i == (int)pred.size()) {
            throw std::runtime_error{"invalid value given no value greater than " + std::to_string(x) + " found"};
        }
        if (i < j) {
            std::swap(pred[i], pred[j]);
            std::swap(true_val[i], true_val[j]);
        } else {
            return j;
        }
    }
}

void quicksort(int p, int r, std::vector<int>& true_val, data_array& pred)
{
    if (p < r) {
        int q = partition(p, r, true_val, pred);
        quicksort(p, q, true_val, pred);
        quicksort(q+1, r, true_val, pred);
    }
}

template<typename T>
value_type calculateRmse(const std::vector<T>& true_val, const data_array& pred,
        value_type& mean_true)
{
    value_type sse = 0.0;
    for(int no_item = 0; no_item < (int)true_val.size(); ++no_item) {
        value_type p1 = pred[no_item];
        sse+= (true_val[no_item]-p1)*(true_val[no_item]-p1);
        mean_true += true_val[no_item];
    }
    mean_true /= (value_type) true_val.size();
    return  sqrt(sse / ((value_type)true_val.size()));

}
std::tuple<value_type, value_type,
    data_array, data_array>
do_calculation(ROC::algo_params&& calc_params)
{
    // now let's do the ROC cruve and area
    quicksort(0, (int)(calc_params.partitions.size() - 1u), calc_params.partitions, calc_params.predictions);
    auto total_0 = calc_params.predictions.size() - calc_params.condition_pos;
    auto tt = 0;
    auto tf = calc_params.condition_pos;
    auto ft = 0;
    auto ff = total_0;

    auto sens = ((value_type) tt) / ((value_type) (tt+tf));
    auto spec = ((value_type) ff) / ((value_type) (ft+ff));
    auto tpf = sens;
    auto fpf = 1.f - spec;
    data_array true_pf, false_pf;

    true_pf.push_back(tpf);
    false_pf.push_back(fpf);
    auto roc_area = 0.f;
    auto tpf_prev = tpf;
    auto fpf_prev = fpf;

    auto no_item = calc_params.predictions.size();
    for (int item=no_item-1; item>-1; item--) {
        tt+= calc_params.partitions[item];
        tf-= calc_params.partitions[item];
        ft+= 1 - calc_params.partitions[item];
        ff-= 1 - calc_params.partitions[item];
        sens = ((value_type) tt) / ((value_type) (tt+tf));
        spec = ((value_type) ff) / ((value_type) (ft+ff));
        tpf  = sens;
        fpf  = 1.f - spec;
        if (item > 0) {
            if (calc_params.predictions[item] != calc_params.predictions[item - 1]) {
                true_pf.push_back(tpf);
                false_pf.push_back(fpf);
                roc_area += 0.5f * (tpf + tpf_prev) * (fpf-fpf_prev);
                tpf_prev = tpf;
                fpf_prev = fpf;
            }
        }
        if (item == 0) {
            true_pf.push_back(tpf);
            false_pf.push_back(fpf);
            roc_area += 0.5f * (tpf+tpf_prev) * (fpf-fpf_prev);
        }
    }
    auto acc = accuracy(0.5, calc_params.partitions, calc_params.predictions);
    return std::make_tuple(roc_area, acc, std::move(true_pf), std::move(false_pf));
}

template<typename T>
std::size_t ROC::algo_params::init(const std::vector<T>& ex,
        std::vector<int>& target,
        value_type threshold)
{
    auto pos = 0u;
    boost::transform(ex, std::back_inserter(target), [&pos, threshold](auto val) {
                if (val < threshold) {
                    return 0;
                } else {
                    ++pos;
                    return 1;
                }
            }
    );
    return pos;
}

ROC::algo_params::algo_params(const data_array& ex, const data_array& predic,
        value_type threshold) :
        predictions(predic)/*, partitions(ex.size(), 0)*/
{
    condition_pos = init(ex, partitions, threshold);
}

ROC::algo_params::algo_params(const std::vector<int>& ex,
        const data_array& p, value_type threshold) :
        predictions(p)/*, partitions(ex.size(), 0)*/
{
    condition_pos = init(ex, partitions, threshold);
}

template<typename T>
bool ROC::calc(const std::vector<T>& expected,
            const data_array& predict)
{
    if (!expected.empty() && expected.size() == predict.size()) {
        value_type mean = 0.f;
        results.rmse = calculateRmse(expected, predict, mean);
        algo_params data4calc{expected, predict, mean};
        if (data4calc.condition_pos != 0) {
            std::tie(results.area, results.accuracy, results.sensitivity, results.fall_out) =
                do_calculation(std::move(data4calc));
            return !results.sensitivity.empty();
        }
    }
    return false;
}

bool ROC::calculate(const data_array& expected,
                    const data_array& predict)
{
    return calc(expected, predict);
}

bool ROC::classes_calculate(const std::vector<int>& expected,
                const data_array& predict)
{
    return calc(expected, predict);
}

bool ROC::classes_calculate(const data_array& expected,
                const data_array& predict)
{
    return calc(expected, predict);
}

std::ostream& operator << (std::ostream& os, const ROC& roc)
{
    os<<"area: "<<std::fixed<<roc.results.area<<" RMS error: "<<roc.results.rmse<<
        " accuracy: "<<roc.results.accuracy;
    os<<"sensitivity   fall_out\n";
    boost::transform(roc.results.sensitivity, roc.results.fall_out,
                    std::ostream_iterator<std::string>(os, "\n"), [](auto s, auto f) {
                        return std::to_string(s) + "      " + std::to_string(f);
                    }
            );
    return os;
}

}   // end of namespace exprs


//
// Created by Alex Beccaro on 07/01/18.
//

#include "problem48.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <generics.hpp>

using std::vector;
using boost::multiprecision::cpp_int;
using generics::digits;

namespace problems {
    vector<uint32_t> problem48::solve(uint32_t ub) {
        cpp_int sum = 1;

        for (uint32_t i = 2; i <= ub; i++)
            sum += pow((cpp_int) i, i);

        vector<uint32_t> result = digits<uint64_t>((uint64_t) (sum % 10000000000));
        result.insert(result.begin(), 10 - result.size(), 0);
        return result;
    }
}


// Copyright (c) 2020 [Yihong Jian]. All rights reserved.

#include <mylibrary/matrixsolver.h>
#include "mylibrary/util.h"
#include <Eigenvalues>

// Taken from numcpp reference
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> EigenIntMatrix;
typedef Eigen::Map<EigenIntMatrix> EigenMatrixMap;

namespace matrixsolver {

    string Rref(const string& input) {
        // Convert to 2d array and solve
        vector<vector<double>> mat;
        try {
            mat = util::StringTo2dVec(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }
        size_t rank = MatReducer(mat);

        // Strech out 2dvec to a string
        string out;
        for (const auto& r:mat) {
            out += "[";
            for (const auto& c:r) {
                out += to_string(c);
                out += ", ";
            }
            out += "]\n";
        }
        return "Rank is " + to_string(rank)
               + "\nReduced Row Echelon Form is\n" + out;
    }

    // Took this from https://github.com/yicheng-w/acm-icpc-notebook/blob/master/general-algorithm/rref.cpp
    int MatReducer(vector<vector<double>>& mat) {
        int num_row = mat.size();
        int num_col = mat[0].size();
        int row = 0;
        for (int col = 0; col < num_col && row < num_row; col++) {
            // Find pivot rows, checking if next n-rows are greater than current
            // row on leading index
            int j = row;
            for (int i = row + 1; i < num_row; i++) {
                if (fabs(mat[i][col]) > fabs(mat[j][col]))
                    j = i;
            }
            if (fabs(mat[j][col]) < 1e-10)
                continue;
            swap(mat[j], mat[row]);

            // Normalize each row based with 1
            // subtract previous row.
            double s = 1.0 / mat[row][col];
            for (int j1 = 0; j1 < num_col; j1++) mat[row][j1] *= s;
            for (int i = 0; i < num_row; i++)
                if (i != row) {
                    double t = mat[i][col];
                    for (int j2 = 0; j2 < num_col; j2++) {
                        mat[i][j2] -= t * mat[row][j2];
                    }
                }
            row++;
        }
        return row;
    }

    string LUDecomp(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        try {
            // LUDecomp may encounter error such as rectangular matrix
            // So we just catch whatever and print it out.
            auto lu_res = linalg::lu_decomposition(mat);
            string l = util::MatToString(get<0>(lu_res));
            string u = util::MatToString(get<1>(lu_res));
            return "L is:\n" + l + "U is\n" + u;
        } catch (exception e) {
            return e.what();
        }
    }

    string Det(const string& input) {
        vector<vector<double>> mat;
        try {
            mat = util::StringTo2dVec(input);
        } catch (exception e) {
            return e.what();
        }

        size_t n = mat.size();
        if (n != mat[0].size())
            return "Determinant Requires Square Matrix";

        double det = 1.0;

        for (int i = 0; i < n; ++i) {
            double pivotElement = mat[i][i];
            int pivotRow = i;
            for (int row = i + 1; row < n; ++row) {
                if (std::abs(mat[row][i]) > std::abs(pivotElement)) {
                    pivotElement = mat[row][i];
                    pivotRow = row;
                }
            }
            if (pivotElement == 0.0) {
                det = 0.0;
                break;
            }
            if (pivotRow != i) {
                mat[i].swap(mat[pivotRow]);
                det *= -1.0;
            }
            det *= pivotElement;

            for (int row = i + 1; row < n; ++row) {
                for (int col = i + 1; col < n; ++col) {
                    mat[row][col] -= mat[row][i] * mat[i][col] / pivotElement;
                }
            }
        }

        return "Determinant is " + to_string(det);
    }

    string Eig(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        if (mat.numCols() != mat.numRows())
            return "Can't perform eigen calculation with non-square matrix";
        // Map numcpp array to Eigen Matrix
        auto eigen_mat = EigenMatrixMap(mat.data(), mat.numRows(), mat.numCols());
        // Retrieve Eigen values
        Eigen::EigenSolver<Eigen::MatrixXd> es(eigen_mat);
        Eigen::VectorXd eigen_values = es.eigenvalues().real();
        Eigen::MatrixXd eigen_vectors = es.eigenvectors().real();

        // Map eigen matrix to string
        ostringstream val;
        val << eigen_values;
        ostringstream vec;
        vec << eigen_vectors;

        return "CAUTION, complex eigenvalue caused UNDEFINED behavior\n"
               "Eigenvalues are\n" + val.str() +
               "\nEigenvectors(in columns) are\n" + vec.str();
    }

    string SVD(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        nc::NdArray<double> u;
        nc::NdArray<double> s;
        nc::NdArray<double> vt;
        try {
            linalg::svd(mat, u, s, vt);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        return "U is:\n" + util::MatToString(u)
               + "S is\n" + util::MatToString(s)
               + "V-Transpose is\n" + util::MatToString(vt);
    }

    string Inv(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        try {
            return "Inverse is\n" + util::MatToString(linalg::inv(mat));
        } catch (exception e) {
            return e.what();
        }
    }

    pair<string, string> PowerIter(const string& input, const string& init_guess) {
        NdArray<double> mat;
        NdArray<double> vec;
        try {
            mat = util::StringToMat(input).astype<double>();
            vec = util::StringToMat(init_guess, true).astype<double>();
        } catch (exception e) {
            return make_pair(e.what(), init_guess);
        }

        try {
            // Normalize input to prevent overflow
            // Broadcasting doesn't work too well between arrays, so
            // I extracted the double contained in array for broadcasting
            NdArray<double> vec_normed = vec / vec.norm()(0, 0);
            NdArray<double> y = mat.dot(vec_normed);
            NdArray<double> res = y / y.norm()(0, 0);
            return make_pair("Initial guess after 1 iteration is\n" + util::MatToString(res),
                             util::VecToLine(res));
        } catch (exception e) {
            return make_pair(e.what(), init_guess);
        }
    }

    string LstSq(const string& input, const string& init_guess) {
        NdArray<double> A;
        // Assume b is only one column
        NdArray<double> b;

        try {
            A = util::StringToMat(input);
            b = util::StringToMat(init_guess, true);
        } catch (exception e) {
            return e.what();
        }

        if (A.numRows() != b.numRows())
            return "For Ax = b, rows of A and b does not match";

        try {
            return util::MatToString(nc::linalg::lstsq(A, b));
        } catch (exception e) {
            return e.what();
        }
    }

}  // namespace matrixsolver


#include <iostream>
#include <limits>

#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <unsupported/Eigen/NonLinearOptimization>

#include <opencv2/calib3d/calib3d.hpp>
#include <sys/stat.h>

#include "Optimization.h"
#include "math.hh"

bool translation_gauss_newton(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& image_pts,
                              const Eigen::Matrix3d &K, const Eigen::Quaterniond& Q, Eigen::Vector3d &T,
                              double& residual, int& iter)
//----------------------------------------------------------------------------------------------------------------------
{
   double tx = T[0], ty = T[1], tz = T[2];
   const Eigen::Matrix3d KI = K.inverse();
   size_t m = std::min(world_pts.size(), image_pts.size());
   std::vector<Eigen::Quaterniond> world_quaternions;
   std::vector<Eigen::Vector3d> image_rays;
   const Eigen::Quaterniond QI = Q.inverse();
   for (size_t row = 0; row < m; row++)
   {
      cv::Point2d &pt = const_cast<cv::Point2d &>(image_pts[row]);
      Eigen::Vector3d pt2d = KI * Eigen::Vector3d(pt.x, pt.y, 1);
      pt2d /= pt2d[2];
      image_rays.push_back(pt2d);
      const cv::Point3d pt3d = world_pts[row];
      const Eigen::Quaterniond QR = Q * Eigen::Quaterniond(0, pt3d.x, pt3d.y, pt3d.z) * QI;
      world_quaternions.push_back(QR);
   }
   iter = 0;
   double prev_min_residual = std::numeric_limits<double>::max();
   double eps = 0.0000001;
   do
   {
      Eigen::MatrixXd J(m, 3);
//      Eigen::MatrixXd r(m, 3);
      Eigen::MatrixXd r(m, 1);
      double min_residual = std::numeric_limits<double>::max();
      for (size_t row = 0; row < m; row++)
      {
         Eigen::Vector3d pt2d = image_rays[row];
         double u = pt2d[0], v = pt2d[1];
         const Eigen::Vector3d Rv = world_quaternions[row].vec();
         //  Eigen::Vector3d Rr = R*Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z);
         Eigen::Vector3d pt3d = Rv + Eigen::Vector3d(tx, ty, tz);
         pt3d /= pt3d[2];
         Eigen::Vector3d diff = pt3d - pt2d;
         residual = diff.dot(diff);
         if (residual < min_residual)
            min_residual = residual;
//         std::cout << "refine: " << pt3d.transpose() << " " << pt2d.transpose() << " " << diff.transpose() << " " << residual << std::endl;
         Eigen::Vector3d d;
         J.row(row) << 2 * (Rv[0] + tx - u), 2 * (Rv[1] + ty - v), 2 * (Rv[2] + tz - 1);
         r.row(row) << residual;
      }
//      std::cout << "minmax " << min_residual << " " << (min_residual - prev_min_residual) <<  std::endl;
      if (min_residual > prev_min_residual) break;
      prev_min_residual = min_residual;
      auto Jt = J.transpose();
//      std::cout << "JTr: " << std::endl << (Jt * r) << std::endl << "==================== " << std::endl;

      //auto llt = (Jt * J).ldlt();
      auto llt = (Jt * J).llt();
      double dx, dy, dz;
      if (llt.info() == Eigen::Success)
      {
         auto delta =  llt.solve(Jt * r * (-1.0));
         dx = delta(0, 0), dy = delta(1, 0), dz = delta(2, 0);
//         std::cout << delta << std::endl;
      }
      else
      {
         auto JtJI = (Jt*J).inverse();
         auto dd = -(JtJI*Jt);
         auto delta = dd*r;
         dx = delta(0, 0), dy = delta(1, 0), dz = delta(2, 0);
      }
      if ( (mut::near_zero(dx, eps)) && (mut::near_zero(dy, eps)) && (mut::near_zero(dz, eps)) ) break;
      tx += dx;
      ty += dy;
      tz += dz;
   } while (iter++ < 200);
   if ( (iter > 1) && (iter < 100) )
   {
      T[0] = tx; T[1] = ty; T[2] = tz;
      return true;
   }
   return false;
}

bool translation_levenberg_marquardt3d(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& image_pts,
                                     const Eigen::Matrix3d &K, const Eigen::Quaterniond& Q, Eigen::Vector3d &T,
                                     int& iterations)
//-----------------------------------------------------------------------------------------------------------------------
{
   TranslationLevenbergMarquardt3D functor(world_pts, image_pts, K, Q);
   Eigen::LevenbergMarquardt<TranslationLevenbergMarquardt3D, double> lm(functor);
   Eigen::VectorXd x(3);
   x << T[0], T[1], T[2];
   iterations = 0;
   Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeInit(x);
   if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      return false;
   do
   {
      status = lm.minimizeOneStep(x);
      if (status == Eigen::LevenbergMarquardtSpace::Running)
         iterations++;
   } while (status == Eigen::LevenbergMarquardtSpace::Running);
   if (iterations > 1)
   {
      T = x;
      return true;
   }
   return false;
}

bool translation_levenberg_marquardt2d_depth(const std::vector<cv::Point3d>& world_pts,
                                             const std::vector<cv::Point3d>& image_pts, const Eigen::Matrix3d &K,
                                             const Eigen::Quaterniond& Q, Eigen::Vector3d &T, const double depth,
                                             int& iterations)
//--------------------------------------------------------------------------------------------------------------
{
   TranslationLevenbergMarquardt2DDepth functor(world_pts, image_pts, K, Q, depth);
   Eigen::LevenbergMarquardt<TranslationLevenbergMarquardt2DDepth, double> lm(functor);
   Eigen::VectorXd x(3);
   x << T[0], T[1], T[2];
   iterations = 0;
   Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeInit(x);
   if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      return false;
   do
   {
      status = lm.minimizeOneStep(x);
      if (status == Eigen::LevenbergMarquardtSpace::Running)
         iterations++;
   } while (status == Eigen::LevenbergMarquardtSpace::Running);
   if (iterations > 1)
   {
      T = x;
      return true;
   }
   return false;
}


// TinyVector<T,N> DAXPY benchmark

//#define BZ_DISABLE_KCC_COPY_PROPAGATION_KLUDGE

#include <blitz/array.h>
#include <blitz/timer.h>
#include <random/uniform.h>

BZ_USING_NAMESPACE(blitz)

ranlib::Uniform<double> rnd;

template<class T>
void optimizationSink(T&);

template<int N_rank>
void tinyDAXPYBenchmark(TinyVector<double,N_rank>, int iters, double a)
{
    Timer timer;

    TinyVector<double,N_rank> ta, tb, tc, td, te, tf, tg, th, ti, tj;
    for (int i=0; i < N_rank; ++i)
    {
        ta[i] = rnd.random()+1;
        tb[i] = rnd.random()+1;
        tc[i] = rnd.random()+1;
        td[i] = rnd.random()+1;
        te[i] = rnd.random()+1;
        tf[i] = rnd.random()+1;
        tg[i] = rnd.random()+1;
        th[i] = rnd.random()+1;
        ti[i] = rnd.random()+1;
        tj[i] = rnd.random()+1;
    }

    double b = -a;

    double numFlops = 0;

    if (N_rank < 20)
    {
      timer.start();
      for (int i=0; i < iters; ++i)
      {
        ta += a * tb;
        tc += a * td;
        te += a * tf;
        tg += a * th;
        ti += a * tj;
        tb += b * ta;
        td += b * tc;
        tf += b * te;
        th += b * tg;
        tj += b * ti;
        ta += a * tb;
        tc += a * td;
        te += a * tf;
        tg += a * th;
        ti += a * tj;
        tb += b * ta;
        td += b * tc;
        tf += b * te;
        th += b * tg;
        tj += b * ti;
      }
      timer.stop();
      numFlops = 40.0 * N_rank * double(iters);
    }
    else {
      timer.start();
      for (int i=0; i < iters; ++i)
      {
        ta += a * tb;
        tb += b * ta;
      }
      timer.stop();
      numFlops = 4.0 * N_rank * double(iters);
    }

    optimizationSink(ta);
    optimizationSink(tb);
    optimizationSink(tc);
    optimizationSink(td);
    optimizationSink(te);
    optimizationSink(tf);
    optimizationSink(tg);
    optimizationSink(th);
    optimizationSink(ti);
    optimizationSink(tj);

    timer.stop();
    float Gflops = numFlops / (1e9*timer.elapsed());

    if (iters > 1)
    {
    cout << setw(5) << N_rank << '\t' << Gflops << endl;
    }
}

double a = 0.3429843;

template<class T>
void optimizationSink(T&)
{
}

int main()
{
    cout << "TinyVector<double,N> DAXPY benchmark" << endl
         << setw(5) << "N" << '\t' << "Gflops/" << Timer::indep_var() << endl;
    tinyDAXPYBenchmark(TinyVector<double,1>(), 800000, a);
    tinyDAXPYBenchmark(TinyVector<double,2>(), 800000, a);
    tinyDAXPYBenchmark(TinyVector<double,3>(), 800000, a);
    tinyDAXPYBenchmark(TinyVector<double,4>(), 700000, a);
    tinyDAXPYBenchmark(TinyVector<double,5>(), 600000, a);
    tinyDAXPYBenchmark(TinyVector<double,6>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,7>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,8>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,9>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,10>(), 500000, a);

    return 0;
}


#ifdef MEX

#include <igl/avg_edge_length.h>
#include <igl/edges.h>
#include <igl/copyleft/cgal/wire_mesh.h>
#include <igl/matlab/validate_arg.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/C_STR.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

#include <cstring>

void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & WV,
  Eigen::MatrixXi & WE,
  double & th,
  int & poly_size,
  bool & solid)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  mexErrMsgTxt(nrhs >= 2, "The number of input arguments must be >=2.");

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3,
    "Mesh vertex list must be #V by 3 list of vertex positions");

  parse_rhs_double(prhs,WV);
  parse_rhs_index(prhs+1,WE);
  if(WE.cols()>2)
  {
    igl::edges(MatrixXi(WE),WE);
  }

  // defaults
  // Thickness
  th = 0.1*igl::avg_edge_length(WV,WE);
  // Size of extrusion polygon
  poly_size = 4;
  solid = true;

  {
    int i = 2;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Thickness",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        th = (double)*mxGetPr(prhs[++i]);
      }else if(strcmp("PolySize",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        poly_size = (int)*mxGetPr(prhs[++i]);
      }else if(strcmp("Solid",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        solid = (bool)*mxGetPr(prhs[++i]);
      }else
      {
        mexErrMsgTxt(false,"Unknown parameter");
      }
      i++;
    }
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;
  using namespace igl::copyleft::cgal;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd WV,V;
  MatrixXi WE,F;
  VectorXi J;
  double th;
  int poly_size;
  bool solid;
  parse_rhs(
    nrhs,prhs,
    WV,WE,
    th,poly_size,solid);
  wire_mesh(WV,WE,th,poly_size,solid,V,F,J);
  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 3:
    {
      prepare_lhs_index(J,plhs+2);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_index(F,plhs+1);
      // Fall through
    }
    case 1:
    {
      prepare_lhs_double(V,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}

#endif


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "bottleneck distance"
#include <boost/test/unit_test.hpp>

#include <random>
#include <gudhi/Bottleneck.h>
#include <gudhi/Unitary_tests_utils.h>

using namespace Gudhi::persistence_diagram;

int n1 = 81;  // a natural number >0
int n2 = 180;  // a natural number >0
double upper_bound = 406.43;  // any real >0

std::uniform_real_distribution<double> unif(0., upper_bound);
std::default_random_engine re;
std::vector< std::pair<double, double> > v1, v2;

BOOST_AUTO_TEST_CASE(persistence_graph) {
  // Random construction
  for (int i = 0; i < n1; i++) {
    double a = unif(re);
    double b = unif(re);
    v1.emplace_back(std::min(a, b), std::max(a, b));
  }
  for (int i = 0; i < n2; i++) {
    double a = unif(re);
    double b = unif(re);
    v2.emplace_back(std::min(a, b), std::max(a, b));
  }
  Persistence_graph g(v1, v2, 0.);
  std::vector<double> d(g.sorted_distances());
  //
  BOOST_CHECK(!g.on_the_u_diagonal(n1 - 1));
  BOOST_CHECK(!g.on_the_u_diagonal(n1));
  BOOST_CHECK(!g.on_the_u_diagonal(n2 - 1));
  BOOST_CHECK(g.on_the_u_diagonal(n2));
  BOOST_CHECK(!g.on_the_v_diagonal(n1 - 1));
  BOOST_CHECK(g.on_the_v_diagonal(n1));
  BOOST_CHECK(g.on_the_v_diagonal(n2 - 1));
  BOOST_CHECK(g.on_the_v_diagonal(n2));
  //
  BOOST_CHECK(g.corresponding_point_in_u(0) == n2);
  BOOST_CHECK(g.corresponding_point_in_u(n1) == 0);
  BOOST_CHECK(g.corresponding_point_in_v(0) == n1);
  BOOST_CHECK(g.corresponding_point_in_v(n2) == 0);
  //
  BOOST_CHECK(g.size() == (n1 + n2));
  //
  BOOST_CHECK((int) d.size() == (n1 + n2)*(n1 + n2) + n1 + n2 + 1);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(0, 0))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(0, n1 - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(0, n1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(0, n2 - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(0, n2))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(0, (n1 + n2) - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(n1, 0))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(n1, n1 - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(n1, n1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(n1, n2 - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(n1, n2))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance(n1, (n1 + n2) - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance((n1 + n2) - 1, 0))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance((n1 + n2) - 1, n1 - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance((n1 + n2) - 1, n1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance((n1 + n2) - 1, n2 - 1))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance((n1 + n2) - 1, n2))) > 0);
  BOOST_CHECK(std::count(d.begin(), d.end(), GUDHI_PROTECT_FLOAT(g.distance((n1 + n2) - 1, (n1 + n2) - 1))) > 0);
}

BOOST_AUTO_TEST_CASE(neighbors_finder) {
  Persistence_graph g(v1, v2, 0.);
  Neighbors_finder nf(g, 1.);
  for (int v_point_index = 1; v_point_index < ((n2 + n1)*9 / 10); v_point_index += 2)
    nf.add(v_point_index);
  //
  int v_point_index_1 = nf.pull_near(n2 / 2);
  BOOST_CHECK((v_point_index_1 == -1) || (g.distance(n2 / 2, v_point_index_1) <= 1.));
  std::vector<int> l = nf.pull_all_near(n2 / 2);
  bool v = true;
  for (auto it = l.cbegin(); it != l.cend(); ++it)
    v = v && (g.distance(n2 / 2, *it) > 1.);
  BOOST_CHECK(v);
  int v_point_index_2 = nf.pull_near(n2 / 2);
  BOOST_CHECK(v_point_index_2 == -1);
}

BOOST_AUTO_TEST_CASE(layered_neighbors_finder) {
  Persistence_graph g(v1, v2, 0.);
  Layered_neighbors_finder lnf(g, 1.);
  for (int v_point_index = 1; v_point_index < ((n2 + n1)*9 / 10); v_point_index += 2)
    lnf.add(v_point_index, v_point_index % 7);
  //
  int v_point_index_1 = lnf.pull_near(n2 / 2, 6);
  BOOST_CHECK((v_point_index_1 == -1) || (g.distance(n2 / 2, v_point_index_1) <= 1.));
  int v_point_index_2 = lnf.pull_near(n2 / 2, 6);
  BOOST_CHECK(v_point_index_2 == -1);
  v_point_index_1 = lnf.pull_near(n2 / 2, 0);
  BOOST_CHECK((v_point_index_1 == -1) || (g.distance(n2 / 2, v_point_index_1) <= 1.));
  v_point_index_2 = lnf.pull_near(n2 / 2, 0);
  BOOST_CHECK(v_point_index_2 == -1);
}

BOOST_AUTO_TEST_CASE(graph_matching) {
  Persistence_graph g(v1, v2, 0.);
  Graph_matching m1(g);
  m1.set_r(0.);
  int e = 0;
  while (m1.multi_augment())
    ++e;
  BOOST_CHECK(e > 0);
  BOOST_CHECK(e <= 2 * sqrt(2 * (n1 + n2)));
  Graph_matching m2 = m1;
  BOOST_CHECK(!m2.multi_augment());
  m2.set_r(upper_bound);
  e = 0;
  while (m2.multi_augment())
    ++e;
  BOOST_CHECK(e <= 2 * sqrt(2 * (n1 + n2)));
  BOOST_CHECK(m2.perfect());
  BOOST_CHECK(!m1.perfect());
}

BOOST_AUTO_TEST_CASE(global) {
  std::uniform_real_distribution<double> unif1(0., upper_bound);
  std::uniform_real_distribution<double> unif2(upper_bound / 10000., upper_bound / 100.);
  std::default_random_engine re;
  std::vector< std::pair<double, double> > v1, v2;
  for (int i = 0; i < n1; i++) {
    double a = unif1(re);
    double b = unif1(re);
    double x = unif2(re);
    double y = unif2(re);
    v1.emplace_back(std::min(a, b), std::max(a, b));
    v2.emplace_back(std::min(a, b) + std::min(x, y), std::max(a, b) + std::max(x, y));
    if (i % 5 == 0)
      v1.emplace_back(std::min(a, b), std::min(a, b) + x);
    if (i % 3 == 0)
      v2.emplace_back(std::max(a, b), std::max(a, b) + y);
  }
  BOOST_CHECK(bottleneck_distance(v1, v2, 0.) <= upper_bound / 100.);
  BOOST_CHECK(bottleneck_distance(v1, v2, upper_bound / 10000.) <= upper_bound / 100. + upper_bound / 10000.);
  BOOST_CHECK(std::abs(bottleneck_distance(v1, v2, 0.) - bottleneck_distance(v1, v2, upper_bound / 10000.)) <= upper_bound / 10000.);

  std::vector< std::pair<double, double> > empty;
  std::vector< std::pair<double, double> > one = {{8, 10}};
  BOOST_CHECK(bottleneck_distance(empty, empty) == 0);
  BOOST_CHECK(bottleneck_distance(empty, one) == 1);
}


#ifndef HOPS_NORMALIZEPOLYTOPE_HPP
#define HOPS_NORMALIZEPOLYTOPE_HPP

#include <Eigen/Core>

namespace hops {

    /**
     * @brief Normalizes polytope defined by Ax < b
     * @tparam Derived1
     * @tparam Derived2
     * @param A Dense representation of A
     * @param b
     */
    template<typename Derived1, typename Derived2>
    void normalizePolytope(Eigen::MatrixBase<Derived1> &A, Eigen::MatrixBase<Derived2> &b) {
        for (int i = 0; i < A.rows(); ++i) {
            const double norm = A.row(i).template lpNorm<2>();
            A.row(i) /= norm;
            b(i) /= norm;
        }
    }
}

#endif //HOPS_NORMALIZEPOLYTOPE_HPP


// unit test file acosh.hpp for the special functions test suite

//  (C) Copyright Hubert Holin 2003.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <functional>
#include <iomanip>
#include <iostream>

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

#include <boost/test/unit_test.hpp>

template<typename T>
T    acosh_error_evaluator(T x)
{
    using    ::std::abs;
    using    ::std::sinh;
    using    ::std::cosh;

    using    ::std::numeric_limits;

    using    ::boost::math::acosh;

    static T const    epsilon = numeric_limits<float>::epsilon();

    T                y = cosh(x);
    T                z = acosh(y);

    T                absolute_error = abs(z-abs(x));
    T                relative_error = absolute_error*abs(sinh(x));
    T                scaled_error = relative_error/epsilon;

    return(scaled_error);
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(acosh_test, T)
{
    BOOST_MESSAGE("Testing acosh in the real domain for "
        << string_type_name<T>::_() << ".");

    for    (int i = 0; i <= 100; i++)
    {
        T    x = static_cast<T>(i-50)/static_cast<T>(5);

        BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
            (acosh_error_evaluator(x))
            (static_cast<T>(4)));
    }
}

void    acosh_manual_check()
{
    BOOST_MESSAGE(" ");
    BOOST_MESSAGE("acosh");

    for    (int i = 0; i <= 100; i++)
    {
        float        xf = static_cast<float>(i-50)/static_cast<float>(5);
        double       xd = static_cast<double>(i-50)/static_cast<double>(5);
        long double  xl =
                static_cast<long double>(i-50)/static_cast<long double>(5);

        BOOST_MESSAGE(  ::std::setw(15)
                     << acosh_error_evaluator(xf)
                     << ::std::setw(15)
                     << acosh_error_evaluator(xd)
                     << ::std::setw(15)
                     << acosh_error_evaluator(xl));
    }

    BOOST_MESSAGE(" ");
}


#include "CubicInterpolation/InterpolantBuilder.h"

#include <boost/math/differentiation/finite_difference.hpp>
#include <Eigen/Dense>

namespace cubic_splines {

/* template <> */
/* BicubicSplines */
/* InterpolantBuilder<BicubicSplines>::build(BicubicSplines::Definition const &def, */
/*                                           std::string save_path, */
/*                                           std::string filename) { */
/*   using boost::math::differentiation::finite_difference_derivative; */
/*   try { */
/*     return load(save_path, filename); */
/*   } catch (std::system_error const &ex) { */
/*     if (ex.code().value() != ENOENT) */
/*       throw ex; */
/*   } */
/*   auto x1nodes = def.axis[0]->required_nodes(); */
/*   auto x2nodes = def.axis[1]->required_nodes(); */
/*   auto data = BicubicSplines::RuntimeData(); */
/*   auto y = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto dydx1 = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto dydx2 = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto d2ydx1dx2 = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto func = [this, &def](double x1, double x2) { */
/*     return transform(def, x1, x2); */
/*   }; */
/*   for (size_t n1 = 0; n1 < x1nodes; ++n1) { */
/*     for (size_t n2 = 0; n2 < x2nodes; ++n2) { */
/*       auto x1 = def.axis[0]->back_transform(n1); */
/*       auto x2 = def.axis[1]->back_transform(n2); */

/*       auto dfdx1 = def.axis[0]->derive(x1); */
/*       auto dfdx2 = def.axis[1]->derive(x2); */

/*       y(n1, n2) = func(x1, x2); */
/*       dydx1(n1, n2) = */
/*           finite_difference_derivative( */
/*               [this, &func, x2](double x) { return func(x, x2); }, x1) * */
/*           dfdx1; */
/*       dydx2(n1, n2) = */
/*           finite_difference_derivative( */
/*               [this, &func, x1](double x) { return func(x1, x); }, x2) * */
/*           dfdx2; */
/*       d2ydx1dx2(n1, n2) = */
/*           finite_difference_derivative( */
/*               [this, &func, x1, x2, dfdx1, dfdx2](double x_1) { */
/*                 return finite_difference_derivative( */
/*                            [this, &func, x_1, dfdx2](double x_2) { */
/*                              return func(x_1, x_2); */
/*                            }, */
/*                            x2) * */
/*                        dfdx2; */
/*               }, */
/*               x1) * */
/*           dfdx1; */
/*     } */
/*   } */
/*   bool sucess = save(save_path, filename, y, dydx1, dydx2, d2ydx1dx2); */
/*   if (not sucess) */
/*     std::cout << "storage of tables have failed" << std::endl; */
/*   return BicubicSplines(y, dydx1, dydx2, d2ydx1dx2); */
/* } */

/* template <> */
/* CubicSplines */
/* InterpolantBuilder<CubicSplines>::build(CubicSplines::Definition const &def, */
/*                                         std::string path, */
/*                                         std::string filename) { */
/*   using boost::math::differentiation::finite_difference_derivative; */
/*   try { */
/*     return load(path, filename); */
/*   } catch (std::system_error const &ex) { */
/*     if (ex.code().value() != ENOENT) */
/*       throw ex; */
/*   } */
/*   auto y = std::vector<double>(def.axis->required_nodes()); */
/*   auto func = [this, &def](double x) { return transform(def, x); }; */
/*   /1* auto func = [&def](double x) { *1/ */
/*   /1*   auto fx = def.f(x); *1/ */
/*   /1*   if (def.f_trafo) *1/ */
/*   /1*     fx = def.f_trafo->transform(fx); *1/ */
/*   /1*   return fx; *1/ */
/*   /1* }; *1/ */
/*   for (size_t n = 0; n < y.size(); ++n) */
/*     y[n] = func(def.axis->back_transform(n)); */
/*   auto low = def.axis->back_transform(0); */
/*   auto low_lim_derivate = */
/*       finite_difference_derivative(func, low) * def.axis->derive(low); */
/*   auto up = def.axis->back_transform(y.size() - 1); */
/*   auto up_lim_derivate = finite_difference_derivative( */
/*                              func, def.axis->back_transform(y.size() - 1)) * */
/*                          def.axis->derive(up); */
/*   bool sucess = save(path, filename, y, low_lim_derivate, up_lim_derivate); */
/*   if (not sucess) */
/*     std::cout << "storage of tables have failed" << std::endl; */
/*   return CubicSplines(y, low_lim_derivate, up_lim_derivate); */
/* } */

} // namespace cubic_splines


//////////////////////////////////////////////////////////////////////////////
// monomials_horner::policy::multi_factorial.hpp                            //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MONOMIALS_HORNER_POLICY_MULTI_FACTORIAL_HPP_ER_2009
#define BOOST_MONOMIALS_HORNER_POLICY_MULTI_FACTORIAL_HPP_ER_2009
#include <boost/math/special_functions/factorials.hpp>

namespace boost{
namespace monomials_horner{

    // (a,b,c) --> a! b! c!
    struct multi_factorial{
        typedef unsigned                      result_type;
        typedef unsigned                      first_argument_type;
        typedef unsigned                      second_argument_type;
        multi_factorial(){}
        result_type operator()(
            first_argument_type a1,
            second_argument_type a2
        )const{
            return a1 * static_cast<result_type>(math::factorial<double>(a2));
        }
        static result_type initial_value;
    };
    multi_factorial::result_type multi_factorial:: initial_value
        = static_cast<result_type>(1);

}// monomials_horner
}// boost

#endif


#include <armadillo>
#include <iostream>
#include <ostream>
#include <target/odesolver.hpp>
#include <target/utils.hpp>

using arma::vec;
using arma::mat;
using std::cout;
using std::endl;

vec dy(const vec &input,  // time (first element) and input variables
       const vec &x,      // state variables
       const vec &par) {
  return par(0) + par(1)*x;
}

int main(int argc, char **argv) {
    cout << target::BLUE << "RK4 test\n\n";

    target::RK4 MyODE(dy);
    vec t = arma::linspace(0, 2, 20);
    vec y0 = arma::zeros(1);
    vec par = { 1.0, 1.0 };
    vec y = MyODE.solve(t, y0, par);
    cout << y << endl;
    std::cout << target::COL_RESET;

    mat ty = arma::join_horiz(t, y);
    ty.save("y.csv", arma::csv_ascii);

    return 0;
}


#include <iostream>
#include <stdio.h>

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

//#include <boost/array.hpp>
//#include <boost/numeric/odeint.hpp>

//using namespace boost::numeric;
//
//#include "filters.h"
//
//namespace syllo {
//
//     Kalman::Kalman()
//     {
//     }
//
//     Kalman::Kalman(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B, const Eigen::VectorXf &C, const Eigen::MatrixXf &R, const Eigen::MatrixXf &Q)
//     {
//	  setModel(A, B, C, R, Q);
//     }
//
//     int Kalman::setModel(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B, const Eigen::VectorXf &C, const Eigen::MatrixXf &R, const Eigen::MatrixXf &Q)
//     {
//	  this->A = A;
//	  this->B = B;
//	  this->C = C;
//	  this->R = R;
//	  this->Q = Q;
//	  eye = Eigen::MatrixXf::Identity(A.rows(), A.cols());
//
//	  return 0;
//     }
//
//     int Kalman::init(const Eigen::VectorXf &mu0, const Eigen::MatrixXf &covar0)
//     {
//	  this->mu = mu0;
//	  this->covar = covar0;
//	  return 0;
//     }
//
//     int Kalman::step(Eigen::VectorXf &mu_prev, Eigen::MatrixXf &covar_prev, const Eigen::VectorXf &u)
//     {
//	  Eigen::VectorXf mu_dx;
//
//	  mu_dx = A*mu_prev + B*u;
//
//	  mu_prev += mu_dx;
//
//	  covar = A*covar_prev*A.transpose() + R;
//	  //K = covar*C.transpose() * (C*covar*C.transpose() + Q).inverse();
//	  ////mu = mu + K*(z-C*mu);
//	  //covar = (eye - K*C) * covar;
//
//
//
//	  return 0;
//     }
//
//     int Kalman::step(Eigen::VectorXf &mu_prev, Eigen::MatrixXf &covar_prev, const Eigen::VectorXf &u, const Eigen::VectorXf &z)
//     {
//	  mu = A*mu_prev + B*u;
//	  covar = A*covar_prev*A.transpose() + R;
//
//	  std::cout << "covar: \n" << covar << std::endl;
//	  std::cout << "C: \n" << C << std::endl;
//	  std::cout << "C': \n" << C.transpose() << std::endl;
//	  std::cout << "Q: \n" << Q << std::endl;
//
//	  //K = covar*C.transpose() * (C*covar*C.transpose() + Q).inverse();
//
//          K = covar*C * (C.transpose()*covar*C + Q).inverse();
//
//	  std::cout << "mu: \n" <<  mu << std::endl;
//	  std::cout << "K: \n" <<  K << std::endl;
//	  std::cout << "z: \n" <<  z << std::endl;
//	  std::cout << "C: \n" <<  C << std::endl;
//
//	  //mu = mu + K*(z-C*mu);
//	  //mu = mu + K*(z-C*mu);
//	  z-C.transpose()*mu;
//
//          //covar = (eye - K*C) * covar;
//
//	  return 0;
//     }
//
//     Eigen::VectorXf Kalman::getMu()
//     {
//	  return mu;
//     }
//
//     Eigen::MatrixXf Kalman::getCovar()
//     {
//	  return covar;
//     }
//}


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

#include "ekf/ekf.h"
#include "ekf/utils.h"

#include <Eigen/Core>
#include <Eigen/LU>

namespace ekf {

void Predict(Ekf& ekf, double dt) {
    auto new_state = ekf.state;
    new_state(0) = ekf.state(3) * dt * std::cos(ekf.state(2)) + ekf.state(0);
    new_state(1) = ekf.state(3) * dt * std::sin(ekf.state(2)) + ekf.state(1);
    new_state(2) = wrapAngle(ekf.state(4) * dt + ekf.state(2));

    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(5, 5);
    // x
    jacobian(0,0) = 1;
    jacobian(0,2) = -ekf.state(3) * dt * std::sin(ekf.state(2));
    jacobian(0,3) = dt * std::cos(ekf.state(2));

    // y
    jacobian(1,1) = 1;
    jacobian(1,2) = ekf.state(3) * dt * std::cos(ekf.state(2));
    jacobian(1,3) = dt * std::sin(ekf.state(2));

    // theta
    jacobian(2,2) = 1;
    jacobian(2,4) = dt;

    // v
    jacobian(3,3) = 1;

    // w
    jacobian(4,4) = 1;

    Eigen::MatrixXd process_noise = Eigen::MatrixXd::Identity(5, 5);
    process_noise = process_noise * 0.1;

    ekf.state = new_state;
    ekf.covariance = jacobian * ekf.covariance * jacobian.transpose() + process_noise;
}

bool Update(Ekf& ekf,
        const Eigen::VectorXd& z,
        const Eigen::MatrixXd& H,
        const Eigen::MatrixXd& R) {
    Eigen::VectorXd y = z - H * ekf.state;
    Eigen::MatrixXd S = H * ekf.covariance * H.transpose() + R;

    Eigen::FullPivLU<Eigen::MatrixXd> lu(S);
    if (!lu.isInvertible()) {
        return false;
    }

    Eigen::MatrixXd K = ekf.covariance * H.transpose() * S.inverse();
    ekf.state = ekf.state + K * y;
    Eigen::MatrixXd KH = K * H;
    ekf.covariance = (Eigen::MatrixXd::Identity(KH.rows(), KH.cols()) - KH) * ekf.covariance;
    return true;
}
}


/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

#include <Eigen/Dense>
#include <array>
#include <memory>

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;
  //====================
  // Your code goes here
  //====================
  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;
  //====================
  // Your code goes here
  //====================
  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
  double I = 0;
  //====================
  // Your code goes here
  //====================
  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;
  //====================
  // Your code goes here
  //====================
  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    //====================
    // Your code goes here
    //====================
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    //====================
    // Your code goes here
    // assign the coefficients of mu to the correct entries of zeta, use
    // the previous subproblem 2-9.a
    //====================
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


#ifndef DART_REALTIME_OBS_LOG
#define DART_REALTIME_OBS_LOG

#include <vector>

#include <Eigen/Dense>

#include "dart/math/MathTypes.hpp"
namespace dart {
namespace realtime {

struct Observation
{
  long time;
  Eigen::VectorXs pos;
  Eigen::VectorXs vel;

  Observation(long time, Eigen::VectorXs pos, Eigen::VectorXs vel);
};

class ObservationLog
{
public:
  ObservationLog(
      long startTime,
      Eigen::VectorXs initialPos,
      Eigen::VectorXs initialVel,
      Eigen::VectorXs initialMass);

  void observe(
      long time,
      Eigen::VectorXs pos,
      Eigen::VectorXs vel,
      Eigen::VectorXs mass);

  Observation getClosestObservationBefore(long time);

  Eigen::VectorXs getMass();

  void discardBefore(long time);

protected:
  int mDofs;
  int mMassDim;
  std::vector<Observation> mObservations;
  Eigen::VectorXs mMass;
};

} // namespace realtime
} // namespace dart

#endif


#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "tools.hpp"

using namespace Eigen;
using namespace std;

// For converting back and forth between radians and degrees.
double pi() { return M_PI; }

double deg2rad(double x) { return x * pi() / 180.; }
double rad2deg(double x) { return x * 180. / pi(); }

double distance(double x1, double y1, double x2, double y2) {
	return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}

double norm(double x, double y) {
	return sqrt(x*x + y*y);
}

double norm(double x, double y, double z) {
	return sqrt(x*x + y*y + z*z);
}

double mph2mps(double mph) {
	return mph / 2.2369362920544;
}

double mps2mph(double mps) {
	return mps * 2.2369362920544;
}

// Evaluate a polynomial.
vector<double> polyeval(vector<double> &coeffs, vector<double> &x)
{
    vector<double> result(x.size());
    for (int j = 0; j < x.size(); j++)
    {
        result[j] = 0;
        for (int i = 0; i < coeffs.size(); i++) {
            result[j] += coeffs[i] * pow(x[j], i);
        }
    }
    return result;
}

double polyeval(vector<double> &coeffs, double x)
{
    double result = 0;
    for (int i = 0; i < coeffs.size(); i++) {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

double polyeval(Eigen::VectorXd &coeffs, double x)
{
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++) {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

vector<double> polyfit(vector<double> &xvals, vector<double> &yvals, int order) {
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::VectorXd xvals_eig = Eigen::VectorXd::Map(xvals.data(), xvals.size());
    Eigen::VectorXd yvals_eig = Eigen::VectorXd::Map(yvals.data(), yvals.size());
	Eigen::VectorXd result_eig = polyfit(xvals_eig, yvals_eig, order);
	vector<double> result(result_eig.data(), result_eig.data() + result_eig.size());
    return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd &xvals, Eigen::VectorXd &yvals, int order) {
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);

    for (int i = 0; i < xvals.size(); i++) {
        A(i, 0) = 1.0;
    }

    for (int j = 0; j < xvals.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * xvals(j);
        }
    }

    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}

vector<double> polyfit_wp(int wp_start, int wp_stop, int order,
                          vector<double> &map_x, vector<double> &map_y)
{
    assert(map_x.size() == map_y.size());
    int wp_count = wp_stop - wp_start;
    Eigen::VectorXd xvals_eig(wp_count);
    Eigen::VectorXd yvals_eig(wp_count);
    Eigen::MatrixXd A(wp_count, order+1);
    // make sure indicies for map_x and map_y wrap around map size!
    wp_start = wp_start % map_x.size();
    wp_stop  = wp_stop  % map_x.size();

    for (int idx = 0; idx < wp_count; idx++) {
        xvals_eig(idx) = map_x[wp_start+idx];
        yvals_eig(idx) = map_y[wp_start+idx];
    }

    for (int i = 0; i < wp_count; i++) {
        A(i, 0) = 1.0;
    }

    for (int j = 0; j < wp_count; j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * xvals_eig(j);
        }
    }
    /*
    IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    cout << "A: " << endl << A.format(CleanFmt) << endl;
    */
    auto Q = A.householderQr();
    VectorXd result_eig = Q.solve(yvals_eig);
    vector<double> result(result_eig.data(), result_eig.data() + result_eig.size());
    return result;
}

int getLane(const double d, const double laneWidth) {
    for (int lane = 0; lane <= 2; lane++) {
        if (d > laneWidth*lane && d <= laneWidth*(lane+1)) {
            return lane;
        }
    }
    cout << "couldn't find lane for d=" << d << endl;
    return 0;
}

double getLaneOffsetD(const int lane_number, const double laneWidth) {
    return (laneWidth*lane_number) + 2.0;
}

vector<double> JMT(vector< double> start, vector <double> end, double T)
{
    /*
    Calculate the Jerk Minimizing Trajectory that connects the initial state
    to the final state in time T.

    INPUTS

    start - the vehicles start location given as a length three array
            corresponding to initial values of [s, s_dot, s_double_dot]

    end   - the desired end state for vehicle. Like "start" this is a
            length three array.

    T     - The duration, in seconds, over which this maneuver should occur.

    OUTPUT
    an array of length 6, each value corresponding to a coefficent in the polynomial
    s(t) = a_0 + a_1 * t + a_2 * t**2 + a_3 * t**3 + a_4 * t**4 + a_5 * t**5

    */

    double T2, T3, T4, T5;
    T2 = T*T;
    T3 = T2*T;
    T4 = T3*T;
    T5 = T4*T;

    MatrixXd c(3,1);
    c << end[0] - (start[0] + start[1]*T + 0.5*start[2]*T2),
         end[1] - (start[1] + start[2]*T),
         end[2] -  start[2];

    MatrixXd A(3,3);
    A <<   T3,    T4,    T5,
         3*T2,  4*T3,  5*T4,
         6*T,  12*T2, 20*T3;

    MatrixXd b = A.inverse() * c;

    return {start[0], start[1], 0.5*start[2],
            b(0),     b(1),     b(2)};
}


/// @file
/// Tests that rotation conversion functions are inverses.

#include <cmath>
#include <iostream>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/math/cross_product.h"
#include "drake/math/normalize_vector.h"
#include "drake/math/quaternion.h"
#include "drake/math/rotation_matrix.h"

using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::AngleAxis;
using Eigen::AngleAxisd;
using Eigen::Quaternion;
using Eigen::Quaterniond;
using std::sin;
using std::cos;
using std::numeric_limits;

namespace drake {
namespace math {
namespace {

// kSweepSize is an even number, so that no samples are taken at zero. This
// test scales as O(N^4) in sweep size, so be cautious about turning it up!
const int kSweepSize = 6;

const double kEpsilon = numeric_limits<double>::epsilon();
const double kTolerance = 1.0E-12;

GTEST_TEST(EigenEulerAngleTest, MakeXYZRotation) {
  // Verify MakeXRotation(theta), MakeYRotation(theta), MakeZRotation(theta) is
  // the same as AngleAxis equivalents.
  const double theta = 0.1234567;  // Arbitrary angle.
  const RotationMatrixd Rx(RotationMatrixd::MakeXRotation(theta));
  const RotationMatrixd Ry(RotationMatrixd::MakeYRotation(theta));
  const RotationMatrixd Rz(RotationMatrixd::MakeZRotation(theta));
  const Quaterniond qx(Eigen::AngleAxisd(theta, Vector3d::UnitX()));
  const Quaterniond qy(Eigen::AngleAxisd(theta, Vector3d::UnitY()));
  const Quaterniond qz(Eigen::AngleAxisd(theta, Vector3d::UnitZ()));
  const double tolerance = 32 * kEpsilon;
  EXPECT_TRUE(Rx.IsNearlyEqualTo(RotationMatrixd(qx), tolerance));
  EXPECT_TRUE(Ry.IsNearlyEqualTo(RotationMatrixd(qy), tolerance));
  EXPECT_TRUE(Rz.IsNearlyEqualTo(RotationMatrixd(qz), tolerance));
}

GTEST_TEST(EigenEulerAngleTest, BodyXYZ) {
  // Verify ea = Eigen::eulerAngles(0, 1, 2) returns Euler angles about
  // Body-fixed x-y'-z'' axes by [ea(0), ea(1), ea(2)].
  const Vector3d input_angles(0.5, 0.4, 0.3);
  const Matrix3d bodyXYZ_rotmat =
      (RotationMatrix<double>::MakeXRotation(input_angles(0)) *
       RotationMatrix<double>::MakeYRotation(input_angles(1)) *
       RotationMatrix<double>::MakeZRotation(input_angles(2))).matrix();
  const Vector3d output_angles = bodyXYZ_rotmat.eulerAngles(0, 1, 2);
  // input_angles.isApprox(output_angles) is a valid test (rathan than
  // comparing the converted quaternions) since all the angles are between
  // 0 and PI/2.
  EXPECT_TRUE(input_angles.isApprox(output_angles));
}

GTEST_TEST(EigenEulerAngleTest, SpaceXYZ) {
  // Verify ea = Eigen::eulerAngles(2, 1, 0) returns Euler angles about
  // Body-fixed z-y'-x'' axes by [ea(0), ea(1), ea(2)].
  const Vector3d input_angles(0.5, 0.4, 0.3);
  const Matrix3d spaceXYZ_rotmat =
      (RotationMatrix<double>::MakeZRotation(input_angles(0)) *
       RotationMatrix<double>::MakeYRotation(input_angles(1)) *
       RotationMatrix<double>::MakeXRotation(input_angles(2))).matrix();
  const Vector3d output_angles = spaceXYZ_rotmat.eulerAngles(2, 1, 0);
  // input_angles.isApprox(output_angles) is a valid test (rathan than
  // comparing the converted quaternions) since all the angles are between
  // 0 and PI/2.
  EXPECT_TRUE(input_angles.isApprox(output_angles));
}

GTEST_TEST(EigenEulerAngleTest, BodyZYZ) {
  // Verify ea = Eigen::eulerAngles(2, 1, 0) returns Euler angles about
  // Body-fixed z-y'-z'' axes by [ea(0), ea(1), ea(2)].
  const Vector3d input_angles(0.5, 0.4, 0.3);
  const Matrix3d bodyZYZ_angles =
      (RotationMatrix<double>::MakeZRotation(input_angles(0)) *
       RotationMatrix<double>::MakeYRotation(input_angles(1)) *
       RotationMatrix<double>::MakeZRotation(input_angles(2))).matrix();
  const Vector3d output_angles = bodyZYZ_angles.eulerAngles(2, 1, 2);
  // input_angles.isApprox(output_angles) is a valid test (rathan than
  // comparing the converted quaternions) since all the angles are between
  // 0 and PI/2.
  EXPECT_TRUE(input_angles.isApprox(output_angles));
}

class RotationConversionTest : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  void SetUp() override {
    SetupRPYTestCases();
    SetupAngleAxisTestCases();
    SetupQuaternionTestCases();
    SetupRotationMatrixTestCases();
  }

  void SetupRPYTestCases() {
    // Set up a variety of specific tests for angles that may cause numerical
    // problems as well as a sweep of values to test general functionality.
    // Singularity issue associated with the second angle = pi/2
    // in Euler Body-fixed z-y'-x'' rotation sequence.
    // Singularity issue associated with the second angle = -pi/2
    // in Euler Body-fixed z-y'-x'' rotation sequence.
    // Singularity issue associated with the second angle close to pi/2
    // in Euler Body-fixed z-y'-x'' rotation sequence.
    // Singularity issue associated with the second angle close to -pi/2
    // in Euler Body-fixed z-y'-x'' rotation sequence.

    // pitch = pi/2
    const RollPitchYaw<double> pitch_half_pi(M_PI / 4, M_PI / 2, M_PI / 3);
    rpy_test_cases_.push_back(pitch_half_pi);

    // pitch = -pi/2
    const RollPitchYaw<double> pitch_neg_half_pi(M_PI / 4, -M_PI / 2, M_PI / 3);
    rpy_test_cases_.push_back(pitch_neg_half_pi);

    // pitch = 0.5*pi-eps
    const RollPitchYaw<double> pitch_near_half_piA(
        M_PI / 4, 0.5 * M_PI - kEpsilon, M_PI / 3);
    rpy_test_cases_.push_back(pitch_near_half_piA);

    // pitch = 0.5*pi-1.5*eps
    const RollPitchYaw<double> pitch_near_half_piB(
        M_PI / 4, 0.5 * M_PI - 1.5 * kEpsilon, M_PI / 3);
    rpy_test_cases_.push_back(pitch_near_half_piB);

    // pitch = 0.5*pi-2*eps
    const RollPitchYaw<double> pitch_near_half_piC(
        M_PI / 4, 0.5 * M_PI - 2 * kEpsilon, M_PI / 3);
    rpy_test_cases_.push_back(pitch_near_half_piC);

    // pitch = 0.5*pi - 1E-15
    const RollPitchYaw<double> pitch_near_half_piD(
        M_PI * 0.8, 0.5 * M_PI - 1E-15, 0.9 * M_PI);
    rpy_test_cases_.push_back(pitch_near_half_piD);

    // pitch = -0.5*pi+eps
    const RollPitchYaw<double> pitch_near_neg_half_piA(
        M_PI * -0.9, -0.5 * M_PI + kEpsilon, M_PI * 0.3);
    rpy_test_cases_.push_back(pitch_near_neg_half_piA);

    // pitch = -0.5*pi+1.5*eps
    const RollPitchYaw<double> pitch_near_neg_half_piB(
        M_PI * -0.6, -0.5 * M_PI + 1.5 * kEpsilon, M_PI * 0.3);
    rpy_test_cases_.push_back(pitch_near_neg_half_piB);

    // pitch = -0.5*pi+2*eps
    const RollPitchYaw<double> pitch_near_neg_half_piC(
        M_PI * -0.5, -0.5 * M_PI + 2 * kEpsilon, M_PI * 0.4);
    rpy_test_cases_.push_back(pitch_near_neg_half_piC);

    // pitch = -0.5*pi + 1E-15
    const RollPitchYaw<double> pitch_near_neg_half_piD(
        M_PI * 0.9, -0.5 * M_PI + 1E-15, 0.8 * M_PI);
    rpy_test_cases_.push_back(pitch_near_neg_half_piD);

    // non-singular cases
    auto roll = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize,
                                           -0.99 * M_PI, M_PI);
    auto pitch = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize,
                                            -0.49 * M_PI, 0.49 * M_PI);
    auto yaw = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize,
                                          -0.99 * M_PI, M_PI);
    for (int i = 0; i < roll.size(); ++i) {
      for (int j = 0; j < pitch.size(); ++j) {
        for (int k = 0; k < yaw.size(); ++k) {
          const RollPitchYaw<double> rpy(roll(i), pitch(j), yaw(k));
          rpy_test_cases_.push_back(rpy);
        }
      }
    }
  }

  void addAngleAxisTestCase(double angle, const Vector3d& axis) {
    angle_axis_test_cases_.push_back(AngleAxisd(angle, axis));
  }

  void SetupAngleAxisTestCases() {
    // Set up a variety of specific tests for angles/axes that may cause
    // numerical problems as well as a sweep of values to test general
    // functionality.
    // Degenerate case, 0 rotation around x axis
    // Degenerate case, 0 rotation around y axis
    // Degenerate case, 0 rotation around z axis
    // Degenerate case, 0 rotation around a unit axis
    // Almost degenerate case, small positive rotation around an arbitrary axis
    // Almost degenerate case, small negative rotation around an arbitrary axis
    // Differentiation issue at 180 rotation around x axis
    // Differentiation issue at 180 rotation around y axis
    // Differentiation issue at 180 rotation around z axis
    // Differentiation issue at 180 rotation around an arbitrary unit axis
    // Differentiation issue close to 180 rotation around an arbitrary axis

    // 0 rotation around x axis
    addAngleAxisTestCase(0, Vector3d::UnitX());

    // 0 rotation around y axis
    addAngleAxisTestCase(0, Vector3d::UnitY());

    // 0 rotation around z axis
    addAngleAxisTestCase(0, Vector3d::UnitZ());

    // 0 rotation around an arbitrary axis
    Vector3d axis(0.5 * sqrt(2), 0.4 * sqrt(2), 0.3 * sqrt(2));
    addAngleAxisTestCase(0, axis);

    // epsilon rotation around an arbitrary axis
    addAngleAxisTestCase(kEpsilon, axis);

    // 1E-10 rotation around an arbitrary axis
    addAngleAxisTestCase(1E-10, axis);

    // -epsilon rotation around an arbitrary axis
    addAngleAxisTestCase(-kEpsilon, axis);

    // -1E-10 rotation around an arbitrary axis
    addAngleAxisTestCase(-1E-10, axis);

    // 180 rotation around x axis
    addAngleAxisTestCase(M_PI, Vector3d::UnitX());

    // 180 rotation around y axis
    addAngleAxisTestCase(M_PI, Vector3d::UnitY());

    // 180 rotation around z axis
    addAngleAxisTestCase(M_PI, Vector3d::UnitZ());

    // -180 rotation around x axis
    addAngleAxisTestCase(-M_PI, Vector3d::UnitX());

    // -180 rotation around y axis
    addAngleAxisTestCase(-M_PI, Vector3d::UnitY());

    // -180 rotation around z axis
    addAngleAxisTestCase(-M_PI, Vector3d::UnitZ());

    // 180 rotation around an arbitrary axis
    addAngleAxisTestCase(M_PI, axis);

    // -180 rotation around an arbitrary axis
    addAngleAxisTestCase(-M_PI, axis);

    // (1-epsilon)*pi rotation around an arbitrary axis
    addAngleAxisTestCase((1 - kEpsilon) * M_PI, axis);

    // (-1+epsilon)*pi rotation around an arbitrary axis
    addAngleAxisTestCase((-1 + kEpsilon) * M_PI, axis);

    // (1-2*epsilon)*pi rotation around an arbitrary axis
    addAngleAxisTestCase((1 - 2 * kEpsilon) * M_PI, axis);

    // (-1+2*epsilon)*pi rotation around an arbitrary axis
    addAngleAxisTestCase((-1 + 2 * kEpsilon) * M_PI, axis);

    // (1-1E-10)*pi rotation around an arbitrary axis
    addAngleAxisTestCase((1 - 1E-10) * M_PI, axis);

    // (-1+1E-10)*pi rotation around an arbitrary axis
    addAngleAxisTestCase((-1 + 1E-10) * M_PI, axis);

    // non-singularity cases
    auto a_x = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    auto a_y = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    auto a_z = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    auto a_angle = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize,
                                              -0.95 * M_PI, 0.95 * M_PI);
    for (int i = 0; i < a_x.size(); ++i) {
      for (int j = 0; j < a_y.size(); ++j) {
        for (int k = 0; k < a_z.size(); ++k) {
          Vector3d axis_ijk(a_x(i), a_y(j), a_z(k));
          if (axis_ijk.norm() > 1E-3) {
            axis_ijk.normalize();
            for (int l = 0; l < a_angle.size(); ++l) {
              addAngleAxisTestCase(a_angle(l), axis_ijk);
            }
          }
        }
      }
    }
  }

  void SetupQuaternionTestCases() {
    // Set up a variety of general tests for quaternions.
    auto qw = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    auto qx = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    auto qy = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    auto qz = Eigen::VectorXd::LinSpaced(Eigen::Sequential, kSweepSize, -1, 1);
    for (int i = 0; i < qw.size(); ++i) {
      for (int j = 0; j < qx.size(); ++j) {
        for (int k = 0; k < qy.size(); ++k) {
          for (int l = 0; l < qz.size(); ++l) {
            Vector4d q(qw(i), qx(j), qy(k), qz(l));
            if (q.norm() > 1E-3) {
              q.normalize();
              quaternion_test_cases_.push_back(
                  Quaterniond(q(0), q(1), q(2), q(3)));
            }
          }
        }
      }
    }
  }

  void SetupRotationMatrixTestCases() {
    for (const RollPitchYaw<double>& rpyi : rpy_test_cases_) {
      const RotationMatrix<double> Ri(rpyi);
      rotation_matrix_test_cases_.push_back(Ri);
    }
    for (const Eigen::AngleAxisd& ai : angle_axis_test_cases_) {
      const RotationMatrix<double> Ri(ai);
      rotation_matrix_test_cases_.push_back(Ri);
    }
    for (const Quaterniond& qi : quaternion_test_cases_) {
      const RotationMatrix<double> Ri(qi);
      rotation_matrix_test_cases_.push_back(Ri);
    }
  }
  std::vector<RollPitchYaw<double>> rpy_test_cases_;
  std::vector<AngleAxisd> angle_axis_test_cases_;
  std::vector<Quaterniond> quaternion_test_cases_;
  std::vector<RotationMatrix<double>> rotation_matrix_test_cases_;
};

TEST_F(RotationConversionTest, quaternionToRotationMatrixTest) {
  for (const Quaterniond& qi : quaternion_test_cases_) {
    // Compute the rotation matrix using Eigen geometry module, compare the
    // result with RotationMatrix(quaternion).
    const Matrix3d rotmat_expected = qi.toRotationMatrix();
    const Matrix3d rotmat = RotationMatrix<double>(qi).matrix();
    EXPECT_TRUE(CompareMatrices(rotmat_expected, rotmat, 1E-10,
                                MatrixCompareType::absolute));
    // RotationMatrix(quaternion) is inverse of RotationMatrix::ToQuaternion().
    const Eigen::Quaterniond quat_expected =
        RotationMatrix<double>::ToQuaternion(rotmat);
    EXPECT_TRUE(
        AreQuaternionsEqualForOrientation(qi, quat_expected, kTolerance));
  }
}

TEST_F(RotationConversionTest, QuatRPY) {
  for (const Quaterniond& qi : quaternion_test_cases_) {
    const RollPitchYaw<double> rpy(qi);
    const Eigen::Quaterniond q_expected = rpy.ToQuaternion();
    // Test RollPitchYaw::ToQuaternion() is inverse of RollPitchYaw(quaternion).
    EXPECT_TRUE(AreQuaternionsEqualForOrientation(qi, q_expected, kTolerance));
    EXPECT_TRUE(rpy.IsRollPitchYawInCanonicalRange());
  }
}

TEST_F(RotationConversionTest, RotmatQuat) {
  // Compare Eigen's rotation matrix to quaternion result with the result from
  // RotationMatrix::ToQuaternion().
  for (const RotationMatrix<double>& Ri : rotation_matrix_test_cases_) {
    const Eigen::Quaterniond quat_drake = Ri.ToQuaternion();
    const Eigen::Quaterniond quat_eigen = Quaterniond(Ri.matrix());
    EXPECT_TRUE(
        AreQuaternionsEqualForOrientation(quat_drake, quat_eigen, kTolerance));
    // Ensure the calculated quaternion produces the same rotation matrix.
    // This test accuracy to near machine precision and uses a tolerance of
    // 32 * kEpsilon (allows for 5 of the 53 mantissa bits to be inaccurate).
    // This 5-bit estimate seems to be a reasonably tight bound which
    // nevertheless passes a representative sampling of compilers and platforms.
    const RotationMatrix<double> rotmat(quat_drake);
    EXPECT_TRUE(Ri.IsNearlyEqualTo(rotmat, 32 * kEpsilon));
  }
}

TEST_F(RotationConversionTest, rotmat2rpyTest) {
  for (const RotationMatrix<double>& Ri : rotation_matrix_test_cases_) {
    const RollPitchYaw<double> rpy(Ri);
    const RotationMatrix<double> rotmat_expected(rpy);
    // RollPitchYaw(RotationMatrix) is inverse of RotationMatrix(RollPitchYaw).
    EXPECT_TRUE(Ri.IsNearlyEqualTo(rotmat_expected, 256 * kEpsilon));
    EXPECT_TRUE(rpy.IsRollPitchYawInCanonicalRange());
  }
}

TEST_F(RotationConversionTest, rpy2rotmatTest) {
  for (const RollPitchYaw<double>& rpyi : rpy_test_cases_) {
    const double roll = rpyi.roll_angle();
    const double pitch = rpyi.pitch_angle();
    const double yaw = rpyi.yaw_angle();
    const Quaterniond q = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) *
                          Eigen::AngleAxisd(pitch, Vector3d::UnitY()) *
                          Eigen::AngleAxisd(roll, Vector3d::UnitX());
    const RotationMatrix<double> R_from_quaternion(q);

    // Compute rotation matrix by rotz(rpy(2))*roty(rpy(1))*rotx(rpy(0)),
    // then compare the result with RotationMatrix(RollPitchYaw).
    const RotationMatrix<double> R_from_rpy(rpyi);
    EXPECT_TRUE(
        R_from_rpy.IsNearlyEqualTo(R_from_quaternion, 512 * kEpsilon));

    // RollPitchYaw(RotationMatrix) is inverse of RotationMatrix(RollPitchYaw).
    const RollPitchYaw<double> rpy_expected(R_from_rpy);
    EXPECT_TRUE(rpyi.IsNearlySameOrientation(rpy_expected, kTolerance));
  }
}

TEST_F(RotationConversionTest, rpy2QuatTest) {
  for (const RollPitchYaw<double>& rpyi : rpy_test_cases_) {
    const Eigen::Quaterniond q = rpyi.ToQuaternion();
    // Verify rpyi.ToQuaternion() is inverse of RollPitchYaw(Quaternion).
    const RollPitchYaw<double> rpy_expected(q);
    EXPECT_TRUE(rpyi.IsNearlySameOrientation(rpy_expected, 512 * kEpsilon));
  }
}

}  // namespace
}  // namespace math
}  // namespace drake


// Copyright 2015-2019 Hans Dembinski and Henry Schreiner
//
// Distributed under the Boost Software License, version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Based on boost/histogram/accumulators/mean.hpp
// Changes:
//  * Internal values are public for access from Python
//  * A special constructor added for construction from Python

#pragma once

#include <boost/core/nvp.hpp>
#include <boost/histogram/weight.hpp>

namespace accumulators {

/** Calculates mean and variance of sample.

  Uses Welford's incremental algorithm to improve the numerical
  stability of mean and variance computation.
*/
template <class ValueType>
struct mean {
    using value_type      = ValueType;
    using const_reference = const value_type&;

    mean() = default;

    mean(const value_type& n,
         const value_type& mean,
         const value_type& variance) noexcept
        : count(n)
        , value(mean)
        , _sum_of_deltas_squared(variance * (n - 1)) {}

    mean(const value_type& sum,
         const value_type& mean,
         const value_type& _sum_of_deltas_squared,
         bool /* Tag to trigger python internal constructor */)
        : count(sum)
        , value(mean)
        , _sum_of_deltas_squared(_sum_of_deltas_squared) {}

    void operator()(const value_type& x) noexcept {
        count += static_cast<value_type>(1);
        const auto delta = x - value;
        value += delta / count;
        _sum_of_deltas_squared += delta * (x - value);
    }

    void operator()(const boost::histogram::weight_type<value_type>& w,
                    const value_type& x) noexcept {
        count += w.value;
        const auto delta = x - value;
        value += w.value * delta / count;
        _sum_of_deltas_squared += w.value * delta * (x - value);
    }

    mean& operator+=(const mean& rhs) noexcept {
        if(rhs.count == 0)
            return *this;

        const auto mu1 = value;
        const auto mu2 = rhs.value;
        const auto n1  = count;
        const auto n2  = rhs.count;

        count += rhs.count;
        value = (n1 * mu1 + n2 * mu2) / count;
        _sum_of_deltas_squared += rhs._sum_of_deltas_squared;
        _sum_of_deltas_squared
            += n1 * (value - mu1) * (value - mu1) + n2 * (value - mu2) * (value - mu2);

        return *this;
    }

    mean& operator*=(const value_type& s) noexcept {
        value *= s;
        _sum_of_deltas_squared *= s * s;
        return *this;
    }

    bool operator==(const mean& rhs) const noexcept {
        return count == rhs.count && value == rhs.value
               && _sum_of_deltas_squared == rhs._sum_of_deltas_squared;
    }

    bool operator!=(const mean& rhs) const noexcept { return !operator==(rhs); }

    value_type variance() const noexcept {
        return _sum_of_deltas_squared / (count - 1);
    }

    template <class Archive>
    void serialize(Archive& ar, unsigned) {
        ar& boost::make_nvp("count", count);
        ar& boost::make_nvp("value", value);
        ar& boost::make_nvp("_sum_of_deltas_squared", _sum_of_deltas_squared);
    }

    value_type count{};
    value_type value{};
    value_type _sum_of_deltas_squared{};
};

} // namespace accumulators