#pragma once

// deal.II includes -------------------------------------------------------
#include <deal.II/base/exceptions.h>
#include <deal.II/base/index_set.h>
#include <deal.II/base/mpi.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/tensor.h>
#include <deal.II/base/timer.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>

#ifdef DEBUG
#include <deal.II/base/conditional_ostream.h>
#endif

// system includes --------------------------------------------------------
#include <yaml-cpp/yaml.h>
#include <boost/multi_array.hpp>
#include <cmath>
#include <complex>
#include <cstdio>
#include <fstream>
#include <functional>
#include <limits>
#include <stdexcept>
#include <tuple>
#include <unordered_map>
// trilinos includes ------------------------------------------------------
#include <Epetra_Export.h>
#include <Epetra_Import.h>
#include <Epetra_Map.h>
#include <Epetra_Vector.h>

// own includes -----------------------------------------------------------
#include "bc_traits.hpp"
#include "grid/dof_mapper_periodic_distributed.hpp"
#include "impl/bd_faces_manager_redist.hpp"
#include "impl/bd_faces_manager_simple.hpp"
#include "quadrature/qhermitew.hpp"
#include "quadrature/qmaxwell.hpp"
#include "spectral/basis/dof_mapper.hpp"
#include "spectral/basis/indexer.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/hermite_to_nodal.hpp"
#include "spectral/polar_to_hermite.hpp"
#include "spectral/rotate_basis.hpp"
#include "base/numbers.hpp"

namespace boltzmann {
namespace local_ {
// helper struct to keep quadrature rules for inflow rhs
struct inflow_bc_quad
{
  /**
   *
   * @param npts quad. degree
   * @param Tw   inflow temperature
   * @param w    basis weight, e.g. exp(-r^2/2) => w=0.5
   *
   *
   * @return
   */
  inflow_bc_quad(int npts, double Tw, double w = 0.5);

  inflow_bc_quad() {}

  QHermiteW x_quad;
  QMaxwell y_quad;
  // hermite polynomials

  typedef HermiteNW<double> hermw_t;
  std::shared_ptr<hermw_t> hermwx;
  std::shared_ptr<hermw_t> hermwy;
};

inflow_bc_quad::inflow_bc_quad(int npts, double Tw, double w)
    : x_quad(w + 1. / (2 * Tw), npts)
    , y_quad(w + 1. / (2 * Tw), npts)
    , hermwx(std::make_shared<HermiteNW<double> >(npts))
    , hermwy(std::make_shared<HermiteNW<double> >(npts))
// 256 digits (mpfr accuracy)
{
  hermwx->compute(x_quad.pts());

  std::vector<double> y(npts);
  for (int i = 0; i < npts; ++i) {
    y[i] = -y_quad.pts(i);
  }

  hermwy->compute(y);
}
}  // end namespace local_

// -------------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
class BoundaryConditions : public BD_FACES_MANAGER
{
 private:
  typedef BD_FACES_MANAGER bd_faces_manager_t;
  typedef dealii::types::global_dof_index size_type;
  typedef dealii::DoFHandler<2> dh_t;
  typedef typename METHOD::spectral_basis_t spectral_basis_t;
  typedef typename traits::DoFMapper<APP::bc_type>::type mapper_t;
  typedef Indexer<mapper_t> indexer_t;
  typedef dealii::TrilinosWrappers::MPI::Vector deal_vector_t;

  // boundary id
  typedef unsigned int bid_t;

 public:
  BoundaryConditions(double dt,
                     const dh_t& dh,
                     const spectral_basis_t& spectral_basis,
                     const indexer_t& indexer,
                     const YAML::Node& config);

  void apply(deal_vector_t& out, const deal_vector_t& in) const;

  void apply(Epetra_MultiVector& out, const Epetra_MultiVector& in) const;

  /**
   *  @brief Assemble inflow type boundary conditions into the right hand side
   *
   */
  template <typename VECTOR>
  void assemble_rhs(VECTOR& dst) const;

 private:
  typedef SpectralBasisFactoryHN::basis_type hermite_basis_t;
  typedef std::shared_ptr<impl::flux_worker> ptr_flux_worker;

 private:
  double dt_;
  const dh_t& dh_;
  const spectral_basis_t& spectral_basis_;
  const indexer_t& indexer_;
  const YAML::Node& config_;

  //@{
  /// spectral basis transformation / rotation operators
  RotateBasis<spectral_basis_t> R_;
  hermite_basis_t hermite_basis_;
  typedef Polar2Hermite<spectral_basis_t, hermite_basis_t> P2H_t;
  typedef Hermite2Nodal<hermite_basis_t> H2N_t;
  std::shared_ptr<P2H_t> ptr_P2H_;
  std::shared_ptr<H2N_t> ptr_H2N_;
  //@}

  std::map<bid_t, ptr_flux_worker> flux_workers_;
  std::set<bid_t> periodic_;

  //@{
  /*  Buffers for Lagrange coefficients
   *  1p: output from flux_worker for vertex 1
   *  2p: output from flux_worker for vertex 2
   */
  mutable Eigen::MatrixXd L1p_;  // output f (1. trial) for flux worker
  mutable Eigen::MatrixXd L1_;   // input f  (1. tiral function) ..
  mutable Eigen::MatrixXd L2p_;  // output f  (2. trial function) ..
  mutable Eigen::MatrixXd L2_;
  mutable Eigen::MatrixXd L_;
  //@}

  /// buffer for hermite coefficients
  /// ybuffer
  mutable Eigen::VectorXd ybuf_;

  // polynomial degree
  int K_ = -1;

  // TODO Epetra Multi vector, Import, Export, Maps
  // imports from base class
  using bd_faces_manager_t::relevant_dofs_;
  using bd_faces_manager_t::get_faces_list;
  // epetra vectors
  mutable std::shared_ptr<Epetra_Vector> x_ghosted_;
  mutable std::shared_ptr<Epetra_Vector> y_ghosted_;
  mutable std::shared_ptr<Epetra_Import> importer_;
  mutable std::shared_ptr<Epetra_Export> exporter_;
};

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::BoundaryConditions(
    double dt,
    const dh_t& dh,
    const spectral_basis_t& spectral_basis,
    const indexer_t& indexer,
    const YAML::Node& config)
    : bd_faces_manager_t(dh, spectral_basis, indexer)
    , dt_(dt)
    , dh_(dh)
    , spectral_basis_(spectral_basis)
    , indexer_(indexer)
    , config_(config)
    , R_(spectral_basis)
{
  int K_ = config_["SpectralBasis"]["deg"].as<int>();

  // initialize hermite basis
  SpectralBasisFactoryHN::create(hermite_basis_, K_, 2);
  static QHermiteW hermite_quad(1.0, K_);

  Eigen::VectorXd hw = hermite_quad.vwts<1>();
  Eigen::VectorXd hx = hermite_quad.vpts<1>();

  // resize buffers
  L1_.resize(K_, K_);
  L2_.resize(K_, K_);
  L1p_.resize(K_, K_);
  L2p_.resize(K_, K_);
  L_.resize(K_, K_);
  ybuf_.resize(spectral_basis_.n_dofs());

  typedef Eigen::MatrixXd mat_t;
  ptr_P2H_ = std::make_shared<P2H_t>(spectral_basis_, hermite_basis_);
  ptr_H2N_ = std::make_shared<H2N_t>(
      hermite_basis_, K_, [K_](mat_t& m1, mat_t& m2) { H2N_1d<>::create(m1, m2, K_); });

  auto node = config_["BoundaryDescriptors"];
  // TODO: process config
  for (YAML::const_iterator it = node.begin(); it != node.end(); ++it) {
    auto entry = it->second;
    id_t id = it->first.as<id_t>();
    std::string type = entry["type"].as<std::string>();

    typedef bc_traits<METHOD::lsq_type> bc_traits_t;
    if (std::strcmp(type.c_str(), "diffusive reflection") == 0) {
      double T = entry["T"].as<double>();
      double rho = 1.0;
      if (entry["rho"]) {
        rho = entry["rho"].as<double>();
      }
      if (entry["vt"]) {
        double vt = entry["vt"].as<double>();
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflection>(hw, hx, vt, T, rho);
      } else {
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflection>(hw, hx, 0, T, rho);
      }
    } else if (std::strcmp(type.c_str(), "diffusive reflection x") == 0) {
      std::string Tx = entry["Tx"].as<std::string>();
      if (entry["vt"]) {
        double vt = entry["vt"].as<double>();
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflectionX>(hw, hx, vt, Tx);
      } else {
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflectionX>(hw, hx, 0, Tx);
      }
    }
    else if (std::strcmp(type.c_str(), "specular reflection") == 0) {
      flux_workers_[id] = std::make_shared<typename bc_traits_t::SpecularReflection>(hw, hx);
    } else if (std::strcmp(type.c_str(), "inflow") == 0) {
      flux_workers_[id] = std::make_shared<typename bc_traits_t::Inflow>(hw, hx);

    } else if (std::strcmp(type.c_str(), "periodic") == 0) {
      periodic_.insert(id);
    } else {
      AssertThrow(false, dealii::ExcMessage("Unkown boundary condition entry"));
    }
  }

  auto epetra_map = relevant_dofs_.make_trilinos_map(MPI_COMM_WORLD, true);

  x_ghosted_ = std::make_shared<Epetra_Vector>(epetra_map);
  y_ghosted_ = std::make_shared<Epetra_Vector>(epetra_map);
}

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
template <typename VECTOR>
void
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::assemble_rhs(VECTOR& dst) const
{
  /* inflow boundary conditions need to be assembled into the rhs vector. this is done in this
     routine */
  const int K = spectral::get_max_k(spectral_basis_);
  std::map<double, local_::inflow_bc_quad> inflow_bc_quad_map;
  auto& bd_conf = config_["BoundaryDescriptors"];
  const int npts = 2 * K;

  // --------------------------------------------------
  // prepare quadrature rules
  for (YAML::const_iterator it = bd_conf.begin(); it != bd_conf.end(); ++it) {
    auto entry = it->second;
    id_t id = it->first.as<id_t>();

    std::string type = entry["type"].as<std::string>();
    if (std::strcmp(type.c_str(), "inflow") == 0) {
      // currently inflow function g(v), can be of Maxwellian type only
      if (std::strcmp(entry["func"].as<std::string>().c_str(), "maxwellian") == 0) {
        double Tw = entry["T"].as<double>();
        // basis exp weight
        const double basis_alpha = 0.5;
        inflow_bc_quad_map[Tw] = local_::inflow_bc_quad(npts, Tw, basis_alpha);
      }
    }
  }

  // --------------------------------------------------
  // prepare deal.II stuff
  const int dimX = 2;
  unsigned int N = spectral_basis_.n_dofs();
  dealii::UpdateFlags update_flags = dealii::update_values | dealii::update_JxW_values |
                                     dealii::update_quadrature_points |
                                     dealii::update_normal_vectors;

  dealii::QGauss<dimX - 1> quad(2);
  int n_qpoints = quad.size();

  auto& fe = dh_.get_fe();
  const int dofs_per_cell = fe.dofs_per_cell;
  dealii::FEFaceValues<2> fe_face_values(fe, quad, update_flags);
  std::vector<size_type> local_dof_indices(fe.dofs_per_cell);

  const auto& faces_list = this->get_faces_list();

  typedef typename VECTOR::size_type size_type;
  std::vector<size_type> global_indices(N);

  Eigen::VectorXd II(N);
  Eigen::VectorXd IIp(N);    // buffer to add into trilinos vector
  Eigen::VectorXd IIloc(N);  // buffer to add into trilinos vector

  // iterate over faces at boundary
  for (const auto& mypair : faces_list) {
    const auto& cell = std::get<0>(mypair);
    int face_idx = std::get<1>(mypair);
    id_t bd_ind = cell.face(face_idx)->boundary_id();

    // check if bc is periodic.
    std::string type = config_["BoundaryDescriptors"][bd_ind]["type"].as<std::string>();
    if (periodic_.find(bd_ind) != periodic_.end() || std::strcmp(type.c_str(), "inflow") != 0)
      // bc is periodic: nothing to do
      continue;
    // --------------------------------------------------
    // load parameter
    auto params = config_["BoundaryDescriptors"][bd_ind];

    std::string func = params["func"].as<std::string>();
    if (std::strcmp(func.c_str(), "zero") == 0) {
      // nothing to do
      continue;
    }
    // check the inflow function
    BAssertThrow(std::strcmp(func.c_str(), "maxwellian") == 0,
                 "BoundaryConditions::assemble_rhs, do not know how to handle boundary type.\n" +
                     "Type was: " + func);

    double Tw = params["T"].as<double>();

    auto iter = inflow_bc_quad_map.find(Tw);
    assert(iter != inflow_bc_quad_map.end());
    const auto& quad_helper = iter->second;
    Eigen::Vector2d v0 = {0, 0};
    if (params["v"]) {
      double vx = params["v"][0].as<double>();
      double vy = params["v"][1].as<double>();
      v0 = {vx, vy};
    }

    double rho;
    if (params["rho"])
      rho = params["rho"].as<double>();
    else
      rho = 1;

    typedef dealii::DoFCellAccessor<dealii::DoFHandler<2>, false> accessor_t;
    typedef dealii::TriaIterator<accessor_t> tria_iterator_t;

    cell.get_dof_indices(local_dof_indices);
    fe_face_values.reinit(tria_iterator_t(cell), face_idx);

    const double nx = fe_face_values.normal_vector(0)[0];
    const double ny = fe_face_values.normal_vector(0)[1];
    // winkel zwischen y-achse und n
    const double alpha = numbers::PI / 2 - std::atan2(ny, nx);

    std::array<double, 4> B;
    B.fill(0);
    for (unsigned int l1 = 0; l1 < fe.dofs_per_cell; ++l1) {
      double val = 0;
      for (int q = 0; q < n_qpoints; ++q) {
        val += fe_face_values.shape_value(l1, q) * fe_face_values.JxW(q);
      }
      B[l1] = val;
    }

    const double v02 = v0.squaredNorm();
    // hermite basis accessors
    typedef typename hermite_basis_t::elem_t elem_t;
    typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type hx_t;
    typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type hy_t;
    typename elem_t::Acc::template get<hx_t> get_hx;
    typename elem_t::Acc::template get<hy_t> get_hy;

    auto hermwx = quad_helper.hermwx;
    auto hermwy = quad_helper.hermwy;
    const auto& xpts = quad_helper.x_quad.pts();
    const auto& xwts = quad_helper.x_quad.wts();
    const auto& ypts = quad_helper.y_quad.pts();
    const auto& ywts = quad_helper.y_quad.wts();

    Eigen::Rotation2D<double> rot2d(alpha);
    Eigen::Matrix2d rotm = rot2d.toRotationMatrix();
    auto v0h = rotm * v0;
    /*
     *  HINT: about Hermite quad. rule: (used in x-direction) The weights are
     *     multiplied by e^(x^2/2), which accounts for the factor e^(-x^2/2)
     *     which is included in the evaluation of the Hermite polynomials (e.g.
     *     Hermite functions in this case)
     */
    for (unsigned int j = 0; j < N; ++j) {
      // iterate over test functions
      const auto& elem = hermite_basis_.get_elem(j);
      const unsigned int jx = get_hx(elem).get_id().k;
      const unsigned int jy = get_hy(elem).get_id().k;
      double val = 0;
      // 2d quadrature
      for (int qx = 0; qx < npts; ++qx) {
        for (int qy = 0; qy < npts; ++qy) {
          Eigen::Vector2d vh = {xpts[qx], -1.0 * ypts[qy]};
          // inflow => -1.0
          val -= hermwx->get(jx)[qx] * hermwy->get(jy)[qy] * std::exp(v0h.dot(vh) / Tw) *
                 (std::exp(-xpts[qx] * xpts[qx] / 2 / Tw) * xwts[qx]) *
                 (std::exp(vh[1] * vh[1] / 2) * ywts[qy]);
        }
      }
      II(j) = std::exp(-v02 / 2 / Tw) * val * rho / (2 * numbers::PI * Tw);
    }
    // transform to polar
    ptr_P2H_->to_hermite_T(IIp, II);
    // rotate back
    R_.apply(IIloc.data(), IIp.data(), -alpha);

    for (unsigned int l1 = 0; l1 < fe.dofs_per_cell; ++l1) {
      if (std::abs(B[l1]) > 1e-15) {
        for (unsigned int j = 0; j < N; ++j) {
          global_indices[j] = indexer_.to_global(local_dof_indices[l1], j);
        }
        II = IIloc * B[l1];
        dst.add(N, global_indices.data(), II.data());
      }
    }
  }

  dst.compress(dealii::VectorOperation::add);
}

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
void
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::apply(deal_vector_t& out,
                                                         const deal_vector_t& in) const
{
  this->apply(out.trilinos_vector(), in.trilinos_vector());
}

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
void
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::apply(Epetra_MultiVector& out,
                                                         const Epetra_MultiVector& in) const
{
#ifdef DEBUG
  out.SetTracebackMode(2);
  in.SetTracebackMode(2);
  x_ghosted_->SetTracebackMode(2);
  y_ghosted_->SetTracebackMode(2);
#endif

  const int dimX = 2;
  // ATTENTION: this makes assumptions on deal.II internals
  static size_type faces_vertex[4][2] = {{0, 2}, {1, 3}, {0, 1}, {2, 3}};

  static dealii::Tensor<2, 2> B;

#ifdef DEBUG
  const unsigned int pid = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  dealii::ConditionalOStream pcout(std::cout, pid == 0);
#endif

  if (importer_.use_count() == 0) {
#ifdef DEBUG
    pcout << "Importer not set. Initializing\n";
#endif
    auto& ghosted_map = x_ghosted_->Map();
    auto& src_map = out.Map();
    importer_ = std::make_shared<Epetra_Import>(ghosted_map, src_map);
  }

  if (exporter_.use_count() == 0) {
#ifdef DEBUG
    pcout << "Exporter not set. Initializing\n";
#endif
    auto& ghosted_map = x_ghosted_->Map();
    auto& src_map = out.Map();
    exporter_ = std::make_shared<Epetra_Export>(ghosted_map, src_map);
  }

  // Import nonlocal elements
  int ier;
  ier = x_ghosted_->Import(in, *importer_.get(), Insert);
  AssertThrow(ier == 0, dealii::ExcTrilinosError(ier));
  // get pointer to x_ghosted_
  const double* data_in = x_ghosted_->Values();
  // reset y_ghosted_
  std::fill(y_ghosted_->Values(), y_ghosted_->Values() + y_ghosted_->MyLength(), 0);

  double* data_out = y_ghosted_->Values();

  unsigned int N = spectral_basis_.n_dofs();
  dealii::UpdateFlags update_flags = dealii::update_values | dealii::update_JxW_values |
                                     dealii::update_quadrature_points |
                                     dealii::update_normal_vectors;

  dealii::QGauss<dimX - 1> quad(2);
  int n_qpoints = quad.size();

  auto& fe = dh_.get_fe();
  dealii::FEFaceValues<2> fe_face_values(fe, quad, update_flags);
  std::vector<size_type> local_dof_indices(fe.dofs_per_face);

  // transform to lagrange coefficients
  unsigned int local_size = in.MyLength();
  const auto& faces_list = this->get_faces_list();
  // rotated Polar coeffs
  Eigen::VectorXd cP_rot(N);
  // Hermite coeffs
  Eigen::VectorXd cH(N);

  for (const auto& mypair : faces_list) {
    const auto& cell = std::get<0>(mypair);
    int face_idx = std::get<1>(mypair);
    id_t bd_ind = cell.face(face_idx)->boundary_id();

    // this is a periodic boundary and thus requires no further computations
    // (since it is built into the enumeration of DoFs already)
    if (periodic_.find(bd_ind) != periodic_.end()) continue;

    auto fentry = flux_workers_.find(bd_ind);
    if (fentry == flux_workers_.end()) {
      AssertThrow(false, dealii::ExcMessage("unknown boundary type"));
    }

    typedef dealii::DoFCellAccessor<dealii::DoFHandler<2>, false> accessor_t;
    typedef dealii::TriaIterator<accessor_t> tria_iterator_t;

    cell.face(face_idx)->get_dof_indices(local_dof_indices);

    fe_face_values.reinit(tria_iterator_t(cell), face_idx);

#ifdef DEBUG
    // checking dof indexing
    std::vector<size_type> ldof_indices(fe.dofs_per_cell);
    cell.get_dof_indices(ldof_indices);
    size_type l0 = cell.face(face_idx)->vertex_dof_index(0, 0);
    size_type l1 = cell.face(face_idx)->vertex_dof_index(1, 0);

    AssertThrow(l0 == ldof_indices[faces_vertex[face_idx][0]],
                dealii::ExcMessage("something with the local dofs is wrong"));
    AssertThrow(l1 == ldof_indices[faces_vertex[face_idx][1]],
                dealii::ExcMessage("something with the local dofs is wrong"));
    AssertThrow(l0 == local_dof_indices[0],
                dealii::ExcMessage("something with local dofs is wrong"));
    AssertThrow(l1 == local_dof_indices[1],
                dealii::ExcMessage("something with local dofs is wrong"));
#endif
    B *= 0;
    for (int i = 0; i < dimX; ++i) {
      int l1 = faces_vertex[face_idx][i];
      for (int j = 0; j < dimX; ++j) {
        int l2 = faces_vertex[face_idx][j];
        for (unsigned int q = 0; q < quad.size(); ++q) {
          B[i][j] += (fe_face_values.shape_value(l1, q) * fe_face_values.shape_value(l2, q) *
                      fe_face_values.JxW(q));
        }
      }
    }

    Assert(std::abs(B[0][1]) > 0, dealii::ExcMessage("something went wrong"));
    Assert(std::abs(B[1][1]) > 0, dealii::ExcMessage("something went wrong"));
    Assert(std::abs(B[0][0]) > 0, dealii::ExcMessage("something went wrong"));

    const double nx = fe_face_values.normal_vector(0)[0];
    const double ny = fe_face_values.normal_vector(0)[1];
    // winkel zwischen y-achse und n
    const double alpha = numbers::PI / 2 - std::atan2(ny, nx);

    typedef dealii::TrilinosWrappers::types::int_type index_t;
    index_t ig1 =
        x_ghosted_->Map().LID(static_cast<index_t>(indexer_.to_global(local_dof_indices[0], 0)));

    Assert(ig1 != -1, dealii::ExcMessage("global index not found in x_ghosted_!"));

    index_t ig2 =
        x_ghosted_->Map().LID(static_cast<index_t>(indexer_.to_global(local_dof_indices[1], 0)));
    Assert(ig2 != -1, dealii::ExcMessage("global index not found in x_ghosted_!"));

    // *** flux ***
    R_.apply(cP_rot.data(), data_in + ig1, alpha);
    ptr_P2H_->to_hermite(cH, cP_rot);
    ptr_H2N_->to_nodal(L1_, cH);
    fentry->second->apply(L1p_, L1_, cell.face(face_idx)->vertex(0));

    R_.apply(cP_rot.data(), data_in + ig2, alpha);
    ptr_P2H_->to_hermite(cH, cP_rot);
    ptr_H2N_->to_nodal(L2_, cH);
    fentry->second->apply(L2p_, L2_, cell.face(face_idx)->vertex(1));

    // *** Assemble into y_ghosted_ ***
    // lfe_idx1-test function => global index ig1, ig1+N
    L_ = B[0][0] * L1p_ + B[0][1] * L2p_;
    ptr_H2N_->to_hermite(cH.data(), L_);               // to_hermite = to_nodal^T
    ptr_P2H_->to_hermite_T(cP_rot, cH);  // to_hermite_T = to_hermite^T
    // rotate back
    R_.apply(ybuf_.data(), cP_rot.data(), -alpha);
    // write to ghosted vector
    Eigen::Map<Eigen::VectorXd> vout1(data_out + ig1, N);
    vout1 += ybuf_;

    // lfe_idx2-test function => global index ig2, ig2+N
    L_ = B[1][0] * L1p_ + B[1][1] * L2p_;
    ptr_H2N_->to_hermite(cH, L_);               // to_hermite = to_nodal^T
    ptr_P2H_->to_hermite_T(cP_rot, cH);  // to_hermite_T = to_hermite^T
    // rotate back
    R_.apply(ybuf_.data(), cP_rot.data(), -alpha);

    // write to ghosted vector
    Eigen::Map<Eigen::VectorXd> vout2(data_out + ig2, N);
    vout2 += ybuf_;
  }

  // scale y_ghosted_
  y_ghosted_->Scale(dt_);
  // *** export y_ghosted_ into global solution vector ***
  ier = out.Export(*y_ghosted_.get(), *exporter_.get(), Epetra_CombineMode::Epetra_AddLocalAlso);
  Assert(ier == 0, dealii::ExcMessage("Export to global solution vector failed."));
}

}  // end namespace boltzmann


//
// Created by James Noeckel on 7/1/20.
//

#include "Construction.h"
#include <igl/triangle/triangulate.h>
#include "geometry/shapes2/Primitive.h"
//#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
//#include "geometry/geom.h"
#include "geometry/primitives3/intersect_planes.h"
#include "geometry/primitives3/BoundedPlane.h"
#include "utils/mesh_valid.h"
#include "geometry/primitives3/Multimesh.h"

using namespace Eigen;
using namespace boost;

Vector3d PartData::normal() const {
    return rot * Vector3d(0, 0, 1);
}

double PartData::offset() const {
    return -normal().dot(pos);
}

MatrixX3d PartData::unproject(const Ref<const MatrixX2d> &points) const {
    MatrixX3d points3d(points.rows(), 3);
    for (size_t r=0; r<points.rows(); ++r) {
        points3d.row(r) = (rot * Vector3d(points(r, 0), points(r, 1), 0) + pos).transpose();
    }
    return points3d;
}

MatrixX2d PartData::project(const Ref<const MatrixX3d> &points) const {
    MatrixX2d points2d(points.rows(), 2);
    Quaterniond rotinv = rot.conjugate();
    for (size_t r=0; r<points.rows(); ++r) {
        points2d.row(r) = (rotinv * (points.row(r).transpose() - pos)).transpose().head(2);
    }
    return points2d;
}

Vector2d PartData::projectDir(const Ref<const Vector3d> &vec) const {
    Quaterniond rotinv = rot.conjugate();
    return (rotinv * vec).head(2);
}

void Construction::setW(const std::vector<bool> &w) {
    w_ = w;
//    computeTotalMesh();
}

double Construction::connectivityEnergy(double a) {
    std::vector<std::tuple<size_t, size_t, double>> weightedEdges;
    std::vector<size_t> indices;
    for (size_t i=0; i<partData.size(); ++i) {
        if (w_[i]) {
            indices.push_back(i);
        }
    }
    if (indices.size() < 2) {
        return 0;
    }
    std::vector<size_t> inv_indices(partData.size(), -1);
    for (size_t i=0; i<indices.size(); ++i) {
        inv_indices[indices[i]] = i;
    }
    //TODO: don't create edges below a threshold probability
    for (auto it=indices.begin(); it != indices.end(); ++it) {
        for (auto it2 = it+1; it2 != indices.end(); ++it2) {
            double distance = distanceBetweenParts(*it, *it2);
            /** probability of non-connection */
            double Qij = std::max(1.0 - exp(-a * distance), 1e-8);
            double weight = -log(Qij);
            weightedEdges.emplace_back(inv_indices[*it], inv_indices[*it2], weight);
        }
    }

    typedef boost::property<boost::edge_weight_t, double> EdgeWeightProperty;
    typedef boost::adjacency_list<boost::listS, boost::vecS,boost::undirectedS,boost::no_property,EdgeWeightProperty> DenseGraph;
    typedef graph_traits < DenseGraph >::edge_descriptor DEdge;
    typedef graph_traits < DenseGraph >::vertex_descriptor DVertex;
    property_map<DenseGraph, vertex_index_t>::type dIndex;
    DenseGraph dg(indices.size());
    property_map<DenseGraph, edge_weight_t>::type weightmap = get(edge_weight, dg);
    for (const auto &edge : weightedEdges) {
        DEdge e;
        bool inserted;
        boost::tie(e, inserted) = add_edge(std::get<0>(edge), std::get<1>(edge), dg);
        weightmap[e] = std::get<2>(edge);
        //std::cout << "edge " << std::get<0>(edge) << "-" << std::get<1>(edge) << ": " << weightmap[e] << std::endl;
    }
    BOOST_AUTO(parities, boost::make_one_bit_color_map(num_vertices(dg), get(boost::vertex_index, dg)));
    double w = boost::stoer_wagner_min_cut(dg, get(boost::edge_weight, dg), boost::parity_map(parities));
    /** max probability of disjoint subsets of parts being disconnected */
    double prodQij = exp(-w);
    return -log(1.0 - prodQij);
}

double Construction::distanceBetweenPartsAsymm(size_t idx1, size_t idx2, Ref<RowVector3d> closestPoint, int &index) const {
    VectorXd sqrD;
    VectorXi II;
    MatrixXd CC;
    const auto &mesh1 = partMeshes[idx1];
    const auto &mesh2 = partMeshes[idx2];
    aabbs[idx1].squared_distance(mesh1.first, mesh1.second, mesh2.first, sqrD, II, CC);
    double d = sqrD.minCoeff(&index);
    closestPoint = CC.row(index);
    return std::sqrt(d);
}

double Construction::distanceBetweenParts(size_t idx1, size_t idx2, Ref<RowVector3d> point1, Ref<RowVector3d> point2) const {
    RowVector3d part1point, part2point;
    int part1index, part2index;
    double minDist1 = distanceBetweenPartsAsymm(idx1, idx2, part1point, part2index);
    double minDist2 = distanceBetweenPartsAsymm(idx2, idx1, part2point, part1index);
    if (minDist1 < minDist2) {
        point1 = part1point;
        point2 = partMeshes[idx2].first.row(part2index);
        return minDist1;
    } else {
        point1 = partMeshes[idx1].first.row(part1index);
        point2 = part2point;
        return minDist2;
    }
}

double Construction::distanceBetweenParts(size_t idx1, size_t idx2) const {
    RowVector3d point1;
    RowVector3d point2;
    return distanceBetweenParts(idx1, idx2, point1, point2);
}

bool Construction::contains(size_t partIdx, const Ref<const RowVector3d> &pt) {
    const auto &pd = partData[partIdx];
    Vector3d localPt = pd.rot.conjugate() * (pt.transpose() - pd.pos);
    if (localPt.z() > 0 || localPt.z() < -getStock(partIdx).thickness) {
        return false;
    }
    return getShape(partIdx).cutPath->contains(localPt.head(2));
}

StockData &Construction::getStock(size_t partIdx) {
    return stockData[shapeData[partData[partIdx].shapeIdx].stockIdx];
}

ShapeData &Construction::getShape(size_t partIdx) {
    return shapeData[partData[partIdx].shapeIdx];
}

const StockData &Construction::getStock(size_t partIdx) const {
    return stockData[shapeData[partData[partIdx].shapeIdx].stockIdx];
}

const ShapeData &Construction::getShape(size_t partIdx) const {
    return shapeData[partData[partIdx].shapeIdx];
}

int Construction::regularizeDepths(double threshold) {
    //TODO: threshold-based merging

    std::vector<double> thicknesses;
    std::vector<std::vector<size_t>> indices;
    for (size_t c=0; c<partData.size(); ++c) {
        if (w_[c]) {
            double thickness = getStock(c).thickness;
            bool found = false;
            for (size_t j=0; j<thicknesses.size(); ++j) {
                double t = thicknesses[j];
                if (std::abs(t - thickness) < threshold) {
                    found = true;
                    indices[j].push_back(c);
                    break;
                }
            }
            if (!found) {
                thicknesses.push_back(thickness);
                indices.push_back(std::vector<size_t>(1, c));
            }
        }
    }
    size_t newStartIndex = stockData.size();
    for (auto thickness : thicknesses) {
        StockData sd;
        sd.thickness = thickness;
        stockData.push_back(sd);
    }

    for (size_t tInd=0; tInd<thicknesses.size(); ++tInd) {
        for (auto c : indices[tInd]) {
//            double thicknessChange = getStock(c).thickness - thicknesses[tInd];
//            Vector3d n = partData[c].normal();
//            partData[c].pos += n * (thicknessChange / 2);
            getShape(c).stockIdx = tInd + newStartIndex;
        }
    }
    return thicknesses.size();
}

MatrixXd Construction::allDistances(const PointCloud3::Handle &points, int stride) {
    size_t nPts = points->P.rows()/stride;
    MatrixX3d pointsReduced;
    if (stride > 1) {
        pointsReduced = MatrixX3d(nPts, 3);
        for (size_t i = 0; i < pointsReduced.rows(); ++i) {
            pointsReduced.row(i) = points->P.row(i * stride);
        }
    }
    MatrixXd allDistances(nPts, partData.size());
    size_t N = partData.size();
#pragma omp parallel for default(none) shared(N, allDistances, aabbs, points, stride, pointsReduced)
    for (size_t k=0; k<N; ++k) {
        const auto &mesh = partMeshes[k];
        VectorXd sqrD;
        VectorXi II;
        MatrixXd CC;
        if (stride > 1) {
            aabbs[k].squared_distance(mesh.first, mesh.second, pointsReduced, sqrD, II, CC);
        } else {
            aabbs[k].squared_distance(mesh.first, mesh.second, points->P, sqrD, II, CC);
        }
        allDistances.col(k) = sqrD;
    }
    return allDistances;
}

double Construction::pointDistance(const PointCloud3::Handle &points) {
    size_t nV = 0;
    if (std::accumulate(w_.begin(), w_.end(), 0) == 0) return std::numeric_limits<double>::max();
    /*{
        igl::opengl::glfw::Viewer viewer;
        viewer.data().set_mesh(std::get<0>(mesh), std::get<1>(mesh));
        viewer.data().set_colors(std::get<2>(mesh));
        viewer.launch();
    }*/
    VectorXd sqrD;
    VectorXi II;
    MatrixXd CC;
    aabb.squared_distance(std::get<0>(mesh), std::get<1>(mesh), points->P, sqrD, II, CC);
    //igl::point_mesh_squared_distance(points->P,V,I,sqrD,II,CC);
    return sqrD.sum();
}

void Construction::computeTotalMesh(bool recomputeBVH) {
    Multimesh multi;
    for (int i=0; i<partData.size(); ++i) {
        if (w_[i]) {
            multi.AddMesh(partMeshes[i]);
        }
    }
    auto totalMesh = multi.GetTotalMesh();

    VectorXi I(totalMesh.second.rows());

    size_t offsetF = 0;
    //size_t realIndex = 0;
    std::cout << "populating vertices, faces" << std::endl;
    for (size_t i=0; i<partMeshes.size(); ++i) {
        if (w_[i]) {
            const auto &Fi = partMeshes[i].second;
            //C.block(offsetF, 0, Fi.rows(), 3).rowwise() = colorAtIndex(i, partData.size());
            for (size_t j=0; j<Fi.rows(); ++j) {
                I(offsetF + j) = i;
            }
            offsetF += partMeshes[i].second.rows();
            //++realIndex;
        }
    }
    mesh = {totalMesh.first, totalMesh.second, I};
    int invalidID = mesh_valid(totalMesh.first, totalMesh.second);
    if (invalidID >= 0) {
        std::cout << "total mesh invalid at face " << invalidID << std::endl;
        hasTotalMeshInfo = false;
        return;
    }
    aabb = igl::AABB<Eigen::MatrixX3d, 3>();
    if (recomputeBVH) {
        std::cout << "recomputing total AABB" << std::endl;
        aabb.init(std::get<0>(mesh), std::get<1>(mesh));
    }
    hasTotalMeshInfo = true;
}

CombinedCurve Construction::getBackCurve(size_t partIdx) const {
    const auto &shape = getShape(partIdx);
    double thickness = getStock(partIdx).thickness;
    CombinedCurve backCurve = shape.cutPath->curves();
    size_t N = backCurve.size();
    for (const auto &pair : backCurve.constraints_) {
        if (pair.second.tiltAngCos != 0) {
            std::cout << "tilting " << partIdx << " curve " << pair.first << " by " << pair.second.tiltAngCos << std::endl;
            //offset in the back plane to account for tilt
            double offset = thickness * pair.second.tiltAngCos / std::sqrt(1 - pair.second.tiltAngCos * pair.second.tiltAngCos);
            Vector2d ab = (pair.second.edge.second - pair.second.edge.first).normalized();
            Vector2d outNormal(ab.y(), -ab.x());
            Vector2d displacement = outNormal * offset;
            backCurve.moveVertex(pair.first, displacement);
            size_t nextIndex = (pair.first + 1) % N;
            backCurve.moveVertex(nextIndex, displacement);
        }
    }

    return backCurve;
}

void Construction::samplePartCurves(size_t c, MatrixX2d &curves2D, MatrixX2d &curves2DBack, MatrixX3d &curves3D, MatrixX3d &curves3DBack, std::vector<std::pair<MatrixX2d, MatrixX3d>> &holes) const {
    const auto &shape = getShape(c);
    double thickness = getStock(c).thickness;
    curves2D = shape.cutPath->points();
    curves3D = MatrixX3d(curves2D.rows(), 3);
    for (int r=0; r<curves2D.rows(); ++r) {
        curves3D.row(r) = (partData[c].rot * Vector3d(curves2D(r, 0), curves2D(r, 1), 0) + partData[c].pos).transpose();
    }

    {
        //back curve
        CombinedCurve backCurve = getBackCurve(c);
        PolyCurveWithHoles newPrimitive(std::move(backCurve));
        curves2DBack = newPrimitive.points();
        curves3DBack = MatrixX3d(curves2D.rows(), 3);
        for (int r=0; r<curves2D.rows(); ++r) {
            curves3DBack.row(r) = (partData[c].rot * Vector3d(curves2DBack(r, 0), curves2DBack(r, 1), -thickness) + partData[c].pos).transpose();
        }
    }

    holes.reserve(shape.cutPath->children().size());
    const auto children = shape.cutPath->children();
    for (const auto & i : children) {
        MatrixX2d childCurve = i->points();
        MatrixX3d childCurve3d(childCurve.rows(), 3);
        for (int r=0; r<childCurve.rows(); ++r) {
            childCurve3d.row(r) = (partData[c].rot * Vector3d(childCurve(r, 0), childCurve(r, 1), 0) + partData[c].pos).transpose();
        }
        holes.emplace_back(std::move(childCurve), std::move(childCurve3d));
    }
}

void Construction::computeMeshes(bool recomputeBVH) {
    partMeshes.clear();
    partMeshes.reserve(partData.size());
    /** part ID, 2d curve, 3d curve, holes */
    std::vector<std::tuple<size_t, MatrixX2d, MatrixX3d, std::vector<std::pair<MatrixX2d, MatrixX3d>>>> sampledCurves_;

    for (size_t c=0; c<partData.size(); ++c) {
        MatrixX2d curves2D;
        MatrixX2d curves2DBack;
        MatrixX3d curves3D;
        MatrixX3d curves3DBack;
        std::vector<std::pair<MatrixX2d, MatrixX3d>> holes;
        samplePartCurves(c, curves2D, curves2DBack, curves3D, curves3DBack, holes);
        if (curves2D.rows() == 0) continue;
        if (!holes.empty()) {
            std::cout << "generating mesh part " << c << " with " << holes.size() << " holes" << std::endl;
        }
        size_t numHolePts = 0;
        for (const auto & hole : holes) {
            numHolePts += hole.first.rows();
        }
        size_t numHalfTotalPoints = curves2D.rows() + numHolePts;
        MatrixX3d V(numHalfTotalPoints * 2, 3);
        V.block(0, 0, curves2D.rows(), 3) = curves3D;
        V.block(numHalfTotalPoints, 0, curves2D.rows(), 3) = curves3DBack;
        double thickness = getStock(c).thickness;
        std::cout << "part " << c << " thickness: " << thickness << " (shapeID " << partData[c].shapeIdx << ", stockID" << getShape(c).stockIdx << std::endl;
        RowVector3d normalShift = ((partData[c].normal()) * thickness).transpose();
        size_t holeOffset = curves2D.rows();
        for (const auto & hole : holes) {
            V.block(holeOffset, 0, hole.first.rows(), 3) = hole.second;
            V.block(holeOffset + numHalfTotalPoints, 0, hole.first.rows(), 3) = hole.second.rowwise() - normalShift;
            holeOffset += hole.first.rows();
        }
        MatrixX3i I(numHalfTotalPoints * 2, 3);
        for (int r = 0; r < curves2D.rows(); ++r) {
            int i0 = r;
            int i1 = (r + 1) % curves2D.rows();
            int i2 = numHalfTotalPoints + r;
            int i3 = numHalfTotalPoints + (r + 1) % curves2D.rows();
            I.row(r) = RowVector3i(i0, i1, i3);
            I.row(numHalfTotalPoints + r) = RowVector3i(i2, i0, i3);
        }
        holeOffset = curves2D.rows();
        for (const auto & hole : holes) {
            for (int r=0; r<hole.first.rows(); ++r) {
                int i0 = r;
                int i1 = (r + 1) % hole.first.rows();
                int i2 = numHalfTotalPoints + r;
                int i3 = numHalfTotalPoints + (r + 1) % hole.first.rows();
                I.row(holeOffset + r) = RowVector3i(i0, i1, i3).array() + holeOffset;
                I.row(holeOffset + numHalfTotalPoints + r) = RowVector3i(i2, i0, i3).array() + holeOffset;
            }
            holeOffset += hole.first.rows();
        }
        //triangulation
        Eigen::MatrixXd boundary2D(numHalfTotalPoints, 2);
        MatrixXi I2d(numHalfTotalPoints, 2);
        boundary2D.block(0, 0, curves2D.rows(), 2) = curves2D;
        for (size_t j=0; j<curves2D.rows(); ++j) {
            I2d(j, 0) = j;
            I2d(j, 1) = (j + 1) % curves2D.rows();
        }
        holeOffset = curves2D.rows();
        //std::cout << "curves2D.rows()=" << curves2D.rows() << "; boundary2d.rows()=" << boundary2D.rows() << std::endl;
        for (const auto &hole : holes) {
            boundary2D.block(holeOffset, 0, hole.first.rows(), 2) = hole.first;
            for (size_t j=0; j<hole.first.rows(); ++j) {
                I2d.row(holeOffset + j) = RowVector2i(
                        holeOffset + j,
                        holeOffset + (j+1) % hole.first.rows()
                        );
            }
            holeOffset += hole.first.rows();
        }

//        std::cout << "num rows: " << curves2D.rows() << std::endl << std::endl;
//        std::cout << I2d << std::endl << std::endl;
//        std::cout << curves2D << std::endl;
        //TODO: use guaranteed interior point instead of bounding box center
        MatrixXd H(holes.size(), 2);
        for (size_t i=0; i<holes.size(); ++i) {
            std::cout << "triangulating hole " << i << " to find interior point" << std::endl;
            MatrixXd HH(0, 2);
            MatrixXd VHole;
            MatrixXi FHole;
            MatrixXi I2dHole(holes[i].first.rows(), 2);
            for (size_t j=0; j<I2dHole.rows(); ++j) {
                I2dHole(j, 0) = j;
                I2dHole(j, 1) = (j+1) % I2dHole.rows();
            }
            igl::triangle::triangulate(holes[i].first, I2dHole, HH, "Qp", VHole, FHole);
            H.row(i) = (VHole.row(FHole(0, 0)) + VHole.row(FHole(0, 1)) + VHole.row(FHole(0, 2)))/3.0;
        }
        std::cout << "triangulating full shape" << std::endl;
        MatrixXd V2;
        MatrixXi F2;
        igl::triangle::triangulate(boundary2D, I2d, H, "p", V2, F2);
        std::cout << "triangulation finished" << std::endl;
        size_t Ioffset = I.rows();
        MatrixX3i Inew(Ioffset + F2.rows() * 2, 3);
        Inew.block(0, 0, Ioffset, 3) = I;
        Inew.block(Ioffset, 0, F2.rows(), 3) = F2;
        //back faces
        MatrixX3i F3 = F2.array() + numHalfTotalPoints;
        Inew.block(Ioffset + F2.rows(), 0, F2.rows(), 1) = F3.col(0);
        Inew.block(Ioffset + F2.rows(), 1, F2.rows(), 1) = F3.col(2);
        Inew.block(Ioffset + F2.rows(), 2, F2.rows(), 1) = F3.col(1);
        partMeshes.emplace_back(std::move(V), std::move(Inew));
    }
    aabbs.clear();
    if (recomputeBVH) {
        std::cout << "recomputing aabbs" << std::endl;
        aabbs.resize(partMeshes.size());
        for (size_t i = 0; i < aabbs.size(); ++i) {
            int invalidID = mesh_valid(partMeshes[i].first, partMeshes[i].second);
            if (invalidID >= 0) {
                std::cout << "Invalid mesh " << i << " at face " << invalidID << "!" << std::endl;
            }
            aabbs[i].init(partMeshes[i].first, partMeshes[i].second);
        }
    }
    std::cout << "recomputing total mesh" << std::endl;
    computeTotalMesh(recomputeBVH);
}

int Construction::buildGraph(double distanceThreshold) {
    size_t numParts = 0;
    std::vector<size_t> partIdxs;
    for (size_t i=0; i<w_.size(); ++i) {
        if (w_[i]) {
            ++numParts;
            partIdxs.push_back(i);
        }
    }
    g = Graph(numParts);
    size_t i=0;
    vertex_iter vi, vend;
    for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi, ++i) {
        Vertex v1 = *vi;
        g[v1].partIdx = partIdxs[i];
    }
    return findNewConnections(distanceThreshold);
}

int Construction::findNewConnections(double distanceThreshold) {
    size_t numEdges = 0;
    vertex_iter vi, vend;
    IndexMap index = get(vertex_index, g);
    for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
        Vertex v1 = *vi;
        const PartData &pd1 = partData[g[v1].partIdx];
        for (vertex_iter vi2 = vi+1; vi2 != vend; ++vi2) {
            Vertex v2 = *vi2;
            if (!edge(v1, v2, g).second) {
                const PartData &pd2 = partData[g[v2].partIdx];
                double distance = distanceBetweenParts(g[v1].partIdx, g[v2].partIdx);
//                std::cout << "part distance: " << distance << std::endl;
                if (distance < distanceThreshold) {
                    std::cout << "adding " << g[v1].partIdx << '-' << g[v2].partIdx << std::endl;
                    numEdges++;
                    Edge e;
                    bool inserted;
                    boost::tie(e, inserted) = add_edge(v1, v2, g);
                    if (inserted) {
                        g[e].part1 = index[v1];
                        g[e].part2 = index[v2];
                    } else {
                        std::cout << "failed to insert edge " << index[v1] << '-' << index[v2] << '!' << std::endl;
                    }
                }
            }
        }
    }
    return numEdges;
}

MultiRay3d Construction::contactEdge(size_t partIdx1, size_t partIdx2, double offset1, double offset2, double margin, bool usePart1Shape, bool usePart2Shape) const {
    const PartData &pd1 = partData[partIdx1];
    const PartData &pd2 = partData[partIdx2];
    offset1 += pd1.offset();
    offset2 += pd2.offset();
    BoundedPlane plane1(getShape(partIdx1).cutPath, pd1.rot.conjugate().matrix(), offset1);
    BoundedPlane plane2(getShape(partIdx2).cutPath, pd2.rot.conjugate().matrix(), offset2);
    if (!usePart1Shape) {
        plane1.setCurrentShape(-1);
    }
    if (!usePart2Shape) {
        plane2.setCurrentShape(-1);
    }
    MultiRay3d intersection;
    std::cout << "intersecting " << partIdx1 << " and " << partIdx2 << " with margin " << margin << "(thicknesses " << getStock(partIdx1).thickness << ", " << getStock(partIdx2).thickness << ")" << std::endl;
    plane1.intersect(plane2, intersection, margin);
    std::cout << "intersected" << std::endl;
    return intersection;
}

bool Construction::connectionValid(ConnectionEdge &connection, double margin) {
    Vertex v1 = vertex(connection.part1, g);
    Vertex v2 = vertex(connection.part2, g);
    size_t part1 = g[v1].partIdx;
    size_t part2 = g[v2].partIdx;
    std::cout << "validating connection [" << part1 << '-' << part2 << ']' << std::endl;
    const auto &pd1 = partData[part1];
    const auto &pd2 = partData[part2];
    Vector3d n1 = pd1.normal();
    Vector3d n2 = pd2.normal();
//    Vector3d tangent = n1.cross(n2);
    double offset1 = pd1.offset();
    double offset2 = pd2.offset();
    double depth1 = getStock(part1).thickness;
    double depth2 = getStock(part2).thickness;

//    MatrixX2d points2 = getShape(part1).cutPath->points();
//    double minOffset = std::numeric_limits<double>::max();
//    double maxOffset = std::numeric_limits<double>::min();
//    for (size_t r = 0; r < points2.rows(); ++r) {
//        Vector3d pt3d = pd2.unproject(points2.row(r)).row(0).transpose();
//        double offset = pt3d.dot(tangent);
//        minOffset = std::min(minOffset, offset);
//        maxOffset = std::max(maxOffset, offset);
//    }

    MatrixX2d points1 = getShape(part1).cutPath->points();
    double minDepth1 = std::numeric_limits<double>::max();
    double maxDepth1 = std::numeric_limits<double>::min();
    //detect whether protrusion of part 1 is not too great
    for (size_t r = 0; r < points1.rows(); ++r) {
        Vector3d pt3d = pd1.unproject(points1.row(r)).row(0).transpose();
//        double offset = pt3d.dot(tangent);
//        if (offset >= minOffset + margin && offset <= maxOffset - margin) {
            double dist = pt3d.dot(n2) + offset2;
            minDepth1 = std::min(minDepth1, dist);
            maxDepth1 = std::max(maxDepth1, dist);
//        }
    }
    if (minDepth1 > maxDepth1) return false;

    connection.type = ConnectionEdge::CET_CUT_TO_FACE;
    if (-(depth2 + minDepth1) > maxDepth1) {
        connection.backface2 = true;
    } else {
        connection.backface2 = false;
    }
    return  std::min(maxDepth1, -(depth2 + minDepth1)) <= depth2;
}

MultiRay3d Construction::connectionContact(const ConnectionEdge &connection, double margin) {
    if (connection.type == ConnectionEdge::CET_UNKNOWN) return MultiRay3d();
    Vertex v1 = vertex(connection.part1, g);
    Vertex v2 = vertex(connection.part2, g);
    size_t partIdx1 = g[v1].partIdx;
    size_t partIdx2 = g[v2].partIdx;
    //set the offsets to interior planes of the corner connection
    double offset1 = !connection.backface1 ? getStock(partIdx1).thickness : 0;
    double offset2 = connection.backface2 ? getStock(partIdx2).thickness : 0;
//        Vector3d n1 = partData[partIdx1].normal();
    Vector3d n2 = partData[partIdx2].normal();
    double offset2Offset = 0;
    if (connection.type == ConnectionEdge::CET_CUT_TO_FACE || (connection.optimizeStage > 0 && connection.type == ConnectionEdge::CET_CORNER)) {
        offset2Offset = connection.backface2 ? margin*2 : -margin*2;
        std::cout << "offsetting connection [" << partIdx1 << '-' << partIdx2 << "] by " << offset2Offset << std::endl;
    }
    offset2 += offset2Offset;
    std::cout << "final offset of [" << partIdx1 << '-' << partIdx2 << "] is " << offset2 << std::endl;
    MultiRay3d mEdge = contactEdge(partIdx1, partIdx2, offset1, offset2, margin);
    if (offset2Offset != 0) {
        MultiRay3d mEdgeOrig = contactEdge(partIdx1, partIdx2, offset1, offset2-offset2Offset, 0);
        double oOrig = mEdgeOrig.o.dot(mEdge.d);
        double o = mEdge.o.dot(mEdge.d);
        mEdge.o = mEdgeOrig.o + (o - oOrig) * mEdge.d;
//        mEdge.o += n2 * offset2Offset;
    }
    return mEdge;
}

void Construction::recomputeConnectionContacts(double margin, bool useCurves) {
    if (useCurves) {
        margin /= 10;
    }
    edge_iter ei, eb;
    for (boost::tie(ei, eb) = edges(g); ei != eb; ++ei) {
        Edge e = *ei;
        g[e].innerEdge = connectionContact(g[e], margin);
        if (g[e].innerEdge.size() == 0) {
            Vertex v1 = vertex(g[e].part1, g);
            Vertex v2 = vertex(g[e].part2, g);
            size_t partIdx1 = g[v1].partIdx;
            size_t partIdx2 = g[v2].partIdx;
            std::cout << "warning: " << partIdx1 << '-' << partIdx2 << " has no contact" << std::endl;
        }
//        g[e].optimizeStage = 2;
    }
}

/*void Construction::realignConnectionEdges() {
    edge_iter ei, eb;
    for (boost::tie(ei, eb) = edges(g); ei != eb; ++ei) {
        Edge e = *ei;
        Vertex v1 = vertex(g[e].part1, g);
        Vertex v2 = vertex(g[e].part2, g);
        size_t partIdx1 = g[v1].partIdx;
        size_t partIdx2 = g[v2].partIdx;
        auto multiray = contactEdge(partIdx1, partIdx2, !g[e].backface1, g[e].backface2, 0);
        g[e].innerEdge.d = multiray.d;
    }
}*/

Edge2d getConstraintEdge(const MultiRay3d &contactEdge, size_t edgeIndex, bool reverseDir, const Vector2d &edgeNormal, double margin, double constraintOffset, const PartData &pd1, const PartData& pd2, bool &valid) {
    valid = true;
    Edge2d edge2d;
    {
        Edge3d edge3d = contactEdge.getEdge(edgeIndex);
        edge2d = Edge2d(pd1.project(edge3d.first.transpose()).transpose(), pd1.project(edge3d.second.transpose()).transpose());
        if (reverseDir) std::swap(edge2d.first, edge2d.second);
    }
    Vector2d edgeDir = edge2d.second - edge2d.first;
    double edgeLength = edgeDir.norm();
    edgeDir /= edgeLength;
//                        double minGap = std::numeric_limits<double>::max();
    double offset = edgeNormal.dot(edge2d.first);
//                        for (int r = 0; r < points.rows(); ++r) {
//                            double dist = std::abs(points.row(r).dot(edgeNormal) - offset);
//                            minGap = std::min(dist, minGap);
//                        }
//                    if (g[e].type == ConnectionEdge::CET_CORNER && reversed) {
//                        minGap += getStock(partIdx2).thickness;
//                    }

//SHRINK
//if (edgeLength <= margin * 2) {
//        valid = false;
//        return {{0,0},{0,0}};
//    }
//    edge2d.first += edgeDir * margin;
//    edge2d.second -= edgeDir * margin;
//    std::cout << "adding " << constraintOffset * edgeNormal.transpose() << std::endl;
    edge2d.first += constraintOffset * edgeNormal;
    edge2d.second += constraintOffset * edgeNormal;
    return edge2d;
}

/** find guide edge for connection constraints 1 by intersecting with connection constraints 2
 * ray1 start and end should be set to finite values based on constraint edges, and will be extended to make way for intersection */
bool guideEdge(Ray2d &ray1, const Ray2d &ray2) {
    double t1;
    ray1.intersect(ray2, t1);
    bool changed = false;
    if (t1 < ray1.start) {
        ray1.start = t1;
        changed = true;
    }
    if (t1 > ray1.end) {
        ray1.end = t1;
        changed = true;
    }
    return changed;
}

void Construction::recomputeConnectionConstraints(double margin, double max_abs_dot_product) {
    IndexMap index = get(vertex_index, g);
//    vertex_iter vi, vend;
    std::cout << "adding constraints from general exclusion" << std::endl;
    size_t N = num_vertices(g);
    for (size_t i=0; i<N; ++i) {
        Vertex v = vertex(i, g);
        size_t partIdx1 = g[v].partIdx;
        std::cout << "processing part " << partIdx1 << std::endl;
        PartData &pd1 = partData[partIdx1];
        MatrixX2d points = getShape(partIdx1).cutPath->points();
        //collect shape constraints from neighboring parts
        out_edge_iter ei, eb;
        getShape(partIdx1).connectionConstraints.clear();

        for (size_t j=0; j<N; ++j) {
            if (i == j) continue;
            Vertex v2 = vertex(j, g);
            auto pair = edge(v, v2, g);
            size_t partIdx2 = g[v2].partIdx;
            const auto &pd2 = partData[partIdx2];
            Vector3d surfaceNormal = pd2.normal();
            if (std::abs(pd1.normal().dot(surfaceNormal)) > max_abs_dot_product) continue;
            //no connection, or cut to face connection, or corner connection where part 1 is truncated
            bool noConnection = !pair.second || g[pair.first].type == ConnectionEdge::CET_UNKNOWN;
            if (noConnection || ((g[pair.first].type == ConnectionEdge::CET_CUT_TO_FACE || g[pair.first].type == ConnectionEdge::CET_CORNER) && g[pair.first].part1 == index[v])) {
                double offset1, offset2;
                std::cout << "adding bigEdge connection constraint from part " << partIdx2 << " to " << partIdx1 << std::endl;
                if (noConnection) {
                    //use heuristics to decide what side the edge is on
                    ConnectionEdge connection12;
                    connection12.part1 = index[v];
                    connection12.part2 = index[v2];
                    ConnectionEdge connection21 = connection12;
                    std::swap(connection21.part1, connection21.part2);
                    bool valid12 = connectionValid(connection12, margin);
                    bool valid21 = connectionValid(connection21, margin);
                    if (!valid12) continue;
                    if (!connection12.backface2) {
                        surfaceNormal = -surfaceNormal;
                    }
                    //"left" tangent of outward surfaceNormal should point ccw along boundary

                    offset1 = connection21.backface2 ? getStock(partIdx1).thickness : 0;
                    offset2 = connection12.backface2 ? getStock(partIdx2).thickness : 0;
                } else {
                    //use known connection
                    {
                        Vertex cv1 = vertex(g[pair.first].part1, g);
                        Vertex cv2 = vertex(g[pair.first].part2, g);
                        std::cout << "using known connection " << g[cv1].partIdx <<'-' << g[cv2].partIdx << std::endl;
                        std::cout << "backface1: " << g[pair.first].backface1 << ", backface2: " << g[pair.first].backface2 << std::endl;
                    }
                    if (!g[pair.first].backface2) {
                        surfaceNormal = -surfaceNormal;
                    }
                    offset1 = !g[pair.first].backface1 ? getStock(partIdx1).thickness : 0;
                    offset2 = g[pair.first].backface2 ? getStock(partIdx2).thickness : 0;
                    std::cout << "offset1: " << offset1 << ", offset2: " << offset2 << std::endl;
                }
                auto bigEdge = contactEdge(partIdx1, partIdx2, offset1, offset2, margin, false, true);

                Vector2d projectedNormal = pd1.projectDir(surfaceNormal).normalized();
                Vector2d tangent(-projectedNormal.y(), projectedNormal.x());
                Vector2d edgeDir2d = pd1.projectDir(bigEdge.d).normalized();
                Vector2d edgeNormal(edgeDir2d.y(), -edgeDir2d.x());
                bool reverseDir = false;
                if (pd1.projectDir(bigEdge.d).dot(tangent) < 0) {
                    edgeNormal = -edgeNormal;
                    reverseDir = true;
                }

                for (size_t edgeIndex = 0; edgeIndex < bigEdge.ranges.size(); ++edgeIndex) {
                    bool valid;
                    Edge2d edge2d = getConstraintEdge(bigEdge, edgeIndex, reverseDir, edgeNormal, margin/3,
                                                      0, pd1, pd2, valid);
                    if (!valid) continue;
                    std::cout << "added bigEdge constraint" << std::endl;
                    ConnectionConstraint cc;
                    cc.edge = std::move(edge2d);
                    cc.inside = false;
                    cc.useInCurveFit = false;
//                    cc.startGuideExtention = margin;
//                    cc.endGuideExtension = margin;
//                    cc.connection = e;
//                    cc.contactID = -1;
                    getShape(partIdx1).connectionConstraints[partIdx2].push_back(std::move(cc));
                }
            }
        }

        std::cout << "adding constraints from connections" << std::endl;
        for (boost::tie(ei, eb) = out_edges(v, g); ei != eb; ++ei) {
            Edge e = *ei;
            bool reversed = g[e].part2 == index[v];
            Vertex v2 = reversed ? vertex(g[e].part1, g) : vertex(
                    g[e].part2, g);
            size_t partIdx2 = g[v2].partIdx;
            PartData &pd2 = partData[partIdx2];
            if (g[e].type == ConnectionEdge::CET_CUT_TO_FACE ||
                g[e].type == ConnectionEdge::CET_CORNER) {
                std::cout << "adding constraints from " << partIdx1 <<'-' << partIdx2 << std::endl;
                const MultiRay3d &innerEdge = g[e].innerEdge;
                //if this part's cut is attached to the other part's face
                if (g[e].part1 == index[v] || g[e].type == ConnectionEdge::CET_CORNER) {
                    /** distance to shift the constraint edge outwards */
                    double constraintOffset = 0;
                    /** normal of the constraint plane (outwards from part 1) */
                    Vector3d surfaceNormal = pd2.normal();
                    if (reversed) {
                        if (g[e].backface1) {
                            surfaceNormal = -surfaceNormal;
                        }
                        constraintOffset = getStock(partIdx2).thickness;
                        std::cout << "shifting " << partIdx1 << " constraint from " << partIdx2 << " by " << constraintOffset << std::endl;
                    } else if (!g[e].backface2) {
                        surfaceNormal = -surfaceNormal;
                    }
                    //"left" tangent of outward surfaceNormal should point ccw along boundary
                    Vector2d projectedNormal = pd1.projectDir(surfaceNormal).normalized();
                    Vector2d tangent(-projectedNormal.y(), projectedNormal.x());
                    Vector2d edgeDir2d = pd1.projectDir(innerEdge.d).normalized();
                    Vector2d edgeNormal(edgeDir2d.y(), -edgeDir2d.x());
                    bool reverseDir = false;
                    if (pd1.projectDir(innerEdge.d).dot(tangent) < 0) {
                        edgeNormal = -edgeNormal;
                        reverseDir = true;
                    }
                    std::cout << "checking " << innerEdge.ranges.size() << " edges" << std::endl;
                    for (size_t edgeIndex=0; edgeIndex < innerEdge.ranges.size(); ++edgeIndex) {
                        bool valid;
                        Edge2d edge2d = getConstraintEdge(innerEdge, edgeIndex, reverseDir, edgeNormal, margin, constraintOffset, pd1, pd2, valid);
                        if (!valid) continue;
                        std::cout << "added regular connection constraint" << std::endl;
                        ConnectionConstraint cc;
                        cc.edge = std::move(edge2d);
                        cc.outside = false;
//                        cc.startGuideExtention = margin;
//                        cc.endGuideExtension = margin;
//                        cc.connection = e;
//                        cc.contactID = edgeIndex;
                        getShape(partIdx1).connectionConstraints[partIdx2].push_back(std::move(cc));
                    }
                }
            } else if (g[e].type == ConnectionEdge::CET_CUT_TO_CUT) {
                std::cout << "adding cut cut constraints between parts " << partIdx1 << " and " << partIdx2 << std::endl;
                ConnectionConstraint cc;
                cc.margin = margin;
                for (const auto &interface : g[e].interfaces) {
                    {
                        ConnectionConstraint cc1 = cc;
                        Vector2d interfaceDirProj = -pd1.projectDir(interface.d); //points outwards from part 1
                        if (reversed) interfaceDirProj = -interfaceDirProj;
                        Vector2d ptA = pd1.project(interface.o.transpose()).transpose();
                        Vector2d n2proj = pd1.projectDir(pd2.normal());
                        Vector2d ptB = ptA - n2proj * getStock(partIdx2).thickness;
                        Vector2d dir1 = ptB - ptA;
                        Vector2d n(dir1.y(), -dir1.x()); //right facing normal should be outwards according to interfaceDir
                        cc1.edge = Edge2d(ptA, ptB);
                        if (n.dot(interfaceDirProj) < 0) {
                            std::swap(cc1.edge.first, cc1.edge.second);
                        }
                        getShape(partIdx1).connectionConstraints[partIdx2].push_back(std::move(cc1));
                    }
                    {
                        ConnectionConstraint cc2 = cc;
                        Vector2d interfaceDirProj = pd2.projectDir(interface.d); //points outwards from part 1
                        if (reversed) interfaceDirProj = -interfaceDirProj;
                        Vector2d ptA = pd2.project(interface.o.transpose()).transpose();
                        Vector2d n1proj = pd2.projectDir(pd1.normal());
                        Vector2d ptB = ptA - n1proj * getStock(partIdx1).thickness;
                        Vector2d dir2 = ptB - ptA;
                        Vector2d n(dir2.y(), -dir2.x()); //right facing normal should be outwards according to interfaceDir
                        cc2.edge = Edge2d(ptA, ptB);
                        if (n.dot(interfaceDirProj) < 0) {
                            std::swap(cc2.edge.first, cc2.edge.second);
                        }
                        getShape(partIdx2).connectionConstraints[partIdx1].push_back(std::move(cc2));
                    }
                }
            }
        }
        std::cout << "adding guides from shape constraints" << std::endl;
        //add guides from shape constraints
        for (const auto &pair : getShape(partIdx1).shapeConstraints) {
            Vector2d n = (pair.second[0].edge.second - pair.second[0].edge.first).normalized();
            for (auto &ci : pair.second) {
                Guide guide;
                guide.edge = Edge2d(ci.edge.first - n * margin, ci.edge.second + n * margin);
                getShape(partIdx1).guides[pair.first].push_back(std::move(guide));
            }
        }
        //Stitch together guides for constraints from parts that are connected
        std::cout << "stitching together guides" << std::endl;
        for (auto &pair1 : getShape(partIdx1).connectionConstraints) {
            std::cout << "checking constraint from " << pair1.first << std::endl;
            auto v1p = partIdxToVertex(pair1.first);
            if (!v1p.second) {
                std::cout << "warning: invalid part ID for construction " << pair1.first << std::endl;
                continue;
            }
            if (v1p.second && !pair1.second.empty()) {
                //add guides from connection constraints and intersect with transitive connections to extend corners
                Ray2d ray1(Edge2d(pair1.second[0].edge.first, pair1.second[0].edge.second));
                double t1min = std::numeric_limits<double>::max();
                double t1max = std::numeric_limits<double>::lowest();
                for (auto & ci : pair1.second) {
                    t1min = std::min(t1min, ray1.project(ci.edge.first.transpose())(0));
                    t1max = std::max(t1max, ray1.project(ci.edge.second.transpose())(0));
                }
                ray1.start = t1min - margin;
                ray1.end = t1max + margin;

                std::cout << "checking for intersecting constraints from parts connected to " << pair1.first << std::endl;
                for (const auto &pair2 : getShape(partIdx1).connectionConstraints) {
                    if (pair1.first != pair2.first) {
                        auto v2p = partIdxToVertex(pair2.first);
                        std::cout << "checked for existing node: " << v2p.second << std::endl;
                        if (v2p.second && !pair2.second.empty()) {
                            auto edgepair = edge(v1p.first, v2p.first, g);
                            std::cout << "checked for edge: " << edgepair.second << std::endl;
                            if (edgepair.second && (g[edgepair.first].type == ConnectionEdge::CET_CORNER ||
                                                    g[edgepair.first].type == ConnectionEdge::CET_CUT_TO_FACE)) {
                                Ray2d ray2(Edge2d(pair2.second[0].edge.first, pair2.second[0].edge.second));
                                std::cout << "intersecting rays" << std::endl;
                                bool changed = guideEdge(ray1, ray2);
                                std::cout << "intersected" << std::endl;
                                if (changed) {
                                    std::cout << "Extending constraint from " << pair1.first << " to constraint "
                                              << pair2.first << " on part " << partIdx1 << std::endl;
                                }
                            }
                        }
                    }
                }
                std::cout << "ray1.start: " << ray1.start << ", ray1.end: " << ray1.end << std::endl;
                Guide guide;
                guide.edge = Edge2d(ray1.sample(ray1.start), ray1.sample(ray1.end));
                std::cout << "setting guide endpoint to " << guide.edge.first.transpose() << ", " << guide.edge.second.transpose() << std::endl;
                std::cout << "adding guide to index " << pair1.first << std::endl;
                getShape(partIdx1).guides[pair1.first].push_back(guide);
                std::cout << "pushed back" << std::endl;
            } else {
                std::cout << "warning: invalid part ID or empty constraint set for part " << pair1.first << " on part " << partIdx1 << std::endl;
            }
        }
    }
}

void Construction::pruneShapeConstraints() {
    size_t N = num_vertices(g);
    for (size_t i=0; i<N; ++i) {
        Vertex v = vertex(i, g);
        size_t partIdx1 = g[v].partIdx;
        const auto &connectionConstraints = getShape(partIdx1).connectionConstraints;
        auto &shapeConstraints = getShape(partIdx1).shapeConstraints;
        std::vector<size_t> removedIndices;
        for (const auto &sc : shapeConstraints) {
            if (connectionConstraints.find(sc.first) != connectionConstraints.end()) removedIndices.push_back(sc.first);
            else if (sc.first >= 0 && sc.first < partData.size()) {
                const auto &otherConnectionConstraints = getShape(sc.first).connectionConstraints;
                if (otherConnectionConstraints.find(partIdx1) != otherConnectionConstraints.end()) removedIndices.push_back(sc.first);
            }
        }
        for (auto ind : removedIndices) {
            shapeConstraints.erase(ind);
        }
    }
}

void Construction::recenter() {
    for (size_t c=0; c<partData.size(); ++c) {
        auto &pd = partData[c];
        auto &sd = getShape(c);
        Vector2d newCenter = getShape(c).cutPath->points().colwise().minCoeff().transpose();
        Vector3d shift = pd.rot * Vector3d(newCenter.x(), newCenter.y(), 0);
        pd.pos += shift;
        if (sd.cutPath)
            sd.cutPath->transform(-newCenter, 0, 1);
//        if (sd.bbox)
//            sd.bbox->transform(-newCenter, 0, 1);
//        if (sd.pointDensityContour)
//            sd.pointDensityContour->transform(-newCenter, 0, 1);
    }
}

void Construction::rotateParts() {
    vertex_iter vi, vend;
//    out_edge_iter ei, eb;

    for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
        Vertex v = *vi;
        auto partIdx = g[v].partIdx;
        auto &shape = getShape(partIdx);
        double ang = 0;
        for (const auto &pair : shape.connectionConstraints) {
            //TODO: use smallest bbox one
            if (pair.second.empty()) continue;
            Vector2d dir = pair.second.front().edge.second - pair.second.front().edge.first;
            ang = std::atan2(dir.y(), dir.x());
            break;
        }
        if (ang != 0) {
            //transform all local coordinate objects
            shape.cutPath->transform(Vector2d(0, 0), -ang, 1);
            Rotation2D rot(-ang);
            for (auto &pair : shape.connectionConstraints) {
                for (auto &cc : pair.second) {
                    cc.edge.first = rot * cc.edge.first;
                    cc.edge.second = rot * cc.edge.second;
                }
            }
            for (auto &pair : shape.shapeConstraints) {
                for (auto &cc : pair.second) {
                    cc.edge.first = rot * cc.edge.first;
                    cc.edge.second = rot * cc.edge.second;
                }
            }
            for (auto &pair : shape.guides) {
                for (auto &cc : pair.second) {
                    cc.edge.first = rot * cc.edge.first;
                    cc.edge.second = rot * cc.edge.second;
                }
            }

            //transform frame
            Matrix2d mat2 = Rotation2D(ang).matrix();
            Matrix3d mat3 = Matrix3d::Identity();
            mat3.block<2, 2>(0, 0) = mat2;
            Quaterniond rotLocal(mat3);
            partData[partIdx].rot = partData[partIdx].rot * rotLocal;
        }
    }
}

void Construction::scale(double scale) {
    for (auto &pd : partData) {
        pd.pos *= scale;
    }
    for (auto &sd : shapeData) {
        if (sd.cutPath)
            sd.cutPath->transform(Vector2d(0, 0), 0, scale);
//        if (sd.bbox)
//            sd.bbox->transform(Vector2d(0, 0), 0, scale);
//        if (sd.pointDensityContour)
//            sd.pointDensityContour->transform(Vector2d(0, 0), 0, scale);
    }
    for (auto &stock : stockData) {
        stock.thickness *= scale;
    }
}

void Construction::rotate(const Eigen::Quaterniond &rot) {
    for (auto &pd : partData) {
        pd.pos = rot * pd.pos;
        pd.rot = rot * pd.rot;
    }
}

std::pair<Construction::Vertex, bool> Construction::partIdxToVertex(size_t partIdx) const {
    vertex_iter vi, vend;
    for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
        Vertex v = *vi;
        if (g[v].partIdx == partIdx) {
            return {v, true};
        }
    }
    return {Construction::Vertex(), false};
}


/**
 * \file WhiteNoiseGeneratorFilter.cpp
 */

#include "WhiteNoiseGeneratorFilter.h"

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename DataType_>
  WhiteNoiseGeneratorFilter<DataType_>::WhiteNoiseGeneratorFilter()
  :Parent(0, 1)
  {
  }

  template<typename DataType_>
  void WhiteNoiseGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
    dist.param(typename boost::random::uniform_real_distribution<DataType_>::param_type(static_cast<DataType>(offset - volume), static_cast<DataType>(offset + volume)));
  }

  template<typename DataType_>
  DataType_ WhiteNoiseGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void WhiteNoiseGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
    dist.param(typename boost::random::uniform_real_distribution<DataType_>::param_type(static_cast<DataType>(offset - volume), static_cast<DataType>(offset + volume)));
  }

  template<typename DataType_>
  DataType_ WhiteNoiseGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void WhiteNoiseGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    DataType* ATK_RESTRICT output = outputs[0];
    for(gsl::index i = 0; i < size; ++i)
    {
      output[i] = dist(gen);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class WhiteNoiseGeneratorFilter<float>;
#endif
  template class WhiteNoiseGeneratorFilter<double>;
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-predicates

    This function object returns  @ref True or @ref False according x is less than y or not.
    Infix notation can be used with operator '<'.

    @par Header <boost/simd/function/is_less.hpp>

    @par Note

     Using `is_less(x,y)` is equivalent to `x < y`

    @par Example:

      @snippet is_less.cpp is_less

    @par Possible output:

      @snippet is_less.txt is_less

  **/
  as_logical_t<Value> is_less(Value const& x, Value const& y);
} }
#endif

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

#endif


/* Copyright (c) 2020 The Neuropod Authors

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#pragma once

// Utilities used to access data in neuropod tensor objects as Eigen library Matrices and Vectors

#include "neuropod/internal/error_utils_header_only.hh"
#include "neuropod/internal/neuropod_tensor.hh"

#include <Eigen/Dense>

namespace neuropod
{

// Wraps neuropod tensor memory with Eigen matrix
template <typename _Scalar>
Eigen::Map<const Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> as_eigen(
    const neuropod::TypedNeuropodTensor<_Scalar> &tensor)
{
    const auto &dims = tensor.get_dims();

    if (dims.size() > 2)
    {
        NEUROPOD_ERROR_HH("Only tensors with rank of 1 or 2 are supported by this function. "
                          "Tensor has rank of {}.",
                          dims.size());
    }

    const auto rows = dims[0];
    const auto cols = dims.size() == 2 ? dims[1] : 1;

    return Eigen::Map<const Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(
        tensor.get_raw_data_ptr(), rows, cols);
}

// Wraps neuropod tensor memory with Eigen matrix
template <typename _Scalar>
Eigen::Map<Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> as_eigen(
    neuropod::TypedNeuropodTensor<_Scalar> &tensor)
{
    const auto &dims = tensor.get_dims();

    if (dims.size() > 2)
    {
        NEUROPOD_ERROR_HH("Only tensors with rank of 1 or 2 are supported by this function. "
                          "Tensor has rank of {}.",
                          dims.size());
    }

    const auto rows = dims[0];
    const auto cols = dims.size() == 2 ? dims[1] : 1;

    return Eigen::Map<Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(
        tensor.get_raw_data_ptr(), rows, cols);
}

// Wraps neuropod tensor memory with Eigen matrix
template <typename _Scalar>
Eigen::Map<const Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> as_eigen(
    const neuropod::NeuropodTensor &tensor)
{
    return as_eigen<_Scalar>(*tensor.as_typed_tensor<_Scalar>());
}

// Wraps neuropod tensor memory with Eigen matrix
template <typename _Scalar>
Eigen::Map<Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> as_eigen(
    neuropod::NeuropodTensor &tensor)
{
    return as_eigen<_Scalar>(*tensor.as_typed_tensor<_Scalar>());
}

} // namespace neuropod


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef TEST_SUPPORT_NUMERIC_HPP
#define TEST_SUPPORT_NUMERIC_HPP

#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/eval.hpp>
#include <boost/hana/fwd/div.hpp>
#include <boost/hana/fwd/equal.hpp>
#include <boost/hana/fwd/eval_if.hpp>
#include <boost/hana/fwd/less.hpp>
#include <boost/hana/fwd/minus.hpp>
#include <boost/hana/fwd/mod.hpp>
#include <boost/hana/fwd/mult.hpp>
#include <boost/hana/fwd/negate.hpp>
#include <boost/hana/fwd/not.hpp>
#include <boost/hana/fwd/one.hpp>
#include <boost/hana/fwd/plus.hpp>
#include <boost/hana/fwd/while.hpp>
#include <boost/hana/fwd/zero.hpp>

struct numeric_type {
    constexpr explicit numeric_type(int v) : value(v) { }
    int value;
    constexpr operator int() const { return value; }
};

using Numeric = boost::hana::tag_of_t<numeric_type>;

struct numeric_t {
    constexpr numeric_type operator()(int x) const {
        return numeric_type{x};
    }
};
constexpr numeric_t numeric{};

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return numeric(x.value == y.value); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct less_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y) {
            // Workaround a _weird_ GCC bug:
            // error: parse error in template argument list
            //      bool cmp = (x.value < y.value);
            //                    ^
            int xv = x.value, yv = y.value;
            return numeric(xv < yv);
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct eval_if_impl<Numeric> {
        template <typename C, typename T, typename E>
        static constexpr auto apply(C const& c, T&& t, E&& e) {
            return c.value ? hana::eval(static_cast<T&&>(t))
                           : hana::eval(static_cast<E&&>(e));
        }
    };

    template <>
    struct not_impl<Numeric> {
        template <typename X>
        static constexpr auto apply(X x)
        { return numeric(!x.value); }
    };

    template <>
    struct while_impl<Numeric> {
        template <typename Pred, typename State, typename F>
        static constexpr auto apply(Pred pred, State state, F f)
            -> decltype(true ? f(state) : state)
        {
            if (pred(state))
                return hana::while_(pred, f(state), f);
            else
                return state;
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct plus_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return numeric(x.value + y.value); }
    };

    template <>
    struct zero_impl<Numeric> {
        static constexpr auto apply()
        { return numeric(0); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Group
    //
    // Define either one to select which MCD is used:
    //  BOOST_HANA_TEST_GROUP_NEGATE_MCD
    //  BOOST_HANA_TEST_GROUP_MINUS_MCD
    //
    // If neither is defined, the MCD used is unspecified.
    //////////////////////////////////////////////////////////////////////////
#if defined(BOOST_HANA_TEST_GROUP_NEGATE_MCD)
    template <>
    struct negate_impl<Numeric> {
        template <typename X>
        static constexpr auto apply(X x)
        { return numeric(-x.value); }
    };
#else
    template <>
    struct minus_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return numeric(x.value - y.value); }
    };
#endif

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct mult_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return numeric(x.value * y.value); }
    };

    template <>
    struct one_impl<Numeric> {
        static constexpr auto apply()
        { return numeric(1); }
    };

    //////////////////////////////////////////////////////////////////////////
    // EuclideanRing
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct div_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return numeric(x.value / y.value); }
    };

    template <>
    struct mod_impl<Numeric, Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return numeric(x.value % y.value); }
    };
}} // end namespace boost::hana

#endif //! TEST_SUPPORT_NUMERIC_HPP


#include "fbstab/fbstab_dense.h"

#include <Eigen/Dense>
#include <memory>
#include <stdexcept>

#include "fbstab/components/dense_cholesky_solver.h"
#include "fbstab/components/dense_data.h"
#include "fbstab/components/full_feasibility.h"
#include "fbstab/components/full_residual.h"
#include "fbstab/components/full_variable.h"
#include "fbstab/fbstab_algorithm.h"
#include "tools/output_stream.h"
#include "tools/utilities.h"

namespace fbstab {

FBstabDense::FBstabDense(int nz, int nl, int nv) {
  if (nz <= 0 || nv <= 0 || nl < 0) {
    throw std::runtime_error(
        "In FBstabDense::FBstabDense: nz and nv must be positive, nl "
        "nonnegative");
  }
  nz_ = nz;
  nl_ = nl;
  nv_ = nv;

  x1_ = tools::make_unique<FullVariable>(nz_, nl_, nv_);
  x2_ = tools::make_unique<FullVariable>(nz_, nl_, nv_);
  x3_ = tools::make_unique<FullVariable>(nz_, nl_, nv_);
  x4_ = tools::make_unique<FullVariable>(nz_, nl_, nv_);
  r1_ = tools::make_unique<FullResidual>(nz_, nl_, nv_);
  r2_ = tools::make_unique<FullResidual>(nz_, nl_, nv_);
  feasibility_checker_ = tools::make_unique<FullFeasibility>(nz_, nl_, nv_);
  linear_solver_ = tools::make_unique<DenseCholeskySolver>(nz_, nl_, nv_);

  algorithm_ = tools::make_unique<FBstabAlgoDense>(
      x1_.get(), x2_.get(), x3_.get(), x4_.get(), r1_.get(), r2_.get(),
      linear_solver_.get(), feasibility_checker_.get());

  opts_ = DefaultOptions();
}

void FBstabDense::UpdateOptions(const Options& options) {
  // No need to validate since there are no additional options and the algorithm
  // will check the algorithmic ones.
  algorithm_->UpdateParameters(&options);
}

FBstabDense::Options FBstabDense::DefaultOptions() {
  Options opts;
  opts.DefaultParameters();
  return opts;
}

FBstabDense::Options FBstabDense::ReliableOptions() {
  Options opts;
  opts.ReliableParameters();
  return opts;
}

FBstabDense::ProblemData::ProblemData(int nz, int nl, int nv) {
  H.resize(nz, nz);
  G.resize(nl, nz);
  A.resize(nv, nz);
  f.resize(nz);
  h.resize(nl);
  b.resize(nv);
}

FBstabDense::ProblemDataRef::ProblemDataRef(
    const Eigen::Map<Eigen::MatrixXd>* H_,
    const Eigen::Map<Eigen::VectorXd>* f_,
    const Eigen::Map<Eigen::MatrixXd>* G_,
    const Eigen::Map<Eigen::VectorXd>* h_,
    const Eigen::Map<Eigen::MatrixXd>* A_,
    const Eigen::Map<Eigen::VectorXd>* b_)
    : H(H_->data(), H_->rows(), H_->cols()),
      G(G_->data(), G_->rows(), G_->cols()),
      A(A_->data(), A_->rows(), A_->cols()),
      f(f_->data(), f_->size()),
      h(h_->data(), h_->size()),
      b(b_->data(), b_->size()) {}

FBstabDense::Variable::Variable(int nz, int nl, int nv) {
  z = Eigen::VectorXd::Zero(nz);
  l = Eigen::VectorXd::Zero(nl);
  v = Eigen::VectorXd::Zero(nv);
  y = Eigen::VectorXd::Zero(nv);
}

FBstabDense::VariableRef::VariableRef(Eigen::Map<Eigen::VectorXd>* z_,
                                      Eigen::Map<Eigen::VectorXd>* l_,
                                      Eigen::Map<Eigen::VectorXd>* v_,
                                      Eigen::Map<Eigen::VectorXd>* y_)
    : z(z_->data(), z_->size()),
      l(l_->data(), l_->size()),
      v(v_->data(), v_->size()),
      y(y_->data(), y_->size()) {}

void FBstabDense::VariableRef::fill(double a) {
  z.fill(a);
  l.fill(a);
  v.fill(a);
  y.fill(a);
}

// Explicit instantiation.
template class FBstabAlgorithm<FullVariable, FullResidual, DenseCholeskySolver,
                               FullFeasibility>;

}  // namespace fbstab


#include <iostream>
#include <vector>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

typedef boost::numeric::ublas::vector<float> Vector3;

int main()
{
	std::vector< std::vector<Vector3> > vertexContainer;

	for (int j = 0; j < 4; ++j) {
		std::vector<Vector3> container;
		for (int i = 0; i < 12; ++i) {
			Vector3 vector(3);
			vector(0) = vector(1) = vector(2) = i;
			container.push_back(vector);
		}
		vertexContainer.push_back(container);
	}

	for (int i = 0; i < vertexContainer.size(); ++i) {
		for (int j = 0; j < vertexContainer[i].size(); ++j) {
			std::cout << vertexContainer[i][j++] << std::endl;
			std::cout << vertexContainer[i][j++] << std::endl;
			std::cout << vertexContainer[i][j] << std::endl;
		}
	}

	vertexContainer.erase();

	std::cout << vertexContainer.remove() << std::endl;

	return 0;
}