/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

#ifndef GTSAM_VIO_FEATURE_H
#define GTSAM_VIO_FEATURE_H

#include <iostream>
#include <map>
#include <vector>

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

#include "math_utils.hpp"
#include "imu_state.h"
#include "cam_state.h"

namespace gtsam_vio {

/*
 * @brief Feature Salient part of an image. Please refer
 *    to the Appendix of "A Multi-State Constraint Kalman
 *    Filter for Vision-aided Inertial Navigation" for how
 *    the 3d position of a feature is initialized.
 */
struct Feature {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef long long int FeatureIDType;

  /*
   * @brief OptimizationConfig Configuration parameters
   *    for 3d feature position optimization.
   */
  struct OptimizationConfig {
    double translation_threshold;
    double huber_epsilon;
    double estimation_precision;
    double initial_damping;
    int outer_loop_max_iteration;
    int inner_loop_max_iteration;

    OptimizationConfig():
      translation_threshold(0.2),
      huber_epsilon(0.01),
      estimation_precision(5e-7),
      initial_damping(1e-3),
      outer_loop_max_iteration(10),
      inner_loop_max_iteration(10) {
      return;
    }
  };

  // Constructors for the struct.
  Feature(): id(0), position(Eigen::Vector3d::Zero()),
    is_initialized(false) {}

  Feature(const FeatureIDType& new_id): id(new_id),
    position(Eigen::Vector3d::Zero()),
    is_initialized(false) {}

  /*
   * @brief cost Compute the cost of the camera observations
   * @param T_c0_c1 A rigid body transformation takes
   *    a vector in c0 frame to ci frame.
   * @param x The current estimation.
   * @param z The ith measurement of the feature j in ci frame.
   * @return e The cost of this observation.
   */
  inline void cost(const Eigen::Isometry3d& T_c0_ci,
      const Eigen::Vector3d& x, const Eigen::Vector2d& z,
      double& e) const;

  /*
   * @brief jacobian Compute the Jacobian of the camera observation
   * @param T_c0_c1 A rigid body transformation takes
   *    a vector in c0 frame to ci frame.
   * @param x The current estimation.
   * @param z The actual measurement of the feature in ci frame.
   * @return J The computed Jacobian.
   * @return r The computed residual.
   * @return w Weight induced by huber kernel.
   */
  inline void jacobian(const Eigen::Isometry3d& T_c0_ci,
      const Eigen::Vector3d& x, const Eigen::Vector2d& z,
      Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
      double& w) const;

  /*
   * @brief generateInitialGuess Compute the initial guess of
   *    the feature's 3d position using only two views.
   * @param T_c1_c2: A rigid body transformation taking
   *    a vector from c2 frame to c1 frame.
   * @param z1: feature observation in c1 frame.
   * @param z2: feature observation in c2 frame.
   * @return p: Computed feature position in c1 frame.
   */
  inline void generateInitialGuess(
      const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
      const Eigen::Vector2d& z2, Eigen::Vector3d& p) const;

  /*
   * @brief checkMotion Check the input camera poses to ensure
   *    there is enough translation to triangulate the feature
   *    positon.
   * @param cam_states : input camera poses.
   * @return True if the translation between the input camera
   *    poses is sufficient.
   */
  inline bool checkMotion(
      const CamStateServer& cam_states) const;

  /*
   * @brief InitializePosition Intialize the feature position
   *    based on all current available measurements.
   * @param cam_states: A map containing the camera poses with its
   *    ID as the associated key value.
   * @return The computed 3d position is used to set the position
   *    member variable. Note the resulted position is in world
   *    frame.
   * @return True if the estimated 3d position of the feature
   *    is valid.
   */
  inline bool initializePosition(
      const CamStateServer& cam_states);

  // An unique identifier for the feature.
  // In case of long time running, the variable
  // type of id is set to FeatureIDType in order
  // to avoid duplication.
  FeatureIDType id;

  // id for next feature
  static FeatureIDType next_id;

  // Store the observations of the features in the
  // state_id(key)-image_coordinates(value) manner.
  std::map<StateIDType, Eigen::Vector4d, std::less<StateIDType>,
    Eigen::aligned_allocator<
      std::pair<const StateIDType, Eigen::Vector4d> > > observations;

  // 3d postion of the feature in the world frame.
  Eigen::Vector3d position;

  // A indicator to show if the 3d postion of the feature
  // has been initialized or not.
  bool is_initialized;

  // Noise for a normalized feature measurement.
  static double observation_noise;

  // Optimization configuration for solving the 3d position.
  static OptimizationConfig optimization_config;

};

typedef Feature::FeatureIDType FeatureIDType;
typedef std::map<FeatureIDType, Feature, std::less<int>,
        Eigen::aligned_allocator<
        std::pair<const FeatureIDType, Feature> > > MapServer;

void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
    const Eigen::Vector3d& x, const Eigen::Vector2d& z,
    double& e) const {
  // Compute hi1, hi2, and hi3 as Equation (37).
  const double& alpha = x(0);
  const double& beta = x(1);
  const double& rho = x(2);

  Eigen::Vector3d h = T_c0_ci.linear()*
    Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation();
  double& h1 = h(0);
  double& h2 = h(1);
  double& h3 = h(2);

  // Predict the feature observation in ci frame.
  Eigen::Vector2d z_hat(h1/h3, h2/h3);

  // Compute the residual.
  e = (z_hat-z).squaredNorm();
  return;
}

void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
    const Eigen::Vector3d& x, const Eigen::Vector2d& z,
    Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
    double& w) const {

  // Compute hi1, hi2, and hi3 as Equation (37).
  const double& alpha = x(0);
  const double& beta = x(1);
  const double& rho = x(2);

  Eigen::Vector3d h = T_c0_ci.linear()*
    Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation();
  double& h1 = h(0);
  double& h2 = h(1);
  double& h3 = h(2);

  // Compute the Jacobian.
  Eigen::Matrix3d W;
  W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
  W.rightCols<1>() = T_c0_ci.translation();

  J.row(0) = 1/h3*W.row(0) - h1/(h3*h3)*W.row(2);
  J.row(1) = 1/h3*W.row(1) - h2/(h3*h3)*W.row(2);

  // Compute the residual.
  Eigen::Vector2d z_hat(h1/h3, h2/h3);
  r = z_hat - z;

  // Compute the weight based on the residual.
  double e = r.norm();
  if (e <= optimization_config.huber_epsilon)
    w = 1.0;
  else
    w = optimization_config.huber_epsilon / (2*e);

  return;
}

void Feature::generateInitialGuess(
    const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const {
  // Construct a least square problem to solve the depth.
  Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);

  Eigen::Vector2d A(0.0, 0.0);
  A(0) = m(0) - z2(0)*m(2);
  A(1) = m(1) - z2(1)*m(2);

  Eigen::Vector2d b(0.0, 0.0);
  b(0) = z2(0)*T_c1_c2.translation()(2) - T_c1_c2.translation()(0);
  b(1) = z2(1)*T_c1_c2.translation()(2) - T_c1_c2.translation()(1);

  // Solve for the depth.
  double depth = (A.transpose() * A).inverse() * A.transpose() * b;
  p(0) = z1(0) * depth;
  p(1) = z1(1) * depth;
  p(2) = depth;
  return;
}

bool Feature::checkMotion(
    const CamStateServer& cam_states) const {

  const StateIDType& first_cam_id = observations.begin()->first;
  const StateIDType& last_cam_id = (--observations.end())->first;

  Eigen::Isometry3d first_cam_pose;
  first_cam_pose.linear() = quaternionToRotation(
      cam_states.find(first_cam_id)->second.orientation).transpose();
  first_cam_pose.translation() =
    cam_states.find(first_cam_id)->second.position;

  Eigen::Isometry3d last_cam_pose;
  last_cam_pose.linear() = quaternionToRotation(
      cam_states.find(last_cam_id)->second.orientation).transpose();
  last_cam_pose.translation() =
    cam_states.find(last_cam_id)->second.position;

  // Get the direction of the feature when it is first observed.
  // This direction is represented in the world frame.
  Eigen::Vector3d feature_direction(
      observations.begin()->second(0),
      observations.begin()->second(1), 1.0);
  feature_direction = feature_direction / feature_direction.norm();
  feature_direction = first_cam_pose.linear()*feature_direction;

  // Compute the translation between the first frame
  // and the last frame. We assume the first frame and
  // the last frame will provide the largest motion to
  // speed up the checking process.
  Eigen::Vector3d translation = last_cam_pose.translation() -
    first_cam_pose.translation();
  double parallel_translation =
    translation.transpose()*feature_direction;
  Eigen::Vector3d orthogonal_translation = translation -
    parallel_translation*feature_direction;

  if (orthogonal_translation.norm() >
      optimization_config.translation_threshold)
    return true;
  else return false;
}

bool Feature::initializePosition(
    const CamStateServer& cam_states) {
  // Organize camera poses and feature observations properly.
  std::vector<Eigen::Isometry3d,
    Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
  std::vector<Eigen::Vector2d,
    Eigen::aligned_allocator<Eigen::Vector2d> > measurements(0);

  for (auto& m : observations) {
    // TODO: This should be handled properly. Normally, the
    //    required camera states should all be available in
    //    the input cam_states buffer.
    auto cam_state_iter = cam_states.find(m.first);
    if (cam_state_iter == cam_states.end()) continue;

    // Add the measurement.
    measurements.push_back(m.second.head<2>());
    measurements.push_back(m.second.tail<2>());

    // This camera pose will take a vector from this camera frame
    // to the world frame.
    Eigen::Isometry3d cam0_pose;
    cam0_pose.linear() = quaternionToRotation(
        cam_state_iter->second.orientation).transpose();
    cam0_pose.translation() = cam_state_iter->second.position;

    Eigen::Isometry3d cam1_pose;
    cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse();

    cam_poses.push_back(cam0_pose);
    cam_poses.push_back(cam1_pose);
  }

  // All camera poses should be modified such that it takes a
  // vector from the first camera frame in the buffer to this
  // camera frame.
  Eigen::Isometry3d T_c0_w = cam_poses[0];
  for (auto& pose : cam_poses)
    pose = pose.inverse() * T_c0_w;

  // Generate initial guess
  Eigen::Vector3d initial_position(0.0, 0.0, 0.0);
  generateInitialGuess(cam_poses[cam_poses.size()-1], measurements[0],
      measurements[measurements.size()-1], initial_position);
  Eigen::Vector3d solution(
      initial_position(0)/initial_position(2),
      initial_position(1)/initial_position(2),
      1.0/initial_position(2));

  // Apply Levenberg-Marquart method to solve for the 3d position.
  double lambda = optimization_config.initial_damping;
  int inner_loop_cntr = 0;
  int outer_loop_cntr = 0;
  bool is_cost_reduced = false;
  double delta_norm = 0;

  // Compute the initial cost.
  double total_cost = 0.0;
  for (int i = 0; i < cam_poses.size(); ++i) {
    double this_cost = 0.0;
    cost(cam_poses[i], solution, measurements[i], this_cost);
    total_cost += this_cost;
  }

  // Outer loop.
  do {
    Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
    Eigen::Vector3d b = Eigen::Vector3d::Zero();

    for (int i = 0; i < cam_poses.size(); ++i) {
      Eigen::Matrix<double, 2, 3> J;
      Eigen::Vector2d r;
      double w;

      jacobian(cam_poses[i], solution, measurements[i], J, r, w);

      if (w == 1) {
        A += J.transpose() * J;
        b += J.transpose() * r;
      } else {
        double w_square = w * w;
        A += w_square * J.transpose() * J;
        b += w_square * J.transpose() * r;
      }
    }

    // Inner loop.
    // Solve for the delta that can reduce the total cost.
    do {
      Eigen::Matrix3d damper = lambda * Eigen::Matrix3d::Identity();
      Eigen::Vector3d delta = (A+damper).ldlt().solve(b);
      Eigen::Vector3d new_solution = solution - delta;
      delta_norm = delta.norm();

      double new_cost = 0.0;
      for (int i = 0; i < cam_poses.size(); ++i) {
        double this_cost = 0.0;
        cost(cam_poses[i], new_solution, measurements[i], this_cost);
        new_cost += this_cost;
      }

      if (new_cost < total_cost) {
        is_cost_reduced = true;
        solution = new_solution;
        total_cost = new_cost;
        lambda = lambda/10 > 1e-10 ? lambda/10 : 1e-10;
      } else {
        is_cost_reduced = false;
        lambda = lambda*10 < 1e12 ? lambda*10 : 1e12;
      }

    } while (inner_loop_cntr++ <
        optimization_config.inner_loop_max_iteration && !is_cost_reduced);

    inner_loop_cntr = 0;

  } while (outer_loop_cntr++ <
      optimization_config.outer_loop_max_iteration &&
      delta_norm > optimization_config.estimation_precision);

  // Covert the feature position from inverse depth
  // representation to its 3d coordinate.
  Eigen::Vector3d final_position(solution(0)/solution(2),
      solution(1)/solution(2), 1.0/solution(2));

  // Check if the solution is valid. Make sure the feature
  // is in front of every camera frame observing it.
  bool is_valid_solution = true;
  for (const auto& pose : cam_poses) {
    Eigen::Vector3d position =
      pose.linear()*final_position + pose.translation();
    if (position(2) <= 0) {
      is_valid_solution = false;
      break;
    }
  }

  // Convert the feature position to the world frame.
  position = T_c0_w.linear()*final_position + T_c0_w.translation();

  if (is_valid_solution)
    is_initialized = true;

  return is_valid_solution;
}
} // namespace gtsam_vio

#endif // GTSAM_VIO_FEATURE_H


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/static_assert.hpp>

//////////////////////////////////////////

void case1()
{
    BOOST_STATIC_ASSERT(2 == sizeof(short));
    BOOST_STATIC_ASSERT(true);
    BOOST_STATIC_ASSERT_MSG(16 == 0x10, "test static assert");
}

//////////////////////////////////////////

template<typename T>
T my_min(T a, T b)
{
    BOOST_STATIC_ASSERT_MSG(sizeof(T) < sizeof(int), "only short or char");
    return a < b? a: b;
}
void case2()
{
    cout << my_min((short)1, (short)3);
    //cout << my_min(1L, 3L);
}

//////////////////////////////////////////

namespace my_space
{
    class empty_class
    {
        BOOST_STATIC_ASSERT_MSG(sizeof(int)>=4, "for 32 bit");
    };

    BOOST_STATIC_ASSERT(sizeof(empty_class) == 1);
}

int main()
{
    case1();
    case2();
}


/********************************************************************************
Copyright (c) 2015, TRACLabs, Inc.
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice,
       this list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright notice,
       this list of conditions and the following disclaimer in the documentation
       and/or other materials provided with the distribution.

    3. Neither the name of the copyright holder nor the names of its contributors
       may be used to endorse or promote products derived from this software
       without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
********************************************************************************/

#include <boost/date_time.hpp>
#include <boost/math/tools/precision.hpp>
#include <cmath>
#include <limits>
#include <trac_ik/dual_quaternion.h>
#include <trac_ik/nlopt_ik.hpp>

namespace NLOPT_IK
{

    dual_quaternion targetDQ;

    double minfunc(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.

        NLOPT_IK *c = (NLOPT_IK *)data;

        return c->minJoints(x, grad);
    }

    double minfuncDQ(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.
        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(x);

        double jump = boost::math::tools::epsilon<float>();
        double result[1];
        c->cartDQError(vals, result);

        if (!grad.empty())
        {
            double v1[1];
            for (uint i = 0; i < x.size(); i++)
            {
                double original = vals[i];

                vals[i] = original + jump;
                c->cartDQError(vals, v1);

                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2 * jump);
            }
        }

        return result[0];
    }

    double minfuncSumSquared(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.

        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(x);

        double jump = boost::math::tools::epsilon<float>();
        double result[1];
        c->cartSumSquaredError(vals, result);

        if (!grad.empty())
        {
            double v1[1];
            for (uint i = 0; i < x.size(); i++)
            {
                double original = vals[i];

                vals[i] = original + jump;
                c->cartSumSquaredError(vals, v1);

                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2.0 * jump);
            }
        }

        return result[0];
    }

    double minfuncL2(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.

        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(x);

        double jump = boost::math::tools::epsilon<float>();
        double result[1];
        c->cartL2NormError(vals, result);

        if (!grad.empty())
        {
            double v1[1];
            for (uint i = 0; i < x.size(); i++)
            {
                double original = vals[i];

                vals[i] = original + jump;
                c->cartL2NormError(vals, v1);

                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2.0 * jump);
            }
        }

        return result[0];
    }

    void constrainfuncm(uint m, double *result, uint n, const double *x, double *grad, void *data)
    {
        //Equality constraint auxilary function for Euclidean distance .
        //This also uses a small walk to approximate the gradient of the
        //constraint function at the current joint angles.

        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(n);

        for (uint i = 0; i < n; i++)
        {
            vals[i] = x[i];
        }

        double jump = boost::math::tools::epsilon<float>();

        c->cartSumSquaredError(vals, result);

        if (grad != NULL)
        {
            std::vector<double> v1(m);
            for (uint i = 0; i < n; i++)
            {
                double o = vals[i];
                vals[i] = o + jump;
                c->cartSumSquaredError(vals, v1.data());
                vals[i] = o;
                for (uint j = 0; j < m; j++)
                {
                    grad[j * n + i] = (v1[j] - result[j]) / (2 * jump);
                }
            }
        }
    }

    NLOPT_IK::NLOPT_IK(const KDL::Chain &_chain, const KDL::JntArray &_q_min, const KDL::JntArray &_q_max, double _maxtime, double _eps, OptType _type) : chain(_chain), fksolver(chain), maxtime(_maxtime), eps(std::abs(_eps)), TYPE(_type)
    {
        assert(chain.getNrOfJoints() == _q_min.data.size());
        assert(chain.getNrOfJoints() == _q_max.data.size());

        //Constructor for an IK Class.  Takes in a Chain to operate on,
        //the min and max joint limits, an (optional) maximum number of
        //iterations, and an (optional) desired error.
        reset();

        if (chain.getNrOfJoints() < 2)
        {
            std::cout << "WARNING :"
                      << "NLOpt_IK can only be run for chains of length 2 or more";
            return;
        }
        opt = nlopt::opt(nlopt::LD_SLSQP, _chain.getNrOfJoints());

        for (uint i = 0; i < chain.getNrOfJoints(); i++)
        {
            lb.push_back(_q_min(i));
            ub.push_back(_q_max(i));
        }

        for (uint i = 0; i < chain.segments.size(); i++)
        {
            std::string type = chain.segments[i].getJoint().getTypeName();
            if (type.find("Rot") != std::string::npos)
            {
                if (_q_max(types.size()) >= std::numeric_limits<float>::max() &&
                    _q_min(types.size()) <= std::numeric_limits<float>::lowest())
                    types.push_back(KDL::BasicJointType::Continuous);
                else
                    types.push_back(KDL::BasicJointType::RotJoint);
            }
            else if (type.find("Trans") != std::string::npos)
                types.push_back(KDL::BasicJointType::TransJoint);
        }

        assert(types.size() == lb.size());

        std::vector<double> tolerance(1, boost::math::tools::epsilon<float>());
        opt.set_xtol_abs(tolerance[0]);

        switch (TYPE)
        {
        case Joint:
            opt.set_min_objective(minfunc, this);
            opt.add_equality_mconstraint(constrainfuncm, this, tolerance);
            break;
        case DualQuat:
            opt.set_min_objective(minfuncDQ, this);
            break;
        case SumSq:
            opt.set_min_objective(minfuncSumSquared, this);
            break;
        case L2:
            opt.set_min_objective(minfuncL2, this);
            break;
        }
    }

    double NLOPT_IK::minJoints(const std::vector<double> &x, std::vector<double> &grad)
    {
        // Actual function to compute the error between the current joint
        // configuration and the desired.  The SSE is easy to provide a
        // closed form gradient for.

        bool gradient = !grad.empty();

        double err = 0;
        for (uint i = 0; i < x.size(); i++)
        {
            err += pow(x[i] - des[i], 2);
            if (gradient)
                grad[i] = 2.0 * (x[i] - des[i]);
        }

        return err;
    }

    void NLOPT_IK::cartSumSquaredError(const std::vector<double> &x, double error[])
    {
        // Actual function to compute Euclidean distance error.  This uses
        // the KDL Forward Kinematics solver to compute the Cartesian pose
        // of the current joint configuration and compares that to the
        // desired Cartesian pose for the IK solve.

        if (aborted || progress != -3)
        {
            opt.force_stop();
            return;
        }

        KDL::JntArray q(x.size());

        for (uint i = 0; i < x.size(); i++)
            q(i) = x[i];

        int rc = fksolver.JntToCart(q, currentPose);

        if (rc < 0)
            std::cout << "KDL FKSolver is failing: " << q.data;

        if (std::isnan(currentPose.p.x()))
        {
            std::cout << "ERROR :"
                      << "NaNs from NLOpt!!";
            error[0] = std::numeric_limits<float>::max();
            progress = -1;
            return;
        }

        KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);

        for (int i = 0; i < 6; i++)
        {
            if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
                delta_twist[i] = 0.0;
        }

        error[0] = KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot);

        if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
        {
            progress = 1;
            best_x = x;
            return;
        }
    }

    void NLOPT_IK::cartL2NormError(const std::vector<double> &x, double error[])
    {
        // Actual function to compute Euclidean distance error.  This uses
        // the KDL Forward Kinematics solver to compute the Cartesian pose
        // of the current joint configuration and compares that to the
        // desired Cartesian pose for the IK solve.

        if (aborted || progress != -3)
        {
            opt.force_stop();
            return;
        }

        KDL::JntArray q(x.size());

        for (uint i = 0; i < x.size(); i++)
            q(i) = x[i];

        int rc = fksolver.JntToCart(q, currentPose);

        if (rc < 0)
            std::cout << "KDL FKSolver is failing: " << q.data;

        if (std::isnan(currentPose.p.x()))
        {
            std::cout << "ERROR :"
                      << "NaNs from NLOpt!!";
            error[0] = std::numeric_limits<float>::max();
            progress = -1;
            return;
        }

        KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);

        for (int i = 0; i < 6; i++)
        {
            if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
                delta_twist[i] = 0.0;
        }

        error[0] = std::sqrt(KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot));

        if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
        {
            progress = 1;
            best_x = x;
            return;
        }
    }

    void NLOPT_IK::cartDQError(const std::vector<double> &x, double error[])
    {
        // Actual function to compute Euclidean distance error.  This uses
        // the KDL Forward Kinematics solver to compute the Cartesian pose
        // of the current joint configuration and compares that to the
        // desired Cartesian pose for the IK solve.

        if (aborted || progress != -3)
        {
            opt.force_stop();
            return;
        }

        KDL::JntArray q(x.size());

        for (uint i = 0; i < x.size(); i++)
            q(i) = x[i];

        int rc = fksolver.JntToCart(q, currentPose);

        if (rc < 0)
            std::cout << "KDL FKSolver is failing: " << q.data;

        if (std::isnan(currentPose.p.x()))
        {
            std::cout << "ERROR :"
                      << "NaNs from NLOpt!!";
            error[0] = std::numeric_limits<float>::max();
            progress = -1;
            return;
        }

        KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);

        for (int i = 0; i < 6; i++)
        {
            if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
                delta_twist[i] = 0.0;
        }

        math3d::matrix3x3<double> currentRotationMatrix(currentPose.M.data);
        math3d::quaternion<double> currentQuaternion = math3d::rot_matrix_to_quaternion<double>(currentRotationMatrix);
        math3d::point3d currentTranslation(currentPose.p.data);
        dual_quaternion currentDQ = dual_quaternion::rigid_transformation(currentQuaternion, currentTranslation);

        dual_quaternion errorDQ = (currentDQ * !targetDQ).normalize();
        errorDQ.log();
        error[0] = 4.0f * dot(errorDQ, errorDQ);

        if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
        {
            progress = 1;
            best_x = x;
            return;
        }
    }

    int NLOPT_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist _bounds, const KDL::JntArray &q_desired)
    {
        // User command to start an IK solve.  Takes in a seed
        // configuration, a Cartesian pose, and (optional) a desired
        // configuration.  If the desired is not provided, the seed is
        // used.  Outputs the joint configuration found that solves the
        // IK.

        // Returns -3 if a configuration could not be found within the eps
        // set up in the constructor.

        boost::posix_time::ptime start_time = boost::posix_time::microsec_clock::local_time();
        boost::posix_time::time_duration diff;

        bounds = _bounds;
        q_out = q_init;

        if (chain.getNrOfJoints() < 2)
        {
            std::cout << "ERROR ："
                      << "NLOpt_IK can only be run for chains of length 2 or more";
            return -3;
        }

        if (q_init.data.size() != types.size())
        {
            printf(" ERROR : IK seeded with wrong number of joints.  Expected %d but got %d", (int)types.size(), (int)q_init.data.size());
            return -3;
        }

        opt.set_maxtime(maxtime);

        double minf; /* the minimum objective value, upon return */

        targetPose = p_in;

        if (TYPE == 1) // DQ
        {
            math3d::matrix3x3<double> targetRotationMatrix(targetPose.M.data);
            math3d::quaternion<double> targetQuaternion = math3d::rot_matrix_to_quaternion<double>(targetRotationMatrix);
            math3d::point3d targetTranslation(targetPose.p.data);
            targetDQ = dual_quaternion::rigid_transformation(targetQuaternion, targetTranslation);
        }
        // else if (TYPE == 1)
        // {
        //   z_target = targetPose*z_up;
        //   x_target = targetPose*x_out;
        //   y_target = targetPose*y_out;
        // }

        //    fksolver.JntToCart(q_init,currentPose);

        std::vector<double> x(chain.getNrOfJoints());

        for (uint i = 0; i < x.size(); i++)
        {
            x[i] = q_init(i);

            if (types[i] == KDL::BasicJointType::Continuous)
                continue;

            if (types[i] == KDL::BasicJointType::TransJoint)
            {
                x[i] = std::min(x[i], ub[i]);
                x[i] = std::max(x[i], lb[i]);
            }
            else
            {

                // Below is to handle bad seeds outside of limits

                if (x[i] > ub[i])
                {
                    //Find actual angle offset
                    double diffangle = fmod(x[i] - ub[i], 2 * M_PI);
                    // Add that to upper bound and go back a full rotation
                    x[i] = ub[i] + diffangle - 2 * M_PI;
                }

                if (x[i] < lb[i])
                {
                    //Find actual angle offset
                    double diffangle = fmod(lb[i] - x[i], 2 * M_PI);
                    // Subtract that from lower bound and go forward a full rotation
                    x[i] = lb[i] - diffangle + 2 * M_PI;
                }

                if (x[i] > ub[i])
                    x[i] = (ub[i] + lb[i]) / 2.0;
            }
        }

        best_x = x;
        progress = -3;

        std::vector<double> artificial_lower_limits(lb.size());

        for (uint i = 0; i < lb.size(); i++)
            if (types[i] == KDL::BasicJointType::Continuous)
                artificial_lower_limits[i] = best_x[i] - 2 * M_PI;
            else if (types[i] == KDL::BasicJointType::TransJoint)
                artificial_lower_limits[i] = lb[i];
            else
                artificial_lower_limits[i] = std::max(lb[i], best_x[i] - 2 * M_PI);

        opt.set_lower_bounds(artificial_lower_limits);

        std::vector<double> artificial_upper_limits(lb.size());

        for (uint i = 0; i < ub.size(); i++)
            if (types[i] == KDL::BasicJointType::Continuous)
                artificial_upper_limits[i] = best_x[i] + 2 * M_PI;
            else if (types[i] == KDL::BasicJointType::TransJoint)
                artificial_upper_limits[i] = ub[i];
            else
                artificial_upper_limits[i] = std::min(ub[i], best_x[i] + 2 * M_PI);

        opt.set_upper_bounds(artificial_upper_limits);

        if (q_desired.data.size() == 0)
        {
            des = x;
        }
        else
        {
            des.resize(x.size());
            for (uint i = 0; i < des.size(); i++)
                des[i] = q_desired(i);
        }

        try
        {
            opt.optimize(x, minf);
        }
        catch (...)
        {
        }

        if (progress == -1) // Got NaNs
            progress = -3;

        if (!aborted && progress < 0)
        {

            double time_left;
            diff = boost::posix_time::microsec_clock::local_time() - start_time;
            time_left = maxtime - diff.total_nanoseconds() / 1000000000.0;

            while (time_left > 0 && !aborted && progress < 0)
            {

                for (uint i = 0; i < x.size(); i++)
                    x[i] = fRand(artificial_lower_limits[i], artificial_upper_limits[i]);

                opt.set_maxtime(time_left);

                try
                {
                    opt.optimize(x, minf);
                }
                catch (...)
                {
                }

                if (progress == -1) // Got NaNs
                    progress = -3;

                diff = boost::posix_time::microsec_clock::local_time() - start_time;
                time_left = maxtime - diff.total_nanoseconds() / 1000000000.0;
            }
        }

        for (uint i = 0; i < x.size(); i++)
        {
            q_out(i) = best_x[i];
        }

        return progress;
    }

}


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

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/16/problem16.hpp"

BOOST_AUTO_TEST_SUITE( Problem16 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem16::solve(15);
        BOOST_CHECK_EQUAL(res, 26);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem16::solve();
        BOOST_CHECK_EQUAL(res, 1366);
    }

BOOST_AUTO_TEST_SUITE_END()


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __LMA_OPT2_TRAIT_USE_ESTIMATOR_HPP__
#define __LMA_OPT2_TRAIT_USE_ESTIMATOR_HPP__

#include <libv/lma/version.hpp>

#include <libv/lma/lm/container/container.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/mpl/bool.hpp>
#include <libv/lma/numeric/mediane.hpp>
#include <iostream>

namespace lma
{
  struct MEstimator_{};

  template<class Base> struct MEstimator : MEstimator_
  {
    Base const& cast() const {return static_cast<Base const &>(*this); }

    template<class Float, size_t N>
    Eigen::Matrix<Float,N,1> me(const std::array<Float,N>& res, Float C) const
    {
      Eigen::Matrix<Float,N,1> ret;
      for(size_t i = 0 ; i < N ; ++i)
        ret[i] = cast().weight(res[i],C);
      return ret;
    }

    template<class Float>
    Eigen::Matrix<Float,1,1> me(const Float& res,const Float& C) const
    {
      Eigen::Matrix<Float,1,1> ret;
      ret << cast().weight(res,C);
      return ret;
    }

    template<class Float, int N>
    Eigen::Matrix<Float,N,1> me(const Eigen::Matrix<Float,N,1>& res, Float C) const
    {
      Eigen::Matrix<Float,N,1> ret;
      for(int i = 0 ; i < N ; ++i)
        ret[i] = cast().weight(res[i],C);
      return ret;
    }
  };

  namespace detail
  {
    template<class F> struct IsMEstimator : boost::is_convertible<F*,MEstimator_*>::type {};

    template<class W> struct Weight_J
    {
      const W& weight;
      Weight_J(const W& w_):weight(w_){}

      template<class Pair> void operator()(Pair& pair) const
      {
        for(int i = 0 ; i < Rows<decltype(pair.second)>::value ; ++i)
          for(int j = 0 ; j < Cols<decltype(pair.second)>::value ; ++j)
            pair.second(i,j) = pair.second(i,j)*weight[i];
      }
    };

    template< class F, class Jacobs, class Erreurs, class Mad>
    void apply_mestimator(const F& f, Jacobs& jacobs, Erreurs& erreur, const Mad& mad, typename boost::enable_if<detail::IsMEstimator<F>>::type* =0)
    {
      auto weight  = f.me(erreur,boost::fusion::at_key<F>(mad));
//       std::cout << " erreur " << erreur[0] << ", " << weight <<  std::endl;
      cwise_product(erreur,weight,erreur);
      boost::fusion::for_each(jacobs,Weight_J<decltype(weight)>(weight));
    }

    template<class F, class Jacobs, class Erreurs, class Mad> void apply_mestimator(const F&, Jacobs&, Erreurs&, const Mad&,typename boost::disable_if<detail::IsMEstimator<F>>::type* =0) { }

    template<class F, class Erreurs, class Mad>
    void apply_mestimator_erreur(const F& f, Erreurs& erreur, const Mad& mad, typename boost::enable_if<detail::IsMEstimator<F>>::type* =0)
    {
      auto weight  = f.me(erreur,boost::fusion::at_key<F>(mad));
      cwise_product(erreur,weight,erreur);
    }

    template<class F, class Erreurs, class Mad> void apply_mestimator_erreur(const F&, Erreurs&, const Mad&, typename boost::disable_if<detail::IsMEstimator<F>>::type* =0) { }

  }

  template<class Float>
  struct GermanMcClure : MEstimator<GermanMcClure<Float>>
  {
    Float coeff_mad;
    GermanMcClure(Float a_):coeff_mad(a_){}

    Float weight(const Float& res, const Float& C) const
    {
      return (C != 0 ? (C / (res*res+C*C)) : 1.0);
    }

    Float compute(std::vector<Float> norms) const
    {
      if (norms.empty())
        return 0;

      Float med = mediane(norms);

      for(Float& m : norms)
        m = std::abs(m-med);

      Float MAD = mediane(norms);

      Float C = med + coeff_mad * MAD;
      // std::cout << " C : " << C << " = " << med << " + " << coeff_mad << " * " << MAD << std::endl;
      return C;
    }
  };
}

#endif


#ifndef PARTICLES_HPP
#define PARTICLES_HPP

#include "integration.hpp"
#include <Eigen/Dense>
#include <vector>

using namespace Eigen;

class Particle;
class Force;

class ParticleSystem: public PhysicalSystem {
public:
    std::vector<Particle*> particles;
    std::vector<Force*> forces;
    void init();          // initialize the system
    void step(double dt); // perform a time step of length dt
    void draw();          // draw everything
    // PhysicalSystem functions, see integration.hpp
    int getDOFs();
    void getState(VectorXd &x, VectorXd &v);
    void setState(const VectorXd &x, const VectorXd &v);
    void getInertia(MatrixXd &M);
    void getForces(VectorXd &f);
    void getJacobians(MatrixXd &Jx, MatrixXd &Jv);
};

class Particle {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    int i;      // index
    double m;   // mass
    Vector2d x; // position
    Vector2d v; // velocity
    Particle(int i, double m, const Vector2d& x, const Vector2d& v): i(i), m(m), x(x), v(v) {}
    void draw();
};

class Force {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void addForces(VectorXd &f) = 0;
    virtual void addJacobians(MatrixXd &Jx, MatrixXd &Jv) = 0;
    virtual void draw() = 0;
};

class DragForce: public Force {
public:
    ParticleSystem *ps; // apply drag to all particles
    double kd;          // drag coefficient
    DragForce(ParticleSystem *ps, double kd): ps(ps), kd(kd) {}
    void addForces(VectorXd &f);
    void addJacobians(MatrixXd &Jx, MatrixXd &Jv);
    void draw();
};

class SpringForce: public Force {
    // connects two particles by a spring
public:
    Particle *p0, *p1; // particles
    double ks, kd;     // spring constant, damping coefficient
    double l0;         // rest length
    SpringForce(Particle *p0, Particle *p1, double ks, double kd, double l0):
        p0(p0), p1(p1), ks(ks), kd(kd), l0(l0) {}
    void addForces(VectorXd &f);
    void addJacobians(MatrixXd &Jx, MatrixXd &Jv);
    void draw();
};

class AnchorForce: public Force {
    // attaches a particle to a fixed point by a spring
public:

    Particle *p;   // particle
    Vector2d x;    // point to anchor it to
    double ks, kd; // spring constant, damping coefficient
    AnchorForce(Particle *p, const Vector2d& x, double ks, double kd):
        p(p), x(x), ks(ks), kd(kd) {}
    void addForces(VectorXd &f);
    void addJacobians(MatrixXd &Jx, MatrixXd &Jv);
    void draw();
};

#endif


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

/* This is an old version of FIRI for temporary usage here. */

#ifndef FIRI_HPP
#define FIRI_HPP

#include "lbfgs.hpp"
#include "sdlp.hpp"

#include <Eigen/Eigen>

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cfloat>
#include <cmath>
#include <vector>

namespace firi
{

    inline void chol3d(const Eigen::Matrix3d &A,
                       Eigen::Matrix3d &L)
    {
        L(0, 0) = sqrt(A(0, 0));
        L(0, 1) = 0.0;
        L(0, 2) = 0.0;
        L(1, 0) = 0.5 * (A(0, 1) + A(1, 0)) / L(0, 0);
        L(1, 1) = sqrt(A(1, 1) - L(1, 0) * L(1, 0));
        L(1, 2) = 0.0;
        L(2, 0) = 0.5 * (A(0, 2) + A(2, 0)) / L(0, 0);
        L(2, 1) = (0.5 * (A(1, 2) + A(2, 1)) - L(2, 0) * L(1, 0)) / L(1, 1);
        L(2, 2) = sqrt(A(2, 2) - L(2, 0) * L(2, 0) - L(2, 1) * L(2, 1));
        return;
    }

    inline bool smoothedL1(const double &mu,
                           const double &x,
                           double &f,
                           double &df)
    {
        if (x < 0.0)
        {
            return false;
        }
        else if (x > mu)
        {
            f = x - 0.5 * mu;
            df = 1.0;
            return true;
        }
        else
        {
            const double xdmu = x / mu;
            const double sqrxdmu = xdmu * xdmu;
            const double mumxd2 = mu - 0.5 * x;
            f = mumxd2 * sqrxdmu * xdmu;
            df = sqrxdmu * ((-0.5) * xdmu + 3.0 * mumxd2 / mu);
            return true;
        }
    }

    inline double costMVIE(void *data,
                           const Eigen::VectorXd &x,
                           Eigen::VectorXd &grad)
    {
        const int *pM = (int *)data;
        const double *pSmoothEps = (double *)(pM + 1);
        const double *pPenaltyWt = pSmoothEps + 1;
        const double *pA = pPenaltyWt + 1;

        const int M = *pM;
        const double smoothEps = *pSmoothEps;
        const double penaltyWt = *pPenaltyWt;
        Eigen::Map<const Eigen::MatrixX3d> A(pA, M, 3);
        Eigen::Map<const Eigen::Vector3d> p(x.data());
        Eigen::Map<const Eigen::Vector3d> rtd(x.data() + 3);
        Eigen::Map<const Eigen::Vector3d> cde(x.data() + 6);
        Eigen::Map<Eigen::Vector3d> gdp(grad.data());
        Eigen::Map<Eigen::Vector3d> gdrtd(grad.data() + 3);
        Eigen::Map<Eigen::Vector3d> gdcde(grad.data() + 6);

        double cost = 0;
        gdp.setZero();
        gdrtd.setZero();
        gdcde.setZero();

        Eigen::Matrix3d L;
        L(0, 0) = rtd(0) * rtd(0) + DBL_EPSILON;
        L(0, 1) = 0.0;
        L(0, 2) = 0.0;
        L(1, 0) = cde(0);
        L(1, 1) = rtd(1) * rtd(1) + DBL_EPSILON;
        L(1, 2) = 0.0;
        L(2, 0) = cde(2);
        L(2, 1) = cde(1);
        L(2, 2) = rtd(2) * rtd(2) + DBL_EPSILON;

        const Eigen::MatrixX3d AL = A * L;
        const Eigen::VectorXd normAL = AL.rowwise().norm();
        const Eigen::Matrix3Xd adjNormAL = (AL.array().colwise() / normAL.array()).transpose();
        const Eigen::VectorXd consViola = (normAL + A * p).array() - 1.0;

        double c, dc;
        Eigen::Vector3d vec;
        for (int i = 0; i < M; ++i)
        {
            if (smoothedL1(smoothEps, consViola(i), c, dc))
            {
                cost += c;
                vec = dc * A.row(i).transpose();
                gdp += vec;
                gdrtd += adjNormAL.col(i).cwiseProduct(vec);
                gdcde(0) += adjNormAL(0, i) * vec(1);
                gdcde(1) += adjNormAL(1, i) * vec(2);
                gdcde(2) += adjNormAL(0, i) * vec(2);
            }
        }
        cost *= penaltyWt;
        gdp *= penaltyWt;
        gdrtd *= penaltyWt;
        gdcde *= penaltyWt;

        cost -= log(L(0, 0)) + log(L(1, 1)) + log(L(2, 2));
        gdrtd(0) -= 1.0 / L(0, 0);
        gdrtd(1) -= 1.0 / L(1, 1);
        gdrtd(2) -= 1.0 / L(2, 2);

        gdrtd(0) *= 2.0 * rtd(0);
        gdrtd(1) *= 2.0 * rtd(1);
        gdrtd(2) *= 2.0 * rtd(2);

        return cost;
    }

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    // R, p, r are ALWAYS taken as the initial guess
    // R is also assumed to be a rotation matrix
    inline bool maxVolInsEllipsoid(const Eigen::MatrixX4d &hPoly,
                                   Eigen::Matrix3d &R,
                                   Eigen::Vector3d &p,
                                   Eigen::Vector3d &r)
    {
        // Find the deepest interior point
        const int M = hPoly.rows();
        Eigen::MatrixX4d Alp(M, 4);
        Eigen::VectorXd blp(M);
        Eigen::Vector4d clp, xlp;
        const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm();
        Alp.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm;
        Alp.rightCols<1>().setConstant(1.0);
        blp = -hPoly.rightCols<1>().array() / hNorm;
        clp.setZero();
        clp(3) = -1.0;
        const double maxdepth = -sdlp::linprog<4>(clp, Alp, blp, xlp);
        if (!(maxdepth > 0.0) || std::isinf(maxdepth))
        {
            return false;
        }
        const Eigen::Vector3d interior = xlp.head<3>();

        // Prepare the data for MVIE optimization
        uint8_t *optData = new uint8_t[sizeof(int) + (2 + 3 * M) * sizeof(double)];
        int *pM = (int *)optData;
        double *pSmoothEps = (double *)(pM + 1);
        double *pPenaltyWt = pSmoothEps + 1;
        double *pA = pPenaltyWt + 1;

        *pM = M;
        Eigen::Map<Eigen::MatrixX3d> A(pA, M, 3);
        A = Alp.leftCols<3>().array().colwise() /
            (blp - Alp.leftCols<3>() * interior).array();

        Eigen::VectorXd x(9);
        const Eigen::Matrix3d Q = R * (r.cwiseProduct(r)).asDiagonal() * R.transpose();
        Eigen::Matrix3d L;
        chol3d(Q, L);

        x.head<3>() = p - interior;
        x(3) = sqrt(L(0, 0));
        x(4) = sqrt(L(1, 1));
        x(5) = sqrt(L(2, 2));
        x(6) = L(1, 0);
        x(7) = L(2, 1);
        x(8) = L(2, 0);

        double minCost;
        lbfgs::lbfgs_parameter_t paramsMVIE;
        paramsMVIE.mem_size = 18;
        paramsMVIE.g_epsilon = 0.0;
        paramsMVIE.min_step = 1.0e-32;
        paramsMVIE.past = 3;
        paramsMVIE.delta = 1.0e-7;
        *pSmoothEps = 1.0e-2;
        *pPenaltyWt = 1.0e+3;

        int ret = lbfgs::lbfgs_optimize(x,
                                        minCost,
                                        &costMVIE,
                                        nullptr,
                                        nullptr,
                                        optData,
                                        paramsMVIE);

        if (ret < 0)
        {
            printf("FIRI WARNING: %s\n", lbfgs::lbfgs_strerror(ret));
        }

        p = x.head<3>() + interior;
        L(0, 0) = x(3) * x(3);
        L(0, 1) = 0.0;
        L(0, 2) = 0.0;
        L(1, 0) = x(6);
        L(1, 1) = x(4) * x(4);
        L(1, 2) = 0.0;
        L(2, 0) = x(8);
        L(2, 1) = x(7);
        L(2, 2) = x(5) * x(5);
        Eigen::JacobiSVD<Eigen::Matrix3d, Eigen::FullPivHouseholderQRPreconditioner> svd(L, Eigen::ComputeFullU);
        const Eigen::Matrix3d U = svd.matrixU();
        const Eigen::Vector3d S = svd.singularValues();
        if (U.determinant() < 0.0)
        {
            R.col(0) = U.col(1);
            R.col(1) = U.col(0);
            R.col(2) = U.col(2);
            r(0) = S(1);
            r(1) = S(0);
            r(2) = S(2);
        }
        else
        {
            R = U;
            r = S;
        }

        delete[] optData;

        return ret >= 0;
    }

    inline bool firi(const Eigen::MatrixX4d &bd,
                     const Eigen::Matrix3Xd &pc,
                     const Eigen::Vector3d &a,
                     const Eigen::Vector3d &b,
                     Eigen::MatrixX4d &hPoly,
                     const int iterations = 4,
                     const double epsilon = 1.0e-6)
    {
        const Eigen::Vector4d ah(a(0), a(1), a(2), 1.0);
        const Eigen::Vector4d bh(b(0), b(1), b(2), 1.0);

        if ((bd * ah).maxCoeff() > 0.0 ||
            (bd * bh).maxCoeff() > 0.0)
        {
            return false;
        }

        const int M = bd.rows();
        const int N = pc.cols();

        Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
        Eigen::Vector3d p = 0.5 * (a + b);
        Eigen::Vector3d r = Eigen::Vector3d::Ones();
        Eigen::MatrixX4d forwardH(M + N, 4);
        int nH = 0;

        for (int loop = 0; loop < iterations; ++loop)
        {
            const Eigen::Matrix3d forward = r.cwiseInverse().asDiagonal() * R.transpose();
            const Eigen::Matrix3d backward = R * r.asDiagonal();
            const Eigen::MatrixX3d forwardB = bd.leftCols<3>() * backward;
            const Eigen::VectorXd forwardD = bd.rightCols<1>() + bd.leftCols<3>() * p;
            const Eigen::Matrix3Xd forwardPC = forward * (pc.colwise() - p);
            const Eigen::Vector3d fwd_a = forward * (a - p);
            const Eigen::Vector3d fwd_b = forward * (b - p);

            const Eigen::VectorXd distDs = forwardD.cwiseAbs().cwiseQuotient(forwardB.rowwise().norm());
            Eigen::MatrixX4d tangents(N, 4);
            Eigen::VectorXd distRs(N);

            for (int i = 0; i < N; i++)
            {
                distRs(i) = forwardPC.col(i).norm();
                tangents(i, 3) = -distRs(i);
                tangents.block<1, 3>(i, 0) = forwardPC.col(i).transpose() / distRs(i);
                if (tangents.block<1, 3>(i, 0).dot(fwd_a) + tangents(i, 3) > epsilon)
                {
                    const Eigen::Vector3d delta = forwardPC.col(i) - fwd_a;
                    tangents.block<1, 3>(i, 0) = fwd_a - (delta.dot(fwd_a) / delta.squaredNorm()) * delta;
                    distRs(i) = tangents.block<1, 3>(i, 0).norm();
                    tangents(i, 3) = -distRs(i);
                    tangents.block<1, 3>(i, 0) /= distRs(i);
                }
                if (tangents.block<1, 3>(i, 0).dot(fwd_b) + tangents(i, 3) > epsilon)
                {
                    const Eigen::Vector3d delta = forwardPC.col(i) - fwd_b;
                    tangents.block<1, 3>(i, 0) = fwd_b - (delta.dot(fwd_b) / delta.squaredNorm()) * delta;
                    distRs(i) = tangents.block<1, 3>(i, 0).norm();
                    tangents(i, 3) = -distRs(i);
                    tangents.block<1, 3>(i, 0) /= distRs(i);
                }
                if (tangents.block<1, 3>(i, 0).dot(fwd_a) + tangents(i, 3) > epsilon)
                {
                    tangents.block<1, 3>(i, 0) = (fwd_a - forwardPC.col(i)).cross(fwd_b - forwardPC.col(i)).normalized();
                    tangents(i, 3) = -tangents.block<1, 3>(i, 0).dot(fwd_a);
                    tangents.row(i) *= tangents(i, 3) > 0.0 ? -1.0 : 1.0;
                }
            }

            Eigen::Matrix<uint8_t, -1, 1> bdFlags = Eigen::Matrix<uint8_t, -1, 1>::Constant(M, 1);
            Eigen::Matrix<uint8_t, -1, 1> pcFlags = Eigen::Matrix<uint8_t, -1, 1>::Constant(N, 1);

            nH = 0;

            bool completed = false;
            int bdMinId, pcMinId;
            double minSqrD = distDs.minCoeff(&bdMinId);
            double minSqrR = distRs.minCoeff(&pcMinId);
            for (int i = 0; !completed && i < (M + N); ++i)
            {
                bdMinId = bdMinId;
                pcMinId = pcMinId;
                if (minSqrD < minSqrR)
                {
                    forwardH.block<1, 3>(nH, 0) = forwardB.row(bdMinId);
                    forwardH(nH, 3) = forwardD(bdMinId);
                    bdFlags(bdMinId) = 0;
                }
                else
                {
                    forwardH.row(nH) = tangents.row(pcMinId);
                    pcFlags(pcMinId) = 0;
                }

                completed = true;
                minSqrD = INFINITY;
                for (int j = 0; j < M; ++j)
                {
                    if (bdFlags(j))
                    {
                        completed = false;
                        if (minSqrD > distDs(j))
                        {
                            bdMinId = j;
                            minSqrD = distDs(j);
                        }
                    }
                }
                minSqrR = INFINITY;
                for (int j = 0; j < N; ++j)
                {
                    if (pcFlags(j))
                    {
                        if (forwardH.block<1, 3>(nH, 0).dot(forwardPC.col(j)) + forwardH(nH, 3) > -epsilon)
                        {
                            pcFlags(j) = 0;
                        }
                        else
                        {
                            completed = false;
                            if (minSqrR > distRs(j))
                            {
                                pcMinId = j;
                                minSqrR = distRs(j);
                            }
                        }
                    }
                }
                ++nH;
            }

            hPoly.resize(nH, 4);
            for (int i = 0; i < nH; ++i)
            {
                hPoly.block<1, 3>(i, 0) = forwardH.block<1, 3>(i, 0) * forward;
                hPoly(i, 3) = forwardH(i, 3) - hPoly.block<1, 3>(i, 0).dot(p);
            }

            if (loop == iterations - 1)
            {
                break;
            }

            maxVolInsEllipsoid(hPoly, R, p, r);
        }

        return true;
    }

}

#endif


/*=============================================================================
Copyright 2019 Sarthak Singhal <singhalsarthak2007@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#define BOOST_TEST_MODULE cartesian_differential_test

#include <boost/test/unit_test.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/astronomy/coordinate/arithmetic.hpp>
#include <boost/astronomy/coordinate/diff/differential.hpp>

using namespace std;
using namespace boost::astronomy::coordinate;
using namespace boost::units::si;
using namespace boost::geometry;
using namespace boost::units;
namespace bud = boost::units::degree;

BOOST_AUTO_TEST_SUITE(cartesian_differential_constructor)

BOOST_AUTO_TEST_CASE(cartesian_differential_default_constructor)
{
    //using set functions
    cartesian_differential<double, quantity<si::velocity>, quantity<si::velocity>,
    quantity<si::velocity>> motion1;
    motion1.set_dx_dy_dz(2.5*meters/seconds, 91.0*meters/seconds, 12.0*meters/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), 2.5, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), 91.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), 12, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_quantities_constructor)
{
    //checking construction from value
    auto motion1 = make_cartesian_differential
    (1.5*meters/seconds, 9.0*si::kilo*meters/seconds, 3.0*meters/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dy()),
        quantity<bu::divide_typeof_helper<decltype(si::kilo*meters), si::time>::type>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dz()), quantity<si::velocity>>::value));

    cartesian_differential<double, quantity<si::velocity>, quantity<si::velocity>, quantity<si::velocity>>
        motion2(1.5*meters/seconds, 9.0*meters/seconds, 3.0*meters/seconds);
    BOOST_CHECK_CLOSE(motion2.get_dx().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dy().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dz().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_copy_constructor)
{
    //checking construction from value
    auto motion1 = make_cartesian_differential
    (1.5*meters/seconds, 9.0*si::kilo*meters/seconds, 3.0*meters/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dy()),
        quantity<bu::divide_typeof_helper<decltype(si::kilo*meters), si::time>::type>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dz()), quantity<si::velocity>>::value));

    //copy constructor
    auto motion2 = make_cartesian_differential(motion1);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), motion2.get_dx().value(), 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), motion2.get_dy().value(), 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), motion2.get_dz().value(), 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dy()),
        quantity<bu::divide_typeof_helper<decltype(si::kilo*meters), si::time>::type>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dz()), quantity<si::velocity>>::value));

    cartesian_differential<double, quantity<bu::divide_typeof_helper<si::length, si::time>::
        type>, quantity<bu::divide_typeof_helper<decltype(si::kilo*meters), si::time>::type>, quantity
        <si::velocity>> motion3(motion1);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), motion3.get_dx().value(), 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), motion3.get_dy().value(), 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), motion3.get_dz().value(), 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion3.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion3.get_dy()),
        quantity<bu::divide_typeof_helper<decltype(si::kilo*meters), si::time>::type>>::value));
    BOOST_TEST((std::is_same<decltype(motion3.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_copy_constructor_with_different_units)
{
    //checking construction from value
    auto motion1 = make_cartesian_differential
    (1.5*meters/seconds, 9.0*si::kilo*meters/seconds, 3.0*meters/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dy()),
        quantity<bu::divide_typeof_helper<decltype(si::kilo*meters), si::time>::type>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dz()), quantity<si::velocity>>::value));

    // //Conversion from one unit type to other
    auto motion2 = make_cartesian_differential
    <double, quantity<si::velocity>, quantity<si::velocity>, quantity<si::velocity>>(motion1);
    BOOST_CHECK_CLOSE(motion2.get_dx().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dy().value(), 9000.0, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dz().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_geometry_point_constructor)
{
    //constructing from boost::geometry::model::motion
    model::point<double, 3, cs::spherical<boost::geometry::degree>> model_point(30, 60, 1);
    auto motion1 = make_cartesian_differential
    <double,quantity<si::velocity>,quantity<si::velocity>,quantity<si::velocity>>(model_point);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), 0.75, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), 0.4330127019, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), 0.5, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dz()), quantity<si::velocity>>::value));

    cartesian_differential<double, quantity<si::velocity>, quantity<si::velocity>,
    quantity<si::velocity>> motion2(model_point);
    BOOST_CHECK_CLOSE(motion2.get_dx().value(), 0.75, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dy().value(), 0.4330127019, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dz().value(), 0.5, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_conversion_from_spherical_differential)
{
    //constructing from spherical differential
    auto spherical_motion = make_spherical_differential
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meters/seconds);
    auto motion1 = make_cartesian_differential(spherical_motion);
    BOOST_CHECK_CLOSE(motion1.get_dx().value(), 0.75, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dy().value(), 0.4330127019, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dz().value(), 0.5, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_conversion_from_spherical_equatorial_differential)
{
    //constructing from spherical_equitorial differential
    auto spherical_equatorial_motion = make_spherical_equatorial_differential
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meters/seconds);
    auto motion2 = make_cartesian_differential(spherical_equatorial_motion);
    BOOST_CHECK_CLOSE(motion2.get_dx().value(), 0.433012646, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dy().value(), 0.250000097, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dz().value(), 0.866025405, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_conversion_from_spherical_coslat_differential)
{
    //constructing from spherical_coslat differential
    auto spherical_coslat_motion = make_spherical_coslat_differential
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meters/seconds);
    auto motion3 = make_cartesian_differential(spherical_coslat_motion);
    BOOST_CHECK_CLOSE(motion3.get_dx().value(), 0.8100222, 0.001);
    BOOST_CHECK_CLOSE(motion3.get_dy().value(), 0.467666778, 0.001);
    BOOST_CHECK_CLOSE(motion3.get_dz().value(), 0.353768031, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion3.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion3.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(motion3.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(cartesian_differential_operators)

BOOST_AUTO_TEST_CASE(cartesian_differential_addition_operator)
{
    auto motion1 = make_cartesian_differential
        (11.0*meters/seconds, 15.0*meters/seconds, 19.0*meters/seconds);
    auto motion2 = make_cartesian_differential
        (6.0*si::milli*meters/seconds, 10.0*si::centi*meters/seconds, 11.0*meters/seconds);

    auto sum = make_cartesian_differential(motion1 + motion2);

    BOOST_CHECK_CLOSE(sum.get_dx().value(), 11.006, 0.001);
    BOOST_CHECK_CLOSE(sum.get_dy().value(), 15.1, 0.001);
    BOOST_CHECK_CLOSE(sum.get_dz().value(), 30, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(sum.get_dx()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_dy()), quantity<si::velocity>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_dz()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(cartesian_differential_multiplication_operator)
{
    auto motion1 = make_cartesian_differential
        (3.0*meters/seconds, 9.0*meters/seconds, 6.0*meters/seconds);

    auto product = make_cartesian_differential
        (motion1 * quantity<si::time>(5*seconds));

    BOOST_CHECK_CLOSE(product.get_dx().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(product.get_dy().value(), 45.0, 0.001);
    BOOST_CHECK_CLOSE(product.get_dz().value(), 30.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(product.get_dx()), quantity<si::length>>::value));
    BOOST_TEST((std::is_same<decltype(product.get_dy()), quantity<si::length>>::value));
    BOOST_TEST((std::is_same<decltype(product.get_dz()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/container/flat_map.hpp>
#include <boost/container/static_vector.hpp>
#include <map>
#include <forward_list>
#include <chrono>
#include <deque>
#include <iomanip>
#include <iostream>
#include <list>
#include <string>
#include <vector>
#include <numeric>
#include <random>

const int SIZE = 100'000'000;

// function by Rainer Grimm
template <typename T>
void accumContainer(const T& t, const std::string& cont){

  std::cout << std::fixed << std::setprecision(10);

  auto begin= std::chrono::steady_clock::now();
  std::size_t res = std::accumulate(t.begin(), t.end(), 0LL);
  std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
  std::cout << cont << " with std::accumulate" <<  std::endl;
  std::cout << "accumulate time: " << last.count() << std::endl;
  std::cout << "res: " << res << std::endl;
  std::cout << std::endl;

  std::cout << std::endl;

}

template <typename T>
void sumContainer(const T& t, const std::string& cont){

   std::cout << std::fixed << std::setprecision(10);

   auto begin= std::chrono::steady_clock::now();
   size_t res{0};
   for (int n : t){
      res += n;
   }
   std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
   std::cout << cont << " foreach loop" <<  std::endl;
   std::cout << "sum time: " << last.count() << std::endl;
   std::cout << "res: " << res << std::endl;
   std::cout << std::endl;

   std::cout << std::endl;

}

template <typename T>
void sumContainerData(const T& t, const std::string& cont){

   std::cout << std::fixed << std::setprecision(10);

   auto begin= std::chrono::steady_clock::now();
   size_t res{0};
   for (int i=0; i < SIZE; ++i){
      res += t[i];
   }
   std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
   std::cout << cont << " loop over container indices" <<  std::endl;
   std::cout << "sum time: " << last.count() << std::endl;
   std::cout << "res: " << res << std::endl;
   std::cout << std::endl;

   std::cout << std::endl;

}

template <typename T>
void sumContainerDataPtr(const T& t, const std::string& cont){

   std::cout << std::fixed << std::setprecision(10);

   auto begin= std::chrono::steady_clock::now();
   const int* ptr = t.data();
   size_t res{0};
   for (int i=0; i < SIZE; ++i){
      res += ptr[i];
   }
   std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
   std::cout << cont << " loop over indices into .data() pointer" <<  std::endl;
   std::cout << "sum time: " << last.count() << std::endl;
   std::cout << "res: " << res << std::endl;
   std::cout << std::endl;

   std::cout << std::endl;

}

void sumCArray(int* t, const std::string& cont){

   std::cout << std::fixed << std::setprecision(10);

   auto begin= std::chrono::steady_clock::now();
   size_t res{0};
   for (int i=0; i < SIZE; ++i){
      res += t[i];
   }
   std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
   std::cout << cont <<  std::endl;
   std::cout << "sum time: " << last.count() << std::endl;
   std::cout << "res: " << res << std::endl;
   std::cout << std::endl;

   std::cout << std::endl;

}

int main(){

    std::cout << std::endl;

    std::random_device seed;
    std::mt19937 engine(seed());
    std::uniform_int_distribution<int> dist(0, 100);
    std::vector<int> randNumbers;
    randNumbers.reserve(SIZE);
    for (int i=0; i < SIZE; ++i){
        randNumbers.push_back(dist(engine));
    }

    auto begin= std::chrono::steady_clock::now();
    std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
    {
      begin = std::chrono::steady_clock::now();
      std::vector<int> myVec(randNumbers.begin(), randNumbers.end());
      std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
      std::cout << "************ Time to create and fill std::vector<int>: " << last.count() << std::endl;
      accumContainer(myVec,"std::vector<int>");
      sumContainer(myVec,"std::vector<int>");
      sumContainerData(myVec,"std::vector<int>");
      sumContainerDataPtr(myVec,"std::vector<int>");
      sumCArray(myVec.data(),"std::vector<int>.data()");
    }

    {
       begin = std::chrono::steady_clock::now();
       boost::container::static_vector<int, SIZE> myVec(randNumbers.begin(), randNumbers.end());
       std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
       std::cout << "************ Time to create and fill boost::container::static_vector<int,SIZE>: " << last.count() << std::endl;
       accumContainer(myVec,"boost::container::static_vector<int>");
    }

    {
       begin = std::chrono::steady_clock::now();
       std::array<int, SIZE> stdArray;
       int counter = 0;
       int next_counter = 10;
       for (int i=0; i < SIZE; ++i){
          stdArray[i] = randNumbers[i];
       }
       std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
       std::cout << "************ Time to create and fill std::array<int>: " << last.count() << std::endl;
       accumContainer(stdArray,"std::array<int>");  // (6)
    }

    {
       begin = std::chrono::steady_clock::now();
       int stackArray[SIZE];
       int counter = 0;
       int next_counter = 10;
       for (int i=0; i < SIZE; ++i){
          stackArray[i] = randNumbers[i];
       }
       std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
       std::cout << "************ Time to create and fill stack C array: " << last.count() << std::endl;
       sumCArray(stackArray,"stack C array<int>");  // (7)
    }

    {
       begin = std::chrono::steady_clock::now();
       int *heapArray = (int*)malloc(sizeof(int)*SIZE);
       int counter = 0;
       int next_counter = 10;
       for (int i=0; i < SIZE; ++i){
          heapArray[i] = randNumbers[i];
       }
       std::chrono::duration<double> last=  std::chrono::steady_clock::now() - begin;
       std::cout << "************ Time to create and fill heap C array: " << last.count() << std::endl;
       sumCArray(heapArray,"heap C array<int>");  // (8)
    }

}


/*
	This file is part of cpp-ethereum.

	cpp-ethereum 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, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum 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 cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file QBigInt.cpp
 * @author Yann yann@ethdev.com
 * @date 2015
 */

#include <boost/variant/multivisitors.hpp>
#include <boost/variant.hpp>
#include <libethcore/CommonJS.h>
#include "QBigInt.h"

using namespace dev;
using namespace dev::mix;
using namespace std;

void QBigInt::manageException() const
{
	try
	{
		throw;
	}
	catch (boost::exception const& _e)
	{
		cerr << boost::diagnostic_information(_e);
	}
	catch (exception const& _e)
	{
		cerr << _e.what();
	}
	catch (...)
	{
		cerr << boost::current_exception_diagnostic_information();
	}
}

QString QBigInt::value() const
{
	try
	{
		ostringstream s;
		s << m_internalValue;
		return QString::fromStdString(s.str());
	}
	catch (...)
	{
		manageException();
		return QString();
	}
}

QBigInt* QBigInt::subtract(QBigInt* const& _value) const
{
	try
	{
		if (!_value)
			return nullptr;
		BigIntVariant toSubtract = _value->internalValue();
		return new QBigInt(boost::apply_visitor(mix::subtract(), m_internalValue, toSubtract));
	}
	catch (...)
	{
		manageException();
		return nullptr;
	}
}

QBigInt* QBigInt::add(QBigInt* const& _value) const
{
	try
	{
		if (!_value)
			return nullptr;
		BigIntVariant toAdd = _value->internalValue();
		return new QBigInt(boost::apply_visitor(mix::add(), m_internalValue, toAdd));
	}
	catch (...)
	{
		manageException();
		return nullptr;
	}
}

QBigInt* QBigInt::multiply(QBigInt* const& _value) const
{
	try
	{
		if (!_value)
			return nullptr;
		BigIntVariant toMultiply = _value->internalValue();
		return new QBigInt(boost::apply_visitor(mix::multiply(), m_internalValue, toMultiply));
	}
	catch (...)
	{
		manageException();
		return nullptr;
	}
}

QBigInt* QBigInt::divide(QBigInt* const& _value) const
{
	try
	{
		if (!_value)
			return nullptr;
		BigIntVariant toDivide = _value->internalValue();
		return new QBigInt(boost::apply_visitor(mix::divide(), m_internalValue, toDivide));
	}
	catch (...)
	{
		manageException();
		return nullptr;
	}
}

QVariantMap QBigInt::checkAgainst(QString const& _type) const
{
	QVariantMap ret;
	try
	{
		QString type = _type;
		QString capacity = type.replace("uint", "").replace("int", "");
		if (capacity.isEmpty())
			capacity = "256";
		bigint range = 1;
		for (int k = 0; k < capacity.toInt() / 8; ++k)
			range = range * 256;
		bigint value = boost::get<bigint>(this->internalValue());
		ret.insert("valid", true);
		if (_type.startsWith("uint") && value > range - 1)
		{
			ret.insert("minValue", "0");
			ostringstream s;
			s << range - 1;
			ret.insert("maxValue", QString::fromStdString(s.str()));
			if (value > range)
				ret["valid"] = false;
		}
		else if (_type.startsWith("int"))
		{
			range = range / 2;
			ostringstream s;
			s << -range;
			ret.insert("minValue", QString::fromStdString(s.str()));
			s.str("");
			s.clear();
			s << range - 1;
			ret.insert("maxValue", QString::fromStdString(s.str()));
			if (-range > value || value > range - 1)
				ret["valid"] = false;
		}
	}
	catch (...)
	{
		manageException();
	}
	return ret;
}


/*
    Copyright (c) 2015-2017 Xavier Leclercq

    Permission is hereby granted, free of charge, to any person obtaining a
    copy of this software and associated documentation files (the "Software"),
    to deal in the Software without restriction, including without limitation
    the rights to use, copy, modify, merge, publish, distribute, sublicense,
    and/or sell copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
    IN THE SOFTWARE.
*/

#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <iostream>
#include <vector>
#include <ctime>

int main(int argc, char* argv[])
{
    // Create a random number generator using the platform default
    boost::random_device rng;

    std::cout << "Random number generator:" << std::endl;
    std::cout << "\tMin generated number: " << rng.min() << std::endl;
    std::cout << "\tMax generated number: " << rng.max() << std::endl;
    std::cout << std::endl;

    // Create a Mersenne Twister pseudo-random number generator
    boost::random::mt19937 prng;

    std::cout << "Pseudo-random number generator:" << std::endl;
    std::cout << "\tMin generated number: " << prng.min() << std::endl;
    std::cout << "\tMax generated number: " << prng.max() << std::endl;
    std::cout << std::endl;

    // Generate a random number to be used as the seed
    unsigned int seed = rng();
    std::cout << "The seed is: " << seed << std::endl;
    std::cout << std::endl;

    prng.seed(seed);

    // Generate a few pseudo-random numbers one by one
    std::cout << "Pseudo-random number 1: " << prng() << std::endl;
    std::cout << "Pseudo-random number 2: " << prng() << std::endl;
    std::cout << "Pseudo-random number 3: " << prng() << std::endl;
    std::cout << std::endl;

    // Fill a vector with pseudo-random numbers
    std::vector<unsigned int> v;
    v.resize(5);
    prng.generate(v.begin(), v.end());

    std::cout << "Pseudo-random numbers vector: ";
    for (size_t i = 0; i < v.size(); ++i)
    {
        std::cout << v[i] << " ";
    }
    std::cout << std::endl;

    return 0;
}


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

#include <Eigen/Dense>
#include <vector>
#include <chrono>  // NOLINT [build/c++11]
#include <iostream>
#include "get_fitzgibbon_cvpr_2001.hpp"
#include "get_kukelova_cvpr_2015.hpp"
#include "get_valtonenornhag_arxiv_2020a.hpp"
#include "get_valtonenornhag_arxiv_2020b.hpp"
#include "problem_instance.hpp"
#include "generate_problem_instance.hpp"
#include "posedata.hpp"

int main() {
    /* Timing experiments */
    int nbr_iter = 1e2;
    Eigen::MatrixXcd out;
    int N = 5;
    HomLib::ProblemInstance inst;

    /* TEST SOLVERS HERE */
    std::cout << "--- COMPLETE SOLVERS, INCLUDING PRE- and POST-PROCESSING ---" << std::endl;
    HomLib::PoseData posedata;
    N = 2;

    // fHf complete
    std::vector<HomLib::PoseData> posedata_fHf;
    auto start = std::chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        inst = HomLib::generate_problem_instance(N);
        posedata_fHf = HomLib::ValtonenOrnhagArxiv2020B::get_fHf(inst.x1, inst.x2, inst.R1g, inst.R2g);
    }
    auto end = std::chrono::steady_clock::now();
    std::cout << "(fHf complete) Elapsed time in microseconds : "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << std::endl;

    // frHfr elim complete
    N = 3;
    start = std::chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        inst = HomLib::generate_problem_instance(N);
        posedata = HomLib::ValtonenOrnhagArxiv2020B::get_frHfr(inst.x1, inst.x2, inst.R1g, inst.R2g);
    }
    end = std::chrono::steady_clock::now();
    std::cout << "(frHfr elim complete) Elapsed time in microseconds : "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << std::endl;

    // floor_fHf complete
    start = std::chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        inst = HomLib::generate_problem_instance(N);
        posedata = HomLib::ValtonenOrnhagArxiv2020A::get_fHf(inst.x1, inst.x2, inst.R1g, inst.R2g);
    }
    end = std::chrono::steady_clock::now();
    std::cout << "(floor fHf complete) Elapsed time in microseconds : "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << std::endl;

    // Kukelova Hlam1lam2 complete
    std::vector<HomLib::PoseData> posedata2;
    start = std::chrono::steady_clock::now();
    N = 5;
    for (int i=0; i < nbr_iter; i++) {
        inst = HomLib::generate_problem_instance(N);
        posedata2 = HomLib::KukelovaCVPR2015::get(inst.x1, inst.x2);
    }
    end = std::chrono::steady_clock::now();
    std::cout << "(Kukelova Hlam1lam2 complete) Elapsed time in microseconds : "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << std::endl;

    // Fitzgibbon
    start = std::chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        inst = HomLib::generate_problem_instance(N);
        posedata = HomLib::FitzgibbonCVPR2001::get(inst.x1, inst.x2);
    }
    end = std::chrono::steady_clock::now();
    std::cout << "(Fitzgibbon complete) Elapsed time in microseconds : "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << std::endl;

    return 0;
}


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

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/matrix/element_matrix.hpp>
#include <boost/numeric/mtl/matrix/element_array.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    cout << "\n" << name << "\n";

    using mtl::mat::inserter; using mtl::element_array;
    typedef typename mtl::Collection<Matrix>::value_type value_type;

    mtl::dense2D<double>       m1(2, 2);
    m1[0][0]= 1.0; m1[0][1]= 2.0;
    m1[1][0]= 3.0; m1[1][1]= 4.0;
    std::vector<int>           row1, col1;
    row1.push_back(1); row1.push_back(2);
    col1.push_back(0); col1.push_back(2);

    double a2[2][2]= {{11., 12.},{13., 14.}};
    std::vector<int>           ind2;
    ind2.push_back(2); ind2.push_back(4);

    std::vector<int>           ind3;
    ind3.push_back(3); ind3.push_back(1);

    set_to_zero(matrix); // dense matrices are not automatically set to zero

    {
	inserter<Matrix, mtl::operations::update_plus<value_type> > ins(matrix);

	ins << element_matrix(m1, row1, col1)
	    << element_array(a2, ind2);
	ins << element_array(a2, ind3);
    }

    cout << "Filled matrix:\n" << matrix << "\n";
    MTL_THROW_IF(matrix[0][0] != 0.0, mtl::runtime_error("wrong zero-element"));
    MTL_THROW_IF(matrix[1][0] != 1.0, mtl::runtime_error("wrong insertion (single value)"));
    MTL_THROW_IF(matrix[2][2] != 15.0, mtl::runtime_error("wrong summation"));
    MTL_THROW_IF(matrix[1][1] != 14.0, mtl::runtime_error("wrong insertion (single value)"));

}

int main(int , char**)
{
    using namespace mtl;
    unsigned size= 5;

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    return 0;
}