/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

#include "packages/pnp/gems/pnp.hpp"
#include "engine/core/logger.hpp"
#include "packages/pnp/gems/tests/simu.hpp"

#include <cmath>
#include <iostream>
#include <random>
#include <sstream>

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

// Global number of repetitions for some randomized tests.
constexpr int kRepeatTests = 100;

// Test equivalence of two poses (a and b) are similar within tolerance parameters.
void TestPoseEquivalence(const isaac::Pose3d& a, const isaac::Pose3d& b, double max_distance = 1e-9,
                         double max_angle_degrees = 1e-9) {
  // Calculate difference in orientation.
  isaac::SO3d delta_rot = a.rotation.inverse() * b.rotation;
  double angle = std::fmod(isaac::RadToDeg(std::abs(delta_rot.angle())), 360.0);
  angle = std::min(angle, 360.0 - angle);

  // Calculate difference in position.
  // Convert world origin in camera frame to camera position in world frame. This can make
  // a significant difference in case of a moving camera far away from world origin.
  isaac::Vector3d position1 = -a.rotation.matrix().transpose() * a.translation;
  isaac::Vector3d position2 = -b.rotation.matrix().transpose() * b.translation;

  ASSERT_LT((position1 - position2).norm(), max_distance);
  ASSERT_LT(angle, max_angle_degrees);
}

// Test EPnP pose estimation with random camera poses and 3D points on a FRONTO-PARALLEL plane.
// Noise-free 2D-3D point matches, no outliers.
TEST(PnpTest, EpnpFrontoPlanarTest) {
  SCOPED_TRACE("EpnpFrontoPlanarTest");

  // Repeat the test with different random poses and points.
  for (int i = 0; i < kRepeatTests; i++) {
    // Generate a camera with fixed intrinsics and random pose.
    const int width = 1280;
    const int height = 720;
    const double focal = 700.0;
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // Construct isaac::Pose3d object from the generated camera pose.
    // This is the known ground-truth pose to compare to the output of pose estimation.
    isaac::Vector3d gt_angle_axis = isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix);
    isaac::Pose3d gt_pose{isaac::SO3d::FromAngleAxis(gt_angle_axis.norm(), gt_angle_axis),
                          -camera.rotation_matrix * camera.position};

    // Point generation parameters.
    const double depth = 5.0;  // Plane depth in meters
    const double angle = 0.0;  // Plane inclination angle in degrees (0 is fronto-parallel).

    // Output of point generation and pose estimation.
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::Vector4d plane;
    isaac::Pose3d pose;

    // Test EPnP camera pose estimation from 6 points on a fronto-parallel plane.
    isaac::pnp::GenerateFovPointsPlanar(6, camera, depth, angle, 0, &points3, &points2, &plane);
    ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                                principal_v, &pose),
              isaac::pnp::Status::kSuccess);
    TestPoseEquivalence(pose, gt_pose);

    // Same test with many points.
    isaac::pnp::GenerateFovPointsPlanar(50, camera, depth, angle, 0, &points3, &points2, &plane);
    ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                                principal_v, &pose),
              isaac::pnp::Status::kSuccess);
    TestPoseEquivalence(pose, gt_pose);
  }
}

// Test EPnP pose estimation with random camera poses and 3D points on a SLANTED plane.
// Noise-free 2D-3D point matches, no outliers.
TEST(PnpTest, EpnpSlantedPlanarTest) {
  SCOPED_TRACE("EpnpSlantedPlanarTest");

  // Repeat the test with different random poses and points.
  for (int i = 0; i < kRepeatTests; i++) {
    // Generate a camera with fixed intrinsics and random pose.
    const int width = 1280;
    const int height = 720;
    const double focal = 700.0;
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // Construct isaac::Pose3d object from the generated camera pose.
    // This is the known ground-truth pose to compare to the output of pose estimation.
    isaac::Vector3d gt_angle_axis = isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix);
    isaac::Pose3d gt_pose{isaac::SO3d::FromAngleAxis(gt_angle_axis.norm(), gt_angle_axis),
                          -camera.rotation_matrix * camera.position};

    // Point generation parameters.
    const double depth = 10.0;  // Plane depth in meters
    const double angle = 30.0;  // Plane inclination angle in degrees (0 is fronto-parallel).

    // Output of point generation and pose estimation.
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::Vector4d plane;
    isaac::Pose3d pose;

    // Test EPnP camera pose estimation from 6 points on a slanted plane.
    GenerateFovPointsPlanar(6, camera, depth, angle, 0, &points3, &points2, &plane);
    ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                                principal_v, &pose),
              isaac::pnp::Status::kSuccess);
    TestPoseEquivalence(pose, gt_pose);

    // Same test with many points.
    GenerateFovPointsPlanar(50, camera, depth, angle, 0, &points3, &points2, &plane);
    ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                                principal_v, &pose),
              isaac::pnp::Status::kSuccess);
    TestPoseEquivalence(pose, gt_pose);
  }
}

// Test EPnP pose estimation with random camera poses and 3D points within a depth range.
// Noise-free 2D-3D point matches, no outliers.
TEST(PnpTest, EpnpNonPlanarTest) {
  SCOPED_TRACE("EpnpNonPlanarTest");

  // Repeat the test with different random poses and points.
  for (int i = 0; i < kRepeatTests; i++) {
    // Generate a camera with fixed intrinsics and random pose.
    const int width = 1280;
    const int height = 720;
    const double focal = 700.0;
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // Construct isaac::Pose3d object from the generated camera pose.
    // This is the known ground-truth pose to compare to the output of pose estimation.
    isaac::Vector3d gt_angle_axis = isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix);
    isaac::Pose3d gt_pose{isaac::SO3d::FromAngleAxis(gt_angle_axis.norm(), gt_angle_axis),
                          -camera.rotation_matrix * camera.position};

    // Point generation parameters.
    const double near = 2.0;
    const double far = 100.0;

    // Output of point generation and pose estimation.
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::Vector4d plane;
    isaac::Pose3d pose;

    // Test EPnP pose estimation from 6 points between the near and far planes.
    GenerateFovPoints(6, camera, near, far, &points3, &points2);
    ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                                principal_v, &pose),
              isaac::pnp::Status::kSuccess);
    TestPoseEquivalence(pose, gt_pose);

    // Same test with many points.
    GenerateFovPoints(50, camera, near, far, &points3, &points2);
    ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                                principal_v, &pose),
              isaac::pnp::Status::kSuccess);
    TestPoseEquivalence(pose, gt_pose);
  }
}

// Print table of the necessary number of RANSAC experiments for different outlier ratios (columns)
// and different sample sizes (in table rows).
void PrintRansacTable(double success_rate = 0.99) {
  // Outlier ratios and sample sizes to sweep over.
  std::vector<double> outlier_ratios{0.05, 0.10, 0.20, 0.25, 0.30, 0.40, 0.50};
  std::vector<int> sample_sizes{2, 3, 4, 5, 6, 7, 8};

  // Print table header.
  std::stringstream ss;
  const int cell_width = 4;
  ss << "    ";
  for (double outlier_ratio : outlier_ratios) {
    ss << std::setw(cell_width) << 100.0 * outlier_ratio << "%";
  }
  LOG_INFO("\t%s", ss.str().c_str());

  // Print table body row by row.
  for (int sample_size : sample_sizes) {
    ss.str("");
    ss.clear();
    ss << sample_size << "-pt";
    for (double outlier_ratio : outlier_ratios) {
      ss << " " << std::setw(cell_width);
      ss << isaac::pnp::EvaluateRansacFormula(success_rate, outlier_ratio, sample_size);
    }
    LOG_INFO("\t%s", ss.str().c_str());
  }
}

// Test if isaac::pnp::EvaluateRansacFormula() returns the values given in a textbook.
TEST(PnpTest, RansacFormulaTest) {
  // Print the RANSAC table for information.
  PrintRansacTable(0.99);

  // Reference values are taken from the book of Hartley & Zisserman, 3rd ed., p. 119, Table 4.3
  std::vector<double> outlier_ratio{0.05, 0.10, 0.20, 0.25, 0.30, 0.40, 0.50};
  std::vector<int> sample_size{2, 3, 4, 5, 6, 7, 8};
  Eigen::MatrixXi num_experiments(sample_size.size(), outlier_ratio.size());
  num_experiments << 2, 3, 5, 6, 7, 11, 17, 3, 4, 7, 9, 11, 19, 35, 3, 5, 9, 13, 17, 34, 72, 4, 6,
      12, 17, 26, 57, 146, 4, 7, 16, 24, 37, 97, 293, 4, 8, 20, 33, 54, 163, 588, 5, 9, 26, 44, 78,
      272, 1177;

  // Compare the output of EvaluateRansacFormula() to the reference values.
  for (size_t i = 0; i < sample_size.size(); i++) {
    for (size_t j = 0; j < outlier_ratio.size(); j++) {
      EXPECT_EQ(isaac::pnp::EvaluateRansacFormula(0.99, outlier_ratio[j], sample_size[i]),
                num_experiments(i, j));
    }
  }
}

// Test RANSAC-EPnP pose estimation on a many random synthetic datasets contaminated with outliers.
// In each each test run:
// (1) Generate a camera with a random pose.
// (2) Generate a fixed number of 2D/3D point match inliers and add a fixed percentage of outliers.
// (3) Run RANSAC-EPnP.
// (4) Test the result and collect statistics.
TEST(PnpTest, EpnpRansacTest) {
  SCOPED_TRACE("EpnpRansacTest");

  // Fixed number of inliers and outliers.
  const double outlier_ratio = 0.3;
  const unsigned num_inliers = 20;
  const unsigned num_outliers = round(outlier_ratio / (1.0 - outlier_ratio) * num_inliers);

  // RANSAC parameters.
  const unsigned ransac_rounds = 40;
  const double ransac_threshold = 1e-3;
  const unsigned max_top_poses = 5;
  unsigned rand_seed = 0;
  std::random_device rnd;

  // Collect outlier ratio statistics over many runs for reporting.
  double min_outlier_ratio = 1.0;
  double max_outlier_ratio = 0;
  // unsigned max_hypotheses_found = 0;

  // Run RANSAC on many different random datasets,
  // each with a random camera pose and 2D/3D matches.
  const unsigned num_runs = 100;
  unsigned num_poses_found = 0;  // Total number of successful pose estimations across all runs.
  for (unsigned i = 0; i < num_runs; i++) {
    // Generate a camera with fixed intrinsics and random pose.
    const int width = 1280;
    const int height = 720;
    const double focal = 700.0;
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // Construct isaac::Pose3d object from the generated camera pose.
    // This is the known ground-truth pose to compare to the output of pose estimation.
    isaac::Vector3d gt_angle_axis = isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix);
    isaac::Pose3d gt_pose{isaac::SO3d::FromAngleAxis(gt_angle_axis.norm(), gt_angle_axis),
                          -camera.rotation_matrix * camera.position};

    // Generate inlier 2D-3D matches without noise.
    // 3D points are within camera FoV between near and far planes.
    const double near = 2.0;
    const double far = 6.0;
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::pnp::GenerateFovPoints(num_inliers, camera, near, far, &points3, &points2);

    // Generate outlier 2D-3D matches
    isaac::pnp::InsertOutliers(num_outliers, camera, 30.0, &points3, &points2);

    // Calculate reprojection error for every single point match.
    isaac::VectorXd errs =
        isaac::pnp::ColwiseNorms(points2 - ProjectPoints(camera, points3, true, true));

    // Separate inliers from outliers based on reprojection error.
    // Because generated outliers have 1.0 chance to be outliers, but that is not guarantee.
    std::vector<bool> is_inlier(errs.size());
    for (unsigned k = 0; k < errs.size(); k++) {
      is_inlier[k] = (errs(k) <= ransac_threshold);
    }
    unsigned num_actual_outliers = (errs.array() > ransac_threshold).count();
    double ratio = double(num_actual_outliers) / points3.cols();
    if (ratio < min_outlier_ratio) {
      min_outlier_ratio = ratio;
    }
    if (ratio > max_outlier_ratio) {
      max_outlier_ratio = ratio;
    }

    // Run RANSAC-EPnP camera pose estimation on the current synthetic dataset.
    rand_seed = rnd();
    auto top_hypotheses = isaac::pnp::ComputeCameraPoseEpnpRansac(
        points3, points2, focal_u, focal_v, principal_u, principal_v, ransac_rounds,
        ransac_threshold, max_top_poses, rand_seed);

    // Count successful pose estimations accross runs.
    if (top_hypotheses.size()) {
      num_poses_found++;
    }

    // Test if the output list of top hypotheses does not exceed the prescribed capacity.
    ASSERT_LE(top_hypotheses.size(), max_top_poses);

    // Check each output pose hypothesis individually.
    for (const auto& hyp : top_hypotheses) {
      // There should be at least 6 inliers for any accepted pose hypothesis.
      ASSERT_GE(hyp.inliers.size(), 6);

      // Inliers are perfect (zero reprojection error),
      // so hypothesis score must be equal to the number of inliers up to numerical precision.
      EXPECT_NEAR(hyp.score, hyp.inliers.size(), 1e-3);

      // Make sure estimated pose matches the ground truth pose.
      TestPoseEquivalence(hyp.pose, gt_pose);
    }
  }

  // Print statistics.
  unsigned min_successful_runs = 0.9 * num_runs;
  LOG_INFO("Ransac: %.2f%% outliers, %d rounds", 100.0 * min_outlier_ratio, ransac_rounds);
  LOG_INFO("Ransac: no result in %d test runs, correct pose in %d of %d cases (%d required)",
           num_runs - num_poses_found, num_poses_found, num_runs, min_successful_runs);

  // Require a high percentage of test runs to succeed.
  // Do not require all test runs to succeed because RANSAC is a randomized algorithm,
  // finding the solution is not guaranteed but should has a high chance.
  EXPECT_GE(num_poses_found, min_successful_runs);
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <blitz/array.h>
#include <blitz/numinquire.h> // for huge()

using namespace blitz;

// A simple fixed point arithmetic class which represents a point
// in the interval [0,1].
class FixedPoint {

public:
    typedef unsigned int T_mantissa;

    FixedPoint() { }

    FixedPoint(T_mantissa mantissa)
    {
        mantissa_ = mantissa;
    }

    FixedPoint(double value)
    {
        assert((value >= 0.0) && (value <= 1.0));
        mantissa_ = static_cast<T_mantissa>(value * huge(T_mantissa()));
    }

    FixedPoint operator+(FixedPoint x)
    { return FixedPoint(mantissa_ + x.mantissa_); }

    double value() const
    { return mantissa_ / double(huge(T_mantissa())); }

private:
    T_mantissa mantissa_;
};

ostream& operator<<(ostream& os, const FixedPoint& a)
{
    os << a.value();
    return os;
}

int main()
{
    // Create an array using the FixedPoint class:

    Array<FixedPoint, 2> A(4,4), B(4,4);

    A = 0.5, 0.3, 0.8, 0.2,
        0.1, 0.3, 0.2, 0.9,
        0.0, 1.0, 0.7, 0.4,
        0.2, 0.3, 0.8, 0.4;

    B = A + 0.05;

    cout << "B = " << B << endl;

    return 0;
}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_LANCZOS_SIMPLEOP_TEMPLATE_HPP
#define MFMG_LANCZOS_SIMPLEOP_TEMPLATE_HPP

#include <deal.II/lac/la_parallel_vector.h>

#include <cassert>

#include "lanczos_simpleop.hpp"

namespace mfmg
{
// This will be a diagonal matrix; specify the diag entries here
#ifdef __CUDACC__
__host__ __device__
#endif
    double
    diag_value(size_t i, size_t multiplicity)
{
  return 1 + i / multiplicity;
}

#ifdef __CUDACC__
__global__ void vmult_kernel(double *y, double const *x, unsigned int dim,
                             unsigned int multiplicity)
{
  int i = threadIdx.x + blockDim.x * blockIdx.x;
  if (i < dim)
  {
    y[i] = diag_value(i, multiplicity) * x[i];
  }
}
#endif

/// \brief Simple test operator: constructor
template <typename VectorType>
SimpleOperator<VectorType>::SimpleOperator(size_t dim, size_t multiplicity)
    : _dim(dim), _multiplicity(multiplicity)
{
  assert(multiplicity > 0);
}

template <typename VectorType>
std::vector<double> SimpleOperator<VectorType>::get_evals() const
{
  std::vector<double> evals(_dim);
  for (size_t i = 0; i < _dim; i++)
    evals[i] = diag_value(i, _multiplicity);

  return evals;
}

/// \brief Simple test operator: apply operator to a vector
template <typename VectorType>
void SimpleOperator<VectorType>::vmult(VectorType &y, VectorType const &x) const
{
  ASSERT(x.size() == _dim, "");
  ASSERT(y.size() == _dim, "");

  for (size_t i = 0; i < _dim; ++i)
    y[i] = diag_value(i, _multiplicity) * x[i];
}

#ifdef __CUDACC__
template <>
void SimpleOperator<dealii::LinearAlgebra::distributed::Vector<
    double, dealii::MemorySpace::CUDA>>::
    vmult(dealii::LinearAlgebra::distributed::Vector<
              double, dealii::MemorySpace::CUDA> &y,
          dealii::LinearAlgebra::distributed::Vector<
              double, dealii::MemorySpace::CUDA> const &x) const
{
  // CUDA does not like size_t. I think that size_t on the device is 32 bits
  // while it 64 bits on the host and thus, dim and multiplicity in the kernel
  // are garbage
  unsigned int d = _dim;
  unsigned int m = _multiplicity;
  int n_blocks = 1 + _dim / 512;
  vmult_kernel<<<n_blocks, 512>>>(y.get_values(), x.get_values(), d, m);
}
#endif

} // namespace mfmg

#endif


/*
 * File:   ConvolutionLayer.hpp
 * Author: heshan
 *
 * Created on June 6, 2018, 6:26 PM
 */

#ifndef CONVOLUTIONLAYER_HPP
#define CONVOLUTIONLAYER_HPP

#include <Eigen>
#include <iostream>
#include "Activation.hpp"
#include <string>

class ConvolutionLayer {
public:
    ConvolutionLayer();
    /**
     * Constructor
     *
     * @param dimensions: dimensions of the input matrix (depth, height, width)
     * @param filterSize: size of the filter N, (N x N)
     * @param stride: displacement units of the filter
     * @param noOfFilters: no of filters
     * @param padding
     */
    ConvolutionLayer(std::tuple<int, int, int> dimensions, int filterSize, int stride, int noOfFilters, int padding);
    /**
     *
     * @param orig
     */
    ConvolutionLayer(const ConvolutionLayer& orig);
    /**
     */
    virtual ~ConvolutionLayer();
    /**
     * Initialize weight matrices and the bias values for the convolutional layer
     *
     * @return 0
     */
    int initMat();
    /**
     * Apply the convolution operation to the image using the generated filters
     *
     * @param input: input image or matrix
     * @return an array of convoluted images (array size = no of filters)
     */
    Eigen::MatrixXd * convolute(Eigen::MatrixXd * input);
    /**
     * Return the dimension of the layer output
     *
     * @return a tuple
     */
    std::tuple<int, int, int> getOutputDims();

private:
    int height, width, padding = 0;

public:
    int stride, filterSize;
    int depth, noOfFilters;
    int outHeight, outWidth;
    double * bias;
    Eigen::MatrixXd ** filters;
    Eigen::MatrixXd * output;
    Eigen::MatrixXd * activatedOut;

};

#endif /* CONVOLUTIONLAYER_HPP */


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

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

// eigen
#include <Eigen/Dense>

#include <iostream>

void setDynamicsMatrices(Eigen::Matrix<double, 2, 2> &a, Eigen::Matrix<double, 2, 1> &b)
{
    a << 1.,      0.020,
        0.,      0.9661;

    b << 0.,
        0.0315;
}

void setInequalityConstraints(Eigen::Matrix<double, 2, 1> &xMax, Eigen::Matrix<double, 2, 1> &xMin,
                              Eigen::Matrix<double, 1, 1> &uMax, Eigen::Matrix<double, 1, 1> &uMin)
{
    double u0 = 0.0;

    // input inequality constraints
    uMin << -6.0 - u0;

    uMax << 10.0 - u0;

    // state inequality constraints
    // TODO : change to present pos +/- ranges
    xMin << -100, -6.0;

    xMax << 100, 10.0;
}

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

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

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

    //populate hessian matrix
    for(int i = 0; i<2*(mpcWindow+1) + 1 * mpcWindow; i++){
        if(i < 2*(mpcWindow+1)){
            int posQ=i%2;
            float value = Q.diagonal()[posQ];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
        else{
            int posR=i%1;
            float value = R.diagonal()[posR];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
    }
}

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

    Eigen::Matrix<double,2,1> Qx_ref;
    Qx_ref = Q * (-xRef);

    // populate the gradient vector
    gradient = Eigen::VectorXd::Zero(2*(mpcWindow+1) +  1*mpcWindow, 1);
    for(int i = 0; i<2*(mpcWindow+1); i++){
        int posQ=i%2;
        float value = Qx_ref(posQ,0);
        gradient(i,0) = value;
    }
}

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

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

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

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

    for(int i = 0; i<2*(mpcWindow+1) + 1*mpcWindow; i++){
        constraintMatrix.insert(i+(mpcWindow+1)*2,i) = 1;
    }
}

void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 2, 1> &xMax, const Eigen::Matrix<double, 2, 1> &xMin,
                                   const Eigen::Matrix<double, 1, 1> &uMax, const Eigen::Matrix<double, 1, 1> &uMin,
                                   const Eigen::Matrix<double, 2, 1> &x0,
                                   int mpcWindow, Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
    // evaluate the lower and the upper inequality vectors
    Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(2*(mpcWindow+1) +  1 * mpcWindow, 1);
    Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(2*(mpcWindow+1) +  1 * mpcWindow, 1);
    for(int i=0; i<mpcWindow+1; i++){
        lowerInequality.block(2*i,0,2,1) = xMin;
        upperInequality.block(2*i,0,2,1) = xMax;
    }
    for(int i=0; i<mpcWindow; i++){
        lowerInequality.block(1 * i + 2 * (mpcWindow + 1), 0, 1, 1) = uMin;
        upperInequality.block(1 * i + 2 * (mpcWindow + 1), 0, 1, 1) = uMax;
    }

    // evaluate the lower and the upper equality vectors
    Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(2*(mpcWindow+1),1 );
    Eigen::VectorXd upperEquality;
    lowerEquality.block(0,0,2,1) = -x0;
    upperEquality = lowerEquality;
    lowerEquality = lowerEquality;

    // merge inequality and equality vectors
    lowerBound = Eigen::MatrixXd::Zero(2*2*(mpcWindow+1) +  1*mpcWindow,1 );
    lowerBound << lowerEquality,
        lowerInequality;

    upperBound = Eigen::MatrixXd::Zero(2*2*(mpcWindow+1) +  1*mpcWindow,1 );
    upperBound << upperEquality,
        upperInequality;
}

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

double getErrorNorm(const Eigen::Matrix<double, 2, 1> &x,
                    const Eigen::Matrix<double, 2, 1> &xRef)
{
    // evaluate the error
    Eigen::Matrix<double, 2, 1> error = x - xRef;

    // return the norm
    return error.norm();
}

int main()
{
    // set the preview window
    int mpcWindow = 200;

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

    // allocate the constraints vector
    Eigen::Matrix<double, 2, 1> xMax;
    Eigen::Matrix<double, 2, 1> xMin;
    Eigen::Matrix<double, 1, 1> uMax;
    Eigen::Matrix<double, 1, 1> uMin;

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

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

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

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

    // set MPC problem quantities
    setDynamicsMatrices(a, b);
    setInequalityConstraints(xMax, xMin, uMax, uMin);
    setWeightMatrices(Q, R);

    // cast the MPC problem as QP problem
    castMPCToQPHessian(Q, R, mpcWindow, hessian);
    castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
    castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix);
    castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);

    // instantiate the solver
    OsqpEigen::Solver solver;

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

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

    // instantiate the solver
    if(!solver.initSolver()) return 1;

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

    // number of iteration steps
    int numberOfSteps = 200;

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

        // solve the QP problem
        if(solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) return 1;

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

        // save data into file
        auto x0Data = x0.data();

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

        // update the constraint bound
        updateConstraintVectors(x0, lowerBound, upperBound);
        if(!solver.updateBounds(lowerBound, upperBound)) return 1;
      }
    return 0;
}


// blas.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>

/*
#ifdef _WIN32
#undef __STRICT_ANSI__
#endif
*/

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

const boost::numeric::ublas::matrix<double> CreateMatrix(
    const std::size_t n_rows,
    const std::size_t n_cols,
    const std::vector<double>& v)
{
    assert(n_rows * n_cols == v.size());
    boost::numeric::ublas::matrix<double> m(n_rows, n_cols);
    for (std::size_t row = 0; row != n_rows; ++row)
    {
        for (std::size_t col = 0; col != n_cols; ++col)
        {
            m(row, col) = v[(col * n_rows) + row];
        }
    }
    return m;
}

//Chop returns a std::vector of sub-matrices
//[ A at [0]   B at [1] ]
//[ C at [2]   D at [4] ]
const std::vector<boost::numeric::ublas::matrix<double> > Chop(
    const boost::numeric::ublas::matrix<double>& m)
{
    //using boost::numeric::ublas::range;
    //using boost::numeric::ublas::matrix;
    //using boost::numeric::ublas::matrix_range;
    std::vector<boost::numeric::ublas::matrix<double> > v;
    v.reserve(4);
    const int midy = m.size1() / 2;
    const int midx = m.size2() / 2;
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > top_left(m, boost::numeric::ublas::range(0, midy), boost::numeric::ublas::range(0, midx));
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > bottom_left(m, boost::numeric::ublas::range(midy, m.size1()), boost::numeric::ublas::range(0, midx));
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > top_right(m, boost::numeric::ublas::range(0, midy), boost::numeric::ublas::range(midx, m.size2()));
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > bottom_right(m, boost::numeric::ublas::range(midy, m.size1()), boost::numeric::ublas::range(midx, m.size2()));
    v.push_back(boost::numeric::ublas::matrix<double>(top_left));
    v.push_back(boost::numeric::ublas::matrix<double>(top_right));
    v.push_back(boost::numeric::ublas::matrix<double>(bottom_left));
    v.push_back(boost::numeric::ublas::matrix<double>(bottom_right));
    return v;
}

bool IsAboutEqual(const double x, const double y) { return std::abs(x - y) < 0.00001; }

int main()
{
    //using boost::numeric::ublas::matrix;
    //using boost::numeric::ublas::prod;
    //using boost::numeric::ublas::vector;
    {
        //                     [ 1.0 ] | [ 2.0   3.0 ]
        // [ 1.0 2.0 3.0 ]     --------+--------------
        // [ 4.0 5.0 6.0 ]     [ 4.0 ] | [ 5.0   6.0 ]
        // [ 7.0 8.0 9.0 ] ->  [ 7.0 ] | [ 8.0   9.0 ]
        const boost::numeric::ublas::matrix<double> m = CreateMatrix(3, 3, { 1.0,4.0,7.0,2.0,5.0,8.0,3.0,6.0,9.0 });
        assert(m(0, 0) == 1.0); assert(m(0, 1) == 2.0); assert(m(0, 2) == 3.0);
        assert(m(1, 0) == 4.0); assert(m(1, 1) == 5.0); assert(m(1, 2) == 6.0);
        assert(m(2, 0) == 7.0); assert(m(2, 1) == 8.0); assert(m(2, 2) == 9.0);
        const std::vector<boost::numeric::ublas::matrix<double> > n = Chop(m);
        assert(n.size() == 4);
        std::wclog
            << L"m   : " << m << L'\n'
            << L"n[0]: " << n[0] << L'\n'
            << L"n[1]: " << n[1] << L'\n'
            << L"n[2]: " << n[2] << L'\n'
            << L"n[3]: " << n[3] << L'\n';
        assert(n[0].size1() == 1);
        assert(n[0].size2() == 1);
        assert(n[1].size1() == 1);
        assert(n[1].size2() == 2);
        assert(n[2].size1() == 2);
        assert(n[2].size2() == 1);
        assert(n[3].size1() == 2);
        assert(n[3].size2() == 2);
        assert(n[0].size1() + n[2].size1() == m.size1());
        assert(n[1].size1() + n[3].size1() == m.size1());
        assert(n[0].size2() + n[1].size2() == m.size2());
        assert(n[2].size2() + n[3].size2() == m.size2());
    }
    {
        const boost::numeric::ublas::matrix<double> m = CreateMatrix(5, 5,
            {
              1.0, 6.0,11.0,16.0,21.0,
              2.0, 7.0,12.0,17.0,22.0,
              3.0, 8.0,13.0,18.0,23.0,
              4.0, 9.0,14.0,19.0,24.0,
              5.0,10.0,15.0,20.0,25.0
            }
        );
        assert(m(0, 0) == 1.0); assert(m(0, 1) == 2.0); assert(m(0, 2) == 3.0); assert(m(0, 3) == 4.0); assert(m(0, 4) == 5.0);
        assert(m(1, 0) == 6.0); assert(m(1, 1) == 7.0); assert(m(1, 2) == 8.0); assert(m(1, 3) == 9.0); assert(m(1, 4) == 10.0);
        assert(m(2, 0) == 11.0); assert(m(2, 1) == 12.0); assert(m(2, 2) == 13.0); assert(m(2, 3) == 14.0); assert(m(2, 4) == 15.0);
        assert(m(3, 0) == 16.0); assert(m(3, 1) == 17.0); assert(m(3, 2) == 18.0); assert(m(3, 3) == 19.0); assert(m(3, 4) == 20.0);
        assert(m(4, 0) == 21.0); assert(m(4, 1) == 22.0); assert(m(4, 2) == 23.0); assert(m(4, 3) == 24.0); assert(m(4, 4) == 25.0);
        const std::vector<boost::numeric::ublas::matrix<double> > n = Chop(m);
        assert(n.size() == 4);
        std::wclog
            << L"m   : " << m << L'\n'
            << L"n[0]: " << n[0] << L'\n'
            << L"n[1]: " << n[1] << L'\n'
            << L"n[2]: " << n[2] << L'\n'
            << L"n[3]: " << n[3] << L'\n';
        assert(n[0].size1() == 2);
        assert(n[0].size2() == 2);
        assert(n[1].size1() == 2);
        assert(n[1].size2() == 3);
        assert(n[2].size1() == 3);
        assert(n[2].size2() == 2);
        assert(n[3].size1() == 3);
        assert(n[3].size2() == 3);
        assert(n[0].size1() + n[2].size1() == m.size1());
        assert(n[1].size1() + n[3].size1() == m.size1());
        assert(n[0].size2() + n[1].size2() == m.size2());
        assert(n[2].size2() + n[3].size2() == m.size2());
    }
}


/*
*  Copyright August 2015
*  Author: Olalekan P. Ogunmolu
*
* Licensed under the Apache License, Version 2.0 (the "License");
*  you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*      http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/

// Include Files
#include "savgol.h"

#include <Eigen/Core>

#include <iostream>

using namespace Eigen;
using namespace std;

void help()
{
  std::cout << "================================================== \n"
            << "USAGE:                                             \n"
            << "\n"
            << "./savgol [<frame_size> [<polynomial_order> [<x_low> <x_high>] ] ] \n"
            << "\n"
            << "       <frame_size>: odd int and ideally greater than\n"
            << "       <polynomial_order>:  an integer             \n"
            << "\n"
            << "       <x_low>, <x_high>: min and max limits of    \n"
            << "       linspaced vector to be filtered.            \n"
            << "===================================================\n"
            << "       Example: ./savgol 9 5   \n\n";;
}

int main (int argc, char** argv)
{
  int F;      //Frame Size
  int k;      //Example Polynomial Order
  double Fd ;
  float x_min, x_max;

  if(argc>1)
  {
    if(argv[1] == "-h" || "-help")
    {
      help();
      return EXIT_SUCCESS;
    }
    else
    {
      for(auto i = 1; i < argc; ++i )
      {
        F = atoi(argv[1]);
        k = atoi(argv[2]);
        x_min = atoi(argv[3]);
        x_max = atoi(argv[4]);
      }
    }
  }
  else  //use default values
  {
    F = 5; k = 3;
    x_min = 900.0; x_max = 980.0;
  }

  auto s = vander(F);        //Compute vandermonde matrix

  cout << "Frame size: " << F << "; \tPolynomial order: " << k << endl;
  cout << "\n Vandermonde Matrix: \n" << s  << endl;

  k = atoi(argv[2]) or 3;

  auto B = sgdiff(k, F, Fd);

  //To express as a real filtering operation, we shift x around the nth time instant
  auto x = VectorXf::LinSpaced(F, x_min, x_max);

  auto Filter = savgolfilt(x, k, F);

  cout <<"\n\nFiltered values in the range \n" << x.transpose().eval() <<"\n are: \n" << Filter << endl;

  return 0;
}

/* Compile:
cd ../; rm build -rf; mkdir build; cd build; cmake ../; make; ./savgol
*/


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>
#include <eve/detail/diff_div.hpp>

EVE_TEST_TYPES( "Check return types of sph_bessel_j1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::sph_bessel_j1(T(0)), T);
  TTS_EXPR_IS(eve::sph_bessel_j1(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of sph_bessel_j1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 60.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;
  auto eve__sph_bessel_j1 =  [](auto x) { return eve::sph_bessel_j1(x); };
  auto std__sph_bessel_j1 =  [](auto x)->v_t { return boost::math::sph_bessel(double(1), double(x)); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_j1(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_j1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_j1(eve::inf(eve::as< T>())),  eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_j1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(500)), std__sph_bessel_j1(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(10)), std__sph_bessel_j1(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(5)),  std__sph_bessel_j1(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(2)),  std__sph_bessel_j1(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(1.5)),std__sph_bessel_j1(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(0.5)),std__sph_bessel_j1(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(1)),  std__sph_bessel_j1(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(v_t(0)),  eve::zero(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(500)),  T(std__sph_bessel_j1(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(10)) ,  T(std__sph_bessel_j1( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(5))  ,  T(std__sph_bessel_j1( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(2))  ,  T(std__sph_bessel_j1( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(1.5)),  T(std__sph_bessel_j1( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(0.5)),  T(std__sph_bessel_j1( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(1))  ,  T(std__sph_bessel_j1( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1( T(0))  , eve::zero(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_j1(a0), map(std__sph_bessel_j1, a0), 40.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(a1), map(std__sph_bessel_j1, a1), 40.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j1(a2), map(std__sph_bessel_j1, a2), 40.0);

};

EVE_TEST( "Check behavior of diff(sph_bessel_j1) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(1.0, 10.0))
        )
  <typename T>(T a0 )
{
  auto eve__diff_bessel_j1 =  [](auto x) { return eve::diff(eve::sph_bessel_j1)(x); };
  auto df = [](auto x){return eve::detail::centered_diffdiv(eve::sph_bessel_j1, x); };

  TTS_RELATIVE_EQUAL(eve__diff_bessel_j1(a0),   df(a0), 2.0e-2);
};


/**
 * @file lmnn_test.cpp
 * @author Marcus Edel
 * @author Ryan Curtin
 * @author Manish Kumar
 *
 * Unit tests for Large Margin Nearest Neighbors and related code (including
 * the constraints class).
 *
 * 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/metrics/lmetric.hpp>
#include <mlpack/methods/lmnn/lmnn.hpp>
#include <mlpack/core/optimizers/lbfgs/lbfgs.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
#include <mlpack/core/optimizers/bigbatch_sgd/bigbatch_sgd.hpp>

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

using namespace mlpack;
using namespace mlpack::metric;
using namespace mlpack::lmnn;
using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(LMNNTest);

//
// Tests for the Constraints.
//

/**
 * The target neighbors function should be correct.
 * point.
 */
BOOST_AUTO_TEST_CASE(LMNNTargetNeighborsTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  Constraints<> constraint(dataset, labels, 1);

  // Calculate norm of datapoints.
  arma::vec norm(dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; i++)
  {
    norm(i) = arma::norm(dataset.col(i));
  }

  //! Store target neighbors of data points.
  arma::Mat<size_t> targetNeighbors =
      arma::Mat<size_t>(1, dataset.n_cols, arma::fill::zeros);

  constraint.TargetNeighbors(targetNeighbors, dataset, labels, norm);

  BOOST_REQUIRE_EQUAL(targetNeighbors(0, 0), 1);
  BOOST_REQUIRE_EQUAL(targetNeighbors(0, 1), 0);
  BOOST_REQUIRE_EQUAL(targetNeighbors(0, 2), 1);
  BOOST_REQUIRE_EQUAL(targetNeighbors(0, 3), 4);
  BOOST_REQUIRE_EQUAL(targetNeighbors(0, 4), 3);
  BOOST_REQUIRE_EQUAL(targetNeighbors(0, 5), 4);
}

/**
 * The impostors function should be correct.
 */
BOOST_AUTO_TEST_CASE(LMNNImpostorsTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  Constraints<> constraint(dataset, labels, 1);

  // Calculate norm of datapoints.
  arma::vec norm(dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; i++)
  {
    norm(i) = arma::norm(dataset.col(i));
  }

  //! Store impostors of data points.
  arma::Mat<size_t> impostors =
      arma::Mat<size_t>(1, dataset.n_cols, arma::fill::zeros);

  constraint.Impostors(impostors, dataset, labels, norm);

  BOOST_REQUIRE_EQUAL(impostors(0, 0), 3);
  BOOST_REQUIRE_EQUAL(impostors(0, 1), 4);
  BOOST_REQUIRE_EQUAL(impostors(0, 2), 5);
  BOOST_REQUIRE_EQUAL(impostors(0, 3), 0);
  BOOST_REQUIRE_EQUAL(impostors(0, 4), 1);
  BOOST_REQUIRE_EQUAL(impostors(0, 5), 2);
}

//
// Tests for the LMNNFunction
//

/**
 * The LMNN function should return the identity matrix as its initial
 * point.
 */
BOOST_AUTO_TEST_CASE(LMNNInitialPointTest)
{
  // Cheap fake dataset.
  arma::mat dataset = arma::randu(5, 5);
  arma::Row<size_t> labels = "0 1 1 0 0";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.5, 1);

  // Verify the initial point is the identity matrix.
  arma::mat initialPoint = lmnnfn.GetInitialPoint();
  for (int row = 0; row < 5; row++)
  {
    for (int col = 0; col < 5; col++)
    {
      if (row == col)
        BOOST_REQUIRE_CLOSE(initialPoint(row, col), 1.0, 1e-5);
      else
        BOOST_REQUIRE_SMALL(initialPoint(row, col), 1e-5);
    }
  }
}

/***
 * Ensure non-seprable objective function is right.
 */
BOOST_AUTO_TEST_CASE(LMNNInitialEvaluationTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  double objective = lmnnfn.Evaluate(arma::eye<arma::mat>(2, 2));

  // Result calculated by hand.
  BOOST_REQUIRE_CLOSE(objective, 9.456, 1e-5);
}

/**
 * Ensure non-seprable gradient function is right.
 */
BOOST_AUTO_TEST_CASE(LMNNInitialGradientTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  arma::mat gradient;
  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  lmnnfn.Gradient(coordinates, gradient);

  // Result calculated by hand.
  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.288, 1e-5);
  BOOST_REQUIRE_SMALL(gradient(1, 0), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(0, 1), 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 12.0, 1e-5);
}

/***
 * Ensure non-seprable EvaluateWithGradient function is right.
 */
BOOST_AUTO_TEST_CASE(LMNNInitialEvaluateWithGradientTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  arma::mat gradient;
  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  double objective = lmnnfn.EvaluateWithGradient(coordinates, gradient);

  // Result calculated by hand.
  BOOST_REQUIRE_CLOSE(objective, 9.456, 1e-5);
  // Check Gradient
  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.288, 1e-5);
  BOOST_REQUIRE_SMALL(gradient(1, 0), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(0, 1), 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 12.0, 1e-5);
}

/**
 * Ensure the separable objective function is right.
 */
BOOST_AUTO_TEST_CASE(LMNNSeparableObjectiveTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  // Result calculated by hand.
  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  BOOST_REQUIRE_CLOSE(lmnnfn.Evaluate(coordinates, 0, 1), 1.576, 1e-5);
  BOOST_REQUIRE_CLOSE(lmnnfn.Evaluate(coordinates, 1, 1), 1.576, 1e-5);
  BOOST_REQUIRE_CLOSE(lmnnfn.Evaluate(coordinates, 2, 1), 1.576, 1e-5);
  BOOST_REQUIRE_CLOSE(lmnnfn.Evaluate(coordinates, 3, 1), 1.576, 1e-5);
  BOOST_REQUIRE_CLOSE(lmnnfn.Evaluate(coordinates, 4, 1), 1.576, 1e-5);
  BOOST_REQUIRE_CLOSE(lmnnfn.Evaluate(coordinates, 5, 1), 1.576, 1e-5);
}

/**
 * Ensure the separable gradient is right.
 */
BOOST_AUTO_TEST_CASE(LMNNSeparableGradientTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  arma::mat gradient(2, 2);

  lmnnfn.Gradient(coordinates, 0, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  lmnnfn.Gradient(coordinates, 1, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  lmnnfn.Gradient(coordinates, 2, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  lmnnfn.Gradient(coordinates, 3, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  lmnnfn.Gradient(coordinates, 4, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  lmnnfn.Gradient(coordinates, 5, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);
}

/**
 * Ensure the separable EvaluateWithGradient function is right.
 */
BOOST_AUTO_TEST_CASE(LMNNSeparableEvaluateWithGradientTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  arma::mat gradient(2, 2);

  double objective = lmnnfn.EvaluateWithGradient(coordinates, 0, gradient, 1);

  BOOST_REQUIRE_CLOSE(objective, 1.576, 1e-5);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  objective = lmnnfn.EvaluateWithGradient(coordinates, 1, gradient, 1);

  BOOST_REQUIRE_CLOSE(objective, 1.576, 1e-5);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  objective = lmnnfn.EvaluateWithGradient(coordinates, 2, gradient, 1);

  BOOST_REQUIRE_CLOSE(objective, 1.576, 1e-5);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  objective = lmnnfn.EvaluateWithGradient(coordinates, 3, gradient, 1);

  BOOST_REQUIRE_CLOSE(objective, 1.576, 1e-5);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  objective = lmnnfn.EvaluateWithGradient(coordinates, 4, gradient, 1);

  BOOST_REQUIRE_CLOSE(objective, 1.576, 1e-5);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);

  objective = lmnnfn.EvaluateWithGradient(coordinates, 5, gradient, 1);

  BOOST_REQUIRE_CLOSE(objective, 1.576, 1e-5);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.048, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 2.0, 1e-5);
}

// Check that final objective value using SGD optimizer is optimal.
BOOST_AUTO_TEST_CASE(LMNNSGDSimpleDatasetTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNN<> lmnn(dataset, labels, 1);

  arma::mat outputMatrix;
  lmnn.LearnDistance(outputMatrix);

  // Ensure that the objective function is better now.
  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  double initObj = lmnnfn.Evaluate(arma::eye<arma::mat>(2, 2));
  double finalObj = lmnnfn.Evaluate(outputMatrix);

  // finalObj must be less than initObj.
  BOOST_REQUIRE_LT(finalObj, initObj);
}

// Check that final objective value using L-BFGS optimizer is optimal.
BOOST_AUTO_TEST_CASE(LMNNLBFGSSimpleDatasetTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNN<SquaredEuclideanDistance, L_BFGS> lmnn(dataset, labels, 1);

  arma::mat outputMatrix;
  lmnn.LearnDistance(outputMatrix);

  // Ensure that the objective function is better now.
  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  double initObj = lmnnfn.Evaluate(arma::eye<arma::mat>(2, 2));
  double finalObj = lmnnfn.Evaluate(outputMatrix);

  // finalObj must be less than initObj.
  BOOST_REQUIRE_LT(finalObj, initObj);
}

double KnnAccuracy(const arma::mat& dataset,
                   const arma::Row<size_t>& labels,
                   const size_t k)
{
  arma::Row<size_t> uniqueLabels = arma::unique(labels);

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  neighbor::KNN knn;

  knn.Train(dataset);
  knn.Search(k, neighbors, distances);

  // Keep count.
  size_t count = 0.0;

  for (size_t i = 0; i < dataset.n_cols; i++)
  {
    arma::vec Map;
    Map.zeros(uniqueLabels.n_cols);

    for (size_t j = 0; j < k; j++)
      Map(labels(neighbors(j, i))) +=
          1 / std::pow(distances(j, i) + 1, 2);

    size_t index = arma::conv_to<size_t>::from(arma::find(Map
        == arma::max(Map)));

    // Increase count if labels match.
    if (index == labels(i))
        count++;
  }

  // return accuracy.
  return ((double) count / dataset.n_cols) * 100;
}

// Check that final accuracy is greater than initial accuracy on
// simple dataset.
BOOST_AUTO_TEST_CASE(LMNNAccuracyTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  // Taking k = 3 as the case of k = 1 can be easily observed.
  double initAccuracy = KnnAccuracy(dataset, labels, 3);

  LMNN<> lmnn(dataset, labels, 2);

  arma::mat outputMatrix;
  lmnn.LearnDistance(outputMatrix);

  double finalAccuracy = KnnAccuracy(outputMatrix * dataset, labels, 3);

  // finalObj must be less than initObj.
  BOOST_REQUIRE_LT(initAccuracy, finalAccuracy);

  // Since this is a very simple dataset final accuracy should be around 100%.
  BOOST_REQUIRE_CLOSE(finalAccuracy, 100.0, 1e-5);
}

// Check that accuracy while learning square distance matrix is the same as when
// we are learning low rank matrix.  I'm ok if this passes only once out of
// three tries.
BOOST_AUTO_TEST_CASE(LMNNLowRankAccuracyLBFGSTest)
{
  bool success = false;
  for (size_t trial = 0; trial < 3; ++trial)
  {
    arma::mat dataPart1;
    dataPart1.randn(5, 50);

    arma::Row<size_t> labelsPart1(50);
    labelsPart1.fill(0);

    arma::mat dataPart2;
    dataPart2.randn(5, 50);

    arma::Row<size_t> labelsPart2(50);
    labelsPart2.fill(1);

    // Generate ordering.
    arma::uvec ordering = arma::shuffle(arma::linspace<arma::uvec>(0, 99, 100));

    // Generate datasets.
    arma::mat dataset = join_rows(dataPart1, dataPart2);
    dataset = dataset.cols(ordering);

    // Generate labels.
    arma::Row<size_t> labels = join_rows(labelsPart1, labelsPart2);
    labels = labels.cols(ordering);

    LMNN<SquaredEuclideanDistance, L_BFGS> lmnn(dataset, labels, 1);

    // Learn a square matrix.
    arma::mat outputMatrix;
    lmnn.LearnDistance(outputMatrix);

    double acc1 = KnnAccuracy(outputMatrix * dataset, labels, 1);

    // Learn a low rank matrix.
    outputMatrix = arma::randu(4, 5);
    lmnn.LearnDistance(outputMatrix);

    double acc2 = KnnAccuracy(outputMatrix * dataset, labels, 1);

    // We keep the tolerance very high.  We need to ensure the accuracy drop
    // isn't any more than 10%.
    success = ((acc1 - acc2) <= 10.0);
    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

// Check that accuracy while learning square distance matrix is the same as when
// we are learning low rank matrix.  I'm ok if this passes only once out of
// three tries.
BOOST_AUTO_TEST_CASE(LMNNLowRankAccuracyTest)
{
  bool success = false;
  for (size_t trial = 0; trial < 3; ++trial)
  {
    arma::mat dataPart1;
    dataPart1.randn(5, 50);

    arma::Row<size_t> labelsPart1(50);
    labelsPart1.fill(0);

    arma::mat dataPart2;
    dataPart2.randn(5, 50);

    arma::Row<size_t> labelsPart2(50);
    labelsPart2.fill(1);

    // Generate ordering.
    arma::uvec ordering = arma::shuffle(arma::linspace<arma::uvec>(0, 99, 100));

    // Generate datasets.
    arma::mat dataset = join_rows(dataPart1, dataPart2);
    dataset = dataset.cols(ordering);

    // Generate labels.
    arma::Row<size_t> labels = join_rows(labelsPart1, labelsPart2);
    labels = labels.cols(ordering);

    LMNN<> lmnn(dataset, labels, 1);

    // Learn a square matrix.
    arma::mat outputMatrix;
    lmnn.LearnDistance(outputMatrix);

    double acc1 = KnnAccuracy(outputMatrix * dataset, labels, 1);

    // Learn a low rank matrix.
    outputMatrix = arma::randu(4, 5);
    lmnn.LearnDistance(outputMatrix);

    double acc2 = KnnAccuracy(outputMatrix * dataset, labels, 1);

    // We keep the tolerance very high.  We need to ensure the accuracy drop
    // isn't any more than 10%.
    success = ((acc1 - acc2) <= 10.0);
    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

// Check that accuracy while learning square distance matrix is the same as when
// we are learning low rank matrix.  I'm ok if this passes only once out of
// five tries, since BBSGD seems to have a harder time converging.
/*
BOOST_AUTO_TEST_CASE(LMNNLowRankAccuracyBBSGDTest)
{
  bool success = false;
  for (size_t trial = 0; trial < 5; ++trial)
  {
    arma::mat dataPart1;
    dataPart1.randn(5, 50);

    arma::Row<size_t> labelsPart1(50);
    labelsPart1.fill(0);

    arma::mat dataPart2;
    dataPart2.randn(5, 50);

    arma::Row<size_t> labelsPart2(50);
    labelsPart2.fill(1);

    // Generate ordering.
    arma::uvec ordering = arma::shuffle(arma::linspace<arma::uvec>(0, 99, 100));

    // Generate datasets.
    arma::mat dataset = join_rows(dataPart1, dataPart2);
    dataset = dataset.cols(ordering);

    // Generate labels.
    arma::Row<size_t> labels = join_rows(labelsPart1, labelsPart2);
    labels = labels.cols(ordering);

    LMNN<SquaredEuclideanDistance, BigBatchSGD<>> lmnn(dataset, labels, 1);

    // Learn a square matrix.
    arma::mat outputMatrix;
    lmnn.LearnDistance(outputMatrix);

    double acc1 = KnnAccuracy(outputMatrix * dataset, labels, 1);

    // Learn a low rank matrix.
    outputMatrix = arma::randu(4, 5);
    lmnn.LearnDistance(outputMatrix);

    double acc2 = KnnAccuracy(outputMatrix * dataset, labels, 1);
    if (acc2 < 5)
      std::cout << "super fail\n" << outputMatrix << std::endl;

    // We keep the tolerance very high.  We need to ensure the accuracy drop
    // isn't any more than 10%.
    success = ((acc1 - acc2) <= 10.0);
    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}
*/

// Comprehensive gradient tests by Marcus Edel & Ryan Curtin.

// Simple numerical gradient checker.
template<class FunctionType>
double CheckGradient(FunctionType& function,
                     arma::mat& coordinates,
                     const double eps = 1e-7)
{
  // Get gradients for the current parameters.
  arma::mat orgGradient, gradient, estGradient;
  function.Gradient(coordinates, orgGradient);

  estGradient = arma::zeros(orgGradient.n_rows, orgGradient.n_cols);

  // Compute numeric approximations to gradient.
  for (size_t i = 0; i < orgGradient.n_elem; ++i)
  {
    double tmp = coordinates(i);

    // Perturb parameter with a positive constant and get costs.
    coordinates(i) += eps;
    double costPlus = function.Evaluate(coordinates);

    // Perturb parameter with a negative constant and get costs.
    coordinates(i) -= (2 * eps);
    double costMinus = function.Evaluate(coordinates);

    // Restore the parameter value.
    coordinates(i) = tmp;

    // Compute numerical gradients using the costs calculated above.
    estGradient(i) = (costPlus - costMinus) / (2 * eps);
  }

  // Estimate error of gradient.
  return arma::norm(orgGradient - estGradient) /
      arma::norm(orgGradient + estGradient);
}

BOOST_AUTO_TEST_CASE(LMNNFunctionGradientTest)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  // 10 trials with random positions.
  for (size_t i = 0; i < 10; ++i)
  {
    arma::mat coordinates(2, 2, arma::fill::randn);
    CheckGradient(lmnnfn, coordinates);
  }
}

BOOST_AUTO_TEST_CASE(LMNNFunctionGradientTest2)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset        = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  // 10 trials with random positions.
  for (size_t i = 0; i < 10; ++i)
  {
    arma::mat coordinates(2, 2, arma::fill::randu);
    CheckGradient(lmnnfn, coordinates);
  }
}

BOOST_AUTO_TEST_CASE(LMNNFunctionGradientTest3)
{
  arma::mat dataset;
  arma::Row<size_t> labels;
  data::Load("iris.csv", dataset);
  data::Load("iris_labels.txt", labels);

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  // 10 trials with random positions.
  for (size_t i = 0; i < 10; ++i)
  {
    arma::mat coordinates(dataset.n_rows, dataset.n_rows, arma::fill::randn);
    CheckGradient(lmnnfn, coordinates);
  }
}

BOOST_AUTO_TEST_CASE(LMNNFunctionGradientTest4)
{
  arma::mat dataset;
  arma::Row<size_t> labels;
  data::Load("iris.csv", dataset);
  data::Load("iris_labels.txt", labels);

  LMNNFunction<> lmnnfn(dataset, labels, 1, 0.6, 1);

  // 10 trials with random positions.
  for (size_t i = 0; i < 10; ++i)
  {
    arma::mat coordinates(dataset.n_rows, dataset.n_rows, arma::fill::randu);
    CheckGradient(lmnnfn, coordinates);
  }
}

BOOST_AUTO_TEST_SUITE_END();


/*
//@HEADER
// ************************************************************************
//
// tutorial1.cc
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#include <array>
#include <Eigen/Core>

#include "pressio/type_traits.hpp"
#include "pressio/ops.hpp"

struct MyCustomVector
{
  MyCustomVector(std::size_t ext) : d_(ext){}

  double & operator()(int i){ return d_[i]; }
  const double & operator()(int i)const { return d_[i]; }

  std::size_t extent(int k)const { return (k==0) ? d_.size() : 0; }

  void fill(double value){
    std::for_each(d_.begin(), d_.end(), [](double & v){ v= 0.; });
  }

private:
  std::vector<double> d_ = {};
};

struct MyCustomMatrix
{
  MyCustomMatrix(std::size_t nr, std::size_t nc)
    : num_rows_(nr), num_cols_(nc), d_(nr*nc){}

  std::size_t extent(int k)const { return (k==0) ? num_rows_ : num_cols_; }

  double & operator()(int i, int j){ return d_[num_cols_*i+j]; }
  const double & operator()(int i, int j) const { return d_[num_cols_*i+j]; }

  void fill(double value){
    std::for_each(d_.begin(), d_.end(), [=](double & v){ v=value; });
  }

private:
  std::size_t num_rows_ = {};
  std::size_t num_cols_ = {};
  std::vector<double> d_ = {};
};

struct MyRosenbrockSystem
{
  using scalar_type   = double;
  using state_type    = Eigen::VectorXd;
  using residual_type = MyCustomVector;
  using jacobian_type = MyCustomMatrix;

  residual_type createResidual() const{ return residual_type(6);   }
  jacobian_type createJacobian() const{ return jacobian_type(6, 4);}

  void residual(const state_type& x, residual_type & res) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    const auto & x4 = x(3);
    res(0) = 10.*(x4 - x3*x3);
    res(1) = 10.*(x3 - x2*x2);
    res(2) = 10.*(x2 - x1*x1);
    res(3) = (1.-x1);
    res(4) = (1.-x2);
    res(5) = (1.-x3);
  }

  void jacobian(const state_type & x, jacobian_type & JJ) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    JJ.fill(0.);

    JJ(0,2) = -20.*x3;
    JJ(0,3) = 10.;
    JJ(1,1) = -20.*x2;
    JJ(1,2) = 10.;
    JJ(2,0) = -20.*x1;
    JJ(2,1) = 10.;
    JJ(3,0) = -1.;
    JJ(4,1) = -1.;
    JJ(5,2) = -1.;
  }
};

namespace pressio{
template<> struct Traits<MyCustomVector>{
  using scalar_type = double;
};

template<> struct Traits<MyCustomMatrix>{
  using scalar_type = double;
};

namespace ops{
MyCustomVector clone(const MyCustomVector & src){ return src; }
MyCustomMatrix clone(const MyCustomMatrix & src){ return src; }

void set_zero(MyCustomVector & o){ o.fill(0); }
void set_zero(MyCustomMatrix & o){ o.fill(0); }

double norm2(const MyCustomVector & v){
  double norm{0};
  for (std::size_t i=0; i<v.extent(0); ++i){
    norm += v(i)*v(i);
  }
  return std::sqrt(norm);
}

template<class HessianType>
void product(pressio::transpose, pressio::nontranspose,
	     const double alpha,
	     const MyCustomMatrix & A,
	     const double beta,
	     HessianType & H)
{
  for (std::size_t i=0; i<A.extent(1); ++i){
    for (std::size_t j=0; j<A.extent(1); ++j)
    {
      H(i,j) *= beta;
      for (std::size_t k=0; k<A.extent(0); ++k){
	H(i,j) += alpha * A(k,i) * A(k,j);
      }
    }
  }
}

template<class GradientType>
void product(pressio::transpose,
	     const double alpha,
	     const MyCustomMatrix & A,
	     const MyCustomVector & b,
	     const double beta,
	     GradientType & g)
{
  for (int i=0; i<g.rows(); ++i){
    g(i) *= beta;
    for (std::size_t k=0; k<A.extent(0); ++k){
      g(i) += alpha * A(k,i) * b(k);
    }
  }
}

template<class HessianType>
HessianType product(pressio::transpose, pressio::nontranspose,
	     double alpha,
	     const MyCustomMatrix & A)
{
  HessianType H(A.extent(1), A.extent(1));
  product(pressio::transpose(), pressio::nontranspose(), alpha, A, 0, H);
  return H;
}

void update(MyCustomVector & v, double a, const MyCustomVector & v1, double b)
{
  for (std::size_t i=0; i<v.extent(0); ++i){
    v(i) = v(i)*a + b*v1(i);
  }
}

void scale(MyCustomVector & v, double factor){
  for (std::size_t i=0; i<v.extent(0); ++i){
    v(i) = v(i)*factor;
  }
}
}}//end namespace pressio::ops

#include "pressio/solvers_linear.hpp"
#include "pressio/solvers_nonlinear.hpp"

int main()
{
  namespace plog   = pressio::log;
  namespace pls    = pressio::linearsolvers;
  namespace pnonls = pressio::nonlinearsolvers;
  plog::initialize(pressio::logto::terminal);
  plog::setVerbosity({plog::level::info});

  using problem_t = MyRosenbrockSystem;
  problem_t problem;

  using state_t   = Eigen::VectorXd;
  state_t x(4);
  x[0] = -0.05; x[1] = 1.1; x[2] = 1.2; x[3] = 1.5;

  using hessian_t    = Eigen::MatrixXd;
  using lin_tag      = pls::direct::HouseholderQR;
  using lin_solver_t = pls::Solver<lin_tag, hessian_t>;
  lin_solver_t linSolver;

  auto gnSolver = pnonls::create_gauss_newton(problem, x, linSolver);
  gnSolver.setTolerance(1e-5);
  gnSolver.solve(problem, x);
  std::cout << std::setprecision(14) << x << std::endl;
  // check solution
  std::cout << "Computed solution: \n "
            << "[" << x(0) << " " << x(1) << " " << x(2) << " " << x(3) << " " << "] \n"
            << "Expected solution: \n "
            << "[1.0000000156741, 0.99999999912477, 0.99999999651993, 0.99999998889888]"
            << std::endl;

  std::vector<double> gold = {
    1.00000001567414e+00,
    9.99999999124769e-01,
    9.99999996519930e-01,
    9.99999988898883e-01};

  std::string sentinel = "PASSED";
  const auto e1 = std::abs(x(0) - gold[0]);
  const auto e2 = std::abs(x(1) - gold[1]);
  const auto e3 = std::abs(x(2) - gold[2]);
  const auto e4 = std::abs(x(3) - gold[3]);
  if (e1>1e-6 or e2>1e-6  or e3>1e-6 or e4>1e-6){
    sentinel = "FAILED";
  }
  std::cout << sentinel << std::endl;

  plog::finalize();
  return 0;
}


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

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
inline void strided_laplacian_setup(Matrix& A, unsigned m, unsigned n)
{
    A.change_dim(2*m*n, 2*m*n);
    set_to_zero(A);
    mtl::mat::inserter<Matrix>      ins(A, 5);

    for (unsigned i= 0; i < m; i++)
	for (unsigned j= 0; j < n; j++) {
	    typename mtl::Collection<Matrix>::value_type four(4.0), minus_one(-1.0);
	    unsigned row= 2 * (i * n + j);
	    ins(row, row) << four;
	    if (j < n-1) ins(row, row+2) << minus_one;
	    if (i < m-1) ins(row, row+2*n) << minus_one;
	    if (j > 0) ins(row, row-2) << minus_one;
	    if (i > 0) ins(row, row-2*n) << minus_one;
	}
    for (unsigned i= 1; i < num_rows(A); i+= 2)
	ins(i, i) << 2;
}

int main()
{
    using mtl::srange; using mtl::imax;
    // For a more realistic example set sz to 1000 or larger
    const int size = 3, N = 2 * size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    typedef itl::pc::ic_0<matrix_type> ic_type;

    mtl::compressed2D<double>          A;
    strided_laplacian_setup(A, size, size);
    mtl::io::tout << "A (merged diagonal and Laplace) is\n" << A << '\n';

    mtl::dense_vector<bool> tags= make_tag_vector(N, srange(0, imax, 2));
    itl::pc::sub_matrix_pc<ic_type, matrix_type> P(tags, A);

    mtl::dense_vector<double>          x(N, 1.0), b(N);

    b = A * x;
    x= 0;

    itl::cyclic_iteration<double> iter(b, N, 1.e-6, 0.0, 3);
    cg(A, x, b, P, iter);

    // Test if adjoint works
    x= 0;
    itl::cyclic_iteration<double> iter2(b, N, 1.e-6, 0.0, 3);
    bicg(A, x, b, P, iter2);

    return 0;
}


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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/gamma.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void limit_test(Env& $)
{
  using p_t = bs::pack<T, N>;

  STF_ULP_EQUAL (bs::gamma(p_t(1))            , p_t(1), 0.5);
  STF_IEEE_EQUAL(bs::gamma(p_t(0))            , p_t(bs::Inf<T>())        );
  STF_IEEE_EQUAL(bs::gamma(p_t(bs::Inf<T>())) , bs::Inf<p_t>());
  STF_IEEE_EQUAL(bs::gamma(p_t(bs::Minf<T>())), p_t(bs::Nan<T>())        );
}

STF_CASE_TPL("Check gamma limit cases" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  limit_test<T, N>($);
  limit_test<T, N/2>($);
  limit_test<T, N*2>($);
}

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : -T(i);
    b[i] = bs::gamma(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_ULP_EQUAL(bs::gamma(aa1), bb, 0.5);
}

STF_CASE_TPL("Check gamma on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
   test<T, N>($);
   test<T, N/2>($);
   test<T, N*2>($);
}


#pragma once
#ifndef ENHANCED_MADGWICK_HPP
#define ENHANCED_MADGWICK_HPP

#include <Eigen/Geometry>

/** This enhanced version of the Madgwick filter is a combination of ideas taken from Madgwick et al [2]
 *	and Admiraal et al [1]. Admiraal improves upon the original Madgwick filter by deriving a steepest
 *	(as apposed to gradient) descent formulation for calculating the quaternion estimate update
 *	direction. This eliminates some of the numerical errors in Madgwick's version while also improving speed
 *	and precision.
 *
 *	The notation used for documentation is as follows: [x](y)
 *	Where 'x' is the source and 'y' is the equation number
 *
 *	[1] 	M. Admiraal, S. Wilson and R. Vaidyanathan, "Improved Formulation of the IMU and MARG
 *			Orientation Gradient Descent Algorithm for Motion Tracking in Human-Machine Interfaces," in
 *			International Conference on Multisensor Fusion and Integration for Intelligent Systems, Daegu,
 *			2017.
 *
 *	[2] 	S. O. Madgwick, A. J. Harrison and R. Vaidyanathan, "Estimation of IMU and MARG Orientation
 *			Using A Gradient Descent Algorithm," in International Conference on Rehabilitation Robotics,
 *			Zurich, 2011.
 *
 **/
template<typename T>
class EnhancedMadgwick
{
public:

	EnhancedMadgwick()
	{
		qAccel.normalize();
		qMag.normalize();
	}
	~EnhancedMadgwick() = default;

private:
	Eigen::Quaternion<T> qAccel;
	Eigen::Quaternion<T> qMag;

	/** Calculates the gradient of the error function when referenced to gravity [1](35). This is then
	 *	used in the gradient descent algorithm to take a single step. This calculation assumes the
	 *	acceleration reference vector is [0, 0, -1] because, ya know, gravity.
	 *
	 *	@param[in]	estAccelQuaternion	The current quaternion estimate of orientation from accelerometer data
	 *	@param[in]	measuredAccelVector	A column matrix [x,y,z]' with measured accelerometer data
	 *	@return A quaternion representing the estimated orientation gradient
	 **/
	Eigen::Quaternion<T> accelGradient(Eigen::Quaternion<T>& estAccelQuaternion, Eigen::Matrix<T, 3, 1> measuredAccelVector)
	{
		Eigen::Matrix<T, 4, 1> result;
		Eigen::Matrix<T, 4, 3> leftProduct;
		Eigen::Matrix<T, 3, 1> rightProduct;

		T qw = estAccelQuaternion.w();
		T qx = estAccelQuaternion.x();
		T qy = estAccelQuaternion.y();
		T qz = estAccelQuaternion.z();
		T vmx = measuredAccelVector[0];
		T vmy = measuredAccelVector[1];
		T vmz = measuredAccelVector[2];

		leftProduct << 2 * (
			 qy, -qx, -qw,
		    -qz, -qw,  qx,
			 qw, -qz,  qy,
			-qx, -qy, -qz
			);

		rightProduct <<
			 2 * qw*qy, -2 * qx*qz, -vmx,
			-2 * qw*qx - 2 * qy*qz - vmy,
			-qw * qw + qx * qx + qy * qy - qz * qz - vmx;

		result = leftProduct * rightProduct;
		return Eigen::Quaternion<T>(result[0], result[1], result[2], result[3]);
	}

	/** Calculates the gradient of the error function when referenced to the earth's magnetic field. This
	 *	is under the assumption that the field is perfectly planar in local space and can be represented
	 *	by the vector [Vrx, 0, Vrz]. [1](36)
	 *
	 **/
	Eigen::Quaternion<T> magGradient(Eigen::Quaternion<T>& estMagQuaternion, Eigen::Matrix<T, 3, 1> measuredMagVector, Eigen::Matrix<T, 3, 1> refMagVector)
	{
		Eigen::Matrix<T, 4, 1> result;
		Eigen::Matrix<T, 4, 3> leftProduct;
		Eigen::Matrix<T, 3, 1> rightProduct;

		T qw = estMagQuaternion.w();
		T qx = estMagQuaternion.x();
		T qy = estMagQuaternion.y();
		T qz = estMagQuaternion.z();
		T vmx = measuredMagVector[0];
		T vmy = measuredMagVector[1];
		T vmz = measuredMagVector[2];
		T vrx = refMagVector[0];
		T vry = 0;
		T vrz = refMagVector[2];

		leftProduct << 2 * (
			( vrx*qw - vrz*qy), (-vrx*qz + vrz*qx), (vrx*qy + vrz*qw),
			( vrx*qx + vrz*qz), ( vrx*qy + vrz*qw), (vrx*qz - vrz*qx),
			(-vrx*qy - vrz*qw), ( vrx*qx + vrz*qz), (vrx*qw - vrz*qy),
			(-vrx*qz + vrz*qx), (-vrx*qw + vrz*qy), (vrx*qx + vrz*qz)
			);

		rightProduct <<
			vrx * (qw*qw + qx*qx - qy*qy - qz*qz) + vrz * (-2*qw*qy + 2*qx*qz)            - vmx,
			vrx * (-2*qw*qz + 2*qx*qy)			  + vrz * (2*qw*qx + 2*qy*qz)			  - vmy,
			vrx * (2*qw*qy + 2*qx*qz)			  + vrz * (qw*qw - qx*qx - qy*qy + qz*qz) - vmz;

		result = leftProduct * rightProduct;
		return Eigen::Quaternion<T>(result[0], result[1], result[2], result[3]);
	}

};

#endif /* !ENHANCED_MADGWICK_HPP */


//  Copyright (c) 2015 Boost.Test team
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE tolerance_04
#include <boost/test/included/unit_test.hpp>
#include <boost/rational.hpp>
namespace utf = boost::unit_test;
namespace tt = boost::test_tools;

namespace boost { namespace math { namespace fpc {

  template <typename I>
  struct tolerance_based< rational<I> > : boost::true_type{};

} } }

typedef boost::rational<int> ratio;

BOOST_AUTO_TEST_CASE(test1, * utf::tolerance(ratio(1, 1000)))
{
  ratio x (1002, 100); // 10.02
  ratio y (1001, 100); // 10.01
  ratio z (1000, 100); // 10.00

  BOOST_TEST(x == y);  // irrelevant diff by default
  BOOST_TEST(x == y, tt::tolerance(ratio(1, 2000)));

  BOOST_TEST(x != z);  // relevant diff by default
  BOOST_TEST(x != z, tt::tolerance(ratio(2, 1000)));
}
//]


#define BOOST_TEST_DYN_LINK
#include <Eigen/Dense>
#include <boost/test/unit_test.hpp>
#include "matrixIO.hpp"

struct matrixIOFixture {
  matrixIOFixture()
  {
    A = Eigen::MatrixXd(3, 3);
    A << 1, 2, 3,
        4, 5, 6,
        7, 8, 9;
  }

  Eigen::MatrixXd A;
  int             matrix_size                = 3;
  std::string     valid_matrix_file          = "../data/matrix_valid.csv";
  std::string     invalid_format_matrix_file = "../data/matrix_invalid_format.csv";
  std::string     invalid_cols_matrix_file   = "../data/matrix_invalid_cols.csv";
  std::string     invalid_rows_matrix_file   = "../data/matrix_invalid_rows.csv";
};

BOOST_FIXTURE_TEST_SUITE(matrixIOTests, matrixIOFixture)

BOOST_AUTO_TEST_CASE(open_valid_data)
{
  Eigen::MatrixXd read = matrixIO::openData(valid_matrix_file, matrix_size);
  BOOST_TEST(read == A);
}

BOOST_AUTO_TEST_CASE(open_invalid_format_data)
{
  BOOST_REQUIRE_THROW(matrixIO::openData(invalid_format_matrix_file, matrix_size), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(open_invalid_rows_data)
{
  BOOST_REQUIRE_THROW(matrixIO::openData(invalid_rows_matrix_file, matrix_size), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(open_invalid_cols_data)
{
  BOOST_REQUIRE_THROW(matrixIO::openData(invalid_cols_matrix_file, matrix_size), std::runtime_error);
}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
// monomials_horner::example::monomials.cpp                                  //
//                                                                           //
//  (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)         //
///////////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range.hpp>
#include <boost/monomials_horner/monomials.hpp>
#include <libs/monomials_horner/example/monomials.h>

void example_monomials(std::ostream& out){
    out << "-> example_monomials" << std::endl;

    using namespace boost;
    using namespace monomials_horner;

    typedef std::vector<double> vars_type;
    typedef monomials<vars_type> monoms_type;

    unsigned max_d = 5;
    unsigned max_p = 5;    vars_type vars;
    {
        using namespace boost::assign;
        vars += 0.0,1.0,2.0,3.0,4.0;
    }

    for(unsigned d = 1; d<max_d; d++){
        out << " -> d=" << d << std::endl;
        vars_type::iterator e = boost::begin(vars);
        std::advance(e,d);
        vars_type sub;
        copy(boost::begin(vars),e,back_inserter(sub));
        BOOST_ASSERT(boost::size(sub)-d==0);

        for(unsigned int p = 1; p<max_p; p++){
            out << "  ->p=" << p << std::endl;
            //out << boost::math::binomial_coefficient<double>(p+d,d)
            //<< std::endl;

            monoms_type monoms;

            monoms(sub,p);
            copy(
                boost::begin(monoms()),
                boost::end(monoms()),
                std::ostream_iterator<double>(out," ")
            );
            out << "  <-" << std::endl;
        }
            out << " <-" << std::endl;
    }
    out << "<-" << std::endl;
}


/*
 * File:   geometry.hpp
 * Copyright (C) 2014  K M Masum Habib <masum.habib@gmail.com>
 *
 * Created on February 3, 2014, 1:45 PM
 */

#ifndef GEOMETRY_HPP
#define	GEOMETRY_HPP

#include "utils/std.hpp"
#include "utils/Printable.hpp"

#include <boost/geometry.hpp>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>

#include <vector>
#include <algorithm>

namespace maths{
namespace geometry{

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
using namespace utils::stds;
using utils::Printable;

typedef bg::model::point<double, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef bg::model::ring<point> ring;
typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
typedef polygon::ring_type polyring;
typedef bg::strategy::within::franklin<point, point, double> stwithin;

/*
 * Simple Quadrilateral
 */
struct SimpleQuadrilateral: public Printable{
    point lb;
    point rb;
    point rt;
    point lt;

    SimpleQuadrilateral(const point &lb, const point &rb, const point &rt,
    const point &lt, const string &prefix = ""):Printable(" " + prefix),
        lb(lb), rb(rb), rt(rt), lt(lt){

        // Commented out since it is buggy
        // correct();
    }

    // sort the points.
    // @TODO: Does not alway work. Fix bug.
    void correct(){
        using std::sort;
        using std::vector;
        vector<point> p;
        p.push_back(lb);
        p.push_back(rb);
        p.push_back(rt);
        p.push_back(lt);

        sort(p.begin(), p.end(), OnRight());

        if (p[0].get<1>() < p[1].get<1>()){
            lb = p[0];
            lt = p[1];
        }else{
            lb = p[1];
            lt = p[0];
        }

        if (p[2].get<1>() < p[3].get<1>()){
            rb = p[2];
            rt = p[3];
        }else{
            rb = p[3];
            rt = p[2];
        }
    }

    virtual string toString() const{
        stringstream out;
        out.precision(3);
        out << "(" << lb.get<0>() << " " << lb.get<1>() << ", "
                   << rb.get<0>() << " " << rb.get<1>() << ", "
                   << rt.get<0>() << " " << rt.get<1>() << ", "
                   << lt.get<0>() << " " << lt.get<1>() << ")";
        return out.str();
    }

protected:
    struct OnRight{
        bool operator()(const point &p1, const point &p2){
            return (p1.get<0>() < p2.get<0>());
        }
    };
};

typedef SimpleQuadrilateral squadrilateral;
typedef SimpleQuadrilateral quadrilateral;

}
}
#endif	/* GEOMETRY_HPP */


// Filename: dot_example.cpp (part of MTL4)

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

int main(int, char**)
{
    using namespace mtl;

    typedef std::complex<double>  cdouble;
    dense_vector<cdouble>         v(10000), x(10, cdouble(3, 2));
    dense_vector<double>          w(10000);

    for (unsigned i= 0; i < size(v); i++)
	v[i]= cdouble(i+1, 10000-i), w[i]= 2 * i + 2;

    std::cout << "dot(v, w) is " << dot(v, w)<< "\n";

    std::cout << "dot<6>(v, w) is " <<  dot<6>(v, w)<< "\n";

    std::cout << "dot_real<6>(v, w) is " <<  dot_real<6>(v, w)<< "\n";

    std::cout << "conj(x) is " <<  mtl::conj(x)<< "\n"; // ADL doesn't work here in g++ 4.4

    return 0;
}


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             :
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2019.01.08, William Yu
                             ver1.1, 2019.01.13, William Yu, add notes
=================================================================================*/

/*-----------------------------[Note]---------------------------
 *
--------------------------------------------------------------*/

/// Include Files
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

/// Global Variables

/// Function Declaration

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches
);

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
);

/// Function definitions

/**
 * @function main
 * @brief
 */
int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 内参矩阵
    //--调参：相机内参--
    //        | fx,  0, cx, | cx主点偏移
    //   K =  |  0, fy, cy, |
    //        |  0,  0,  1  |
    Mat K = ( Mat_<double> ( 3,3 ) << 518.0,     0, 325.5,
                                          0, 519.0, 253.5,
                                          0,     0,     1 );
    double depthScale = 1000.0;

    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像

    vector<Point3f> pts1, pts2;
    for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth //深度值为0，无法使用
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );//是测试图像的特征点描述子（descriptor）的下标，同时也是描述符对应特征点（keypoint)的下标。
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );//是样本图像的特征点描述子的下标，同样也是相应的特征点的下标。
        float dd1 = float ( d1 ) /depthScale;
        float dd2 = float ( d2 ) /depthScale;
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d 点对: "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );
    cout<<"---------------------------------------------"<<endl;
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R_12 = "<<R<<endl;
    cout<<"t_12 = "<<t<<endl;
    /*-----------------------------[Note]---------------------------
    * pts1 = R*pts2 + t
    * 则有pts2 = R_inv*pts1 - R_inv*t
    * 对应pts2 = R`*pts1 + t`
    * 即逆运动R` = R_inv
    *        t` = - R_inv*t
    * Notice:旋转矩阵R为正交矩阵，
    * 所有又有 R^(-1) = R^T，逆等于转置
    --------------------------------------------------------------*/
    cout<<"逆运动R：R` = R_inv = "<<R.t() <<endl;   //Mat.t()为矩阵求转置
    //cout<<"逆运动R：R` = R_inv = "<<R.inv() <<endl;  //Mat.inv()为矩阵求逆
    cout<<"逆运动t：t` = - R_inv*t = "<<-R.t() *t <<endl;

    cout<<"---------------------------------------------"<<endl;
    //--校验5组 p1 = R*p2 + t  和  p2 = R_inv*p1 - R_inv*t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t << endl;
        cout<<"(R`*p1+t`) = "<<
            R.inv() * (Mat_<double>(3,1)<<pts1[i].x, pts1[i].y, pts1[i].z) -R.inv() *t << endl<< endl;
    }
}

/**
 * @function find_feature_matches
 * @brief 特征点匹配与筛选
 * @param  const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& good_matches
 * @retval None
 */

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& good_matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create("BruteForce-Hamming");

    //--[1]:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //--[2]:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //绘制特征点
    Mat outimg1, outimg2;
    drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    namedWindow("ORB_img1", WINDOW_NORMAL);
    imshow("ORB_img1",outimg1);
    drawKeypoints( img_2, keypoints_2, outimg2, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    namedWindow("ORB_img2", WINDOW_NORMAL);
    imshow("ORB_img2",outimg2);
    cout<<"Img1特征点数"<<keypoints_1.size() <<endl;//500个特征点
    cout<<"Img2特征点数"<<keypoints_2.size() <<endl;

    //--[3]:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> matches;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, matches );
    cout<<"初步匹配"<<matches.size() <<"对匹配点"<<endl; //500对匹配

    //--[4]:匹配点对筛选
    //--筛选要求：1.确保都是正确匹配，删去错误匹配
    //          2.确保正确匹配的前提下，保证点对数足够多

    //--调参：筛选阈值--
    double min_dist=10000, max_dist=0;
    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = matches[i].distance;
        if ( dist < min_dist ) min_dist = dist;//最短距离，最相似
        if ( dist > max_dist ) max_dist = dist;//最长距离，最不相似
    }
    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //--调参：筛选阈值--
    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.
    //但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            good_matches.push_back ( matches[i] );
        }
    }

    //--TODO：调参：逐点检查--

    //--[5]:绘制匹配结果
    Mat img_match;
    Mat img_goodmatch;
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );
    namedWindow("img_match", WINDOW_NORMAL);
    namedWindow("img_goodmatch", WINDOW_NORMAL);
    imshow ( "img_match", img_match );
    imshow ( "img_goodmatch", img_goodmatch );
    waitKey(0);
}

/**
 * @function pixel2cam
 * @brief 像素坐标转相机归一化坐标
 * @param  const Point2d& p, const Mat& K
 * @retval Point2d
 */
Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

/**
 * @function pose_estimation_3d3d
 * @brief   求解R、t
 * @param   const vector<Point3f>& pts1,
            const vector<Point3f>& pts2,
            Mat& R, Mat& t
 * @retval None
 */
void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    if (U.determinant() * V.determinant() < 0)
	{
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
	}

    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}