#include "tbb/parallel_for.h"
#include <iostream>
#include <mex.h>
#include <omp.h>
#include <vector>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
// #include <Eigen/StdVector>

// #include <range/v3/all.hpp>
// #include <range/v3/core.hpp>

// #include "storage_aliases.h"

// #define ngp int(*mxGetPr(prhs[0]))
#define input prhs[0]
#define output plhs[0]

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  if (!(mxIsStruct(prhs[0]) && mxGetNumberOfFields(prhs[0]) > 0 && (nlhs > 0) &&
        (nrhs > 0))) {
    mexErrMsgIdAndTxt("MATLAB:mexcpp:typeargin",
                      "First argument has to be double scalar.");
    return;
  }

  auto eps_p_bar = mxGetField(prhs[0], 0, "eps_p_bar");
  auto sigma_bar = mxGetField(prhs[0], 0, "sigma_bar");
  auto Hdo = mxGetField(prhs[0], 0, "Hdo");
  auto inv_Hdo = mxGetField(prhs[0], 0, "iHdo");
  auto delta_in = mxGetField(prhs[0], 0, "delta");
  // auto DG_proj = mxGetField(prhs[0], 0, "DG_proj");
  // auto D_operator = mxGetField(prhs[0], 0, "D_operator");
  // auto ntp_in = mxGetField(prhs[0], 0, "ntp");
  auto dt = mxGetField(prhs[0], 0, "dt");
  auto I_g = mxGetField(prhs[0], 0, "I_g");
  auto sgn = mxGetField(prhs[0], 0, "sign");
  auto sign = int(*mxGetPr(sgn));
  auto d_t = double(*mxGetPr(dt));

  auto Hdo_Dimensions = mxGetDimensions(Hdo);
  auto sigma_Dimensions = mxGetDimensions(sigma_bar);
  size_t const ncomp = 6; // TODO 3D only
  if (Hdo_Dimensions[0] != 6)
    mexErrMsgIdAndTxt("MATLAB:mexcpp:nargin", "3D only, ncomp should equal 6.");
  size_t const ngp = Hdo_Dimensions[2];
  size_t const ntp = Hdo_Dimensions[3];
  size_t const nmodes = sigma_Dimensions[1];

  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;
  using matrix6 = Eigen::Matrix<double, 6, 6>;

  // Eigen::Map<Vector> Hdo_v(mxGetPr(Hdo), ntp * ngp * ncomp * ncomp);
  // Eigen::Map<Vector> inv_Hdo_v(mxGetPr(inv_Hdo), ntp * ngp * ncomp * ncomp);
  // Eigen::Map<Vector> delta_v(mxGetPr(delta_in), ntp * ngp * ncomp);
  // Eigen::Map<Vector> sigma_v(mxGetPr(sigma_bar), ngp * ncomp * nmodes);
  // Eigen::Map<Vector> eps_p_v(mxGetPr(eps_p_bar), ngp * ncomp * nmodes);

  // clang-format off
  // time_matrix6_storage H(ntp, gauss_matrix6_storage(ngp, matrix6_storage()));
  // time_matrix6_storage iH(ntp, gauss_matrix6_storage(ngp, matrix6_storage()));
  // time_vector_storage delta(ntp, gauss_vector_storage(ngp, vector_storage(ncomp)));
  // gauss_matrix_storage sigma(ngp, matrix_storage(ncomp, nmodes));
  // gauss_matrix_storage eps_p(ngp, matrix_storage(ncomp, nmodes));
  // clang-format on

  auto const tensor_shift = ncomp * ncomp;
  auto const vector_shift = ncomp * nmodes;
  auto const delta_shift = ncomp;

  // using sparse_matrix = Eigen::SparseMatrix<double, Eigen::RowMajor>;
  // sparse_matrix A(nmodes * ntp, nmodes * ntp);
  // A.reserve(2 * nmodes * nmodes * ntp - (2 * nmodes - 1));
  // A.coeffs() = 0.0;
  Matrix A = Matrix::Zero(nmodes * ntp, nmodes * ntp);
  Vector b = Vector::Zero(ntp * nmodes);

  auto const nel = ntp - 1;

  // TODO
  // std::vector<std::int32_t> dofs(nel);
  // std::generate(begin(dofs), end(dofs), [j = -nmodes] () mutable {
  //      j += nmodes;
  //      return j;
  // });

  std::vector<std::int32_t> dofs;
  for (size_t j = 0; j < nel * nmodes; j += nmodes)
    dofs.push_back(j);

  std::vector<std::int32_t> fixed_dofs;
  for (size_t j = 0; j < nmodes; j++)
    fixed_dofs.push_back(j);

    // how come I don't have race condition here? TODO
    // clang-format off
// #pragma omp parallel for collapse(2)
#pragma omp declare reduction(+ : Eigen::MatrixXd : omp_out =  omp_out + omp_in)
#pragma omp declare reduction(+ : Eigen::VectorXd : omp_out =  omp_out + omp_in)

#pragma omp parallel for
  for (size_t i = 0; i < ntp; i++) {
    Matrix a11 = Matrix::Zero(nmodes, nmodes);
    Matrix a10 = Matrix::Zero(nmodes, nmodes);
    Matrix a00 = Matrix::Zero(nmodes, nmodes);
    Vector d1 = Vector::Zero(nmodes);
    Vector d0 = Vector::Zero(nmodes);

// #pragma omp parallel for reduction(+ : a11,a10,a00,d1,d0)
// #pragma omp parallel for
    for (size_t j = 0; j < ngp; j++) {
      int const int_idx = j * tensor_shift;
      int const tensor_idx = (i * ngp + j) * tensor_shift;
      int const delta_idx = (i * ngp + j) * delta_shift;
      int const vector_idx = j * vector_shift;

      //Hdo_v.segment(tensor_idx, tensor_shift);
      // Eigen::Map<matrix6> Int_space(mxGetPr(I_g));
      matrix6 Int_space = Eigen::Map<matrix6>(mxGetPr(I_g) + int_idx, ncomp, ncomp);
      matrix6 H = Eigen::Map<matrix6>(mxGetPr(Hdo) + tensor_idx, ncomp, ncomp);
      matrix6 iH = Eigen::Map<matrix6>(mxGetPr(inv_Hdo) + tensor_idx);
      Vector delta = Eigen::Map<Vector>(mxGetPr(delta_in) + delta_idx, ncomp);
      Matrix sigma = Eigen::Map<Matrix>(mxGetPr(sigma_bar) + vector_idx, ncomp,nmodes);
      Matrix eps_p = Eigen::Map<Matrix>(mxGetPr(eps_p_bar) + vector_idx, ncomp,nmodes);

      a11.noalias() += eps_p.transpose() * (iH * (Int_space * eps_p)); // TODO noalias
      a10 += -1.0 * sign * eps_p.transpose() * Int_space * sigma;
      a00 += sigma.transpose() * (H * (Int_space * sigma)); // TODO I_g is the same for all elements? shouldn't be
      d1 += -1.0 * sign * eps_p.transpose() * (iH * (Int_space * delta));
      d0 += sigma.transpose() * Int_space * delta;
    }

    Matrix t11 = a11 / (d_t * d_t); // TODO use cwiseQuotient
    Matrix t12 = a10 / (d_t);       //   a01 = a10;
    Matrix t22 = t11 + t12 + t12 + a00;
    t12 = -t12 - t11;
    Vector b0 = -d1 / (d_t);
    Vector b1 = d1 / (d_t) + d0;

    if (i>0) { //for (size_t i = 0; i < nel; i++) {
      Matrix a_el(2 * nmodes, 2 * nmodes);
      Vector b_el(2 * nmodes);

      a_el << t11, t12,
              t12, t22;

      b_el << b0,
              b1; // TODO use I_time

//       for (size_t j = 0; j < 2 * nmodes; j++) {
//         for (size_t k = 0; k < 2 * nmodes; k++) {
// #pragma omp critical
//           A.coeffRef(dofs[i-1]+j, dofs[i-1]+k) += a_el(j,k);
//         }
//       }
//       for (size_t j = 0; j < 2 * nmodes; j++) {
// #pragma omp atomic
//           b.coeffRef(dofs[i-1]+j) += b_el(j);
//       }

#pragma omp critical
{
      A.block(dofs[i-1], dofs[i-1], 2 * nmodes, 2 * nmodes) += a_el; // TODO this assembly is not final
      b.segment(dofs[i-1], 2 * nmodes) += b_el;
}
    }
  }
  // clang-format on

  for (auto const fixed_dof : fixed_dofs) {
    auto const diagonal_entry = A.coeff(fixed_dof, fixed_dof); // TODO // sparse

    b(fixed_dof) = 0.0;

    std::vector<std::int64_t> non_zero_visitor;

    // // Zero the rows and columns
    // for (sparse_matrix::InnerIterator it(A, fixed_dof); it; ++it) {
    //   // Set the value of the col or row resp. to zero
    //   it.valueRef() = 0.0;
    //   non_zero_visitor.push_back(A.IsRowMajor ? it.col() : it.row());
    // }
    // // Zero the row or col respectively
    // for (auto const &non_zero : non_zero_visitor) {
    //   const auto row = A.IsRowMajor ? non_zero : fixed_dof;
    //   const auto col = A.IsRowMajor ? fixed_dof : non_zero;
    //
    //   A.coeffRef(row, col) = 0.0;
    // }
    for (int i = 0; i < A.rows(); i++) {
      for (int j = 0; j < A.cols(); j++) {
        A.coeffRef(fixed_dof, j) = 0.0;
        A.coeffRef(i, fixed_dof) = 0.0;
      }
    }
    // Reset the diagonal to the same value to preserve conditioning
    A.coeffRef(fixed_dof, fixed_dof) = diagonal_entry;
  }

  // TODO //Vector x = (b.norm() > 0.0) ? A.partialPivLu().solve(b)

  Vector x;
  if (b.norm() > 0)
    x = A.partialPivLu().solve(b); // fullPivLu
  else {
    x = b;
  }
  // Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> chol(
  //     A); // performs a Cholesky factorization of A
  // Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> chol(
  //     A); // performs a Cholesky factorization of A
  // Eigen::VectorXd x = chol.solve(
  //     b); // use the factorization to solve for the given right hand side

  output = mxCreateDoubleMatrix(nmodes, ntp, mxREAL);
  auto out_pt = mxGetPr(output);
  Eigen::Map<Eigen::MatrixXd> mapp(out_pt, nmodes, ntp);
  mapp = x; // copy
  // std::copy(x.begin(), x.end(), mxGetPr(output));
  return;
}

// mxArray *cpp_to_MexArray(const std::vector<double> &v) {
//   mxArray *mx = mxCreateDoubleMatrix(1, v.size(), mxREAL);
//   std::copy(v.begin(), v.end(), mxGetPr(mx));
//
//   return mx;
// }


#include <LBFGSB.h>
#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include <MathUtil.h>
#include <fstream>
#include <iomanip>

#include <nlopt.hpp>
#include <Pluckertree.h>
#include <PluckertreeSegments.h>

using namespace LBFGSpp;

using Vector = Eigen::VectorXd;

class FixedMomentMinDist
{
private:
    using number_t = double;
    using Vector3_t = Eigen::Vector3d;
    using Vector2_t = Eigen::Vector2d;
    using Matrix3_t = Eigen::Matrix3d;

    Vector3_t q;
    Vector3_t h1;
    Vector3_t h2;
public:
    FixedMomentMinDist(Vector3_t q, Vector3_t dirLowerBound, Vector3_t dirUpperBound)
            : q(std::move(q)), h1(std::move(dirLowerBound)), h2(std::move(dirUpperBound)) {}

    double operator()(const Vector& x, Vector& grad)
     {
        // Careful, don't disturb the big pile of math!
        number_t phi_k = x[0];
        number_t theta_k = x[1];
        number_t r_k = x[2];

        // f(k)

        Vector3_t k = spherical2cart(phi_k, theta_k, r_k);
        Vector3_t kd = k / r_k;
        Vector3_t qp = q - kd*(q.dot(kd));
        Vector3_t qpd = qp.normalized();
        auto u = (qp - q).norm();

        auto sin_gamma = std::min(r_k/qp.norm(), (number_t)1.0f);
        auto cos_gamma = std::sqrt(1 - sin_gamma * sin_gamma);

        Vector3_t da = qpd * cos_gamma + kd.cross(qpd) * sin_gamma;
        Vector3_t d_alpha;
        auto da_dot_h1 = da.dot(h1);
        auto da_dot_h2 = da.dot(h2);
        if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
        {
            d_alpha = da;
        }else if(da_dot_h1 <= da_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_alpha = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_alpha = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        Vector3_t db = - qpd * cos_gamma + kd.cross(qpd) * sin_gamma;
        Vector3_t d_beta;
        auto db_dot_h1 = db.dot(h1);
        auto db_dot_h2 = db.dot(h2);
        if(db_dot_h1 >= 0 && db_dot_h2 >= 0)
        {
            d_beta = db;
        }else if(db_dot_h1 < db_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_beta = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_beta = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        auto va = (qp.cross(d_alpha) - k).norm();
        auto vb = (qp.cross(d_beta) - k).norm();
        auto v = std::min(va, vb);

        auto f = std::sqrt(u*u + v*v);

        // partial
        /*auto sin_theta_k = std::sin(theta_k);
        auto pow2_sin_theta_k = sin_theta_k * sin_theta_k;
        auto sin_2theta_k = std::sin(2*theta_k);

        auto cos_theta_k = std::cos(theta_k);
        auto cos_2theta_k = std::cos(2*theta_k);
        auto pow2_cos_theta_k = cos_theta_k * cos_theta_k;

        auto sin_phi_k = std::sin(phi_k);
        auto pow2_sin_phi_k = sin_phi_k * sin_phi_k;
        auto sin_2phi_k = std::sin(2*phi_k);

        auto cos_phi_k = std::cos(phi_k);
        auto cos_2phi_k = std::cos(2*phi_k);
        auto pow2_cos_phi_k = cos_phi_k * cos_phi_k;

        ///
        auto partial_k_phi_k = r_k * Vector3_t(sin_theta_k * (-sin_phi_k), sin_theta_k * cos_phi_k, cos_theta_k);
        auto partial_k_theta_k = r_k * Vector3_t(cos_theta_k * cos_phi_k, cos_theta_k * sin_phi_k, (-sin_theta_k));
        auto partial_k_r_k = Vector3_t(sin_theta_k * cos_phi_k, sin_theta_k * sin_phi_k, cos_theta_k);

        Matrix3_t partial_k;
        partial_k.row(0) = partial_k_phi_k;
        partial_k.row(1) = partial_k_theta_k;
        partial_k.row(2) = partial_k_r_k;
        ///

        auto k_norm = k.norm();
        Matrix3_t partial_kd;
        for(int i = 0; i < 3; ++i)
        {
            partial_kd.row(i) = (partial_k.row(i)/k_norm) - k.transpose() * ((-1/(std::pow(k_norm, 3))) * (k.x() * partial_k.row(i).x() + k.y() * partial_k.row(i).y() + k.z() * partial_k.row(i).z()));
        }

        ///
        Matrix3_t partial_qp;
        for(int i = 0; i < 3; ++i)
        {
            partial_qp.row(i).x() = q.x() * 2 * kd.x() * partial_kd.row(i).x();
            partial_qp.row(i).x() += q.y() * (partial_kd.row(i).x() * kd.y() + kd.x() * partial_kd.row(i).y());
            partial_qp.row(i).x() += q.z() * (partial_kd.row(i).x() * kd.z() + kd.x() * partial_kd.row(i).z());

            partial_qp.row(i).y() = q.x() * (partial_kd.row(i).y() * kd.x() + kd.y() * partial_kd.row(i).x());
            partial_qp.row(i).y() += q.y() * 2 * kd.y() * partial_kd.row(i).y();
            partial_qp.row(i).y() += q.z() * (partial_kd.row(i).y() * kd.z() + kd.y() * partial_kd.row(i).z());

            partial_qp.row(i).z() = q.x() * (partial_kd.row(i).z() * kd.x() + kd.z() * partial_kd.row(i).x());
            partial_qp.row(i).z() += q.y() * (partial_kd.row(i).z() * kd.y() + kd.z() * partial_kd.row(i).y());
            partial_qp.row(i).z() += q.z() * 2 * kd.z() * partial_kd.row(i).z();

            partial_qp.row(i) *= -1;
        }
        ///

        //auto partial_pow2_u_phi_k = 2*(qp.x() - q.x()) * partial_qp_phi_k.x() + 2*(qp.y() - q.y()) * partial_qp_phi_k.y() + 2*(qp.z() - q.z()) * partial_qp_phi_k.z();
        auto partial_pow2_u = 2 * (partial_qp * (qp - q));

        auto da_xy_norm = Eigen::Vector2f(da.x(), da.y()).norm();

        auto qp_norm = qp.norm();
        //double partial_delta_phi_k = (1/qp_norm) * (qp.x() * partial_qp_phi_k.x() + qp.y() * partial_qp_phi_k.y() + qp.z() * partial_qp_phi_k.z());
        Vector3_t partial_delta;
        for(int i = 0; i < 3; i++)
        {
            partial_delta[i] = (1/qp_norm) * (qp.x() * partial_qp.row(i).x() + qp.y() * partial_qp.row(i).y() + qp.z() * partial_qp.row(i).z());
        }

        Matrix3_t partial_qpd;
        for(int i = 0; i < 3; i++)
        {
            partial_qpd.row(i) = (partial_qp.row(i)/qp_norm) + qp.transpose() * (-1/qp.squaredNorm()) * partial_delta[i];
        }

        Vector3_t partial_sin_gamma(0, 0, 0);
        //double partial_sin_gamma_phi_k = 1;
        if(r_k/qp_norm > 1)
        {
            for(int i = 0; i < 3; i++)
            {
                partial_sin_gamma[i] = -(r_k/qp.squaredNorm()) * partial_delta[i]; //TODO: is this wrong for i=2?
                //partial_sin_gamma_phi_k = -(r_k/qp.squaredNorm()) * partial_delta_phi_k;
            }
        }

        Vector3_t partial_cos_gamma;
        for(int i = 0; i < 3; i++)
        {
            //TODO
            partial_cos_gamma[i] = (1/std::sqrt(1-sin_gamma*sin_gamma)) * (-sin_gamma * partial_sin_gamma[i]);
        }

        Matrix3_t partial_da = Matrix3_t::Zero();
        float sign = 1.0f;
        if(va >= vb)
        {
            sign = -1.0f; //partial_db
        }

        for(int i = 0; i < 3; i++)
        {
            partial_da.row(i).x() += sign * partial_qpd.row(i).x() * cos_gamma + qpd.x() * partial_cos_gamma[i];
            partial_da.row(i).y() += sign * partial_qpd.row(i).y() * cos_gamma + qpd.x() * partial_cos_gamma[i];
            partial_da.row(i).z() += sign * partial_qpd.row(i).z() * cos_gamma + qpd.z() * partial_cos_gamma[i];

            partial_da.row(i).x() += partial_kd.row(i).y() * qpd.z() * sin_gamma + kd.y() * partial_qpd.row(i).z() * sin_gamma + kd.y() * qpd.z() * partial_sin_gamma[i];
            partial_da.row(i).y() += partial_kd.row(i).x() * qpd.z() * sin_gamma + kd.x() * partial_qpd.row(i).z() * sin_gamma + kd.x() * qpd.z() * partial_sin_gamma[i];
            partial_da.row(i).z() += partial_kd.row(i).x() * qpd.y() * sin_gamma + kd.x() * partial_qpd.row(i).y() * sin_gamma + kd.x() * qpd.y() * partial_sin_gamma[i];

            partial_da.row(i).x() += partial_kd.row(i).z() * qpd.y() * sin_gamma + kd.z() * partial_qpd.row(i).y() * sin_gamma + kd.z() * qpd.y() * partial_sin_gamma[i];
            partial_da.row(i).y() += partial_kd.row(i).z() * qpd.x() * sin_gamma + kd.z() * partial_qpd.row(i).x() * sin_gamma + kd.z() * qpd.x() * partial_sin_gamma[i];
            partial_da.row(i).z() += partial_kd.row(i).y() * qpd.x() * sin_gamma + kd.y() * partial_qpd.row(i).x() * sin_gamma + kd.y() * qpd.x() * partial_sin_gamma[i];
        }

        Matrix3_t partial_d_alpha; //also partial_d_beta, depending on va >= vb
        if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
        {
            partial_d_alpha = partial_da;
        }else if(da_dot_h1 < da_dot_h2 || (da_dot_h1 == da_dot_h2 && va < vb))
        {
            for(int i = 0; i < 3; i++)
            {
                auto h1_cross_k_sqr_norm = h1.cross(k).squaredNorm();
                auto h1_cross_k_norm = std::sqrt(h1_cross_k_sqr_norm);

                auto partial_h1_cross_k_norm = (h1.y()*k.z() - h1.z()*k.y())*(h1.y()*partial_k.row(i).z() - h1.z()*partial_k.row(i).y());
                partial_h1_cross_k_norm += (h1.x()*k.z() - h1.z()*k.x())*(h1.x()*partial_k.row(i).z() - h1.z()*partial_k.row(i).x());
                partial_h1_cross_k_norm += (h1.x()*k.y() - h1.y()*k.x())*(h1.x()*partial_k.row(i).y() - h1.y()*partial_k.row(i).x());
                partial_h1_cross_k_norm *= (1.0/h1_cross_k_norm);

                auto fact = std::copysign(1.0, k.dot(h2.cross(h1))) / h1_cross_k_sqr_norm;

                partial_d_alpha.row(i).x() = h1_cross_k_norm * (h1.y() * partial_k.row(i).z() - h1.z() * partial_k.row(i).y());
                partial_d_alpha.row(i).x() -= partial_h1_cross_k_norm * (h1.y() * k.z() - h1.z() * k.y());

                partial_d_alpha.row(i).y() = h1_cross_k_norm * (h1.x() * partial_k.row(i).z() - h1.z() * partial_k.row(i).x());
                partial_d_alpha.row(i).y() -= partial_h1_cross_k_norm * (h1.x() * k.z() - h1.z() * k.x());

                partial_d_alpha.row(i).z() = h1_cross_k_norm * (h1.x() * partial_k.row(i).y() - h1.y() * partial_k.row(i).x());
                partial_d_alpha.row(i).z() -= partial_h1_cross_k_norm * (h1.x() * k.y() - h1.y() * k.x());

                partial_d_alpha.row(i) *= fact;
            }
        }else
        {
            for(int i = 0; i < 3; i++)
            {
                auto h2_cross_k_sqr_norm = h2.cross(k).squaredNorm();
                auto h2_cross_k_norm = std::sqrt(h2_cross_k_sqr_norm);

                auto partial_h2_cross_k_norm = (h1.y()*k.z() - h2.z()*k.y())*(h2.y()*partial_k.row(i).z() - h2.z()*partial_k.row(i).y());
                partial_h2_cross_k_norm += (h2.x()*k.z() - h2.z()*k.x())*(h2.x()*partial_k.row(i).z() - h2.z()*partial_k.row(i).x());
                partial_h2_cross_k_norm += (h2.x()*k.y() - h2.y()*k.x())*(h2.x()*partial_k.row(i).y() - h2.y()*partial_k.row(i).x());
                partial_h2_cross_k_norm *= (1.0/h2_cross_k_norm);

                auto fact = std::copysign(1.0, k.dot(h2.cross(h1))) / h2_cross_k_sqr_norm;

                partial_d_alpha.row(i).x() = h2_cross_k_norm * (h2.y() * partial_k.row(i).z() - h2.z() * partial_k.row(i).y());
                partial_d_alpha.row(i).x() -= partial_h2_cross_k_norm * (h2.y() * k.z() - h2.z() * k.y());

                partial_d_alpha.row(i).y() = h2_cross_k_norm * (h2.x() * partial_k.row(i).z() - h2.z() * partial_k.row(i).x());
                partial_d_alpha.row(i).y() -= partial_h2_cross_k_norm * (h2.x() * k.z() - h2.z() * k.x());

                partial_d_alpha.row(i).z() = h2_cross_k_norm * (h2.x() * partial_k.row(i).y() - h2.y() * partial_k.row(i).x());
                partial_d_alpha.row(i).z() -= partial_h2_cross_k_norm * (h2.x() * k.y() - h2.y() * k.x());

                partial_d_alpha.row(i) *= fact;
            }
        }

        ////

        auto c_alpha = qp.cross(da);
        Matrix3_t partial_c_alpha;
        for(int i = 0; i < 3; i++)
        {
            partial_c_alpha.row(i) = Vector3_t(
                    (partial_qp.row(i).y()*d_alpha.z() + qp.y() * partial_d_alpha.row(i).z()) - (partial_qp.row(i).z()*d_alpha.y() + qp.z()*partial_d_alpha.row(i).y()),
                    (partial_qp.row(i).x()*d_alpha.z() + qp.x() * partial_d_alpha.row(i).z()) - (partial_qp.row(i).z()*d_alpha.x() + qp.z()*partial_d_alpha.row(i).x()),
                    (partial_qp.row(i).x()*d_alpha.y() + qp.x() * partial_d_alpha.row(i).y()) - (partial_qp.row(i).y()*d_alpha.x() + qp.y()*partial_d_alpha.row(i).x())
            );
        }

        Vector3_t partial_v;
        for(int i = 0; i < 3; i++)
        {
            partial_v[i] = (1.0/va) * (
                    (c_alpha.x() - k.x()) * (partial_c_alpha.row(i).x() - partial_k.row(i).x()) +
                    (c_alpha.y() - k.y()) * (partial_c_alpha.row(i).y() - partial_k.row(i).y()) +
                    (c_alpha.z() - k.z()) * (partial_c_alpha.row(i).z() - partial_k.row(i).z())
            );
        }

        Vector3_t partial_pow2_v = 2*va*partial_v;

        grad = (1.0/(2.0*std::sqrt(u*u + v*v))) * (partial_pow2_u + partial_pow2_v);
        /*assert(!grad.hasNaN());
        assert(!std::isinf(grad[0]));
        assert(!std::isinf(grad[1]));
        assert(!std::isinf(grad[2]));*/

        return f;
    }
};

class FixedMomentMinHitDist
{
private:
    using number_t = double;
    using Vector3_t = Eigen::Vector3d;
    using Vector2_t = Eigen::Vector2d;
    using Matrix3_t = Eigen::Matrix3d;

    Vector3_t q;
    Vector3_t q_n;
    Vector3_t h1;
    Vector3_t h2;
public:
    FixedMomentMinHitDist(Vector3_t q, Vector3_t q_n, Vector3_t dirLowerBound, Vector3_t dirUpperBound)
            : q(std::move(q)), q_n(std::move(q_n)), h1(std::move(dirLowerBound)), h2(std::move(dirUpperBound)) {}

    double operator()(const Vector& x, Vector& grad)
    {
        // Careful, don't disturb the big pile of math!
        number_t phi_k = x[0];
        number_t theta_k = x[1];
        number_t r_k = x[2];

        // f(k)

        Vector3_t k = spherical2cart(phi_k, theta_k, r_k);
        Vector3_t kd = k.normalized();

        //Find intersection line of directionvector plane with query plane
        Vector3_t kd_cross_qn = kd.cross(q_n);
        auto kd_cross_qn_norm = kd_cross_qn.norm();
        if(kd_cross_qn_norm < 1e-6) // All lines are parallel to the query plane, assume all miss.
        {
            return std::numeric_limits<double>::infinity();
        }
        Vector3_t isect_d = kd_cross_qn / kd_cross_qn_norm;
        Vector3_t isect_k = kd * (q_n.dot(q)/kd_cross_qn_norm);
        Vector3_t isect_p = isect_d.cross(isect_k);
        Vector3_t qpl = isect_p + isect_d * isect_d.dot(q - isect_p);
        Vector3_t qpld = qpl.normalized();

        auto sin_gamma = std::min(r_k/qpl.norm(), (number_t)1.0f);
        auto cos_gamma = std::sqrt(1 - sin_gamma * sin_gamma);

        Vector3_t da = qpld * cos_gamma + kd.cross(qpld) * sin_gamma;
        Vector3_t d_alpha;
        auto da_dot_h1 = da.dot(h1);
        auto da_dot_h2 = da.dot(h2);
        if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
        {
            d_alpha = da;
        }else if(da_dot_h1 <= da_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_alpha = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_alpha = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        Vector3_t db = - qpld * cos_gamma + kd.cross(qpld) * sin_gamma;
        Vector3_t d_beta;
        auto db_dot_h1 = db.dot(h1);
        auto db_dot_h2 = db.dot(h2);
        if(db_dot_h1 >= 0 && db_dot_h2 >= 0)
        {
            d_beta = db;
        }else if(db_dot_h1 < db_dot_h2)
        {
            Vector h1_cross_k = h1.cross(k);
            d_beta = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
        }else
        {
            Vector h2_cross_k = h2.cross(k);
            d_beta = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
        }

        //Calculate intersection point of isect line with (d;k)
        number_t va = std::numeric_limits<double>::infinity();
        if(1 - std::abs(d_alpha.dot(isect_d)) > 1e-3)
        {
            Vector3_t isect_d_cross_d_alpha = isect_d.cross(d_alpha);
            Vector3_t vp_a = (1.0/isect_d.cross(d_alpha).squaredNorm()) * (isect_d * (k.dot(isect_d_cross_d_alpha)) - d_alpha * (isect_k.dot(isect_d_cross_d_alpha)));
            va = (q - vp_a).norm();
        }

        number_t vb = std::numeric_limits<double>::infinity();
        if(1 - std::abs(d_beta.dot(isect_d)) > 1e-3)
        {
            Vector3_t isect_d_cross_d_beta = isect_d.cross(d_beta);
            Vector3_t vp_b = (1.0/isect_d.cross(d_beta).squaredNorm()) * (isect_d * (k.dot(isect_d_cross_d_beta)) - d_beta * (isect_k.dot(isect_d_cross_d_beta)));
            vb = (q - vp_b).norm();
        }

        auto v = std::min(va, vb);
        if(std::isinf(v))
        {
            return std::numeric_limits<double>::infinity();
        }

        return v;
    }
};

class FixedMomentMinSegmentDist
{
private:
    using number_t = double;
    using Vector3_t = Eigen::Vector3d;
    using Vector2_t = Eigen::Vector2d;
    using Matrix3_t = Eigen::Matrix3d;

    Vector3_t q;
    Vector3_t h1;
    Vector3_t h2;
    float t1;
    float t2;
public:
    FixedMomentMinSegmentDist(Vector3_t q, Vector3_t dirLowerBound, Vector3_t dirUpperBound, float t1, float t2)
            : q(std::move(q)), h1(std::move(dirLowerBound)), h2(std::move(dirUpperBound)), t1(t1), t2(t2) {}

    double operator()(const Vector& x, Vector& grad)
    {
        number_t phi_k = x[0];
        number_t theta_k = x[1];
        number_t r_k = x[2];

        // f(k)

        Vector3_t k = spherical2cart(phi_k, theta_k, r_k);
        Vector3_t kd = k / r_k;
        Vector3_t qp = q - kd*(q.dot(kd));
        auto qp_norm = qp.norm();
        Vector3_t qpd = qp / qp_norm;
        auto u = (qp - q).norm();

        auto calc_dist_for_gamma = [](const Vector3_t& qp, const Vector3_t& qpd, const Vector3_t& k, const Vector3_t& kd,
                const Vector3_t& h1, const Vector3_t& h2, number_t orientation, number_t sin_gamma, number_t t1, number_t t2)
        {
            auto cos_gamma = std::sqrt(1 - sin_gamma * sin_gamma);
            Vector3_t da = orientation * qpd * cos_gamma + kd.cross(qpd) * sin_gamma;
            Vector3_t d_alpha;
            auto da_dot_h1 = da.dot(h1);
            auto da_dot_h2 = da.dot(h2);
            if(da_dot_h1 >= 0 && da_dot_h2 >= 0)
            {
                d_alpha = da;
            }else if(da_dot_h1 <= da_dot_h2)
            {
                Vector h1_cross_k = h1.cross(k);
                d_alpha = std::copysign(1.0f, h2.dot(h1_cross_k)) * h1_cross_k.normalized();
            }else
            {
                Vector h2_cross_k = h2.cross(k);
                d_alpha = std::copysign(1.0f, h1.dot(h2_cross_k)) * h2_cross_k.normalized();
            }

            //auto v = (qp.cross(d_alpha) - k).norm();
            Vector3_t p = d_alpha.cross(k);
            auto t = std::min(std::max(d_alpha.dot(qp), t1), t2);
            auto v = ((p + t*d_alpha) - qp).norm();
            return v;
        };

        auto t1_k_hypo_sqr = t1*t1 + r_k*r_k;
        auto t2_k_hypo_sqr = t2*t2 + r_k*r_k;
        number_t v;
        if(t1 >= 0 && t2 >= 0)
        {
            number_t sin_gamma;
            if(qp_norm > t2_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t2_k_hypo_sqr), (number_t)1.0f);
            }
            else if(qp_norm < t1_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t1_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            v = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, 1, sin_gamma, t1, t2);
        }
        else if(t1 <= 0 && t2 <= 0)
        {
            number_t sin_gamma;
            if(qp_norm > t1_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t1_k_hypo_sqr), (number_t)1.0f);
            }
            else if(qp_norm < t2_k_hypo_sqr)
            {
                sin_gamma = std::min(r_k/std::sqrt(t2_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            v = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, -1, sin_gamma, t1, t2);
        }
        else
        {
            number_t sin_gamma_a;
            if(qp_norm > t2_k_hypo_sqr)
            {
                sin_gamma_a = std::min(r_k/std::sqrt(t2_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma_a = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            number_t va = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, 1, sin_gamma_a, t1, t2);

            number_t sin_gamma_b;
            if(qp_norm > t1_k_hypo_sqr)
            {
                sin_gamma_b = std::min(r_k/std::sqrt(t1_k_hypo_sqr), (number_t)1.0f);
            }
            else
            {
                sin_gamma_b = std::min(r_k/qp_norm, (number_t)1.0f);
            }
            number_t vb = calc_dist_for_gamma(qp, qpd, k, kd, h1, h2, -1, sin_gamma_b, t1, t2);

            v = std::min(va, vb);
        }

        auto f = std::sqrt(u*u + v*v);
        return f;
    }
};

namespace pluckertree
{
    double FindMinDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& momentLowerBound,
            const Eigen::Vector3f& momentUpperBound,
            Eigen::Vector3f& minimum
    )
    {
        auto minimize = [point,dirLowerBound,dirUpperBound,momentLowerBound,momentUpperBound](Eigen::Vector3f& minimum){
            //nlopt::opt opt(nlopt::LN_COBYLA, 3);//LN_NELDERMEAD, LN_SBPLX
            nlopt::opt opt(nlopt::LN_SBPLX, 3);//LN_NELDERMEAD, LN_SBPLX

            std::vector<double> lb(momentLowerBound.data(), momentLowerBound.data() + momentLowerBound.rows() * momentLowerBound.cols());
            opt.set_lower_bounds(lb);

            std::vector<double> hb(momentUpperBound.data(), momentUpperBound.data() + momentUpperBound.rows() * momentUpperBound.cols());
            opt.set_upper_bounds(hb);

            FixedMomentMinDist fun(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());
            auto obj_func = [](const std::vector<double> &x, std::vector<double> &grad, void* f_data) -> double {
                FixedMomentMinDist* fun = reinterpret_cast<FixedMomentMinDist*>(f_data);
                Vector x_vect = Eigen::Vector3d {x[0], x[1], x[2]};
                Vector grad_vect = Eigen::Vector3d {0, 0, 0};
                auto result = (*fun)(x_vect, grad_vect);
                return result;
            };
            opt.set_min_objective(obj_func, &fun);

            opt.set_xtol_rel(1e-3);
            opt.set_stopval(1e-3);
            opt.set_maxtime(1);
            opt.set_maxeval(1000);

            //Vector vec = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();
            Vector vec = minimum.cast<double>();
            std::vector<double> x(vec.data(), vec.data() + vec.rows() * vec.cols());

            try
            {
                double minf;
                nlopt::result result = opt.optimize(x, minf);

                minimum = {x[0], x[1], x[2]};
                return minf;
            }
            catch(std::exception &e) {
                std::cout << "nlopt failed: " << e.what() << std::endl;
                throw;
            }
        };
        Eigen::Vector3f minHint;
        //minHint = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f);
        double minVal = 1E99;
        minHint = momentLowerBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; } //TODO: this can be higher if the parent node has a higher min dist value
        minHint = momentUpperBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        return minVal;

        /*// Set up parameters
        LBFGSBParam<double> param;
        param.epsilon = 1e-6;
        param.max_iterations = 100;

        // Create solver and function object
        LBFGSBSolver<double> solver(param);  // New solver class
        FixedMomentMinDist fun(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

        // Initial guess
        Vector x = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();

        // x will be overwritten to be the best point found
        double dist;
        int nbIter = solver.minimize(fun, x, dist, momentLowerBound.cast<double>(), momentUpperBound.cast<double>());
        minimum = x.cast<float>();

        std::cout << nbIter << " iterations" << std::endl;
        std::cout << "x = \n" << x.transpose() << std::endl;
        std::cout << "f(x) = " << dist << std::endl;

        return dist;*/
    }

    double FindMinHitDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& point_normal,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& momentLowerBound,
            const Eigen::Vector3f& momentUpperBound,
            Eigen::Vector3f& minimum
    )
    {
        auto minimize = [point,point_normal,dirLowerBound,dirUpperBound,momentLowerBound,momentUpperBound](Eigen::Vector3f& minimum){
            //nlopt::opt opt(nlopt::LN_COBYLA, 3);//LN_NELDERMEAD, LN_SBPLX
            nlopt::opt opt(nlopt::LN_SBPLX, 3);//LN_NELDERMEAD, LN_SBPLX

            std::vector<double> lb(momentLowerBound.data(), momentLowerBound.data() + momentLowerBound.rows() * momentLowerBound.cols());
            opt.set_lower_bounds(lb);

            std::vector<double> hb(momentUpperBound.data(), momentUpperBound.data() + momentUpperBound.rows() * momentUpperBound.cols());
            opt.set_upper_bounds(hb);

            FixedMomentMinHitDist fun(point.cast<double>(), point_normal.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());
            auto obj_func = [](const std::vector<double> &x, std::vector<double> &grad, void* f_data) -> double {
                FixedMomentMinHitDist* fun = reinterpret_cast<FixedMomentMinHitDist*>(f_data);
                Vector x_vect = Eigen::Vector3d {x[0], x[1], x[2]};
                Vector grad_vect = Eigen::Vector3d {0, 0, 0};
                auto result = (*fun)(x_vect, grad_vect);
                return result;
            };
            opt.set_min_objective(obj_func, &fun);

            opt.set_xtol_rel(1e-3);
            opt.set_stopval(1e-3);
            opt.set_maxtime(1);
            opt.set_maxeval(1000);

            //Vector vec = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();
            Vector vec = minimum.cast<double>();
            std::vector<double> x(vec.data(), vec.data() + vec.rows() * vec.cols());

            try
            {
                double minf;
                nlopt::result result = opt.optimize(x, minf);

                minimum = {x[0], x[1], x[2]};
                return minf;
            }
            catch(std::exception &e) {
                std::cout << "nlopt failed: " << e.what() << std::endl;
                throw;
            }
        };
        Eigen::Vector3f minHint;
        //minHint = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f);
        double minVal = 1E99;
        minHint = momentLowerBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; } //TODO: this can be higher if the parent node has a higher min dist value
        minHint = momentUpperBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        return minVal;
    }

    // Segments
    double segments::FindMinDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& momentLowerBound,
            const Eigen::Vector3f& momentUpperBound,
            float t1Min,
            float t2Max,
            Eigen::Vector3f& min
    )
    {
        auto minimize = [point,dirLowerBound,dirUpperBound,momentLowerBound,momentUpperBound, t1Min, t2Max](Eigen::Vector3f& minimum){
            nlopt::opt opt(nlopt::LN_COBYLA, 3);//LN_NELDERMEAD, LN_SBPLX

            std::vector<double> lb(momentLowerBound.data(), momentLowerBound.data() + momentLowerBound.rows() * momentLowerBound.cols());
            opt.set_lower_bounds(lb);

            std::vector<double> hb(momentUpperBound.data(), momentUpperBound.data() + momentUpperBound.rows() * momentUpperBound.cols());
            opt.set_upper_bounds(hb);

            FixedMomentMinSegmentDist fun(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>(), t1Min, t2Max);
            auto obj_func = [](const std::vector<double> &x, std::vector<double> &grad, void* f_data) -> double {
                FixedMomentMinSegmentDist* fun = reinterpret_cast<FixedMomentMinSegmentDist*>(f_data);
                Vector x_vect = Eigen::Vector3d {x[0], x[1], x[2]};
                Vector grad_vect = Eigen::Vector3d {0, 0, 0};
                auto result = (*fun)(x_vect, grad_vect);
                return result;
            };
            opt.set_min_objective(obj_func, &fun);

            opt.set_xtol_rel(1e-3);
            opt.set_stopval(1e-3);
            opt.set_maxtime(1);
            opt.set_maxeval(1000);

            //Vector vec = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f).cast<double>();
            Vector vec = minimum.cast<double>();
            std::vector<double> x(vec.data(), vec.data() + vec.rows() * vec.cols());

            try
            {
                double minf;
                nlopt::result result = opt.optimize(x, minf);

                minimum = {x[0], x[1], x[2]};
                return minf;
            }
            catch(std::exception &e) {
                std::cout << "nlopt failed: " << e.what() << std::endl;
                throw;
            }
        };
        Eigen::Vector3f minHint;
        //minHint = (momentLowerBound + (momentUpperBound - momentLowerBound)/2.0f);
        double minVal = 1E99;
        minHint = momentLowerBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; } //TODO: this can be higher if the parent node has a higher min dist value
        minHint = momentUpperBound;
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentLowerBound.x(), momentUpperBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentUpperBound.y(), momentLowerBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        if(minVal < 1e-3){ return minVal; }
        minHint = Eigen::Vector3f(momentUpperBound.x(), momentLowerBound.y(), momentUpperBound.z());
        minVal = std::min(minVal, minimize(minHint));
        Diag::minimizations++;
        return minVal;
    }

    double FindMinDist(
            const Eigen::Vector3f& point,
            const Eigen::Vector3f& dirLowerBound,
            const Eigen::Vector3f& dirUpperBound,
            const Eigen::Vector3f& moment
    )
    {
        FixedMomentMinDist f(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

        Vector vect = moment.cast<double>();
        Vector grad;
        auto dist = f(vect, grad);

        return dist;
    }

///// POINTCLOUD EXPORT
thread_local unsigned int pluckertree::Diag::visited = 0;
thread_local unsigned int pluckertree::Diag::minimizations = 0;
std::optional<std::function<void(float, float, float, int)>> pluckertree::Diag::on_node_visited;
std::optional<std::function<void(float, float, int)>> pluckertree::Diag::on_node_enter;
std::optional<std::function<void(float, float, float, int)>> pluckertree::Diag::on_node_leave;
std::optional<std::function<void(float, float, float, float)>> pluckertree::Diag::on_build_variance_calculated;
bool pluckertree::Diag::force_visit_all = false;

std::random_device pluckertree::MyRand::rand_dev;

struct GridPoint
{
    Eigen::Vector3f pos;
    Eigen::Vector3f grad;
    float dist;
};

/*std::vector<GridPoint> CalculateGrid(
        const Eigen::Vector3f& point,
        const Eigen::Vector3f& query_point,
        const Eigen::Vector3f& dirLowerBound,
        const Eigen::Vector3f& dirUpperBound,
        float x_start,
        float x_end,
        float y_start,
        float y_end,
        float z_start,
        float z_end,
        float resolution
)
{
    FixedMomentMinHitDist f(point.cast<double>(), query_point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

    auto x_steps = int((x_end - x_start) / resolution);
    auto y_steps = int((y_end - y_start) / resolution);
    auto z_steps = int((z_end - z_start) / resolution);

    std::vector<GridPoint> points(x_steps * y_steps * z_steps);

    for(int x_i = 0; x_i < x_steps; x_i++)
    {
        std::cout << ((float)x_i/(float)x_steps)*100.0f << "%\r";
        std::cout.flush();
        auto x = x_start + resolution*x_i;
        for(int y_i = 0; y_i < y_steps; y_i++)
        {
            auto y = y_start + resolution*y_i;
            for(int z_i = 0; z_i < z_steps; z_i++)
            {
                auto z = z_start + resolution*z_i;

                Vector vect = cart2spherical(Eigen::Vector3d(x, y, z));
                Vector grad;
                auto dist = f(vect, grad);

                auto& curPoint = points[(x_i * y_steps * z_steps) + (y_i * z_steps) + z_i];
                curPoint.pos = Eigen::Vector3f(x, y, z);
                curPoint.dist = dist;
                //curPoint.grad = grad.cast<float>();
            }
        }
    }
    std::cout << std::endl;

    return points;
}*/

std::vector<GridPoint> CalculateBallSlice(
        const Eigen::Vector3f& point,
        const Eigen::Vector3f& dirLowerBound,
        const Eigen::Vector3f& dirUpperBound,
        const Eigen::Vector3f& mlb,
        const Eigen::Vector3f& mub,
        float resolution
)
{
    FixedMomentMinDist f(point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

    Eigen::Vector3f stepSize = (mub - mlb) / resolution;

    std::vector<GridPoint> points(resolution * resolution * resolution);

    for(int x_i = 0; x_i < resolution; x_i++)
    {
        std::cout << ((float)x_i/(float)resolution)*100.0f << "%\r";
        std::cout.flush();
        auto x = mlb.x() + stepSize.x()*x_i;
        for(int y_i = 0; y_i < resolution; y_i++)
        {
            auto y = mlb.y() + stepSize.y()*y_i;
            for(int z_i = 0; z_i < resolution; z_i++)
            {
                auto z = mlb.z() + stepSize.z()*z_i;

                Vector vect = Eigen::Vector3d(x, y, z);
                Vector grad;
                auto dist = f(vect, grad);

                auto& curPoint = points[(x_i * resolution * resolution) + (y_i * resolution) + z_i];
                curPoint.pos = spherical2cart(x, y, z);
                curPoint.dist = dist;
                //curPoint.grad = grad.cast<float>();
            }
        }
    }
    std::cout << std::endl;

    return points;
}

std::vector<GridPoint> CalculateBallSlice(
        const Eigen::Vector3f& point,
        const Eigen::Vector3f& query_point,
        const Eigen::Vector3f& dirLowerBound,
        const Eigen::Vector3f& dirUpperBound,
        const Eigen::Vector3f& mlb,
        const Eigen::Vector3f& mub,
        float resolution
)
{
    FixedMomentMinHitDist f(point.cast<double>(), query_point.cast<double>(), dirLowerBound.cast<double>(), dirUpperBound.cast<double>());

    Eigen::Vector3f stepSize = (mub - mlb) / resolution;

    std::vector<GridPoint> points(resolution * resolution * resolution);

    for(int x_i = 0; x_i < resolution; x_i++)
    {
        std::cout << ((float)x_i/(float)resolution)*100.0f << "%\r";
        std::cout.flush();
        auto x = mlb.x() + stepSize.x()*x_i;
        for(int y_i = 0; y_i < resolution; y_i++)
        {
            auto y = mlb.y() + stepSize.y()*y_i;
            for(int z_i = 0; z_i < resolution; z_i++)
            {
                auto z = mlb.z() + stepSize.z()*z_i;

                Vector vect = Eigen::Vector3d(x, y, z);
                Vector grad;
                auto dist = f(vect, grad);

                auto& curPoint = points[(x_i * resolution * resolution) + (y_i * resolution) + z_i];
                curPoint.pos = spherical2cart(x, y, z);
                curPoint.dist = dist;
                //curPoint.grad = grad.cast<float>();
            }
        }
    }
    std::cout << std::endl;

    return points;
}

void show_me_the_grid(std::string& file,
                      const Eigen::Vector3f& dlb,
                      const Eigen::Vector3f& dub,
                      const Eigen::Vector3f& mlb,
                      const Eigen::Vector3f& mub,
                      const Eigen::Vector3f& q)
{
    /*Eigen::Vector3f dlb = Eigen::Vector3f(0,0,-1);
    Eigen::Vector3f dub = Eigen::Vector3f(-std::sqrt(2)/2.0, std::sqrt(2)/2, 0);
    Eigen::Vector3f mlb(-M_PI, 0.785398185, 1);
    Eigen::Vector3f mub(-M_PI/2, 2.3561945, 80);
    Eigen::Vector3f q(25.216011, 86.2393799, 64.2581253);
    Eigen::Vector3f q_normal(0.742901862, 0.662636876, 0.0949169919);*/

    float resolution = 300;
    //auto data = CalculateBallSlice(q, q_normal, dlb, dub, mlb, mub, resolution);
    auto data = CalculateBallSlice(q, dlb, dub, mlb, mub, resolution);

    std::fstream myfile;
    myfile = std::fstream(file, std::ios::out | std::ios::binary);

    unsigned int size = data.size();
    myfile.write(reinterpret_cast<char*>(&size), sizeof(unsigned int));
    for(const auto& entry : data)
    {
        myfile.write(reinterpret_cast<const char*>(&entry.pos.x()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.pos.y()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.pos.z()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.grad.x()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.grad.y()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.grad.z()), sizeof(float));
        myfile.write(reinterpret_cast<const char*>(&entry.dist), sizeof(float));
    }
    myfile.close();
}

/////

}


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

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

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    typedef typename mtl::Collection<Matrix>::value_type   value_type;

    A= 3, 7.2, 0,
       2, 4.444, 5;

    std::cout << "\n" << name << ", assignment: A = \n" << A << "\n";

    MTL_THROW_IF(A[1][0] != value_type(2), mtl::runtime_error("Wrong value inserted"));
}

int main(int, char**)
{
    using namespace mtl;
    dense2D<double>                                      dr(2, 3);
    dense2D<double, mat::parameters<col_major> >      dc(2, 3);
    morton_dense<double, recursion::morton_z_mask>       mzd(2, 3);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(2, 3);
    compressed2D<double>                                 cr(2, 3);
    compressed2D<double, mat::parameters<col_major> > cc(2, 3);

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

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

    return 0;
}


//
// $Id: bucket1dtest.cpp 5313 2013-12-17 18:06:54Z chambm $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "pwiz/utility/findmf/base/resample/bucket1d.hpp"
#include "pwiz/utility/findmf/base/resample/bin1d.hpp"

#include <algorithm>
#include <boost/timer.hpp>
#include <boost/cstdint.hpp>

#include "pwiz/utility/misc/unit.hpp"

#include "pwiz/utility/findmf/base/resample/breakspec.hpp"
#include "pwiz/utility/findmf/base/resample/masscomparefunctors.hpp"

namespace {

using namespace pwiz::util;
typedef boost::uint32_t uint32_t;
typedef boost::int32_t int32_t;
  void testBin1D(){
    double ref [] = {2., 2.1 , 5. , 5.1 , 7.1 , 7.15 , 10. , 10.1};
    std::vector<double> breaks;
    breaks.assign(ref , ref + sizeof(ref)/sizeof(double));

    ralab::base::resample::Bin1D bin(breaks);
    unit_assert_operator_equal(bin(2.-1e-8) , 0);
    unit_assert_operator_equal(bin(2.+1e-8) , 1);
    unit_assert_operator_equal(bin(2.1) ,1);
    unit_assert_operator_equal(bin(2.1 + 1e-9) , 2);
  }

  void testBin1D2(){
double epsilon = 1e-8;
    double ref [] = {2., 2.1 , 5. , 5.1 , 7.1 , 7.15 , 10. , 10.1};
    std::vector<double> breaks;
    breaks.assign(ref , ref + sizeof(ref)/sizeof(double));

    ralab::base::resample::Bin1D bin(breaks);
    std::vector<int32_t> idx;
    std::vector<double> dist;
    bin(2. -1e-8 , 2. + 2e-4,  idx, dist ) ;
    unit_assert_operator_equal(idx[0],-1);
    unit_assert_operator_equal(idx[1],0);
    unit_assert_equal(dist[0],1e-8,epsilon);
    unit_assert_equal(dist[1],2e-4,epsilon);

    bin(2. -1e-8 , 2.1 + 2e-4,  idx, dist ) ;
    unit_assert_operator_equal(idx[0],-1);
    unit_assert_operator_equal(idx[1],0);
    unit_assert_operator_equal(idx[2],1);
    unit_assert_equal(dist[0], 1e-8 ,epsilon);
    unit_assert_equal(dist[1], 0.1 ,epsilon);
    unit_assert_equal(dist[2], 2e-4 ,epsilon);

    bin(2. -1e-8 , 5.1 + 2e-4,  idx, dist ) ;
    double resd [] = {1e-8, 0.1, 2.9, 0.1, 2e-4};
    int  idxr[] = {-1,0,1,2,3};
    bool x = std::equal(idxr , idxr + sizeof(idxr)/sizeof(int), idx.begin());
    unit_assert(x);
    x = std::equal(resd , resd + sizeof(resd)/sizeof(double), dist.begin(), ralab::base::resample::DaCompFunctor<double>(1e-8));
    unit_assert(x);

    bin(2. - 2e-4 , 2. - 1e-4,  idx, dist ) ;
    unit_assert_operator_equal(idx[0],-1);
    unit_assert_equal(dist[0],1e-4,1e-14);
    bin(2.1 - 2e-4 , 2.1 - 1e-4,  idx, dist ) ;
    unit_assert_operator_equal(idx[0],0);
    unit_assert_equal(dist[0],1e-4,1e-14);
    bin(2.1 - 1e-4 , 7.1 + 2e-4,  idx, dist ) ;

    //testing end span
    bin(10.1 - 1e-4 , 10.1 + 2e-4,  idx, dist ) ;

    bin(5.1 - 1e-4 , 10.1 + 2e-4,  idx, dist ) ;

    bin(10.1 + 1e-4 , 10.1 + 2e-4,  idx, dist ) ;

  }

  void testHist()
  {
    std::vector<double> breaks;
    std::vector<uint32_t> indicator;
    //                 0     1     2    3     4      5     6
    double ref [] = {2., 2.1 , 5. , 5.1 , 7.1 , 7.15 , 10. , 10.1};
    // We cover 0.1+0.1+0.05+0.1 = 0.35 //
    breaks.assign(ref , ref + sizeof(ref)/sizeof(double));

    /*!\brief length indic is length(ref) - 1
                                */
    //                   0   1   2   3   4   5   6
    uint32_t indic[] = { 1 , 0 , 1 , 0 , 1 , 0 , 1};

    indicator.assign(
          indic
          ,indic + sizeof(indic)/sizeof(uint32_t)
          );

    ralab::base::resample::Bucket1D b1d( breaks, indicator);
    std::vector<double> sample;
    std::pair< size_t , bool > rb;
    rb = b1d.operator()( 1. );
    rb = b1d.operator()( 2. );
    rb = b1d.operator()( 2.05 );
    rb = b1d.operator()( 2.1 );
    rb = b1d.operator()( 4. );
    rb = b1d.operator()( 5. );
    rb = b1d.operator()( 5.01 );
    rb = b1d.operator()( 5.1 );
    rb = b1d.operator()(6.);
    rb = b1d.operator()(7.);
    rb = b1d.operator()(7.12); // 4,true
    rb = b1d.operator()(8.);
    rb = b1d.operator()( 10.1 );
    rb = b1d.operator()( 13. );

    sample.push_back( 1. );//false
    sample.push_back( 2. );//false
    sample.push_back( 2.05 );//#2 -> 0
    sample.push_back( 2.1 );//#3 -> 0
    sample.push_back( 4. );//false
    sample.push_back( 5. );//false
    sample.push_back( 5.01 );//#6 -> 2
    sample.push_back( 5.1 );//#7 -> 2
    sample.push_back( 10.1 );//#8 -> 6
    sample.push_back( 13. );//false

    std::vector< std::pair< size_t , size_t > > res;
    b1d( sample.begin() , sample.end() , res );
    unit_assert(res[0].first == 0 && res[0].second == 2);
    unit_assert(res[1].first == 0 && res[1].second == 3);
  }

}//end namespace

int main(int argc, char **argv) {
  testBin1D();
testBin1D2();
testHist();
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/equal.hpp>
#include <boost/hana/functional/iterate.hpp>
#include <boost/hana/functional/placeholder.hpp>
#include <boost/hana/tail.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

constexpr auto next_10 = hana::iterate<10>(hana::_ + 1);
static_assert(next_10(3) == 13, "");

constexpr auto xs = hana::make_tuple(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
static_assert(hana::iterate<5>(hana::tail, xs) == hana::make_tuple(6, 7, 8, 9, 10), "");

int main() { }


#ifndef __MODEL_HPP__
#define __MODEL_HPP__

#include <string>
#include <vector>
#include <Eigen/Eigen>
#include <stdint.h>
#include "tiny_obj_loader.h"
#include "Logger.hpp"

struct EigenTypes {
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Vector4d Vector4;
  typedef Eigen::Matrix3d Matrix3;
  typedef Eigen::Matrix4d Matrix4;
};

struct Pixel : public EigenTypes {
  int x, y;  /// image coordinates
  Vector3 t; /// barycentric coordinates

  Pixel(int x, int y, Vector3 t)
    : x(x), y(y), t(t)
  {}
};

struct Triangle : public EigenTypes
{
  Vector3 vertices[3];
  Vector3 normals[3];

  void raster(std::vector<Pixel> &pixels, int w, int h) const;
  float getDepth(const Pixel &p) const;
  uint32_t getColor(const Pixel &p) const;
};

class Model : public EigenTypes {
public:

public:
  Model(const char *filename);
  ~Model();

public:
  void debug() const;
  size_t numShapes() const;
  size_t vertexSize(size_t i) const;
  size_t normalSize(size_t i) const;
  size_t indexSize(size_t i) const;
  void *vertexData(size_t i);
  void *normalData(size_t i);
  void *indexData(size_t i);

  void getTriangles(std::vector<Triangle> &triangles, const Matrix4 &transform);

protected:
  /** \brief Calculate normals for each vertex.
   */
  void calculate_normal(size_t idx);

protected:
  std::string m_filename;
  std::vector<tinyobj::shape_t> m_shapes;

};

#endif // __MODEL_HPP__


#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <chrono>
#include <memory>
#include "bandit.h"
#include "ucb.h"
#include "rewards.h"
#include "utils.h"

using namespace std;
using namespace Eigen;

int PREC = 4;
int EVERY = 1;

std::vector<std::shared_ptr<RewardDistribution>> make_problem(std::vector<double> p, int seed) {
    std::vector<std::shared_ptr<RewardDistribution>> vec;
    for(auto& y : p) {
        vec.push_back(std::make_shared<Bernoulli>(Bernoulli(y, seed)));
    }
    return vec;
}

int main()
{
    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    int seed = time(NULL);
    cout << "seed: " << seed << endl;
    srand (seed);
    int n_runs = 200, T = 600;
    double rrange = 1.;
    double bonus_scale = 1.;
    int n_arms = 4;

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    /// define rewards
    cout << "Bernoulli( ";
    std::vector<double> means;
    for (int i=0; i < n_arms; ++i) {
        double p = ((double) rand() / (RAND_MAX));
        cout << p << ", ";
        means.push_back(p);
    }
    cout << ")" << endl;
    ///

    vec2double regrets(n_runs), pseudo_regrets(n_runs);
    #pragma omp parallel for
    for (int i = 0; i < n_runs; ++i)
    {
        auto start = TIC();
        std::vector<std::shared_ptr<RewardDistribution>> rews = make_problem(means, seeds[i]);
        UCB localg(rews.size(), rrange, bonus_scale);
        MultiArmedBandit prb(rews, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
        auto tottime = TOC(start);
        cout << "i: " << tottime << endl;
    }
    save_vector_csv_gzip(regrets, "UCBV_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "UCBV_pseudoregrets.csv.gz", EVERY, PREC);

    for(auto& y : means) {
        cout << y << ", ";
    }
    cout << endl;
    return 0;
}


#define BOOST_TEST_MODULE object
#include <boost/test/included/unit_test.hpp>

#include "../src/constant.hpp"
#include "../src/object.hpp"
#include "../src/ray.hpp"

struct Fixture {
    Material material_magenta;

    Fixture()
        : material_magenta(new ConstantTexture(Color(1.0, 0.0, 1.0)), Color(0.0, 0.0, 0.0),
                           ReflectionType::DIFFUSE) {}
};

BOOST_FIXTURE_TEST_SUITE(test_object_sute, Fixture)

BOOST_AUTO_TEST_SUITE(test_sphere_sute)
BOOST_AUTO_TEST_CASE(test_correct_bbox_init) {
    Sphere sphere;
    BOOST_CHECK(sphere.bbox.corner0 == Vec(-1.0 - EPS, -1.0 - EPS, -1.0 - EPS));
    BOOST_CHECK(sphere.bbox.corner1 == Vec(1.0 + EPS, 1.0 + EPS, 1.0 + EPS));

    const double radius = 2.0;
    const Vec center(1.0, 1.0, 1.0);

    sphere = Sphere(radius, center, &material_magenta);
    BOOST_CHECK(sphere.bbox.corner0 == Vec(-1.0 - EPS, -1.0 - EPS, -1.0 - EPS));
    BOOST_CHECK(sphere.bbox.corner1 == Vec(3.0 + EPS, 3.0 + EPS, 3.0 + EPS));
}

BOOST_AUTO_TEST_CASE(test_correct_intersect) {
    const double radius = 1.0;
    const Vec center(2.0, 0.0, 0.0);

    const Sphere sphere(radius, center, &material_magenta);

    Hitpoint hitpoint;

    Ray ray(Vec(0.0, 0.0, 0.0), Vec(0.0, 1.0, 0.0));
    bool result = sphere.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, false);
    BOOST_CHECK_EQUAL(hitpoint.distance, INF);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 0.0, 0.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 0.0, 0.0));

    ray = Ray(Vec(0.0, 0.0, 0.0), Vec(1.0, 0.0, 0.0));
    result = sphere.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, true);
    BOOST_CHECK_EQUAL(hitpoint.distance, 1.0);
    BOOST_CHECK(hitpoint.normal == Vec(-1.0, 0.0, 0.0));
    BOOST_CHECK(hitpoint.position == Vec(1.0, 0.0, 0.0));
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(test_plane_sute)
BOOST_AUTO_TEST_CASE(test_correct_bbox_init) {
    Plane plane;
    BOOST_CHECK(plane.bbox.corner0 == Vec(-INF, -EPS, -INF));
    BOOST_CHECK(plane.bbox.corner1 == Vec(INF, EPS, INF));

    const Vec origin(0.0, 1.0, 0.0);
    const Vec normal(0.0, 1.0, 0.0);
    plane = Plane(origin, normal, &material_magenta);
    BOOST_CHECK(plane.bbox.corner0 == Vec(-INF, 1.0 - EPS, -INF));
    BOOST_CHECK(plane.bbox.corner1 == Vec(INF, 1.0 + EPS, INF));
}

BOOST_AUTO_TEST_CASE(test_correct_intersect) {
    const Vec origin(0.0, 0.0, 0.0);
    const Vec normal(0.0, 1.0, 0.0);

    Plane plane(origin, normal, &material_magenta);

    Hitpoint hitpoint;

    Ray ray(Vec(0.0, 1.0, 0.0), Vec(0.0, 1.0, 0.0));
    bool result = plane.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, false);
    BOOST_CHECK_EQUAL(hitpoint.distance, INF);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 0.0, 0.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 0.0, 0.0));

    ray = Ray(Vec(0.0, 1.0, 0.0), Vec(0.0, -1.0, 0.0));
    result = plane.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, true);
    BOOST_CHECK_EQUAL(hitpoint.distance, 1.0);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 1.0, 0.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 0.0, 0.0));
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(test_triangle_sute)
BOOST_AUTO_TEST_CASE(test_correct_bbox_init) {
    Triangle triangle;
    BOOST_CHECK(triangle.bbox.corner0 == Vec(-EPS, -EPS, -EPS));
    BOOST_CHECK(triangle.bbox.corner1 == Vec(1.0 + EPS, EPS, 1.0 + EPS));

    const Vec v0(-2.0, 0.0, 0.0), v1(2.0, 0.0, 0.0), v2(0.0, 2.0, 0.0);
    triangle = Triangle(v0, v1, v2, &material_magenta);
    BOOST_CHECK(triangle.bbox.corner0 == Vec(-2.0 - EPS, -EPS, -EPS));
    BOOST_CHECK(triangle.bbox.corner1 == Vec(2.0 + EPS, 2.0 + EPS, EPS));
}

BOOST_AUTO_TEST_CASE(test_correct_intersect) {
    const Vec v0(-1.0, 0.0, 0.0), v1(1.0, 0.0, 0.0), v2(0.0, 2.0, 0.0);
    const Triangle triangle(v0, v1, v2, &material_magenta);

    Hitpoint hitpoint;

    Ray ray(Vec(1.0, 2.0, 1.0), Vec(0.0, 0.0, -1.0));
    bool result = triangle.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, false);
    BOOST_CHECK_EQUAL(hitpoint.distance, INF);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 0.0, 0.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 0.0, 0.0));

    ray = Ray(Vec(0.0, 1.0, 1.0), Vec(0.0, 0.0, -1.0));
    result = triangle.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, true);
    BOOST_CHECK_EQUAL(hitpoint.distance, 1.0);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 0.0, 1.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 1.0, 0.0));
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(test_box_sute)
BOOST_AUTO_TEST_CASE(test_correct_intersect) {
    const Vec corner0(-1.0, 1.0, -1.0);
    const Vec corner1(1.0, -1.0, 1.0);

    Box box(corner0, corner1, &material_magenta);
    Hitpoint hitpoint;

    Ray ray(Vec(0.0, 2.0, 3.0), Vec(0.0, 0.0, -1.0));
    bool result = box.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, false);
    BOOST_CHECK_EQUAL(hitpoint.distance, INF);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 0.0, 0.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 0.0, 0.0));

    ray = Ray(Vec(0.0, 0.0, 3.0), Vec(0.0, 0.0, -1.0));
    result = box.intersect(ray, hitpoint);
    BOOST_CHECK_EQUAL(result, true);
    BOOST_CHECK_EQUAL(hitpoint.distance, 2.0);
    BOOST_CHECK(hitpoint.normal == Vec(0.0, 0.0, 1.0));
    BOOST_CHECK(hitpoint.position == Vec(0.0, 0.0, 1.0));
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(test_bbox_sute)
BOOST_AUTO_TEST_CASE(test_correct_hit) {
    const Vec corner0(-1.0, 1.0, -1.0);
    const Vec corner1(1.0, -1.0, 1.0);

    BBox bbox(corner0, corner1);

    // バウンディングボックスの外部からレイを飛ばす
    Ray ray(Vec(0.0, 2.0, 3.0), Vec(0.0, 0.0, -1.0));
    bool result = bbox.hit(ray);
    BOOST_CHECK_EQUAL(result, false);

    ray = Ray(Vec(0.0, 0.0, 3.0), Vec(0.0, 0.00001, -1.0));
    result = bbox.hit(ray);
    BOOST_CHECK_EQUAL(result, true);

    // 軸平行なレイ
    ray = Ray(Vec(0.0, 0.0, 3.0), Vec(0.0, 0.0, -1.0));
    result = bbox.hit(ray);
    BOOST_CHECK_EQUAL(result, true);

    // バウンディングボックスの内部からレイを飛ばす
    ray = Ray(Vec(0.0, 0.0, 0.0), normalize(Vec(1.0, 2.0, 0.0)));
    result = bbox.hit(ray);
    BOOST_CHECK_EQUAL(result, true);
}

BOOST_AUTO_TEST_CASE(test_correct_empty_bbox) {
    Vec corner0(-1.0, 0.0, -1.0), corner1(1.0, 2.0, 1.0);
    BBox bbox(corner0, corner1);

    bbox.empty();
    BOOST_CHECK_EQUAL(bbox.corner0.x, INF);
    BOOST_CHECK_EQUAL(bbox.corner0.y, INF);
    BOOST_CHECK_EQUAL(bbox.corner0.z, INF);
    BOOST_CHECK_EQUAL(bbox.corner1.x, -INF);
    BOOST_CHECK_EQUAL(bbox.corner1.y, -INF);
    BOOST_CHECK_EQUAL(bbox.corner1.z, -INF);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include "buildOneCategoryRecursively.h"
#include <iostream>
#include <algorithm>

#include <boost/graph/depth_first_search.hpp>
#include <boost/property_map/vector_property_map.hpp>

std::set<std::size_t> build_one_category_recursively(const std::size_t i_category, const CategoryHirachyGraph& graph, const std::vector<std::vector<std::size_t>>& category_has_article)
{
	class CategoryTree : public boost::default_dfs_visitor
	{
		public:
			CategoryTree(const std::vector<std::vector<std::size_t>>& category_has_article, std::set<std::size_t>& s)
				:_category_has_article(category_has_article),
				articles_in_category(s)
			{}

			void discover_vertex(CategoryHirachyGraph::vertex_descriptor v, const CategoryHirachyGraph& g)
			{
				articles_in_category.insert(_category_has_article[v].begin(), _category_has_article[v].end());
			}

			const std::vector<std::vector<std::size_t>>& _category_has_article;
			std::set<std::size_t>& articles_in_category;
	};

	std::set<std::size_t> articles_in_category;
	CategoryTree tree(category_has_article, articles_in_category);
	boost::vector_property_map<boost::default_color_type> color_map;
	boost::depth_first_visit(graph, i_category, tree, color_map);

	return articles_in_category;
}


#include <catch.hpp>
#include <map>
#include <Eigen/Core>
#include <random>

#include "check_adjoint.h"
#include "test_utils.h"
#include "renderer_tf.cuh"

template<kernel::TFMode tfMode>
MatrixXr createTF(std::default_random_engine& rnd);

template<>
MatrixXr createTF<kernel::TFIdentity>(std::default_random_engine& rnd)
{
	MatrixXr tf(1, 2);
	std::uniform_real_distribution<real_t> distr;
	tf(0, 0) = distr(rnd);
	tf(0, 1) = distr(rnd);
	return tf;
}

template<>
MatrixXr createTF<kernel::TFTexture>(std::default_random_engine& rnd)
{
	static const int R = 8;
	MatrixXr tf(R, 4);
	std::uniform_real_distribution<real_t> distr;
	for (int r = 0; r < R; ++r)
		for (int c = 0; c < 4; ++c)
			tf(r, c) = distr(rnd);
	return tf;
}

template<>
MatrixXr createTF<kernel::TFLinear>(std::default_random_engine& rnd)
{
	static const int R = 5;
	std::uniform_real_distribution<real_t> distr(0.1, 0.9);
	std::vector<real_t> positions(R);
	positions[0] = 0.0;
	positions[R - 1] = 1.0;
	for (int r = 1; r < R - 1; ++r)
		positions[r] = distr(rnd);
	std::sort(positions.begin(), positions.end());

	MatrixXr tf(R, 5);
	for (int r=0; r<R; ++r)
	{
		for (int c = 0; c < 4; ++c)
			tf(r, c) = distr(rnd);
		tf(r, 4) = positions[r];
	}
	return tf;
}

template<>
MatrixXr createTF<kernel::TFGaussian>(std::default_random_engine& rnd)
{
	static const int R = 4;
	MatrixXr tf(R, 6);
	std::uniform_real_distribution<real_t> distr;
	for (int r = 0; r < R; ++r) {
		for (int c = 0; c < 4; ++c)
			tf(r, c) = distr(rnd);
		tf(r, 4) = distr(rnd); //mean
		tf(r, 5) = 0.1 + distr(rnd); //sigma
	}
	return tf;
}
template<>
MatrixXr createTF<kernel::TFGaussianLog>(std::default_random_engine& rnd)
{
	return createTF<kernel::TFGaussian>(rnd);
}

static VectorXr linearize(const MatrixXr& mat)
{
	Eigen::Map<const VectorXr> v(mat.data(), mat.size());
	return v;
}
static MatrixXr delinearize(const VectorXr& input, const MatrixXr& sizeReference)
{
	Eigen::Map<const MatrixXr> m(input.data(), sizeReference.rows(), sizeReference.cols());
	return m;
}
template<typename Tensor_t>
Tensor_t accessor3D(const MatrixXr& tf)
{
	const int64_t sizes[] = {
		1, tf.rows(), tf.cols()
	};
	const int64_t strides[] = {
		0, tf.rowStride(), tf.colStride()
	};
	return Tensor_t(
		const_cast<real_t*>(tf.data()), sizes, strides);
}

template<kernel::TFMode tfMode, bool hasTFDerivative>
void testAdjointTF(double epsilon=1e-6, double precision=1e-4, double margin=1e-6)
{
	typedef empty TmpStorage_t;
	typedef VectorXr Vector_t;

	std::default_random_engine rnd(42);
	std::uniform_real_distribution<real_t> distr(0.01, 0.99);
	int N1 = 10; //TFs
	int N2 = 10; //densities
	for (int i1 = 0; i1 < N1; ++i1)
	{
		INFO("N1=" << i1);
		const MatrixXr tf = createTF<tfMode>(rnd);
		INFO("tf:\n" << tf);

		const VectorXr tfLinear = linearize(tf);
		const MatrixXr tfTest = delinearize(tfLinear, MatrixXr::Zero(tf.rows(), tf.cols()));
		REQUIRE(tf == tfTest);

		kernel::TransferFunctionEval<tfMode> tfEval;

		auto forward = [&tf, &tfEval](const Vector_t& x, TmpStorage_t* tmp) -> Vector_t
		{
			MatrixXr myTF;
			real_t myDensity = x[0];
			if constexpr (hasTFDerivative)
				myTF = delinearize(x.tail(x.size() - 1), tf);
			else
				myTF = tf;
			const kernel::Tensor3Read myTFacc = accessor3D<kernel::Tensor3Read>(myTF);

			const real4 color = tfEval.eval(myTFacc, 0, myDensity);
			return toEigen(color);
		};
		auto adjoint = [&tf, &tfEval](const Vector_t& x, const Vector_t& e, const Vector_t& g,
			Vector_t& z, const TmpStorage_t& tmp)
		{
			MatrixXr myTF;
			real_t myDensity = x[0];
			if constexpr (hasTFDerivative)
				myTF = delinearize(x.tail(x.size() - 1), tf);
			else
				myTF = tf;
			const auto myTFacc = accessor3D<kernel::Tensor3Read>(myTF);

			const real4 adj_color = fromEigen4(g);

			real_t adj_density;
			MatrixXr adj_tf = MatrixXr::Zero(tf.rows(), tf.cols());
			auto adj_tfAcc = accessor3D<kernel::BTensor3RW>(adj_tf);

			tfEval.adjoint<hasTFDerivative, false>(
				myTFacc, 0, myDensity, adj_color, adj_density, adj_tfAcc, nullptr);

			z[0] = adj_density;
			if constexpr(hasTFDerivative)
			{
				z.tail(z.size() - 1) = linearize(adj_tf);
			} else
			{
				REQUIRE(adj_tf.isZero());
			}
		};

		for (int i2 = 0; i2 < N2; ++i2)
		{
			INFO("N2=" << i2);
			real_t density = distr(rnd);
			INFO("density: " << density);

			Vector_t x;
			if constexpr(hasTFDerivative)
			{
				x.resize(1 + tf.rows() * tf.cols());
				x[0] = density;
				x.tail(x.size() - 1) = tfLinear;
			} else
			{
				x.resize(1);
				x[0] = density;
			}

			checkAdjoint<Vector_t, TmpStorage_t>(x, forward, adjoint,
				epsilon, precision, margin);
		}
	}
}

TEST_CASE("Adjoint-TFIdentity-NoTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFIdentity, false>();
}
TEST_CASE("Adjoint-TFIdentity-WithTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFIdentity, true>();
}

TEST_CASE("Adjoint-TFTexture-NoTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFTexture, false>();
}
TEST_CASE("Adjoint-TFTexture-WithTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFTexture, true>();
}

TEST_CASE("Adjoint-TFLinear-NoTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFLinear, false>();
}
TEST_CASE("Adjoint-TFLinear-WithTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFLinear, true>();
}

TEST_CASE("Adjoint-TFGauss-NoTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFGaussian, false>(1e-5, 1e-3, 1e-4);
}
TEST_CASE("Adjoint-TFGauss-WithTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFGaussian, true>(1e-5, 1e-3, 1e-3);
}

TEST_CASE("Adjoint-TFGaussLog-NoTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFGaussianLog, false>(1e-5, 1e-3, 1e-4);
}
TEST_CASE("Adjoint-TFGaussLog-WithTFDerivatives", "[adjoint]")
{
	testAdjointTF<kernel::TFGaussianLog, true>(1e-6, 1e-3, 1e-4);
}

TEST_CASE("TFGaussianLog-vs-TFGaussian", "[math]")
{
	std::default_random_engine rnd(42);
	std::uniform_real_distribution<real_t> distr(0.01, 0.99);
	int N1 = 10; //TFs
	int N2 = 20; //densities
	for (int i1 = 0; i1 < N1; ++i1)
	{
		INFO("N1=" << i1);
		const MatrixXr tf = createTF<kernel::TFGaussian>(rnd);
		INFO("tf:\n" << tf);

		kernel::TransferFunctionEval<kernel::TFGaussian> tfEvalGauss;
		kernel::TransferFunctionEval<kernel::TFGaussianLog> tfEvalGaussLog;

		for (int i2 = 0; i2 < N2; ++i2)
		{
			INFO("N2=" << i2);
			real_t density = distr(rnd);
			INFO("density: " << density);

			const kernel::Tensor3Read myTFacc = accessor3D<kernel::Tensor3Read>(tf);
			real4 colorExpected = tfEvalGauss.eval(myTFacc, 0, density);
			real4 colorActualLog = tfEvalGaussLog.eval(myTFacc, 0, density);
			real4 colorActual = rexp(colorActualLog);

			REQUIRE(colorExpected.x == Approx(colorActual.x));
			REQUIRE(colorExpected.y == Approx(colorActual.y));
			REQUIRE(colorExpected.z == Approx(colorActual.z));
			REQUIRE(colorExpected.w == Approx(colorActual.w));
		}
	}
}


#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
namespace py = pybind11;

#include <Eigen/Dense>
#include <MeshFEM/ElasticityTensor.hh>
#include <MeshFEM/Fields.hh>
#include <MeshFEM/SymmetricMatrix.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/VonMises.hh>
#include <MeshFEM/Utilities/NameMangling.hh>

template<typename _Real, size_t N>
void bindTensors(py::module& module, py::module& detail_module) {
    using ETensor = ElasticityTensor    <_Real, N>;
    using SMValue = SymmetricMatrixValue<_Real, N>;
    using SMF     = SymmetricMatrixField<_Real, N>;

    auto py_et = py::class_<ETensor>(module, NameMangler<ETensor>::name().c_str())
        .def(py::init<>())
        .def(py::init([](const std::string& material_file) { return Materials::Constant<N>(material_file).getTensor(); }), py::arg("material_file"))
        .def(py::init<_Real, _Real>(), py::arg("E"), py::arg("nu"))
        .def("setIsotropic", &ETensor::setIsotropic, py::arg("E"), py::arg("nu"))
        .def("setIdentity",  &ETensor::setIdentity)

        .def("getOrthotropicParameters", py::overload_cast<>(&ETensor::getOrthotropicParameters, py::const_))
        .def("anisotropy", &ETensor::anisotropy)

        .def("__call__", [](const ETensor &E, size_t i, size_t j, size_t k, size_t l) {
                    if ((i >= N) || (j >= N) || (k >= N) || (l >= N))
                        throw std::runtime_error("Index out of bounds");
                    return E(i, j, k, l);
                })
        .def_property_readonly("D", [](const ETensor &E) {
                    typename ETensor::DType D;
                    for (int i = 0; i < D.rows(); ++i) {
                        for (int j = 0; j < D.cols(); ++j) {
                            D(i, j) = E.D(i, j);
                        }
                    }
                    return D;
                })
        .def("doubleContract", [](const ETensor &E, const SMValue &smat) { return E.doubleContract(smat); }, py::arg("smat"))
        .def("doubleContract", [](const ETensor &E, const SMF     & smf) {
                    SMF result(smf.domainSize());
                    for (size_t i = 0; i < smf.domainSize(); ++i)
                        result(i) = E.doubleContract(smf(i));
                    return result;
                }, py::arg("smat"))
        // .def("doubleContract",    [](const ETensor &E, const ETensor &Eother) { return E.doubleContract(Eother);    }, py::arg("E")) // this produces a non-major-symmetric result, which we haven't bound yet
        .def("quadrupleContract", [](const ETensor &E, const ETensor &Eother) { return E.quadrupleContract(Eother); }, py::arg("E"))

        .def("computeEigenstrains", &ETensor::computeEigenstrains)

        .def("inverse",         &ETensor::inverse)
        .def("pseudoinverse",   &ETensor::inverse)
        .def("frobeniusNormSq", &ETensor::frobeniusNormSq)
        .def("transform",       &ETensor::transform, py::arg("R"), "Apply a *orthogonal* change of coordinates to this tensor")
        .def("__sub__", [](const ETensor &E, const ETensor &Eother) { return E - Eother; })
        .def("__repr__", [](const ETensor &E) {
                std::stringstream ss;
                ss << N << "D elasticity tensor with orthotropic moduli: ";
                E.printOrthotropic(ss);
                return ss.str(); })
        ;

    if (N == 3) {
        py_et.def("setOrthotropic",
            &ETensor::setOrthotropic3D,
            py::arg("Ex"),   py::arg("Ey"),   py::arg("Ez"),
            py::arg("nuYX"), py::arg("nuZX"), py::arg("nuZY"),
            py::arg("muYZ"), py::arg("myZX"), py::arg("muXY"));
    }

    if (N == 2) {
        py_et.def("setOrthotropic",
            &ETensor::setOrthotropic2D,
            py::arg("Ex"),   py::arg("Ey"),
            py::arg("nuYX"), py::arg("muXY"));
    }

    py::class_<SMValue>(detail_module, NameMangler<SMValue>::name().c_str())
        .def("__call__", [](const SMValue &sm, size_t i, size_t j) { if ((i >= N) || (j >= N)) throw std::runtime_error("Index out of bounds"); return sm(i, j); }, py::arg("i"), py::arg("j"))
        .def("toMatrix", [](const SMValue &sm) { return sm.toMatrix(); })
        .def("eigenvalues",        &SMValue::eigenvalues)
        .def("eigenDecomposition", &SMValue::eigenDecomposition)
        ;

    py::class_<SMF>(detail_module, ("SymmetricMatrixField" + std::to_string(N) + "D" + floatingPointTypeSuffix<_Real>()).c_str())
        .def("vonMises", [](const SMF &smf) {
                SMF smf_vm = vonMises(smf);
                Eigen::VectorXd result(smf_vm.domainSize());
                for (size_t i = 0; i < smf_vm.domainSize(); ++i)
                    result[i] = std::sqrt(smf_vm(i).frobeniusNormSq());
                return result;
            })
        .def("eigendecomposition", [](const SMF &smf) {
                std::vector<SMEigenDecompositionType<_Real, N>> result;
                result.reserve(smf.domainSize());
                for (size_t i = 0; i < smf.domainSize(); ++i)
                    result.push_back(smf(i).eigenDecomposition());
                return result;
            })
        .def("__call__", [](const SMF &smf, size_t i) { if (i >= smf.domainSize()) throw std::runtime_error("Index out of bounds."); return SMValue(smf(i)); })
        ;

    module.def("SymmetricMatrix", [](const Eigen::Matrix<_Real, flatLen(N), 1> &flatValues) { return SMValue(flatValues); }, py::arg("flatValues"));
    module.def("SymmetricMatrix", [](const Eigen::Matrix<_Real, N, N>          &mat)        { return SMValue(       mat); }, py::arg("mat"));
}

template<typename _Real>
void addBindings(py::module &m) {
    py::module detail_module = m.def_submodule("detail");
    py::class_<ETensorEigenDecomposition>(detail_module, "ETensorEigenDecomposition")
        .def_readonly("eigenstrains", &ETensorEigenDecomposition::strains) // flattened symmetric matrix field
        .def_readonly("eigenvalues",  &ETensorEigenDecomposition::lambdas)
        ;
    bindTensors<_Real, 2>(m, detail_module);
    bindTensors<_Real, 3>(m, detail_module);
}

PYBIND11_MODULE(tensors, m) {
    m.doc() = "Tensors and tensor fields used for elasticity simulations";

    addBindings<double>(m);
}


// test_real_concept.cpp

// Copyright Paul A. Bristow 2010.
// Copyright John Maddock 2010.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Tests real_concept for Negative Binomial and Geometric Distribution.
// find_upper_bound ...

#include <boost/math/concepts/real_concept.hpp> // for real_concept
using ::boost::math::concepts::real_concept;

#include <boost/math/distributions/geometric.hpp> // for geometric_distribution
using boost::math::geometric_distribution;
using boost::math::geometric; // using typedef for geometric_distribution<double>

#include <boost/math/distributions/negative_binomial.hpp> // for some comparisons.

#include <iostream>
using std::cout;
using std::endl;
using std::setprecision;
using std::showpoint;
#include <limits>
using std::numeric_limits;

template <class RealType>
void test_spot(RealType)
{
    using boost::math::negative_binomial_distribution;

    // NOT boost::math::negative_binomial or boost::math::geometric
    // - because then you get the default negative_binomial_distribution<double>!!!

    RealType k = static_cast<RealType>(2.L);
    RealType alpha = static_cast<RealType>(0.05L);
    RealType p = static_cast<RealType>(0.5L);
    RealType result;
    result = negative_binomial_distribution<RealType>::find_lower_bound_on_p(static_cast<RealType>(k), static_cast<RealType>(1), static_cast<RealType>(alpha));
    result = negative_binomial_distribution<RealType>::find_lower_bound_on_p(k, 1, alpha);
    result = geometric_distribution<RealType>::find_lower_bound_on_p(k, alpha);
}

int main()
{
  test_spot(boost::math::concepts::real_concept(0.)); // Test real concept.
  return 0;
}


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <string.h>
#include <opencv2/opencv.hpp>

#include <fstream>
#include <vector>
#include <cmath>
#include <map>
#include <ctime>
#include <sstream>
#include <algorithm>
#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include "colormod.h" // namespace Color

#ifndef Included_MATH_HELPER_H
#define Included_MATH_HELPER_H
#include "math_helper.h"
#endif
#ifndef Included_STRING_CONVERTOR_H
#define Included_STRING_CONVERTOR_H
#include "string_convertor.h"
#endif
#include "transformation2D.h"
#include "keyFrame.h"
#include "pair_solver.h"
#include "chris_ICP.h"

using namespace std;

Color::Modifier c_red(Color::FG_RED);
Color::Modifier c_yellow(Color::FG_YELLOW);
Color::Modifier c_green(Color::FG_GREEN);
Color::Modifier c_default(Color::FG_DEFAULT);
int numThres =100;
pair_solver mySolver(numThres);

keyFrame previousKF;
keyFrame currentRobotPose;
string currentRobotPoseStr="";
ofstream f1;

void twc_callback(const std_msgs::String::ConstPtr& msg)
 {
   //cout<<"twc_callback"<<endl;
    std::string str= msg->data.c_str();
    vector<string> sta4=string_convertor::split(str,',');
    //cout<<c_yellow<<str<<endl<<c_default;
    if(sta4.size()==18)
    {
       int mnid = std::atoi(sta4[0].c_str());
       double tsecs = atof(sta4[1].c_str());
       cv::Mat currentTWC(4,4,cv::DataType<double>::type);
       for(int i=2;i<sta4.size();i++)
       {
         double tempt=atof(sta4[i].c_str());
         int row=(i-2)/4;
         int col=i-2-4*row;
      	 currentTWC.at<double>(row,col)=tempt;
      }
      //cout<<"current TWC"<<currentTWC<<endl;
      keyFrame kf(mnid, tsecs,currentTWC);
      if(previousKF.mnFrameId ==-1)
        previousKF = kf;
      else if(keyFrame::getBaseLine(previousKF.translation,kf.translation)>0.02)
      {
        //get robot pose, add to pair
        cout<<c_red<<"add to pair, baseline: "<<keyFrame::getBaseLine(previousKF.translation,kf.translation)<<endl<<c_default;
        if(currentRobotPose.mnFrameId!=-1)
        {
          mySolver.AddNewPair(currentRobotPose, kf);
          f1<<str<<endl;
          f1<<currentRobotPoseStr<<endl;
          cout << endl << "paired samples saved!" << endl;
        }
        cout<<c_green<<"pair Count: "<<mySolver.GetPairCount()<<endl<<c_default;
        cout<<"numthres"<<numThres<<endl;
        previousKF = kf;
      }

      if(mySolver.GetPairCount()>=numThres && !(mySolver.isReady()))
      {
            //solve the eqations
            mySolver.solveTranslationICP();
            mySolver.solveOrientationSDP();
            mySolver.Save2File();

      }
      //else //wait untill the base line is Good
    }

}

void solveByLoadingFile()
{
    std::ifstream file("/home/chris/catkin_ws/src/long_range_teleoperation/robot-side/3_Calibration_Solver/dataset/data_backup.txt");
    std::string str;
    int count=0;
    keyFrame cameraPose;
    while (std::getline(file, str))
    {
      cout<<str<<endl;
      count ++;
      if(count%2==0)  //robot pose
      {
        keyFrame kf(str, 1);
        mySolver.AddNewPair(kf, cameraPose);
        cout <<"paired samples saved!" << endl;
      }
      else
      {
        keyFrame kf(str, 0);
        cameraPose = kf;
      }
    }

    //if(mySolver.GetPairCount()>=numThres && !(mySolver.isReady()))
    {
      cout<<"start solving"<<endl;
          //solve the eqations
          mySolver.solveTranslationICP();
          mySolver.solveOrientationSDP();
          mySolver.Save2File();

    }

}
string addDouble(double d)
{
  std::ostringstream ssss;
  ssss << d;
  return ssss.str();
}

string recordRobotPoseString(int mnid, double timesecs,double x, double y, double z, double qx, double qy, double qz, double qw)
{
  std::stringstream ss;
  ss<<mnid<<",";
  ss<<std::setprecision(15)<<timesecs<<",";
  std::ostringstream ssss;
  ss<<addDouble(x)<<",";
  ss<<addDouble(y)<<",";
  ss<<addDouble(z)<<",";
  ss<<addDouble(qx)<<",";
  ss<<addDouble(qy)<<",";
  ss<<addDouble(qz)<<",";
  ss<<addDouble(qw)<<",";
  return ss.str();
}
void wamPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
  //cout<<"wam msgs"<<endl;
   geometry_msgs::Pose thisPose = msg->pose;
   //cout<<c_yellow<<"current pose: "<<thisPose<<endl<<c_default;
   //cout<<"current header: "<<msg->header.stamp.toSec()<<endl;
   //note that: the robot pose, is actually, translation: from robot to endeffector. rotation: from robot to endeffector.
   //but what we use in keyframe class is, translation: from robot to end effector. Rotation: from endeffector to robot
   //so we need to inverse the robot's rotation.
   std::vector<double> v = math_helper::getInverseQuaternion(thisPose.orientation.x, thisPose.orientation.y, thisPose.orientation.z, thisPose.orientation.w);

   keyFrame fr(mySolver.GetPairCount(),msg->header.stamp.toSec(), thisPose.position.x, thisPose.position.y, thisPose.position.z, v[0], v[1], v[2], v[3]);
   currentRobotPose=fr;
   currentRobotPoseStr = recordRobotPoseString(mySolver.GetPairCount(),msg->header.stamp.toSec(), thisPose.position.x, thisPose.position.y, thisPose.position.z, v[0], v[1], v[2], v[3]);
}

//===========================MAIN FUNCTION START===========================

int main(int argc, char* argv[]){
  // Initialize the ROS system and become a node.
  bool loadFromtxtFile=false;
   if(argc==2){
        numThres = atoi(argv[1]);
        loadFromtxtFile = true;
      }

   ros::init(argc, argv, "solver");
   ros::start();
   ros::NodeHandle nh;

   if(!loadFromtxtFile)
   {
     f1.open("/home/chris/catkin_ws/src/long_range_teleoperation/robot-side/3_Calibration_Solver/dataset/data.txt");
     f1 << fixed;

    }
    else
    {
      solveByLoadingFile();
    }
    ros::Subscriber sub2 = nh.subscribe("/chris/twc", 1000, twc_callback);
    ros::Subscriber subP = nh.subscribe("/zeus/wam/pose", 1000, wamPoseCallback);
   //waitKey();
   //chris_ICP::test("p.txt");
   ros::spin();
     //declaration of function
   ros::shutdown();

   f1.close();

   return 0;
}


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

  @copyright 2015 NumScale SAS

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

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_1t.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/detail/constant/pio2_2t.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/simd/detail/constant/pio2_3t.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/quadrant.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD (rem_pio2_medium_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_ < bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 t) const
    {
      const A0 fn = nearbyint(t*Twoopi<A0>());
      A0 r  = t-fn*Pio2_1<A0>();
      A0 w  = fn*Pio2_1t<A0>();
      A0 t2 = r;
      w  = fn*Pio2_2<A0>();
      r  = t2-w;
      w  = fn*Pio2_2t<A0>()-((t2-r)-w);
      t2 = r;
      w  = fn*Pio2_3<A0>();
      r  = t2-w;
      w  = fn*Pio2_3t<A0>()-((t2-r)-w);
      return  {quadrant(fn), r-w};
    }
  };

} } }

#endif