#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>

#include <vector>
#include <cmath>
#include <stdexcept>
#include <scitbx/lstbx/normal_equations.h>

using namespace boost::python;
namespace xfel{

namespace xes { namespace example {
  class gaussian_fit_inheriting_from_non_linear_ls: public scitbx::lstbx::normal_equations::non_linear_ls<double> {

    public:
      gaussian_fit_inheriting_from_non_linear_ls(int n_parameters):
        scitbx::lstbx::normal_equations::non_linear_ls<double>(n_parameters),
        jacobian_one_row(n_parameters)
        {}

      void set_cpp_data(scitbx::af::shared<double> x,
                        scitbx::af::shared<double> y,
                        double const& g,
                        double const& s) {
                        free_x=x, free_y=y, gain_to_sigma=g, sigmafac=s; sigmafac_sq=s*s;
                        residual_terms_0 = std::vector<double>(x.size());
                        residual_terms_1 = std::vector<double>(x.size());
                        residuals = scitbx::af::shared<double>(x.size());
      }

      void access_cpp_build_up_directly(bool objective_only, scitbx::af::shared<double> current_values) {
        fvec_callable(current_values);
        if (objective_only) {
          add_residuals(residuals.const_ref(), scitbx::af::shared<double>().const_ref());
        }else{
          // add one of the normal equations for each observation
          for (int ix = 0; ix < free_x.size(); ++ix) {
            /*
              z_mean = current_values[0]
              z_ampl = current_values[1]
              z_sigm = current_values[2]
              o_ampl = current_values[3]
              o_mean = z_mean + z_sigm * GAIN_TO_SIGMA
              o_sigm = z_sigm * SIGMAFAC
              # leaving out small cross terms where one-photon peak influences
              # derivatives with respect to z_mean and z_sigm.
              # XXX not accounting for charge sharing at all
             */
            double udiff = free_x[ix] - current_values[0];
            double Afactor = udiff/(current_values[2] * current_values[2]);
            double Sfactor = Afactor * udiff/current_values[2];
            jacobian_one_row[0] = ( residual_terms_0[ix] * Afactor );
            jacobian_one_row[1] = ( residual_terms_0[ix] / current_values[1] );
            jacobian_one_row[2] = ( residual_terms_0[ix] * Sfactor );
            jacobian_one_row[3] = ( residual_terms_1[ix] / current_values[3] );

            add_equation(residuals[ix], jacobian_one_row.const_ref(), 1.);
          }
        }
      }

      void
      fvec_callable(scitbx::af::shared<double> &current_values) {
        /*
          z_mean = current_values[0]
          z_ampl = current_values[1]
          z_sigm = current_values[2]
          o_ampl = current_values[3]
          o_mean = z_mean + z_sigm * GAIN_TO_SIGMA
          o_sigm = z_sigm * SIGMAFAC

          # model minus obs
          # sqrt2pi_inv = 1./math.sqrt(2.*math.pi)
          # the gaussian function is sqrt2pi_inv * exp( - (x-mean)**2 / (2.*(sigma**2)))/sigma
          # take off the coefficient sqrt2pi_inv / sigma, use ampl
        */

        for (int ix = 0; ix < free_x.size(); ++ix) {
          double model=0;
          //zero-photon Gaussian
          double diff = free_x[ix] - current_values[0];
          residual_terms_0[ix] = (
            current_values[1] * std::exp( - (diff*diff) / (2. * current_values[2] * current_values[2])));
          model += residual_terms_0[ix];

          //one-photon Gaussian
          diff = free_x[ix] - ( current_values[0] + current_values[2] * gain_to_sigma );
          residual_terms_1[ix] = (
            current_values[3] * std::exp( - (diff*diff) / (2. * current_values[2] * current_values[2] * sigmafac_sq)));
          model += residual_terms_1[ix];

          //terms = [
          //ampl * flex.exp(-flex.pow2(free_x - mean) / (2.*sigm*sigm))
          //for mean,ampl,sigm in [(z_mean,z_ampl,z_sigm),(o_mean,o_ampl,o_sigm)]]
          //#print "residual", math.sqrt(flex.sum((model-free_y)*(model-free_y)))
          residuals[ix] = ( model - free_y[ix] );
        }
      }

    private:
      scitbx::af::shared<double> free_x, free_y, jacobian_one_row, residuals;
      double gain_to_sigma, sigmafac, sigmafac_sq;
      std::vector<double> residual_terms_0, residual_terms_1;

  };

  class gaussian_3fit_inheriting_from_non_linear_ls: public scitbx::lstbx::normal_equations::non_linear_ls<double> {

    public:
      gaussian_3fit_inheriting_from_non_linear_ls(int n_parameters):
        scitbx::lstbx::normal_equations::non_linear_ls<double>(n_parameters),
        jacobian_one_row(n_parameters)
        {}

      void set_cpp_data(scitbx::af::shared<double> const& val,
                        scitbx::af::shared<double> const& x,
                        scitbx::af::shared<double> const& y) {
                        free_x=x, free_y=y;
                        residual_terms_0 = std::vector<double>(x.size());
                        residual_terms_1 = std::vector<double>(x.size());
                        residual_terms_2 = std::vector<double>(x.size());
                        residuals = scitbx::af::shared<double>(x.size());
                        constants = val;
      }

      void access_cpp_build_up_directly(bool objective_only, scitbx::af::shared<double> current_values) {

        fvec_callable(current_values);

        if (objective_only) {

          add_residuals(residuals.const_ref(), scitbx::af::shared<double>().const_ref());
        }else{
          // add one of the normal equations for each observation
          for (int ix = 0; ix < free_x.size(); ++ix) {
            /*
              z_mean = current_values[0]
              z_ampl = current_values[1]
              z_sigm = current_values[2]
              inelast_mean = zmean + zsigm * constants[0]
              inelast_ampl = current_values[3]
              inelast_sigm = (constants[0]/constants[1])*(elast_sigm[5] - zsigm[2])+zsigm[2]
              elast_mean = z_mean + z_sigm * constants[1]
              elast_ampl = current_values[4]
              elast_sigm = current_values[5]
              # leaving out small cross terms where one-photon peak influences
              # derivatives with respect to z_mean and z_sigm.
              # XXX not accounting for charge sharing or 2-photon peak
             */
            double udiff = free_x[ix] - current_values[0];
            double Afactor = udiff/(current_values[2] * current_values[2]);
            double Sfactor = Afactor * udiff/current_values[2];
            jacobian_one_row[0] = ( residual_terms_0[ix] * Afactor );
            jacobian_one_row[1] = ( residual_terms_0[ix] / current_values[1] );
            jacobian_one_row[2] = ( residual_terms_0[ix] * Sfactor );
            jacobian_one_row[3] = ( residual_terms_1[ix] / current_values[3] );
            jacobian_one_row[4] = ( residual_terms_2[ix] / current_values[4] );
            double udiff3 = free_x[ix] - (current_values[0] + current_values[2] * constants[1]);
            double Afactor3 = udiff3/(current_values[5] * current_values[5]);
            double Sfactor3 = Afactor3 * udiff3/current_values[5];
            jacobian_one_row[5] = ( residual_terms_2[ix] * Sfactor3 );

            add_equation(residuals[ix], jacobian_one_row.const_ref(), 1.);
          }
        }

      }

      void
      fvec_callable(scitbx::af::shared<double> &current_values) {
        /*
          # model minus obs
          # sqrt2pi_inv = 1./math.sqrt(2.*math.pi)
          # the gaussian function is sqrt2pi_inv * exp( - (x-mean)**2 / (2.*(sigma**2)))/sigma
          # take off the coefficient sqrt2pi_inv / sigma, use ampl
        */

        for (int ix = 0; ix < free_x.size(); ++ix) {
          double model=0;

          //zero-photon Gaussian
          double diff = free_x[ix] - current_values[0];
          residual_terms_0[ix] = (
            current_values[1] * std::exp( - (diff*diff) / (2. * current_values[2] * current_values[2])));
          model += residual_terms_0[ix];

          //inelastic-photon Gaussian
          {
          double pmean = current_values[0] + current_values[2] * constants[0];
          diff = free_x[ix] - pmean;
          double psigma = (constants[0]/constants[1])*(current_values[5] - current_values[2])+current_values[2];
          residual_terms_1[ix] = (
            current_values[3] * std::exp( - (diff*diff) / (2. * psigma * psigma)));
          model += residual_terms_1[ix];
          }

          //elastic-photon Gaussian
          {
          double pmean = current_values[0] + current_values[2] * constants[1];
          diff = free_x[ix] - pmean;
          double psigma = current_values[5];
          residual_terms_2[ix] = (
            current_values[4] * std::exp( - (diff*diff) / (2. * psigma * psigma)));
          model += residual_terms_2[ix];
          }

          //terms = [
          //ampl * flex.exp(-flex.pow2(free_x - mean) / (2.*sigm*sigm))
          //for mean,ampl,sigm in [(z_mean,z_ampl,z_sigm),(o_mean,o_ampl,o_sigm)]]
          //#print "residual", math.sqrt(flex.sum((model-free_y)*(model-free_y)))
          residuals[ix] = ( model - free_y[ix] );
        }
      }

    private:
      scitbx::af::shared<double> free_x, free_y, jacobian_one_row, residuals;
      scitbx::af::shared<double> constants;
      double gain_to_sigma, sigmafac, sigmafac_sq;
      std::vector<double> residual_terms_0, residual_terms_1, residual_terms_2;

  };

}} //xes::example

namespace boost_python { namespace {

  void
  xes_ext_init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;
    typedef xes::example::gaussian_fit_inheriting_from_non_linear_ls wt;
    typedef xes::example::gaussian_3fit_inheriting_from_non_linear_ls wt3;

    class_<xes::example::gaussian_fit_inheriting_from_non_linear_ls,
           bases<scitbx::lstbx::normal_equations::non_linear_ls<double> > >(
      "gaussian_fit_inheriting_from_non_linear_ls", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("access_cpp_build_up_directly",&wt::access_cpp_build_up_directly,
        (arg("objective_only"),arg("current_values")))
      .def("set_cpp_data",&wt::set_cpp_data,
        (arg("free_x"),arg("free_y"),arg("gain_to_sigma"),arg("sigmafac")))
    ;
    class_<xes::example::gaussian_3fit_inheriting_from_non_linear_ls,
           bases<scitbx::lstbx::normal_equations::non_linear_ls<double> > >(
      "gaussian_3fit_inheriting_from_non_linear_ls", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("access_cpp_build_up_directly",&wt3::access_cpp_build_up_directly,
        (arg("objective_only"),arg("current_values")))
      .def("set_cpp_data",&wt3::set_cpp_data,
        (arg("constants"),arg("free_x"),arg("free_y")))
    ;
  }

}
}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(xes_ext)
{
  xfel::boost_python::xes_ext_init_module();

}


#ifndef GUARD_PERM_GROUP_H
#define GUARD_PERM_GROUP_H

#include <cassert>
#include <map>
#include <tuple>
#include <type_traits>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

#include "bsgs.hpp"
#include "perm.hpp"
#include "perm_set.hpp"
#include "timeout.hpp"
#include "util.hpp"

namespace mpsym
{

namespace internal
{

class Orbit;
class OrbitPartition;

class BlockSystem;

class PermGroup
{
  friend std::ostream &operator<<(std::ostream &os, PermGroup const &pg);

public:
  using value_type = Perm;
  using const_reference = Perm const &;

  class const_iterator : public util::Iterator<const_iterator, Perm const>
  {
  public:
    const_iterator() : _end(true) {};
    const_iterator(PermGroup const &pg);

    bool operator==(const_iterator const &rhs) const override;

    PermSet const &factors() const
    { return _current_factors; }

  private:
    reference current() override;
    void next() override;

    std::vector<unsigned> _state;
    bool _trivial;
    bool _end;

    std::vector<PermSet> _transversals;
    Perm _current;
    bool _current_valid;
    PermSet _current_factors;
  };

  explicit PermGroup(unsigned degree = 1)
  : _bsgs(degree),
    _order(1)
  {}

  explicit PermGroup(BSGS const &bsgs)
  : _bsgs(bsgs),
    _order(bsgs.order())
  {}

  PermGroup(PermSet const &generators)
  : PermGroup(generators.degree(), generators)
  {}

  PermGroup(unsigned degree, PermSet const &generators);

  static PermGroup symmetric(unsigned degree);
  static PermGroup cyclic(unsigned degree);
  static PermGroup dihedral(unsigned degree);

  template<typename IT>
  static PermGroup direct_product(IT first,
                                  IT last,
                                  BSGSOptions const *bsgs_options_ = nullptr,
                                  timeout::flag aborted = timeout::unset())
  {
    assert(std::distance(first, last) > 0);

    // direct product degree
    unsigned dp_degree = 0u;

    for (auto it = first; it != last; ++it)
      dp_degree += it->degree();

    // direct product order
    auto dp_order(direct_product_order(first, last));

    // direct product generators
    unsigned d = 0u;

    PermSet dp_generators;
    for (auto it = first; it != last; ++it) {
      for (Perm const &perm : it->generators())
        dp_generators.insert(perm.shifted(d).extended(dp_degree));

      d += it->degree();
    }

    // construct direct product
    auto bsgs_options(BSGSOptions::fill_defaults(bsgs_options_));
    bsgs_options.schreier_sims_random_known_order = dp_order;

    return PermGroup(BSGS(dp_degree, dp_generators, &bsgs_options, aborted));
  }

  template<typename IT>
  static BSGS::order_type direct_product_order(IT first, IT last)
  {
    BSGS::order_type dp_order = 1u;

    for (auto it = first; it != last; ++it)
      dp_order *= it->order();

    return dp_order;
  }

  static PermSet wreath_product_generators(PermGroup const &lhs,
                                           PermGroup const &rhs);

  static PermGroup wreath_product(PermGroup const &lhs,
                                  PermGroup const &rhs,
                                  BSGSOptions const *bsgs_options = nullptr,
                                  timeout::flag aborted = timeout::unset());

  static BSGS::order_type wreath_product_order(PermGroup const &lhs,
                                               PermGroup const &rhs);

  bool operator==(PermGroup const &rhs) const;
  bool operator!=(PermGroup const &rhs) const;

  const_iterator begin() const { return const_iterator(*this); }
  const_iterator end() const { return const_iterator(); }

  PermSet generators() const { return _bsgs.strong_generators(); }

  BSGS &bsgs() { return _bsgs; }
  BSGS const &bsgs() const { return _bsgs; }

  unsigned degree() const { return _bsgs.degree(); }
  BSGS::order_type order() const { return _order; }

  unsigned smallest_moved_point() const
  { return generators().smallest_moved_point(); }

  unsigned largest_moved_point() const
  { return generators().largest_moved_point(); }

  std::vector<unsigned> support() const
  { return generators().support(); }

  bool is_trivial() const { return _bsgs.base_empty(); }
  bool is_symmetric() const;
  bool is_shifted_symmetric() const;
  bool is_transitive() const;

  bool contains_element(Perm const &perm) const;
  Perm random_element() const;

  std::vector<PermGroup> disjoint_decomposition(
    bool complete = true, bool disjoint_orbit_optimization = false) const;

  std::vector<PermGroup> wreath_decomposition() const;

private:
  static boost::multiprecision::cpp_int symmetric_order(unsigned deg)
  {
    boost::multiprecision::cpp_int ret(1);
    for (unsigned i = deg; i > 0u; --i)
      ret *= i;

    return ret;
  }

  // complete disjoint decomposition
  bool disjoint_decomp_orbits_dependent(
    Orbit const &orbit1,
    Orbit const &orbit2) const;

  void disjoint_decomp_generate_dependency_classes(
    OrbitPartition &orbits) const;

  static bool disjoint_decomp_restricted_subgroups(
    OrbitPartition const &orbit_split,
    PermGroup const &perm_group,
    std::pair<PermGroup, PermGroup> &restricted_subgroups);

  static std::vector<PermGroup> disjoint_decomp_join_results(
    std::vector<PermGroup> const &res1,
    std::vector<PermGroup> const &res2);

  static std::vector<PermGroup> disjoint_decomp_complete_recursive(
    OrbitPartition const &orbits,
    PermGroup const &perm_group);

  std::vector<PermGroup> disjoint_decomp_complete(
    bool disjoint_orbit_optimization = true) const;

  // incomplete disjoint decomposition
  struct MovedSet : public std::vector<unsigned>
  {
    void init(Perm const &perm);
    bool equivalent(MovedSet const &other) const;
    void extend(MovedSet const &other);
  };

  struct EquivalenceClass
  {
    EquivalenceClass(Perm const &init, MovedSet const &moved)
    : generators({init}),
      moved(moved),
      merged(false)
    {}

    PermSet generators;
    MovedSet moved;
    bool merged;
  };

  std::vector<EquivalenceClass> disjoint_decomp_find_equivalence_classes() const;

  void disjoint_decomp_merge_equivalence_classes(
    std::vector<EquivalenceClass> &equivalence_classes) const;

  std::vector<PermGroup> disjoint_decomp_incomplete() const;

  // wreath decomposition
  std::vector<PermGroup> wreath_decomp_find_stabilizers(
    BlockSystem const &block_system,
    PermGroup const &block_permuter) const;

  PermSet wreath_decomp_construct_block_permuter_image(
    BlockSystem const &block_system,
    PermGroup const &block_permuter) const;

  bool wreath_decomp_reconstruct_block_permuter(
    BlockSystem const &block_system,
    PermGroup const &block_permuter,
    PermSet const &block_permuter_image) const;

  BSGS _bsgs;
  BSGS::order_type _order;
};

std::ostream &operator<<(std::ostream &os, PermGroup const &pg);

} // namespace internal

} // namespace mpsym

#endif // GUARD_PERM_GROUP_H


#pragma once
#ifndef CANNON_RAY_FILTER_H
#define CANNON_RAY_FILTER_H

/*!
 * \file cannon/ray/filter.hpp
 * File containing class definitions for Film reconstruction filters.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace ray {

    /*!
     * \brief Abstract base class representing a reconstruction filter for
     * integrating image samples.
     */
    class Filter {
      public:

        /*!
         * \brief Constructor taking a radius for this filter.
         */
        Filter(const Vector2d& radius) : radius_(radius), inv_radius_(1.0 /
            radius.x(), 1.0 / radius.y()) {}

        /*!
         * \brief Get the value of this filter at the input point. The filter
         * is assumed to be centered at the origin, so the input point should
         * be with respect to the actual filter position.
         *
         * \param p The point to evaluate the filter for.
         *
         * \returns The value of this filter at the point.
         */
        virtual double evaluate(const Vector2d& p) const = 0;

        /*!
         * \brief Destructor.
         */
        virtual ~Filter() {}

      public:
        const Vector2d radius_; //!< Radius of this filter
        const Vector2d inv_radius_; //!< 1 / radius, speeds up math

    };

    /*!
     * \brief Class representing the most basic image reconstruction filter,
     * the Box Filter.
     */
    class BoxFilter : public Filter {
      public:

        /*!
         * \brief Constructor taking the radius of this filter.
         */
        BoxFilter(const Vector2d& radius) : Filter(radius) {}

        /*!
         * \brief Destructor.
         */
        virtual ~BoxFilter() {}

        /*!
         * \brief Inherited from Filter.
         */
        virtual double evaluate(const Vector2d& /*p*/) const override {
          return 1.0;
        }
    };

    /*!
     * \brief Class representing a triangle filter, which is not much better
     * than a box filter.
     */
    class TriangleFilter : public Filter {
      public:

        /*!
         * \brief Constructor taking the radius of this filter.
         */
        TriangleFilter(const Vector2d& radius) : Filter(radius) {}

        /*!
         * \brief Destructor.
         */
        virtual ~TriangleFilter() {}

        /*!
         * \brief Inherited from Filter.
         */
        virtual double evaluate(const Vector2d& p) const override {
          return std::max(0.0, radius_.x() - std::abs(p.x())) *
                 std::max(0.0, radius_.y() - std::abs(p.y()));
        }
    };

    /*!
     * \brief Class representing a Gaussian filter. Reasonable for image
     * reconstruction.
     */
    class GaussianFilter : public Filter {
      public:

        /*!
         * \brief Constructor taking the radius of this filter and rate of
         * Gaussian falloff.
         */
        GaussianFilter(const Vector2d &radius, double alpha)
            : Filter(radius), alpha_(alpha * radius.x()),
              exp_x_(std::exp(-alpha * radius.x() * radius.x())),
              exp_y_(std::exp(-alpha * radius.y() * radius.y())) {}

        /*!
         * \brief Destructor.
         */
        virtual ~GaussianFilter() {}

        /*!
         * \brief Inherited from Filter.
         */
        virtual double evaluate(const Vector2d& p) const override {
          return gaussian_(p.x(), exp_x_) * gaussian_(p.y(), exp_y_);
        }

      private:

        /*!
         * \brief 1D Gaussian function, used to compute 2D Gaussian filter
         * value since this filter is separable.
         *
         * \param d Distance from center of filter
         * \param exp_v Constant term
         */
        double gaussian_(double d, double exp_v) const {
          return std::max(0.0, std::exp(-alpha_ * d * d) - exp_v);
        }

        const double alpha_; //!< Gaussian falloff rate
        const double exp_x_; //!< Cached constant term in x
        const double exp_y_; //!< Cached constant term in y
    };

    /*!
     * \brief Class representing a Mitchell-Netravali filter. Reasonable for image
     * reconstruction.
     */
    class MitchellFilter : public Filter {
      public:

        /*!
         * \brief Constructor taking the radius of this filter and
         * Mitchell-Netravali parameters B and C.
         */
        MitchellFilter(const Vector2d &radius, double b, double c)
            : Filter(radius), b_(b), c_(c) {}

        /*!
         * \brief Destructor.
         */
        virtual ~MitchellFilter() {}

        /*!
         * \brief Inherited from Filter.
         */
        virtual double evaluate(const Vector2d& p) const override {
          return mitchell_1d_(p.x() * inv_radius_.x()) * mitchell_1d_(p.y() * inv_radius_.y());
        }

      private:

        /*!
         * \brief 1D Mitchell function, used to compute 2D Mitchell filter
         * value since this filter is separable.
         *
         * \param x Scaled distance from center of filter
         */
        double mitchell_1d_(double x) const {
          x = std::abs(2 * x);

          if (x > 1)
            return ((-b_ - 6*c_) * x*x*x + (6*b_ + 30*c_) * x*x +
                    (-12*b_ - 48*c_) * x + (8*b_ + 24*c_)) * (1.0 / 6.0);
          else
            return ((12 - 9*b_ - 6*c_) * x*x*x +
                    (-18 + 12*b_ + 6*c_) * x*x +
                    (6 - 2*b_)) * (1.0 / 6.0);
        }

        double b_;
        double c_;

    };

  } // namespace ray
} // namespace cannon

#endif /* ifndef CANNON_RAY_FILTER_H */


//! [distance-all]
#include <algorithm>
#include <chrono>
#include <iostream>
#include <vector>
#include <limits>

#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/deinterleave.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/pack.hpp>

int main(int argc, char** argv)
{
  using namespace std::chrono;
  namespace bs = boost::simd;
  typedef float T;
  using pack_t = bs::pack<T>;

  //! [distance-declare]
  std::size_t num_points = 1600000;
  std::vector<T, bs::allocator<T>> X(num_points);
  std::vector<T, bs::allocator<T>> Y(num_points);
  std::vector<T, bs::allocator<T>> distance0(num_points);
  std::vector<T, bs::allocator<T>> distance1(num_points);
  std::vector<T, bs::allocator<T>> distance2(num_points);
  std::vector<T, bs::allocator<T>> distance3(num_points);

  std::generate(X.begin(), X.end(),
                []() { return T(std::rand()) / std::numeric_limits<int>::max(); });
  std::generate(Y.begin(), Y.end(),
                []() { return T(std::rand()) / std::numeric_limits<int>::max(); });

  T refX = 0, refY = 0;
  //! [distance-declare]

  auto t0 = high_resolution_clock::now();
  //! [distance-scalar]
  for (int i = 0; i < num_points; ++i) {
    auto x       = refX - X[i];
    auto y       = refY - Y[i];
    distance0[i] = std::sqrt(x * x + y * y);
  }
  //! [distance-scalar]
  auto t1 = high_resolution_clock::now();
  std::cout << " time scalar " << duration_cast<microseconds>(t1 - t0).count() << std::endl;

  //! [distance-time]
  t0 = high_resolution_clock::now();
  //! [distance-calc]
  pack_t vrefX = pack_t(refX);
  pack_t vrefY = pack_t(refY);

  for (int i = 0; i < num_points; i += pack_t::static_size) {
    pack_t vX  = bs::aligned_load<pack_t>(&X[i]);
    pack_t vY  = bs::aligned_load<pack_t>(&Y[i]);
    pack_t res = bs::sqrt(bs::sqr(vrefX - vX) + bs::sqr(vrefY - vY));
    bs::aligned_store(res, &distance1[i]);
  }
  //! [distance-calc]
  t1 = high_resolution_clock::now();
  std::cout << " time SIMD " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
  //! [distance-time]

  //! [distance-interleave]
  // The input vector contains interleaved X and Y data, i.e. x0, y0, x1, y1,
  // ..., xn, yn
  std::vector<T, bs::allocator<T>> interleaved_data(num_points * 2);
  for (int i = 0; i < num_points * 2; i += 2) {
    interleaved_data[i]     = X[i / 2];
    interleaved_data[i + 1] = Y[i / 2];
  }
  t0 = high_resolution_clock::now();
  for (int i = 0; i < num_points * 2; i += pack_t::static_size * 2) {
    pack_t v0 = bs::aligned_load<pack_t>(&interleaved_data[i]);
    pack_t v1 = bs::aligned_load<pack_t>(&interleaved_data[i + pack_t::static_size]);

    auto V     = bs::deinterleave(v0, v1);
    pack_t res = bs::sqrt(bs::sqr(vrefX - V[0]) + bs::sqr(vrefY - V[1]));
    bs::aligned_store(res, &distance2[i / 2]);
  }
  t1 = high_resolution_clock::now();
  //! [distance-interleave]
  std::cout << " time SIMD de-interleave " << duration_cast<microseconds>(t1 - t0).count()
            << std::endl;
}
//! [distance-all]


/*
   bern_rat.cpp:  multi-modular algorithm for computing Bernoulli numbers

   Copyright (C) 2008, 2009, David Harvey

   This file is part of the bernmm package (version 1.1).

   bernmm is released under a BSD-style license. See the README file in
   the source distribution for details.
*/

#include <gmp.h>
#include <NTL/ZZ.h>
#include <cmath>
#include <vector>
#include <set>
#include "bern_modp_util.h"
#include "bern_modp.h"
#include "bern_rat.h"

#ifdef USE_THREADS
#include <pthread.h>
#endif

using namespace std;
using namespace NTL;

namespace bernmm {

/*
   Computes the denominator of B_k using Clausen/von Staudt.
*/
void bern_den(mpz_t res, long k, const PrimeTable& table)
{
   mpz_set_ui(res, 1);

   // loop through factors of k
   for (long f = 1; f*f <= k; f++)
   {
      // if f divides k....
      if (k % f == 0)
      {
         // ... then both f + 1 and k/f + 1 are candidates for primes
         // dividing the denominator of B_k
         if (table.is_prime(f + 1))
            mpz_mul_ui(res, res, f + 1);

         if (f*f != k)
            if (table.is_prime(k/f + 1))
               mpz_mul_ui(res, res, k/f + 1);
      }
   }
}

// width of interval for each block
#define BLOCK_SIZE 1000

/*
   Represents that B_k is congruent to _residue_ modulo _modulus_.
*/
struct Item
{
   mpz_t modulus;
   mpz_t residue;

   Item()
   {
      mpz_init(modulus);
      mpz_init(residue);
   }

   ~Item()
   {
      mpz_clear(residue);
      mpz_clear(modulus);
   }
};

/*
   Items get sorted by modulus.
*/
struct Item_cmp
{
   bool operator()(const Item* x, const Item* y)
   {
      return mpz_cmp(x->modulus, y->modulus) < 0;
   }
};

/*
   Returns new Item that combines information from op1 and op2 via CRT.
*/
Item* CRT(Item* op1, Item* op2)
{
   Item* res = new Item;

   // let n1, n2 be the moduli, and r1, r2 be the residues

   // res->modulus = t, where t = 0 mod n1, t = 1 mod n2
   mpz_invert(res->modulus, op1->modulus, op2->modulus);
   mpz_mul(res->modulus, res->modulus, op1->modulus);

   // res->residue = r2 - r1
   mpz_sub(res->residue, op2->residue, op1->residue);
   // res->residue = t * (r2 - r1)
   mpz_mul(res->residue, res->residue, res->modulus);
   // res->residue = r1 + t * (r2 - r1)
   mpz_add(res->residue, res->residue, op1->residue);
   // res->modulus = n1 * n2
   mpz_mul(res->modulus, op1->modulus, op2->modulus);
   // res->residue = r1 mod n1, r2 = mod n2
   mpz_mod(res->residue, res->residue, res->modulus);

   return res;
}

struct State
{
   long k;
   long bound;   // only use primes less than this bound
   const PrimeTable* table;

   // index of block that should be processed next
   long next;

   std::set<Item*, Item_cmp> items;
#ifdef USE_THREADS
   pthread_mutex_t lock;
#endif

   State(long k, long bound, const PrimeTable& table)
   {
      this->k = k;
      this->bound = bound;
      this->next = 0;
      this->table = &table;
#ifdef USE_THREADS
      pthread_mutex_init(&lock, NULL);
#endif
   }

   ~State()
   {
#ifdef USE_THREADS
      pthread_mutex_destroy(&lock);
#endif
   }
};

void* worker(void* arg)
{
   State& state = *((State*) arg);
   long k = state.k;

#ifdef USE_THREADS
   pthread_mutex_lock(&state.lock);
#endif

   while (1)
   {
      if (state.next * BLOCK_SIZE < state.bound)
      {
         // need to generate more modular data

         long next = state.next++;
#ifdef USE_THREADS
         pthread_mutex_unlock(&state.lock);
#endif

         Item* item = new Item;

         mpz_set_ui(item->modulus, 1);
         mpz_set_ui(item->residue, 0);

         for (long p = max(5, state.table->next_prime(next * BLOCK_SIZE));
              p < state.bound && p < (next+1) * BLOCK_SIZE;
              p = state.table->next_prime(p))
         {
            if (k % (p-1) == 0)
               continue;

            // compute B_k mod p
            long b = bern_modp(p, k);

            // CRT into running total
            long x = MulMod(SubMod(b, mpz_fdiv_ui(item->residue, p), p),
                            InvMod(mpz_fdiv_ui(item->modulus, p), p), p);
            mpz_addmul_ui(item->residue, item->modulus, x);
            mpz_mul_ui(item->modulus, item->modulus, p);
         }

#ifdef USE_THREADS
         pthread_mutex_lock(&state.lock);
#endif
         state.items.insert(item);
      }
      else
      {
         // all modular data has been generated

         if (state.items.size() <= 1)
         {
            // no more CRTs for this thread to perform
#ifdef USE_THREADS
            pthread_mutex_unlock(&state.lock);
#endif
            return NULL;
         }

         // CRT two smallest items together
         Item* item1 = *(state.items.begin());
         state.items.erase(state.items.begin());
         Item* item2 = *(state.items.begin());
         state.items.erase(state.items.begin());
#ifdef USE_THREADS
         pthread_mutex_unlock(&state.lock);
#endif

         Item* item3 = CRT(item1, item2);
         delete item1;
         delete item2;

#ifdef USE_THREADS
         pthread_mutex_lock(&state.lock);
#endif
         state.items.insert(item3);
      }
   }
}

void bern_rat(mpq_t res, long k, int num_threads)
{
   // special cases

   if (k == 0)
   {
      // B_0 = 1
      mpq_set_ui(res, 1, 1);
      return;
   }

   if (k == 1)
   {
      // B_1 = -1/2
      mpq_set_si(res, -1, 2);
      return;
   }

   if (k == 2)
   {
      // B_2 = 1/6
      mpq_set_si(res, 1, 6);
      return;
   }

   if (k & 1)
   {
      // B_k = 0 if k is odd
      mpq_set_ui(res, 0, 1);
      return;
   }

   if (num_threads <= 0)
      num_threads = 1;

   mpz_t num, den;
   mpz_init(num);
   mpz_init(den);

   const double log2 =    0.69314718055994528622676;
   const double invlog2 = 1.44269504088896340735992;   // = 1/log(2)

   // compute preliminary prime bound and build prime table
   long bound1 = (long) max(37.0, ceil((k + 0.5) * log(k) * invlog2));
   PrimeTable table(bound1);

   // compute denominator of B_k
   bern_den(den, k, table);

   // compute number of bits we need to resolve the numerator
   long bits = (long) ceil((k + 0.5) * log(k) * invlog2 - 4.094 * k + 2.470
                                      + log(mpz_get_d(den)) * invlog2);

   // compute tighter prime bound
   // (note: we can safely get away with double-precision here. It would
   // only start being insufficient around k = 10^13 or so, which is totally
   // impractical at present.)
   double prod = 1.0;
   long prod_bits = 0;
   long p;
   for (p = 5; prod_bits < bits + 1; p = table.next_prime(p))
   {
      if (p >= NTL_SP_BOUND)
         abort();   // !!!!! not sure what else we can do here...
      if (k % (p-1) != 0)
         prod *= (double) p;
      int exp;
      prod = frexp(prod, &exp);
      prod_bits += exp;
   }
   long bound2 = p;

   State state(k, bound2, table);

#ifdef USE_THREADS
   vector<pthread_t> threads(num_threads - 1);

   pthread_attr_t attr;
   pthread_attr_init(&attr);
#ifdef THREAD_STACK_SIZE
   pthread_attr_setstacksize(&attr, THREAD_STACK_SIZE * 1024);
#endif

   // spawn worker threads to process blocks
   for (long i = 0; i < num_threads - 1; i++)
      pthread_create(&threads[i], &attr, worker, &state);
#endif

   worker(&state);    // make this thread a worker too

#ifdef USE_THREADS
   for (long i = 0; i < num_threads - 1; i++)
      pthread_join(threads[i], NULL);
#endif

   pthread_attr_destroy (&attr);

   // reconstruct B_k as a rational number
   Item* item = *(state.items.begin());
   mpz_mul(num, item->residue, den);
   mpz_mod(num, num, item->modulus);

   if (k % 4 == 0)
   {
      // B_k is negative
      mpz_sub(num, item->modulus, num);
      mpz_neg(num, num);
   }

   delete item;

   mpz_swap(num, mpq_numref(res));
   mpz_swap(den, mpq_denref(res));

   mpz_clear(num);
   mpz_clear(den);
}

};    // end namespace

// end of file ================================================================


#include <iostream>
#include <cmath>
#include <numeric>
#include <fstream>
#include <algorithm>
#include <iomanip>

#include "field2d.h"
#include "weno.h"
#include "rk.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/constants/constants.hpp>
namespace math = boost::math::constants;
using namespace boost::numeric;

auto
pacman_factory ( double r , double alpha , double value=1.0 ) {
  return [=]( double x , double y ) {
    if ( ( x*x + y*y < r*r ) && ( (x<0.0) || (std::abs(y)>alpha*x) ) ) {
      return value;
    }
    return 0.0;
  };
}
#define SQ(X) ((X)*(X))
auto
gauss_factory ( double X0 , double Y0 , double tx , double ty ) {
  return [=]( double x , double y ) {
    return std::exp(-SQ(x-X0)/tx - SQ(y-Y0)/ty);
  };
}
#undef SQ

int
main(int,char**)
{
  std::size_t N = 100;
  double xmax = 5.0;
  std::size_t Nx=N,Ny=N;
  field2d<double> f(boost::extents[Nx][Ny]);
  f.range.x_min = -xmax; f.range.x_max = xmax;
  f.range.y_min = -xmax; f.range.y_max = xmax;
  f.compute_steps();

  field2d<double> g(boost::extents[Nx][Ny]);
  g.range = f.range;
  g.steps = f.steps;

  auto pacman = pacman_factory(1.0,0.5);
  auto gauss = gauss_factory(0.0,1.0,0.75,0.25);
  for ( auto i=0u ; i<f.size(0) ; ++i ) {
    for ( auto j=0u ; j<f.size(1) ; ++j ) {
       //f[i][j] = gauss(f.x(i),f.y(j));
       f[i][j] = pacman(f.x(i),f.y(j));
      // f[i][j] = std::cos(2.*math::pi<double>()*f.x(i)/xmax)*std::sin(2.*math::pi<double>()*f.y(j)/xmax);
      //f[i][j] = std::cos(2.*math::pi<double>()*f.x(i)/xmax);

       g[i][j] = f[i][j];
    }
  }
  ublas::vector<double> E(2,0); E[0] = 0.1; E[1] = 0.1;

  //double dt= 1.3*f.steps.dx/(std::abs(E[0])+std::abs(E[1])+2.0*xmax);
  double dt= 1.3*f.steps.dx/(std::abs(E[0])+std::abs(E[1]));
  double Tf= 1.0; //.5*math::pi<double>();

  auto save_f = [](field2d<double> const& f,std::string filename) {
    std::ofstream of(filename);
    of << f << std::endl;
    of.close();
  };

  save_f(f,"finit.dat");
  save_f(g,"ginit.dat");

  auto Lij_f = [&](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    return - (weno2d::weno_x( E[0]+u.y(j) ,u,i,j) + weno2d::weno_y(  E[1]-u.x(i) ,u,i,j));
  };
  auto Lij_g = [&](double tn , const field2d<double> & u , std::size_t i , std::size_t j){
    return - ( weno2d::weno_x( E[0]*std::cos(tn) - E[1]*std::sin(tn) ,u,i,j)
             + weno2d::weno_y( E[0]*std::sin(tn) + E[1]*std::cos(tn) ,u,i,j) );
  };

  std::cout << "dt: "<< dt << "\tNiter: " << std::floor(Tf/dt) << "\n";
  double current_time = 0.0;
  std::size_t i_iter = 0;
  while ( current_time < Tf ) {
    std::cout << "[" << std::setw(10) << current_time << "] \r" << std::flush;

    f = rk33( Lij_f , current_time , f , dt );

    g = rk33( Lij_g , current_time , g , dt );

    ++i_iter;
    current_time += dt;
    if ( current_time+dt > Tf ) { dt = Tf - current_time; }
  }
  std::cout << "[" << std::setw(10) << current_time << "]" << std::endl;

  save_f(f,"fend.dat");
  save_f(g,"gend.dat");

  f = rotation(g,current_time);

  save_f(f,"ffend.dat");

  return 0;
}


#include "../include/ScaramuzzaCamera.h"
#include "../include/Utils.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SVD>
#include <iomanip>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>

Eigen::VectorXd polyfit(Eigen::VectorXd &xVec, Eigen::VectorXd &yVec,
                        int poly_order)
{
  assert(poly_order > 0);
  assert(xVec.size() > poly_order);
  assert(xVec.size() == yVec.size());

  Eigen::MatrixXd A(xVec.size(), poly_order + 1);
  Eigen::VectorXd B(xVec.size());

  for (int i = 0; i < xVec.size(); ++i)
  {
    const double x = xVec(i);
    const double y = yVec(i);

    double x_pow_k = 1.0;

    for (int k = 0; k <= poly_order; ++k)
    {
      A(i, k) = x_pow_k;
      x_pow_k *= x;
    }

    B(i) = y;
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      A, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::VectorXd x = svd.solve(B);

  return x;
}

OCAMCamera::Parameters::Parameters()
    : Camera::Parameters(SCARAMUZZA),
      m_C(0.0),
      m_D(0.0),
      m_E(0.0),
      m_center_x(0.0),
      m_center_y(0.0)
{
  memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE);
  memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
}

bool OCAMCamera::Parameters::readFromYamlFile(const std::string &filename)
{
  cv::FileStorage fs(filename, cv::FileStorage::READ);

  if (!fs.isOpened())
  {
    return false;
  }

  if (!fs["model_type"].isNone())
  {
    std::string sModelType;
    fs["model_type"] >> sModelType;

    if (!boost::iequals(sModelType, "scaramuzza"))
    {
      return false;
    }
  }

  m_modelType = SCARAMUZZA;
  fs["camera_name"] >> m_cameraName;
  m_imageWidth = static_cast<int>(fs["image_width"]);
  m_imageHeight = static_cast<int>(fs["image_height"]);

  cv::FileNode n = fs["poly_parameters"];
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    m_poly[i] = static_cast<double>(
        n[std::string("p") + boost::lexical_cast<std::string>(i)]);

  n = fs["inv_poly_parameters"];
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    m_inv_poly[i] = static_cast<double>(
        n[std::string("p") + boost::lexical_cast<std::string>(i)]);

  n = fs["affine_parameters"];
  m_C = static_cast<double>(n["ac"]);
  m_D = static_cast<double>(n["ad"]);
  m_E = static_cast<double>(n["ae"]);

  m_center_x = static_cast<double>(n["cx"]);
  m_center_y = static_cast<double>(n["cy"]);

  return true;
}

void OCAMCamera::Parameters::writeToYamlFile(
    const std::string &filename) const
{
  cv::FileStorage fs(filename, cv::FileStorage::WRITE);

  fs << "model_type"
     << "scaramuzza";
  fs << "camera_name" << m_cameraName;
  fs << "image_width" << m_imageWidth;
  fs << "image_height" << m_imageHeight;

  fs << "poly_parameters";
  fs << "{";
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    fs << std::string("p") + boost::lexical_cast<std::string>(i) << m_poly[i];
  fs << "}";

  fs << "inv_poly_parameters";
  fs << "{";
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    fs << std::string("p") + boost::lexical_cast<std::string>(i)
       << m_inv_poly[i];
  fs << "}";

  fs << "affine_parameters";
  fs << "{"
     << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy"
     << m_center_y << "}";

  fs.release();
}

OCAMCamera::Parameters &OCAMCamera::Parameters::operator=(
    const OCAMCamera::Parameters &other)
{
  if (this != &other)
  {
    m_modelType = other.m_modelType;
    m_cameraName = other.m_cameraName;
    m_imageWidth = other.m_imageWidth;
    m_imageHeight = other.m_imageHeight;
    m_C = other.m_C;
    m_D = other.m_D;
    m_E = other.m_E;
    m_center_x = other.m_center_x;
    m_center_y = other.m_center_y;

    memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);
    memcpy(m_inv_poly, other.m_inv_poly,
           sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
  }

  return *this;
}

std::ostream &operator<<(std::ostream &out,
                         const OCAMCamera::Parameters &params)
{
  out << "Camera Parameters:" << std::endl;
  out << "    model_type "
      << "scaramuzza" << std::endl;
  out << "   camera_name " << params.m_cameraName << std::endl;
  out << "   image_width " << params.m_imageWidth << std::endl;
  out << "  image_height " << params.m_imageHeight << std::endl;

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

  out << "Poly Parameters" << std::endl;
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    out << std::string("p") + boost::lexical_cast<std::string>(i) << ": "
        << params.m_poly[i] << std::endl;

  out << "Inverse Poly Parameters" << std::endl;
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    out << std::string("p") + boost::lexical_cast<std::string>(i) << ": "
        << params.m_inv_poly[i] << std::endl;

  out << "Affine Parameters" << std::endl;
  out << "            ac " << params.m_C << std::endl
      << "            ad " << params.m_D << std::endl
      << "            ae " << params.m_E << std::endl;
  out << "            cx " << params.m_center_x << std::endl
      << "            cy " << params.m_center_y << std::endl;

  return out;
}

OCAMCamera::OCAMCamera() : m_inv_scale(0.0) {}

OCAMCamera::OCAMCamera(const OCAMCamera::Parameters &params)
    : mParameters(params)
{
  m_inv_scale = 1.0 / (params.C() - params.D() * params.E());
}

Camera::ModelType OCAMCamera::modelType(void) const
{
  return mParameters.modelType();
}

const std::string &OCAMCamera::cameraName(void) const
{
  return mParameters.cameraName();
}

int OCAMCamera::imageWidth(void) const { return mParameters.imageWidth(); }

int OCAMCamera::imageHeight(void) const { return mParameters.imageHeight(); }

void OCAMCamera::estimateIntrinsics(
    const cv::Size &boardSize,
    const std::vector<std::vector<cv::Point3f>> &objectPoints,
    const std::vector<std::vector<cv::Point2f>> &imagePoints)
{
  // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" <<
  // std::endl;
  // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED");

  // Reference: Page 30 of
  // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion
  // Estimation, ETH Zurich. Thesis no. 17635."
  // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf
  // Matlab code: calibrate.m

  // First, estimate every image's extrinsics parameters
  std::vector<Eigen::Matrix3d> RList;
  std::vector<Eigen::Vector3d> TList;

  RList.reserve(imagePoints.size());
  TList.reserve(imagePoints.size());

  // i-th image
  for (size_t image_index = 0; image_index < imagePoints.size();
       ++image_index)
  {
    const std::vector<cv::Point3f> &objPts = objectPoints.at(image_index);
    const std::vector<cv::Point2f> &imgPts = imagePoints.at(image_index);

    assert(objPts.size() == imgPts.size());
    assert(objPts.size() ==
           static_cast<unsigned int>(boardSize.width * boardSize.height));

    Eigen::MatrixXd M(objPts.size(), 6);

    for (size_t corner_index = 0; corner_index < objPts.size();
         ++corner_index)
    {
      double X = objPts.at(corner_index).x;
      double Y = objPts.at(corner_index).y;
      assert(objPts.at(corner_index).z == 0.0);

      double u = imgPts.at(corner_index).x;
      double v = imgPts.at(corner_index).y;

      M(corner_index, 0) = -v * X;
      M(corner_index, 1) = -v * Y;
      M(corner_index, 2) = u * X;
      M(corner_index, 3) = u * Y;
      M(corner_index, 4) = -v;
      M(corner_index, 5) = u;
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        M, Eigen::ComputeFullU | Eigen::ComputeFullV);
    assert(svd.matrixV().cols() == 6);
    Eigen::VectorXd h = -svd.matrixV().col(5);

    // scaled version of R and T
    const double sr11 = h(0);
    const double sr12 = h(1);
    const double sr21 = h(2);
    const double sr22 = h(3);
    const double st1 = h(4);
    const double st2 = h(5);

    const double AA = square(sr11 * sr12 + sr21 * sr22);
    const double BB = square(sr11) + square(sr21);
    const double CC = square(sr12) + square(sr22);

    const double sr32_squared_1 =
        (-(CC - BB) + sqrt(square(CC - BB) + 4.0 * AA)) / 2.0;
    const double sr32_squared_2 =
        (-(CC - BB) - sqrt(square(CC - BB) + 4.0 * AA)) / 2.0;

    // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 +
    // (CC-BB)*sr32_squared_1 - AA);

    std::vector<double> sr32_squared_values;
    if (sr32_squared_1 > 0)
      sr32_squared_values.push_back(sr32_squared_1);
    if (sr32_squared_2 > 0)
      sr32_squared_values.push_back(sr32_squared_2);
    assert(!sr32_squared_values.empty());

    std::vector<double> sr32_values;
    std::vector<double> sr31_values;
    for (auto sr32_squared : sr32_squared_values)
    {
      for (int sign = -1; sign <= 1; sign += 2)
      {
        const double sr32 = static_cast<double>(sign) * std::sqrt(sr32_squared);
        sr32_values.push_back(sr32);
        if (sr32_squared == 0.0)
        {
          // sr31 can be calculated through norm equality,
          // but it has positive and negative posibilities
          // positive one
          sr31_values.push_back(std::sqrt(CC - BB));
          // negative one
          sr32_values.push_back(sr32);
          sr31_values.push_back(-std::sqrt(CC - BB));

          break; // skip the same situation
        }
        else
        {
          // sr31 can be calculated throught dot product == 0
          sr31_values.push_back(-(sr11 * sr12 + sr21 * sr22) / sr32);
        }
      }
    }

    // std::cout << "h= " << std::setprecision(12) << h.transpose() <<
    // std::endl;
    // std::cout << "length: " << sr32_values.size() << " & " <<
    // sr31_values.size() << std::endl;

    assert(!sr31_values.empty());
    assert(sr31_values.size() == sr32_values.size());

    std::vector<Eigen::Matrix3d> H_values;
    for (size_t i = 0; i < sr31_values.size(); ++i)
    {
      const double sr31 = sr31_values.at(i);
      const double sr32 = sr32_values.at(i);
      const double lambda = 1.0 / sqrt(sr11 * sr11 + sr21 * sr21 + sr31 * sr31);
      Eigen::Matrix3d H;
      H.setZero();
      H(0, 0) = sr11;
      H(0, 1) = sr12;
      H(0, 2) = st1;
      H(1, 0) = sr21;
      H(1, 1) = sr22;
      H(1, 2) = st2;
      H(2, 0) = sr31;
      H(2, 1) = sr32;
      H(2, 2) = 0;

      H_values.push_back(lambda * H);
      H_values.push_back(-lambda * H);
    }

    for (auto &H : H_values)
    {
      // std::cout << "H=\n" << H << std::endl;
      Eigen::Matrix3d R;
      R.col(0) = H.col(0);
      R.col(1) = H.col(1);
      R.col(2) = H.col(0).cross(H.col(1));
      // std::cout << "R33 = " << R(2,2) << std::endl;
    }

    std::vector<Eigen::Matrix3d> H_candidates;

    for (auto &H : H_values)
    {
      Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4);
      Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size());
      A_mat.setZero();
      B_vec.setZero();

      size_t line_index = 0;

      // iterate images
      const double &r11 = H(0, 0);
      const double &r12 = H(0, 1);
      // const double& r13 = H(0,2);
      const double &r21 = H(1, 0);
      const double &r22 = H(1, 1);
      // const double& r23 = H(1,2);
      const double &r31 = H(2, 0);
      const double &r32 = H(2, 1);
      // const double& r33 = H(2,2);
      const double &t1 = H(0);
      const double &t2 = H(1);

      // iterate chessboard corners in the image
      for (size_t j = 0; j < imagePoints.at(image_index).size(); ++j)
      {
        assert(line_index == 2 * j);

        const double &X = objectPoints.at(image_index).at(j).x;
        const double &Y = objectPoints.at(image_index).at(j).y;
        const double &u = imagePoints.at(image_index).at(j).x;
        const double &v = imagePoints.at(image_index).at(j).y;

        double A = r21 * X + r22 * Y + t2;
        double B = v * (r31 * X + r32 * Y);
        double C = r11 * X + r12 * Y + t1;
        double D = u * (r31 * X + r32 * Y);
        double rou = std::sqrt(u * u + v * v);

        A_mat(line_index + 0, 0) = A;
        A_mat(line_index + 1, 0) = C;
        A_mat(line_index + 0, 1) = A * rou;
        A_mat(line_index + 1, 1) = C * rou;
        A_mat(line_index + 0, 2) = A * rou * rou;
        A_mat(line_index + 1, 2) = C * rou * rou;

        A_mat(line_index + 0, 3) = -v;
        A_mat(line_index + 1, 3) = -u;
        B_vec(line_index + 0) = B;
        B_vec(line_index + 1) = D;

        line_index += 2;
      }

      assert(line_index == static_cast<unsigned int>(A_mat.rows()));

      // pseudo-inverse for polynomial parameters and all t3s
      {
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
            A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);

        Eigen::VectorXd x = svd.solve(B_vec);

        // std::cout << "x(poly and t3) = " << x << std::endl;

        if (x(2) > 0 && x(3) > 0)
        {
          H_candidates.push_back(H);
        }
      }
    }

    // printf("H_candidates.size()=%zu\n", H_candidates.size());
    assert(H_candidates.size() == 1);

    Eigen::Matrix3d &H = H_candidates.front();

    Eigen::Matrix3d R;
    R.col(0) = H.col(0);
    R.col(1) = H.col(1);
    R.col(2) = H.col(0).cross(H.col(1));

    Eigen::Vector3d T = H.col(2);
    RList.push_back(R);
    TList.push_back(T);

    // std::cout << "#" << image_index << " frame" << " R =" << R << " \nT = "
    // << T.transpose() << std::endl;
  }

  // Second, estimate camera intrinsic parameters and all t3
  Eigen::MatrixXd A_mat(2 * imagePoints.size() * imagePoints.at(0).size(),
                        SCARAMUZZA_POLY_SIZE - 1 + imagePoints.size());
  Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size());
  A_mat.setZero();
  B_vec.setZero();

  size_t line_index = 0;

  // iterate images
  for (size_t i = 0; i < imagePoints.size(); ++i)
  {
    const double &r11 = RList.at(i)(0, 0);
    const double &r12 = RList.at(i)(0, 1);
    // const double& r13 = RList.at(i)(0,2);
    const double &r21 = RList.at(i)(1, 0);
    const double &r22 = RList.at(i)(1, 1);
    // const double& r23 = RList.at(i)(1,2);
    const double &r31 = RList.at(i)(2, 0);
    const double &r32 = RList.at(i)(2, 1);
    // const double& r33 = RList.at(i)(2,2);
    const double &t1 = TList.at(i)(0);
    const double &t2 = TList.at(i)(1);

    // iterate chessboard corners in the image
    for (size_t j = 0; j < imagePoints.at(i).size(); ++j)
    {
      assert(line_index == 2 * (i * imagePoints.at(0).size() + j));

      const double &X = objectPoints.at(i).at(j).x;
      const double &Y = objectPoints.at(i).at(j).y;
      const double &u = imagePoints.at(i).at(j).x;
      const double &v = imagePoints.at(i).at(j).y;

      double A = r21 * X + r22 * Y + t2;
      double B = v * (r31 * X + r32 * Y);
      double C = r11 * X + r12 * Y + t1;
      double D = u * (r31 * X + r32 * Y);
      double rou = std::sqrt(u * u + v * v);

      for (int k = 1; k <= SCARAMUZZA_POLY_SIZE - 1; ++k)
      {
        double pow_rou = 0.0;
        if (k == 1)
        {
          pow_rou = 1.0;
        }
        else
        {
          pow_rou = std::pow(rou, k);
        }

        A_mat(line_index + 0, k - 1) = A * pow_rou;
        A_mat(line_index + 1, k - 1) = C * pow_rou;
      }

      A_mat(line_index + 0, SCARAMUZZA_POLY_SIZE - 1 + i) = -v;
      A_mat(line_index + 1, SCARAMUZZA_POLY_SIZE - 1 + i) = -u;
      B_vec(line_index + 0) = B;
      B_vec(line_index + 1) = D;

      line_index += 2;
    }
  }

  assert(line_index == static_cast<unsigned int>(A_mat.rows()));

  Eigen::Matrix<double, SCARAMUZZA_POLY_SIZE, 1> poly_coeff;
  // pseudo-inverse for polynomial parameters and all t3s
  {
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::VectorXd x = svd.solve(B_vec);

    poly_coeff[0] = x(0);
    poly_coeff[1] = 0.0;
    for (int i = 1; i < poly_coeff.size() - 1; ++i)
    {
      poly_coeff[i + 1] = x(i);
    }
    assert(x.size() ==
           static_cast<unsigned int>(SCARAMUZZA_POLY_SIZE - 1 + TList.size()));
  }

  Parameters params = getParameters();

  // Affine matrix A is constructed as [C D; E 1]
  params.C() = 1.0;
  params.D() = 0.0;
  params.E() = 0.0;

  params.center_x() = params.imageWidth() / 2.0;
  params.center_y() = params.imageHeight() / 2.0;

  for (size_t i = 0; i < SCARAMUZZA_POLY_SIZE; ++i)
  {
    params.poly(i) = poly_coeff[i];
  }

  // params.poly(0) = -216.9657476318;
  // params.poly(1) = 0.0;
  // params.poly(2) = 0.0017866911;
  // params.poly(3) = -0.0000019866;
  // params.poly(4) =  0.0000000077;

  // inv_poly
  {
    std::vector<double> rou_vec;
    std::vector<double> z_vec;
    for (double rou = 0.0;
         rou <= (params.imageWidth() + params.imageHeight()) / 2; rou += 0.1)
    {
      double rou_pow_k = 1.0;
      double z = 0.0;

      for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++)
      {
        z += rou_pow_k * params.poly(k);
        rou_pow_k *= rou;
      }

      rou_vec.push_back(rou);
      z_vec.push_back(z);
    }

    assert(rou_vec.size() == z_vec.size());
    Eigen::VectorXd xVec(rou_vec.size());
    Eigen::VectorXd yVec(rou_vec.size());

    for (size_t i = 0; i < rou_vec.size(); ++i)
    {
      xVec(i) = std::atan2(-z_vec.at(i), rou_vec.at(i));
      yVec(i) = rou_vec.at(i);
    }

    // use lower order poly to eliminate over-fitting cause by noisy/inaccurate
    // data
    const int poly_fit_order = 4;
    Eigen::VectorXd inv_poly_coeff = polyfit(xVec, yVec, poly_fit_order);

    for (int i = 0; i <= poly_fit_order; ++i)
    {
      params.inv_poly(i) = inv_poly_coeff(i);
    }
  }

  setParameters(params);

  std::cout << "initial params:\n"
            << params << std::endl;
}

/**
 * \brief Lifts a point from the image plane to the unit sphere
 *
 * \param p image coordinates
 * \param P coordinates of the point on the sphere
 */
void OCAMCamera::liftSphere(const Eigen::Vector2d &p,
                            Eigen::Vector3d &P) const
{
  liftProjective(p, P);
  P.normalize();
}

/**
 * \brief Lifts a point from the image plane to its projective ray
 *
 * \param p image coordinates
 * \param P coordinates of the projective ray
 */
void OCAMCamera::liftProjective(const Eigen::Vector2d &p,
                                Eigen::Vector3d &P) const
{
  // Relative to Center
  Eigen::Vector2d xc(p[0] - mParameters.center_x(),
                     p[1] - mParameters.center_y());

  // Affine Transformation
  // xc_a = inv(A) * xc;
  Eigen::Vector2d xc_a(
      m_inv_scale * (xc[0] - mParameters.D() * xc[1]),
      m_inv_scale * (-mParameters.E() * xc[0] + mParameters.C() * xc[1]));

  double phi = std::sqrt(xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]);
  double phi_i = 1.0;
  double z = 0.0;

  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
  {
    z += phi_i * mParameters.poly(i);
    phi_i *= phi;
  }

  P << xc[0], xc[1], -z;
}

/**
 * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *
 * \param P 3D point coordinates
 * \param p return value, contains the image point coordinates
 */
void OCAMCamera::spaceToPlane(const Eigen::Vector3d &P,
                              Eigen::Vector2d &p) const
{
  double norm = std::sqrt(P[0] * P[0] + P[1] * P[1]);
  double theta = std::atan2(-P[2], norm);
  double rho = 0.0;
  double theta_i = 1.0;

  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
  {
    rho += theta_i * mParameters.inv_poly(i);
    theta_i *= theta;
  }

  double invNorm = 1.0 / norm;
  Eigen::Vector2d xn(P[0] * invNorm * rho, P[1] * invNorm * rho);

  p << xn[0] * mParameters.C() + xn[1] * mParameters.D() +
           mParameters.center_x(),
      xn[0] * mParameters.E() + xn[1] + mParameters.center_y();
}

/**
 * \brief Projects an undistorted 2D point p_u to the image plane
 *
 * \param p_u 2D point coordinates
 * \return image point coordinates
 */
void OCAMCamera::undistToPlane(const Eigen::Vector2d &p_u,
                               Eigen::Vector2d &p) const
{
  Eigen::Vector3d P(p_u[0], p_u[1], 1.0);
  spaceToPlane(P, p);
}

#if 0
void
OCAMCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const
{
    cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());

    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);

    for (int v = 0; v < imageSize.height; ++v)
    {
        for (int u = 0; u < imageSize.width; ++u)
        {
            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;

            double xi = mParameters.xi();
            double d2 = mx_u * mx_u + my_u * my_u;

            Eigen::Vector3d P;
            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));

            Eigen::Vector2d p;
            spaceToPlane(P, p);

            mapX.at<float>(v,u) = p(0);
            mapY.at<float>(v,u) = p(1);
        }
    }

    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
}
#endif

cv::Mat OCAMCamera::initUndistortRectifyMap(cv::Mat &map1, cv::Mat &map2,
                                            float fx, float fy,
                                            cv::Size imageSize, float cx,
                                            float cy, cv::Mat rmat) const
{
  if (imageSize == cv::Size(0, 0))
  {
    imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());
  }

  cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
  cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);

  Eigen::Matrix3f K_rect;

  K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy,
      cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1;

  if (fx < 0 || fy < 0)
  {
    throw std::string(std::string(__FUNCTION__) +
                      ": Focal length must be specified");
  }

  Eigen::Matrix3f K_rect_inv = K_rect.inverse();

  Eigen::Matrix3f R, R_inv;
  cv::cv2eigen(rmat, R);
  R_inv = R.inverse();

  for (int v = 0; v < imageSize.height; ++v)
  {
    for (int u = 0; u < imageSize.width; ++u)
    {
      Eigen::Vector3f xo;
      xo << u, v, 1;

      Eigen::Vector3f uo = R_inv * K_rect_inv * xo;

      Eigen::Vector2d p;
      spaceToPlane(uo.cast<double>(), p);

      mapX.at<float>(v, u) = p(0);
      mapY.at<float>(v, u) = p(1);
    }
  }

  cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);

  cv::Mat K_rect_cv;
  cv::eigen2cv(K_rect, K_rect_cv);
  return K_rect_cv;
}

int OCAMCamera::parameterCount(void) const
{
  return SCARAMUZZA_CAMERA_NUM_PARAMS;
}

const OCAMCamera::Parameters &OCAMCamera::getParameters(void) const
{
  return mParameters;
}

void OCAMCamera::setParameters(const OCAMCamera::Parameters &parameters)
{
  mParameters = parameters;

  m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());
}

void OCAMCamera::readParameters(const std::vector<double> &parameterVec)
{
  if ((int)parameterVec.size() != parameterCount())
  {
    return;
  }

  Parameters params = getParameters();

  params.C() = parameterVec.at(0);
  params.D() = parameterVec.at(1);
  params.E() = parameterVec.at(2);
  params.center_x() = parameterVec.at(3);
  params.center_y() = parameterVec.at(4);
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    params.poly(i) = parameterVec.at(5 + i);
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);

  setParameters(params);
}

void OCAMCamera::writeParameters(std::vector<double> &parameterVec) const
{
  parameterVec.resize(parameterCount());
  parameterVec.at(0) = mParameters.C();
  parameterVec.at(1) = mParameters.D();
  parameterVec.at(2) = mParameters.E();
  parameterVec.at(3) = mParameters.center_x();
  parameterVec.at(4) = mParameters.center_y();
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    parameterVec.at(5 + i) = mParameters.poly(i);
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);
}

void OCAMCamera::writeParametersToYamlFile(const std::string &filename) const
{
  mParameters.writeToYamlFile(filename);
}

std::string OCAMCamera::parametersToString(void) const
{
  std::ostringstream oss;
  oss << mParameters;

  return oss.str();
}


#include <base/cgal_typedefs.h>
#include <util/geometricOperations.h>
#include <Eigen/Core>
#include <CGAL/Polyhedron_copy_3.h>

using namespace std;

double dot(const Vector& v1, const Vector& v2){
    return v1.x()*v2.x() + v1.y()*v2.y() + v1.z()*v2.z();
}

double dot(const Point& v1, const Point& v2){
    return v1.x()*v2.x() + v1.y()*v2.y() + v1.z()*v2.z();
}

double dot(const Vector& v1, const Point& v2){
    return v1.x()*v2.x() + v1.y()*v2.y() + v1.z()*v2.z();
}

double dot(const Point& v1, const Vector& v2){
    return v1.x()*v2.x() + v1.y()*v2.y() + v1.z()*v2.z();
}

Vector crossV(const Vector& v1, const Vector& v2){

    return Vector(v1.y()*v2.z() - v1.z()*v2.y(),
                  v1.z()*v2.x() - v1.x()*v2.z(),
                  v1.x()*v2.y() - v1.y()*v2.x());
}

Vector crossV(const Point& v1, const Point& v2){

    return Vector(v1.y()*v2.z() - v1.z()*v2.y(),
                  v1.z()*v2.x() - v1.x()*v2.z(),
                  v1.x()*v2.y() - v1.y()*v2.x());
}

Point crossP(const Vector& v1, const Vector& v2){

    return Point(v1.y()*v2.z() - v1.z()*v2.y(),
                  v1.z()*v2.x() - v1.x()*v2.z(),
                  v1.x()*v2.y() - v1.y()*v2.x());
}

Point crossP(const Point& v1, const Point& v2){

    return Point(v1.y()*v2.z() - v1.z()*v2.y(),
                  v1.z()*v2.x() - v1.x()*v2.z(),
                  v1.x()*v2.y() - v1.y()*v2.x());
}

Point neg(const Point& p){

    return Point(-p.x(), -p.y(), -p.z());
}

const double polyhedronVolume(vector<pair<Triangle, Vector>> polyhedron){

    double volume;
    for(auto const& facet : polyhedron){
        volume+= dot(facet.first[0], facet.second);
    }
    return volume/6;

}

double pointPlaneDistance(EPICK::Plane_3 plane, Point point){

    // implemented from: http://mathworld.wolfram.com/Point-PlaneDistance.html

    double dist = (plane.a()*point.x() + plane.b()*point.y() + plane.c()*point.z() + plane.d()) /
            std::sqrt(plane.a()*plane.a() + plane.b()*plane.b() + plane.c()*plane.c());

    return dist;
}

// Implementation of geometry visualized in Figure 9 in P. Labatut, J‐P. Pons,
// and R. Keriven. "Robust and efficient surface reconstruction from range
// data." Computer graphics forum, 2009.
double computeCosFacetCellAngle(const Delaunay& Dt,
                                const Delaunay::Facet& facet) {
  if (Dt.is_infinite(facet.first)) {
    return 1.0;
  }

  const Triangle triangle = Dt.triangle(facet);

  const Vector facet_normal =
      CGAL::cross_product(triangle[1] - triangle[0], triangle[2] - triangle[0]);
  const double facet_normal_length_squared = facet_normal.squared_length();
  if (facet_normal_length_squared == 0.0) {
    return 0.5;
  }

  const Vector co_tangent = CGAL::circumcenter(Dt.tetrahedron(facet.first)) - triangle[0];
  const float co_tangent_length_squared = co_tangent.squared_length();
  if (co_tangent_length_squared == 0.0) {
    return 0.5;
  }

  return (facet_normal * co_tangent) /
         std::sqrt(facet_normal_length_squared * co_tangent_length_squared);
}

double computeFacetArea(const Delaunay& Dt,
                        const Delaunay::Facet& facet){
    if(Dt.is_infinite(facet))
        return 0.0;
    else
        return sqrt(Dt.triangle(facet).squared_area());
};

const Point barycenter(const Cell_handle& ch){
    double x = 0, y = 0, z = 0;
    for(int i = 0; i < 4; i++){
        x+=ch->vertex(i)->point().x();
        y+=ch->vertex(i)->point().y();
        z+=ch->vertex(i)->point().z();
    }
    return Point(x/4.0,y/4.0,z/4.0);
}

bool rayTriangleIntersection(const Point& rayOrigin,
                           const Vector& rayVector,
                           const Triangle& inTriangle,
                           Point& outIntersectionPoint){

    // implemented after: https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm

    using namespace Eigen;
    const double EPSILON = 0.0000001;

    // init "Eigen" vectors
    Vector3d rayO(rayOrigin.x(), rayOrigin.y(), rayOrigin.z());
    Vector3d rayV(rayVector.x(), rayVector.y(), rayVector.z());

    Vector3d vertex0(inTriangle.vertex(0).x(), inTriangle.vertex(0).y(), inTriangle.vertex(0).z());
    Vector3d vertex1(inTriangle.vertex(1).x(), inTriangle.vertex(1).y(), inTriangle.vertex(1).z());
    Vector3d vertex2(inTriangle.vertex(2).x(), inTriangle.vertex(2).y(), inTriangle.vertex(2).z());

    Vector3d edge1, edge2, h, s, q;
    double a;
    edge1 = vertex1 - vertex0;
    edge2 = vertex2 - vertex0;

    h = rayV.cross(edge2);
    a = edge1.dot(h);
    if (a > -EPSILON && a < EPSILON)
        return false;    // This ray is parallel to this triangle.
    double f = 1.0/a;
    s = rayO - vertex0;
    double u = f * s.dot(h);    // barycentric coordinate u
    if (u < 0.0 || u > 1.0)     // if u is not between 0 and 1, intersection point does not lie in triangle
        return false;
    q = s.cross(edge1);
    double v = f * rayV.dot(q); // barycentric coordinate v
    if (v < 0.0 || u + v > 1.0) // if v is not between 0 and 1, intersection point does not lie in triangle
        return false;
    // At this stage we can compute t to find out where the intersection point is on the line.
    double t = f * edge2.dot(q);
    if (t > EPSILON) // ray intersection
    {
        outIntersectionPoint = rayOrigin + rayVector * t;
        return true;
    }
    else // This means that there is a line intersection but not a ray intersection.
        return false;
}


#include "Classes.h"
#include <Eigen/Dense>
#include <fstream>
#include <cstdlib>
using namespace Eigen;

PlaneProjection* Object3D::project3D(double projectionPlane[4]) {
    ///
    /// General Function to project the current 3D object onto the projection plane passed as parameter "projectionPlane"
    ///
    // cout << "Check"<<endl; // --------Remove
    // vector<Point> projectedVertices(len); --------Remove
    bool checkinput=check3dobject();
    vector<bool> isHidden;
    vector<int> isHiddenEdge;
    int len = vertices.size();
    // Before anything else, make all faces cyclic
    for(int i = 0; i < this->faces.size(); i++) {
        // cout << "Operating over " << faces[i];
        faces[i] = getCyclicFace(faces[i]);
        // cout << " Got this face: " << faces[i] << endl;
    }
    // cout << "Cyclic Faces: ";
    // for(int i = 0; i < this->faces.size(); i++) {
    //     cout << faces[i];
    // }
    // cout << endl;
    // Iteration to find all the projected vertices
    for (int i = 0; i < len; i++) {
        projectedVertices.push_back(vertices[i].projectPoint(projectionPlane));
    }
    cout << "vertices projected "<<endl;
    // Now, we check if any of the vertices is hidden or not
    for (int i = 0; i < len; i++) {
        // cout << vertices[i].x << vertices[i].y << vertices[i].z << endl; -------Remove
        int flag = 0;
        for (int j = 0; j < this->faces.size(); j++) {
            if(checkHiddenVertice(vertices[i],faces[j],projectionPlane,0))
            {
                flag = 1;
                break;
            }
        }
        if(flag==1)
            isHidden.push_back(true);
        else
            isHidden.push_back(false);
    }
    cout << "Vertices Done" << endl;
    // Next, we check for each edge, if it is hidden or not
    /* Note: If a part of an edge is hidden then the edge is replaced by the first part
       and the remaining part is added in the edges vector of the Object3D object */
    for (auto i = 0; i < this->edges.size(); i++) {
        int flag = 0;
        for (int j = 0; j < this->faces.size(); j++) {
            // cout << "Calling CHE " << j <<  endl;
            if(checkHiddenEdge(this->edges[i],faces[j],projectionPlane,i))
            {
                flag = 1;
                break;
            }
        }
        isHiddenEdge.push_back(flag);
        // cout << i << " " << edges[i] << ":" << flag << isHiddenEdge[i] << isHiddenEdge.size() << endl;
    }
    cout << "Edges Done" << endl;
    // The vectors isHidden and isHiddenEdge store whether or not a point/edge is hidden.
    // This can be used to generate an OrthoProjection object that can be returned from this function.
    PlaneProjection* projection = new PlaneProjection;
    for (int i = 0; i < 4; i++) {
        projection->normal[i] = projectionPlane[i];
    }
    for (auto i = 0; i < projectedVertices.size(); i++) {
        projection->vertices.push_back(projectedVertices[i]);
    }
    for (auto i = 0; i < this->edges.size(); i++) {
        Edge edge;
        edge.p1 = this->edges[i].p1.projectPoint(projectionPlane);
        edge.p2 = this->edges[i].p2.projectPoint(projectionPlane);
        if(isHiddenEdge[i])
            projection->hiddenEdges.push_back(edge);
        else
            projection->visibleEdges.push_back(edge);
    }
    cout << "project3D done" << endl;
    return projection;
}

int countIntersections(Vector3d avertice,Vector3d bvertice,Vector3d dvertice,Vector3d evertice,Vector3d linevector){
    ///
    /// Function to count intersections of the line originating from avertice in the direction of linevector,
    /// with the edges of a polygon, formed using bvertice, dvertice, evertice
    ///
    int retValue = 0;
    Vector3d zerovector(0,0,0);
    Vector3d abvector = bvertice - avertice;
    Vector3d acvector = linevector;
    Vector3d advector = dvertice - avertice;
    Vector3d aevector = evertice - avertice;
    Vector3d t1 = abvector.cross(acvector);
    Vector3d t2 = advector.cross(acvector);
    Vector3d t3 = abvector.cross(advector);
    Vector3d t6 = aevector.cross(acvector);
    double t4 = t1.dot(t2);
    double t5 = t1.dot(t3);
    double t7 = t6.dot(t2);
    double abdotac = abvector.dot(acvector);
    if(t1==zerovector || t2==zerovector){
        // Do Nothing
    }else if((t4<=0) && (t5>=0)){
        retValue = 1;
    }
    if(t1==zerovector && abdotac>0){
        if(t7<=0)
            retValue = 2;
        else
            retValue = 3;
    }
    return retValue;
}

Vector3d intersectLines(Point a,Point b,Vector3d c,Vector3d d) {
    Vector3d retVal;
    Vector3d av(a.x,a.y,a.z);
    Vector3d bv(b.x,b.y,b.z);
    Vector3d f = bv-av;
    Vector3d e = d-c;
    Vector3d g = c-av;
    Vector3d fcrossg = f.cross(g);
    Vector3d fcrosse = f.cross(e);
    double h = abs(fcrossg.norm());
    double k = abs(fcrosse.norm());
    if(fcrossg.dot(fcrosse)>0){
        retVal = c + (h/k)*e;
    }else{
        retVal = c - (h/k)*e;
    }
    return retVal;
}

bool Object3D::rayCasting(Point point, vector<Point> polygon) {
    ///
    /// Function that returns if the Point "point" lies within the polygon formed by the Face "polygon", passed as parameter.
    ///
    int numverticesinpolygon = polygon.size();
    // cout<< "num of points in polygon is "<< numverticesinpolygon<<endl;
    int flag = 0;
    for(int i = 0; i < numverticesinpolygon; i++) {
        if(point.x==polygon[i].x && point.y==polygon[i].y  && point.z==polygon[i].z ){
            flag = 1;
            break;
        }
    }
    if(flag==1)
        return true;
    Vector3d avertice;
    avertice << point.x, point.y, point.z;
    int numintersections = 0;
    Vector3d zerovector(0,0,0);
    Vector3d icap(1,0,0);
    Vector3d jcap(0,1,0);
    Vector3d startone;
    startone << polygon[0].x, polygon[0].y, polygon[0].z;
    Vector3d secondone;
    secondone << polygon[1].x, polygon[1].y, polygon[1].z;
    Vector3d thirdone;
    thirdone << polygon[2].x, polygon[2].y, polygon[2].z;
    Vector3d firstvector = secondone-startone;
    Vector3d secondvector = thirdone-secondone;
    Vector3d perpendicular = firstvector.cross(secondvector);
    Vector3d linevector = perpendicular.cross(icap);

    double checkzero = linevector.dot(linevector);
    if(checkzero==0){
        linevector = perpendicular.cross(jcap);
    }
    // Vector3d linevector(1,1,0);
    int i=0;
    for(auto it= polygon.begin();it!=polygon.end();it++){
        Point thisone = *it;
        Point nextone,prevone;
        if(i==numverticesinpolygon-1){
            prevone = *(it-1);
            nextone = polygon.front();
        }else if(i==0){
            prevone = polygon.back();
            nextone = *(it+1);
        }else{
            prevone = *(it-1);
            nextone = *(it+1);
        }
        //Point nextone = *(it + 1);
        Vector3d bvertice;
        bvertice << thisone.x, thisone.y, thisone.z;
        Vector3d dvertice;
        dvertice << nextone.x,nextone.y, nextone.z;
        Vector3d evertice;
        evertice << prevone.x, prevone.y, prevone.z;
        int ci = countIntersections(avertice,bvertice,dvertice,evertice,linevector);
        if(ci==0){
            // Do Nothing
        }else if(ci==1 || ci==2){
            numintersections = numintersections +1;
        }else if(ci==3){
            numintersections = numintersections + 2;
        }
        i++;
    }
    // cout<< "numintersections is "<< numintersections<<endl;
    if(numintersections%2==1){
        return true;
    }else{
        return false;
    }
}

bool Object3D::checkHiddenVertice(Point vertex, Face face, double plane[4], int predicate) {
    ///
    /// Function to check if the Point passed as parameter "vertex" is hidden by the face,
    /// which is passed in the parameter "face", the projection being taken on the plane "plane"
    ///
    bool retValue = false;
    Vector3d point1(vertices[face.vertices[0]].x,vertices[face.vertices[0]].y,vertices[face.vertices[0]].z);
    Vector3d point2(vertices[face.vertices[1]].x,vertices[face.vertices[1]].y,vertices[face.vertices[1]].z);
    Vector3d point3(vertices[face.vertices[2]].x,vertices[face.vertices[2]].y,vertices[face.vertices[2]].z);
    Vector3d dir1 = (point3-point1);
    Vector3d dir2 = (point2-point1);
    Vector3d normal = dir1.cross(dir2);
    double facePlane[4] = {normal.x(),normal.y(),normal.z(),normal.dot(point1)};
    vector<Point> faceProject;
    for (int i = 0; i < face.vertices.size(); i++) {
        faceProject.push_back(projectedVertices[face.vertices[i]]);
    }
    // cout << "Face Projected" << endl;
    Point projectedVertex = vertex.projectPoint(plane);
    if(rayCasting(projectedVertex,faceProject)) {
        if(predicate==0) {
            if(vertex.relativePosition(facePlane)*projectedVertex.relativePosition(facePlane) >= 0)
                retValue = false;
            else
                retValue = true;
        }else{
            if(vertex.relativePosition(facePlane)*projectedVertex.relativePosition(facePlane) > 0)
                retValue = false;
            else
                retValue = true;
        }
    }
    // cout << "retvalue is " << retValue << endl;
    return retValue;
}

bool Object3D::checkHiddenEdge(Edge edge, Face face, double plane[4], int index) {
    ///
    /// Function to evaluate if the Edge "edge", passed as parameter is hidden by the face,
    /// whose projection on the plane "plane" is passed as argument "face"
    ///
    cout << "In CHE" << endl;
    bool retValue = false;
    Point p1 = edge.p1;
    Point p2 = edge.p2;
    auto it1 = find_if(this->vertices.begin(), this->vertices.end(),
        [p1](Point p) -> bool {return (p.x==p1.x && p.y==p1.y && p.z==p1.z);});
    auto it2 = find_if(this->vertices.begin(), this->vertices.end(),
        [p2](Point p) -> bool {return (p.x==p2.x && p.y==p2.y && p.z==p2.z);});
    int index1,index2;
    if(it1 != vertices.end())
        index1 = distance(vertices.begin(),it1);
    if(it2 != vertices.end())
        index2 = distance(vertices.begin(),it2);
    auto itin1 = find(face.vertices.begin(),face.vertices.end(),index1);
    auto itin2 = find(face.vertices.begin(),face.vertices.end(),index2);
    if (itin1 != face.vertices.end() && itin2 != face.vertices.end())
        return false;
    // If both endpoints are on the face, it isn't hidden.
    Point projectp1 = edge.p1.projectPoint(plane);
    Point projectp2 = edge.p2.projectPoint(plane);
    vector<Point> faceProject;
    for (int i = 0; i < face.vertices.size(); i++) {
        faceProject.push_back(projectedVertices[face.vertices[i]]);
    }
    if(rayCasting(projectp1,faceProject)) {
        if(checkHiddenVertice(edge.p1,face,plane,1)) {
            // Evaluate the point of intersection of edge with the polygon faceProject
            // cout << "First point is hidden."; // -------Remove
            Vector3d avertice(projectp1.x,projectp1.y,projectp1.z);
            Vector3d endvertice(projectp2.x,projectp2.y,projectp2.z);
            Vector3d linevector(projectp2.x-projectp1.x,projectp2.y-projectp1.y,projectp2.z-projectp1.z);
            vector<Vector3d> intersections;
            vector<Point> polygon;
            for(int i = 0; i < face.vertices.size(); i++) {
                polygon.push_back(this->projectedVertices[face.vertices[i]]);
                // pointIndex.push_back(face.vertices[i]);
            }
            int i=0, numverticesinpolygon=polygon.size();
            for(auto it= polygon.begin();it!=polygon.end();it++){
                Point thisone = *it;
                Point nextone,prevone;
                if(i==numverticesinpolygon-1){
                    prevone = *(it-1);
                    nextone = polygon.front();
                }else if(i==0){
                    prevone = polygon.back();
                    nextone = *(it+1);
                }else{
                    prevone = *(it-1);
                    nextone = *(it+1);
                }
                //Point nextone = *(it + 1);
                Vector3d bvertice;
                bvertice << thisone.x, thisone.y, thisone.z;
                Vector3d dvertice;
                dvertice << nextone.x,nextone.y, nextone.z;
                Vector3d evertice;
                evertice << prevone.x, prevone.y, prevone.z;
                int ci = countIntersections(avertice,bvertice,dvertice,evertice,linevector);
                if(ci==2 || ci==3){
                    intersections.push_back(bvertice);
                }else if(ci==1){
                    // Intersects (bvertice,dvertice) in the middle somewhere
                    Vector3d intersection = intersectLines(edge.p1,edge.p2,bvertice,dvertice);
                    cout << "Between " << edge << " and " << bvertice << ", " << dvertice << endl; // ---------Remove
                    intersections.push_back(intersection);
                }
                i++;
            }
            // All intersections of the edge have been taken with the face, and are stored in intersections
            Vector3d closestIntersection;
            if(intersections.size()==1){
                closestIntersection = intersections[0];
            }else if(intersections.size()!=0){
                closestIntersection = *min_element(intersections.begin(),intersections.end(),
                [avertice] (Vector3d v1, Vector3d v2) -> bool {return (avertice-v1).norm()<(avertice-v2).norm();});
            }
            if(intersections.size()!=0) {
                // cout << "Intersection found."; -------Remove
                if(closestIntersection.x()==projectp2.x && closestIntersection.y()==projectp2.y && closestIntersection.z()==projectp2.z){
                    if(checkHiddenVertice(edge.p2,face,plane,1))
                        retValue = true;
                    else
                        retValue = false;
                }else{
                    // The vector closestIntersection holds the closest point of intersection, from p1
                    // This point is on the projected plane, and we must find the corresponding point in 3D
                    Vector3d thirdPoint;
                    double d1 = (closestIntersection-avertice).norm();
                    double d2 = (endvertice-closestIntersection).norm();
                    Vector3d p1(edge.p1.x,edge.p1.y,edge.p1.z);
                    Vector3d p2(edge.p2.x,edge.p2.y,edge.p2.z);
                    thirdPoint = (d1*p2 + d2*p1)/(d1+d2);
                    // cout << "Inserting new element " << thirdPoint << endl; --------Remove
                    Point newPoint,newPointProjected;
                    newPoint.setCoordinates(thirdPoint.x(),thirdPoint.y(),thirdPoint.z());
                    newPointProjected.setCoordinates(closestIntersection.x(),closestIntersection.y(),closestIntersection.z());
                    Edge segment;
                    segment.p1.setCoordinates(thirdPoint.x(),thirdPoint.y(),thirdPoint.z());
                    segment.p2.setCoordinates(edge.p2.x,edge.p2.y,edge.p2.z);
                    edge.p2.setCoordinates(thirdPoint.x(),thirdPoint.y(),thirdPoint.z());
                    this->edges.insert(this->edges.begin()+index+1,segment);
                    this->vertices.push_back(newPoint);
                    this->projectedVertices.push_back(newPointProjected);
                    retValue = true;
                }
            }else{
                retValue = checkHiddenVertice(edge.p2,face,plane,1);
            }
        }else{
            if(checkHiddenVertice(edge.p2,face,plane,0)) {
                retValue = true;
            }else{
                retValue = false;
            }
        }
    }
    // if(retValue)
    //     cout << edge << " is hidden by the face formed by: " << face << endl;
    // else
    //     cout << edge << " is not hidden by the face formed by: " << face << endl; // -------Remove
    return retValue;
}

Face Object3D::getCyclicFace(Face face) {
    ///
    /// Function that reeturns the face with the vertices, re-arranged so that a cyclic order is maintained
    ///
    vector<int> pointIndex;
    Face newFace;
    for(int i = 0; i < face.vertices.size(); i++) {
        // polygon.push_back(this->projectedVertices[face.vertices[i]]);
        pointIndex.push_back(face.vertices[i]);
    }
    int i=0, numverticesinpolygon=face.vertices.size();
    bool done = false;
    Vector3d edge1(vertices[face.vertices[1]].x-vertices[face.vertices[0]].x,vertices[face.vertices[1]].y-vertices[face.vertices[0]].y,vertices[face.vertices[1]].z-vertices[face.vertices[0]].z);
    Vector3d edge2(vertices[face.vertices[2]].x-vertices[face.vertices[1]].x,vertices[face.vertices[2]].y-vertices[face.vertices[1]].y,vertices[face.vertices[2]].z-vertices[face.vertices[1]].z);
    Vector3d normalToFace = edge1.cross(edge2);
    for(int i = 0; i < numverticesinpolygon; i++) {
        if(i==0){
            newFace.vertices.push_back(pointIndex[0]);
        }else if(i==1){
            int index = newFace.vertices[0];
            Point p = this->vertices[index];
            auto it = find_if(this->edges.begin(), this->edges.end(),
                [p,normalToFace](Edge e) -> bool {
                    bool retVal = false;
                    if ((p.x==e.p1.x && p.y==e.p1.y && p.z==e.p1.z) || (p.x==e.p2.x && p.y==e.p2.y && p.z==e.p2.z)){
                        Vector3d edge(e.p1.x-e.p2.x,e.p1.y-e.p2.y,e.p1.z-e.p2.z);
                        if(edge.dot(normalToFace)==0){
                            retVal = true;
                        }
                    }
                    return retVal;});
            Point neighbor;
            Edge e = *it;
            if(p.x==e.p1.x && p.y==e.p1.y && p.z==e.p1.z)
                neighbor = e.p2;
            else
                neighbor = e.p1;
            auto it1 = find_if(this->vertices.begin(), this->vertices.end(),
                [neighbor](Point p) -> bool {return (p.x==neighbor.x && p.y==neighbor.y && p.z==neighbor.z);});
            int index1;
            if(it1 != vertices.end())
                index1 = distance(vertices.begin(),it1);
            newFace.vertices.push_back(index1);
        }else{
            int index = newFace.vertices[i-1];
            int prevIndex = newFace.vertices[i-2];
            Point p = this->vertices[index];
            Point prevp = this->vertices[prevIndex];
            auto it = find_if(this->edges.begin(), this->edges.end(),
                [p,prevp,normalToFace](Edge e) -> bool {
                    bool retVal = false;
                    if ((p.x==e.p1.x && p.y==e.p1.y && p.z==e.p1.z) || (p.x==e.p2.x && p.y==e.p2.y && p.z==e.p2.z)){
                        Vector3d edge(e.p1.x-e.p2.x,e.p1.y-e.p2.y,e.p1.z-e.p2.z);
                        if(edge.dot(normalToFace)==0){
                            if((p.x==e.p1.x && p.y==e.p1.y && p.z==e.p1.z) && !(prevp.x==e.p2.x && prevp.y==e.p2.y && prevp.z==e.p2.z)){
                                retVal = true;
                            }else if((p.x==e.p2.x && p.y==e.p2.y && p.z==e.p2.z) && !(prevp.x==e.p1.x && prevp.y==e.p1.y && prevp.z==e.p1.z)){
                                retVal = true;
                            }
                        }
                    }
                    return retVal;});
            Point neighbor;
            Edge e = *it;
            if(p.x==e.p1.x && p.y==e.p1.y && p.z==e.p1.z)
                neighbor = e.p2;
            else
                neighbor = e.p1;
            auto it1 = find_if(this->vertices.begin(), this->vertices.end(),
                [neighbor](Point p) -> bool {return (p.x==neighbor.x && p.y==neighbor.y && p.z==neighbor.z);});
            int index1;
            if(it1 != vertices.end())
                index1 = distance(vertices.begin(),it1);
            newFace.vertices.push_back(index1);
        }
    }
    return newFace;
}

bool Object3D::check3dobject(){
    ///
    /// Function to check the validity of input file, by rendering the Object in OpenScad
    ///
    try{
        ofstream scadfile ("object.scad");
        if(scadfile.is_open()){
            // scadfile<< "cube([2,3,9]);";
            scadfile <<"ObjectPoints = [\n";
            int numpoints = vertices.size();
            for(int i=0;i<numpoints;i++){
                Point temp = vertices[i];
                scadfile<<"    [ "<<temp.x<<", "<<temp.y<<", "<<temp.z<<" ]";
                if(i!=numpoints-1){
                    scadfile<<",";
                }else{
                    scadfile<<"];";
                }
                scadfile<<  "  //"<<i<<"\n";
            }
            scadfile <<"\n";
            scadfile << "ObjectFaces = [\n";
            int numfaces = faces.size();
            for(int i=0;i<numfaces;i++){
                scadfile<<"[";
                for(int j=0;j<faces[i].vertices.size();j++){
                    scadfile<<faces[i].vertices[j];
                    if(j!=faces[i].vertices.size()-1){
                        scadfile<<",";
                    }
                }
                scadfile<< "]";
                if(i!=numfaces-1){
                    scadfile<<",\n";
                }else{
                    scadfile<<"];\n";
                }
            }
            scadfile<< "polyhedron( ObjectPoints, ObjectFaces );";
            scadfile.close();
            system("openscad -o object.stl object.scad");
            ofstream renderfile ("render.scad");
            if(renderfile.is_open()){
                renderfile<< "render(){import(\"object.stl\");}";
                renderfile.close();
                system("openscad render.scad");
            }else{
                cout<< "unable to render the stl";
            }
        }else{
            cout<< "unable to open file"<<endl;
        }
        return true;
    }catch(...){
        return false;
    }
}