/*
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/epnp/epnp.hpp"

#include <Eigen/SVD>

#include <cmath>
#include <utility>

namespace isaac {
namespace pnp {
namespace epnp {

// High-level function that implements the full EPnP pipeline
pnp::Status ComputeCameraPose(double focal_u, double focal_v, double principal_u,
                              double principal_v, const Matrix3Xd& points3,
                              const Matrix2Xd& points2, epnp::Result* result) {
  if (result == nullptr) {
    return pnp::Status::kErrorBadInputParams;
  }

  // Make sure focal lengths in pixels are non-zero.
  // There is no restriction on the principal point location.
  if (focal_u == 0 || focal_v == 0) {
    return pnp::Status::kErrorBadInputParams;
  }

  // At least 6 2D-3D point correspondences are required currently.
  // TODO: extend to 5 and 4 (requires complicated relinearization)
  if (points2.cols() != points3.cols() || points3.cols() < 6) {
    return pnp::Status::kErrorBadNumInputPoints;
  }

  // Compute control points in world coordinates. They represent a chosen 3D basis.
  constexpr double tol = 1e-3;
  result->ctl_points_world = ChooseBasis(points3, tol);

  // The dimensionality of the point cloud is the number of control points minus one because
  // the first control point is always the centroid.
  // Except when zero input points passed to ChooseBasis().
  result->input_dims = result->ctl_points_world.cols();
  if (result->input_dims) {
    result->input_dims--;
  }

  // Only planar and 3D input point cloud shapes are useful for camera pose estimation.
  if (result->input_dims < 2 || result->input_dims > 3) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  // Compute barycentric coordinates of all input 3D points with respect to chosen basis.
  result->bary_coeffs = ComputeBaryCoords(points3, result->ctl_points_world);
  if (result->bary_coeffs.cols() == 0) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  // Calculate space of the control point camera coordinates such that all
  // projection constraints are approximately satisified.
  if (!SolveProjConstraints(focal_u, focal_v, principal_u, principal_v, result->bary_coeffs,
                            points2, &result->proj_coeffs, &result->solution_basis,
                            &result->singular_values)) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  // Precompute distances between control points in the world frame.
  VectorXd ctl_distances = ComputeDistances(result->ctl_points_world);

  // Construct calibration matrix needed for reprojections at residual calculation.
  Matrix3d calib_matrix;
  calib_matrix << focal_u, 0, principal_u, 0, focal_v, principal_v, 0, 0, 1;

  // Given the solution basis, solve for control point camera coordinates.
  // Solve for various possible dimensions of the solution space and retain
  // the resulting pose that has the least reprojection error.
  const int max_solution_dims = 3;  // Only 1,2,3 are supported
  double min_rss_error = Inf;
  result->rss_repr_errors = VectorXd::Constant(max_solution_dims, Inf);
  for (int k = 0; k < max_solution_dims; k++) {
    const int sol_dims = k + 1;

    // Compute weights that describe the concrete solution in the solution space.
    Vector4d weights = SolveControlPoints(result->solution_basis, sol_dims, ctl_distances);
    if (weights == Vector4d::Zero()) {
      continue;
    }

    // Calculate control points in camera frame given the weights and the base vectors.
    Matrix3Xd ctl_points_cam = ReshapeToMatrix3xN(result->solution_basis * weights);

    // Compute rigid-body transform between control points in world and in camera frame.
    // The resulting transformation is the sought camera pose.
    Matrix3d rotation;
    Vector3d translation;
    if (!pnp::RegisterPointsEucl(result->ctl_points_world, ctl_points_cam, &rotation,
                                 &translation)) {
      continue;
    }

    // Compute squared reprojection errors of the input points, given the computed camera pose.
    VectorXd repr_errors = pnp::ColwiseSquaredNorms(
        points2 - pnp::EuclideanFromHomogeneous(calib_matrix *
                                                ((rotation * points3).colwise() + translation)));

    // Calculate reprojection Residual Sum of Squares (RSS).
    double rss_error = repr_errors.sum();
    result->rss_repr_errors(k) = rss_error;

    // Keep the best solution in terms of reprojection RSS.
    if (rss_error < min_rss_error) {
      min_rss_error = rss_error;
      result->ctl_points_cam = ctl_points_cam;
      result->rotation = rotation;
      result->translation = translation;
      result->solution_dims = sol_dims;
    }
  }

  // Solution could not be found with any dimensionality of the solution space.
  if (min_rss_error == Inf) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  return pnp::Status::kSuccess;
}

// Choose a suitable 3D basis to represent the input 3D points in (via barycentric coordinates).
// Principal Component Analysis of the 3D point cloud.
Matrix3Xd ChooseBasis(const Matrix3Xd& points3, double tol) {
  // The output basis will be represented by a small set of 3D control points (up to 4).
  Matrix3Xd ctl_points;
  ctl_points.resize(3, 0);

  // At least 1 input point is required for further calculations to make sense.
  const int num_points = points3.cols();
  if (num_points < 1) {
    return ctl_points;
  }

  // Calculate centroid of the point cloud - requires at least 1 point.
  Vector3d centroid = points3.rowwise().sum() / num_points;

  // Center the point cloud - requires at least 1 point
  Matrix3Xd centered_points3 = points3;
  for (int i = 0; i < num_points; i++) {
    centered_points3.col(i) -= centroid;
  }

  // Compute principal components.
  // Singular values are sorted in decreasing order.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd;
  svd.compute(centered_points3, Eigen::ComputeFullU | Eigen::ComputeThinV);
  const VectorXd& singular_values = svd.singularValues();
  const MatrixXd& principal_directions = svd.matrixU();

  // Determine the number of non-vanishing principal components given a tolerance.
  int dims = 0;
  for (int i = 0; i < singular_values.rows(); i++) {
    if (singular_values(i) > tol) {
      dims++;
    }
  }

  // The output matrix is always of size 3-by-(dims+1) with the first column being the centroid
  // of the input point cloud.
  ctl_points = Matrix3Xd::Zero(3, dims + 1);
  ctl_points.col(0) = centroid;

  // Scale the other control points with respect to the centroid along the principal directions
  // such that their distance from the centroid equals the square root of the point variance.
  // This is only done for non-vanishing principal directions.
  const double factor = 1.0 / std::sqrt(static_cast<double>(num_points));
  for (int i = 0; i < dims; i++) {
    ctl_points.col(i + 1) = principal_directions.col(i) * factor * singular_values(i) + centroid;
  }

  return ctl_points;
}

// Compute barycentric coords of points with respect to a basis defined by 3 or 4 control points.
// Solves the linear problem A*X = B for X where:
// B is the 4xN matrix of homogeneous 3D points (3xN matrix points3 extended by a row of 1's)
// A is the 4x3 or 4x4 homogenenous matrix of control point coordinates,
// X is the 3xN or 4xN matrix of barycentric coordinates.
// Since the last columns of A and B are all 1's, the column sums of X are 1.
MatrixXd ComputeBaryCoords(const Matrix3Xd& points3, const Matrix3Xd& ctl_points) {
  MatrixXd bary_coords;
  bary_coords.resize(ctl_points.cols(), 0);
  if (points3.cols() && (ctl_points.cols() == 3 || ctl_points.cols() == 4)) {
    Matrix4Xd basis = pnp::HomogeneousFromEuclidean(ctl_points);

    // Solve the problem A*X = B
    bary_coords = basis.householderQr().solve(pnp::HomogeneousFromEuclidean(points3));
  }
  return bary_coords;
}

// Solve the projection equations via Singular-Value Decomposition and
// find the solution space of the control point camera coordinates.
bool SolveProjConstraints(double focal_u, double focal_v, double principal_u, double principal_v,
                          const MatrixXd& bary_coords, const Matrix2Xd points2,
                          MatrixXd* proj_coeffs, MatrixXd* sol_basis, VectorXd* singular_values) {
  if (proj_coeffs == nullptr || sol_basis == nullptr || singular_values == nullptr) {
    return false;
  }

  proj_coeffs->resize(0, 0);

  // At least 6 input points are required, each generating 2 equations.
  int num_points = points2.cols();
  if (num_points < 6) {
    return false;
  }
  if (bary_coords.cols() != num_points) {
    return false;
  }

  // Focal lengths should be non-zero.
  if (focal_u == 0 || focal_v == 0) {
    return false;
  }

  // Shorthands to make it easier to visually verify projection coefficients below.
  const double fu = focal_u;
  const double fv = focal_v;
  const double uc = principal_u;
  const double vc = principal_v;

  // Non-planar case with 4 control points (12 unknowns)
  if (bary_coords.rows() == 4) {
    // Construct the 2Nx12 coefficient matrix
    proj_coeffs->resize(2 * num_points, 12);
    for (int i = 0; i < num_points; i++) {
      // Shorthands to make it easier to check coefficient matrix
      const double u = points2(0, i);
      const double v = points2(1, i);
      const double b1 = bary_coords(0, i);
      const double b2 = bary_coords(1, i);
      const double b3 = bary_coords(2, i);
      const double b4 = bary_coords(3, i);
      proj_coeffs->row(2 * i + 0) << b1 * fu, 0, b1 * (uc - u), b2 * fu, 0, b2 * (uc - u), b3 * fu,
          0, b3 * (uc - u), b4 * fu, 0, b4 * (uc - u);
      proj_coeffs->row(2 * i + 1) << 0, b1 * fv, b1 * (vc - v), 0, b2 * fv, b2 * (vc - v), 0,
          b3 * fv, b3 * (vc - v), 0, b4 * fv, b4 * (vc - v);
    }

    // Planar case with 3 control points (9 unknowns)
  } else if (bary_coords.rows() == 3) {
    // Construct the 2Nx9 coefficient matrix
    proj_coeffs->resize(2 * num_points, 9);
    for (int i = 0; i < num_points; i++) {
      // Shorthands to make it easier to check coefficient matrix
      const double u = points2(0, i);
      const double v = points2(1, i);
      const double b1 = bary_coords(0, i);
      const double b2 = bary_coords(1, i);
      const double b3 = bary_coords(2, i);
      proj_coeffs->row(2 * i + 0) << b1 * fu, 0, b1 * (uc - u), b2 * fu, 0, b2 * (uc - u), b3 * fu,
          0, b3 * (uc - u);
      proj_coeffs->row(2 * i + 1) << 0, b1 * fv, b1 * (vc - v), 0, b2 * fv, b2 * (vc - v), 0,
          b3 * fv, b3 * (vc - v);
    }

    // Cases that are not solved with the current implementation.
  } else {
    return false;
  }

  // Solve the homogeneous linear problem by SVD
  // Matrix V is 9x9 (planar case) or 12x12 (non-planar case)
  // There are 9 (planar case) or 12 singular values  (non-planar case)
  Eigen::JacobiSVD<Eigen::MatrixXd> svd;
  svd.compute(*proj_coeffs, Eigen::ComputeThinU | Eigen::ComputeFullV);
  *singular_values = svd.singularValues();
  const MatrixXd& singular_vectors = svd.matrixV();

  // Output the last 4 columns of V in reverse order
  sol_basis->resize(singular_vectors.rows(), 4);
  for (int i = 0; i < 4; i++) {
    sol_basis->col(i) = singular_vectors.col(singular_vectors.cols() - i - 1);
  }

  // Put singular values in increasing order
  for (int i = 0; i < singular_values->size() / 2; i++) {
    std::swap((*singular_values)(i), (*singular_values)(singular_values->size() - 1 - i));
  }

  return true;
}

// Convert a solution vector of control point coordinates of length 3*C to a 3xC matrix for C=3,4
// such that the input vector contains the elements of the output matrix column-wise.
Matrix3Xd ReshapeToMatrix3xN(const VectorXd& vec) {
  Matrix3Xd matrix;
  if (vec.size() % 3) {
    return matrix;  // return a 3x0 matrix
  }
  matrix.resize(3, vec.size() / 3);
  for (int i = 0; i < matrix.cols(); i++) {
    matrix.col(i) = vec.block(3 * i, 0, 3, 1);
  }

  return matrix;
}

// Project points directly given in camera coordinates to the image
Matrix2Xd ProjectPoints(const Matrix3d& calib_matrix, const Matrix3Xd& points3) {
  Matrix2Xd points2;
  points2.resize(2, points3.cols());
  for (int i = 0; i < points3.cols(); i++) {
    Vector3d proj = calib_matrix * points3.col(i);
    if (proj(2) == 0) {
      // Avoids NaNs due to 0/0.
      points2(0, i) = Inf;
      points2(1, i) = Inf;
    } else {
      // Division by homogeneous coordinate.
      points2(0, i) = proj(0) / proj(2);
      points2(1, i) = proj(1) / proj(2);
    }
  }
  return points2;
}

// Compute distances between all possible pairs (i,j) of input points (in matrix columns).
VectorXd ComputeDistances(const MatrixXd& points) {
  int num_points = points.cols();
  if (num_points < 2) {
    return VectorXd(0);
  }

  VectorXd distances(num_points * (num_points - 1) / 2);
  int k = 0;
  for (int i = 0; i < points.cols(); i++) {
    for (int j = i + 1; j < points.cols(); j++) {
      distances[k++] = (points.col(i) - points.col(j)).norm();
    }
  }

  return distances;
}

// Compute the weights that correspond to a concrete solution for the control point camera
// coordinates in the basis given as input.
Vector4d SolveControlPoints(const MatrixXd& sol_basis, int sol_dims, const VectorXd& distances) {
  Vector4d weights = Vector4d::Zero();
  if (sol_basis.cols() != 4) {
    return weights;
  }
  if (sol_basis.rows() != 12 && sol_basis.rows() != 9) {
    return weights;
  }

  // non-planar case
  if (sol_basis.rows() == 12 && distances.size() != 6) {
    return weights;
  }

  // planar case
  if (sol_basis.rows() == 9 && distances.size() != 3) {
    return weights;
  }

  // Planar and non-planar case for 1-dimensional solution space
  if (sol_dims == 1) {
    // Only use singular vector corresponding to the least singular value.
    // Solution space is 1-dimensional -> only need to find a single global scale.

    // Calculate the unscaled camera coordinates of the control points.
    Matrix3Xd v = ReshapeToMatrix3xN(sol_basis.col(0));

    // Calculate distances between the unscaled control points.
    VectorXd distances_cam = ComputeDistances(v);

    // Calculate the unknown scale from the distances between control points in the world frame
    // and the corresponding distances in the camera frame.
    double scale = distances.dot(distances_cam) / distances_cam.dot(distances_cam);

    // Find sign of the scale such that a majority of control points are in front of the camera.
    int num_points_behind = (v.row(2).array() < 0).count();
    if (num_points_behind > 1) {
      scale = -scale;
    }

    weights(0) = scale;

    // Planar and non-planar for 2-dimensional solution space
  } else if (sol_dims == 2) {
    // Reshape the two basis vectors of the solution space into two 3xN matrices.
    // The sought matrix ctl_points_cam is a linear combination of these matrices.
    // ctl_points_cam = w1*v1 + w2*v2, where the weights w1 and w2 are still unknown.
    Matrix3Xd v1 = ReshapeToMatrix3xN(sol_basis.col(0));
    Matrix3Xd v2 = ReshapeToMatrix3xN(sol_basis.col(1));

    // Looking for the linear combination preserving the distances between control points.
    // This is a non-linear problem in the linear combination weights (w1,w2).
    // Strategy:
    //   (Step1) Solve for (w11,w22,w12) = (w1*w1, w2*w2, w1*w2) (linearized problem)
    //   (Step2) Extract (w1,w2) from (w11,w22,w12).
    // Both steps are over-determined problems.

    // Construct coefficient matrix of linearized problem (see Eq.13 in the paper).
    // This matrix is 6x4 in the non-planar case.
    const int num_points = v1.cols();
    const int num_equations = num_points * (num_points - 1) / 2;
    int k = 0;
    MatrixXd coeff_matrix(num_equations, 3);
    for (int i = 0; i < num_points; i++) {
      for (int j = i + 1; j < num_points; j++) {
        Vector3d d1 = v1.col(i) - v1.col(j);
        Vector3d d2 = v2.col(i) - v2.col(j);
        coeff_matrix.row(k++) << d1.dot(d1), d2.dot(d2), 2 * d1.dot(d2);
      }
    }

    // Step1: Compute least squares solution to the linearized problem.
    VectorXd squared_distances = distances.cwiseProduct(distances);
    VectorXd lin_sol =
        coeff_matrix.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(squared_distances);

    // Solution of the linearized problem.
    const double w11 = lin_sol(0);
    const double w22 = lin_sol(1);
    const double w12 = lin_sol(2);

    // Step2: Calculate weights w1 and w2 (denoted beta_i in the EPnP paper)
    // from the non-linear constraints (w11,w22,w12) = (w1*w1, w2*w2, w1*w2).
    // There are always two solutions, (w1,w2) and (-w1,-w2) -> sign is solved further below.
    // Over-constrained quadratic problem - least-squares solution has no simple closed form.
    // Empirical tests show that solution order largely affects pose accuracy.
    // Strategy: we exactly match the larger magnitude wij and hope for less overall alg. error.
    double w1, w2;
    w1 = w2 = 0;
    if (w11 >= w22 && w11 > 0) {
      w1 = std::sqrt(w11);
      if (w22 > std::abs(w12)) {
        w2 = std::sqrt(w22);
      } else {
        w2 = w12 / w1;
      }
    } else if (w11 <= w22 && w22 > 0) {
      w2 = std::sqrt(w22);
      if (w11 > std::abs(w12)) {
        w1 = std::sqrt(w11);
      } else {
        w1 = w12 / w2;
      }
    }

    // There are always two solutions: (w1,w2) and (-w1,-w2) but
    // one corresponds to control points behind the camera.
    // Find sign such that a majority all control points are in front of the camera.
    VectorXd ctl_point_depths = w1 * v1.row(2) + w2 * v2.row(2);
    int num_points_behind = (ctl_point_depths.array() < 0).count();
    if (num_points_behind > int{num_points / 2}) {
      weights << -w1, -w2, 0, 0;
    } else {
      weights << w1, w2, 0, 0;
    }

    // Non-planar case for 2-dimensional solution space
  } else if (sol_dims == 3 && sol_basis.rows() == 12) {
    // Reshape the 3 basis vectors of the solution space into three 3xN matrices.
    // The sought matrix ctl_points_cam is a linear combination of these matrices.
    // ctl_points_cam = w1*v1 + w2*v2 + w3*v3, where the weights (w1,w2,w3) are still unknown.
    Matrix3Xd v1 = ReshapeToMatrix3xN(sol_basis.col(0));
    Matrix3Xd v2 = ReshapeToMatrix3xN(sol_basis.col(1));
    Matrix3Xd v3 = ReshapeToMatrix3xN(sol_basis.col(2));

    // Looking for the linear combination preserving the distances between control points.
    // This is a non-linear problem in the linear combination weights (w1,w2,w3).
    // Strategy:
    //   (Step1) Solve for (w11,w22,w33,w12,w13,w23) = (w1*w1, w2*w2, w3*w3, w1*w2, w1*w3, w2*w3)
    //           (linearized problem).
    //   (Step2) Extract (w1,w2,w3) from (w11,w22,w33,w12,w13,w23).

    // Step1: Construct coefficient matrix of linearized problem (Case N=3 in the paper).
    // This matrix is 6x6 in the non-planar case.
    const int num_points = v1.cols();
    const int num_equations = num_points * (num_points - 1) / 2;
    int k = 0;
    MatrixXd coeff_matrix(num_equations, 6);
    for (int i = 0; i < num_points; i++) {
      for (int j = i + 1; j < num_points; j++) {
        Vector3d d1 = v1.col(i) - v1.col(j);
        Vector3d d2 = v2.col(i) - v2.col(j);
        Vector3d d3 = v3.col(i) - v3.col(j);
        coeff_matrix.row(k++) << d1.dot(d1), d2.dot(d2), d3.dot(d3), 2 * d1.dot(d2), 2 * d1.dot(d3),
            2 * d2.dot(d3);
      }
    }

    // Step1: Compute least squares solution to the linearized problem.
    VectorXd squared_distances = distances.cwiseProduct(distances);
    VectorXd lin_sol =
        coeff_matrix.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(squared_distances);

    // Solution of the linearized problem.
    const double w11 = lin_sol(0);
    const double w22 = lin_sol(1);
    const double w33 = lin_sol(2);
    const double w12 = lin_sol(3);
    const double w13 = lin_sol(4);
    const double w23 = lin_sol(5);

    // Calculate weights w1, w2, w3 from the non-linear constraints
    // (w11, w22, w33, w12, w13, w23) = (w1*w1, w2*w2, w3*w3, w1*w2, w1*w3, w2*w3).
    // There are always two solutions (w1,w2,w3) and (-w1,-w2,-w3) -> sign solved further below.
    // Over-constrained quadratic problem - leats-squares solution has no simple closed form.
    // Empirical tests show that solution order largely affects pose accuracy.
    // Strategy: we exactly match the larger magnitude wij and hope for less overall alg. error.
    double w1 = 0, w2 = 0, w3 = 0;
    if (w11 > w22 && w11 > w33 && w11 > 0) {
      w1 = std::sqrt(w11);
      if (w22 > std::abs(w12)) {
        w2 = std::sqrt(w22);
      } else {
        w2 = w12 / w1;
      }
      if (w33 > std::abs(w13)) {
        w3 = std::sqrt(w33);
      } else {
        w3 = w13 / w1;
      }
    } else if (w22 > w11 && w22 > w33 && w22 > 0) {
      w2 = std::sqrt(w22);
      if (w11 > std::abs(w12)) {
        w1 = std::sqrt(w11);
      } else {
        w1 = w12 / w2;
      }
      if (w33 > std::abs(w23)) {
        w3 = std::sqrt(w33);
      } else {
        w3 = w23 / w2;
      }
    } else if (w33 > w11 && w33 > w22 && w33 > 0) {
      w3 = std::sqrt(w33);
      if (w11 > std::abs(w13)) {
        w1 = std::sqrt(w11);
      } else {
        w1 = w13 / w3;
      }
      if (w22 > std::abs(w23)) {
        w2 = std::sqrt(w22);
      } else {
        w2 = w23 / w3;
      }
    }

    // There are always two solutions: (w1,w2,w3) and (-w1,-w2,-w3) but
    // one corresponds to control points behind the camera.
    // Find sign such that a majority all control points are in front of the camera.
    VectorXd ctl_point_depths = w1 * v1.row(2) + w2 * v2.row(2) + w3 * v3.row(2);
    int num_points_behind = (ctl_point_depths.array() < 0).count();
    if (num_points_behind > int{num_points / 2}) {
      weights << -w1, -w2, -w3, 0;
    } else {
      weights << w1, w2, w3, 0;
    }
  }

  // TODO: planar N=3 case using relinearization
  // TODO: non-planar N=4 (affine camera) case using relinearization
  // planar N=4 case is underconstrained = ambiguous (3 equations but 4 unknowns)

  return weights;
}

}  // namespace epnp
}  // namespace pnp
}  // namespace isaac


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/multi_array.hpp>

#include <limits>
#include <vector>
#include <cmath>

#include "tudat/basics/testMacros.h"
#include "tudat/io/matrixTextFileReader.h"

#include "tudat/math/interpolators/linearInterpolator.h"
#include "tudat/math/interpolators/multiLinearInterpolator.h"
#include "tudat/io/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_multi_linear_interpolation )

// Test 1: Comparison to MATLAB solution of the example provided in matlab's interp2 function
// documentation.
BOOST_AUTO_TEST_CASE( test2Dimensions )
{
    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 2 );
    for ( int i = 0; i < 5; i++ )
    {
        independentValues[ 0 ].push_back( 1950.0 + static_cast< double >( i ) * 10.0 );
    }

    for ( int i = 0; i < 3; i++ )
    {
        independentValues[ 1 ].push_back( 10.0 + static_cast< double >( i ) * 10.0 );
    }

    // Create two-dimensional array for dependent values.
    boost::multi_array< double, 2 > dependentValues;
    dependentValues.resize( boost::extents[ 5 ][ 3 ] );

    dependentValues[ 0 ][ 0 ] = 150.697;
    dependentValues[ 0 ][ 1 ] = 199.592;
    dependentValues[ 0 ][ 2 ] = 187.625;
    dependentValues[ 1 ][ 0 ] = 179.323;
    dependentValues[ 1 ][ 1 ] = 195.072;
    dependentValues[ 1 ][ 2 ] = 250.287;
    dependentValues[ 2 ][ 0 ] = 203.212;
    dependentValues[ 2 ][ 1 ] = 179.092;
    dependentValues[ 2 ][ 2 ] = 322.767;
    dependentValues[ 3 ][ 0 ] = 226.505;
    dependentValues[ 3 ][ 1 ] = 153.706;
    dependentValues[ 3 ][ 2 ] = 426.730;
    dependentValues[ 4 ][ 0 ] = 249.633;
    dependentValues[ 4 ][ 1 ] = 120.281;
    dependentValues[ 4 ][ 2 ] = 598.243;

    // Initialize interpolator.
    interpolators::MultiLinearInterpolator< double, double, 2 > twoDimensionalInterpolator(
                independentValues, dependentValues );

    // Set interpolation target's independent values.
    std::vector< double > targetValue( 2 );
    targetValue[ 0 ] = 1975.0;
    targetValue[ 1 ] = 15.0;

    // Declare interpolated dependent variable value and execute interpolation.
    const double interpolationResult = twoDimensionalInterpolator.interpolate( targetValue );

    // Check if equal to MATLAB result up to machine precision.
    BOOST_CHECK_CLOSE_FRACTION( 190.62875, interpolationResult,
                                std::numeric_limits< double >::epsilon( ) );
}

// Test 2: 4-dimensional test. Comparison to matlab interpolated solution of analytical function.
BOOST_AUTO_TEST_CASE( test4Dimensions )
{
    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 4 );
    for ( int i = 0; i < 11; i++ )
    {
        independentValues[ 0 ].push_back( -1.0 + static_cast< double >( i ) * 0.2 );
        independentValues[ 1 ].push_back( -1.0 + static_cast< double >( i ) * 0.2 );
        independentValues[ 2 ].push_back( -1.0 + static_cast< double >( i ) * 0.2 );
    }

    for ( int i = 0; i < 6; i++ )
    {
        independentValues[ 3 ].push_back( static_cast< double >( i ) * 2.0 );
    }

    // Create four-dimensional array for dependent values based on analytical function
    // f = t*e^(-x^2 - y^2 - z^2 ).
    boost::multi_array< double, 4 > dependentValues;
    dependentValues.resize( boost::extents[ 11 ][ 11 ][ 11 ][ 6 ] );

    for ( int i = 0; i < 11; i++ )
    {
        for ( int j = 0; j < 11; j++ )
        {
            for ( int k = 0; k < 11; k++ )
            {
                for ( int l = 0; l < 6; l++ )
                {
                    dependentValues[ i ][ j ][ k ][ l ] =
                            independentValues[ 3 ][ l ] *
                            std::exp( -std::pow( independentValues[ 0 ][ i ], 2 ) -
                            std::pow( independentValues[ 1 ][ j ], 2 ) -
                            std::pow( independentValues[ 2 ][ k ], 2 ) );
                }
            }
        }
    }

    // Initialize interpolator.
    interpolators::MultiLinearInterpolator< double, double, 4 > fourDimensionalInterpolator(
                independentValues, dependentValues );

    // Set interpolation target's independent values.
    std::vector< double > targetValue( 4 );
    targetValue[ 0 ] = -1.0;
    targetValue[ 1 ] = 0.1;
    targetValue[ 2 ] = 0.5;
    targetValue[ 3 ] = 7.0;

    // Declare interpolated dependent variable value and execute interpolation.
    const double interpolationResult = fourDimensionalInterpolator.interpolate( targetValue );

    // Check if equal to MATLAB result up to machine precision
    BOOST_CHECK_CLOSE_FRACTION( 1.956391733957447, interpolationResult,
                                std::numeric_limits< double >::epsilon( ) );
}

// Test 3: 2 dimensions, with independent variables out of range
BOOST_AUTO_TEST_CASE( test2DimensionsBoundaryCase )
{
    using namespace tudat::interpolators;

    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 2 );
    for ( int i = 0; i < 5; i++ )
    {
        independentValues[ 0 ].push_back( 1950.0 + static_cast< double >( i ) * 10.0 );
    }

    for ( int i = 0; i < 3; i++ )
    {
        independentValues[ 1 ].push_back( 10.0 + static_cast< double >( i ) * 10.0 );
    }

    // Create two-dimensional array for dependent values.
    boost::multi_array< double, 2 > dependentValues;
    dependentValues.resize( boost::extents[ 5 ][ 3 ] );

    dependentValues[ 0 ][ 0 ] = 150.697;
    dependentValues[ 0 ][ 1 ] = 199.592;
    dependentValues[ 0 ][ 2 ] = 187.625;
    dependentValues[ 1 ][ 0 ] = 179.323;
    dependentValues[ 1 ][ 1 ] = 195.072;
    dependentValues[ 1 ][ 2 ] = 250.287;
    dependentValues[ 2 ][ 0 ] = 203.212;
    dependentValues[ 2 ][ 1 ] = 179.092;
    dependentValues[ 2 ][ 2 ] = 322.767;
    dependentValues[ 3 ][ 0 ] = 226.505;
    dependentValues[ 3 ][ 1 ] = 153.706;
    dependentValues[ 3 ][ 2 ] = 426.730;
    dependentValues[ 4 ][ 0 ] = 249.633;
    dependentValues[ 4 ][ 1 ] = 120.281;
    dependentValues[ 4 ][ 2 ] = 598.243;

    // Set interpolation target's independent values.
    std::vector< std::vector< double > > targetValue( 5 );
    targetValue[ 0 ].resize( 2 );
    targetValue[ 0 ][ 0 ] = independentValues.at( 0 ).front( ) - 10.0;
    targetValue[ 0 ][ 1 ] = 15.0;
    targetValue[ 1 ].resize( 2 );
    targetValue[ 1 ][ 0 ] = independentValues.at( 0 ).back( ) + 10.0;
    targetValue[ 1 ][ 1 ] = 15.0;
    targetValue[ 2 ].resize( 2 );
    targetValue[ 2 ][ 0 ] = 1975.0;
    targetValue[ 2 ][ 1 ] = independentValues.at( 1 ).front( ) - 2.5;
    targetValue[ 3 ].resize( 2 );
    targetValue[ 3 ][ 0 ] = 1975.0;
    targetValue[ 3 ][ 1 ] = independentValues.at( 1 ).back( ) + 2.5;
    targetValue[ 4 ].resize( 2 );
    targetValue[ 4 ][ 0 ] = independentValues.at( 0 ).front( ) - 10.0;
    targetValue[ 4 ][ 1 ] = independentValues.at( 1 ).back( ) + 2.5;

    // Expected results
    std::vector< double > boundaryResults( 5 ); // computed with MATLAB
    boundaryResults[ 0 ] = 175.1445;
    boundaryResults[ 1 ] = 184.957;
    boundaryResults[ 2 ] = 214.8585;
    boundaryResults[ 3 ] = 374.7485;
    boundaryResults[ 4 ] = 187.625;
    std::vector< double > extrapolationResults( 5 ); // computed with MATLAB
    extrapolationResults[ 0 ] = 163.0915;
    extrapolationResults[ 1 ] = 179.8085;
    extrapolationResults[ 2 ] = 226.973375;
    extrapolationResults[ 3 ] = 426.835875;
    extrapolationResults[ 4 ] = 105.17575;

    // Loop over test cases and boundary handling methods.
    bool exceptionIsCaught;
    double interpolatedValue;
    for( unsigned int i = 0; i < 7; i++ )
    {
        // Initialize interpolator.
        MultiLinearInterpolator< double, double, 2 > twoDimensionalInterpolator(
                    independentValues, dependentValues, huntingAlgorithm,
                    static_cast< BoundaryInterpolationType >( i ) );

        for ( unsigned int j = 0; j < 5; j++ )
        {
            if( static_cast< BoundaryInterpolationType >( i ) == throw_exception_at_boundary )
            {
                exceptionIsCaught = false;
                try
                {
                    twoDimensionalInterpolator.interpolate( targetValue.at( j ) );
                }
                catch( std::runtime_error const& )
                {
                    exceptionIsCaught = true;
                }
                BOOST_CHECK_EQUAL( exceptionIsCaught, true );
            }
            else if( ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value ) ||
                     ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value_with_warning ) )
            {
                interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( j ) );
                BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, boundaryResults.at( j ), 1.0E-15 );
            }
            else if( ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary ) ||
                     ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary_with_warning ) )
            {
                interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( j ) );
                BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, extrapolationResults.at( j ), 1.0E-15 );
            }
            else if( ( static_cast< BoundaryInterpolationType >( i ) == use_default_value ) ||
                     ( static_cast< BoundaryInterpolationType >( i ) == use_default_value_with_warning ) )
            {
                interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( j ) );
                BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0, 1.0E-15 );
            }
        }
    }
}

// Test linear interpolation outside of independent variable range and compare with 1D linear interpolator
BOOST_AUTO_TEST_CASE( test1DimensionBoundaryCase )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                tudat::paths::getTudatTestDataPath( ) +
                "/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< std::vector< double > > independentVariableValuesMulti;
    std::vector< double > dependentVariableValues;
    boost::multi_array< double, 1 > dependentVariableValuesMulti( boost::extents[ inputData.rows( ) ] );

    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
        dependentVariableValuesMulti[ i ] = inputData( i, 1 );
    }
    independentVariableValuesMulti.push_back( independentVariableValues );

    // Create linear interpolator using hunting algorithm.
    double valueOffset = 2.0;
    double valueBelowMinimumValue = independentVariableValues[ 0 ] - valueOffset;
    std::vector< double > valueBelowMinimumValueVector = { valueBelowMinimumValue };
    double valueAboveMaximumValue = independentVariableValues[ inputData.rows( ) - 1 ] + valueOffset;
    std::vector< double > valueAboveMaximumValueVector = { valueAboveMaximumValue };
    double linearInterpolatedValue;
    double multiLinearInterpolatedValue;

    for ( unsigned int i = 1; i < 5; i++ )
    {
        // Initialize interpolatos.
        LinearInterpolatorDouble linearInterpolator(
                    independentVariableValues, dependentVariableValues, huntingAlgorithm,
                    static_cast< BoundaryInterpolationType >( i ) );
        MultiLinearInterpolator< double, double, 1 > multiLinearInterpolator(
                    independentVariableValuesMulti, dependentVariableValuesMulti, huntingAlgorithm,
                    static_cast< BoundaryInterpolationType >( i ) );

        if ( ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value ) ||
             ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value_with_warning ) )
        {
            linearInterpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            multiLinearInterpolatedValue = multiLinearInterpolator.interpolate( valueBelowMinimumValueVector );
            BOOST_CHECK_CLOSE_FRACTION( linearInterpolatedValue, multiLinearInterpolatedValue, 1.0E-15 );

            linearInterpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            multiLinearInterpolatedValue = multiLinearInterpolator.interpolate( valueAboveMaximumValueVector );
            BOOST_CHECK_CLOSE_FRACTION( linearInterpolatedValue, multiLinearInterpolatedValue, 1.0E-15 );

        }
        else if ( ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary ) ||
                  ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary_with_warning ) )
        {
            linearInterpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            multiLinearInterpolatedValue = multiLinearInterpolator.interpolate( valueBelowMinimumValueVector );
            BOOST_CHECK_CLOSE_FRACTION( linearInterpolatedValue, multiLinearInterpolatedValue, 1.0E-15 );

            linearInterpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            multiLinearInterpolatedValue = multiLinearInterpolator.interpolate( valueAboveMaximumValueVector );
            BOOST_CHECK_CLOSE_FRACTION( linearInterpolatedValue, multiLinearInterpolatedValue, 1.0E-15 );
        }
    }
}

// Test linear interpolation outside of independent variable range with default extrapolation value
BOOST_AUTO_TEST_CASE( test2DimensionsDefaultExtrapolationCase )
{
    using namespace interpolators;

    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 2 );
    for ( int i = 0; i < 5; i++ )
    {
        independentValues[ 0 ].push_back( 1950.0 + static_cast< double >( i ) * 10.0 );
    }

    for ( int i = 0; i < 3; i++ )
    {
        independentValues[ 1 ].push_back( 10.0 + static_cast< double >( i ) * 10.0 );
    }

    // Create two-dimensional array for dependent values.
    boost::multi_array< double, 2 > inputData;
    inputData.resize( boost::extents[ 5 ][ 3 ] );

    inputData[ 0 ][ 0 ] = 150.697;
    inputData[ 0 ][ 1 ] = 199.592;
    inputData[ 0 ][ 2 ] = 187.625;
    inputData[ 1 ][ 0 ] = 179.323;
    inputData[ 1 ][ 1 ] = 195.072;
    inputData[ 1 ][ 2 ] = 250.287;
    inputData[ 2 ][ 0 ] = 203.212;
    inputData[ 2 ][ 1 ] = 179.092;
    inputData[ 2 ][ 2 ] = 322.767;
    inputData[ 3 ][ 0 ] = 226.505;
    inputData[ 3 ][ 1 ] = 153.706;
    inputData[ 3 ][ 2 ] = 426.730;
    inputData[ 4 ][ 0 ] = 249.633;
    inputData[ 4 ][ 1 ] = 120.281;
    inputData[ 4 ][ 2 ] = 598.243;

    // Set interpolation target's independent values.
    std::vector< std::vector< double > > targetValue( 2 );
    targetValue[ 0 ].resize( 2 );
    targetValue[ 0 ][ 0 ] = independentValues.at( 0 ).front( ) - 10.0;
    targetValue[ 0 ][ 1 ] = 15.0;
    targetValue[ 1 ].resize( 2 );
    targetValue[ 1 ][ 0 ] = independentValues.at( 0 ).front( ) - 10.0;
    targetValue[ 1 ][ 1 ] = independentValues.at( 1 ).back( ) + 2.5;

    // Test with long double
    {
        // Put data in STL vectors.
        boost::multi_array< long double, 2 > dependentValues;
        dependentValues.resize( boost::extents[ 5 ][ 3 ] );
        for ( size_t i = 0; i < inputData.shape( )[ 0 ]; i++ )
        {
            for ( size_t j = 0; j < inputData.shape( )[ 1 ]; j++ )
            {
                dependentValues[ i ][ j ] = static_cast< long double >( inputData[ i ][ j ] );
            }
        }
        long double interpolatedValue;

        for( unsigned int i = 5; i < 7; i++ )
        {
            MultiLinearInterpolator< double, long double, 2 > twoDimensionalInterpolator(
                        independentValues, dependentValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ) );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 0 ) );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0L, 1.0E-15 );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 1 ) );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0L, 1.0E-15 );
        }
    }

    // Test with Eigen::Vector3d
    {
        // Put data in STL vectors.
        boost::multi_array< Eigen::Vector3d, 2 > dependentValues;
        dependentValues.resize( boost::extents[ 5 ][ 3 ] );
        Eigen::Vector3d tempData = Eigen::Vector3d::Zero( );
        for ( size_t i = 0; i < inputData.shape( )[ 0 ]; i++ )
        {
            for ( size_t j = 0; j < inputData.shape( )[ 1 ]; j++ )
            {
                tempData[ 0 ] = inputData[ i ][ j ];
                dependentValues[ i ][ j ] = tempData;
            }
        }
        Eigen::Vector3d interpolatedValue;

        for( unsigned int i = 5; i < 7; i++ )
        {
            MultiLinearInterpolator< double, Eigen::Vector3d, 2 > twoDimensionalInterpolator(
                        independentValues, dependentValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ) );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 0 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Vector3d::Zero( ) ).norm( ), 1.0E-15 );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 1 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Vector3d::Zero( ) ).norm( ), 1.0E-15 );
        }
    }

    // Test with Eigen::Matrix3d
    {
        boost::multi_array< Eigen::Matrix3d, 2 > dependentValues;
        dependentValues.resize( boost::extents[ 5 ][ 3 ] );
        Eigen::Matrix3d tempData = Eigen::Matrix3d::Zero( );
        for ( size_t i = 0; i < inputData.shape( )[ 0 ]; i++ )
        {
            for ( size_t j = 0; j < inputData.shape( )[ 1 ]; j++ )
            {
                tempData( 0, 2 ) = inputData[ i ][ j ];
                dependentValues[ i ][ j ] = tempData;
            }
        }
        Eigen::Matrix3d interpolatedValue;

        for( unsigned int i = 5; i < 7; i++ )
        {
            MultiLinearInterpolator< double, Eigen::Matrix3d, 2 > twoDimensionalInterpolator(
                        independentValues, dependentValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ) );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 0 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Matrix3d::Zero( ) ).norm( ), 1.0E-15 );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 1 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Matrix3d::Zero( ) ).norm( ), 1.0E-15 );
        }
    }
}

// Test linear interpolation outside of independent variable range with extrapolation value given by user
BOOST_AUTO_TEST_CASE( test2DimensionsUserExtrapolationCase )
{
    using namespace interpolators;

    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 2 );
    for ( int i = 0; i < 5; i++ )
    {
        independentValues[ 0 ].push_back( 1950.0 + static_cast< double >( i ) * 10.0 );
    }

    for ( int i = 0; i < 3; i++ )
    {
        independentValues[ 1 ].push_back( 10.0 + static_cast< double >( i ) * 10.0 );
    }

    // Create two-dimensional array for dependent values.
    boost::multi_array< double, 2 > inputData;
    inputData.resize( boost::extents[ 5 ][ 3 ] );

    inputData[ 0 ][ 0 ] = 150.697;
    inputData[ 0 ][ 1 ] = 199.592;
    inputData[ 0 ][ 2 ] = 187.625;
    inputData[ 1 ][ 0 ] = 179.323;
    inputData[ 1 ][ 1 ] = 195.072;
    inputData[ 1 ][ 2 ] = 250.287;
    inputData[ 2 ][ 0 ] = 203.212;
    inputData[ 2 ][ 1 ] = 179.092;
    inputData[ 2 ][ 2 ] = 322.767;
    inputData[ 3 ][ 0 ] = 226.505;
    inputData[ 3 ][ 1 ] = 153.706;
    inputData[ 3 ][ 2 ] = 426.730;
    inputData[ 4 ][ 0 ] = 249.633;
    inputData[ 4 ][ 1 ] = 120.281;
    inputData[ 4 ][ 2 ] = 598.243;

    // Set interpolation target's independent values.
    std::vector< std::vector< double > > targetValue( 2 );
    targetValue[ 0 ].resize( 2 );
    targetValue[ 0 ][ 0 ] = independentValues.at( 0 ).front( ) - 10.0;
    targetValue[ 0 ][ 1 ] = 15.0;
    targetValue[ 1 ].resize( 2 );
    targetValue[ 1 ][ 0 ] = independentValues.at( 0 ).front( ) - 10.0;
    targetValue[ 1 ][ 1 ] = independentValues.at( 1 ).back( ) + 2.5;

    // Test with long double
    {
        // Put data in STL vectors.
        boost::multi_array< long double, 2 > dependentValues;
        dependentValues.resize( boost::extents[ 5 ][ 3 ] );
        for ( size_t i = 0; i < inputData.shape( )[ 0 ]; i++ )
        {
            for ( size_t j = 0; j < inputData.shape( )[ 1 ]; j++ )
            {
                dependentValues[ i ][ j ] = static_cast< long double >( inputData[ i ][ j ] );
            }
        }
        long double interpolatedValue;
        long double extrapolationValue = 1.0L;

        for( unsigned int i = 5; i < 7; i++ )
        {
            MultiLinearInterpolator< double, long double, 2 > twoDimensionalInterpolator(
                        independentValues, dependentValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ), extrapolationValue );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 0 ) );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, extrapolationValue, 1.0E-15 );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 1 ) );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, extrapolationValue, 1.0E-15 );
        }
    }

    // Test with Eigen::Vector3d
    {
        // Put data in STL vectors.
        boost::multi_array< Eigen::Vector3d, 2 > dependentValues;
        dependentValues.resize( boost::extents[ 5 ][ 3 ] );
        Eigen::Vector3d tempData = Eigen::Vector3d::Zero( );
        for ( size_t i = 0; i < inputData.shape( )[ 0 ]; i++ )
        {
            for ( size_t j = 0; j < inputData.shape( )[ 1 ]; j++ )
            {
                tempData[ 0 ] = inputData[ i ][ j ];
                dependentValues[ i ][ j ] = tempData;
            }
        }
        Eigen::Vector3d interpolatedValue;
        Eigen::Vector3d extrapolationValue;
        extrapolationValue[ 0 ] = 1.5;
        extrapolationValue[ 1 ] = -3;
        extrapolationValue[ 2 ] = 0;

        for( unsigned int i = 5; i < 7; i++ )
        {
            MultiLinearInterpolator< double, Eigen::Vector3d, 2 > twoDimensionalInterpolator(
                        independentValues, dependentValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ), extrapolationValue );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 0 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 1 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );
        }
    }

    // Test with Eigen::Matrix3d
    {
        boost::multi_array< Eigen::Matrix3d, 2 > dependentValues;
        dependentValues.resize( boost::extents[ 5 ][ 3 ] );
        Eigen::Matrix3d tempData = Eigen::Matrix3d::Zero( );
        for ( size_t i = 0; i < inputData.shape( )[ 0 ]; i++ )
        {
            for ( size_t j = 0; j < inputData.shape( )[ 1 ]; j++ )
            {
                tempData( 0, 2 ) = inputData[ i ][ j ];
                dependentValues[ i ][ j ] = tempData;
            }
        }
        Eigen::Matrix3d interpolatedValue;
        Eigen::Matrix3d extrapolationValue = Eigen::Matrix3d::Random( );

        for( unsigned int i = 5; i < 7; i++ )
        {
            MultiLinearInterpolator< double, Eigen::Matrix3d, 2 > twoDimensionalInterpolator(
                        independentValues, dependentValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ), extrapolationValue );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 0 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );

            interpolatedValue = twoDimensionalInterpolator.interpolate( targetValue.at( 1 ) );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <stan/math/prim/scal.hpp>
#include <stan/math/prim/arr/fun/log_sum_exp.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

void test_log_sum_exp(double a, double b) {
  using std::log;
  using std::exp;
  using stan::math::log_sum_exp;
  EXPECT_FLOAT_EQ(log(exp(a) + exp(b)),
                  log_sum_exp(a,b));
}

void test_log_sum_exp(const std::vector<double>& as) {
  using std::log;
  using std::exp;
  using stan::math::log_sum_exp;
  double sum_exp = 0.0;
  for (size_t n = 0; n < as.size(); ++n)
    sum_exp += exp(as[n]);
  EXPECT_FLOAT_EQ(log(sum_exp),
                  log_sum_exp(as));
}

TEST(MathFunctions, log_sum_exp) {
  using stan::math::log_sum_exp;
  std::vector<double> as;
  test_log_sum_exp(as);
  as.push_back(0.0);
  test_log_sum_exp(as);
  as.push_back(1.0);
  test_log_sum_exp(as);
  as.push_back(-1.0);
  test_log_sum_exp(as);
  as.push_back(-10000.0);
  test_log_sum_exp(as);

  as.push_back(10000.0);
  EXPECT_FLOAT_EQ(10000.0, log_sum_exp(as));
}

TEST(MathFunctions, log_sum_exp_2) {
  using stan::math::log_sum_exp;
  test_log_sum_exp(1.0,2.0);
  test_log_sum_exp(1.0,1.0);
  test_log_sum_exp(3.0,2.0);
  test_log_sum_exp(-20.0,12);
  test_log_sum_exp(-20.0,12);

  // exp(10000.0) overflows
  EXPECT_FLOAT_EQ(10000.0,log_sum_exp(10000.0,0.0));
  EXPECT_FLOAT_EQ(0.0,log_sum_exp(-10000.0,0.0));
}

TEST(MathFunctions, log_sum_exp_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_sum_exp(1.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_sum_exp(nan, 1.0));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_sum_exp(nan, nan));
}


/**
 * math lib test
 * @author Tobias Weber <tweber@ill.fr>
 * @date nov-20
 * @license GPLv3, see 'LICENSE' file
 *
 * g++ -std=c++20 -I.. -o cov cov.cpp
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Cov1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"
using namespace tl2_ops;

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_equals, t_real, t_types)
{
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;

	std::vector<t_vec> vecs{{
		tl2::create<t_vec>({1, 2, 3}),
		tl2::create<t_vec>({3, 2, 1}),
		tl2::create<t_vec>({5, -2, 0.5}),
	}};

	std::vector<t_real> probs{{ 1., 1., 0.5 }};

	auto [cov, corr] = tl2::covariance<t_mat, t_vec>(vecs, &probs);
	std::cout << "cov  = " << cov << std::endl;
	std::cout << "corr = " << corr << std::endl;

	// comparing with: np.cov([[1,2,3], [3,2,1], [5,-2,0.5]], rowvar=False, aweights=[1,1,0.5], ddof=0)
	const t_mat cov_cmp = tl2::create<t_mat>({
		 2.24, -1.92, -1.52,
		-1.92,  2.56,  0.96,
		-1.52,  0.96,  1.16
	});
	BOOST_TEST(tl2::equals(cov, cov_cmp, 1e-5));
}


/**
 * TSAI 1999 Hand-eye calibration
 * Emanuele Ruffaldi 2016
 *
 * Copyright Scuola Superiore Sant'Anna (2016)
 * Emanuele Ruffaldi, e.ruffaldi@sssup.it
 */
#pragma once

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>

template <class T>
 using Vector3 = Eigen::Matrix<T,3,1>;

template <class T>
 using Matrix3 = Eigen::Matrix<T,3,3>;

template <class T>
 using Matrix4 = Eigen::Matrix<T,4,4>;

/**
 * Computes Tsai calibration: four frames
 *	 R = robot root
 * 	 E = robot end-effector
 *   C = camera
 *   M = marker
 *
 * The objective is EC
 * We move E wrt R and keep R and M fixed each other
 */

float calibrationTsai(const std::vector<Eigen::Matrix4f> & cMm,
                     const std::vector<Eigen::Matrix4f> & rMe,
                     Eigen::Matrix4f &eMc, bool computeResidual = false);

double calibrationTsai(const std::vector<Eigen::Matrix4d> & cMm,
                     const std::vector<Eigen::Matrix4d> & rMe,
                     Eigen::Matrix4d &eMc, bool computeResidual = false);

/*
 * These are internal functions used for testing the Tsai
 * They mainly deal with the specific Tsai parametrization of the vector that is more robust to small values than the regular one
 */
template <class T>
Eigen::Matrix<T,3,1> paratsaiprime2paratsai(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Matrix<T,3,1> paratsai2paratsaiprime(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Matrix<T,3,3> paratsai2rot(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Quaternion<T> paratsai2quat(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Matrix<T,3,1> quat2paratsai(const Eigen::Quaternion<T> & q);
template <class T>
T paratsaiprime2theta(const Eigen::Matrix<T,3,1> & x);
template <class T>
T paratsai2theta(const Eigen::Matrix<T,3,1> & x);

/// returns the skew of the vector
template <class T>
inline Matrix3<T> skewtsai(const Vector3<T> & a)
{
  Matrix3<T> r;
  r  << 0, -a(2), a(1),  a(2),0,-a(0),-a(1),a(0),0;
  return r;
}

template <class T>
Vector3<T> paratsaiprime2paratsai(const Vector3<T> & a)
{
    return 2*a/sqrt(1+a.squaredNorm()); // equation 14
}

template <class T>
Vector3<T> paratsai2paratsaiprime(const Vector3<T> & a)
{
  return a/sqrt(4-a.squaredNorm());
}

/// converts the parametric in tsai form to rotation
/// NO NEED OF TRIGONOMETRICS
/// equation 10
template <class T>
Matrix3<T> paratsai2rot(const Vector3<T> & a)
{
  T sn = a.squaredNorm();
  T alpha = sqrt(4-sn);
  return (1-sn/2)*Matrix3<T>::Identity() + 0.5*(a*a.transpose() + alpha * skewtsai(a));
}

/// converts the parametric in tsai form to rotation
/// NO NEED OF TRIGONOMETRICS
/// equation 9 modified NOT USED
template <class T>
Eigen::Quaternion<T> paratsai2quat(const Vector3<T> & a)
{
  double sinthetahalf = a.norm()/2; // 2 * sin(theta)/2 => sin(theta)/2
  return Eigen::Quaternion<T>(sqrt(1-sinthetahalf*sinthetahalf),a.x()/2,a.y()/2,a.z()/2); // cos(theta/2)
}

template <class T>
T paratsai2theta(const Vector3<T> & x)
{
  return 2*asin(x.norm()/2); // eq. 9
}


// Catch includes
#include "catch.hpp"

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff includes
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;
using namespace autodiff::forward;

#include <iostream>

template<typename T>
auto approx(T&& expr) -> Approx
{
    const double zero = std::numeric_limits<double>::epsilon();
    return Approx(val(std::forward<T>(expr))).margin(zero);
}

TEST_CASE("autodiff::dual tests", "[dual]")
{
    dual x = 100;
    dual y = 10;

    SECTION("trivial tests")
    {
        REQUIRE( x == 100 );
        x += 1;
        REQUIRE( x == 101 );
        x -= 1;
        REQUIRE( x == 100 );
        x *= 2;
        REQUIRE( x == 200 );
        x /= 20;
        REQUIRE( x == 10 );
    }

    SECTION("aliasing tests")
    {
        x = 1; x = x + 3*x - 2*x + x;
        REQUIRE( x == 3 );

        x = 1; x += x + 3*x - 2*x + x;
        REQUIRE( x == 4 );

        x = 1; x -= x + 3*x - 2*x + x;
        REQUIRE( x == -2 );

        x = 1; x *= x + 3*x - 2*x + x;
        REQUIRE( x == 3 );

        x = 1; x /= x + x;
        REQUIRE( x == 0.5 );
    }

    SECTION("testing comparison operators")
    {
        x = 6;
        y = 5;

        REQUIRE( x == 6 );
        REQUIRE( 6 == x );
        REQUIRE( x == x );

        REQUIRE( x != 5 );
        REQUIRE( 5 != x );
        REQUIRE( x != y );

        REQUIRE( x > 5 );
        REQUIRE( x > y );

        REQUIRE( x >= 6 );
        REQUIRE( x >= x );
        REQUIRE( x >= y );

        REQUIRE( 5 < x );
        REQUIRE( y < x );

        REQUIRE( 6 <= x );
        REQUIRE( x <= x );
        REQUIRE( y <= x );
    }

    SECTION("testing unary negative operator")
    {
        std::function<dual(dual)> f;

        // Testing positive operator on a dual
        f = [](dual x) -> dual { return +x; };
        REQUIRE( f(x) == x );
        REQUIRE( derivative(f, wrt(x), at(x)) == 1.0 );

        // Testing positive operator on an expression
        f = [](dual x) -> dual { return +(x * x); };
        REQUIRE( f(x) == x * x );
        REQUIRE( derivative(f, wrt(x), at(x)) == 2.0 * x );

        // Testing negative operator on a dual
        f = [](dual x) -> dual { return -x; };
        REQUIRE( f(x) == -x );
        REQUIRE( derivative(f, wrt(x), at(x)) == -1.0 );

        // Testing negative operator on a negative expression
        f = [](dual x) -> dual { return -(-x); };
        REQUIRE( f(x) == x );
        REQUIRE( derivative(f, wrt(x), at(x)) == 1.0 );

        // Testing negative operator on a scaling expression expression
        f = [](dual x) -> dual { return -(2.0 * x); };
        REQUIRE( f(x) == -2.0 * x );
        REQUIRE( derivative(f, wrt(x), at(x)) == -2.0 );

        // Testing negative operator on a more complex expression
        f = [](dual x) -> dual { return -x - (2*x); };
        REQUIRE( f(x) == -val(x) - 2.0 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x)) == -3.0 );

        // Testing negative operator on a more complex expression
        f = [](dual x) -> dual { return -x - 2.0 * log(1.0 + exp(-x)); };
        REQUIRE( f(x) == -val(x) - 2.0 * std::log(1.0 + std::exp(-val(x))) );
        REQUIRE( derivative(f, wrt(x), at(x)) == -1.0 - 2.0/(1.0 + std::exp(-val(x))) * (-std::exp(-val(x))) );

        // Testing negative operator on a more complex expression
        f = [](dual x) -> dual { return -x - log(1.0 + exp(-x)); };
        REQUIRE( f(x) == -val(x) - std::log(1.0 + std::exp(-val(x))) );
        REQUIRE( derivative(f, wrt(x), at(x)) == -1.0 - 1.0/(1.0 + std::exp(-val(x))) * (-std::exp(-val(x))) );
    }

    SECTION("testing unary inverse operator")
    {
        std::function<dual(dual)> f;

        // Testing inverse operator on a dual
        f = [](dual x) -> dual { return 1.0 / x; };
        REQUIRE( f(x) == 1/x );
        REQUIRE( derivative(f, wrt(x), at(x)) == -1.0 / (x * x) );

        // Testing inverse operator on a trivial expression
        f = [](dual x) -> dual { return x / x; };
        REQUIRE( f(x) == 1.0 );
        REQUIRE( derivative(f, wrt(x), at(x)) == 0.0 );

        // Testing inverse operator on an inverse expression
        f = [](dual x) -> dual { return 1.0 / (1.0 / x); };
        REQUIRE( f(x) == x );
        REQUIRE( derivative(f, wrt(x), at(x)) == 1.0 );

        // Testing inverse operator on a scaling expression
        f = [](dual x) -> dual { return 1.0 / (2.0 * x); };
        REQUIRE( f(x) == 0.5 / x );
        REQUIRE( derivative(f, wrt(x), at(x)) == -0.5 / (x * x) );
    }

    SECTION("testing binary addition operator")
    {
        std::function<dual(dual, dual)> f;

        // Testing addition operator on a `number + dual` expression
        f = [](dual x, dual y) -> dual { return 1 + x; };
        REQUIRE( f(x, y) == 1 + val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        // Testing addition operator on a `dual + number` expression
        f = [](dual x, dual y) -> dual { return x + 1; };
        REQUIRE( f(x, y) == val(x) + 1 );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        // Testing addition operator on a `dual + dual` expression
        f = [](dual x, dual y) -> dual { return (-x) + (-y); };
        REQUIRE( f(x, y) == -(val(x) + val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == -1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -1.0 );

        // Testing addition operator on a `(-dual) + (-dual)` expression
        f = [](dual x, dual y) -> dual { return (-x) + (-y); };
        REQUIRE( f(x, y) == -(val(x) + val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == -1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -1.0 );

        // Testing addition operator on a `dual * dual + dual * dual` expression
        f = [](dual x, dual y) -> dual { return x * y + x * y; };
        REQUIRE( f(x, y) == 2.0 * (val(x) * val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx( 2.0 * y ) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx( 2.0 * x ) );

        // Testing addition operator on a `1/dual * 1/dual` expression
        f = [](dual x, dual y) -> dual { return 1.0 / x + 1.0 / y; };
        REQUIRE( f(x, y) == approx( 1.0 / val(x) + 1.0 / val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx( -1.0 / (x * x) ) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx( -1.0 / (y * y) ) );
    }

    SECTION("testing binary subtraction operator")
    {
        std::function<dual(dual, dual)> f;

        // Testing subtraction operator on a `number - dual` expression
        f = [](dual x, dual y) -> dual { return 1 - x; };
        REQUIRE( f(x, y) == 1 - val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == -1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) ==  0.0 );

        // Testing subtraction operator on a `dual - number` expression
        f = [](dual x, dual y) -> dual { return x - 1; };
        REQUIRE( f(x, y) == val(x) - 1 );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        // Testing subtraction operator on a `(-dual) - (-dual)` expression
        f = [](dual x, dual y) -> dual { return (-x) - (-y); };
        REQUIRE( f(x, y) == -(val(x) - val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == -1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) ==  1.0 );

        // Testing subtraction operator on a `dual * dual - dual * dual` expression
        f = [](dual x, dual y) -> dual { return x * y - x * y; };
        REQUIRE( f(x, y) == 0.0 );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx( 0.0 ) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx( 0.0 ) );

        // Testing subtraction operator on a `1/dual * 1/dual` expression
        f = [](dual x, dual y) -> dual { return 1.0 / x - 1.0 / y; };
        REQUIRE( f(x, y) == approx( 1.0 / val(x) - 1.0 / val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx( -1.0 / (x * x) ) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(  1.0 / (y * y) ) );
    }

    SECTION("testing binary multiplication operator")
    {
        std::function<dual(dual, dual)> f;

        // Testing multiplication operator on a `number * dual` expression
        f = [](dual x, dual y) -> dual { return 2.0 * x; };
        REQUIRE( f(x, y) == 2.0 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 2.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        // Testing multiplication operator on a `dual * number` expression
        f = [](dual x, dual y) -> dual { return x * 2; };
        REQUIRE( f(x, y) == 2.0 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 2.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        // Testing multiplication operator on a `dual * dual` expression
        f = [](dual x, dual y) -> dual { return x * y; };
        REQUIRE( f(x, y) == val(x) * val(y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == y );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == x );

        // Testing multiplication operator on a `number * (number * dual)` expression
        f = [](dual x, dual y) -> dual { return 5 * (2.0 * x); };
        REQUIRE( f(x, y) == 10 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 10.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) ==  0.0 );

        // Testing multiplication operator on a `(dual * number) * number` expression
        f = [](dual x, dual y) -> dual { return (x * 2) * 5; };
        REQUIRE( f(x, y) == 10 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 10.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) ==  0.0 );

        // Testing multiplication operator on a `number * (-dual)` expression
        f = [](dual x, dual y) -> dual { return 2.0 * (-x); };
        REQUIRE( f(x, y) == -2.0 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == -2.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) ==  0.0 );

        // Testing multiplication operator on a `(-dual) * number` expression
        f = [](dual x, dual y) -> dual { return (-x) * 2; };
        REQUIRE( f(x, y) == -2.0 * val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == -2.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) ==  0.0 );

        // Testing multiplication operator on a `(-dual) * (-dual)` expression
        f = [](dual x, dual y) -> dual { return (-x) * (-y); };
        REQUIRE( f(x, y) == val(x) * val(y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == y );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == x );

        // Testing multiplication operator on a `(1/dual) * (1/dual)` expression
        f = [](dual x, dual y) -> dual { return (1 / x) * (1 / y); };
        REQUIRE( f(x, y) == 1 / (val(x) * val(y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(-1/(x * x * y)) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-1/(x * y * y)) );
    }

    SECTION("testing binary division operator")
    {
        std::function<dual(dual, dual)> f;

        // Testing division operator on a `number / dual` expression
        f = [](dual x, dual y) -> dual { return 1 / x; };
        REQUIRE( f(x, y) == 1 / val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(-1.0 / (x * x)) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        // Testing division operator on a `dual / number` expression
        f = [](dual x, dual y) -> dual { return x / 2; };
        REQUIRE( f(x, y) == val(x) / 2 );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.5) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        // Testing division operator on a `dual / dual` expression
        f = [](dual x, dual y) -> dual { return x / y; };
        REQUIRE( f(x, y) == val(x) / val(y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(1 / y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-x / (y * y)) );

        // Testing division operator on a `1 / (number * dual)` expression
        f = [](dual x, dual y) -> dual { return 1 / (2.0 * x); };
        REQUIRE( f(x, y) == approx(0.5 / x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(-0.5 / (x * x)) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        // Testing division operator on a `1 / (1 / dual)` expression
        f = [](dual x, dual y) -> dual { return 1 / (1 / x); };
        REQUIRE( f(x, y) == val(x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );
    }

    SECTION("testing operator+=")
    {
        std::function<dual(dual, dual)> f;

        f = [](dual x, dual y) -> dual { return x += 1; };
        REQUIRE( f(x, y) == approx(x + 1) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        f = [](dual x, dual y) -> dual { return x += y; };
        REQUIRE( f(x, y) == approx(x + y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 1.0 );

        f = [](dual x, dual y) -> dual { return x += -y; };
        REQUIRE( f(x, y) == approx(x - y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) ==  1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -1.0 );

        f = [](dual x, dual y) -> dual { return x += -(x - y); };
        REQUIRE( f(x, y) == approx(y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 0.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 1.0 );

        f = [](dual x, dual y) -> dual { return x += 1.0 / y; };
        REQUIRE( f(x, y) == approx(x + 1.0 / y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) ==  1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-1.0 / (y * y)) );

        f = [](dual x, dual y) -> dual { return x += 2.0 * y; };
        REQUIRE( f(x, y) == approx(x + 2.0 * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 2.0 );

        f = [](dual x, dual y) -> dual { return x += x * y; };
        REQUIRE( f(x, y) == approx(x + x * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 + y );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == x );

        f = [](dual x, dual y) -> dual { return x += x + y; };
        REQUIRE( f(x, y) == approx(x + x + y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 2.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 1.0 );
    }

    SECTION("testing operator-=")
    {
        std::function<dual(dual, dual)> f;

        f = [](dual x, dual y) -> dual { return x -= 1; };
        REQUIRE( f(x, y) == approx(x - 1) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 0.0 );

        f = [](dual x, dual y) -> dual { return x -= y; };
        REQUIRE( f(x, y) == approx(x - y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) ==  1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -1.0 );

        f = [](dual x, dual y) -> dual { return x -= -y; };
        REQUIRE( f(x, y) == approx(x + y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 1.0 );

        f = [](dual x, dual y) -> dual { return x -= -(x - y); };
        REQUIRE( f(x, y) == approx(2.0 * x - y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) ==  2.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -1.0 );

        f = [](dual x, dual y) -> dual { return x -= 1.0 / y; };
        REQUIRE( f(x, y) == approx(x - 1.0 / y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) ==  1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(1.0 / (y * y)) );

        f = [](dual x, dual y) -> dual { return x -= 2.0 * y; };
        REQUIRE( f(x, y) == approx(x - 2.0 * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) ==  1.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -2.0 );

        f = [](dual x, dual y) -> dual { return x -= x * y; };
        REQUIRE( f(x, y) == approx(x - x * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 1.0 - y );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == -x );

        f = [](dual x, dual y) -> dual { return x -= x - y; };
        REQUIRE( f(x, y) == approx(y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == 0.0 );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == 1.0 );
    }

    SECTION("testing operator*=")
    {
        std::function<dual(dual, dual)> f;

        f = [](dual x, dual y) -> dual { return x *= 2; };
        REQUIRE( f(x, y) == approx(2.0 * x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(2.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        f = [](dual x, dual y) -> dual { return x *= y; };
        REQUIRE( f(x, y) == approx(x * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(x) );

        f = [](dual x, dual y) -> dual { return x *= -x; };
        REQUIRE( f(x, y) == approx(-x * x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(-2.0 * x) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        f = [](dual x, dual y) -> dual { return x *= (2.0 / y); };
        REQUIRE( f(x, y) == approx(2.0 * x / y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(2.0 / y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-2.0 * x / (y * y)) );

        f = [](dual x, dual y) -> dual { return x *= (2.0 * x); };
        REQUIRE( f(x, y) == approx(2.0 * x * x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(4.0 * x) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        f = [](dual x, dual y) -> dual { return x *= (2.0 * y); };
        REQUIRE( f(x, y) == approx(2.0 * x * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(2.0 * y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(2.0 * x) );

        f = [](dual x, dual y) -> dual { return x *= x + y; };
        REQUIRE( f(x, y) == approx(x * (x + y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(2.0 * x + y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(x) );

        f = [](dual x, dual y) -> dual { return x *= x * y; };
        REQUIRE( f(x, y) == approx(x * (x * y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(2.0 * x * y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(x * x) );
    }

    SECTION("testing operator/=")
    {
        std::function<dual(dual, dual)> f;

        f = [](dual x, dual y) -> dual { return x /= 2; };
        REQUIRE( f(x, y) == approx(0.5 * x) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.5) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        f = [](dual x, dual y) -> dual { return x /= y; };
        REQUIRE( f(x, y) == approx(x / y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(1.0 / y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-x / (y * y)) );

        f = [](dual x, dual y) -> dual { return x /= -x; };
        REQUIRE( f(x, y) == approx(-1.0) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        f = [](dual x, dual y) -> dual { return x /= (2.0 / y); };
        REQUIRE( f(x, y) == approx(0.5 * x * y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.5 * y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.5 * x) );

        f = [](dual x, dual y) -> dual { return x /= (2.0 * y); };
        REQUIRE( f(x, y) == approx(0.5 * x / y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx( 0.5 / y) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-0.5 * x / (y * y)) );

        f = [](dual x, dual y) -> dual { return x /= x + y; };
        REQUIRE( f(x, y) == approx(x / (x + y)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(1.0 / (x + y) - x / (x + y) / (x + y)) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-x / (x + y) / (x + y)) );

        f = [](dual x, dual y) -> dual { return x /= x * y; };
        REQUIRE( f(x, y) == approx(1.0 / y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-1.0 / (y * y)) );
    }

    SECTION("testing combination of operations")
    {
        std::function<dual(dual, dual)> f;

        // Testing multiplication with addition
        f = [](dual x, dual y) -> dual { return 2.0 * x + y; };
        REQUIRE( f(x, y) == 2.0 * val(x) + val(y) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(2.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(1.0) );

        // Testing a complex expression that is actually equivalent to one
        f = [](dual x, dual y) -> dual { return (2.0 * x * x - x * y + x / y + x / (2.0 * y)) / (x * (2.0 * x - y + 1 / y + 1 / (2.0 * y))); };
        REQUIRE( f(x, y) == approx(1.0) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );
    }

    SECTION("testing mathematical functions")
    {
        std::function<dual(dual)> f;

        x = 0.5;

        // Testing sin function
        f = [](dual x) -> dual { return sin(x); };
        REQUIRE( f(x) == std::sin(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(cos(x)) );

        // Testing cos function
        f = [](dual x) -> dual { return cos(x); };
        REQUIRE( f(x) == std::cos(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(-sin(x)) );

        // Testing tan function
        f = [](dual x) -> dual { return tan(x); };
        REQUIRE( f(x) == std::tan(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1 / (cos(x) * cos(x))) );

        // Testing sinh function
        f = [](dual x) -> dual { return sinh(x); };
        REQUIRE( f(x) == std::sinh(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(cosh(x)) );

        // Testing cosh function
        f = [](dual x) -> dual { return cosh(x); };
        REQUIRE( f(x) == std::cosh(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(sinh(x)) );

        // Testing tanh function
        f = [](dual x) -> dual { return tanh(x); };
        REQUIRE( f(x) == std::tanh(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1 / (cosh(x) * cosh(x))) );

        // Testing asin function
        f = [](dual x) -> dual { return asin(x); };
        REQUIRE( f(x) == std::asin(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1 / sqrt(1 - x * x)) );

        // Testing acos function
        f = [](dual x) -> dual { return acos(x); };
        REQUIRE( f(x) == std::acos(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(-1 / sqrt(1 - x * x)) );

        // Testing atan function
        f = [](dual x) -> dual { return atan(x); };
        REQUIRE( f(x) == std::atan(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1 / (1 + x * x)) );

        // Testing exp function
        f = [](dual x) -> dual { return exp(x); };
        REQUIRE( f(x) == std::exp(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(exp(x)) );

        // Testing log function
        f = [](dual x) -> dual { return log(x); };
        REQUIRE( f(x) == std::log(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1 / x) );

        // Testing log function
        f = [](dual x) -> dual { return log10(x); };
        REQUIRE( f(x) == std::log10(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1 / (log(10) * x)) );

        // Testing sqrt function
        f = [](dual x) -> dual { return sqrt(x); };
        REQUIRE( f(x) == std::sqrt(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(0.5 / sqrt(x)) );

        // Testing pow function (with number exponent)
        f = [](dual x) -> dual { return pow(x, 2.0); };
        REQUIRE( f(x) == std::pow(val(x), 2.0) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(2.0 * x) );

        // Testing pow function (with dual exponent)
        f = [](dual x) -> dual { return pow(x, x); };
        REQUIRE( f(x) == std::pow(val(x), val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx((log(x) + 1) * pow(x, x)) );

        // Testing pow function (with expression exponent)
        f = [](dual x) -> dual { return pow(x, 2.0 * x); };
        REQUIRE( f(x) == std::pow(val(x), 2.0 * val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(2.0 * (log(x) + 1) * pow(x, 2.0 * x)) );

        // Testing abs function (for x > 0, x < 0, and x = 0)
        f = [](dual x) -> dual { return abs(x); };
        x = 1.0;
        REQUIRE( f(x) == std::abs(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(1.0) );
        x = -1.0;
        REQUIRE( f(x) == std::abs(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(-1.0) );
        x = 0;
        REQUIRE( f(x) == std::abs(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(0.0) );

        // Testing erf function
        f = [](dual x) -> dual { return erf(x); };
        x = 1.0;
        REQUIRE( f(x) == std::erf(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(0.415107) );
        x = -1.4;
        REQUIRE( f(x) == std::erf(val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(0.158942) );

        // Testing atan2 function on (double, dual)
        f = [](dual x) -> dual { return atan2(2.0, x); };
        x = 1.0;
        REQUIRE( f(x) == std::atan2(2.0, val(x)) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(-2.0 / (2*2 + x*x)) );

        // Testing atan2 function on (dual, double)
        f = [](dual y) -> dual { return atan2(y, 2.0); };
        x = 1.0;
        REQUIRE( f(x) == std::atan2(val(x), 2.0) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(2.0 /  (2*2 + x*x)) );

        // Testing atan2 function on (dual, dual)
        std::function<dual(dual, dual)> g = [](dual y, dual x) -> dual { return atan2(y, x); };
        x = 1.1;
        dual y = 0.9;
        REQUIRE( g(y, x) == std::atan2(val(y), val(x)) );
        REQUIRE( derivative(g, wrt(y), at(y, x)) == approx(x / (x*x + y*y)) );
        REQUIRE( derivative(g, wrt(x), at(y, x)) == approx(-y / (x*x + y*y)) );

        // Testing atan2 function on (expr, expr)
        g = [](dual y, dual x) -> dual { return 3 * atan2(sin(y), 2*x+1); };
        REQUIRE( g(y, x) == 3 * std::atan2(sin(val(y)), 2*val(x)+1) );
        REQUIRE( derivative(g, wrt(y), at(y, x)) == approx(3*(2*x+1)*cos(y) / ((2*x+1)*(2*x+1) + sin(y)*sin(y))) );
        REQUIRE( derivative(g, wrt(x), at(y, x)) == approx(3*-2*sin(y) / ((2*x+1)*(2*x+1) + sin(y)*sin(y))) );

        // Testing hypot function on (dual, double)
        f = [](dual y) -> dual { return hypot(y, 2.0); };
        x = 1.5;
        REQUIRE( f(x) == std::hypot(val(x), 2.0) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(x / std::hypot(val(x), 2.0)) );

        // Testing hypot function on (double, dual)
        f = [](dual y) -> dual { return hypot(2.0, y); };
        y = 1.5;
        REQUIRE( f(y) == std::hypot(2.0, val(y)) );
        REQUIRE( derivative(f, wrt(y), at(y)) == approx(y / std::hypot(2.0, val(y))) );

        // Testing hypot function on (dual, dual)
        g = [](dual x, dual y) -> dual { return hypot(x,y); };
        x = 1.1;
        y = 0.9;
        REQUIRE( g(x, y) == std::hypot(val(x), val(y)) );
        REQUIRE( derivative(g, wrt(y), at(x,y)) == approx(y / std::hypot(val(x), val(y))) );
        REQUIRE( derivative(g, wrt(x), at(x,y)) == approx(x / std::hypot(val(x), val(y))) );

        // Testing hypot function on (expr, expr)
        g = [](dual x, dual y) -> dual { return hypot(2.0*x,3.0*y); };
        x = 1.1;
        y = 0.9;
        REQUIRE( g(x, y) == std::hypot(2.0*val(x), 3.0*val(y)) );
        REQUIRE( derivative(g, wrt(x), at(x,y)) == approx(4.0*x / std::hypot(2.0*val(x), 3.0*val(y))) );
        REQUIRE( derivative(g, wrt(y), at(x,y)) == approx(9.0*y / std::hypot(2.0*val(x), 3.0*val(y))) );

        // Testing hypot function on (dual, double, double)
        f = [](dual x) -> dual { return hypot(x, 2.0, 2.0); };
        x = 1.5;
        REQUIRE( f(x) == std::hypot(val(x), 2.0, 2.0) );
        REQUIRE( derivative(f, wrt(x), at(x)) == approx(x / std::hypot(val(x), 2.0, 2.0)) );

        // Testing hypot function on (double, dual, double)
        f = [](dual y) -> dual { return hypot(2.0, y, 2.0); };
        y = 2.5;
        REQUIRE( f(y) == std::hypot(2.0, val(y), 2.0) );
        REQUIRE( derivative(f, wrt(y), at(y)) == approx(y / std::hypot(2.0, val(y), 2.0)) );

        // Testing hypot function on (double, double, dual)
        f = [](dual z) -> dual { return hypot(2.0, 2.0, z); };
        dual z = 3.5;
        REQUIRE( f(z) == std::hypot(2.0, 2.0, val(z)) );
        REQUIRE( derivative(f, wrt(z), at(z)) == approx(z / std::hypot(2.0, 2.0, val(z))) );

        // Testing hypot function on (dual, dual, double)
        g = [](dual x, dual y) -> dual { return hypot(x, y, 2.0); };
        x = 1.4;
        y = 2.4;
        REQUIRE( g(x,y) == std::hypot(val(x), val(y), 2.0) );
        REQUIRE( derivative(g, wrt(x), at(x,y)) == approx(x / std::hypot(val(x), val(y), 2.0)) );
        REQUIRE( derivative(g, wrt(y), at(x,y)) == approx(y / std::hypot(val(x), val(y), 2.0)) );

        // Testing hypot function on (double, dual, dual)
        g = [](dual y, dual z) -> dual { return hypot(2.0, y, z); };
        y = 2.4;
        z = 3.4;
        REQUIRE( g(y,z) == std::hypot(2.0, val(y), val(z)) );
        REQUIRE( derivative(g, wrt(y), at(y,z)) == approx(y / std::hypot(2.0, val(y), val(z))) );
        REQUIRE( derivative(g, wrt(z), at(y,z)) == approx(z / std::hypot(2.0, val(y), val(z))) );

        // Testing hypot function on (dual, double, dual)
        g = [](dual x, dual z) -> dual { return hypot(x, 2.0, z); };
        x = 3.4;
        z = 4.4;
        REQUIRE( g(x,z) == std::hypot(val(x), 2.0, val(z)) );
        REQUIRE( derivative(g, wrt(x), at(x,z)) == approx(x / std::hypot(val(x), 2.0, val(z))) );
        REQUIRE( derivative(g, wrt(z), at(x,z)) == approx(z / std::hypot(val(x), 2.0, val(z))) );

        // Testing hypot function on (dual, double, dual)
        std::function<dual(dual, dual, dual)> h = [](dual x, dual y, dual z) -> dual { return hypot(x, y, z); };
        x = 1.6;
        y = 2.6;
        z = 3.6;
        REQUIRE( h(x,y,z) == std::hypot(val(x), val(y), val(z)) );
        REQUIRE( derivative(h, wrt(x), at(x,y,z)) == approx(x / std::hypot(val(x), val(y), val(z))) );
        REQUIRE( derivative(h, wrt(y), at(x,y,z)) == approx(y / std::hypot(val(x), val(y), val(z))) );
        REQUIRE( derivative(h, wrt(z), at(x,y,z)) == approx(z / std::hypot(val(x), val(y), val(z))) );

        // Testing hypot function on (expr, expr, expr)
        h = [](dual x, dual y, dual z) -> dual { return hypot(2.0*x, 3.0*y, 4.0*z); };
        x = 2.6;
        y = 3.6;
        z = 4.6;
        REQUIRE( h(x,y,z) == std::hypot(2.0*val(x), 3.0*val(y), 4.0*val(z)) );
        REQUIRE( derivative(h, wrt(x), at(x,y,z)) == approx(4.0*x / std::hypot(2.0*val(x), 3.0*val(y), 4.0*val(z))) );
        REQUIRE( derivative(h, wrt(y), at(x,y,z)) == approx(9.0*y / std::hypot(2.0*val(x), 3.0*val(y), 4.0*val(z))) );
        REQUIRE( derivative(h, wrt(z), at(x,y,z)) == approx(16.*z / std::hypot(2.0*val(x), 3.0*val(y), 4.0*val(z))) );
    }

    SECTION("testing complex expressions")
    {
        std::function<dual(dual, dual)> f;

        x = 0.5;
        y = 0.8;

        // Testing complex function involving sin, cos, and tan
        f = [](dual x, dual y) -> dual { return sin(x + y) * cos(x / y) + tan(2.0 * x * y) - sin(4*(x + y)*2/8) * cos(x*x / (y*y) * y/x) - tan((x + y) * (x + y) - x*x - y*y); };
        REQUIRE( val(f(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );

        // Testing complex function involving log, exp, pow, and sqrt
        f = [](dual x, dual y) -> dual { return log(x + y) * exp(x / y) + sqrt(2.0 * x * y) - 1 / pow(x, x + y) - exp(x*x / (y*y) * y/x) * log(4*(x + y)*2/8) - 4 * sqrt((x + y) * (x + y) - x*x - y*y) * 0.5 * 0.5 + 2 / pow(2.0 * x - x, y + x) * 0.5; };
        REQUIRE( val(f(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(0.0) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(0.0) );
    }

    SECTION("testing higher order derivatives")
    {
        using dual2nd = HigherOrderDual<2>;

        std::function<dual2nd(dual2nd, dual2nd)> f;

        dual2nd x = 5;
        dual2nd y = 7;

        // Testing simpler functions
        f = [](dual2nd x, dual2nd y) -> dual2nd
        {
            return x*x + x*y + y*y;
        };

        REQUIRE( val(derivative(f, wrt(x), at(x, y))) == Approx(17.0) );
        REQUIRE( derivative(f, wrt<2>(x), at(x, y)) == Approx(2.0) );
        REQUIRE( derivative(f, wrt(x, y), at(x, y)) == Approx(1.0) );
        REQUIRE( derivative(f, wrt(y, x), at(x, y)) == Approx(1.0) );
        REQUIRE( derivative(f, wrt<2>(y), at(x, y)) == Approx(2.0) );

        // Testing complex function involving log
        x = 2.0;
        y = 1.0;

        f = [](dual2nd x, dual2nd y) -> dual2nd
        {
            return 1 + x + y + x * y + y / x + log(x / y);
        };

        REQUIRE( val(f(x, y)) == Approx( 1 + val(x) + val(y) + val(x) * val(y) + val(y) / val(x) + log(val(x) / val(y)) ) );
        REQUIRE( val(derivative(f, wrt(x), at(x, y))) == Approx( 1 + val(y) - val(y) / (val(x) * val(x)) + 1.0/val(x) - log(val(y)) ) );
        REQUIRE( val(derivative(f, wrt(y), at(x, y))) == Approx( 1 + val(x) + 1.0 / val(x) - 1.0/val(y) ) );
        REQUIRE( val(derivative(f, wrt<2>(x), at(x, y))) == Approx( 2 * val(y) / (val(x) * val(x) * val(x)) + -1.0/(val(x) * val(x)) ) );
        REQUIRE( val(derivative(f, wrt(y, y), at(x, y))) == Approx( 1.0/(val(y)*val(y)) ) );
        REQUIRE( val(derivative(f, wrt(y, x), at(x, y))) == Approx( 1 - 1.0 / (val(x) * val(x)) ) );
        REQUIRE( val(derivative(f, wrt(x, y), at(x, y))) == Approx( 1 - 1.0 / (val(x) * val(x)) ) );
    }

    SECTION("testing higher order derivatives")
    {
        using dual3rd = HigherOrderDual<3>;

        dual3rd x = 5;
        dual3rd y = 7;

        // Testing complex function involving sin and cos
        auto f = [](dual3rd x, dual3rd y) -> dual3rd
        {
            return (x + y)*(x + y)*(x + y);
        };

        REQUIRE( derivative(f, wrt<3>(x), at(x, y)) == Approx(6.0) );
        REQUIRE( derivative(f, wrt(x, x, x), at(x, y)) == Approx(6.0) );
        REQUIRE( derivative(f, wrt(x, x, y), at(x, y)) == Approx(6.0) );
        REQUIRE( derivative(f, wrt(x, y, y), at(x, y)) == Approx(6.0) );
        REQUIRE( derivative(f, wrt<3>(y), at(x, y)) == Approx(6.0) );
    }

    SECTION("testing reference to unary and binary expression nodes are not present")
    {
        x = -0.3;
        y =  0.5;

        auto pow2 = [](const auto& x)
        {
            return x*x;
        };

        auto rosenbrock = [&](const auto& x, const auto& y)
        {
            return 100.0 * pow2(x*x - y) + pow2(1.0 - x);
        };

        auto f = [&](const auto& x, const auto& y) -> dual
        {
            return rosenbrock(x, y);
        };

        REQUIRE( val(f(x, y)) == approx(100 * (x*x - y)*(x*x - y) + (1 - x)*(1 - x)) );
        REQUIRE( derivative(f, wrt(x), at(x, y)) == approx(400*(x*x - y)*x - 2*(1 - x)) );
        REQUIRE( derivative(f, wrt(y), at(x, y)) == approx(-200*(x*x - y)) );
    }
}

TEST_CASE("Eigen::VectorXdual tests", "[dual]")
{
    SECTION("testing gradient derivatives")
    {
        // Testing complex function involving sin and cos
        auto f = [](const VectorXdual& x) -> dual
        {
            return 0.5 * ( x.array() * x.array() ).sum();
        };

        VectorXdual x(3);
        x << 1.0, 2.0, 3.0;

        VectorXd g = gradient(f, wrt(x), at(x));

        REQUIRE( g[0] == approx(x[0]) );
        REQUIRE( g[1] == approx(x[1]) );
        REQUIRE( g[2] == approx(x[2]) );
    }

    SECTION("testing gradient derivatives of only the last two variables")
    {
        // Testing complex function involving sin and cos
        auto f = [](const VectorXdual& x) -> dual
        {
            return 0.5 * ( x.array() * x.array() ).sum();
        };

        VectorXdual x(3);
        x << 1.0, 2.0, 3.0;

        VectorXd g = gradient(f, wrt(x.tail(2)), at(x));

        REQUIRE( g[0] == approx(x[1]) );
        REQUIRE( g[1] == approx(x[2]) );
    }

    SECTION("testing jacobian derivatives")
    {
        // Testing complex function involving sin and cos
        auto f = [](const VectorXdual& x) -> VectorXdual
        {
            return x / x.array().sum();
        };

        VectorXdual x(3);
        x << 0.5, 0.2, 0.3;

        VectorXdual F;

        const MatrixXd J = jacobian(f, wrt(x), at(x), F);

        for(auto i = 0; i < 3; ++i)
            for(auto j = 0; j < 3; ++j)
                REQUIRE( J(i, j) == approx(-F[i] + ((i == j) ? 1.0 : 0.0)) );
    }

    SECTION("testing jacobian derivatives of only the last two variables")
    {
        // Testing complex function involving sin and cos
        auto f = [](const VectorXdual& x) -> VectorXdual
        {
            return x / x.array().sum();
        };

        VectorXdual x(3);
        x << 0.5, 0.2, 0.3;

        VectorXdual F;

        const MatrixXd J = jacobian(f, wrt(x.tail(2)), at(x), F);

        for(auto i = 0; i < 3; ++i)
            for(auto j = 0; j < 2; ++j)
                REQUIRE( J(i, j) == approx(-F[i] + ((i == j + 1) ? 1.0 : 0.0)) );
    }

    SECTION("testing casting to VectorXd")
    {
        VectorXdual x(3);
        x << 0.5, 0.2, 0.3;

        VectorXd y = x.cast<double>();

        for(auto i = 0; i < 3; ++i)
            REQUIRE( x(i) == approx(y(i)) );
    }

    SECTION("testing casting to VectorXf")
    {
    	MatrixXdual x(2, 2);
        x << 0.5, 0.2, 0.3, 0.7;
        MatrixXd y = x.cast<double>();
        for(auto i = 0; i < 2; ++i)
            for(auto j = 0; j < 2; ++j)
                REQUIRE( x(i, j) == approx(y(i, j)) );
    }

    SECTION("test gradient size with respect to few arguments")
    {
        // Testing complex function involving sin and cos
        auto f = [](const VectorXdual& x, dual y, const VectorXdual& z) -> dual
        {
            return 0.5 * (( x.array() * x.array() ).sum() + y * y + (z.array() * z.array()).sum());
        };

        VectorXdual x(3);
        x << 1.0, 2.0, 3.0;

        dual y = 2;

        VectorXdual z(4);
        z << 1.0, 2.0, 3.0, 4.0;

        VectorXd g = gradient(f, wrtpack(x.tail(2), y, z), at(x, y, z));

        REQUIRE( g.size() == 7 );
    }

    SECTION("testing gradient derivatives wrt pack variables")
    {
        auto f = [](const VectorXdual& x, dual y, const VectorXdual& z) -> dual
        {
            return 0.5 * (( x.array() * x.array() ).sum() + y * y + (z.array() * z.array()).sum());
        };

        VectorXdual x(2);
        x << 1.0, 2.0;

        dual y = 3.0;

        VectorXdual z(1);
        z << 4.0;

        VectorXd g = gradient(f, wrtpack(x, y, z), at(x, y, z));

        REQUIRE(g[0] == approx(x[0]));
        REQUIRE(g[1] == approx(x[1]));
        REQUIRE(g[2] == approx(y));
        REQUIRE(g[3] == approx(z[0]));
    }

    SECTION("test jacobian size with respect to few arguments")
    {
        auto f = [](const VectorXdual& x, dual y, const VectorXdual& z) -> VectorXdual
        {
            VectorXdual ret(x.size() + z.size());
            ret.head(x.size()) = x * y / x.array().sum();
            ret.tail(z.size()) = y * z;

            return ret;
        };

        VectorXdual x(3);
        x << 0.5, 0.2, 0.3;

        dual y = 2.0;

        VectorXdual z(3);
        z << 1.0, 2.0, 3.0;

        VectorXdual F;

        const MatrixXd J = jacobian(f, wrtpack(x.tail(2), y), at(x, y, z), F);

        REQUIRE(J.rows() == 6);
        REQUIRE(J.cols() == 3);
    }

    SECTION("test jacobian size with respect to few arguments")
    {
        auto f = [](const VectorXdual& x, dual y, const VectorXdual& z) -> VectorXdual
        {
            VectorXdual ret(x.size() + z.size());
            ret.head(x.size()) = x * y / x.array().sum();
            ret.tail(z.size()) = y * z;

            return ret;
        };

        VectorXdual x(3);
        x << 0.5, 0.2, 0.3;

        dual y = 2.0;

        VectorXdual z(3);
        z << 1.0, 2.0, 3.0;

        VectorXdual F;

        const MatrixXd J = jacobian(f, wrtpack(x, y, z), at(x, y, z), F);

        for (auto i = 0; i < 3; ++i)
            for (auto j = 0; j < 3; ++j)
                REQUIRE(J(i, j) == approx(-F[i] + ((i == j) ? y.val : 0.0)));

        for (auto i = 0; i < 3; ++i)
            for (auto j = 0; j < 3; ++j)
                REQUIRE(J(i + 3, j) == approx(0.0));

        for (auto i = 0; i < 6; ++i)
                REQUIRE(J(i, 3) == approx( i < 3 ? x(i) : z(i - 3)));

        for (auto i = 0; i < 3; ++i)
            for (auto j = 0; j < 3; ++j)
                REQUIRE(J(i, j + 4) == approx(0.0));

        for (auto i = 0; i < 3; ++i)
            for (auto j = 0; j < 3; ++j)
                REQUIRE(J(i + 3, j + 4) == approx((i == j) ? y.val : 0.0));
    }
}


#include<iostream>
#include <Eigen/Dense>
#include <math.h>
#include <vector>
#include <Eigen/QR>

using namespace std;
Eigen::MatrixXf pseudoinverse(Eigen::MatrixXf m)
{
    //Eigen::Matrix<float,2,3> m;
  //  m<<0.68,0.597,-0.211, 0.823,0.566,-0.605;
   // m<<1,2,3,4,5,6;
    cout<<m<<endl;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd =m.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
   // Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
    const Eigen::MatrixXf singularValues = svd.singularValues();
	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> singularValuesInv(m.cols(), m.rows());
	singularValuesInv.setZero();
	double  pinvtoler = 1.e-6; // choose your tolerance wisely
	for (unsigned int i = 0; i < singularValues.size(); ++i) {
	 	if (singularValues(i) > pinvtoler)
	 		singularValuesInv(i, i) = 1.0f / singularValues(i);
	 	else
	 		singularValuesInv(i, i) = 0.f;
	 }
    Eigen::MatrixXf pinvmat = svd.matrixV() * singularValuesInv * svd.matrixU().transpose();
    std::cout << pinvmat << std::endl;
	return pinvmat;
}
int main(int argc, char **argv){
      Eigen::Matrix<double, 6, 1> error;
      for(int i=0; i<6;i++)
      for (int j = 0; j < 1; j++)
      {
          error(i,j)=1;
          std::cout<<error(i,j);
      }
    cout<<endl;
    std::vector<int>ve;
    ve.push_back(1);
    ve.push_back(2);
    cout<<ve.data()<<endl;
    Eigen::Vector4f ve4(1,2,3,4);
    std::cout<<error.cols()<<error.rows()<<endl;
    cout<<ve4<<endl;

    Eigen::MatrixXf m(2,2);
    m(0,0) = 3;
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);

    array<double,4>arr={2,6,7.11,8.23};

    Eigen::Map<const Eigen::Matrix<double, 2, 2>>m2(arr.data());
    cout<<m2<<endl;
    // m2<<1,2,3,4;
    // cout<<m2<<endl;
    //pseudoinverse();
    // Eigen::Matrix<float, 6, 7> name=Eigen::Matrix<float,6,7>::Random();
    // Eigen::Matrix<float,3,7> res;
    // for (int i = 0; i < res.rows(); i++)
    // {
    //     for (int j = 0; j < res.cols(); j++)
    //     {
    //             res(i,j)=name(i,j);
    //     }
    // }
    // cout<<res;

//    Eigen::Matrix<float,2,3> m2;
//    m2<<0.68,0.597,-0.211, 0.823,0.566,-0.605;
//    m2<<1,2,3,4,5,6;
//    cout<<m2;

  //cout<<(pseudoinverse(res));

}