#include "Quaca.h"
#include "catch.hpp"
#include <armadillo>
#include <complex>

TEST_CASE("Vacuum Green's tensor constructors work as expected",
          "[GreensTensorVacuum]") {

  SECTION("Direct constructor") {
    auto v = GENERATE(3.2e-3,1.2e-1);
    auto beta = GENERATE(3.65,100.34);
    auto relerr = GENERATE(1e-8,1e-7);

    GreensTensorVacuum Greens(v, beta, relerr);

    REQUIRE(Greens.get_v() == v);
    REQUIRE(Greens.get_beta() == beta);
    REQUIRE(Greens.get_relerr() == relerr);
  }

  SECTION("json file constructor") {
    double v = 0.1;
    double beta = 5;
    double relerr = 1E-9;

    GreensTensorVacuum Greens("../data/test_files/GreensTensorVacuum.json");

    REQUIRE(Approx(Greens.get_v()).epsilon(1E-6) == v);
    REQUIRE(Approx(Greens.get_beta()).epsilon(1E-6) == beta);
    REQUIRE(Greens.get_relerr() == relerr);
  }
}

TEST_CASE("Integrand 1d k is correctly implemented", "[GreensTensorVacuum]") {
  // Generate a Green's tensor with random attributes v and beta
  auto v = GENERATE(1e-4,1e-8);
  auto beta = GENERATE(0.21,1.32,5.23);
  double relerr = 1E-9;
  GreensTensorVacuum Greens(v, beta, relerr);

  // Create the variables for the num_result, taking care, that
  //\omega^2 - k^2 >= 0 to stay in the non-trivial regime
  auto omega = GENERATE(-7.43,0.21,1.76);
  auto k_v = GENERATE(-.9,.1,.8);
  if (k_v < 0)
    k_v *= omega / (1 + v);
  if (k_v >= 0)
    k_v *= omega / (1 - v);

  // Check the integrand for all possible integration options
  double omega_kv = omega + v * k_v;
  double xi = pow(omega_kv, 2) - pow(k_v, 2);

  cx_mat::fixed<3,3> LHS(fill::zeros);
  cx_mat::fixed<3,3> RHS(fill::zeros);

  //Set the values of the analytical result
  LHS(0,0) = .5 * xi;
  LHS(1,1) = .5 * (pow(omega_kv, 2) - .5 * xi);
  LHS(2,2) = LHS(1,1);

  SECTION("Option: IM") {
    double factor = 1.;
    for(size_t i = 0; i < 3; ++i) {
      for(size_t j = 0; j < 3; ++j) {
	RHS(i,j) = Greens.integrand_k(k_v, omega, {i, j}, IM, UNIT);
      }
    }
    //Ensure that result is non-trivial
    REQUIRE(!RHS.is_zero());

    REQUIRE(approx_equal(LHS,RHS,"reldiff",1e-12));
  }

  SECTION("Option: IM, KV") {
    double factor = k_v;
    for(size_t i = 0; i < 3; ++i) {
      for(size_t j = 0; j < 3; ++j) {
	RHS(i,j) = Greens.integrand_k(k_v, omega, {i, j}, IM, KV);
      }
    }
    //Ensure that result is non-trivial
    REQUIRE(!RHS.is_zero());

    REQUIRE(approx_equal(factor*LHS,RHS,"reldiff",1e-12));
  };

  SECTION("Option: IM, TEMP") {
    double factor = 1. / (1. - exp(-beta * omega_kv));
    for(size_t i = 0; i < 3; ++i) {
      for(size_t j = 0; j < 3; ++j) {
	RHS(i,j) = Greens.integrand_k(k_v, omega, {i,j}, IM, TEMP);
      }
    }
    //Ensure that result is non-trivial
    REQUIRE(!RHS.is_zero());

    REQUIRE(approx_equal(factor*LHS,RHS,"reldiff",1e-12));
  };

  SECTION("Option: IM, KV_TEMP") {
    double factor = k_v / (1. - exp(-beta * omega_kv));
    for(size_t i = 0; i < 3; ++i) {
      for(size_t j = 0; j < 3; ++j) {
	RHS(i,j) = Greens.integrand_k(k_v, omega, {i,j}, IM, KV_TEMP);
      }
    }
    //Ensure that result is non-trivial
    REQUIRE(!RHS.is_zero());

    REQUIRE(approx_equal(factor*LHS,RHS,"reldiff",1e-12));
  };

  SECTION("Option: IM, NON_LTE") {
    double factor =
        1. / (1. - exp(-beta * (omega_kv))) - 1. / (1. - exp(-beta * omega));
    for(size_t i = 0; i < 3; ++i) {
      for(size_t j = 0; j < 3; ++j) {
	RHS(i,j) = Greens.integrand_k(k_v, omega, {i,j}, IM, NON_LTE);
      }
    }

    //Ensure non-trivial result
    REQUIRE(!RHS.is_zero());

    REQUIRE(approx_equal(factor*LHS,RHS,"reldiff",1e-12));
  };
};

/*!
 * Some basic relations any Green's tensor should fulfill which can
 * be found in docs under: Relations_and_num_results.pdf
 */
TEST_CASE("Crossing relation in frequency domain see eq. [1]",
          "[GreensTensorVacuum]") {
  // Generate a Green's tensor with random attributes v and beta
  auto v = GENERATE(1e-4,1e-8);
  auto beta = GENERATE(0.01,10.);
  double relerr = 1E-9;

  // Create the variables for the num_result, taking care, that
  //\omega^2 - k^2 >= 0 to stay in the non-trivial regime
  auto k_x = GENERATE(-12.42,0.124,76.543);
  auto k_y = GENERATE(-6.543,-1.43,34.123);
  auto omega = GENERATE(1.1, 5.3,10.2);
  double k = sqrt(k_x * k_x + k_y * k_y);
  omega *= k;

  GreensTensorVacuum Greens(v, beta, relerr);

  cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
  cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

  Greens.calculate_tensor(omega, {k_x, k_y}, Greens_lhs);
  Greens.calculate_tensor(-omega, {-k_x, -k_y}, Greens_rhs);

  //Ensure non-trivial result
  REQUIRE(!Greens_lhs.is_zero());
  REQUIRE(!Greens_rhs.is_zero());

  REQUIRE(approx_equal(Greens_lhs, trans(conj(Greens_rhs)), "reldiff", 10E-5));
}

TEST_CASE("Reciprocity, see eq. [6]", "[GreensTensorVacuum]") {
  // Generate a Green's tensor with random attributes v and beta
  auto v = GENERATE(1e-4,1e-8);
  auto beta = GENERATE(0.01,10.);
  double relerr = 1E-9;

  auto k_x = GENERATE(-12.42,0.124,76.543);
  auto k_y = GENERATE(-6.543,-1.43,34.123);
  auto omega = GENERATE(1.1, 5.3,10.2);

  // Take care that we are looking at the non trivial part of the Green's tensor
  // where \omega^2 - k^2 >= 0
  double k = sqrt(k_x * k_x + k_y * k_y);
  omega *= k;

  GreensTensorVacuum Greens(v, beta, relerr);

  // Create the matries storing the Green's tensors
  cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
  cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

  Greens.calculate_tensor(omega, {k_x, k_y}, Greens_lhs);
  Greens.calculate_tensor(omega, {-k_x, -k_y}, Greens_rhs);

  //Ensure non-trivial result
  REQUIRE(!Greens_lhs.is_zero());
  REQUIRE(!Greens_rhs.is_zero());

  REQUIRE(approx_equal(Greens_lhs, trans(Greens_rhs), "reldiff", 10E-5));
}

TEST_CASE("Reality, see eq. [7]", "[GreensTensorVacuum]") {
  // Generate a Green's tensor with random attributes v and beta
  auto v = GENERATE(1e-4,1e-8);
  auto beta = GENERATE(0.01,10.);
  auto omega = GENERATE(1.32,6.34,10.32,54.21);
  double relerr = 1E-9;

  GreensTensorVacuum Greens(v, beta, relerr);

  cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
  cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

  Greens.integrate_k(omega, Greens_lhs, IM, UNIT);
  Greens.integrate_k(-omega, Greens_rhs, IM, UNIT);

  //Ensure non-trivial result
  REQUIRE(!Greens_lhs.is_zero());
  REQUIRE(!Greens_rhs.is_zero());

  REQUIRE(approx_equal(Greens_lhs, -Greens_rhs, "reldiff", 10E-5));
}

TEST_CASE("Test the integration routine", "[GreensTensorVacuum]") {

  SECTION("Option: IM") {
    // Generate a Green's tensor with random attributes v and beta
    auto v = GENERATE(1e-4);
    auto beta = GENERATE(1e-3);
    auto omega = GENERATE(1.32);
    double relerr = 1E-9;
    GreensTensorVacuum Greens(v, beta, relerr);

    // Matrix to store the analytic results
    cx_mat::fixed<3, 3> ana_result(fill::zeros);
    // Computing the analytical result and storing it in analytic
    double ana_pref = 2. / 3. * pow(omega, 3) / pow(1 - pow(v, 2), 2);
    ana_result(0, 0) = ana_pref;
    ana_result(1, 1) = ana_pref * (1 + pow(v, 2)) / (1 - pow(v, 2));
    ana_result(2, 2) = ana_pref * (1 + pow(v, 2)) / (1 - pow(v, 2));

    // Matrix storing the numerical integration
    cx_mat::fixed<3, 3> num_result(fill::zeros);
    Greens.integrate_k(omega, num_result, IM, UNIT);

    //Ensure non-trivial results
    REQUIRE(!num_result.is_zero());
    REQUIRE(!ana_result.is_zero());

    REQUIRE(approx_equal(num_result, ana_result, "reldiff", 10E-5));
  }

  SECTION("Option: IM, KV") {

    // Generate a Green's tensor with random attributes v and beta
    auto v = GENERATE(1e-4);
    auto beta = GENERATE(1e-3);
    auto omega = GENERATE(1.32);
    double relerr = 1E-9;
    GreensTensorVacuum Greens(v, beta, relerr);

    // Matrix to store the analytic results
    cx_mat::fixed<3, 3> ana_result(fill::zeros);
    // Computing the analytical result and storing it in analytic
    double ana_pref = 2. / 3. * pow(omega, 4) * v / pow(1 - pow(v, 2), 3);
    ana_result(0.0) = ana_pref;
    ana_result(1, 1) = ana_pref * (2. + pow(v, 2)) / (1. - pow(v, 2));
    ana_result(2, 2) = ana_result(1, 1);

    // Matrix storing the numerical integration
    cx_mat::fixed<3, 3> num_result(fill::zeros);
    Greens.integrate_k(omega, num_result, IM, KV);

    //Ensure non-trivial results
    REQUIRE(!num_result.is_zero());
    REQUIRE(!ana_result.is_zero());

    REQUIRE(approx_equal(num_result, ana_result, "reldiff", 10E-5));
  }

  SECTION("Option: IM, TEMP") {
    // Generate a Green's tensor with random attributes v and beta
    auto v = GENERATE(1e-4);
    auto beta = GENERATE(1e-3);
    auto omega = GENERATE(1.32);
    beta *= fabs(omega);
    double relerr = 1E-9;
    GreensTensorVacuum Greens(v, beta, relerr);

    // Matrix to store the analytic results
    cx_mat::fixed<3, 3> ana_result(fill::zeros);
    // Computing the analytical result and storing it in analytic
    double ana_pref = pow(omega, 2) / (2. * pow(v, 3) * beta);
    ana_result(0.0) = ana_pref * (2. * v / (1. - pow(v, 2)) - 2. * atanh(v));
    ana_result(1, 1) =
        ana_pref * ((3. * pow(v, 3) - v) / pow(1 - pow(v, 2), 2) + atanh(v));
    ana_result(2, 2) = ana_result(1, 1);

    // Matrix storing the numerical integration
    cx_mat::fixed<3, 3> num_result(fill::zeros);
    Greens.integrate_k(omega, num_result, IM, TEMP);

    //Ensure non-trivial results
    REQUIRE(!num_result.is_zero());
    REQUIRE(!ana_result.is_zero());

    REQUIRE(approx_equal(num_result, ana_result, "reldiff", 10E-4));
  }

  SECTION("Option: IM, KV_TEMP") {
    // Generate a Green's tensor with random attributes v and beta
    auto v = GENERATE(1e-2);
    auto beta = GENERATE(1e-12);
    auto omega = GENERATE(.54);
    double relerr = 1E-9;
    GreensTensorVacuum Greens(v, beta, relerr);

    // Matrix to store the analytic results
    cx_mat::fixed<3, 3> ana_result(fill::zeros);
    // Computing the analytical result and storing it in analytic
    double ana_pref = std::pow(omega, 3) / (6. * std::pow(v, 4) * beta);
    ana_result(0, 0) =
        ana_pref * (2 * (5 * pow(v, 3) - 3. * v) / pow(1. - pow(v, 2), 2) +
                    6. * std::atanh(v));
    ana_result(1, 1) = ana_pref * ((8. * pow(v, 3) - 3. * v - 13. * pow(v, 5)) /
                                       pow(pow(v, 2) - 1, 3) -
                                   3. * std::atanh(v));
    ana_result(2, 2) = ana_result(1, 1);

    // Matrix storing the numerical integration
    cx_mat::fixed<3, 3> num_result(fill::zeros);
    Greens.integrate_k(omega, num_result, IM, KV_TEMP);

    //Ensure non-trivial results
    REQUIRE(!num_result.is_zero());
    REQUIRE(!ana_result.is_zero());

    REQUIRE(approx_equal(num_result, ana_result, "reldiff", 10E-5));
  }
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
int
main()
{
  using namespace boost;
  GraphvizGraph g_dot;
  read_graphviz("figs/telephone-network.dot", g_dot);

  typedef adjacency_list < vecS, vecS, undirectedS, no_property,
    property < edge_weight_t, int > > Graph;
  Graph g(num_vertices(g_dot));
  property_map < GraphvizGraph, edge_attribute_t >::type
    edge_attr_map = get(edge_attribute, g_dot);
  graph_traits < GraphvizGraph >::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g_dot); ei != ei_end; ++ei) {
    int weight = lexical_cast < int >(edge_attr_map[*ei]["label"]);
    property < edge_weight_t, int >edge_property(weight);
    add_edge(source(*ei, g_dot), target(*ei, g_dot), edge_property, g);
  }

  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  std::vector < Vertex > parent(num_vertices(g));
  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  property_map<Graph, vertex_index_t>::type indexmap = get(vertex_index, g);
  std::vector<std::size_t> distance(num_vertices(g));
  prim_minimum_spanning_tree(g, *vertices(g).first, &parent[0], &distance[0],
                             weight, indexmap, default_dijkstra_visitor());
#else
  prim_minimum_spanning_tree(g, &parent[0]);
#endif

  int total_weight = 0;
  for (int v = 0; v < num_vertices(g); ++v)
    if (parent[v] != v)
      total_weight += get(weight, edge(parent[v], v, g).first);
  std::cout << "total weight: " << total_weight << std::endl;

  for (int u = 0; u < num_vertices(g); ++u)
    if (parent[u] != u)
      edge_attr_map[edge(parent[u], u, g_dot).first]["color"] = "black";
  std::ofstream out("figs/telephone-mst-prim.dot");
  graph_property < GraphvizGraph, graph_edge_attribute_t >::type &
    graph_edge_attr_map = get_property(g_dot, graph_edge_attribute);
  graph_edge_attr_map["color"] = "gray";
  write_graphviz(out, g_dot);

  return EXIT_SUCCESS;
}


#include "NumCpp.hpp"
#include <Eigen/Dense>

#include <iostream>

typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> EigenIntMatrix;
typedef Eigen::Map<EigenIntMatrix> EigenIntMatrixMap;

int main()
{
    // construct some NumCpp arrays
    auto ncA = nc::random::randInt<int>({ 5, 5 }, 0, 10);
    auto ncB = nc::random::randInt<int>({ 5, 5 }, 0, 10);

    std::cout << "ncA:\n" << ncA << std::endl;
    std::cout << "ncB:\n" << ncB << std::endl;

    // map the arrays to Eigen
    auto eigenA = EigenIntMatrixMap(ncA.data(), ncA.numRows(), ncA.numCols());
    auto eigenB = EigenIntMatrixMap(ncB.data(), ncB.numRows(), ncB.numCols());

    // add the two Eigen matrices
    auto eigenC = eigenA + eigenB;

    // add the two NumCpp arrays for a sanity check
    auto ncC = ncA + ncB;

    // convert the Eigen result back to NumCpp
    int* dataPtr = new int[eigenC.rows() * eigenC.cols()];
    EigenIntMatrixMap(dataPtr, eigenC.rows(), eigenC.cols()) = eigenC;

    constexpr bool takeOwnership = true;
    auto ncCeigen = nc::NdArray<int>(dataPtr, eigenC.rows(), eigenC.cols(), takeOwnership);

    // compare the two outputs
    if (nc::array_equal(ncC, ncCeigen))
    {
        std::cout << "Arrays are equal." << std::endl;
        std::cout << ncC << std::endl;
    }
    else
    {
        std::cout << "Arrays are not equal." << std::endl;
        std::cout << "ncCeigen:\n" << ncCeigen << std::endl;
        std::cout << "ncC:\n" << ncC << std::endl;
    }

    return 0;
}


#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 */


//
// Test module for ntl_wrapper.cpp
//
#include <cstdlib>
#include <string>
#include <fstream>
#include <stdexcept>

#define BOOST_TEST_MODULE ntl_wrapper
#include <boost/test/included/unit_test.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <NTL/matrix.h>
#include <NTL/ZZ.h>

#include "types.h"

/* module being tested */
#include "ntl_wrapper.h"

using std::ifstream;
using std::string;
using std::runtime_error;

using boost::numeric::ublas::matrix;
using NTL::ZZ;
using NTL::Mat;

static const string LATTICE_95 = "../test/data/lattice_95.tst";

// rank of the 64-dim'l lattice after LLL is still 64
BOOST_AUTO_TEST_CASE( test_rk_LLL_lattice_95 ) {
    Mat<ZZ> N = read_lattice_ntl(LATTICE_95);
    ZZ det2;
    BOOST_CHECK( NTL::LLL(det2, N) == 64 );
}

// rank of half the 64-dim'l lattice is 32
BOOST_AUTO_TEST_CASE( test_rk_LLL_sublattice_95 ) {
    Mat<ZZ> N = read_lattice_ntl(LATTICE_95);
    Mat<ZZ> SN = take_cols(N, 32);
    ZZ det2;
    BOOST_CHECK( NTL::LLL(det2, SN) == 32 );
}

long populate(size_t, size_t);
long populate(size_t i, size_t j) { return (long)(3*i + 2*j + 1); }

// Fill a 3x3 boost matrix, convert to NTL, then test elements
BOOST_AUTO_TEST_CASE( test_matrix_to_ntl ) {
    matrix<int64> M (3, 3);
    for (size_t i=0; i != M.size1(); i++) {
        for (size_t j=0; j != M.size2(); j++) {
            M(i, j) = populate(i, j);
        }
    }
    Mat<ZZ> N;
    matrix_to_ntl(M, N);
    for (size_t i=0; i != (size_t)N.NumRows(); i++) {
        for (size_t j=0; j != (size_t)N.NumCols(); j++) {
            BOOST_CHECK( to_long(N[i][j]) == populate(i, j) );
        }
    }
}


/*=============================================================================
    Copyright (c) 2001-2010 Hartmut Kaiser
    http://spirit.sourceforge.net/

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
//[porting_guide_qi_includes
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <iostream>
#include <string>
#include <algorithm>
//]

//[porting_guide_qi_namespace
using namespace boost::spirit;
//]

//[porting_guide_qi_grammar
template <typename Iterator>
struct roman : qi::grammar<Iterator, unsigned()>
{
    roman() : roman::base_type(first)
    {
        hundreds.add
            ("C"  , 100)("CC"  , 200)("CCC"  , 300)("CD" , 400)("D" , 500)
            ("DC" , 600)("DCC" , 700)("DCCC" , 800)("CM" , 900) ;

        tens.add
            ("X"  , 10)("XX"  , 20)("XXX"  , 30)("XL" , 40)("L" , 50)
            ("LX" , 60)("LXX" , 70)("LXXX" , 80)("XC" , 90) ;

        ones.add
            ("I"  , 1)("II"  , 2)("III"  , 3)("IV" , 4)("V" , 5)
            ("VI" , 6)("VII" , 7)("VIII" , 8)("IX" , 9) ;

        // qi::_val refers to the attribute of the rule on the left hand side
        first = eps          [qi::_val = 0]
            >>  (  +lit('M') [qi::_val += 1000]
                ||  hundreds [qi::_val += qi::_1]
                ||  tens     [qi::_val += qi::_1]
                ||  ones     [qi::_val += qi::_1]
                ) ;
    }

    qi::rule<Iterator, unsigned()> first;
    qi::symbols<char, unsigned> hundreds;
    qi::symbols<char, unsigned> tens;
    qi::symbols<char, unsigned> ones;
};
//]

int main()
{
    {
        //[porting_guide_qi_parse
        std::string input("1,1");
        std::string::iterator it = input.begin();
        bool result = qi::parse(it, input.end(), qi::int_);

        if (result)
            std::cout << "successful match!\n";

        if (it == input.end())
            std::cout << "full match!\n";
        else
            std::cout << "stopped at: " << std::string(it, input.end()) << "\n";

        // seldomly needed: use std::distance to calculate the length of the match
        std::cout << "matched length: " << std::distance(input.begin(), it) << "\n";
        //]
    }

    {
        //[porting_guide_qi_phrase_parse
        std::string input(" 1, 1");
        std::string::iterator it = input.begin();
        bool result = qi::phrase_parse(it, input.end(), qi::int_, ascii::space);

        if (result)
            std::cout << "successful match!\n";

        if (it == input.end())
            std::cout << "full match!\n";
        else
            std::cout << "stopped at: " << std::string(it, input.end()) << "\n";

        // seldomly needed: use std::distance to calculate the length of the match
        std::cout << "matched length: " << std::distance(input.begin(), it) << "\n";
        //]
    }

    {
        //[porting_guide_qi_use_grammar
        std::string input("MMIX");        // MMIX == 2009
        std::string::iterator it = input.begin();
        unsigned value = 0;
        roman<std::string::iterator> r;
        if (qi::parse(it, input.end(), r, value))
            std::cout << "successfully matched: " << value << "\n";
        //]
    }
    return 0;
}


/*
 * NOTE: This solution is based on https://www.reddit.com/r/adventofcode/comments/3xflz8/day_19_solutions/cy4etju/
 *
 * Conceptually, to get the minimum steps from molecule "e" to the medicine, we need to go backwards and
 * start with the medicine. The medicine is composed of output molecules, each for which there's an input
 * molecule. So we keep replacing each molecule in the medicine with its input until there's only molecule
 * "e" left.
 *
 * Looking at the input file, there are a couple observations we can make:
 *
 * a) There are only two kinds of replacements ("|" denotes multiple replacements for the same input):
 * 1. X => XX
 * 2. X => X Rn X Ar | X Rn X Y X Ar | X Rn X Y X Y X Ar
 *
 * b) Following observation a), Rn Y Ar is equivalent to ( , )
 * - X => X(X) | X(X,X) | X(X,X,X)
 *
 * c) When you have a molecule of type XX, that is, none of Rn, Y or Ar, you can apply the first production (see a)),
 * i.e. reverse the replacement like this:
 * - XX => X
 *
 * When you have a molecule of type X(X) | X(X,X) | X(X,X,X), you can apply the second production (see a)), i.e.
 * reverse the replacement like this:
 * - X(X) | X(X,X) | X(X,X,X) => X
 *
 * Applying a production counts as one step.
 *
 * d) Repeatedly applying XX => X until there's only one molecule left takes `count(X) - 1` steps
 * - ABCDE => XCDE => XDE => XE => X
 *
 * This example produces `count(`ABCDE`) - 1` = `5 - 1` = 4 steps.
 *
 * Applying X(X) => X is similar, but `()` must be taken into account, since it increases the count.
 * This is expressed by expanding the formula: `count(`X(X)`) - count(no. of parentheses) - 1` steps. Example:
 * - A(B(C(D(E)))) => A(B(C(X))) => A(B(X)) => A(X) => X
 *
 * count(`A(B(C(D(E))))`) = 13
 * count(`(((())))`) = 8
 *
 * Result: 13 - 8 - 1 = 4 steps
 *
 * Applying X(X,X) | X(X,X,X) => X adds another variable to the count formula, the comma `,`, representing molecule Y.
 * As you can observe, each comma adds two molecules `,X` to the output. Taking this into account, we can write
 * the final formula as follows:
 *
 * `count(`X(X,X)`) - count(parentheses) - 2*count(commas) - 1` steps.
 *
 * - X(X,X) => X is expressed as `6 - 2 - 2 - 1 = 1 step`
 * - X(X,X,X) => X is expressed as `8 - 2 - 4 - 1 = 1 step`
 *
 * Once those observations are understood, the implementation becomes rather simple:
 * count all the molecules in the medicine and subtract the ones that serve as `(,)`
 * using the formula as described above.
 */
#include <iostream>
#include <string_view>
#include <unordered_set>

#include <boost/range/adaptor/reversed.hpp>
#include <boost/range/irange.hpp>

#include "input.hpp"

constexpr auto NEWLINE = '\n';

constexpr auto parse_stats(std::string_view input) {

  auto num_lines = 0;

  for(auto c : input) {
    num_lines += (c == NEWLINE);
  }

  return ++num_lines;
}

constexpr auto NUM_LINES = parse_stats(puzzle_input);

using Molecules = std::unordered_set<std::string_view>;
using Machine = std::pair<std::string_view, Molecules>;

auto parse(std::string_view input) {

  auto molecules = std::unordered_set<std::string_view>{};

  constexpr auto SPACE = ' ';

  for(const auto i : boost::irange(NUM_LINES-2)) {

    const auto linepos = input.find(NEWLINE);

    auto line = input.substr(0, linepos);

    molecules.insert(line.substr(0, line.find(SPACE)));

    auto output = line.substr((line.rfind(SPACE) + 1), line.npos);

    auto outpos = decltype(output.size()){};

    while(outpos != output.npos) {
      const auto endpos = output.find_first_of("ABCDEFGHIJKLMNOPQRSTUVWXYZ", (outpos + 1));
      molecules.insert(output.substr(outpos, (endpos - outpos)));
      outpos = endpos;
    }

    input.remove_prefix(linepos + 1);
  }
  // only one newline remains, remove it, and the input is now equal to the medicine
  input.remove_prefix(1);

  return Machine{input, molecules};
}

auto min_steps(Machine machine) {

  auto& [medicine, molecules] = machine;

  int sum_molecules = 0,
      sum_rn_ar = 0,
      sum_y = 0;

  const auto medsize = medicine.size();

  const auto reversed_range = boost::irange(medsize) | boost::adaptors::reversed;

  // going backwards through the medicine allows us to count the molecules in a single pass
  for(const auto i : reversed_range) {

    const auto molecule = molecules.find(medicine.substr(i, (medsize - i)));

    if(molecule != molecules.end()) {

      ++sum_molecules;

      const auto value = *molecule;

      sum_rn_ar += (value == "Rn" || value == "Ar");
      sum_y += (value == "Y");

      medicine.remove_suffix(molecule->size());
    }
  }

  const auto result = (sum_molecules - sum_rn_ar - (2 * sum_y) - 1);

  return result;
}

auto solution(std::string_view input) {

  auto machine = parse(input);

  return min_steps(machine);
}

int main() {

	std::cout << solution(puzzle_input) << std::endl;

}


#include "alpha_shape_area.hpp"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>

#include "weight_data.hpp"
#include "weight_func_obj_min.hpp"
#include "create_alpha_shape.hpp"
#include "alpha_shape_weighted_area.hpp"

typedef float f_t;
typedef size_t i_t;

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

	feenableexcept(FE_INVALID | FE_OVERFLOW);

	if(argc != 3){
		std::cout << "usage: " << argv[0] << " <window size> <index/coord/radii file>" << std::endl;
		return 1;
	}
	i_t window_size = boost::lexical_cast<i_t>(argv[1]);
	f_t alpha = 0.0;
	const char* infilename = argv[2];

	std::vector<i_t> indices;
	std::vector<f_t> points, radii;
	i_t index;
	f_t x, y, z, r;
	std::ifstream fs;
	fs.open(infilename, std::ifstream::in);
	while(fs >> index >> x >> y >> z >> r){
		indices.push_back(index);
		points.push_back(x);
		points.push_back(y);
		points.push_back(z);
		radii.push_back(r);
	}
	size_t numPoints = radii.size();
	std::cout << "number of points: " << numPoints << std::endl;

	std::vector<f_t> areas(numPoints);

	if(0){
		busv::alpha_shape_area<f_t, i_t>(numPoints, alpha, &(points[0]), &(radii[0]), &(areas[0]));
	}
	else{
		std::vector<busv::weight_data<f_t, i_t> > wd(numPoints);
		for(i_t i = 0; i<numPoints; ++i){
			wd[i].index = indices[i];
		}
		busv::weight_func_obj_min<f_t, i_t> wfo(&(wd[0]), window_size);
		busv::AlphaShapeContainer<f_t, i_t> asc = busv::create_alpha_shape(numPoints, alpha,  &(points[0]), &(radii[0]));
		busv::alpha_shape_weighted_area<f_t, i_t>(asc, wfo, &(areas[0]));
	}

	for(i_t i = 0; i<numPoints; ++i){
		std::cout << i << "\t" << indices[i] << "\t" << areas[i] << std::endl;
	}
	std::cout << std::endl;

	return 0;
}


//! [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]


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

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/static_assert.hpp>
#include "test.hpp"

using namespace boost::multiprecision;

int main()
{
   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_single>::digits10, 6);
   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_single>::max_digits10, 9);

   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_double>::digits10, 15);
   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_double>::max_digits10, 17);

   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_double_extended>::digits10, 18);
   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_double_extended>::max_digits10, 21);

   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_quad>::digits10, 33);
   BOOST_CHECK_EQUAL(std::numeric_limits<cpp_bin_float_quad>::max_digits10, 36);

   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_single>::digits10 == 6);
   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_single>::max_digits10 == 9);

   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_double>::digits10 == 15);
   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_double>::max_digits10 == 17);

   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_double_extended>::digits10 == 18);
   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_double_extended>::max_digits10 == 21);

   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_quad>::digits10 == 33);
   BOOST_STATIC_ASSERT(std::numeric_limits<cpp_bin_float_quad>::max_digits10 == 36);

   return boost::report_errors();
}


#ifndef BOOSTGRAPH
#define BOOSTGRAPH
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edge_connectivity.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include <boost/graph/dominator_tree.hpp>

#include <iostream>

using namespace std;
using namespace boost;

typedef int v_index;
typedef long e_index;

// This struct is the output of the edge connectivity Boost algorithm.
typedef struct {
    v_index ec; // The edge connectivity
    vector<v_index> edges; // The edges in a minimum cut, stored as a list of
                       // nodes. For instance, if the minimum cut is
                       // {(1,2),(3,4)}, the output vector will be (1,2,3,4).
} result_ec;

// This struct is the output of the clustering coefficient Boost algorithm.
typedef struct {
    double average_clustering_coefficient; // The average clustering coefficient
    vector<double> clust_of_v;             // The clustering coefficient of each node.
} result_cc;

template <class OutEdgeListS, // How neighbors are stored
          class VertexListS,  // How vertices are stored
          class DirectedS,    // The kind of graph (undirectedS, directedS, or bidirectionalS)
          class EdgeListS>    // How the list of edges is stored
class BoostGraph
/*
 * This generic class wraps a Boost graph, in order to make it Cython-friendly.
 *
 * In particular, it allows to "keep together" the Boost graph and the vector
 * *vertices: these two variables are generic, and Cython is not able to deal
 * with them properly, since it does not support generic classes.
 *
 * Vertices are numbers from 0 to n-1, where n is the total number of vertices:
 * this class takes care of the relation between number i and the corresponding
 * Boost vertex descriptor (which might be any object, depending on the value of
 * VertexListS). In particular, (*vertices)[i] contains the Boost vertex
 * corresponding to number i, while to transform a Boost vertex v into a number
 * we use vertex properties, and the syntax is (*graph)[v].
*/
{
    typedef typename boost::adjacency_list<OutEdgeListS, VertexListS, DirectedS,
    property<vertex_index_t, v_index>, no_property, no_property, EdgeListS> adjacency_list;
    typedef typename boost::graph_traits<adjacency_list>::vertex_descriptor vertex_descriptor;
    typedef typename boost::graph_traits<adjacency_list>::edge_descriptor edge_descriptor;
    typedef typename std::vector<edge_descriptor> edge_container;
    typedef typename boost::property_map<adjacency_list, boost::vertex_index_t>::type vertex_to_int_map;

public:
    adjacency_list *graph;
    vector<vertex_descriptor> *vertices;
    vertex_to_int_map index;

    BoostGraph() {
        graph = new adjacency_list();
        vertices = new vector<vertex_descriptor>();
    }

    ~BoostGraph() {
        delete graph;
        delete vertices;
    }

    v_index num_verts() {
        return num_vertices(*graph);
    }

    e_index num_edges() {
        return boost::num_edges(*graph);
    }

    void add_vertex() {
        (*vertices).push_back(boost::add_vertex((*vertices).size(), *graph));
    }

    void add_edge(v_index u, v_index v) {
        boost::add_edge((*vertices)[u], (*vertices)[v], *graph);
    }

    result_ec edge_connectivity() {
        result_ec to_return;
        edge_container disconnecting_set;
        back_insert_iterator<edge_container> inserter(disconnecting_set);
        to_return.ec = boost::edge_connectivity(*graph, inserter);

        for (v_index i = 0; i < disconnecting_set.size(); i++) {
            edge_descriptor edge = disconnecting_set[i];
            to_return.edges.push_back(index[boost::source(edge, *graph)]);
            to_return.edges.push_back(index[boost::target(edge, *graph)]);
        }
        return to_return;
    }

    double clustering_coeff(v_index v) {
        return clustering_coefficient(*graph, (*vertices)[v]);
    }

    result_cc clustering_coeff_all() {
        result_cc to_return;
        to_return.clust_of_v.resize(num_verts());
        to_return.average_clustering_coefficient = all_clustering_coefficients(*graph,
                        make_iterator_property_map(to_return.clust_of_v.begin(), index));
        return to_return;
    }

    vector<v_index> dominator_tree(v_index v) {
        vector<v_index> fathers(num_verts());
        vector<vertex_descriptor> fathers_descr(num_verts(),
                    boost::graph_traits<adjacency_list>::null_vertex());

        lengauer_tarjan_dominator_tree(*graph, (*vertices)[v],
                                       make_iterator_property_map(
                                           fathers_descr.begin(), index));

        for (v_index i = 0; i < num_verts(); i++) {
            vertex_descriptor v = fathers_descr[i];
            if (v == boost::graph_traits<adjacency_list>::null_vertex()) {
                fathers[i] = -1;
            } else {
                fathers[i] = index[v];
            }
        }
        return fathers;
    }
};

#endif // BOOSTGRAPH


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "BlitzArraySliceIterator.h"

#include "Mode_.h"

#include "LazyDensityOperator.tcc"
#include "StateVector.h"

#include <boost/bind.hpp>

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

using mathutils::fcmp; using mathutils::sqr; using mathutils::sqrAbs;

using quantumdata::partialTrace;

const double eps=1e-12;

const tmptools::Vector<0> v0=tmptools::Vector<0>();
const tmptools::Vector<1> v1=tmptools::Vector<1>();

const dcomp alpha(2,3), beta(-1,2), gammaa(2,-2);

const mode::StateVector c23(mode::coherent(alpha,78)), c12(mode::coherent(beta,30)), c22(mode::coherent(gammaa,48)),
  d23(mode::coherent(alpha,78)), d12(mode::coherent(beta,78)), d22(mode::coherent(gammaa,78));

boost::function<double(const mode::LazyDensityOperator&)> photonNumber(static_cast<double(*)(const mode::LazyDensityOperator&)>(mode::photonNumber));

template<int I>
double
photonNumberRecurse(const quantumdata::LazyDensityOperator<2>& m)
{
  return partialTrace(m,photonNumber,tmptools::Vector<I>(),0.);
}

const mode::DensityOperatorLow
densityOperator(const mode::LazyDensityOperator& m)
{
  size_t dim=m.getDimension();
  mode::DensityOperatorLow res(dim,dim);
  for (int i=0; i<dim; i++) for (int j=0; j<dim; j++) res(i,j)=m(i,j);
  return res;
}

BOOST_AUTO_TEST_CASE( RANK_TWO )
{
  quantumdata::StateVector<2> psi(c23*c12);

  BOOST_CHECK(!fcmp(photonNumberRecurse<0>(psi),13.,eps));
  BOOST_CHECK(!fcmp(photonNumberRecurse<1>(psi), 5.,eps));

  quantumdata::DensityOperator<2> rho(psi);

  BOOST_CHECK(!fcmp(photonNumberRecurse<0>(rho),13.,eps));
  BOOST_CHECK(!fcmp(photonNumberRecurse<1>(rho), 5.,eps));

  dcomp alpha1(alpha), beta1(beta), alpha2(beta), beta2(gammaa);

  quantumdata::StateVector<2> psii(d23*d12-d12*d22); double norm=psii.renorm();

  BOOST_CHECK(!fcmp((sqrAbs(beta1)+sqrAbs(beta2)-2.*std::real(braket(d23,d12)*braket(d12,d22)*conj(beta1)*beta2))/sqr(norm),
                    photonNumberRecurse<1>(psii),
                    eps)
              );

  // The most stringent test for the whole of a partial-trace density operator:

  mode::DensityOperator res(-braket(d23,d12)/sqr(norm)*dyad(d22,d12));

  {
    linalg::CMatrix tempView(res.matrixView());
    linalg::calculateTwoTimesRealPartOfSelf(tempView);
  }

  res()+=(d12.dyad()+d22.dyad())/sqr(norm);

  BOOST_CHECK(max(blitzplusplus::sqrAbs(partialTrace(psii,densityOperator,tmptools::Vector<1>(),mode::DensityOperatorLow())-res()))<sqr(eps));

}

BOOST_AUTO_TEST_CASE( RANK_THREE_RECURSIVE )
{
  quantumdata::StateVector<3> psi(c23*c12*c22);

  BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<0>,tmptools::Vector<0,1>(),0.),13.,eps));
  BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<1>,tmptools::Vector<0,1>(),0.), 5.,eps));
  BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<0>,tmptools::Vector<1,2>(),0.), 5.,eps));
  BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<1>,tmptools::Vector<1,2>(),0.), 8.,eps));
  BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<0>,tmptools::Vector<2,0>(),0.), 8.,eps));
  BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<1>,tmptools::Vector<2,0>(),0.),13.,eps));

  // BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse<0>,v1,0.), 5.,eps));
  // BOOST_CHECK(!fcmp(partialTrace(psi,photonNumberRecurse,tmptools::Vector<2,0>(),0.), 5.,eps));

}

const mode::StateVector e0(mode::fock(2,3)), e1(mode::fock(3,7)), e2(mode::fock(0,1));

int specialChecker(const quantumdata::LazyDensityOperator<3>& m, size_t i1, size_t i2, size_t i3)
{
  BOOST_CHECK(all(m.getDimensions()==ExtTiny<3>(i1,i2,i3)));
  partialTrace(m,photonNumberRecurse<0>,tmptools::Vector<1,2>(),0.);
  // This is only for checking whether the special implementations are also recursive.
  return 0;
}

BOOST_AUTO_TEST_CASE( THE_SPECIAL_CASE )
{
  quantumdata::StateVector<3> psi(e0*e1*e2);
  partialTrace(psi,bind(specialChecker,_1,3,7,1),tmptools::Vector<0,1,2>(),0);
  partialTrace(psi,bind(specialChecker,_1,3,1,7),tmptools::Vector<0,2,1>(),0);

  quantumdata::DensityOperator<3> rho(psi);
  partialTrace(psi,bind(specialChecker,_1,3,7,1),tmptools::Vector<0,1,2>(),0);
  partialTrace(psi,bind(specialChecker,_1,3,1,7),tmptools::Vector<0,2,1>(),0);

}


#include <Eigen/Core>
#include <optional>

#include <citlali/core/timestream/ptc/sensitivity.h>

namespace internal {

std::tuple<Eigen::Index, Eigen::Index, double> stat(Eigen::Index scanlength, double fsmp) {
    // make an even number of data points by rounding-down
    Eigen::Index npts = scanlength;
    if (npts % 2 == 1)
        npts--;
    // prepare containers in frequency domain
    Eigen::Index nfreqs = npts / 2 + 1; // number of one sided freq bins
    double dfreq = fsmp / npts;

    return {npts, nfreqs, dfreq};
}

Eigen::VectorXd freq(Eigen::Index npts, Eigen::Index nfreqs, double dfreq) {
    return dfreq * Eigen::VectorXd::LinSpaced(nfreqs, 0, npts / 2);
}

} // namespace internal


/*
   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 <autodiff/forward/real.hpp>
#include <autodiff/forward/real/eigen.hpp>
#include <Eigen/Dense>
#include <Eigen/Core>

#include "object.hpp"

// using namespace autodiff;
using namespace Eigen;

class Box : public Object {
    public:
        Box();
        autodiff::real U(const autodiff::ArrayXreal& pos);  // potential energy function
    private:

    protected:
        double length = 1;
        double width = 1;
        double height = 1;

};


#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();
}


//! \file examples/Minkowski_sum_2/approx_inset.cpp
// Computing the approximated inset of a polygon.

#include <fstream>
#include <iostream>
#include <list>
#include <boost/timer.hpp>

#include <CGAL/approximated_offset_2.h>

#include "bops_circular.h"

typedef CGAL::Polygon_2<Kernel>                         Linear_polygon;

int main(int argc, char* argv[])
{
  // Open the input file and read a polygon.
  const char* filename = (argc > 1) ? argv[1] : "tight.dat";
  std::ifstream in_file(filename);

  if (! in_file.is_open()) {
    std::cerr << "Failed to open the input file." << std::endl;
    return -1;
  }

  // Read the input polygon.
  Linear_polygon P;
  in_file >> P;
  in_file.close();

  std::cout << "Read an input polygon with " << P.size() << " vertices."
            << std::endl;

  // Approximate the offset polygon.
  std::list<Polygon_2> inset_polygons;
  boost::timer timer;
  approximated_inset_2(P, 1, 0.00001, std::back_inserter(inset_polygons));
  double secs = timer.elapsed();

  std::list<Polygon_2>::iterator it;
  std::cout << "The inset comprises " << inset_polygons.size()
            << " polygon(s)." << std::endl;
  for (it = inset_polygons.begin(); it != inset_polygons.end(); ++it)
    std::cout << "    Polygon with " << it->size() << " vertices." << std::endl;
  std::cout << "Inset computation took " << secs << " seconds." << std::endl;
  return 0;
}


#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;
}