// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <tuple>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <fft/fft2.hpp>
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

#include <operators/operators.hpp>
#include <solver/cg.hpp>
#include <solver/ridgelet_solver.hpp>

using namespace std;

typedef RT<> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;
typedef FFTr2c<PlannerR2COD> fft_t;

const double tol = 1e-9;
const int maxit = 400;
double pi = boost::math::constants::pi<double>();
bool save = false;

void dump_frc(const std::vector<rt_coeff_t>& f_rc, const RidgeletFrame& rt, std::string ffname)
{
  const char* fname = ffname.c_str();
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "Written f(lambda, t) to " << fname << "\n";
}

// --------------------------------------------------------------------------------
// solve in Ridgelet domain
template <typename ARRAY_T>
std::tuple<int, double> solve_rt(
    ARRAY_T& Fh, const RidgeletFrame& rf, const double dt, Eigen::Vector2d& v, bool log)
{
  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows

  RT_t rt(rf);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  rca_t rt_cell_array(rf);
  auto& rt_coeffs = rt_cell_array.coeffs();
  rt.rt(rt_coeffs, Fh);

  double vx = v[0];
  double vy = v[1];

  // since vx, vy = 0, this has no effect
  double Lx = 1;
  double Ly = 1;

  // initialize operators
  AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, dt);
  PTransportOp<RT_t> A(rt, AhA, vx, vy);

  RidgeletSolver<rt_coeff_t> rt_solver(rf, vx, vy, dt);
  // assert(vx == 0);
  // assert(vy == 0);
  // have to apply A.T to rhs otherwise!!!

  /////////////
  // b = A*x //
  /////////////
  rca_t x(rf);
  x.resize(rt_cell_array);
  rca_t b(rf);
  b.resize(rt_cell_array);
  x = rt_cell_array;

  // b = A*x
  // Note if v=[0,0]: A is the identity operator
  b = x;

  /* cout << "RT_SOLVER::tol  : " << tol << "\n"; */
  /* cout << "RT_SOLVER::maxit: " << maxit << "\n"; */
  Logger::GetInstance().push_prefix("RTCG");
  rt_solver.set_log(log);
  rt_solver.solve(x, A, b, tol, maxit);
  Logger::GetInstance().clear_prefix();

  if (save) {
    rt.irt(Fh, x.coeffs());
    ARRAY_T Fh2 = ftcut(Fh, Ny / 2, Nx / 2);
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> X(Ny / 2, Nx / 2);
    fft_t fft;
    fft.ift(X, Fh2);
    hid_t file = H5Fcreate("cg-result.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "X", X);
    H5Fclose(file);
  }

  return std::make_tuple(rt_solver.iter(), rt_solver.relres());

  /* cout << "RT CG::RELRES " << rt_solver.relres() << "\n" */
  /*      << "RT CG::ITER "   << rt_solver.iter() << "\n"; */
}

// --------------------------------------------------------------------------------
// solve in Ridgelet domain
template <typename ARRAY_T>
std::tuple<int, double> solve_rtnop(
    ARRAY_T& Fh, const RidgeletFrame& rf, const double dt, Eigen::Vector2d& v, bool log)
{
  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows

  RT_t rt(rf);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  rca_t rt_cell_array(rf);
  auto& rt_coeffs = rt_cell_array.coeffs();
  rt.rt(rt_coeffs, Fh);

  double vx = v[0];
  double vy = v[1];

  // since vx, vy = 0, this has no effect
  double Lx = 1;
  double Ly = 1;

  // initialize operators
  AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, dt);
  PTransportOpId<RT_t> A(rt, AhA, vx, vy);

  RidgeletSolverNOP<rt_coeff_t> rt_solver(rf, vx, vy, dt);
  // assert(vx == 0);
  // assert(vy == 0);
  // have to apply A.T to rhs otherwise!!!

  /////////////
  // b = A*x //
  /////////////
  rca_t x(rf);
  x.resize(rt_cell_array);
  rca_t b(rf);
  b.resize(rt_cell_array);
  x = rt_cell_array;

  // b = A*x
  // Note if v=[0,0]: A is the identity operator
  b = x;

  /* cout << "RT_SOLVER::tol  : " << tol << "\n"; */
  /* cout << "RT_SOLVER::maxit: " << maxit << "\n"; */
  Logger::GetInstance().push_prefix("RTCGNOP");
  rt_solver.set_log(log);
  rt_solver.solve(x, A, b, tol, maxit);
  Logger::GetInstance().clear_prefix();

  return std::make_tuple(rt_solver.iter(), rt_solver.relres());

  /* cout << "RT CG::RELRES " << rt_solver.relres() << "\n" */
  /*      << "RT CG::ITER "   << rt_solver.iter() << "\n"; */
}

// --------------------------------------------------------------------------------
// solve in Fourier domain
template <typename ARRAY_T>
std::tuple<int, double> solve_ft(ARRAY_T& Fh, const double dt, Eigen::Vector2d& v, bool log)
{
  const unsigned int Nx = Fh.cols();  // #cols
  const unsigned int Ny = Fh.rows();  // #rows

  double vx = v[0];
  double vy = v[1];
  // since vx, vy = 0, this has no effect
  double Lx = 1;
  double Ly = 1;

  // initialize operators
  AhAOp A(vx, vy, Lx, Ly, Nx, Ny, dt);

  /////////////
  // b = A*x //
  /////////////
  ARRAY_T x(Ny, Nx);
  x = Fh;

  ARRAY_T b(Ny, Nx);
  b = x;

  Logger::GetInstance().push_prefix("FTCG");
  CG cg;
  cg.set_log(log);
  cg.solve(x, A, b, tol, maxit);
  Logger::GetInstance().clear_prefix();

  return std::make_tuple(cg.iter(), cg.relres());

  /* cout << "FT CG::RELRES " << cg.relres() << "\n" */
  /*      << "FT CG::ITER "  << cg.iter() << "\n"; */
}

int main(int argc, char* argv[])
{
  SOURCE_INFO();

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;
  int nv;
  double dt, r;
  bool log;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("rad,r", po::value<double>(&r)->default_value(1), "|v|")
      /* ("vx", po::value<double>(&vx)->default_value(1), "vx") */
      /* ("vy", po::value<double>(&vy)->default_value(0), "vy") */
      ("dt,t", po::value<double>(&dt)->default_value(0.1), "dt")
      ("nv", po::value<int>(&nv)->default_value(6), "#directons in velocity")
      ("log", po::value<bool>(&log)->default_value(false), "log cg history")
      ("smooth", "use smooth initial condition")
      ("save", "save solution");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  if (vm.count("save")) save = true;

  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n"
       << setw(20) << "dt"
       << ": " << dt << "\n"
       << setw(20) << "|v|"
       << ": " << r << "\n";

  RidgeletFrame rf(Jx, Jy, rho_x, rho_y);

  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows
  cout << "Nx: " << Nx << "\n";
  cout << "Ny: " << Ny << "\n";

  auto phi = 2 * pi * Eigen::VectorXd::LinSpaced(nv + 1, 0, 1);

  for (int i = 0; i < nv; ++i) {
    cout << "---------- phi = " << phi[i] << " ----------\n";
    const double vx = std::cos(phi[i]) * r;
    const double vy = std::sin(phi[i]) * r;
    Eigen::Vector2d v;
    v[0] = vx;
    v[1] = vy;

    Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx, 0, 1);
    Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny, 0, 1);
    array_t F(Ny, Nx);
    F = (-100 * ((yi - 0.5)).cwiseAbs2()).exp().replicate(1, xi.rows()) *
        (-100 * ((xi - 0.5).cwiseAbs2())).exp().transpose().replicate(yi.rows(), 1);
    if (!vm.count("smooth")) {
      F = (((yi - 0.5).replicate(1, xi.rows()) * vy -
            (xi - 0.5).transpose().replicate(yi.rows(), 1) * vx) <=
           0).select(F, Eigen::ArrayXXd::Zero(Ny, Nx));
    }

    hid_t file = H5Fcreate("cg.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "x0", F);
    H5Fclose(file);

    // F.setOnes();

    // ----------------------------------------
    /* double n[2] = {1,1}; */
    /* F = ((n[0]*(yi.replicate(1, xi.rows())-0.5*pi) + n[1]*(xi.transpose().replicate(yi.rows(),
     * 1)-0.5*pi)) > 1e-8) */
    /*   .select(F, array_t::Zero(Ny, Nx)); */
    fft_t fft;
    complex_array_t Fh(Ny, Nx);
    fft.ft(Fh, F, false);

    auto ret_rt = solve_rt(Fh, rf, dt, v, log);
    auto ret_ft = solve_ft(Fh, dt, v, log);
    auto ret_rtnop = solve_rtnop(Fh, rf, dt, v, log);

    cout << "RTCG " << std::scientific << std::setprecision(3) << std::setw(7) << vx << " "
         << std::setw(7) << vy << " " << std::setw(7) << std::get<1>(ret_rt) << " " << std::setw(7)
         << std::get<0>(ret_rt) << "\n";
    cout << "RTCGNOP " << std::scientific << std::setprecision(3) << std::setw(7) << vx << " "
         << std::setw(7) << vy << " " << std::setw(7) << std::get<1>(ret_rtnop) << " "
         << std::setw(7) << std::get<0>(ret_rtnop) << "\n";
    cout << "FTCG " << std::scientific << std::setprecision(3) << std::setw(7) << vx << " "
         << std::setw(7) << vy << " " << std::setw(7) << std::get<1>(ret_ft) << " " << std::setw(7)
         << std::get<0>(ret_ft) << "\n";

    /* std::vector<double> diagp = make_inv_diagonal_preconditioner(rf, dt*vx, dt*vy); */
    /* cout  << "\n"; */
    /* for (int i = 0; i < diagp.size(); ++i) { */
    /*   cout << diagp[i] << "\n"; */
    /* } */
    /* cout << "\n"; */
  }

  return 0;
}


#include "shiftinvert_solver.hpp"

#include <Eigen/Dense>
#include <cmath>
#include <complex>
#include <fmt/format.h>

using namespace Eigen;

ShiftinvertSolver::ShiftinvertSolver(const Ref<const MatrixXd> &matA,
                                     const Ref<const MatrixXd> &matB,
                                     int maxiter, double tol)
    : ndim_(matA.rows()), matA_(matA), matB_(matB), maxiter_(maxiter),
      tol_(tol) {}

std::complex<double> ShiftinvertSolver::compute(std::complex<double> sigma) {
  MatrixXcd matL = matA_ - sigma * matB_;
  auto lhh = matL.householderQr();
  VectorXcd x = VectorXcd::Random(ndim_);
  std::complex<double> lamb;
  std::complex<double> lamb_pre = 1.0e10;
  for (int i = 0; i < maxiter_; ++i) {
    VectorXcd u = x / x.norm();
    VectorXcd vecR = matB_ * u;
    x = lhh.solve(vecR);
    lamb = u.dot(x);
    lamb = 1.0 / lamb + sigma;
    double diff = std::abs(lamb - lamb_pre);
    // fmt::print("{:d} {:12.5f}{:12.5f}({:12.5e})\n", i, lamb.real(),
    // lamb.imag(),
    //            diff);
    if (diff < tol_) {
      break;
    } else {
      lamb_pre = lamb;
    }
  }
  return lamb;
}


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
//#include "cor_type/sources/math/vector2_tmpl_impl.h"
//#include "cor_type/sources/primitive/box_tmpl_impl.h"
#include "cor_type/sources/primitive/box.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(box)

BOOST_AUTO_TEST_CASE(box2d)
{

    typedef cor::type::Box2F Box;

    Box b0(0.0f, 0.0f, 1.0f, 1.0f);

    cor::RInt32 i;
    cor::RInt32 j;

    for(i = 0 ; i < 2 ; i++)
    {
        for(j = 0 ; j < 2 ; j++)
        {
            Box b1(j - 0.5f, i - 0.5f, 1.0f, 1.0f);
            BOOST_CHECK(b0.is_cross(b1));
        }

    }

    for(i = 0 ; i < 2 ; i++)
    {
        for(j = 0 ; j < 2 ; j++)
        {
            Box b1(j * 3 - 1.5f, i * 3 - 1.5f, 1.0f, 1.0f);
            BOOST_CHECK(!b0.is_cross(b1));
        }

    }

}

BOOST_AUTO_TEST_CASE(box2d_distance)
{

    typedef cor::type::Box2F Box;
    typedef cor::type::Vector2F V;

    Box b0(0.0f, 0.0f, 1.0f, 1.0f);
    V v(2.0f, 2.0f);

    auto d = b0.get_distance(v);
    BOOST_CHECK_CLOSE(d, sqrtf(2.0f), 0.0001f);

}

BOOST_AUTO_TEST_SUITE_END()


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::vector_type                                vector_type;
typedef std::vector<double>                                    knot_container;
typedef std::vector<vector_type>                               point_container;

typedef OpenTissue::spline::NUBSpline<knot_container, point_container> spline_type;

BOOST_AUTO_TEST_SUITE(opentissue_spline_compute_derivatives);

BOOST_AUTO_TEST_CASE(test_compute_derivatives)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);  //k = 3
  U.push_back(1.0);
  U.push_back(2.0);
  U.push_back(3.0);
  U.push_back(4.0);  // n = 6  => |P| = 7
  U.push_back(5.0);
  U.push_back(5.0);
  U.push_back(5.0);  // m = 9  => |U| = 10

  // Indices of basis functions belongs to the interval [0..n]

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 0.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 0.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;
  vector_type p4(2);  p4(0) = 4.0; p4(1) = 0.0;
  vector_type p5(2);  p5(0) = 5.0; p5(1) = 0.0;
  vector_type p6(2);  p6(0) = 6.0; p6(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);
  P.push_back(p4);
  P.push_back(p5);
  P.push_back(p6);

  spline_type spline(3,U,P);

  double const tolerance = 0.00001;

  point_container dC0;
  point_container dC1;
  point_container dC2;
  point_container dC3;
  point_container dC4;
  point_container dC5;
  point_container dC6;

  double const du = 5.0/6.0;
  double u = 0.0;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC0, math_types() );
  u += du;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC1, math_types());
  u += du;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC2, math_types());
  u += du;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC3, math_types());
  u += du;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC4, math_types());
  u += du;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC5, math_types());
  u += du;
  OpenTissue::spline::compute_spline_derivatives(spline, u, 1, dC6, math_types());

  BOOST_CHECK_CLOSE( dC0[0](1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( dC1[0](1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( dC2[0](1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( dC3[0](1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( dC4[0](1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( dC5[0](1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( dC6[0](1), 0.0, tolerance );

  //BOOST_CHECK( dC0[0](0) > 0.0 ); // why is this zero?
  BOOST_CHECK( dC1[0](0) > 0.0 );
  BOOST_CHECK( dC2[0](0) > 0.0 );
  BOOST_CHECK( dC3[0](0) > 0.0 );
  BOOST_CHECK( dC4[0](0) > 0.0 );
  BOOST_CHECK( dC5[0](0) > 0.0 );
  BOOST_CHECK( dC6[0](0) > 0.0 );

  //std::cout << dC0[0](0) << " " << dC0[0](1) << std::endl;
  //std::cout << dC1[0](0) << " " << dC1[0](1) << std::endl;
  //std::cout << dC2[0](0) << " " << dC2[0](1) << std::endl;
  //std::cout << dC3[0](0) << " " << dC3[0](1) << std::endl;
  //std::cout << dC4[0](0) << " " << dC4[0](1) << std::endl;
  //std::cout << dC5[0](0) << " " << dC5[0](1) << std::endl;
  //std::cout << dC6[0](0) << " " << dC6[0](1) << std::endl;
}

BOOST_AUTO_TEST_CASE(test_bezier_curve_case)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(1.0);
  U.push_back(1.0);
  U.push_back(1.0);
  U.push_back(1.0);

  // k = 4
  // n = 3  => |P| = 4
  // m = 7  => |U| = 8

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 1.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 1.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);

  spline_type bezier(4,U,P);

  double const tolerance = 10e-15;

  // Now we have constructed the equivalent of a bezier curve
  //
  //   B(t) =     a t^3 +   b t^2 + c t + d
  //   B'(t) =  3 a t^2 + 2 b t   + c
  //
  //   B(0) = p0  = d
  //   B(1) = p3  = a + b + c + d
  //   B'(0) = 3*(p1-p0) = c
  //   B'(1) = 3*(p1-p0) = 3 a + 2 b + c
  //
  //  working out the equations one would get
  vector_type a = -1*p0 + 3*p1 - 3*p2 + 1*p3;
  vector_type b =  3*p0 - 6*p1 + 3*p2;
  vector_type c = -3*p0 + 3*p1;
  vector_type d =  1*p0;

  double u = 0.0;
  int const N = 11;
  double const du = 1.0 / (N-1);
  for(int i=0;i<N;++i)
  {
    point_container dC;
    // ask for zero'th, first and second order derivatives
    OpenTissue::spline::compute_spline_derivatives(bezier, u, 2, dC, math_types() );

    vector_type q(2);
    q = (u*u*u)*a + (u*u)*b + u*c + d;

    vector_type dq(2);
    dq = 3*(u*u)*a + 2*(u)*b + c;

    vector_type ddq(2);
    ddq = 6*u*a + 2*b;

    BOOST_CHECK( std::fabs( dC[0](0) - q(0) ) < tolerance );
    BOOST_CHECK( std::fabs( dC[0](1) - q(1) ) < tolerance );

    BOOST_CHECK( std::fabs( dC[1](0) - dq(0) ) < tolerance );
    BOOST_CHECK( std::fabs( dC[1](1) - dq(1) ) < tolerance );

    BOOST_CHECK( std::fabs( dC[2](0) - ddq(0) ) < tolerance );
    BOOST_CHECK( std::fabs( dC[2](1) - ddq(1) ) < tolerance );
    u += du;
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include "convex_hull.hpp"
#include <Eigen/Geometry>
#include "ear/helpers/assert.hpp"

namespace ear {

  std::vector<Facet> convex_hull(const std::vector<Eigen::Vector3d> &positions,
                                 double tolerance) {
    using Vec = Eigen::Vector3d;
    using Tri = std::array<size_t, 3>;

    // the mean point, guaranteed to be inside the convex hull
    Vec inside_point = Vec::Zero();
    for (auto &pos : positions) inside_point += pos;
    inside_point /= (double)positions.size();

    // find all triangles on the convex hull:
    // - iterate through all possible triangles
    // - find the triangle normal, checking for collinearity
    // - it's on the convex hull if:
    //    - the inside point is not on the plane of the triangle
    //    - all points are on the same side of the plane of the triangle as the
    //    inside point
    std::vector<Tri> hull_tris;
    std::vector<Vec> hull_tri_normals;

    for (size_t i = 0; i < positions.size(); i++)
      for (size_t j = i + 1; j < positions.size(); j++)
        for (size_t k = j + 1; k < positions.size(); k++) {
          Vec normal =
              (positions[j] - positions[i]).cross(positions[k] - positions[i]);
          ear_assert(normal.squaredNorm() > tolerance,
                     "collinear points in convex hull");
          normal.normalize();  // XXX: not really required, but helps make
                               // tolerances consistent

          double dot_inside = normal.dot(inside_point - positions[i]);

          if (std::abs(dot_inside) < tolerance)
            continue;  // tri coplanar with inside point

          bool points_on_same_side_as_inside = true;
          for (auto &pos : positions) {
            double dot_point = normal.dot(pos - positions[i]);

            // check if signs are equal, with tolerance
            if (!(dot_inside > 0 ? dot_point > -tolerance
                                 : dot_point < tolerance)) {
              points_on_same_side_as_inside = false;
              break;
            }
          }

          if (points_on_same_side_as_inside) {
            hull_tris.push_back({i, j, k});
            hull_tri_normals.push_back(normal);
          }
        }

    // merge coplanar triangles into facets
    std::vector<Facet> hull_facets;
    std::vector<Vec> hull_facet_normals;

    for (size_t tri_i = 0; tri_i < hull_tris.size(); tri_i++) {
      const Vec &tri_point = positions[hull_tris[tri_i][0]];
      const Vec &tri_norm = hull_tri_normals[tri_i];

      // check each facet to see if this triangle is on the same plane.
      // if it is, add the points to the facet. if no facet is found, make a
      // new facet.
      bool found_facet = false;
      for (size_t facet_i = 0; facet_i < hull_facets.size(); facet_i++) {
        const Vec &facet_norm = hull_facet_normals[facet_i];
        const Vec &facet_point = positions[*hull_facets[facet_i].begin()];

        // they are on the same plane if the line between the test points is
        // not along the normal, and if the normals point in the same or
        // opposite direction. alternatively, we could just test all tri
        // vertices.
        if (std::abs((tri_point - facet_point).dot(facet_norm)) < tolerance &&
            facet_norm.cross(tri_norm).squaredNorm() < tolerance) {
          for (size_t corner_idx : hull_tris[tri_i]) {
            hull_facets[facet_i].insert((Facet::value_type)corner_idx);
          }

          found_facet = true;
          break;
        }
      }

      if (!found_facet) {
        Facet f;
        for (size_t corner_idx : hull_tris[tri_i])
          f.insert((Facet::value_type)corner_idx);
        hull_facets.push_back(std::move(f));
        hull_facet_normals.push_back(tri_norm);
      }
    }

    return hull_facets;
  }
}  // namespace ear


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_pio2 capabilities

    Computes the remainder modulo \f$\pi/2\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r;
    as_integer<T> n;
    std::tie(n, r) = rem_pio2(x);
    @endcode

    is similar to:

    @code
    as_integer<T> n = div(inearbyint, x, Pio_2<T>());
    T r =  remainder(x, Pio_2<T>());
    @endcode

  **/
  std::pair<IntegerValue, Value> rem_pio2(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/rem_pio2.hpp>
#include <boost/simd/function/simd/rem_pio2.hpp>

#endif


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

#include <pch_light.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include "test_jacobi_theta.hpp"

// Test file for the Jacobi Theta functions, a.k.a the four horsemen of the
// Jacobi elliptic integrals. At the moment only Wolfrma Alpha spot checks are
// used. We should generate extra-precise numbers with NTL::RR or some such.

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                  // test type(s)
      ".*Small Tau.*",      // test data group
      ".*", 1000, 200);  // test function

   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                  // test type(s)
      ".*Wolfram Alpha.*",      // test data group
      ".*", 60, 15);  // test function

   // Catch all cases come last:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                  // test type(s)
      ".*",      // test data group
      ".*", 20, 5);  // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
    expected_results();
    BOOST_MATH_CONTROL_FP;
    BOOST_MATH_STD_USING

    using namespace boost::math;

    BOOST_CHECK_THROW(jacobi_theta1(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta1(0.0, 1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta2(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta2(0.0, 1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta3(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta3(0.0, 1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta4(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta4(0.0, 1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta1tau(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta1tau(0.0, -1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta2tau(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta2tau(0.0, -1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta3tau(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta3tau(0.0, -1.0), std::domain_error);

    BOOST_CHECK_THROW(jacobi_theta4tau(0.0, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jacobi_theta4tau(0.0, -1.0), std::domain_error);

    double eps = std::numeric_limits<double>::epsilon();
    for (double q=0.0078125; q<1.0; q += 0.0078125) { // = 1/128
        for (double z=-8.0; z<=8.0; z += 0.125) {
            test_periodicity(z, q, 100 * eps);
            test_argument_translation(z, q, 100 * eps);
            test_sums_of_squares(z, q, 100 * eps);
            // The addition formula is complicated, cut it some extra slack
            test_addition_formulas(z, constants::ln_two<double>(), q, sqrt(sqrt(eps)));
            test_duplication_formula(z, q, 100 * eps);
            test_transformations_of_nome(z, q, 100 * eps);
            test_watsons_identities(z, 0.5, q, 101 * eps);
            test_landen_transformations(z, -log(q)/constants::pi<double>(), sqrt(eps));
            test_elliptic_functions(z, q, 5 * sqrt(eps));
        }
        test_elliptic_integrals(q, 10 * eps);
    }

    test_special_values(eps);

    for (double s=0.125; s<3.0; s+=0.125) {
        test_mellin_transforms(2.0 + s, eps, 3 * eps);
        test_laplace_transforms(s, eps, 4 * eps);
    }

    test_spots(0.0F, "float");
    test_spots(0.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_spots(0.0L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
    test_spots(concepts::real_concept(0), "real_concept");
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::endl;
#endif
}


#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <string>
#include <vector>

using namespace std;
using boost::lexical_cast;
using boost::multiprecision::cpp_int;

/*
Just a normal bruteforce, we'll generate the first couple dozen thousand cubes, sort them as strings (to match permutations) into a vector, then run a count on them.
*/

int main(int argc, char *argv[]) {
	vector<string> permutations;
	// Generate all cubes.
	for(int i = 0; i < 10'000; i++) {
		cpp_int cube = boost::multiprecision::pow((cpp_int)i, 3);
		string t = lexical_cast<string>(cube);
		sort(t.begin(), t.end());
		permutations.push_back(t);
	}
	// Go through it and count occurances of each string, break when we find what we need.
	for(int i = 0; i < permutations.size(); i++) {
		int existingPermutations = count(permutations.begin(), permutations.end(), permutations[i]);
		if(existingPermutations == 5) {
			cpp_int cube = boost::multiprecision::pow((cpp_int)i, 3);
			cout << cube << endl;
			break;
		}
	}
	return 0;
}


#ifndef Circumference_hpp
#define Circumference_hpp

#include <shapes/Circle.hpp>
#include <shapes/Rectangle.hpp>

#include <boost/math/constants/constants.hpp>
namespace bmc = boost::math::constants;

float
circumference( Rectangle const & r )
{
  return 2 * r.width + 2 * r.height;
}

float
circumference( Circle const & c )
{
  return 2 * bmc::pi<float>() * c.radius;
}

#endif // Circumference_hpp


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

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // matlab
    // >> samples = [1:5];
    // >> mean(samples)
    // ans = 3
    // >> sum(samples .* samples) / length(samples)
    // ans = 11
    // >> sum(samples .* samples) / length(samples) - mean(samples)^2
    // ans = 2

    // lazy variance, now lazy with syntactic sugar, thanks to Eric
    accumulator_set<int, stats<tag::variance(lazy)> > acc1;

    acc1(1);
    acc1(2);
    acc1(3);
    acc1(4);
    acc1(5);

    BOOST_CHECK_EQUAL(5u, count(acc1));
    BOOST_CHECK_CLOSE(3., mean(acc1), 1e-5);
    BOOST_CHECK_CLOSE(11., moment<2>(acc1), 1e-5);
    BOOST_CHECK_CLOSE(2., variance(acc1), 1e-5);

    // immediate variance
    accumulator_set<int, stats<tag::variance> > acc2;

    acc2(1);
    acc2(2);
    acc2(3);
    acc2(4);
    acc2(5);

    BOOST_CHECK_EQUAL(5u, count(acc2));
    BOOST_CHECK_CLOSE(3., mean(acc2), 1e-5);
    BOOST_CHECK_CLOSE(2., variance(acc2), 1e-5);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("variance test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include <vector>
#include <boost/math/distributions/lognormal.hpp>
#include "lognormal_dist.h"

stochastic::LognormalDistribution::LognormalDistribution(double mean, double std_dev)
  : Distribution(),
    mean_{mean},
    std_dev_{std_dev},
    distribution_{mean, std_dev_}
{}

std::vector<double> stochastic::LognormalDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::LognormalDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}


#ifndef AMT_ML_MODEL_LINEAR_REGRESSION_OPTIMIZER_HPP
#define AMT_ML_MODEL_LINEAR_REGRESSION_OPTIMIZER_HPP

#include <armadillo>
#include <functional>
#include <cmath>

namespace amt::classification{

    struct default_opt{

    //     auto operator()(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const {
    //         // x_t = x^T
    //         auto x_t = x.t();

    //         // xt_y = x^T * y
    //         auto xt_y = x_t * y;

    //         if( lm == 0 ){
    //             // xt_x = x^T * x
    //             auto xt_x = x_t * x;

    //             // (X * X^T) * B = X^T * Y
    //             beta = arma::solve(xt_x,xt_y,arma::solve_opts::fast);
    //         }else{

    //             arma::Mat<double> l_I = arma::eye(x.n_cols, x.n_cols) * lm;

    //             // xt_x = x^T * x + l_I
    //             auto xt_x_lI = ( x_t * x ) + l_I;

    //             // (X * X^T) * B = X^T * Y
    //             beta = arma::solve(xt_x_lI,xt_y,arma::solve_opts::fast);
    //         }

    //     }

    };

    struct gradient_descent{

        gradient_descent() = default;
        gradient_descent(gradient_descent const&  other) = default;
        gradient_descent(gradient_descent &&  other) = default;
        gradient_descent& operator=(gradient_descent const&  other) = default;
        gradient_descent& operator=(gradient_descent &&  other) = default;
        ~gradient_descent() = default;

        template<typename Fn>
        gradient_descent(double alpha, std::size_t iter, Fn&& fn)
            : alpha(alpha)
            , iteration(iter)
            , fn(std::move(fn))
        {}

        gradient_descent(double alpha, std::size_t iter = 300000)
            : alpha(alpha)
            , iteration(iter)
        {}

        auto operator()(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y) const {
            beta = arma::zeros(x.n_cols, 1ul);
            eval(beta,x,y);
        }

        void eval(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y) const{
            auto m = alpha / static_cast<double>(x.n_rows);
            auto x_t = x.t();
            for( auto i = 0u; i < iteration; ++i ){
                auto p = ( x * beta );
                auto h = apply_fn(p,fn);
                arma::Mat<double> grad = ( x_t * (h - y) );
                beta -= ( m * grad );
            }
        }

        arma::Mat<double> apply_fn(arma::Mat<double> const& mat, std::function<double(double)> const& f) const{
            arma::Mat<double> res(mat.n_rows, mat.n_cols);
            for(auto i = 0u; i < mat.n_rows; ++i)
                for(auto j = 0u; j < mat.n_cols; ++j)
                    res(i,j) = f(mat(i,j));
            return res;
        }

        double alpha{0.1};
        std::size_t iteration{300000};
        std::function<double(double)> fn = [](double el){
            return 1.0 / ( 1.0 + std::exp(-el) );
        };
        std::function<double(double)> diff_fn = [](double el){
            auto temp = std::exp(-el);
            return temp / ( 1.0 + temp );
        };
    };

    template<typename T>
    inline static constexpr bool is_default_opt_v = std::is_same_v<T,default_opt>;

    template<typename T>
    inline static constexpr bool is_gradient_descent_v = std::is_same_v<T,gradient_descent>;

} // namespace amt::classification

#endif


#include "ExperimentAmbientCube.h"

#include <Eigen/Eigen>
#include <Eigen/nnls.h>

namespace Probulator {

ExperimentAmbientCube::AmbientCube ExperimentAmbientCube::solveAmbientCubeLeastSquares(const ImageBase<vec3>& directions, const Image& irradiance)
{
	using namespace Eigen;

	AmbientCube ambientCube;

	const u64 sampleCount = directions.getPixelCount();

	MatrixXf A;
	A.resize(sampleCount, 6);

	for (u64 sampleIt = 0; sampleIt < sampleCount; ++sampleIt)
	{
		const vec3& direction = directions.at(sampleIt);
		vec3 dirSquared = direction * direction;

		if (direction.x < 0)
		{
			A(sampleIt, 0) = dirSquared.x;
			A(sampleIt, 1) = 0.0f;
		}
		else
		{
			A(sampleIt, 0) = 0.0f;
			A(sampleIt, 1) = dirSquared.x;
		}

		if (direction.y < 0)
		{
			A(sampleIt, 2) = dirSquared.y;
			A(sampleIt, 3) = 0.0f;
		}
		else
		{
			A(sampleIt, 2) = 0.0f;
			A(sampleIt, 3) = dirSquared.y;
		}

		if (direction.z < 0)
		{
			A(sampleIt, 4) = dirSquared.z;
			A(sampleIt, 5) = 0.0f;
		}
		else
		{
			A(sampleIt, 4) = 0.0f;
			A(sampleIt, 5) = dirSquared.z;
		}
	}

	NNLS<MatrixXf> solver(A);

	VectorXf b;
	b.resize(sampleCount);

	for (u32 channelIt = 0; channelIt < 3; ++channelIt)
	{
		for (u64 sampleIt = 0; sampleIt < sampleCount; ++sampleIt)
		{
			b[sampleIt] = irradiance.at(sampleIt)[channelIt];
		}

		solver.solve(b);
		VectorXf x = solver.x();

		for (u64 basisIt = 0; basisIt < 6; ++basisIt)
		{
			ambientCube.irradiance[basisIt][channelIt] = x[basisIt];
		}
	}

	return ambientCube;
}

ExperimentAmbientCube::AmbientCube ExperimentAmbientCube::solveAmbientCubeProjection(const Image& irradiance)
{
	AmbientCube ambientCube;

	vec3 cubeDirections[6] =
	{
		vec3(-1.0f, 0.0f, 0.0f),
		vec3(+1.0f, 0.0f, 0.0f),
		vec3(0.0f, -1.0f, 0.0f),
		vec3(0.0f, +1.0f, 0.0f),
		vec3(0.0f, 0.0f, -1.0f),
		vec3(0.0f, 0.0f, +1.0f),
	};

	for(u32 i=0; i<6; ++i)
	{
		vec2 texcoord = cartesianToLatLongTexcoord(cubeDirections[i]);
		ambientCube.irradiance[i] = (vec3)irradiance.sampleNearest(texcoord);
	}

	return ambientCube;
}

void ExperimentAmbientCube::run(SharedData& data)
{
	AmbientCube ambientCube;

	if (m_projectionEnabled)
	{
		ambientCube = solveAmbientCubeProjection(m_input->m_irradianceImage);
	}
	else
	{
		ambientCube = solveAmbientCubeLeastSquares(data.m_directionImage, m_input->m_irradianceImage);
	}

	m_radianceImage = Image(data.m_outputSize);
	m_irradianceImage = Image(data.m_outputSize);

	data.m_directionImage.forPixels2D([&](const vec3& direction, ivec2 pixelPos)
	{
		vec3 sampleIrradianceH = ambientCube.evaluate(direction);
		m_irradianceImage.at(pixelPos) = vec4(sampleIrradianceH, 1.0f);
		m_radianceImage.at(pixelPos) = vec4(0.0f);
	});
}

}


#include "math_helper.h"

#include <iostream>
#include <Eigen/LU>
#include <Eigen/Dense>

namespace recon
{

namespace MathHelper
{

void getTopViewHomography(const double* cparams, int src_w, int src_h, int dst_w, int dst_h,
                          const Eigen::Vector4d& plane, Eigen::Matrix3d& H)
{
  double fx = cparams[0];
  double fy = cparams[1];
  double cx = cparams[2];
  double cy = cparams[3];

  Eigen::Vector3d n;
  n << plane[0], plane[1], plane[2];
  double d = plane[3];
  if(d < 0.0) {
    throw "Error!\n";
  }
  std::cout << "n = \n" << n << "\n\n";
  std::cout << "d = " << d << "\n\n";

  Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity();
  K1(0,0) = fx;
  K1(1,1) = fy;
  K1(0,2) = cx;
  K1(1,2) = cy;
  Eigen::Matrix3d K1_inv = K1.inverse();

  Eigen::Vector3d q1a;
  Eigen::Vector3d q1b;
  q1a << (double) 0.0, (double) src_h - 1.0, 1.0;
  q1b << (double) src_w - 1.0, (double) src_h - 1.0, 1.0;
  q1a = K1_inv * q1a;
  q1b = K1_inv * q1b;

  Eigen::Vector3d Q1a, Q1b;
  Q1a = (-d * q1a) / n.dot(q1a);
  Q1b = (-d * q1b) / n.dot(q1b);
  std::cout << "Q1a:\n" << Q1a << "\nQ1b:\n" << Q1b << "\n\n";

  // determine R matrix
  Eigen::Matrix3d R;
  //Eigen::Vector3d Q1diff = Q1a - Q1b;
  Eigen::Vector3d Q1diff = Q1b - Q1a;
  std::cout << Q1diff << "\n";
  R.row(0) = Q1diff / Q1diff.norm();
  R.row(2) = -n;
  R.row(1) = -(R.row(0).cross(R.row(2)));

  std::cout << "0 == " << Q1diff.dot(n) << "\n";

  //double Ty = -10.0 - (R.row(1) * Q1a);
  double Ty = 10.0 - (R.row(1) * Q1a);
  std::cout << "Ty = " << Ty << "\n";
  Eigen::Vector3d t;
  t << 0.0, Ty, 0.0;

  Eigen::Vector3d Q2a = R * Q1a + t;
  Eigen::Vector3d Q2b = R * Q1b + t;
  std::cout << "Q2a:\n" << Q2a << "\nQ2b:\n" << Q2b << "\n\n";;
  double dst_cx = dst_w / 2.0;
  double dst_cy = dst_h / 2.0;
  double f2 = (dst_cy - 1.0) * (Q2a[2] / Q2a[1]);

  Eigen::Matrix3d K2;
  K2 << f2,   0.0,  dst_cx,
        0.0,  f2,   dst_cy,
        0.0,  0.0,  1.0;
  std::cout << "K2:\n" << K2 << "\n";
  std::cout << "q2a:\n" << (K2 * Q2a) / Q2a[2] << "\nq2b:\n" << (K2 * Q2b) / Q2b[2] << "\n\n";;

  //std::cout << "d = " << d << "\n";
  //std::cout << "<n,Q1a> = " << n.dot(Q1a) << "\n\n";
  Eigen::Matrix3d H_n = R - ((t * n.transpose()) / d);
  std::cout << "Q2a_H:\n" << H_n * Q1a << "\n\n";
  H = K2 * H_n * K1_inv;
  Eigen::Vector3d q2a_H = H * q1a;
  q2a_H = q2a_H / q2a_H[2];
  std::cout << "q2a_H:\n" << q2a_H << "\n\n";
}

void debugTopViewHomography(const double* cparams, int src_w, int src_h, int dst_w, int dst_h,
                            const Eigen::Vector4d& plane, pcl::visualization::PCLVisualizer& viewer)
{
  double fx = cparams[0];
  double fy = cparams[1];
  double cx = cparams[2];
  double cy = cparams[3];

  Eigen::Vector3d n;
  n << plane[0], plane[1], plane[2];
  double d = plane[3];
  if(d < 0.0) {
    throw "Error!\n";
  }
  std::cout << "n = \n" << n << "\n\n";
  std::cout << "d = " << d << "\n\n";

  Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity();
  K1(0,0) = fx;
  K1(1,1) = fy;
  K1(0,2) = cx;
  K1(1,2) = cy;
  Eigen::Matrix3d K1_inv = K1.inverse();

  Eigen::Vector3d q1a;
  Eigen::Vector3d q1b;
  q1a << (double) 0.0, (double) src_h - 1.0, 1.0;
  q1b << (double) src_w - 1.0, (double) src_h - 1.0, 1.0;
  q1a = K1_inv * q1a;
  q1b = K1_inv * q1b;

  Eigen::Vector3d Q1a, Q1b;
  Q1a = (-d * q1a) / n.dot(q1a);
  Q1b = (-d * q1b) / n.dot(q1b);
  std::cout << "Q1a:\n" << Q1a << "\nQ1b:\n" << Q1b << "\n\n";

  pcl::PointXYZ pt1, pt2;
  for(int i = 0; i < 3; i++) {
    pt1.data[i] = Q1a[i];
    pt2.data[i] = Q1b[i];
  }
  viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2, 0, 200, 0, "line1");
}

void projectPoint(const double* cam_params, Eigen::Vector3d& point, Eigen::Vector2d& proj)
{
  double fx = cam_params[0];
  double fy = cam_params[1];
  double cx = cam_params[2];
  double cy = cam_params[3];

  proj[0] = point[0] / point[2];
  proj[1] = point[1] / point[2];
  proj[0] = fx * proj[0] + cx;
  proj[1] = fy * proj[1] + cy;
}

void setTransform2DEM(Eigen::Vector4d& plane_model, Eigen::Matrix4d& transform)
{
  Eigen::Vector3d n1, n2;
  for(int i = 0; i < 3; i++) {
    n1[i] = plane_model[i];
    n2[i] = 0.0;
  }
  n2[1] = 1.0;
  // find axis with cross product

  // find angle with dot product

  // move Y with d after rotation
}

void createPolyFromPlane(const Eigen::Vector4d& plane, pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d)
{
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  pt3d.at<double>(0) = (x - cx) * b / disp;
  pt3d.at<double>(1) = (y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void getSignedDistancesToModel(const Eigen::Vector4d& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                                           std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = p / norm;
  }
}

void getDistancesToModel(const Eigen::Vector4d& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                         std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = std::abs(p / norm);
  }
}

}

}


#include <NTL/mat_poly_lzz_p.h>

#include <NTL/new.h>

NTL_START_IMPL

static
void HessCharPoly(zz_pX& g, const zz_pX& a, const zz_pX& f)
{
   long n = deg(f);
   if (n <= 0 || deg(a) >= n)
      Error("HessCharPoly: bad args");

   mat_zz_p M;
   M.SetDims(n, n);

   long i, j;

   zz_pX t;
   t = a;

   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++)
         M[i][j] = coeff(t, j);

      if (i < n-1)
         MulByXMod(t, t, f);
   }

   CharPoly(g, M);
}

void CharPolyMod(zz_pX& g, const zz_pX& a, const zz_pX& ff)
{
   zz_pX f = ff;
   MakeMonic(f);
   long n = deg(f);

   if (n <= 0 || deg(a) >= n)
      Error("CharPoly: bad args");

   if (IsZero(a)) {
      clear(g);
      SetCoeff(g, n);
      return;
   }

   if (n > 90 || (zz_p::PrimeCnt() <= 1 && n > 45)) {
      zz_pX h;
      MinPolyMod(h, a, f);
      if (deg(h) == n) {
         g = h;
         return;
      }
   }

   if (zz_p::modulus() < n+1) {
      HessCharPoly(g, a, f);
      return;
   }

   vec_zz_p u(INIT_SIZE, n+1), v(INIT_SIZE, n+1);

   zz_pX h, h1;
   negate(h, a);
   long i;

   for (i = 0; i <= n; i++) {
      u[i] = i;
      add(h1, h, u[i]);
      resultant(v[i], f, h1);
   }

   interpolate(g, u, v);
}

NTL_END_IMPL


/*
2019.03.29 by Aurora. Contact:fanyi@mail.ustc.edu.cn

Utilities functions.

*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
/*#include <complex.h>*/
#include <algorithm>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include "struct.h"

/*****************MACRO FOR DEBUG*****************/
#define DEBUG_UTIL

#ifdef DEBUG_UTIL
#define printf_d printf
#else
#define printf_d //
#endif
/***************MACRO FOR DEBUG END***************/

#define PI 3.141592653589793238462643383279

double s_r(double r_ij, parameters_info_struct * parameters_info)
{
    double result;
    double rc = parameters_info->cutoff_2;
    double rcs = parameters_info->cutoff_1;
    result = (r_ij >= rc) ? 0 : ((r_ij >= rcs) ? 1 / r_ij * (0.5 * cos((r_ij - rcs) / (rc - rcs) * PI) + 0.5) : 1 / r_ij);
    return result;
}

double fastpow2(double number, int dummy)
{
    return number * number;
}

double fastpown(double number, int power)
{
    int i;
    double result = 1;
    int N = power;
    if (power == 0)
    {
        return 1;
    }
    if (power < 0)
    {
        N *= -1;
    }
    for (i = 1; i <= N; i++)
    {
        result *= number;
    }
    if (power < 0)
    {
        return 1.0 / result;
    }
    else
    {
        return result;
    }

}

double f_c(double r_ij, double r_c)
{
    double result;
    double rc = r_c;
    result = (r_ij <= rc) ? 0.5 * tanh(1 - r_ij / rc) * tanh(1 - r_ij / rc) * tanh(1 - r_ij / rc) : 0;
    return result;
}

double d_f_c_d_r(double r_ij, double r_c)
{
    double result;
    double rc = r_c;
    result = (r_ij <= rc) ? (-3.0 / cosh(1 - r_ij / rc) / cosh(1 - r_ij / rc) * tanh(1 - r_ij / rc) * tanh(1 - r_ij / rc) / 2.0 / rc ) : (0);
    return result;
}

double R_sup_n(double r_ij, double n, double r_c)
{
    double f_c(double r_ij, double r_c);

    double result;
    return fastpown(r_ij, (int)n) * f_c(r_ij, r_c);
}

double d_R_sup_n_d_r(double r_ij, double n, double r_c)
{
    double f_c(double r_ij, double r_c);
    double d_f_c_d_r(double r_ij, double r_c);
    double result;

    result = n * fastpown(r_ij, (int)(n - 1)) * f_c(r_ij, r_c) + fastpown(r_ij, (int)n) * d_f_c_d_r(r_ij, r_c);
    return result;
}

double factorial(int n)
{
    if (n < 0)
    {
        printf("From factorial: n must be positive!\n");
        exit(999);
    }
    return ((n == 0)||(n == 1)) ? 1 : factorial(n - 1) * n;
}

/*double P_LM(double x, int l, int m)
{
    double factorial(int n);

    if (m < 0)
    {
        return pow(-1, -m) * factorial(l + m) / factorial(l - m) * P_LM(x, l, (-1) * m);
    }

    switch (l)
    {
        case 0:
        {
            return 1.0;
        }
        case 1:
        {
            switch (m)
            {
                case 0:
                {
                    return x;
                }
                case 1:
                {
                    return -1 * sqrt(1 - x * x);
                }
            }
        }
        case 2:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.5 * (3 * x * x - 1);
                }
                case 1:
                {
                    return -3 * x * sqrt(1 - x * x);
                }
                case 2:
                {
                    return 3 * (1 - x * x);
                }
            }
        }
        case 3:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.5 * x * (5 * x * x - 3);
                }
                case 1:
                {
                    return 1.5 * (1 - 5 * x * x) * sqrt(1 - x * x);
                }
                case 2:
                {
                    return 15 * x * (1 - x * x);
                }
                case 3:
                {
                    return -15 * sqrt(1 - x * x) * sqrt(1 - x * x) * sqrt(1 - x * x);
                }
            }
        }
        case 4:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.125 * (35 * x * x * x * x - 30 * x * x + 3);
                }
                case 1:
                {
                    return 2.5 * x * (3 - 7 * x * x) * sqrt(1 - x * x);
                }
                case 2:
                {
                    return 7.5 * (7 * x * x - 1) * (1 - x * x);
                }
                case 3:
                {
                    return -105 * x * sqrt(1 - x * x) * sqrt(1 - x * x) * sqrt(1 - x * x);
                }
                case 4:
                {
                    return 105 * (1 - x * x) * (1 - x * x);
                }
            }
        }
        case 5:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.125 * x * (63 * x * x * x * x - 70 * x * x + 15);
                }
            }
        }
        default:
        {
            printf("From P_LM: Invalid value of l and m!\n");
            exit(999);
        }
    }
}*/

/*double complex Y_LM(double * r_ij, int l, int m)//here r_ij is a vector
{
    double P_LM(double cos_theta, int l, int m);
    double factorial(int n);

    double complex result;
    double theta, phi;
    double r = sqrt(r_ij[0] * r_ij[0] + r_ij[1] * r_ij[1] + r_ij[2] * r_ij[2]);
    theta = acos(r_ij[2] / r);
    phi = atan(r_ij[1] / r_ij[0]);
    result = sqrt((2 * l + 1) / (4 * PI) * factorial(l - m) / factorial(l + m)) * P_LM(cos(theta), l, m) * cexp(m * phi * I);
    return result;
}*/

double Y_LM_r(double * coord_ij, int L, int m)//here L is n of the boost::math::spherical_harmonics
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    double result_r;
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    result_r = boost::math::spherical_harmonic_r(L, m, theta, phi);
    return result_r;
}
double Y_LM_i(double * coord_ij, int L, int m)//here L is n of the boost::math::spherical_harmonics
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    double result_i;
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    result_i = boost::math::spherical_harmonic_i(L, m, theta, phi);
    return result_i;
}
std::complex<double> Y_LM(double * coord_ij, int L, int m)
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    std::complex<double> result;
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    if (phi < 0)
    {
        phi += (2 * PI);
    }
    //printf_d("L, m, theta, phi: %d, %d, %.6lf, %.6lf\n", L, m, theta, phi);
    result = boost::math::spherical_harmonic(L, m, theta, phi);
    return result;
}
std::complex<double> d_Y_LM_d_theta(double * coord_ij, int L, int m)
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    std::complex<double> result;
    std::complex<double> I(0.0, 1.0);
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    if (phi < 0)
    {
        phi += (2 * PI);
    }
    /*\partial Y/\partial \theta = m * cot(\theta) * Y_LM(\theta, \phi) + \sqrt((L-m) * (L + m + 1)) * exp(- I * \phi) * Y_L(M+1)(\theta, \phi)*/
    result = m * tan((double)PI / 2.0 - theta) * boost::math::spherical_harmonic(L, m, theta, phi) + sqrt((L-m) * (L + m + 1)) * exp(-I * phi) * boost::math::spherical_harmonic(L, m + 1, theta, phi);
    return result;
}
std::complex<double> d_Y_LM_d_phi(double * coord_ij, int L, int m)
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    std::complex<double> result;
    std::complex<double> I(0.0, 1.0);
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    if (phi < 0)
    {
        phi += (2 * PI);
    }
    /*\partial Y/\partial \phi = I * m * Y_LM(\theta, \phi)*/
    result = I * (double)m * boost::math::spherical_harmonic(L, m, theta, phi);
    return result;
}

double cos_bond_angle(double * coord_i, double * coord_j, double * coord_k)//centered at i
{
    double coord_ji[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ki[3];
    double norm_ji, norm_ki;//norm = (x^2 + y^2 + z^2)
    double dot_jiki = 0;
    double result = 0;
    int i, j, k;
    norm_ji =0; norm_ki = 0;
    for (i = 0; i <= 2; i++)
    {
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ki[i] = coord_k[i] - coord_i[i];
        norm_ji += (coord_ji[i] * coord_ji[i]);
        norm_ki += (coord_ki[i] * coord_ki[i]);
        dot_jiki += (coord_ji[i] * coord_ki[i]);
    }
    /*cos(\theta_ijk_centered_i) = dot_prod(coord_ji, coord_ki) / \sqrt(norm_ji * norm_ki)*/
    result = dot_jiki / sqrt(norm_ji * norm_ki);
    return result;
}
int d_cos_bond_angle_d_coord(double * coord_i, double * coord_j, double * coord_k, double * result)
{
    double fastpown(double number, int power);

    double coord_ji[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ki[3];
    double norm_ji = 0;
    double norm_ki = 0;//norm = (x^2 + y^2 + z^2)
    double dot_jiki = 0;
    //double result = 0;
    int i, j, k;
    //norm_ji =0; norm_ki = 0;
    for (i = 0; i <= 2; i++)
    {
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ki[i] = coord_k[i] - coord_i[i];
        norm_ji += (coord_ji[i] * coord_ji[i]);
        norm_ki += (coord_ki[i] * coord_ki[i]);
        dot_jiki += (coord_ji[i] * coord_ki[i]);
    }
    double dist_ji = sqrt(norm_ji);
    double dist_ki = sqrt(norm_ki);
    /*dxi, yi, zi, xj, yj, zj, xk, yk, zk*/
    result[0] = (- coord_ji[0] - coord_ki[0]) / (dist_ji * dist_ki) - (dot_jiki * (- 2 * coord_ki[0] * norm_ji - 2 * coord_ji[0] * norm_ki)) / ( 2 * fastpown((dist_ji * dist_ki), 3));
    result[1] = (- coord_ji[1] - coord_ki[1]) / (dist_ji * dist_ki) - (dot_jiki * (- 2 * coord_ki[1] * norm_ji - 2 * coord_ji[1] * norm_ki)) / ( 2 * fastpown((dist_ji * dist_ki), 3));
    result[2] = (- coord_ji[2] - coord_ki[2]) / (dist_ji * dist_ki) - (dot_jiki * (- 2 * coord_ki[2] * norm_ji - 2 * coord_ji[2] * norm_ki)) / ( 2 * fastpown((dist_ji * dist_ki), 3));
    result[3] = coord_ki[0] / (dist_ji * dist_ki) - coord_ji[0] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[4] = coord_ki[1] / (dist_ji * dist_ki) - coord_ji[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[5] = coord_ki[2] / (dist_ji * dist_ki) - coord_ji[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[6] = coord_ji[0] / (dist_ji * dist_ki) - coord_ki[0] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[7] = coord_ji[1] / (dist_ji * dist_ki) - coord_ki[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[8] = coord_ji[2] / (dist_ji * dist_ki) - coord_ki[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);

    return 0;
}

int cross_prod(double * vec1, double * vec2, double * vec_result)
{
    vec_result[0] = (vec1[1] * vec2[2] - vec1[2] * vec2[1]);
    vec_result[1] = (vec1[2] * vec2[0] - vec1[0] * vec2[2]);
    vec_result[2] = (vec1[0] * vec2[1] - vec1[1] * vec2[0]);
    return 0;
}
double cos_dihedral_angle(double * coord_i, double * coord_j, double * coord_k, double * coord_l)//centered at i and j, plane_ijk and plane_ijl
{
    int cross_prod(double * vec1, double * vec2, double * vec_result);

    /*Use coord_ki, ji to calculate norm_vec of plane_kij = ji \times ki; Use coord_ij, lj to calculate norm_vec of plane_ijl = ij \times lj*/
    double coord_ki[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ji[3];
    double coord_ij[3];
    double coord_lj[3];
    double norm_vec_kij[3];
    double norm_vec_ijl[3];
    double norm_norm_vec_kij = 0;//norm = (x^2 + y^2 + z^2)
    double norm_norm_vec_ijl = 0;
    double dot_kij_ijl = 0;
    double result;
    int error_code;
    int i, j, k;
    for (i = 0; i <= 2; i++)
    {
        coord_ki[i] = coord_k[i] - coord_i[i];
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ij[i] = coord_i[i] - coord_j[i];
        coord_lj[i] = coord_l[i] - coord_j[i];
    }
    error_code = cross_prod(coord_ji, coord_ki, norm_vec_kij);
    error_code = cross_prod(coord_ij, coord_lj, norm_vec_ijl);
    for (i = 0 ; i <= 2; i++)
    {
        norm_norm_vec_kij += (norm_vec_kij[i] * norm_vec_kij[i]);
        norm_norm_vec_ijl += (norm_vec_ijl[i] * norm_vec_ijl[i]);
        dot_kij_ijl += norm_vec_kij[i] * norm_vec_ijl[i];
    }
    if (norm_norm_vec_kij * norm_norm_vec_ijl == 0)//three points are on a line; no plane is formed.
    {
        return 999;
    }
    result = dot_kij_ijl / sqrt(norm_norm_vec_kij * norm_norm_vec_ijl) * ((double)-1.0);
    return result;
}

int d_cos_dihedral_angle_d_coord(double * coord_i, double * coord_j, double * coord_k, double * coord_l, double * result)//centered at i and j, plane_ijk and plane_ijl
{
    int cross_prod(double * vec1, double * vec2, double * vec_result);

    /*Use coord_ki, ji to calculate norm_vec of plane_kij = ji \times ki; Use coord_ij, lj to calculate norm_vec of plane_ijl = ij \times lj*/
    double coord_ki[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ji[3];
    double coord_ij[3];
    double coord_lj[3];
    double norm_vec_kij[3];
    double norm_vec_ijl[3];
    double norm_norm_vec_kij = 0;//norm = (x^2 + y^2 + z^2)
    double norm_norm_vec_ijl = 0;
    double dot_kij_ijl = 0;
    double xi, yi, zi, xj, yj, zj, xk, yk, zk, xl, yl, zl;
    xi = coord_i[0]; yi = coord_i[1]; zi=coord_i[2];
    xj = coord_j[0]; yj = coord_j[1]; zj=coord_j[2];
    xk = coord_k[0]; yk = coord_k[1]; zk=coord_k[2];
    xl = coord_l[0]; yl = coord_l[1]; zl=coord_l[2];
    //double result;
    int error_code;
    int i, j, k;
    for (i = 0; i <= 2; i++)
    {
        coord_ki[i] = coord_k[i] - coord_i[i];
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ij[i] = coord_i[i] - coord_j[i];
        coord_lj[i] = coord_l[i] - coord_j[i];
    }
    error_code = cross_prod(coord_ji, coord_ki, norm_vec_kij);
    error_code = cross_prod(coord_ij, coord_lj, norm_vec_ijl);
    for (i = 0 ; i <= 2; i++)
    {
        norm_norm_vec_kij += (norm_vec_kij[i] * norm_vec_kij[i]);
        norm_norm_vec_ijl += (norm_vec_ijl[i] * norm_vec_ijl[i]);
        dot_kij_ijl += norm_vec_kij[i] * norm_vec_ijl[i];
    }
    /*dxi, yi, zi, xj, yj, zj, xk, yk, zk, xl, yl, zl*/
    result[0] = (-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-yj+yl))-(yj-yk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*(zj-zl)-(-zj+zk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-yj+yl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(zj-zl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))+(2*(yj-yk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(-zj+zk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[1] = (-((xj-xl)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(-xj+xk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-zj+zl)-(zj-zk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(xj-xl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(-zj+zl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(-xj+xk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(zj-zk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[2] = (-((-xj+xl)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))-(yj-yl)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))-(xj-xk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-yj+yk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-xj+xl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))+2*(yj-yl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(xj-xk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))+2*(-yj+yk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[3] = (-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(yi-yl))-(-yi+yk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*(-zi+zl)-(zi-zk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(yi-yl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(-zi+zl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))+(2*(-yi+yk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(zi-zk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[4] = (-((-xi+xl)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(xi-xk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(zi-zl)-(-zi+zk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-xi+xl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(zi-zl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(xi-xk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(-zi+zk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[5] = (-((xi-xl)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))-(-yi+yl)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))-(-xi+xk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(yi-yk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(xi-xl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))+2*(-yi+yl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(-xi+xk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))+2*(yi-yk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[6] = -((2*(yi-yj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(-zi+zj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((yi-yj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-(-zi+zj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[7] = -((2*(-xi+xj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(zi-zj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((-xi+xj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-(zi-zj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[8] = -((2*(xi-xj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))+2*(-yi+yj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((xi-xj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))-(-yi+yj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[9] = -((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-yi+yj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(zi-zj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((-yi+yj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(zi-zj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[10] = -((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(xi-xj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(-zi+zj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((xi-xj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(-zi+zj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[11] = -((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-xi+xj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))+2*(yi-yj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((-xi+xj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))-(yi-yj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    //result = dot_kij_ijl / sqrt(norm_norm_vec_kij * norm_norm_vec_ijl) * ((double)-1.0);
    return 0;
}

int calc_N_neigh_inter(int K, int N)// total N types; K body interaction; MULTIPLY N to the return value to get correct answer!!!
{
    int i;
    int result = 0;
    if (K == 1) return 1;
    if (N == 1) return 1;
    for (i = 1; i <= N; i++)
    {
        result += calc_N_neigh_inter(K - 1, i);
    }
    return result;
}

int compare_Nei_type(int N_neighb_atom, int * current_type, int * params_type)//For example, current_type = {1, 8, 8}, params_type = {8, 1, 8}, then return 1
{
    int i, j, k;
    int sum = 0;
    /*std::vector<int> current_type_ (current_type, current_type + N_neighb_atom);
    std::vector<int> params_type_ (params_type, params_type + N_neighb_atom);*/
    for (i = 0; i <= N_neighb_atom - 1; i++)
    {
        if (current_type[i] == -1)
        {
            return 0;
        }
    }
    if (params_type[0] == -1)
    {
        return 1;
    }
    /*std::sort(current_type_.begin(), current_type_.end());
    std::sort(params_type_.begin(), params_type_.end());
    for (i = 0; i <= N_neighb_atom - 1; i++)
    {
        sum += ((current_type_[i] - params_type_[i]) * (current_type_[i] - params_type_[i]));
    }
    return ( sum == 0 ? 1 : 0);*/
    switch (N_neighb_atom)
    {
        case 1://two body
        {
            if (current_type[0] == params_type[0])
            {
                return 1;
            }
            break;
        }
        case 2://three body
        {
            if ((((current_type[0] == params_type[0])&&(current_type[1] == params_type[1]))) || (((current_type[0] == params_type[1])&&(current_type[1] == params_type[0]))))
            {
                return 1;
            }
            break;
        }
        case 3://four body
        {
            int mul1 = current_type[0] * current_type[1] * current_type[2];
            int mul2 = params_type[0] * params_type[1] * params_type[2];
            int sum1 = current_type[0] + current_type[1] + current_type[2];
            int sum2 = params_type[0] + params_type[1] + params_type[2];
            if ((mul1 == mul2)&&(sum1 == sum2))
            {
                return 1;
            }
            break;
        }
    }
    return 0;
}

int find_index_int(int target, int * array, int array_length)
{
    std::vector<int> array_ (array, array + array_length);
    std::vector<int>::iterator it = std::find(array_.begin(), array_.end(), target);
    int index = std::distance(array_.begin(), it);
    return index;
}

double **** calloc_params_LASP(int dim1, int dim2, int ** dim3_, int ** dim4_)
{
    int N_types_all_frame = dim1;
    int N_PTSD_types = dim2;
    int ** N_cutoff_radius = dim3_;
    int ** N_neigh_inter = dim4_;
    int i, j, k, l;
    double **** result = NULL;
    result = (double ****)calloc(dim1, sizeof(double ***));
    for (i = 0; i <= dim1 - 1; i++)
    {
        result[i] = (double ***)calloc(dim2, sizeof(double **));
        for (j = 0 ; j <= dim2 - 1; j++)
        {
            result[i][j] = (double **)calloc(N_cutoff_radius[i][j], sizeof(double *));
            for (k = 0; k <= N_cutoff_radius[i][j] - 1; k++)
            {
                result[i][j][k] = (double *)calloc(N_neigh_inter[i][j], sizeof(double));
            }
        }
    }
    return result;
}

int free_params_LASP(double **** target, int dim1, int dim2, int ** dim3_, int ** dim4_)
{
    return 1;
}

int free_sym_coord(void * sym_coord_, int sym_coord_type, parameters_info_struct * parameters_info)
{
    switch (sym_coord_type)
    {
        case 1:
        {
            int i, j;
            sym_coord_DeePMD_struct * sym_coord_DeePMD = (sym_coord_DeePMD_struct *)sym_coord_;
            for (i = 0; i <= parameters_info->Nframes_tot - 1; i++)
            {
                //free(sym_coord_DeePMD[i].type);
                for (j = 0; j <= sym_coord_DeePMD[i].N_Atoms - 1; j++)
                {
                    free(sym_coord_DeePMD[i].coord_converted[j]);
                    free(sym_coord_DeePMD[i].d_to_center_x[j]);
                    free(sym_coord_DeePMD[i].d_to_center_y[j]);
                    free(sym_coord_DeePMD[i].d_to_center_z[j]);
                }

                free(sym_coord_DeePMD[i].coord_converted);
                free(sym_coord_DeePMD[i].d_to_center_x);
                free(sym_coord_DeePMD[i].d_to_center_y);
                free(sym_coord_DeePMD[i].d_to_center_z);
            }
            free(sym_coord_DeePMD);
            return 0;
            break;
        }
        case 2:
        {
            int i, j, k;
            sym_coord_LASP_struct * sym_coord_LASP = (sym_coord_LASP_struct *)sym_coord_;
            for (i = 0; i <= parameters_info->Nframes_tot - 1; i++)
            {
                //free(sym_coord_DeePMD[i].type);
                for (j = 0; j <= sym_coord_LASP[i].N_Atoms - 1; j++)
                {
                    free(sym_coord_LASP[i].coord_converted[j]);
                    for (k = 0; k <= parameters_info->N_sym_coord - 1; k++)
                    {
                        free(sym_coord_LASP[i].idx_nei[j][k]);
                        free(sym_coord_LASP[i].d_x[j][k]);
                        free(sym_coord_LASP[i].d_y[j][k]);
                        free(sym_coord_LASP[i].d_z[j][k]);
                    }
                    free(sym_coord_LASP[i].idx_nei[j]);
                    free(sym_coord_LASP[i].d_x[j]);
                    free(sym_coord_LASP[i].d_y[j]);
                    free(sym_coord_LASP[i].d_z[j]);
                    /*Not completed
                    free(sym_coord_LASP[i].d_to_center_x[j]);
                    free(sym_coord_LASP[i].d_to_center_y[j]);
                    free(sym_coord_LASP[i].d_to_center_z[j]);*/
                }

                free(sym_coord_LASP[i].coord_converted);
                free(sym_coord_LASP[i].idx_nei);
                free(sym_coord_LASP[i].d_x);
                free(sym_coord_LASP[i].d_y);
                free(sym_coord_LASP[i].d_z);
                /*Not completed
                free(sym_coord_LASP[i].d_to_center_x);
                free(sym_coord_LASP[i].d_to_center_y);
                free(sym_coord_LASP[i].d_to_center_z);*/
            }
            free(sym_coord_LASP);
            return 0;
            break;
        }
        default:
        {
            break;
        }
    }
    return 0;

}

int cart_to_frac(double * cart, double box[3][3], double * frac)
{
    double a1 = box[0][0], a2 = box[0][1], a3 = box[0][2], b1 = box[1][0], b2 = box[1][1], b3 = box[1][2], c1 = box[2][0], c2 = box[2][1], c3 = box[2][2];
    double denominator = (a1 * b2 * c3 + b1 * c2 * a3 + c1 * a2 * b3 - c1 * b2 * a3 - b1 * a2 * c2 - a1 * c2 * b3);
    if (denominator == 0)
    {
        return 1;//Two or more box vectors are parallel.
    }
    /*Don't forget the tranpose in the reverse-matrix formula!*/
    double prefact = 1.0 / denominator;
    double rev_a1 = prefact * (b2 * c3 - c2 * b3);
    double rev_b1 = prefact * (c1 * b3 - b1 * c3);
    double rev_c1 = prefact * (b1 * c2 - c1 * b2);
    double rev_a2 = prefact * (c2 * a3 - a2 * c3);
    double rev_b2 = prefact * (a1 * c3 - c1 * a3);
    double rev_c2 = prefact * (c1 * a2 - a1 * c2);
    double rev_a3 = prefact * (a2 * b3 - b2 * a3);
    double rev_b3 = prefact * (b1 * a3 - a1 * b3);
    double rev_c3 = prefact * (a1 * b2 - b1 * a2);
    double x = cart[0], y = cart[1], z = cart[2];
    *frac = rev_a1 * x + rev_b1 * y + rev_c1 * z;
    *(frac + 1) = rev_a2 * x + rev_b2 * y + rev_c2 * z;
    *(frac + 2) = rev_a3 * x + rev_b3 * y + rev_c3 * z;
    return 0;
}
int frac_to_cart(double * cart, double box[3][3], double * frac)
{
    double a1 = box[0][0], a2 = box[0][1], a3 = box[0][2], b1 = box[1][0], b2 = box[1][1], b3 = box[1][2], c1 = box[2][0], c2 = box[2][1], c3 = box[2][2];
    double x_ = frac[0], y_ = frac[1], z_ = frac[2];
    *cart = a1 * x_ + b1 * y_ + c1 * z_;
    *(cart + 1) = a2 * x_ + b2 * y_ + c2 * z_;
    *(cart + 2) = a3 * x_ + b3 * y_ + c3 * z_;
    return 0;
}

double max(double a, double b)
{
    return (a >= b ? a : b);
}


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::matrix_type                                matrix_type;
typedef std::vector<double>                                    knot_container;

BOOST_AUTO_TEST_SUITE(opentissue_spline_compute_basis_derivatives);

BOOST_AUTO_TEST_CASE(test_compute_basis_derivatives)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);  //k = 3
  U.push_back(1.0);
  U.push_back(2.0);
  U.push_back(3.0);
  U.push_back(4.0);  // n = 6  => |P| = 7
  U.push_back(5.0);
  U.push_back(5.0);
  U.push_back(5.0);  // m = 9  => |U| = 10

  // Indices of basis functions belongs to the interval [0..n]

  double const tolerance = 0.00001;

  // u-parameter at half-way
  {
    double const u = 2.5;

    matrix_type dQ;
    BOOST_CHECK_NO_THROW( OpenTissue::spline::detail::compute_basis_derivatives(u, 2, 3, U, dQ) );

    BOOST_CHECK_CLOSE(dQ(0,0), 1.0/8.0,  tolerance);
    BOOST_CHECK_CLOSE(dQ(0,1), 6.0/8.0,  tolerance);
    BOOST_CHECK_CLOSE(dQ(0,2), 1.0/8.0,  tolerance);
    BOOST_CHECK_CLOSE(dQ(1,0), -1.0/2.0, tolerance);
    BOOST_CHECK_CLOSE(dQ(1,1), 0.0,    tolerance);
    BOOST_CHECK_CLOSE(dQ(1,2), 1.0/2.0,  tolerance);
    BOOST_CHECK_CLOSE(dQ(2,0), 1.0,    tolerance);
    BOOST_CHECK_CLOSE(dQ(2,1), -2.0,   tolerance);
    BOOST_CHECK_CLOSE(dQ(2,2), 1.0,    tolerance);
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include "matrix.h"
#include "mex.h"

#include <iostream>
#include <math.h>
#include <chrono>
#include <random>
#include <vector>

#include <Eigen\Eigen>
#include <Eigen\LU>
#include <Eigen\Dense>
#include <Eigen\SVD>

#include "SampleConsensusProblem.hpp"
#include "SampleConsensus.hpp"
#include "Ransac.hpp"
#include "Msac.hpp"
#include "Prosac.hpp"
#include "Lmeds.hpp"

using namespace std;
using namespace Eigen;
using namespace aslam;

class HomographyEstimatorProblem : public aslam::SampleConsensusProblem <MatrixXd>
{
public:
	HomographyEstimatorProblem() { }
	virtual ~HomographyEstimatorProblem() { }

	size_t numElements() const{ return points1_.rows(); }

	virtual int getSampleSize() const { return 4; }

	// Take a subset of size 4 of points and estimate homography matrix using them
	virtual bool computeModelCoefficients(const std::vector<int> & indices, MatrixXd& model) const
	{
		if (indices.size() != getSampleSize())
		{
			mexPrintf("Invalid set of sample points given (%d)!\n", indices.size());
			return false;
		}

		MatrixXd p1(indices.size(), 3);
		MatrixXd p2(indices.size(), 3);
		for (int i = 0; i < indices.size(); i++)
		{
			p1.row(i) = points1_.row(indices[i]);
			p2.row(i) = points2_.row(indices[i]);
		}
		auto H = calculateHomography(p1, p2);

		model = H;
		// end calculate homography
		return true;
	}

	virtual void optimizeModelCoefficients(const vector<int> & inliers, const MatrixXd& model, MatrixXd& optimized_model)
	{
		optimized_model = model;
	}

	// It will return a matrix H, the homography matrix, such that x1=H*x2
	// Note: points are assumed to be normalized and in homogeneous coordinate
	// Sizes of two sets of points must be equal. for performace purposes, this condition is not checked in this function
	// Note: since normalise2Dpoints works in-place on the points matrix, normalizing points inside this function will
	// harm our collection of points. But because of two observations, we are allowed to normalise points inside this function
	// and it will cause no harm:
	//	1) Points from each step of ransac will be passed to this function to calculate the homography matrix. To do so, we will
	//	   copy those points from the main point matrix to temporary matrices. So normalisation won't affect the original matrix.
	//  2) After exhausting the SAC operation, the final Homography matrix will be calculated using all of the inliers. After this
	//	   calculation, the program will return. So affecting the data points, won't do harm.
	MatrixXd calculateHomography(MatrixXd& x1, MatrixXd& x2) const
	{
		MatrixXd T1, T2;
		normalize2Dpoints(x1, T1);
		normalize2Dpoints(x2, T2);
		MatrixXd A = MatrixXd::Zero(3 * x1.rows(), 9);
		for (int i = 0; i < x1.rows(); i++)
		{
			A(3 * i, 3) = -x1(i, 0);
			A(3 * i, 4) = -x1(i, 1);
			A(3 * i, 5) = -x1(i,2);
			A(3 * i, 6) = x2(i, 1) * x1(i, 0);
			A(3 * i, 7) = x2(i, 1) * x1(i, 1);
			A(3 * i, 8) = x2(i, 1) * x1(i, 2);

			A(3 * i + 1, 0) = x1(i, 0);
			A(3 * i + 1, 1) = x1(i, 1);
			A(3 * i + 1, 2) = x1(i, 2);
			A(3 * i + 1, 6) = -x2(i, 0) * x1(i, 0);
			A(3 * i + 1, 7) = -x2(i, 0) * x1(i, 1);
			A(3 * i + 1, 8) = -x2(i, 0) * x1(i, 2);

			A(3 * i + 2, 0) = x2(i, 1) * x1(i, 0);
			A(3 * i + 2, 1) = x2(i, 1) * x1(i, 1);
			A(3 * i + 2, 2) = x2(i, 1) * x1(i, 2);
			A(3 * i + 2, 3) = x2(i, 0) * x1(i, 0);
			A(3 * i + 2, 4) = x2(i, 0) * x1(i, 1);
			A(3 * i + 2, 5) = x2(i, 0) * x1(i, 2);
		}
		JacobiSVD<MatrixXd> svd(A, ComputeThinV);
		svd.computeV();
		MatrixXd minEigenVector = MatrixXd(svd.matrixV().col(8));
		minEigenVector.resize(3, 3);
		auto H = T2.inverse() * minEigenVector * T1;
		return H;
	}

	// As the final step, estimate the homography matrix using all of the inlier correspondences
	void calculateModelUsingAllInliers(const vector<int>& inliers, MatrixXd& finalModel)
	{
		MatrixXd p1(inliers.size(), 3);
		MatrixXd p2(inliers.size(), 3);
		for (int i = 0; i < inliers.size(); i++)
		{
			p1.row(i) = points1_.row(inliers[i]);
			p2.row(i) = points2_.row(inliers[i]);
		}
		auto H = calculateHomography(p1, p2);

		finalModel = H;
	}

	/// evaluate the score for the elements at indices based on this model.
	/// low scores mean a good fit.
	virtual void getSelectedDistancesToModel(const MatrixXd& model, const vector<int> & indices, vector<double> & scores) const
	{
		scores.resize(indices.size());
		// Iterate through correspondences and calculate the projection error

		auto H = model;
		for (size_t i = 0; i < indices.size(); ++i)
		{
			auto x1 = points1_.row(indices[i]);
			auto x2 = points2_.row(indices[i]);
			auto Hx1 = H * x1;
			auto invHx2 = H.inverse() * x2;
			x1 /= x1(2);
			x2 /= x2(2);
			Hx1 /= Hx1(2);
			invHx2 /= invHx2(2);
			scores[i] = (x1 - invHx2).unaryExpr([](double a) {return a*a; }).sum() + (x2 - Hx1).unaryExpr([](double a) {return a*a; }).sum();
		}
	}

	// normalize 2d points so that they would have a zero mean and their mean distance from origin would be sqrt(2)
	void normalize2Dpoints(MatrixXd& points, MatrixXd& normalizationMat) const
	{
		double meanx = points.col(0).mean();
		double meany = points.col(1).mean();
		/*auto colx = points.col(0).unaryExpr([meanx](double x) { return x - meanx; });
		auto coly = points.col(1).unaryExpr([meany](double y) { return y - meany; });
		auto dist = (colx.unaryExpr([](double x) { return x*x; }) + coly.unaryExpr([](double y) {return y*y; })).unaryExpr([](double d) {return sqrt(d); });*/
		double meandist = (points.col(0).unaryExpr([meanx](double x) { return (x - meanx)*(x - meanx); }) + points.col(1).unaryExpr([meany](double y) { return (y - meany)*(y - meany); }))
			.unaryExpr([](double d){return sqrt(d); }).mean();
		double scale = sqrt(2) / meandist;

		MatrixXd T(3, 3);
		T << scale, 0, -scale*meanx
			, 0, scale, -scale*meany
			, 0, 0, 1;
		points = (T*points.transpose()).transpose();
		normalizationMat = T;
	}

	MatrixXd points1_;
	MatrixXd norm1_;
	MatrixXd points2_;
	MatrixXd norm2_;
};

// This is the entry point from MATLAB to this routine
// plhs and prhs represent a pointer to the left hand side (output) and a pointer to the right hand side (input) respectively
// matlab function signature: H = homography_estimator(points1, points2, 'mode')
// H -> 9-by-9 homography matrix as output
// points1 -> n-by-2 matrix, coordinates of the keypoints of the first image
// points2 -> n-by-2 matrix, coordinates of the keypoints of the second image
// 'mode' -> {'ransac', 'prosac', 'mlesac'}
void mexFunction(int nlhs, mxArray *plhs [], int nrhs, const mxArray*prhs [])
{
	if (nlhs != 1)
	{
		mexPrintf("Only one output expected\n");
		return;
	}
	if (nrhs != 3)
	{
		mexPrintf("Only three inputs expected\n");
		return;
	}

	const mwSize *dims1, *dims2;
	char* modeStr;
	double *points1, *points2;

	dims1 = mxGetDimensions(prhs[0]);
	dims2 = mxGetDimensions(prhs[1]);

	int dim1y = (int) dims1[0]; int dim1x = (int) dims1[1];
	int dim2y = (int) dims2[0]; int dim2x = (int) dims2[1];

	if (dim1x != dim2x || dim1y != dim2y)
	{
		mexPrintf("Error! two input points arrays must be of the same size\n");
		return;
	}

	if (dim1x != 2 || dim2x != 2)
	{
		mexPrintf("Error! Points must be 2-D!\n");
		return;
	}

	if (dim1y < 4)
	{
		mexPrintf("Not enough correspondences provided\n");
		return;
	}

	modeStr = mxArrayToString(prhs[2]);

	if (!modeStr || mxGetDimensions(prhs[2])[0] != 1)
	{
		mexPrintf("Error! Last argument must be an string!\n");
		return;
	}

	mexPrintf("string:\n%s\n", modeStr);
	mexPrintf("dimx = %d, dimy = %d\n", dim1x, dim1y);
	//vector<Correspondence> correspondences(dim1y);
	points1 = mxGetPr(prhs[0]);
	points2 = mxGetPr(prhs[1]);

	boost::shared_ptr<HomographyEstimatorProblem> homoproblem_ptr(new HomographyEstimatorProblem);
	HomographyEstimatorProblem& homoproblem = *homoproblem_ptr;
	homoproblem.points1_.resize(dim1y, 3);
	homoproblem.points1_.col(2).setOnes();
	homoproblem.points2_.resize(dim1y, 3);
	homoproblem.points2_.col(2).setOnes();
	homoproblem.setUniformIndices(dim1y);

	for (size_t j = 0; j < dim1y; j++)
	{
		/*Correspondence corr;
		corr.p1.x = points1[j];
		corr.p2.x = points2[j];
		corr.p1.y = points1[dim1y + j];
		corr.p2.y = points2[dim1y + j];*/
		//correspondences[j] = Correspondence(points1[j], points1[dim1y + j], points2[j], points2[dim1y + j]);
		//  mexPrintf("correspondence[%d] = (%lf, %lf; %lf, %lf)\n", j, points1[j], points1[dim1y + j], points2[j], points2[dim1y + j]);
		// mexPrintf("correspondence[%d] = (%lf, %lf; %lf, %lf)\n", j, correspondences[j].p1.x, correspondences[j].p1.y, correspondences[j].p2.x, correspondences[j].p2.y);
		homoproblem.points1_(j, 0) = points1[j];
		homoproblem.points1_(j, 1) = points1[dim1y + j];
		homoproblem.points2_(j, 0) = points2[j];
		homoproblem.points2_(j, 1) = points2[dim1y + j];
	}

	homoproblem.normalize2Dpoints(homoproblem.points1_, homoproblem.norm1_);
	homoproblem.normalize2Dpoints(homoproblem.points2_, homoproblem.norm2_);

	// HomographyEstimatorProblem *homoproblem = new HomographyEstimatorProblem();

	SampleConsensus<HomographyEstimatorProblem> *sac;
	if (!strcmp("ransac", modeStr))
	{
		sac = new Ransac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else if (!strcmp("lmeds", modeStr))
	{
		sac = new Lmeds<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else if (!strcmp("msac", modeStr))
	{
		sac = new Msac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else if (!strcmp("prosac", modeStr))
	{
		sac = new Prosac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else
	{
		mexPrintf("Error! unknown estimation mode selected!\n");
		return;
	}
	// if(!strcmp("rmsac", modeStr))
	// {
	// 	sac = new Rmsac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	// }

	// Msac<HomographyEstimatorProblem> ransac(1000, 0.1, 0.9);

	sac->sac_model_ = homoproblem_ptr;
	sac->computeModel(4);
	auto homographyModel = sac->model_coefficients_;
	mexPrintf("Number of iterations: %d\n", sac->iterations_);
	mexPrintf("Inliers size: %d (out of %d input correspondences)\n", sac->inliers_.size(), homoproblem.points1_.rows());

	homoproblem.calculateModelUsingAllInliers(sac->inliers_, homographyModel);
	auto H = homoproblem.norm2_.inverse() * homographyModel * homoproblem.norm1_;

	auto outArray = mxCreateDoubleMatrix(3, 3, mxREAL);
	double *outp = mxGetPr(outArray);

	// for (size_t i = 0; i < 3; i++)
	// {
	// 	for (size_t j = 0; j < 3; j++)
	// 	{
	// 		mexPrintf("homography matrix(%d, %d) = %lf\n", i, j, ransac.model_coefficients_.Matrix[j*3+i]);
	// 	}
	// }

	// Copy the estimated homography matrix to the output matrix, note the difference between MATLAB's column-wise arrays vs. C's row-wise arrays convention
	for (size_t i = 0; i < 3; i++)
	{
		for (size_t j = 0; i < 3; j++)
		{
			outp[i + 3 * j] = H(i, j);
		}
	}

	plhs[0] = outArray;
}

PCL SAC + Boost/homography_estimator_pcl.cpp


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

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

#include <iostream>
#include <vector>

#include "libs/maths.h"

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_mat2, t_real, t_types)
{
	using namespace tl2_ops;

	using t_cplx = std::complex<t_real>;
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;
	using t_vec_cplx = tl2::vec<t_cplx, std::vector>;
	using t_mat_cplx = tl2::mat<t_cplx, std::vector>;

	{
		auto M = tl2::create<t_mat>({
			1., 2., 3.,
			3., 1., 4.,
			9., -4., 2.
		});

		// test determinant
		t_real det = tl2::det(M);
		std::cout << "M = " << M << std::endl;
		std::cout << "|M| = " << det << std::endl;

		BOOST_TEST(tl2::equals<t_real>(det, 15., 1e-4));
	}

	{
		auto M = tl2::create<t_mat>({
			1., 3., 2.,
			3., 4., 1.,
			9., 2., -4.
		});

		// test determinant
		t_real det = tl2::det(M);
		std::cout << "M = " << M << std::endl;
		std::cout << "|M| = " << det << std::endl;

		BOOST_TEST(tl2::equals<t_real>(det, -15., 1e-4));
	}

	{
		auto M = tl2::create<t_mat_cplx>({
			1., 2., 3.,
			3., 1., 4.,
			9., -4., 2
		});

		// test determinant
		t_cplx det = tl2::det(M);
		std::cout << "M = " << M << std::endl;
		std::cout << "|M| = " << det << std::endl;

		BOOST_TEST(tl2::equals<t_cplx>(det, 15., 1e-4));
	}
}