#include <gtest/gtest.h>
#include <stan/math/rev/core.hpp>
#include <stan/math/rev.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_system.hpp>
#include <stan/math/torsten/dsolve/pmx_odeint_integrator.hpp>
#include <stan/math/torsten/dsolve/pmx_integrate_ode_rk45.hpp>
#include <stan/math/torsten/dsolve/pmx_integrate_ode_bdf.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_rk45.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_bdf.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_adams.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_ckrk.hpp>
#include <stan/math/torsten/test/unit/pmx_ode_test_fixture.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>
#include <boost/fusion/adapted/std_tuple.hpp>
#include <boost/fusion/include/algorithm.hpp>

TEST_F(TorstenOdeTest_sho, variadic_ode_system_odeint) {
  using torsten::dsolve::PMXVariadicOdeSystem;
  using stan::math::value_of;
  using stan::math::to_var;

  y0[1] = 0.5;
  y0_vec[1] = 0.5;
  Eigen::Matrix<stan::math::var, -1, 1> y0_var(to_var(y0_vec));
  std::vector<stan::math::var> theta_var(to_var(theta));

  PMXVariadicOdeSystem<harm_osc_ode_fun_eigen, double, stan::math::var,
                       std::vector<stan::math::var>, std::vector<double>, std::vector<int>>
    ode(f_eigen, 0.0, ts, y0_var, nullptr, theta_var, x_r, x_i);

  Eigen::VectorXd dydt = f_eigen(ts[0], y0_vec, nullptr, theta, x_r, x_i);

  Eigen::VectorXd ydot = ode.dbl_rhs_impl(ts[0], y0_vec);
  EXPECT_FLOAT_EQ(ydot[0], dydt[0]);
  EXPECT_FLOAT_EQ(ydot[1], dydt[1]);

  torsten::dsolve::PMXOdeSystem<harm_osc_ode_fun, double, stan::math::var, stan::math::var>
    ode0(f, 0.0, ts, to_var(y0), theta_var, x_r, x_i, nullptr);
  std::vector<double> dydt_vec(ode.system_size), dydt_vec2(ode.system_size);
  ode0(ode0.y0_fwd_system, dydt_vec, ts[0]);

  ode(ode.y0_fwd_system, dydt_vec2, ts[0]);
  EXPECT_EQ(ode.system_size, ode0.system_size);
  for (size_t i = 0; i < ode0.system_size; ++i) {
    EXPECT_FLOAT_EQ(dydt_vec2[i], dydt_vec[i]);
  }
}

TEST_F(TorstenOdeTest_sho, variadic_ode_system_cvodes) {
  using torsten::dsolve::PMXVariadicOdeSystem;

  using stan::math::value_of;
  using stan::math::to_var;

  y0[1] = 0.5;
  y0_vec[1] = 0.5;

  Eigen::Matrix<stan::math::var, -1, 1> y0_var(to_var(y0_vec));
  std::vector<stan::math::var> theta_var(to_var(theta));

  PMXVariadicOdeSystem<harm_osc_ode_fun_eigen, double, stan::math::var,
                       std::vector<stan::math::var>, std::vector<double>, std::vector<int>>
    ode(f_eigen, 0.0, ts, y0_var, nullptr, theta_var, x_r, x_i);

  Eigen::VectorXd dydt = f_eigen(ts[0], y0_vec, nullptr, theta, x_r, x_i);
  N_Vector nv_y(N_VNew_Serial(2));
  N_Vector ydot(N_VNew_Serial(2));
  NV_Ith_S(nv_y, 0) = y0[0];
  NV_Ith_S(nv_y, 1) = y0[1];
  ode(ts[0], nv_y, ydot);
  EXPECT_FLOAT_EQ(NV_Ith_S(ydot, 0), dydt[0]);
  EXPECT_FLOAT_EQ(NV_Ith_S(ydot, 1), dydt[1]);
  N_VDestroy(nv_y);
  N_VDestroy(ydot);
}

TEST_F(TorstenOdeTest_sho, eigen_vector_rk45) {
  using torsten::dsolve::PMXVariadicOdeSystem;

  using stan::math::value_of;
  using torsten::dsolve::PMXOdeintIntegrator;

  {                             // data only
    auto y = torsten::pmx_ode_rk45_ctrl(f_eigen, y0_vec, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = torsten::pmx_integrate_ode_rk45(f, y0, t0, ts,
                                                 theta, x_r, x_i, rtol, atol, max_num_steps, msgs);
    for (size_t j = 0; j < ts.size(); ++j) {
      for (size_t i = 0; i < y0.size(); ++i) {
        EXPECT_FLOAT_EQ(y[j][i], y_sol[j][i]);
      }
    }
  }

  {                             // theat var
    std::vector<stan::math::var> theta_var(stan::math::to_var(theta));
    auto y = torsten::pmx_ode_rk45_ctrl(f_eigen, y0_vec, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta_var, x_r, x_i);
    auto y_sol = stan::math::ode_rk45_tol(f_eigen, y0_vec, t0, ts, rtol, atol, max_num_steps, msgs,
                                          theta_var, x_r, x_i);
    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(theta_var, y_sol[j], y[j], 1e-8, 1e-8);
    }
  }

  {                             // theat & y0 var
    std::vector<stan::math::var> theta_var(stan::math::to_var(theta));
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));

    auto y = torsten::pmx_ode_rk45_ctrl(f_eigen, y0_vec_var, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta_var, x_r, x_i);
    auto y_sol = stan::math::ode_rk45_tol(f_eigen, y0_vec_var, t0, ts, rtol, atol, max_num_steps, msgs,
                                          theta_var, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(theta_var, y_sol[j], y[j], 1e-8, 1e-8);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-8, 1e-8);
    }
  }

  {                             // y0 & ts var
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));
    std::vector<stan::math::var> ts_var(stan::math::to_var(ts));

    auto y = torsten::pmx_ode_rk45_ctrl(f_eigen, y0_vec_var, t0, ts_var, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = stan::math::ode_rk45_tol(f_eigen, y0_vec_var, t0, ts_var, rtol, atol, max_num_steps, msgs,
                                          theta, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(ts_var, y_sol[j], y[j], 1e-8, 1e-8);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-8, 1e-8);
    }
  }
}

TEST_F(TorstenOdeTest_chem, eigen_vector_bdf) {
  using torsten::dsolve::PMXVariadicOdeSystem;

  using stan::math::value_of;
  using torsten::dsolve::PMXOdeintIntegrator;

  {                             // data only
    auto y = torsten::pmx_ode_bdf_ctrl(f_eigen, y0_vec, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = torsten::pmx_integrate_ode_bdf(f, y0, t0, ts,
                                                 theta, x_r, x_i, rtol, atol, max_num_steps, msgs);
    for (size_t j = 0; j < ts.size(); ++j) {
      for (size_t i = 0; i < y0.size(); ++i) {
        EXPECT_FLOAT_EQ(y[j][i], y_sol[j][i]);
      }
    }
  }

  {                             // theat var
    std::vector<stan::math::var> theta_var(stan::math::to_var(theta));
    auto y = torsten::pmx_ode_bdf_ctrl(f_eigen, y0_vec, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta_var, x_r, x_i);
    auto y_sol = stan::math::ode_bdf_tol(f_eigen, y0_vec, t0, ts, rtol, atol, max_num_steps, msgs,
                                          theta_var, x_r, x_i);
    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(theta_var, y_sol[j], y[j], 1e-8, 1e-6);
    }
  }

  {                             // theat & y0 var
    std::vector<stan::math::var> theta_var(stan::math::to_var(theta));
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));

    auto y = torsten::pmx_ode_bdf_ctrl(f_eigen, y0_vec_var, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta_var, x_r, x_i);
    auto y_sol = stan::math::ode_bdf_tol(f_eigen, y0_vec_var, t0, ts, rtol, atol, max_num_steps, msgs,
                                          theta_var, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(theta_var, y_sol[j], y[j], 1e-8, 1e-6);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-8, 1e-6);
    }
  }

  {                             // y0 & ts var
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));
    std::vector<stan::math::var> ts_var(stan::math::to_var(ts));

    auto y = torsten::pmx_ode_bdf_ctrl(f_eigen, y0_vec_var, t0, ts_var, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = stan::math::ode_bdf_tol(f_eigen, y0_vec_var, t0, ts_var, rtol, atol, max_num_steps, msgs,
                                          theta, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(ts_var, y_sol[j], y[j], 1e-8, 1e-6);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-8, 1e-6);
    }
  }
}

TEST_F(TorstenOdeTest_lorenz, eigen_vector_adams) {
  using torsten::dsolve::PMXVariadicOdeSystem;

  using stan::math::value_of;
  using torsten::dsolve::PMXOdeintIntegrator;

  {                             // theat & y0 var
    std::vector<stan::math::var> theta_var(stan::math::to_var(theta));
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));

    auto y = torsten::pmx_ode_adams_ctrl(f_eigen, y0_vec_var, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta_var, x_r, x_i);
    auto y_sol = stan::math::ode_adams_tol(f_eigen, y0_vec_var, t0, ts, rtol, atol, max_num_steps, msgs,
                                          theta_var, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(theta_var, y_sol[j], y[j], 1e-6, 8e-6);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-6, 8e-6);
    }
  }

  {                             // y0 & ts var
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));
    std::vector<stan::math::var> ts_var(stan::math::to_var(ts));

    auto y = torsten::pmx_ode_adams_ctrl(f_eigen, y0_vec_var, t0, ts_var, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = stan::math::ode_adams_tol(f_eigen, y0_vec_var, t0, ts_var, rtol, atol, max_num_steps, msgs,
                                          theta, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(ts_var, y_sol[j], y[j], 1e-6, 8e-6);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-6, 8e-6);
    }
  }

  {                             // ts var
    std::vector<stan::math::var> ts_var(stan::math::to_var(ts));
    auto y = torsten::pmx_ode_adams_ctrl(f_eigen, y0_vec, t0, ts_var, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = stan::math::ode_adams_tol(f_eigen, y0_vec, t0, ts_var, rtol, atol, max_num_steps, msgs,
                                          theta, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(ts_var, y_sol[j], y[j], 1e-6, 8e-6);
    }
  }
}

TEST_F(TorstenOdeTest_neutropenia, eigen_vector_ckrk) {
  using torsten::dsolve::PMXVariadicOdeSystem;

  using stan::math::value_of;
  using torsten::dsolve::PMXOdeintIntegrator;

  {                             // theat & y0 var
    std::vector<stan::math::var> theta_var(stan::math::to_var(theta));
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));

    auto y = torsten::pmx_ode_ckrk_ctrl(f_eigen, y0_vec_var, t0, ts, msgs, rtol, atol, max_num_steps,
                                        theta_var, x_r, x_i);
    auto y_sol = stan::math::ode_rk45_tol(f_eigen, y0_vec_var, t0, ts, rtol, atol, max_num_steps, msgs,
                                          theta_var, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(theta_var, y_sol[j], y[j], 1e-8, 1e-8);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-8, 1e-8);
    }
  }

  {                             // y0 & ts var
    std::vector<stan::math::var> y0_var(stan::math::to_var(y0));
    Eigen::Matrix<stan::math::var, -1, 1> y0_vec_var(stan::math::to_vector(y0_var));
    std::vector<stan::math::var> ts_var(stan::math::to_var(ts));

    auto y = torsten::pmx_ode_ckrk_ctrl(f_eigen, y0_vec_var, t0, ts_var, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = stan::math::ode_rk45_tol(f_eigen, y0_vec_var, t0, ts_var, rtol, atol, max_num_steps, msgs,
                                          theta, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(ts_var, y_sol[j], y[j], 1e-8, 1e-7);
      torsten::test::test_grad(y0_var, y_sol[j], y[j], 1e-8, 1e-7);
    }
  }

  {                             // ts var
    std::vector<stan::math::var> ts_var(stan::math::to_var(ts));
    auto y = torsten::pmx_ode_ckrk_ctrl(f_eigen, y0_vec, t0, ts_var, msgs, rtol, atol, max_num_steps,
                                        theta, x_r, x_i);
    auto y_sol = stan::math::ode_rk45_tol(f_eigen, y0_vec, t0, ts_var, rtol, atol, max_num_steps, msgs,
                                          theta, x_r, x_i);

    for (size_t j = 0; j < ts.size(); ++j) {
      torsten::test::test_grad(ts_var, y_sol[j], y[j], 1e-8, 1e-7);
    }
  }
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// 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 <algorithm>
#include <iterator>
#include <numeric>
#include <random>
#include <unordered_set>

#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"

namespace open3d {
namespace geometry {

/// \class RANSACResult
///
/// \brief Stores the current best result in the RANSAC algorithm.
class RANSACResult {
public:
    RANSACResult() : fitness_(0), inlier_rmse_(0) {}
    ~RANSACResult() {}

public:
    double fitness_;
    double inlier_rmse_;
};

// Calculates the number of inliers given a list of points and a plane model,
// and the total distance between the inliers and the plane. These numbers are
// then used to evaluate how well the plane model fits the given points.
RANSACResult EvaluateRANSACBasedOnDistance(
        const std::vector<Eigen::Vector3d> &points,
        const Eigen::Vector4d plane_model,
        std::vector<size_t> &inliers,
        double distance_threshold,
        double error) {
    RANSACResult result;

    for (size_t idx = 0; idx < points.size(); ++idx) {
        Eigen::Vector4d point(points[idx](0), points[idx](1), points[idx](2),
                              1);
        double distance = std::abs(plane_model.dot(point));

        if (distance < distance_threshold) {
            error += distance;
            inliers.emplace_back(idx);
        }
    }

    size_t inlier_num = inliers.size();
    if (inlier_num == 0) {
        result.fitness_ = 0;
        result.inlier_rmse_ = 0;
    } else {
        result.fitness_ = (double)inlier_num / (double)points.size();
        result.inlier_rmse_ = error / std::sqrt((double)inlier_num);
    }
    return result;
}

// Find the plane such that the summed squared distance from the
// plane to all points is minimized.
//
// Reference:
// https://www.ilikebigbits.com/2015_03_04_plane_from_points.html
Eigen::Vector4d GetPlaneFromPoints(const std::vector<Eigen::Vector3d> &points,
                                   const std::vector<size_t> &inliers) {
    Eigen::Vector3d centroid(0, 0, 0);
    for (size_t idx : inliers) {
        centroid += points[idx];
    }
    centroid /= double(inliers.size());

    double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;

    for (size_t idx : inliers) {
        Eigen::Vector3d r = points[idx] - centroid;
        xx += r(0) * r(0);
        xy += r(0) * r(1);
        xz += r(0) * r(2);
        yy += r(1) * r(1);
        yz += r(1) * r(2);
        zz += r(2) * r(2);
    }

    double det_x = yy * zz - yz * yz;
    double det_y = xx * zz - xz * xz;
    double det_z = xx * yy - xy * xy;

    Eigen::Vector3d abc;
    if (det_x > det_y && det_x > det_z) {
        abc = Eigen::Vector3d(det_x, xz * yz - xy * zz, xy * yz - xz * yy);
    } else if (det_y > det_z) {
        abc = Eigen::Vector3d(xz * yz - xy * zz, det_y, xy * xz - yz * xx);
    } else {
        abc = Eigen::Vector3d(xy * yz - xz * yy, xy * xz - yz * xx, det_z);
    }

    double norm = abc.norm();
    // Return invalid plane if the points don't span a plane.
    if (norm == 0) {
        return Eigen::Vector4d(0, 0, 0, 0);
    }
    abc /= abc.norm();
    double d = -abc.dot(centroid);
    return Eigen::Vector4d(abc(0), abc(1), abc(2), d);
}

std::tuple<Eigen::Vector4d, std::vector<size_t>> PointCloud::SegmentPlane(
        const double distance_threshold /* = 0.01 */,
        const int ransac_n /* = 3 */,
        const int num_iterations /* = 100 */) const {
    RANSACResult result;
    double error = 0;

    // Initialize the plane model ax + by + cz + d = 0.
    Eigen::Vector4d plane_model = Eigen::Vector4d(0, 0, 0, 0);
    // Initialize the best plane model.
    Eigen::Vector4d best_plane_model = Eigen::Vector4d(0, 0, 0, 0);

    // Initialize consensus set.
    std::vector<size_t> inliers;

    size_t num_points = points_.size();
    std::vector<size_t> indices(num_points);
    std::iota(std::begin(indices), std::end(indices), 0);

    std::random_device rd;
    std::mt19937 rng(rd());

    // Return if ransac_n is less than the required plane model parameters.
    if (ransac_n < 3) {
        utility::LogError(
                "ransac_n should be set to higher than or equal to 3.");
        return std::make_tuple(best_plane_model, inliers);
    }
    if (num_points < size_t(ransac_n)) {
        utility::LogError("There must be at least 'ransac_n' points.");
        return std::make_tuple(best_plane_model, inliers);
    }

    for (int itr = 0; itr < num_iterations; itr++) {
        for (int i = 0; i < ransac_n; ++i) {
            std::swap(indices[i], indices[rng() % num_points]);
        }
        inliers.clear();
        for (int idx = 0; idx < ransac_n; ++idx) {
            inliers.emplace_back(indices[idx]);
        }

        // Fit model to num_model_parameters randomly selected points among the
        // inliers.
        plane_model = TriangleMesh::ComputeTrianglePlane(
                points_[inliers[0]], points_[inliers[1]], points_[inliers[2]]);
        if (plane_model.isZero(0)) {
            continue;
        }

        error = 0;
        inliers.clear();
        auto this_result = EvaluateRANSACBasedOnDistance(
                points_, plane_model, inliers, distance_threshold, error);
        if (this_result.fitness_ > result.fitness_ ||
            (this_result.fitness_ == result.fitness_ &&
             this_result.inlier_rmse_ < result.inlier_rmse_)) {
            result = this_result;
            best_plane_model = plane_model;
        }
    }

    // Find the final inliers using best_plane_model.
    inliers.clear();
    for (size_t idx = 0; idx < points_.size(); ++idx) {
        Eigen::Vector4d point(points_[idx](0), points_[idx](1), points_[idx](2),
                              1);
        double distance = std::abs(best_plane_model.dot(point));

        if (distance < distance_threshold) {
            inliers.emplace_back(idx);
        }
    }

    // Improve best_plane_model using the final inliers.
    best_plane_model = GetPlaneFromPoints(points_, inliers);

    utility::LogDebug("RANSAC | Inliers: {:d}, Fitness: {:e}, RMSE: {:e}",
                      inliers.size(), result.fitness_, result.inlier_rmse_);
    return std::make_tuple(best_plane_model, inliers);
}

}  // namespace geometry
}  // namespace open3d


#include <body_interact_game/body_analizer.hpp>

#include <Eigen/SVD>

namespace
{

constexpr auto pi {3.141592653589793};

inline Eigen::AngleAxisd get_yaw_inverse_matrix(Eigen::Affine3d pos) noexcept
{
  const auto ypr {pos.rotation().eulerAngles(2, 0, 1)};
  const auto yaw_angle {ypr(0) < pi / 2 ? ypr(2) : ypr(2) - pi};
  return {-yaw_angle, Eigen::Vector3d::UnitY()};
}

}

body_analizer::body_analizer(std::string root, std::size_t target_number)
  : root_ {root},
    target_number_ {std::to_string(target_number)},
    base_name_ {"torso_" + target_number_},
    right_hand_name_ {"right_hand_" + target_number_},
    left_hand_name_ {"left_hand_" + target_number_},
    right_knee_name_ {"right_knee_" + target_number_},
    left_knee_name_ {"left_knee_" + target_number_}
{
}

void body_analizer::update()
{
  const auto current_time {ros::Time{0}};
  const auto base_pos {get_position(base_name_, current_time)};
  const auto yaw_canceller {get_yaw_inverse_matrix(base_pos)};
  head_ = yaw_canceller * base_pos.rotation() * Eigen::Vector3d::UnitY();
  const auto base_vec {base_pos.translation()};
  right_hand_ = yaw_canceller * (get_position(right_hand_name_, current_time).translation() - base_vec);
  left_hand_ = yaw_canceller * (get_position(left_hand_name_, current_time).translation() - base_vec);
  right_knee_ = yaw_canceller * (get_position(right_knee_name_, current_time).translation() - base_vec);
  left_knee_ = yaw_canceller * (get_position(left_knee_name_, current_time).translation() - base_vec);
}


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  WrenchFactor.cpp
 * @brief Wrench balance factor, common between forward and inverse dynamics.
 * @author Frank Dellaert, Mandy Xie, Yetong Zhang, and Gerry Chen
 */

#include "gtdynamics/factors/WrenchFactor.h"

#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>

#include <boost/optional.hpp>
#include <iostream>
#include <vector>

#include "gtdynamics/dynamics/Dynamics.h"
#include "gtdynamics/statics/Statics.h"

using gtsam::Matrix;
using gtsam::Matrix6;
using gtsam::Pose3;
using gtsam::Values;
using gtsam::Vector;
using gtsam::Vector6;

namespace gtdynamics {

WrenchFactor::WrenchFactor(
    gtsam::Key twist_key, gtsam::Key twistAccel_key,
    const std::vector<DynamicsSymbol> &wrench_keys, gtsam::Key pose_key,
    const gtsam::noiseModel::Base::shared_ptr &cost_model,
    const Matrix6 &inertia, const boost::optional<gtsam::Vector3> &gravity)
    : Base(cost_model), inertia_(inertia), gravity_(gravity) {
  keys_.reserve(wrench_keys.size() + 3);
  keys_.push_back(twist_key);
  keys_.push_back(twistAccel_key);
  keys_.insert(keys_.end(), wrench_keys.cbegin(), wrench_keys.cend());
  keys_.push_back(pose_key);
}

Vector WrenchFactor::unwhitenedError(
    const Values &x, boost::optional<std::vector<Matrix> &> H) const {
  if (!this->active(x)) {
    return Vector::Zero(this->dim());
  }

  // Collect wrenches to implement L&P Equation 8.48 (F = ma)
  std::vector<Vector6> wrenches;

  // Coriolis forces.
  const Vector6 twist = x.at<Vector6>(keys_.at(0));
  Matrix6 H_twist;
  wrenches.push_back(Coriolis(inertia_, twist, H ? &H_twist : 0));

  // Change in generalized momentum.
  const Vector6 twistAccel = x.at<Vector6>(keys_.at(1));
  wrenches.push_back(-inertia_ * twistAccel);

  // External wrenches.
  for (auto key = keys_.cbegin() + 2; key != keys_.cend() - 1; ++key) {
    wrenches.push_back(x.at<Vector6>(*key));
  }

  // Calculate resultant wrench, fills up H with identity matrices if asked,
  // except the last H contains the pose derivative or zero if no gravity.
  const Vector6 error = ResultantWrench(wrenches, inertia_(3, 3),
                                        x.at<Pose3>(keys_.back()), gravity_, H);

  // If asked, update Jacobians not yet calculated by ResultantWrench.
  if (H) {
    (*H)[0] = H_twist;    // Coriolis depends in twist (key 0)
    (*H)[1] = -inertia_;  // Derivative with respect to twist acceleration
  }

  return error;
}

}  // namespace gtdynamics


#include <utility>

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

#include "total_unimodularity.hpp"

namespace tu
{

  /**
   * Tests if the given matrix contains only -1,0,+1 entries,
   * returning the violating position if this is not the case.
   *
   * @param matrix The given matrix
   * @param position Returns a violating entry
   * @return true if and only if it is a -1,0,+1 matrix
   */

  bool is_zero_plus_minus_one_matrix(const integer_matrix& matrix, std::pair <integer_matrix::size_type, integer_matrix::size_type>& position)
  {
    for (size_t row = 0; row < matrix.size1(); ++row)
    {
      for (size_t column = 0; column < matrix.size2(); ++column)
      {
        const int value = matrix(row, column);
        if (value < -1 || value > 1)
        {
          position.first = row;
          position.second = column;
          return false;
        }
      }
    }

    return true;
  }

  /**
   * Tests if the given matrix contains only -1,0,+1 entries.
   *
   * @param matrix The given matrix
   * @return true if and only if it is a -1,0,+1 matrix
   */

  bool is_zero_plus_minus_one_matrix(const integer_matrix& matrix)
  {
    std::pair <size_t, size_t> result;

    return is_zero_plus_minus_one_matrix(matrix, result);
  }

  /**
   * Tests if the given matrix contains only 0 or 1 entries,
   * returning the violating position if this is not the case.
   *
   * @param matrix The given matrix
   * @param position Returns a violating entry
   * @return true if and only if it is a 0-1 matrix
   */

  bool is_zero_one_matrix(const integer_matrix& matrix, std::pair <integer_matrix::size_type, integer_matrix::size_type>& position)
  {
    for (size_t i = 0; i < matrix.size1(); ++i)
    {
      for (size_t j = 0; j < matrix.size2(); ++j)
      {
        const int value = matrix(i, j);
        if (value < 0 || value > 1)
        {
          position .first = i;
          position.second = j;
          return false;
        }
      }
    }

    return true;
  }

  /**
   * Tests if the given matrix contains only 0 or 1 entries.
   *
   * @param matrix The given matrix
   * @return true if and only if it is a 0-1 matrix
   */

  bool is_zero_one_matrix(const integer_matrix& matrix)
  {
    std::pair <size_t, size_t> result;

    return is_zero_one_matrix(matrix, result);
  }

} /* namespace tu */


/*
   Copyright (c) Marshall Clow 2014.

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

 Revision history:
    2 Dec 2014 mtc First version; power

*/

/// \file algorithm.hpp
/// \brief Misc Algorithms
/// \author Marshall Clow
///

#ifndef BOOST_ALGORITHM_HPP
#define BOOST_ALGORITHM_HPP

#include <functional> // for plus and multiplies

#include <boost/utility/enable_if.hpp> // for boost::disable_if
#include <boost/type_traits/is_integral.hpp>

namespace boost { namespace algorithm {

template <typename T>
BOOST_CXX14_CONSTEXPR T identity_operation ( std::multiplies<T> ) { return T(1); }

template <typename T>
BOOST_CXX14_CONSTEXPR T identity_operation ( std::plus<T> ) { return T(0); }

/// \fn power ( T x, Integer n )
/// \return the value "x" raised to the power "n"
///
/// \param x     The value to be exponentiated
/// \param n     The exponent (must be >= 0)
///
//  \remark Taken from Knuth, The Art of Computer Programming, Volume 2:
//  Seminumerical Algorithms, Section 4.6.3
template <typename T, typename Integer>
BOOST_CXX14_CONSTEXPR typename boost::enable_if<boost::is_integral<Integer>, T>::type
power (T x, Integer n) {
    T y = 1; // Should be "T y{1};"
    if (n == 0) return y;
    while (true) {
        if (n % 2 == 1) {
            y = x * y;
            if (n == 1)
                return y;
            }
        n = n / 2;
        x = x * x;
        }
    return y;
    }

/// \fn power ( T x, Integer n, Operation op )
/// \return the value "x" raised to the power "n"
/// using the operation "op".
///
/// \param x     The value to be exponentiated
/// \param n     The exponent (must be >= 0)
/// \param op    The operation used
///
//  \remark Taken from Knuth, The Art of Computer Programming, Volume 2:
//  Seminumerical Algorithms, Section 4.6.3
template <typename T, typename Integer, typename Operation>
BOOST_CXX14_CONSTEXPR typename boost::enable_if<boost::is_integral<Integer>, T>::type
power (T x, Integer n, Operation op) {
    T y = identity_operation(op);
    if (n == 0) return y;
    while (true) {
        if (n % 2 == 1) {
            y = op(x, y);
            if (n == 1)
                return y;
            }
        n = n / 2;
        x = op(x, x);
        }
    return y;
    }

}}

#endif // BOOST_ALGORITHM_HPP


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012 Alexey Korepanov <kaikaikai@yandex.ru>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_RUNTIME_NO_MALLOC
#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

template<typename MatrixType> void real_qz(const MatrixType& m)
{
  /* this test covers the following files:
     RealQZ.h
  */
  using std::abs;
  typedef typename MatrixType::Index Index;
  typedef typename MatrixType::Scalar Scalar;

  Index dim = m.cols();

  MatrixType A = MatrixType::Random(dim,dim),
             B = MatrixType::Random(dim,dim);

  // Regression test for bug 985: Randomly set rows or columns to zero
  Index k=internal::random<Index>(0, dim-1);
  switch(internal::random<int>(0,10)) {
  case 0:
    A.row(k).setZero(); break;
  case 1:
    A.col(k).setZero(); break;
  case 2:
    B.row(k).setZero(); break;
  case 3:
    B.col(k).setZero(); break;
  default:
    break;
  }

  RealQZ<MatrixType> qz(dim);
  // TODO enable full-prealocation of required memory, this probably requires an in-place mode for HessenbergDecomposition
  //Eigen::internal::set_is_malloc_allowed(false);
  qz.compute(A,B);
  //Eigen::internal::set_is_malloc_allowed(true);

  VERIFY_IS_EQUAL(qz.info(), Success);
  // check for zeros
  bool all_zeros = true;
  for (Index i=0; i<A.cols(); i++)
    for (Index j=0; j<i; j++) {
      if (abs(qz.matrixT()(i,j))!=Scalar(0.0))
      {
        std::cerr << "Error: T(" << i << "," << j << ") = " << qz.matrixT()(i,j) << std::endl;
        all_zeros = false;
      }
      if (j<i-1 && abs(qz.matrixS()(i,j))!=Scalar(0.0))
      {
        std::cerr << "Error: S(" << i << "," << j << ") = " << qz.matrixS()(i,j) << std::endl;
        all_zeros = false;
      }
      if (j==i-1 && j>0 && abs(qz.matrixS()(i,j))!=Scalar(0.0) && abs(qz.matrixS()(i-1,j-1))!=Scalar(0.0))
      {
        std::cerr << "Error: S(" << i << "," << j << ") = " << qz.matrixS()(i,j)  << " && S(" << i-1 << "," << j-1 << ") = " << qz.matrixS()(i-1,j-1) << std::endl;
        all_zeros = false;
      }
    }
  VERIFY_IS_EQUAL(all_zeros, true);
  VERIFY_IS_APPROX(qz.matrixQ()*qz.matrixS()*qz.matrixZ(), A);
  VERIFY_IS_APPROX(qz.matrixQ()*qz.matrixT()*qz.matrixZ(), B);
  VERIFY_IS_APPROX(qz.matrixQ()*qz.matrixQ().adjoint(), MatrixType::Identity(dim,dim));
  VERIFY_IS_APPROX(qz.matrixZ()*qz.matrixZ().adjoint(), MatrixType::Identity(dim,dim));
}

void test_real_qz()
{
  int s = 0;
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( real_qz(Matrix4f()) );
    s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
    CALL_SUBTEST_2( real_qz(MatrixXd(s,s)) );

    // some trivial but implementation-wise tricky cases
    CALL_SUBTEST_2( real_qz(MatrixXd(1,1)) );
    CALL_SUBTEST_2( real_qz(MatrixXd(2,2)) );
    CALL_SUBTEST_3( real_qz(Matrix<double,1,1>()) );
    CALL_SUBTEST_4( real_qz(Matrix2d()) );
  }

  TEST_SET_BUT_UNUSED_VARIABLE(s)
}


//
// Created by Сергей Кривонос on 01.09.17.
//
#include "Exponentiation.h"

#include "e.h"
#include "i.h"
#include "Infinity.h"
#include "pi.h"
#include "Integer.h"
#include "Sum.h"
#include "Product.h"

#include <cmath>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

namespace omnn{
namespace math {

    Exponentiation::Exponentiation(const Valuable& _1, const Valuable& _2)
    : base(_1,_2)
    {
        InitVars();
    }

    max_exp_t Exponentiation::getMaxVaExp(const Valuable& b, const Valuable& e)
    {
        if (e.IsInt()) {
            return b.getMaxVaExp() * e.ca();
        } else if (e.FindVa()) {
            auto i = b.getMaxVaExp();
            if (i) {
                auto _ = e;
                const Variable* v;
                while ((v = _.FindVa())) {
                    _.Eval(*v, 0);
                }
                _.optimize();
                i *= static_cast<a_int>(_);
            }
            return i;
        } else {
            auto maxVaExp = e * b.getMaxVaExp();
            if (maxVaExp.IsInt()) {
                return maxVaExp.ca();
            } else if (maxVaExp.IsSimpleFraction()) {
                auto& f = maxVaExp.as<Fraction>();
                return {f.getNumerator().ca(), f.getDenominator().ca()};
            } else if(!optimizations) {
                optimizations = true;
                maxVaExp.optimize();
                optimizations = {};
                if (maxVaExp.IsInt()) {
                    return maxVaExp.ca();
                }
            }
        }

        IMPLEMENT
    }

    max_exp_t Exponentiation::getMaxVaExp() const
    {
        return getMaxVaExp(getBase(), getExponentiation());
    }

	Valuable Exponentiation::operator -() const
    {
        return Product {*this, -1};
    }

    void Exponentiation::InitVars() {
        v.clear();
        if (ebase().IsVa())
            v[ebase().as<Variable>()] = eexp();
    }

    void Exponentiation::optimize()
    {
        if (optimized) {
            return;
        }

        if (!optimizations)
        {
            hash = ebase().Hash() ^ eexp().Hash();
            return;
        }

        ebase().optimize();
        eexp().optimize();

        if (ebase().IsExponentiation() || ebase().IsProduct())
        {
            ebase() ^= eexp();
            Become(std::move(ebase()));
            return;
        }

        if (eexp().IsSum())
        {
            auto& s = eexp().as<Sum>();
            auto sz = s.size();
            auto v = 1_v;
            for(auto it = s.begin(), e = s.end();
                it != e; )
            {
                if (it->IsInt()) {
                    v *= ebase()^*it;
                    s.Delete(it);
                }
                else
                    ++it;
            }
            if (sz != s.size()) {
                Become(*this * v);
                return;
            }
        }

        // todo : check it, comment this and try System test
//        if (ebase().IsSum() && eexp().IsFraction())
//        {
//            auto f = Fraction::cast(eexp());
//            auto& d = f->getDenominator();
//            if (d == ebase().getMaxVaExp()) {
//                auto vars = ebase().Vars();
//                if (vars.size() == 1) {
//                    auto va = *vars.begin();
//                    auto baseToSolve = ebase();
//                    baseToSolve.SetView(View::Solving);
//                    baseToSolve.optimize();
//                    auto eq = va - baseToSolve(va);
//                    auto sq = eq ^ d;
//                    auto check = ebase()/sq;
//                    if (check.IsInt() || check.IsSimpleFraction()) {
//                        ebase() = eq;
//                        eexp() = f->getNumerator();
//                    } else {
//                        // TODO : IMPLEMENT
//                    }
//                }
//            }
//        }
//
//        if (ebase().IsSum() && eexp().IsFraction())
//        {
//            auto f = Fraction::cast(eexp());
//            auto& d = f->getDenominator();
//            auto e = ebase() ^ f->getNumerator();
//            if (d == e.getMaxVaExp()) {
//                auto vars = e.Vars();
//                if (vars.size() == 1) {
//                    auto va = *vars.begin();
//                    auto eq = va - e(va);
//                    auto sq = eq ^ d;
//                    auto check = e / sq;
//                    if (check.IsInt() || check.IsSimpleFraction()) {
//                        Become(std::move(eq));
//                        return;
//                    } else {
//                        // TODO : IMPLEMENT
//                    }
//                }
//            }
//        }

        if (ebase().IsFraction() && eexp().IsMultival()==YesNoMaybe::No) {
            auto& f = ebase().as<Fraction>();
            auto _ = (f.getNumerator() ^ eexp()) / (f.getDenominator() ^ eexp());
            if (_.IsExponentiation()) {
                auto& e = _.as<Exponentiation>();
                if (!(e.ebase()==ebase() && eexp()==e.eexp())) {
                    IMPLEMENT
                }
            } else {
                Become(std::move(_));
                return;
            }
        }

        if (ebase().IsFraction() && eexp().IsInt() && eexp() < 0_v) {
            eexp() = -eexp();
            ebase() = ebase().as<Fraction>().Reciprocal();
        }

        if (ebase().Is_e()) {
            if (eexp().IsProduct()) {
                auto& p = eexp().as<Product>();
                if (p.Has(constant::pi) && p.Has(constant::i)) { // TODO : sequence does metter
//                    ebase() = -1;
//                    eexp() /= constant::i;
//                    eexp() /= constant::pi;
                }
            }
        }
        // todo : check
        if (ebase().IsSimple()) {
            if (eexp().IsProduct()) {
                auto& p = eexp().as<Product>();
                auto it = p.GetFirstOccurence<Integer>();
                auto in = ebase() ^ *it;  // IsExponentiationSimplifiable
                if (in.IsInt()) {
                    ebase() = in;
                    p.Delete(it);
                    if (p.size() == 1) {
                        eexp().optimize();
                    }
                }
            }
            if (ebase()==1) {
                if (eexp().IsInt()) {
                    Become(std::move(ebase()));
                    return;
                } else if (eexp().IsSimpleFraction()) {
                    if (eexp().as<Fraction>().getDenominator().bit(0)) {
                        Become(std::move(ebase()));
                        return;
                    }
                } else if (!!eexp().IsMultival()) {
                } else if (!(eexp().IsInfinity() || eexp().IsMInfinity())) {
                    Become(std::move(ebase()));
                    return;
                } else
                    IMPLEMENT;
            } else if (ebase()==-1 && eexp().IsInt() && eexp() > 0 && eexp()!=1) {
                eexp() = eexp().bit(0);
            } else if (eexp()==-1) {
                Become(Fraction{1,ebase()});
                return;
            } else if (eexp().IsInfinity()) {
                IMPLEMENT
            } else if (eexp().IsFraction()) {
                auto& f = eexp().as<Fraction>();
                auto& n = f.getNumerator();
                if (n != 1) {
                    // TODO: auto is = ebase().IsExponentiationSimplifiable(n);
                    auto newBase = ebase() ^ n;
                    if(!newBase.IsExponentiation()){
                        Become(newBase ^ (1_v / f.getDenominator()));
                        return;
                    }
                }
            }
        }

        bool ebz = ebase() == 0_v;
        bool exz = eexp() == 0_v;
        if(exz)
        {
            if (ebase().IsInfinity() || ebase().IsMInfinity()) {
                IMPLEMENT
            }
            if(ebz)
                throw "NaN";

            Become(1_v);
            return;
        }
        else if(eexp() == 1_v)
        {
            Become(std::move(ebase()));
            return;
        }
        else if (ebz)
        {
            if (exz)
                throw "NaN";
            Become(0_v);
            return;
        }
        else if (ebase().IsInfinity())
        {
            if (eexp() > 0) {
                Become(std::move(ebase()));
            } else
                IMPLEMENT
        }
        else if (ebase().IsMInfinity())
        {
            if (eexp() > 0) {
                if ((eexp() % 2) > 0) // TODO : test with non-ints
                    Become(std::move(ebase()));
                else
                    Become(Infinity());
            } else
                IMPLEMENT
        }
        else if (ebase().IsVa() && eexp().IsSimple())
        {
        }
        else
        {
            switch(view)
            {
                case View::Solving:

                case View::None:
                case View::Calc:
                {
                    if (eexp().IsInt() && eexp()>0) {
                        auto b = 1_v;
                        for (; eexp()--;) {
                            b *= ebase();
                        }
                        Become(std::move(b));
                        return;
                    }
                    if (ebase().IsInt() && eexp().IsInt()) {
                        Become(ebase() ^ eexp());
                        return;
                    }
                    break;
                }
                case View::Flat: {

                    if(eexp().IsInt())
                    {
                        if (ebase().IsVa()) {
                            break;
                        }
                        if (eexp() != 0_v) {
                            if (eexp() > 1) {
                                Valuable x = ebase();
                                Valuable n = eexp();
                                if (n < 0_v)
                                {
                                    x = 1_v / x;
                                    n = -n;
                                }
                                if (n == 0_v)
                                {
                                    Become(1_v);
                                    return;
                                }
                                auto y = 1_v;
                                while(n > 1)
                                {
                                    bool isInt = n.IsInt();
                                    if (!isInt)
                                        IMPLEMENT
                                    if (isInt && n.bit(0) == 0_v)
                                    {
                                        x.sq();
                                        n /= 2;
                                    }
                                    else
                                    {
                                        y *= x;
                                        x.sq();
                                        --n;
                                        n /= 2;
                                    }
                                }
                                x *= y;
                                Become(std::move(x));
                            } else if (eexp()!=-1){
                                // negative
                                Become(1_v/(ebase()^(-eexp())));
                            }
                        }
                        else { // zero
                            if (ebase() == 0_v)
                            {
                                IMPLEMENT
                                throw "NaN"; // feel free to handle this properly
                            }
                            else
                            {
                                Become(1_v);
                            }
                        }
                    }
//                    else
//                    IMPLEMENT
                    break;
                }
                case View::Equation: {
                    if(eexp().IsSimple())
                        Become(std::move(ebase()));
                    break;
                }
                default:
                	LOG_AND_IMPLEMENT(str() << " mode is " << view);
            }
        }

        if(IsExponentiation() && ebase().IsExponentiation())
        {
            auto& e = ebase().as<Exponentiation>();
            auto& eeexp = e.getExponentiation();
            if ((eeexp.FindVa() == nullptr) == (eexp().FindVa() == nullptr)) {
                eexp() *= eeexp;
                // todo : copy if it shared
                ebase() = std::move(const_cast<Valuable&>((e.getBase())));
            }
        }

        if (IsExponentiation()) {
            hash = ebase().Hash() ^ eexp().Hash();
            optimized = true;
            InitVars();
        }
    }

    Valuable& Exponentiation::operator +=(const Valuable& v)
    {
        return Become(Sum {*this, v});
    }

    Valuable& Exponentiation::operator *=(const Valuable& v)
    {
        const Exponentiation* e;
        const Fraction* f;
        const Product* fdn = {};
        const Exponentiation* fdne;
        auto isProdHasExpWithSameBase = [this](const Product* p) -> const Exponentiation*
        {
            for(auto& it : *p){
                if (it.IsExponentiation()) {
                    auto& e = it.as<Exponentiation>();
                    if (ebase() == e.getBase()) {
                        return &e;
                    }
                }
            }
            return {};
        };
        if (v.IsExponentiation()
            && ebase() == (e = &v.as<Exponentiation>())->getBase()
            && (eexp().IsInt() || eexp().IsSimpleFraction()) && eexp() > 0
            && (e->eexp().IsInt() || e->eexp().IsSimpleFraction()) && e->eexp() > 0
            )
        {
            eexp() += e->getExponentiation();
            optimized={};
        }
        else if(v.IsFraction()
                && (f = &v.as<Fraction>())->getDenominator() == ebase())
        {
            --eexp();
            optimized={};
            optimize();
            return *this *= f->getNumerator();
        }
        else if(v.IsFraction()
                && f->getDenominator().IsProduct()
                && (fdn = &f->getDenominator().as<Product>())->Has(ebase()))
        {
            --eexp();
            optimized={};
            optimize();
            return *this *= f->getNumerator() / (*fdn / ebase());
        }
        else if(fdn
                && (fdne = isProdHasExpWithSameBase(fdn)))
        {
            eexp() -= fdne->getExponentiation();
            optimized={};
            optimize();
            return *this *= f->getNumerator() / (*fdn / *fdne);
        }
        else if(ebase() == v && v.FindVa())
        {
            ++eexp();
            optimized={};
        }
        else if(v.IsProduct())
        {
            return Become(v * *this);
        }
        else if(v.IsInt())
        {
            if(v==1)
                return *this;
            else if(eexp()==-1 && ebase().IsInt())
                return Become(v/ebase());
            else
                return Become(Product{v, *this});
        }
        else
            return Become(Product{v, *this});

        optimize();
        return *this;
    }

    bool Exponentiation::MultiplyIfSimplifiable(const Valuable& v)
    {
        auto is = v == getBase();
        if (is) {
            ++eexp();
            optimized = {};
            optimize();
        } else if (v.IsExponentiation()) {
            auto& vexpo = v.as<Exponentiation>();
            is = vexpo.getBase() == getBase();
            if (is) {
                eexp() += vexpo.eexp();
                optimized = {};
                optimize();
            } // TODO : else if ? (base^2 == v.base)
        } else if (v.IsInt()) {
            IMPLEMENT
        } else {
//            std::cout << str() << " * " << v.str() << std::endl;
        }
        return is;
    }

    static auto one = 1_v;
    std::pair<bool,Valuable> Exponentiation::IsMultiplicationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is, expSumSimplifiable = {};
        is.first = v == getBase()
            && (expSumSimplifiable = vo<1>::get().IsSummationSimplifiable(eexp())).first;
        if (is.first) {
            is.second = getBase() ^ expSumSimplifiable.second;
        } else if (v.IsExponentiation()) {
            auto& vexpo = v.as<Exponentiation>();
            is.first = vexpo.getBase() == getBase();
            if (is.first) {
                is.second = ebase() ^ (eexp() + vexpo.eexp());
            } // TODO : else if ? (base^2 == v.base)
        } else if (v.IsSimple()) {
//            if (getBase().IsVa()) {
//            } else if (getExponentiation().IsSimpleFraction()) {
//                auto
//                is.first = IsMultiplicationSimplifiable()
//            } else if (getExponentiation().IsSimple()) {
//                is = base::IsMultiplicationSimplifiable(v);
//            } else {
//                IMPLEMENT
//            }
        } else if (v.IsVa()) {
            // covered by (v==base()) case
        } else {
#ifndef NDEBUG
            std::cout << "IsMultiplication simplifiable?: " << str() << " * " << v.str() << std::endl;
#endif
        }
        return is;
    }

    bool Exponentiation::SumIfSimplifiable(const Valuable& v)
    {
        auto is = !v.IsSimple() && !v.IsFraction() && !v.IsExponentiation();
        if(is){
            auto sumIfSimplifiable = v.IsSummationSimplifiable(*this);
            is = sumIfSimplifiable.first;
            if (is)
                Become(std::move(sumIfSimplifiable.second));
        }
        return is;
    }

    std::pair<bool,Valuable> Exponentiation::IsSummationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is;
        is.first = operator==(v);
        if (is.first) {
            is.second = *this * 2;
        } else if ((is.first = operator==(-v))) {
                is.second = 0;
        } else if (v.IsSimple()
                || v.IsExponentiation()
                || v.IsVa()
                || v.IsFraction())
        {
        } else {
            is = v.IsSummationSimplifiable(*this);
        }
        return is;
    }

    Valuable& Exponentiation::operator /=(const Valuable& v)
    {
        auto isMultival = IsMultival()==YesNoMaybe::Yes;
        auto vIsMultival = v.IsMultival()==YesNoMaybe::Yes;
        if(isMultival && vIsMultival) {
            solutions_t vals, thisValues;
            Values([&](auto& thisVal){
                thisValues.insert(thisVal);
                return true;
            });

            v.Values([&](auto&vVal){
                for(auto& tv:thisValues)
                    vals.insert(tv/vVal);
                return true;
            });

            return Become(Valuable(vals));
        }
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            if(ebase() == e.ebase() && (ebase().IsVa() || !ebase().IsMultival()))
            {
                eexp() -= e.eexp();
            }
            else
            {
                Become(Fraction(*this, v));
                return *this;
            }
        }
        else if(v.IsFraction())
        {
            *this *= v.as<Fraction>().Reciprocal();
            return *this;
        }
        else if(ebase() == v)
        {
            --eexp();
        }
        else
        {
            Become(Fraction(*this, v));
            return *this;
        }

        optimized={};
        optimize();
        return *this;
    }

    Valuable& Exponentiation::operator^=(const Valuable& v)
    {
        eexp() *= v;
        optimized={};
        optimize();
        return *this;
    }

    bool Exponentiation::operator ==(const Valuable& v) const
    {
        auto eq = v.IsExponentiation() && Hash()==v.Hash();
        if(eq){
            auto& e = v.as<Exponentiation>();
            eq = _1.Hash() == e._1.Hash()
                && _2.Hash() == e._2.Hash()
                && _1 == e._1
                && _2 == e._2;
        } else if (v.IsFraction()) {
            eq = eexp().IsInt()
                 && eexp() < 0
                 && ebase() == (v.as<Fraction>().getDenominator() ^ (-eexp()));
        }
        return eq;
    }

    Exponentiation::operator double() const
    {
        return std::pow(static_cast<double>(ebase()), static_cast<double>(eexp()));
    }

    Valuable& Exponentiation::d(const Variable& x)
    {
        optimized={};
        bool bhx = ebase().HasVa(x);
        bool ehx = eexp().HasVa(x);
        if(ehx) {
            IMPLEMENT
            if(bhx){

            }else{

            }
        } else if (bhx) {
            if(ebase() == x)
                Become(eexp() * (ebase() ^ (eexp()-1)));
            else
                IMPLEMENT
        } else
            Become(0_v);
        optimize();
        return *this;
    }

    Valuable& Exponentiation::i(const Variable& x, const Variable& C)
    {
        if ((eexp().IsInt() || eexp().IsSimpleFraction()) && ebase()==x) {
            ++eexp();
            operator/=(eexp());
            operator+=(C);
        } else {
            IMPLEMENT
        }

        optimize();
        return *this;
    }

    Valuable Exponentiation::I(const Variable& x, const Variable& C) const
    {
        if ((eexp().IsInt() || eexp().IsSimpleFraction()) && ebase()==x) {
            auto einc = eexp()+1;
            return (ebase() ^ einc) / einc + C;
        } else {
            IMPLEMENT
        }
    }

    bool Exponentiation::operator <(const Valuable& v) const
    {
        if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            if (e.getBase() == getBase())
                return getExponentiation() < e.getExponentiation();
            if (e.getExponentiation() == getExponentiation())
                return getBase() < e.getBase();
        }

        return base::operator <(v);
    }

    std::ostream& Exponentiation::print_sign(std::ostream& out) const
    {
        return out << "^";
    }

    Valuable::YesNoMaybe Exponentiation::IsMultival() const
    {
        auto is = _1.IsMultival() || _2.IsMultival();
        if (is != YesNoMaybe::Yes && _2.IsFraction())
            is = _2.as<Fraction>().getDenominator().IsEven() || is;
        return is;
    }

    void Exponentiation::Values(const std::function<bool(const Valuable&)>& fun) const
    {
        if (fun) {
            auto cache = optimized; // TODO: multival caching (inspect all optimized and optimization transisions) auto isCached =

            std::set<Valuable> vals;
            {
            std::deque<Valuable> d1;
            _1.Values([&](auto& v){
                d1.push_back(v);
                return true;
            });

            _2.Values([&](auto& v){
                auto vIsFrac = v.IsFraction();
                const Fraction* f;
                if(vIsFrac)
                    f = &v.template as<Fraction>();
                auto vMakesMultival = vIsFrac && f->getDenominator().IsEven()==YesNoMaybe::Yes;

                for(auto& item1:d1){
                    if(vMakesMultival){
                        Variable x;
                        auto& dn = f->getDenominator();
                        auto solutions = (x ^ dn).Equals(*this ^ dn).Solutions(x);
                        for(auto&& s:solutions)
                            vals.insert(s);
                    } else {
                        auto value=item1^v;
                        if(value.IsMultival()==YesNoMaybe::No)
                            vals.insert(value);
                        else {
                            IMPLEMENT
                        }
                    }
                }
                return true;
            });
            }

            for(auto& v:vals)
                fun(v);
        }
    }

    std::ostream& Exponentiation::code(std::ostream& out) const
    {
        if(!getExponentiation().IsInt())
            IMPLEMENT;

        out << "(1";
        for (auto i=getExponentiation(); i-->0;) {
            out << '*' << getBase();
        }
        out << ')';

        return out;
    }

    bool Exponentiation::IsComesBefore(const Valuable& v) const
    {
        auto mve = getMaxVaExp();
        auto vmve = v.getMaxVaExp();
        auto is = mve > vmve;
        if (mve != vmve)
        {}
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            bool baseIsVa = getBase().IsVa();
            bool vbaseIsVa = e.getBase().IsVa();
            if (baseIsVa && vbaseIsVa)
                is = getExponentiation() == e.getExponentiation() ? getBase().IsComesBefore(e.getBase()) : getExponentiation() > e.getExponentiation();
            else if(baseIsVa)
                is = false;
            else if(vbaseIsVa)
                is = true;
            else if(getBase() == e.ebase())
                is = getExponentiation().IsComesBefore(e.getExponentiation());
            else if(getExponentiation() == e.getExponentiation())
                is = getBase().IsComesBefore(e.getBase());
            else
            {
                auto c = Complexity();
                auto ec = e.Complexity();
                if (c != ec)
                    is = c > ec;
                else {
                    is = getBase().IsComesBefore(e.getBase()) ||
                        (!e.ebase().IsComesBefore(ebase()) && getExponentiation().IsComesBefore(e.getExponentiation())); //  || str().length() > e->str().length();
    //                auto expComesBefore = eexp().IsComesBefore(e->eexp());
    //                auto ebase()ComesBefore = ebase().IsComesBefore(e->ebase());
    //                is = expComesBefore==ebase()ComesBefore || str().length() > e->str().length();
                }
            }
        }
        else if(v.IsProduct())
        {
            is = !v.IsComesBefore(*this);
        }
        else if(v.IsInt())
            is = true;
//        else if(v.IsFraction())
//        {is=}
        else if(v.IsVa())
            is = !!FindVa();
        else if(v.IsSum())
            is = IsComesBefore(*v.as<Sum>().begin());
        else
            IMPLEMENT

        return is;
    }

    Valuable Exponentiation::calcFreeMember() const
    {
        Valuable c;
        if(getBase().IsSum() && getExponentiation().IsInt()){
            c = getBase().calcFreeMember() ^ getExponentiation();
        } else if(getBase().IsVa()) {
            c = 0_v;
        } else
            IMPLEMENT;
        return c;
    }

    Valuable & Exponentiation::sq()
    {
        eexp() *= 2;
        optimized = {};
        optimize();
        return *this;
    }

    const Valuable::vars_cont_t& Exponentiation::getCommonVars() const
    {
        return v;
    }

    Valuable Exponentiation::InCommonWith(const Valuable& v) const
    {
        auto c = 1_v;
        if (v.IsProduct()) {
            for(auto& m: v.as<Product>()){
                c = InCommonWith(m);
                if (c != 1_v) {
                    break;
                }
            }
        } else if (v.IsExponentiation()) {
            auto& e = v.as<Exponentiation>();
            if (e.getBase() == getBase()) {
                if (e.getExponentiation() == getExponentiation()) {
                    c = e;
                } else if (getExponentiation().IsSimple() && e.getExponentiation().IsSimple()) {
                    if (getExponentiation() > 0 || e.getExponentiation() > 0) {
                        if (e.getExponentiation() >= getExponentiation()) {
                            c = *this;
                        } else
                            c = e;
                    } else if (getExponentiation() < 0 || e.getExponentiation() < 0) {
                        if (e.getExponentiation() >= getExponentiation()) {
                            c = e;
                        } else
                            c = *this;
                    } else {
                        IMPLEMENT
                    }
                } else if (getExponentiation().IsSimpleFraction() && e.getExponentiation().IsSimpleFraction()) {
                    if (getExponentiation()<0 == e.getExponentiation()<0) {
                        c = getBase() ^ getExponentiation().InCommonWith(e.getExponentiation());
                    }
                } else if (getExponentiation().IsSum()) {
                    auto sz = getExponentiation().as<Sum>().size();
                    auto diff = getExponentiation() - e.getExponentiation();
                    if (!diff.IsSum() || diff.as<Sum>().size() < sz)
                        c = v;
                } else if (e.getExponentiation().IsSum()) {
                    c = e.InCommonWith(*this);
                } else if (e.getExponentiation().IsProduct()) {
                    c = ebase() ^ e.eexp().InCommonWith(eexp());
                } else {
                    IMPLEMENT
                }
            }
        } else if (getExponentiation().IsInt()) {
            if(getExponentiation() > 0)
                c = getBase().InCommonWith(v);
        } else if (getExponentiation().IsFraction()) {
        } else if (v.IsVa()) {
            c = v.InCommonWith(*this);
        } else if (v.IsInt() || v.IsSimpleFraction()) {
        } else if (getExponentiation().IsVa()) {
        } else {
            IMPLEMENT
        }
        return c;
    }

    Valuable Exponentiation::operator()(const Variable& va) const
    {
        return operator()(va, 0_v);
    }

    Valuable Exponentiation::operator()(const Variable& v, const Valuable& augmentation) const
    {
        if (!getExponentiation().FindVa() && getExponentiation()!=0 && augmentation==0) {
            return getBase()(v,augmentation);
        } else if (getExponentiation().IsSimpleFraction()) {
            auto& f = getExponentiation().as<Fraction>();
            return (getBase()^f.getNumerator())(v,augmentation^f.getDenominator());
        } else {
            IMPLEMENT
        }
    }

    Valuable::solutions_t Exponentiation::Distinct() const
    {
        solutions_t branches;
        if (eexp().IsSimpleFraction()){
            auto& f = eexp().as<Fraction>();
            auto& denom = f.denominator();
            if (denom.IsEven() == YesNoMaybe::Yes) {
                // TODO : de-recoursefy:
//                auto branchesSz = boost::multiprecision::msb(denom); // the largest bit
//                branches.reserve(branchesSz);
//                ...
                if(!ebase().IsInt()){
                    LOG_AND_IMPLEMENT("Distinct for " << str());
                } else {
                    for (auto&& branch
                            : (ebase().Sqrt() ^ (f.numerator() / (denom / 2))).Distinct())
                    {
                        branches.emplace(-branch);
                        branches.emplace(std::move(branch));
                    }
                }
            }
        } else {
            branches.emplace(*this);
        }
        return branches;
    }
}}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2016 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/multiprecision/cpp_bin_float.hpp>

int main()
{
   typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<256> >  ext_float_t;
   typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<2046> > long_ext_float_t;

   ext_float_t x = 5e15;
   x += 0.5;
   ext_float_t x1 = x + 255.0 / (1 << 20); // + 2^-12 - eps
   ext_float_t x2 = x + 257.0 / (1 << 20); // + 2^-12 + eps
   double      d1 = x1.convert_to<double>();
   double      d2 = x2.convert_to<double>();

   std::cout << std::setprecision(18) << d1 << std::endl;
   std::cout << std::setprecision(18) << d2 << std::endl;

   x        = 1e7 + 0.5;
   x1       = x + ldexp(255.0, -38); // + 2^-30 - eps
   x2       = x + ldexp(257.0, -38); // + 2^-30 + eps
   float f1 = x1.convert_to<float>();
   float f2 = x2.convert_to<float>();

   std::cout << std::setprecision(9) << f1 << std::endl;
   std::cout << std::setprecision(9) << f2 << std::endl;

   long_ext_float_t lf(1);
   lf += std::numeric_limits<long_ext_float_t>::epsilon();
   lf += std::numeric_limits<float>::epsilon() / 2;
   BOOST_MP_ASSERT(lf != 1);
   float f3 = lf.convert_to<float>();
   std::cout << std::setprecision(9) << f3 << std::endl;

   return (d1 == d2) && (f1 == f2) && (f3 != 1) ? 0 : 1;
}


#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/QR>
#include <Eigen/Householder>
#include <Eigen/SVD>
#include "operations.h"

using namespace std;
using namespace Eigen;

/* QR */
void QR::fwd(){
	MatrixXd Q = MatrixXd::Zero(nrows, ncols);
	VectorXd xc = VectorXd::Zero(nrows);

	int curr_row = 0;
	int i=0;
	for (auto e: c->edgesOut){
		if (e->A21 != nullptr){
			int s = A21_indices[i];
			xc.segment(curr_row, s) = e->n2->get_x()->segment(0, s);
			Q.middleRows(curr_row, s) = *(e->A21);
			curr_row += s;
			++i;
		}
	}

	larfb(&Q, c->get_T(), &xc);
	curr_row = 0;
	i=0;
	for (auto e: c->edgesOut){
		if (e->A21 != nullptr){
			int s = A21_indices[i];
			e->n2->get_x()->segment(0, s) = xc.segment(curr_row, s);
			curr_row += s;
			++i;
		}
	}
}

void QR::bwd(){
	MatrixXd R;
	int i=0;
	Segment xs = c->get_x()->segment(0, ccols);
	for (auto e: c->edgesIn){
		assert(e->A21 != nullptr);
		int s = A12_indices[i];
		assert(e->A21->cols() == s);
		xs -= (e->A21->topRows(ccols))*(e->n1->get_x()->segment(0, s));
		++i;
	}
	assert((*c->edgesOut.begin())->n2 == c);

	R = ((*c->edgesOut.begin())->A21->topRows(xs.size()));
	trsv(&R, &xs, CblasUpper, CblasNoTrans, CblasNonUnit);

}

/* Reassign rows */
void Reassign::fwd(){
	// cout << c->get_id() << " " << n->get_id() << endl;
	assert(indices.size() == nrows);
	for (int i= nstart; i < nstart+nrows; ++i){
		// n->get_x()->segment(i,1) = c->get_x()->segment(indices[i-nstart],1);
		(*n->get_x())[i] = (*c->get_x())[indices[i-nstart]];

	}

}

/* Scaling */
void Scale::bwd(){
    trsv(R, &xs, CblasUpper, CblasNoTrans, CblasNonUnit);
}

void ScaleD::fwd(){
	assert(t != nullptr);
	ormqr_trans(Q, t, &xsf);
}

void ScaleD::bwd(){
	MatrixXd R = Q->topRows(xs.size());
	trsv(&R, &xs, CblasUpper, CblasNoTrans, CblasNonUnit);
}

/* Sparsification using Orthogonal transformations */
void Orthogonal::bwd(){ormqr_notrans(V, tau, &xs);}

void OrthogonalD::fwd(){ormqr_trans(V, tau, &xs);}
void OrthogonalD::bwd(){ormqr_notrans(V, tau, &xs);}

/* Merging in the cluster heirarchy */
void Merge::fwd(){
	int k=0;
	for (auto c: parent->children){
		for (int i=0; i < c->rows(); ++i){
			(*parent->get_x())[k] = (*c->get_x())[i];
			++k;
		}
	}
	// assert(k== parent->get_x()->size());
}

void Merge::bwd(){
	int k=0;
	// cout << "merge bwd " << parent->get_id() << endl;
	for (auto c: parent->children){
		for (int i=0; i < c->cols(); ++i){
			(*c->get_x())[i] = (*parent->get_x())[k];
			++k;
		}
	}
	// assert(k== parent->get_x()->size());
}

/* Split between coarse and fine nodes */
void Split::fwd(){xsf = xsc.bottomRows(xsf.size());}
void Split::bwd(){xsc_head.bottomRows(xsf.size()) = xsf;}

void SplitD::fwd(){
	xsf = xsc.middleRows(rank, ccols-rank);
	VectorXd xsc_rem = xsc.bottomRows(crows-ccols);
	xsc.middleRows(rank, crows-ccols) = xsc_rem;
	xsc.bottomRows(ccols-rank) = xsf;
}
void SplitD::bwd(){
	VectorXd xsc_rem = xsc.middleRows(rank, crows-ccols);
	xsc.middleRows(rank, ccols-rank) = xsf;
	xsc.bottomRows(crows-ccols) = xsc_rem;
}


#ifndef __TARGET_FIELD_HPP__
#define __TARGET_FIELD_HPP__ 1

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

class TargetField {
 public:
  const double ignore = -666.666;

  class Target {
   public:
    Target(
      const double p_value,
      const std::pair<double, double> p_coordinates
    );

    ~Target();

    double value() const;

    std::pair<double, double> coordinates() const;

   private:
    double value_;
    std::pair<double, double> coordinates_;
  };

  TargetField(
    const double p_x_size,
    const double p_y_size,
    const int p_num_cells
  );

  ~TargetField();

  TargetField& add_target(const Target& p_target);

  std::vector<Target> get_targets() const;

  Eigen::MatrixXd as_cells() const;

  std::pair<double, double> coordinates_of_cell(
    const int p_cell_x,
    const int p_cell_y
  ) const;

 private:
  int num_cells_;
  double x_size_;
  double y_size_;
  std::vector<Target> targets_;
};

#endif //__TARGET_FIELD_HPP__


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#ifndef SOLVER_GUROBI_HPP
#define SOLVER_GUROBI_HPP
#include <Eigen/Dense>
#include "gurobi_c++.h"
#include <Eigen/StdVector>

#include <iomanip>  //set precision
#include "mader_types.hpp"
#include "utils.hpp"
#include "timer.hpp"
#include <decomp_geometry/polyhedron.h>  //For Polyhedron  and Hyperplane definition
#include "separator.hpp"
#include "octopus_search.hpp"
#include "solver_params.hpp"

typedef MADER_timers::Timer MyTimer;

class SolverGurobi
{
public:
  SolverGurobi(ms::par_solver &par);

  ~SolverGurobi();

  bool optimize();

  // setters
  void setMaxRuntimeKappaAndMu(double runtime, double kappa, double mu);
  bool setInitStateFinalStateInitTFinalT(mt::state initial_state, mt::state final_state, double t_init,
                                         double &t_final);
  void setHulls(mt::ConvexHullsOfCurves_Std &hulls);

  mt::trajectory traj_solution_;

  // getters
  void getPlanes(std::vector<Hyperplane3D> &planes);
  int getNumOfLPsRun();
  int getNumOfQCQPsRun();
  void getSolution(mt::PieceWisePol &solution);
  double getTimeNeeded();

  int B_SPLINE = 1;  // B-Spline Basis
  int MINVO = 2;     // Minimum volume basis
  int BEZIER = 3;    // Bezier basis

  bool checkGradientsUsingFiniteDiff();

protected:
private:
  bool getIntersectionWithPlane(const Eigen::Vector3d &P1, const Eigen::Vector3d &P2, const Eigen::Vector4d &coeff,
                                Eigen::Vector3d &intersection);

  void addObjective();
  void addConstraints();

  void saturateQ(std::vector<Eigen::Vector3d> &q);

  // transform functions (with Eigen)
  void transformPosBSpline2otherBasis(const Eigen::Matrix<double, 3, 4> &Qbs, Eigen::Matrix<double, 3, 4> &Qmv,
                                      int interval);
  void transformVelBSpline2otherBasis(const Eigen::Matrix<double, 3, 3> &Qbs, Eigen::Matrix<double, 3, 3> &Qmv,
                                      int interval);

  // transform functions (with std)
  void transformPosBSpline2otherBasis(const std::vector<std::vector<GRBLinExpr>> &Qbs,
                                      std::vector<std::vector<GRBLinExpr>> &Qmv, int interval);

  void transformVelBSpline2otherBasis(const std::vector<std::vector<GRBLinExpr>> &Qbs,
                                      std::vector<std::vector<GRBLinExpr>> &Qmv, int interval);

  void generateRandomGuess();
  bool generateAStarGuess();
  void generateStraightLineGuess();

  void printStd(const std::vector<Eigen::Vector3d> &v);
  void printStd(const std::vector<double> &v);
  void generateGuessNDFromQ(const std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n,
                            std::vector<double> &d);

  void fillPlanesFromNDQ(const std::vector<Eigen::Vector3d> &n, const std::vector<double> &d,
                         const std::vector<Eigen::Vector3d> &q);

  void generateRandomD(std::vector<double> &d);
  void generateRandomN(std::vector<Eigen::Vector3d> &n);
  void generateRandomQ(std::vector<Eigen::Vector3d> &q);

  void printQVA(const std::vector<Eigen::Vector3d> &q);

  void printQND(std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n, std::vector<double> &d);

  GRBEnv *env_ = new GRBEnv();
  GRBModel m_ = GRBModel(*env_);

  std::vector<std::vector<GRBVar>> q_var_;      // Each q_var_[i] has 3 elements (x,y,z)
  std::vector<std::vector<GRBLinExpr>> q_exp_;  // Each q_exp_[i] has 3 elements (x,y,z)

  std::vector<Eigen::Vector3d> n_;  // Each n_[i] has 3 elements (nx,ny,nz)
  std::vector<double> d_;           // d_[i] has 1 element

  void findCentroidHull(const mt::Polyhedron_Std &hull, Eigen::Vector3d &centroid);

  // void printIndexesConstraints();
  // void printIndexesVariables();

  mt::PieceWisePol solution_;

  int basis_ = B_SPLINE;

  int p_ = 5;
  int i_min_;
  int i_max_;
  int j_min_;
  int j_max_;
  int k_min_;
  int k_max_;
  int M_;
  int N_;

  int num_of_normals_;

  int num_of_obst_;
  int num_of_segments_;

  std::vector<Hyperplane3D> planes_;

  double dc_;
  Eigen::RowVectorXd knots_;
  double t_init_;
  double t_final_;
  double deltaT_;

  // double weight_ = 10000;
  double weight_modified_ = 10000;

  mt::state initial_state_;
  mt::state final_state_;

  Eigen::Vector3d q0_, q1_, q2_, qNm2_, qNm1_, qN_;

  mt::ConvexHullsOfCurves_Std hulls_;

  MyTimer opt_timer_;

  double max_runtime_ = 2;  //[seconds]

  // Guesses
  std::vector<Eigen::Vector3d> n_guess_;  // Guesses for the normals
  std::vector<Eigen::Vector3d> q_guess_;  // Guesses for the normals
  std::vector<double> d_guess_;           // Guesses for the normals

  double kappa_ = 0.2;  // kappa_*max_runtime_ is spent on the initial guess
  double mu_ = 0.5;     // mu_*max_runtime_ is spent on the optimization

  int num_of_QCQPs_run_ = 0;

  // transformation between the B-spline control points and other basis
  std::vector<Eigen::Matrix<double, 4, 4>> M_pos_bs2basis_;
  std::vector<Eigen::Matrix<double, 3, 3>> M_vel_bs2basis_;
  std::vector<Eigen::Matrix<double, 4, 4>> A_pos_bs_;

  separator::Separator *separator_solver_;
  OctopusSearch *octopusSolver_;

  ms::par_solver par_;

  GRBQuadExpr control_cost_ = 0.0;
  GRBQuadExpr terminal_cost_ = 0.0;
  GRBQuadExpr cost_ = 0.0;

  // double x_min_ = -std::numeric_limits<double>::max();
  // double x_max_ = std::numeric_limits<double>::max();

  // double y_min_ = -std::numeric_limits<double>::max();
  // double y_max_ = std::numeric_limits<double>::max();

  // double z_min_ = -std::numeric_limits<double>::max();
  // double z_max_ = std::numeric_limits<double>::max();

  // int deg_pol_ = 3;
  // int num_pol_ = 5;

  // int a_star_samp_x_ = 7;
  // int a_star_samp_y_ = 7;
  // int a_star_samp_z_ = 7;

  // double a_star_bias_ = 1.0;
  // double a_star_fraction_voxel_size_ = 0.5;
  // double Ra_ = 1e10;
};
#endif


////////////////////////////////////////////////////////////////////////////////
// TensionFieldEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  The tension-field-theory-based strain energy density used in Skouras 2014:
//  Designing Inflatable Structures.
//
//  This energy is implemented as a function of the right Green-Green
//  deformation tensor.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  04/04/2019 18:21:53
////////////////////////////////////////////////////////////////////////////////
#ifndef TENSIONFIELDENERGY_HH
#define TENSIONFIELDENERGY_HH

#include <Eigen/Dense>
#include <array>
#include "EigSensitivity.hh"
#include "IncompressibleBalloonEnergy.hh"

template<typename Real>
struct TensionFieldEnergy {
    using V2d  = Eigen::Matrix<Real, 2, 1>;
    using M2d  = Eigen::Matrix<Real, 2, 2>;

    TensionFieldEnergy() { }

    template<typename Derived>
    TensionFieldEnergy(const Eigen::MatrixBase<Derived> &C) { setMatrix(C); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &C) {
        m_balloonEnergy   .setMatrix(C);
        m_eigSensitivities.setMatrix(C);
        setEigs(m_eigSensitivities.lambda(0), m_eigSensitivities.lambda(1));
    }
    Real energy() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde)            return m_balloonEnergy.stiffness * (m_l1 + 2.0 * m_l2_tilde - 3.0);
        return m_balloonEnergy.energy();
    }

    Real denergy(const M2d &dC) const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde)            return m_balloonEnergy.stiffness * ((1.0 - m_l2_tilde * m_l2_tilde * m_l2_tilde) * m_eigSensitivities.dLambda(dC)[0]);
        return m_balloonEnergy.denergy(dC);
    }

    M2d denergy() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return M2d::Zero();
        if (m_l2 < m_l2_tilde)            return (m_balloonEnergy.stiffness * ((1.0 - m_l2_tilde * m_l2_tilde * m_l2_tilde))) * m_eigSensitivities.dLambda(0);
        return m_balloonEnergy.denergy();
    }

    Real d2energy(const M2d &dC_a, const M2d &dC_b) const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde) {
            Real inv_l1 = 1.0 / m_l1;
            return m_balloonEnergy.stiffness * ((1.0 - inv_l1 * m_l2_tilde)         * m_eigSensitivities.d2Lambda(dC_a, dC_b)[0]
                                               + 1.5 * inv_l1 * inv_l1 * m_l2_tilde * m_eigSensitivities. dLambda(0, dC_a)
                                                                                * m_eigSensitivities. dLambda(0, dC_b));
        }
        return m_balloonEnergy.d2energy(dC_a, dC_b);
    }

    M2d delta_denergy(const M2d &dC) const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return m_relaxedStiffnessEps * m_balloonEnergy.delta_denergy_undeformed(dC); // add a small artificial stiffness to avoid rank-deficient Hessian in fully compressed regions
        if (m_l2 < m_l2_tilde) {
            Real inv_l1 = 1.0 / m_l1;
            return m_balloonEnergy.stiffness * ((1.0 - inv_l1 * m_l2_tilde)         * m_eigSensitivities.delta_dLambda(0, dC)
                                              + (1.5 * inv_l1 * inv_l1 * m_l2_tilde * m_eigSensitivities.      dLambda(0, dC))
                                                                                * m_eigSensitivities.      dLambda(0)
                                              + (m_relaxedStiffnessEps * m_eigSensitivities.dLambda(1, dC)) // add a small artificial stiffness to avoid rank-deficient Hessian in regions of partial tension
                                                                       * m_eigSensitivities.dLambda(1));
        }
        return m_balloonEnergy.delta_denergy(dC);
    }

    size_t tensionState() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0; // compression in both directions
        if (m_l2 < m_l2_tilde)            return 1; //     tension in one  direction
        return 2;                                   //     tension in both directions
    }

    Real stiffness() const { return m_balloonEnergy.stiffness; }
    void setStiffness(Real val ) { m_balloonEnergy.stiffness = val; }

    // Note: the stiffness added in the relaxed case is also proportional to stiffness()!
    void setRelaxedStiffnessEpsilon(Real val) { m_relaxedStiffnessEps = val; }
    Real getRelaxedStiffnessEpsilon() const { return m_relaxedStiffnessEps; }

    const EigSensitivity<Real> &eigSensitivities() const { return m_eigSensitivities; }

    // Useful for visualizing/debugging the energy and its derivatives at
    // arbitrary (l1, l2)...
    void setEigs(Real l1, Real l2) {
        if (l1 < l2) std::swap(l1, l2);
        m_l1 = l1;
        m_l2 = l2;
        m_l2_tilde = 1.0 / std::sqrt(l1);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Expressions in terms of eigenvalues for debugging/developing smoothed
    // energy density.
    ////////////////////////////////////////////////////////////////////////////
    // Energy expressed in terms of eigenvalues.
    Real psi() const {
        const Real k = m_balloonEnergy.stiffness;
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde)            return k * (m_l1 + 2.0 * m_l2_tilde - 3.0);
        // full neo-Hookean energy in terms of eigenvalues
        return k * (m_l1 + m_l2 + 1.0 / (m_l1 * m_l2) - 3.0);
    }

    // Derivatives of energy with respect to eigenvalues l1, l2 (Note: these formulas can
    // not be used for computing denergy(dC) when l1 == l2 since the eigenvalues become
    // non-smooth functions of C in this case).
    V2d dpsi_dl() const {
        const Real k = m_balloonEnergy.stiffness;
        V2d result(V2d::Zero());
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return V2d::Zero();
        if (m_l2 < m_l2_tilde) return k * V2d(1.0 - m_l2_tilde * m_l2_tilde * m_l2_tilde, 0.0);
        return k * V2d(1.0 - 1.0 / (m_l1 * m_l1 * m_l2),
                       1.0 - 1.0 / (m_l1 * m_l2 * m_l2));
    }

    M2d d2psi_dl2() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return M2d::Zero();
        M2d result;
        const Real k = m_balloonEnergy.stiffness;
        const Real inv_l1 = 1.0 / m_l1;
        if (m_l2 < m_l2_tilde) {
            result << k * 1.5 * inv_l1 * inv_l1 * m_l2_tilde, 0.0, 0.0, 0.0;
        }
        else {
            result << 2.0 / (m_l1 * m_l1 * m_l1 * m_l2), 1.0 / (m_l1 * m_l1 * m_l2 * m_l2),
                      1.0 / (m_l1 * m_l1 * m_l2 * m_l2), 2.0 / (m_l1 * m_l2 * m_l2 * m_l2);
            result *= k;
        }
        return result;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
    IncompressibleBalloonEnergy<Real> m_balloonEnergy;
    EigSensitivity<Real>              m_eigSensitivities;
    Real m_l1 = 0.0, m_l2 = 0.0, m_l2_tilde = 0.0;
    bool useSmoothedTensionField = false;
    Real m_relaxedStiffnessEps = 1e-8;
};

// Optionally use either TensionFieldEnergy or IncompressibleBalloonEnergy
template<typename _Real>
struct OptionalTensionFieldEnergy : public TensionFieldEnergy<Real> {
    using Real = _Real;
    using TFE = TensionFieldEnergy<Real>;
    using V2d = typename TFE::V2d;
    using M2d = typename TFE::M2d;

    OptionalTensionFieldEnergy() { }

    template<typename Derived>
    OptionalTensionFieldEnergy(const Eigen::MatrixBase<Derived> &C) : TFE(C) { }

    Real energy() const {
        if (useTensionField) return TFE::energy();
        return TFE::m_balloonEnergy.energy();
    }

    Real denergy(const M2d &dC) const {
        if (useTensionField) return TFE::denergy(dC);
        return TFE::m_balloonEnergy.denergy(dC);
    }

    M2d denergy() const {
        if (useTensionField) return TFE::denergy();
        return TFE::m_balloonEnergy.denergy();
    }

    M2d delta_denergy(const M2d &dC) const {
        if (useTensionField) return TFE::delta_denergy(dC);
        return TFE::m_balloonEnergy.delta_denergy(dC);
    }

    Real d2energy(const M2d &dC_a, const M2d &dC_b) const {
        if (useTensionField) return TFE::d2energy(dC_a, dC_b);
        return TFE::m_balloonEnergy.d2energy(dC_a, dC_b);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Conform to the C-based energy interface
    // (So we can use the membrane energy density wrapper)
    ////////////////////////////////////////////////////////////////////////////
    OptionalTensionFieldEnergy(const OptionalTensionFieldEnergy &other, UninitializedDeformationTag &&) {
        copyMaterialProperties(other);
    }

    static constexpr EDensityType EDType = EDensityType::CBased;
    static constexpr size_t Dimension = 2;
    static constexpr size_t N         = 2;
    void setC(Eigen::Ref<const M2d> C) { setMatrix(C); }
    M2d PK2Stress() const { return 2.0 * denergy(); }

    template<class Mat_>
    M2d delta_PK2Stress(const Mat_ &dC) const { return 2.0 * delta_denergy(dC.matrix()); }

    template<class Mat_, class Mat2_>
    M2d delta2_PK2Stress(const Mat_ &/* dC_a */, const Mat2_ &/* dC_b */) const {
        throw std::runtime_error("Unimplemented");
    }

    bool useTensionField = true;

    void copyMaterialProperties(const OptionalTensionFieldEnergy &b) {
        setStiffness(b.stiffness());
        setRelaxationEnabled(b.getRelaxationEnabled());
        setRelaxedStiffnessEpsilon(b.getRelaxedStiffnessEpsilon());
    }
    bool getRelaxationEnabled() const { return useTensionField; }
    void setRelaxationEnabled(bool enable) { useTensionField = enable; }
};

#endif /* end of include guard: TENSIONFIELDENERGY_HH */


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::example_boost_party unit test

#include <libs/icl/test/disable_test_warnings.hpp>
#include "../unit_test_unwarned.hpp"
//#include <boost/icl/set.hpp> // Needed for implicit calls of operator << on
//JODO CLANG                   // GuestSets via test macros.

//------------------------------------------------------------------------------
// begin example code. return value added to function boost_party
//------------------------------------------------------------------------------
#include <boost/icl/ptime.hpp>
#include <iostream>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::icl;

// Type set<string> collects the names of party guests. Since std::set is
// a model of the itl's set concept, the concept provides an operator +=
// that performs a set union on overlap of intervals.
typedef std::set<string> GuestSetT;

interval_map<ptime, GuestSetT> boost_party()
{
    GuestSetT mary_harry;
    mary_harry.insert("Mary");
    mary_harry.insert("Harry");

    GuestSetT diana_susan;
    diana_susan.insert("Diana");
    diana_susan.insert("Susan");

    GuestSetT peter;
    peter.insert("Peter");

    // A party is an interval map that maps time intervals to sets of guests
    interval_map<ptime, GuestSetT> party;

    party.add( // add and element
      make_pair(
        interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        mary_harry));

    party += // element addition can also be done via operator +=
      make_pair(
        interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        diana_susan);

    party +=
      make_pair(
        interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        peter);

    interval_map<ptime, GuestSetT>::iterator it = party.begin();
    cout << "----- History of party guests -------------------------\n";
    while(it != party.end())
    {
        interval<ptime>::type when = it->first;
        // Who is at the party within the time interval 'when' ?
        GuestSetT who = (*it++).second;
        cout << when << ": " << who << endl;
    }

    return party;
}
//------------------------------------------------------------------------------
// end example code
//------------------------------------------------------------------------------

typedef interval_map<ptime, GuestSetT> PartyHistory;

typedef PartyHistory::segment_type SegmentT;

SegmentT episode(const char* from, const char* to, GuestSetT guests)
{
    return make_pair( interval<ptime>
                      ::right_open( time_from_string(from)
                                  , time_from_string(to)   )
                    , guests);
}

PartyHistory check_party()
{
    GuestSetT mary_harry;
    mary_harry.insert("Mary");
    mary_harry.insert("Harry");

    GuestSetT diana_susan;
    diana_susan.insert("Diana");
    diana_susan.insert("Susan");

    GuestSetT peter;
    peter.insert("Peter");

    GuestSetT Diana_Harry_Mary_Susan       = mary_harry + diana_susan;
    GuestSetT Diana_Harry_Mary_Peter_Susan = Diana_Harry_Mary_Susan + peter;
    GuestSetT Diana_Peter_Susan            = Diana_Harry_Mary_Peter_Susan - mary_harry;

    PartyHistory party;

    party += episode("2008-05-20 19:30", "2008-05-20 20:10", mary_harry);
    party += episode("2008-05-20 20:10", "2008-05-20 22:15", Diana_Harry_Mary_Susan);
    party += episode("2008-05-20 22:15", "2008-05-20 23:00", Diana_Harry_Mary_Peter_Susan);
    party += episode("2008-05-20 23:00", "2008-05-21 00:00", Diana_Peter_Susan);
    party += episode("2008-05-21 00:00", "2008-05-21 00:30", peter);

    return party;
}

BOOST_AUTO_TEST_CASE(icl_example_boost_party)
{
    PartyHistory party1 = boost_party();
    PartyHistory party2 = check_party();
    bool party_equality = (party1==party2);
    BOOST_CHECK(party_equality);
    //BOOST_CHECK_EQUAL(boost_party(), check_party());
}


#include <gtest/gtest.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <stdexcept>

#include "../filters/Filter/ParticleFilter/particlefilter.hh"
#include "helpers.hh"

using namespace std;
using namespace bold;
using namespace Eigen;

TEST (ParticleFilterTests, ESSCheck)
{
  // With uniform weights, ESS should be number of non-zero weights
  ESSFun<100> essFun;

  VectorXd weights(100);

  weights.fill(1.0 / 100);
  EXPECT_EQ(100, essFun.ESS(weights));

  weights.fill(0);
  weights.head<50>().fill(1.0 / 50);
  EXPECT_EQ(50, essFun.ESS(weights));
}

TEST (ParticleFilterTests, SystematicResample)
{
  constexpr int N = 6;
  SystematicResample<1, N> resample;
  MatrixXd particles = Matrix<double, 1, N>();
  for (unsigned i = 0; i < N; ++i)
    particles(i) = i;

  // With uniform weights, we should get the same samples
  VectorXd weights = Matrix<double, N, 1>::Constant(1.0 / N);
  auto newParticlesWeights = resample.resample(particles, weights, N);
  EXPECT_TRUE( MatricesEqual(particles, newParticlesWeights.first) );

  // Sampling half the samples should give either all odd or all even samples
  newParticlesWeights = resample.resample(particles, weights, N / 2);
  for (unsigned i = 0; i < N / 2; ++i)
    EXPECT_TRUE( newParticlesWeights.first(i) == particles(2 * i) || newParticlesWeights.first(i) == particles(2 * i + 1) );

  // A lone particle with non-zero weight should be sampled always
  weights.fill(0);
  weights(0) = 1;
  newParticlesWeights = resample.resample(particles, weights, N);
  for (unsigned i = 0 ; i < N; ++i)
    EXPECT_TRUE( VectorsEqual(VectorXd(newParticlesWeights.first.col(i)), VectorXd(particles.col(0))) );

  weights.fill(0);
  weights(N / 2) = 1;
  newParticlesWeights = resample.resample(particles, weights, N);
  for (unsigned i = 0 ; i < N; ++i)
    EXPECT_TRUE( VectorsEqual(VectorXd(newParticlesWeights.first.col(i)), VectorXd(particles.col(N / 2))) );

  weights.fill(0);
  weights(N - 1) = 1;
  newParticlesWeights = resample.resample(particles, weights, N);
  for (unsigned i = 0 ; i < N; ++i)
    EXPECT_TRUE( VectorsEqual(VectorXd(newParticlesWeights.first.col(i)), VectorXd(particles.col(N - 1))) );

  // A particle with twice the weight should be sampled twice as often
  // (in this test case where N is even, and weights are larger than 1.0 / N)
  weights.fill(0);
  weights(0) = 1.0 / 3.0;
  weights(1) = 2.0 / 3.0;
  newParticlesWeights = resample.resample(particles, weights, N);
  unsigned count1 = (newParticlesWeights.first.array() == 0).count();
  unsigned count2 = (newParticlesWeights.first.array() == 1).count();
  EXPECT_EQ( count2, 2 * count1 );
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 trigonometric toolbox - rem_pio2_straight/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of trigonometric components in scalar mode
//////////////////////////////////////////////////////////////////////////////
///
#include <nt2/toolbox/trigonometric/include/functions/rem_pio2_straight.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/toolbox/constant/constant.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/type_traits/is_same.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( rem_pio2_straight_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::rem_pio2_straight;
  using nt2::tag::rem_pio2_straight_;
  typedef typename nt2::meta::call<rem_pio2_straight_(T)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  typedef typename nt2::meta::call<rem_pio2_straight_(T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;

  // specific values tests
  typedef typename boost::fusion::result_of::value_at_c<r_t,0>::type r_t0;
  typedef typename boost::fusion::result_of::value_at_c<r_t,1>::type r_t1;
  typedef typename boost::fusion::result_of::value_at_c<r_t,2>::type r_t2;
  {
    r_t res = rem_pio2_straight(nt2::Pio_2<T>());
    NT2_TEST_ULP_EQUAL( boost::fusion::get<0>(res), nt2::Zero<r_t0>(), 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<1>(res), nt2::Zero<r_t1>(), 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<2>(res), nt2::One<r_t2>(), 0.5);
  }
  {
    r_t res = rem_pio2_straight(nt2::Pio_4<T>());
    NT2_TEST_ULP_EQUAL( boost::fusion::get<0>(res), -nt2::Pio_4<r_t0>(), 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<1>(res), nt2::Zero<r_t1>(), 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<2>(res), nt2::One<r_t2>(), 0.5);
  }
  {
    r_t res = rem_pio2_straight(nt2::Zero<T>());
    NT2_TEST_ULP_EQUAL( boost::fusion::get<0>(res), -nt2::Pio_2<r_t0>(), 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<1>(res), nt2::Zero<r_t1>(), 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<2>(res), nt2::One<r_t2>(), 0.5);
  }
} // end of test for floating_


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/at_c.hpp>
#include <fcppt/math/dim/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

BOOST_AUTO_TEST_CASE(
	math_at_c
)
{
	typedef
	fcppt::math::dim::static_<
		int,
		2
	>
	dim2;

	dim2 const dim_c(
		1,
		2
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::at_c<
			0
		>(
			dim_c
		),
		1
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::at_c<
			1
		>(
			dim_c
		),
		2
	);

	dim2 dim_m(
		1,
		2
	);

	fcppt::math::at_c<
		1
	>(
		dim_m
	) =
		42;

	BOOST_CHECK_EQUAL(
		fcppt::math::at_c<
			1
		>(
			dim_m
		),
		42
	);
}