#include "Topology.h"
#include "Joint.h"
#include "JointPool.h"
#include "SmallVector.h"
#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/math/constants/constants.hpp>
#include <fmt/format.h>
#include <smartmet/macgyver/Exception.h>

#if 0
#include <iostream>
#endif

namespace Trax
{
// Geometry types for r-tree searches
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
using bg_point = bg::model::point<double, 2, bg::cs::cartesian>;
using bg_box = bg::model::box<bg_point>;
using bg_value = std::pair<bg_box, Polygons::iterator>;
using bg_rtree = bgi::rtree<bg_value, bgi::rstar<16>>;  // R* variant

// We may sort polylines base on the head coordinate or the tail coordinate. Head sort
// is for appending to a growing polyline, tail for prepending. Appending is sufficient
// for isobands since the rings always close. Prepending may be needed to grow isolines
// to their maximum possible length.

struct JoinCoordinate
{
  double x;
  double y;

  bool operator<(const JoinCoordinate& other) const
  {
    if (x != other.x)
      return (x < other.x);
    return (y < other.y);
  }
};

using Duplicates = SmallVector<Joint*, 8UL>;

using SortedPolylines = std::multimap<JoinCoordinate, Polyline>;

using JoinCandidate = SortedPolylines::iterator;
using JoinCandidates = std::list<JoinCandidate>;

namespace
{
double turn_angle(double angle1, double angle2)
{
  // max turn left > -180 and max turn right < 180
  auto turn = fmod(angle1 - angle2, 360.0);
  if (turn < -180)
    turn += 360;
  else if (turn > 180)
    turn -= 360;
  return turn;
}

inline bool has_duplicates(Joint* j)
{
  return (j->alt != nullptr);
}

// Find joints with the same vertex (including the joint itself)
Duplicates get_duplicates(Joint* j)
{
  Duplicates ret;
  ret.push_back(j);

  auto* i = j;
  while ((i = i->alt) != j)  // push until reached j again
    ret.push_back(i);

  return ret;
}

JoinCandidates find_append_candidates(const Polyline& polyline, SortedPolylines& polylines)
{
  JoinCandidates result;

  const auto x = polyline.xend();
  const auto y = polyline.yend();

  auto it = polylines.find(JoinCoordinate{x, y});

  for (auto end = polylines.end(); it != end; ++it)
  {
    const auto& coord = it->first;
    if (x == coord.x && y == coord.y)
      result.push_back(it);
    else
      break;
  }
  return result;
}

// Assumes !polylines.empty()
JoinCandidate select_turn(const Polyline& polyline, JoinCandidates& candidates, bool turn_right)
{
  using boost::math::double_constants::radian;

  // std::list::size is slow: if (candidates.size() == 1)
  if (++candidates.begin() == candidates.end())
    return candidates.front();

  // Tail angle of the polyline to append a candidate to
  const auto end_angle = polyline.end_angle();

  // Find best candidate
  auto best_candidate = candidates.front();
  double best_turn = (turn_right ? -999.0 : 999.0);  // anything above 180 will do

#if 0
  std::cout << "Polyline : " << polyline.wkt() << " angle = " << end_angle << "\n";
#endif

  for (auto& candidate : candidates)
  {
    const auto start_angle = candidate->second.start_angle();

#if 0
    std::cout << "\tCandidate angle " << start_angle << " : " << candidate->second.wkt() << "\n";
#endif

    // max turn left > -180 and max turn right < 180
    auto turn = fmod(end_angle - start_angle, 360.0);
    if (turn < -180)
      turn += 360;
    else if (turn > 180)
      turn -= 360;

#if 0
    std::cout << "\tTurn angle = " << turn << "\n";
#endif

    bool better_turn = (turn_right ? turn > best_turn : turn < best_turn);

    if (better_turn)
    {
      best_candidate = candidate;
      best_turn = turn;
    }
  }

  return best_candidate;
}

Joint* select_right_turn(const Vertex& vertex,
                         const Polylines& polylines,
                         const Duplicates& duplicates)
{
  // Select free joint with the rightmost turn

  using boost::math::double_constants::radian;
  const auto angle1 = polylines.back().end_angle();

  Joint* best = nullptr;
  double best_turn = -999;  // possible range is -180...180

  for (auto* j : duplicates)
  {
    if (!j->used)  // Ignore used joints, we only needed them for the num_joints test
    {
      const auto& next_vertex = j->next->vertex;
      const auto angle2 = atan2(next_vertex.y - vertex.y, next_vertex.x - vertex.x) * radian;
      const auto turn = turn_angle(angle1, angle2);
      if (turn > best_turn)
      {
        best = j;
        best_turn = turn;
      }
    }
  }

  if (best_turn == -999)
    throw Fmi::Exception(BCP, fmt::format("All joints already used at {},{}", vertex.x, vertex.y));

  return best;
}

Polylines extract_right_turning_sequence(Joint* joint, bool strict)
{
  Polylines polylines;  // polylines extracted by taking right turns split at double joints
  Polyline polyline;    // grow a polyline until a double joint is encountered

#if 0
  std::cout << "RIGHT\n";
#endif

  while (true)
  {
    const auto& vertex = joint->vertex;

#if 0
    std::cout << fmt::format("\t{},{}\n", vertex.x, vertex.y);
#endif

    polyline.append(vertex);

    // Stop when exterior (and holes attached to it) becomes closed
    if (polylines.empty() && polyline.closed())
    {
      polylines.emplace_back(std::move(polyline));
      return polylines;
    }
    if (!polylines.empty() && polylines.front().xbegin() == polyline.xend() &&
        polylines.front().ybegin() == polyline.yend())
    {
      polylines.emplace_back(std::move(polyline));
      return polylines;
    }

    // Choose rightmost turn. If there are multiple connections, start a new polyline
    // so that we can choose the leftmost turns later on to separate holes touching
    // the exerior.

    if (!has_duplicates(joint))
    {
      // Trivial case of only one possible continuation
      if (joint->used)
      {
        if (polyline.size() == 1)  // collapsed to a corner point, just discard it
          return polylines;
        if (!strict)
        {
          // Ignore the built polyline and try to continue. This problem can happen
          // near the poles when projected coordinates may be inaccurate.
          polylines.clear();
          return polylines;
        }

        throw Fmi::Exception(BCP,
                             fmt::format("Only joint already used at {},{}", vertex.x, vertex.y))
            .addParameter("Column", fmt::format("{}", vertex.column))
            .addParameter("Row", fmt::format("{}", vertex.row))
            .addParameter("Type", to_string(vertex.type));
      }
      joint->used = true;
      joint = joint->next;
    }
    else
    {
      // Start new polyline so we can extract holes later on unless the it contains
      // current vertex only
      if (polyline.size() > 1)
      {
        polylines.emplace_back(std::move(polyline));
        polyline = Polyline{vertex.x, vertex.y};
      }

      // Note that we do not remove the best joint from the sequence of alternatives
      // since eventually we'd lose information on which vertices contain alternatives
      // and hence would not know when to interrupt the right turning sequence. Instead,
      // Joints::duplicates will return used joints too.

      auto duplicates = get_duplicates(joint);
      auto* best = select_right_turn(vertex, polylines, duplicates);
      best->used = true;
      joint = best->next;
    }
  }
}

// Extract a polyline by always taking a left turn
Polyline extract_left_turning_polyline(SortedPolylines& polylines)
{
  // Start from the first available polyline
  auto it = polylines.begin();
  Polyline result = std::move(it->second);
  polylines.erase(it);

  const bool turn_right = false;

  // Note: The first selection may already be a closed hole
  while (!result.closed())
  {
    auto candidates = find_append_candidates(result, polylines);
    if (candidates.empty())
      return result;

    auto candidate = select_turn(result, candidates, turn_right);

    result.append(candidate->second);
    polylines.erase(candidate);
  }
  return result;
}

void extract_left_turning_sequence(Polylines& polylines, Polylines& shells, Holes& holes)
{
  // Quick exit if there is only one polyline
  if (polylines.size() == 1)
  {
    polylines.front().update_bbox();
    if (polylines.front().clockwise())
      shells.emplace_back(std::move(polylines.front()));
    else
      holes.emplace_back(std::move(polylines.front()));
    return;
  }

  // Create a search structure for finding the left turns quickly
  SortedPolylines sorted_polylines;
  for (auto&& polyline : polylines)
  {
    JoinCoordinate xy{polyline.xbegin(), polyline.ybegin()};
    sorted_polylines.insert(std::make_pair(xy, polyline));
  }
  polylines.clear();

  Polylines new_shells;
  Holes new_holes;

  while (!sorted_polylines.empty())
  {
    auto polyline = extract_left_turning_polyline(sorted_polylines);

#if 0
    std::cout << "Left turning polyline: " << polyline.wkt() << "\n";
#endif

    polyline.update_bbox();
    if (polyline.clockwise())
      shells.emplace_back(std::move(polyline));
    else
      holes.emplace_back(std::move(polyline));
  }
}

bg_box make_bg_box(const BBox& bbox)
{
  return bg_box(bg_point(bbox.xmin(), bbox.ymin()), bg_point(bbox.xmax(), bbox.ymax()));
}

bg_rtree build_rtree(Polygons& polygons)
{
  bg_rtree rtree;

  for (auto it = polygons.begin(), end = polygons.end(); it != end; ++it)
    rtree.insert(std::make_pair(make_bg_box(it->exterior().bbox()), it));

  return rtree;
}

std::list<Polygons::iterator> possible_rtree_shells(const bg_rtree& rtree, const Polyline& hole)
{
  auto query_box = make_bg_box(hole.bbox());
  std::vector<bg_value> results;
  rtree.query(bgi::contains(query_box), std::back_inserter(results));

  // Accept only those that contain the hole bbox (should be true for all elements)
  std::list<Polygons::iterator> ret;

  for (const auto& result : results)
    if (result.second->bbox_contains(hole))
      ret.push_back(result.second);

  return ret;
}

std::list<Polygons::iterator> possible_shells(const bg_rtree& rtree, const Polyline& hole)
{
  auto ret = possible_rtree_shells(rtree, hole);

  // We assume all holes have an exterior to avoid an expensive contains() test

#if 0
  if (ret.size() < 2)  // std::list::size is slow
    return ret;
#else
  if (ret.empty())
    return ret;
  if (++ret.begin() == ret.end())
    return ret;
#endif

  // Find ones that actually contain the point

  for (auto it = ret.begin(), end = ret.end(); it != end;)
  {
    if ((*it)->contains(hole))
      ++it;
    else
      it = ret.erase(it);
  }

  return ret;
}

Polygons::iterator innermost_polygon(const std::list<Polygons::iterator>& polygons)
{
  // For valid touching rule polygons the innermost polygon must necessarily have
  // a bbox that is contained by the bbox of all the other available choices since
  // polygons may touch only at a single vertex.

  auto ret = polygons.front();  // first candidate

  for (const auto& it : polygons)
    if (ret != it && ret->exterior().bbox().contains(it->exterior().bbox()))
      ret = it;

  return ret;
}

}  // namespace

/*
 * Build rings by
 *   1. picking the next unused edge from the joints
 *   2. selecting right turn at all branch points
 *   3. until the path closes
 *   4. iterate through the selected vertices but take left turns
 *   5. result will be one clockwise exterior and 0-N holes directly attached
 *      to the exterior or via a chain of holes
 * The process is repeated until all edges have been processed, and the
 * output is polygons and holes which were not in contact with any exterior.
 */

void build_rings(Polylines& shells, Holes& holes, JointPool& joints, bool strict)
{
#if 0
  std::cout << "Joint map at start:\n" << Trax::to_string(joints);
#endif

  for (auto* joint : joints)
  {
    // Skip fully used joints
    if (joint->used)
      continue;

#if 0
    const auto& vertex = joint->vertex;
    std::cout << fmt::format("Start joint:\n\t{},{} --> {}:{},{}\n",
                             vertex.x,
                             vertex.y,
                             joint->used,
                             joint->next->vertex.x,
                             joint->next->vertex.y);
#endif

    // Skip unused multijoints as start points since then we cannot calculate the polyline end angle
    if (!has_duplicates(joint))
    {
      auto polylines = extract_right_turning_sequence(joint, strict);

      if (!polylines.empty())  // the sequence may have been discarded due to a problem
      {
#if 0
        std::cout << "Extracted right turning sequence:\n";
        for (const auto& polyline : polylines)
          std::cout << fmt::format("\t{}\n", polyline.wkt());

        std::cout << "Joint map now:\n" << Trax::to_string(joints);
#endif

        // Now extract a left turning sequence to separate holes touching the exterior
        extract_left_turning_sequence(polylines, shells, holes);
      }
    }
    else
    {
      // std::cout << "Skipping duplicate start point. Duplicates: " << num_joints << "\n";
    }
  }
}

/*
 * Assign holes to polygons utilizing bbox information for speed.
 * If a hole is in multiple polygons, we must choose the innermost
 * polygon to obey topology rules for polygons. Note that in isoline
 * mode there may be unassigned holes if the exterior is cut at the
 * grid borders.
 */

void build_polygons(Polygons& polygons, Polylines& shells, Holes& holes)
{
#if 0
  int counter = 0;
  std::cout << fmt::format(
      "{} : Assigning {} holes to {} polygons\n", ++counter, holes.size(), shells.size());

  auto i = 0UL;
  for (const auto& shell : shells)
    std::cout << "Shell " << i++ << " " << shell.wkt() << "\n\tbbox = " << shell.bbox().wkt()
              << "\n";
  i = 0UL;
  for (const auto& hole : holes)
    std::cout << "Hole " << i++ << " " << hole.wkt() << "\n\tbbox = " << hole.bbox().wkt() << "\n";
  counter = 0;
#endif

  for (auto&& shell : shells)
  {
    if (shell.size() >= 4)  // discard too small shells
      polygons.emplace_back(shell);
  }

  if (holes.empty())
    return;

  // TODO: Should we optimize in case there is just one polygon?

  // Build an R-tree of the polygon exterior bounding boxes for speed

  auto rtree = build_rtree(polygons);

  for (auto it = holes.begin(); it != holes.end();)
  {
    auto& hole = *it;  // shorthand variable

    if (hole.size() < 4)  // discard too small holes
      continue;

    auto candidates = possible_shells(rtree, hole);
#if 0
    std::cout << fmt::format("\t{} candidates for hole {}\n", candidates.size(), counter++);
#endif
    if (candidates.empty())
    {
      ++it;  // should be isolated hole when calculating isolines
    }
    else
    {
      auto polygon = innermost_polygon(candidates);
#if 0
      std::cout << fmt::format("\tInnermost polygon: {}\n", polygon->wkt());
#endif
      polygon->hole(hole);
      it = holes.erase(it);
    }
  }

  // Convert any remaining holes to shells. This can happen when an exterior shell has
  // been stripped of ghost lines when calculating isolines.
  for (auto&& hole : holes)
  {
    hole.reverse();
    polygons.emplace_back(std::move(hole));
  }
}

}  // namespace Trax


#include <iostream>
#include "source/tessellation/geometry.hpp"
#include "source/tessellation/Delaunay.hpp"
#include "source/misc/simple_io.hpp"
#include "source/newtonian/two_dimensional/geometric_outer_boundaries/SquareBox.hpp"
#include "source/misc/mesh_generator.hpp"
#ifdef RICH_MPI
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#endif // RICH_MPI
#include "source/newtonian/two_dimensional/hdf5_diagnostics.hpp"
#include <boost/foreach.hpp>
#include "source/tessellation/VoronoiMesh.hpp"

using namespace std;

#ifdef RICH_MPI
vector<Vector2D> delineate_rectangle
(const pair<Vector2D,Vector2D>& boundary)
{
  vector<Vector2D> res(4);
  res.at(0) = boundary.first;
  res.at(1) = Vector2D(boundary.second.x,boundary.first.y);
  res.at(2) = boundary.second;
  res.at(3) = Vector2D(boundary.first.x,boundary.second.y);
  return res;
}

vector<Vector2D> my_convex_hull
(const Tessellation& tess,
 int index)
{
  vector<Vector2D> res;
  ConvexHull(res,tess,index);
  return res;
}

vector<Vector2D> distribute_grid
(const vector<Vector2D>& complete_grid,
 const Tessellation& proc_tess)
{
  const boost::mpi::communicator world;
  vector<Vector2D> res;
  const vector<Vector2D> ch_list =
    my_convex_hull(proc_tess,world.rank());
  BOOST_FOREACH(const Vector2D& v, complete_grid)
    {
      if(PointInCell(ch_list,v))
	res.push_back(v);
    }
  return res;
}
#endif // RICH_MPI

int main(void)
{
#ifdef RICH_MPI

  boost::mpi::environment env;
  boost::mpi::communicator world;

  try{

  const SquareBox obc
    (Vector2D(0,0),
     Vector2D(1,1));
  const vector<Vector2D> all_points =
    RandSquare(1000,
	       obc.getBoundary().first.x,
	       obc.getBoundary().second.x,
	       obc.getBoundary().first.y,
	       obc.getBoundary().second.y);
  if(world.rank()==0){
    Delaunay tri;
    tri.build_delaunay
      (all_points,
       delineate_rectangle
       (obc.getBoundary()));
    tri.BuildBoundary(&obc, obc.GetBoxEdges());
    WriteDelaunay(tri,string("whole.h5"));
  }
  VoronoiMesh super
    (RandSquare(world.size(),
		obc.getBoundary().first.x,
		obc.getBoundary().second.x,
		obc.getBoundary().first.y,
		obc.getBoundary().second.y),
     obc);
  const vector<Vector2D> local_points =
    distribute_grid
    (all_points,
     super);
  Delaunay tri;
  tri.build_delaunay
    (local_points,
     delineate_rectangle
     (obc.getBoundary()));
  vector<vector<int> > dummy_1;
  //vector<int> dummy_2;
  tri.BuildBoundary
    (&obc,
     super,
     dummy_1);
  WriteDelaunay(tri,string("part_"+int2str(world.rank())+".h5"));
  }
  catch(const UniversalError& eo){
    cout << eo.GetErrorMessage() << endl;
    throw;
  }
#else

  write_number(0,"serial_ignore.txt");

#endif // RICH_MPI
  return 0;
}


#include <sm/eigen/gtest.hpp>

#include <numeric>

#include "SampleDvAndError.hpp"

#include <aslam/backend/BlockCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/DenseQrLinearSystemSolver.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/Optimizer2.hpp>
#include <aslam/backend/SparseCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/SparseQrLinearSystemSolver.hpp>
#include <boost/lexical_cast.hpp>

using namespace aslam::backend;

template <typename S1_TYPE, typename S2_TYPE>
void compareSolvers(int D, int E, bool useM, bool useDiag, int nThreads) {
    std::string S1Name = typeid(S1_TYPE).name();
    std::string S2Name = typeid(S2_TYPE).name();
    SCOPED_TRACE(("Comparing S1: " + S1Name + " and S2: " + S2Name).c_str());
    std::vector<DesignVariable*> dvs;
    std::vector<ErrorTerm*> errs;
    try {
        buildSystem(D, E, dvs, errs);
        S1_TYPE S1;
        S1.initMatrixStructure(dvs, errs, useDiag);
        S2_TYPE S2;
        S2.initMatrixStructure(dvs, errs, useDiag);
        ASSERT_EQ(S1.JRows(), S2.JRows());
        ASSERT_EQ(S1.JCols(), S2.JCols());
        Eigen::VectorXd diag(S1.JCols());
        diag.setRandom();
        if (useDiag) {
            S1.setConditioner(diag);
            S2.setConditioner(diag);
        }
        Eigen::VectorXd dxS1, dxS2, eS2, eS1, rhsS2, rhsS1;
        double e2S1, e2S2;
        e2S1 = S1.evaluateError(nThreads, useM);
        e2S2 = S2.evaluateError(nThreads, useM);
        ASSERT_NEAR(e2S1, e2S2, 1e-9);
        eS2 = S2.e();
        eS1 = S1.e();
        ASSERT_DOUBLE_MX_EQ(eS2, eS1, 1e-6, "Checking the error vectors");
        ASSERT_EQ((size_t)S2.e().size(), S2.JRows());
        ASSERT_EQ((size_t)S1.e().size(), S1.JRows());
        S1.buildSystem(nThreads, useM);
        S2.buildSystem(nThreads, useM);
        rhsS1 = S1.rhs();
        rhsS2 = S2.rhs();
        ASSERT_DOUBLE_MX_EQ(rhsS1, rhsS2, 1e-6, "Checking right-hand sides");
        S1.solveSystem(dxS1);
        S2.solveSystem(dxS2);
        ASSERT_DOUBLE_MX_EQ(dxS2, dxS1, 1e-6, "Checking the solutions");
        deleteSystem(dvs, errs);
    } catch (const std::exception& e) {
        deleteSystem(dvs, errs);
        FAIL() << e.what();
    }
}
/*
TEST(LinearSolverTestSuite, testDenseQr)
{
   using namespace aslam::backend;
   std::vector<DesignVariable *> dvs;
   std::vector<ErrorTerm *> errs;
   const int D = 4;
   const int E = 20;
   const bool useM = false;
   bool useDiag = true;

   for(int nThreads = 0; nThreads < 16; ++nThreads)
   {
       {
           useDiag = false;
           SCOPED_TRACE(("No Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
           compareSolvers<BlockCholeskyLinearSystemSolver, DenseQrLinearSystemSolver>(D,E,useM, useDiag, nThreads);
       }

       {
           useDiag = true;
           SCOPED_TRACE(("With Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
           compareSolvers<BlockCholeskyLinearSystemSolver, DenseQrLinearSystemSolver>(D,E,useM, useDiag, nThreads);
       }
   }

}
*/

TEST(LinearSolverTestSuite, testSparseCholesky) {
    using namespace aslam::backend;
    std::vector<DesignVariable*> dvs;
    std::vector<ErrorTerm*> errs;
    const int D = 4;
    const int E = 20;
    const bool useM = false;
    bool useDiag = true;
    for (int nThreads = 0; nThreads < 4; ++nThreads) {
        {
            useDiag = false;
            SCOPED_TRACE(("No Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
            compareSolvers<SparseCholeskyLinearSystemSolver, BlockCholeskyLinearSystemSolver>(D, E, useM, useDiag,
                                                                                nThreads);
        }
        {
            useDiag = true;
            SCOPED_TRACE(("With Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
            compareSolvers<SparseCholeskyLinearSystemSolver, BlockCholeskyLinearSystemSolver>(D, E, useM, useDiag,
                                                                                nThreads);
        }
    }
}

TEST(LinearSolverTestSuite, testSparseQR) {
    using namespace aslam::backend;
    std::vector<DesignVariable*> dvs;
    std::vector<ErrorTerm*> errs;
    const int D = 4;
    const int E = 20;
    const bool useM = false;
    bool useDiag = false;
    for (int nThreads = 0; nThreads < 4; ++nThreads) {
        {
            useDiag = false;
            SCOPED_TRACE(("No Diagonal and " + boost::lexical_cast<std::string>(nThreads) + " threads").c_str());
            compareSolvers<SparseCholeskyLinearSystemSolver, SparseQrLinearSystemSolver>(D, E, useM, useDiag, nThreads);
        }
        /*
                {
                    useDiag = true;
                    SCOPED_TRACE(("With Diagonal and " + boost::lexical_cast<std::string>(nThreads) + "
           threads").c_str()); compareSolvers<SparseCholeskyLinearSystemSolver, SparseQrLinearSystemSolver>(D,E,useM,
           useDiag, nThreads);
                }*/
    }
}

/*
TEST(LinearSolverTestSuite, testOptimizerLevenbergMarquardt)
{
  using namespace aslam::backend;
  std::vector<DesignVariable*> dvs;
  std::vector<ErrorTerm*> errs;
  const int D = 4;
  const int E = 20;
  const int seed = 1;
  try {
    std::string baseline_solver = "block_cholesky";
    std::string baseline_policy = "LevenbergMarquardt";
    std::vector<std::string> solvers;
    std::vector<std::string> policies;
    solvers.push_back("sparse_cholesky");
    solvers.push_back("dense_qr");

    policies.push_back("LevenbergMarquardt");
    policies.push_back("DogLeg");
    policies.push_back("GaussNewton");

    boost::shared_ptr<OptimizationProblem> pb = buildProblem(seed, D, E);
    std::vector< boost::shared_ptr<OptimizationProblem> > problems;
    for (size_t i = 0; i < solvers.size()*policies.size(); ++i) {
      boost::shared_ptr<OptimizationProblem> pi = buildProblem(seed, D, E);
      problems.push_back(pi);
      for (size_t j = 0; j < pb->numErrorTerms(); ++j) {
        double eb = pb->errorTerm(j)->evaluateError();
        double ei = pi->errorTerm(j)->evaluateError();
        ASSERT_EQ(ei, eb) << "The problems are not the same";
        //std::cout << "ei: " << ei << ", eb: " << eb << std::endl;
      }
    }
    Optimizer2Options options;
    options.linearSolver = baseline_solver;
    options.maxIterations = 5;
    options.verbose = true;
    options.trustRegionPolicy = baseline_policy;
    Optimizer2 optimizer(options);
    optimizer.setProblem(pb);
    optimizer.optimize();
      int p = 0;
      for(size_t k = 0; k < policies.size(); ++k) {
        for (size_t i = 0; i < solvers.size(); ++i) {
          options.linearSolver = solvers[i];
          options.trustRegionPolicy = policies[k];
          Optimizer2 optimizer_i(options);
          optimizer_i.setProblem(problems[p]);
          p++;
          optimizer_i.optimize();
          for (size_t j = 0; j < pb->numErrorTerms(); ++j) {
            double eb = pb->errorTerm(j)->evaluateError();
            double ei = problems[i*k]->errorTerm(j)->evaluateError();
            ASSERT_NEAR(ei, eb, 1e-6) << "The errors did not reduce in the same way";
            //std::cout << "ei: " << ei << ", eb: " << eb << std::endl;
          }
        }
      }
  } catch (const std::exception& e) {
    deleteSystem(dvs, errs);
    FAIL() << e.what();
  }
}

*/

/*
TEST(LinearSolverTestSuite, testXXX)
{
using namespace aslam::backend;
    std::vector<DesignVariable *> dvs;
    std::vector<ErrorTerm *> errs;
    const int D = 4;
    const int E = 20;
    try {
        buildSystem(D, E, dvs, errs);

        deleteSystem(dvs, errs);
    }
    catch(const std::exception & e)
    {
        deleteSystem(dvs, errs);
        FAIL() << e.what();
    }

}
*/


/**
 *  Copyright (C) 2012
 *    Ekaterina Potapova
 *    Automation and Control Institute
 *    Vienna University of Technology
 *    Gusshausstraße 25-29
 *    1040 Vienna, Austria
 *    potapova(at)acin.tuwien.ac.at
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/
 */

#include "v4r/attention_segmentation/algo.h"

#include <Eigen/Dense>

namespace v4r
{

void filterGaussian(cv::Mat &input, cv::Mat &output, cv::Mat &mask)
{
  float kernel[5] = {1.0,4.0,6.0,4.0,1.0};

  cv::Mat temp = cv::Mat_<float>::zeros(input.rows, input.cols);

  for(int i = 0; i < input.rows; ++i)
  {
    for(int j = 0; j < input.cols; j = j+2)
    {
      if(mask.at<float>(i,j) > 0)
      {

	float value = 0;
        float kernel_sum = 0;

        for(int k = 0; k < 5; ++k)
        {
	  int jj = j + (k-2);
	  if( (jj >=0) && (jj < input.cols) )
	  {
	    if(mask.at<float>(i,jj) > 0)
	    {
	      value += kernel[k] * input.at<float>(i,jj);
	      kernel_sum += kernel[k];
	    }
	  }
        }

        if(kernel_sum > 0)
        {
	  temp.at<float>(i,j) = value / kernel_sum;
        }
      }
    }
  }

  cv::Mat temp2 = cv::Mat_<float>::zeros(temp.rows, temp.cols);

  for(int i = 0; i < temp.rows; i = i+2)
  {
    for(int j = 0; j < temp.cols; j = j+2)
    {

      if(mask.at<float>(i,j) > 0)
      {

	float value = 0;
        float kernel_sum = 0;

        for(int k = 0; k < 5; ++k)
        {
	  int ii = i + (k-2);
	  if( (ii >=0) && (ii < temp.rows) )
	  {
	    if(mask.at<float>(ii,j) > 0)
	    {
	      value += kernel[k] * temp.at<float>(ii,j);
	      kernel_sum += kernel[k];
	    }
	  }
        }

        if(kernel_sum > 0)
        {
	  temp2.at<float>(i,j) = value / kernel_sum;
        }
      }
    }
  }

  int new_width = input.cols / 2;
  int new_height = input.rows / 2;

  cv::Mat mask_output = cv::Mat_<float>::zeros(new_height,new_width);
  output = cv::Mat_<float>::zeros(new_height,new_width);

  for(int i = 0; i < new_height; ++i)
  {
    for(int j = 0; j < new_width; ++j)
    {
      if(mask.at<float>(2*i,2*j) > 0)
      {
	mask_output.at<float>(i,j) = 1.0;
	output.at<float>(i,j) = temp2.at<float>(2*i,2*j);
      }
    }
  }

  mask_output.copyTo(mask);
}

void buildDepthPyramid(cv::Mat &image, std::vector<cv::Mat> &pyramid, cv::Mat &mask, unsigned int levelNumber)
{
  pyramid.resize(levelNumber+1);
  image.copyTo(pyramid.at(0));

  cv::Mat oldMask;
  mask.copyTo(oldMask);

  for(unsigned int i = 1; i <= levelNumber; ++i)
  {
    cv::Mat tempImage;

    filterGaussian(pyramid.at(i-1),tempImage,oldMask);
    tempImage.copyTo(pyramid.at(i));

  }
}

void createPointCloudPyramid(std::vector<cv::Mat> &pyramidX, std::vector<cv::Mat> &pyramidY, std::vector<cv::Mat> &pyramidZ,
			     std::vector<cv::Mat> &pyramidIndices, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr > &pyramidCloud)
{
  assert( pyramidX.size() == pyramidY.size() );
  assert( pyramidX.size() == pyramidZ.size() );
  assert( pyramidX.size() == pyramidIndices.size() );

  unsigned int num_levels = pyramidX.size();

  pyramidCloud.resize(num_levels);

  for(unsigned int idx = 0; idx < num_levels; ++idx)
  {
    assert( pyramidX.at(idx).rows == pyramidY.at(idx).rows );
    assert( pyramidX.at(idx).cols == pyramidY.at(idx).cols );
    assert( pyramidX.at(idx).rows == pyramidZ.at(idx).rows );
    assert( pyramidX.at(idx).cols == pyramidZ.at(idx).cols );
    assert( pyramidX.at(idx).rows == pyramidIndices.at(idx).rows );
    assert( pyramidX.at(idx).cols == pyramidIndices.at(idx).cols );

    pyramidCloud.at(idx) = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>() );

    unsigned int cur_height = pyramidX.at(idx).rows;
    unsigned int cur_width = pyramidX.at(idx).cols;

    pyramidCloud.at(idx)->points.resize(cur_height*cur_width);
    pyramidCloud.at(idx)->width = cur_width;
    pyramidCloud.at(idx)->height = cur_height;

//     cv::imshow("pyramidZ.at(idx)",pyramidZ.at(idx));
//     cv::waitKey(-1);

    for(unsigned int i = 0 ; i < cur_height; ++i)
    {
      for(unsigned int j = 0; j < cur_width; ++j)
      {
	pcl::PointXYZRGB p_cur;

	//std::cerr << "(" << pyramidIndices.at(idx).at<float>(i,j) << "-- " << pyramidX.at(idx).at<float>(i,j) << "," << pyramidY.at(idx).at<float>(i,j) << "," << pyramidZ.at(idx).at<float>(i,j) << ") ";

        //if(pyramidIndices.at(idx).at<float>(i,j) > 0)
	//{
	  p_cur.x = pyramidX.at(idx).at<float>(i,j);
	  p_cur.y = pyramidY.at(idx).at<float>(i,j);
	  p_cur.z = pyramidZ.at(idx).at<float>(i,j);
	//}
	//else
// 	{
// 	  p_cur.x = std::numeric_limits<float>::quiet_NaN();
// 	  p_cur.y = std::numeric_limits<float>::quiet_NaN();
// 	  p_cur.z = std::numeric_limits<float>::quiet_NaN();
// 	}

	int p_idx = i*cur_width + j;
	pyramidCloud.at(idx)->points.at(p_idx) = p_cur;
      }
    }

    //std::cerr <<  std::endl << std::endl;

//   cv::imshow("output",output);
//   cv::waitKey(-1);

  }
}

void createNormalPyramid(std::vector<cv::Mat> &pyramidNx, std::vector<cv::Mat> &pyramidNy, std::vector<cv::Mat> &pyramidNz, std::vector<cv::Mat> &pyramidIndices, std::vector<pcl::PointCloud<pcl::Normal>::Ptr > &pyramidNormal)
{
  assert( pyramidNx.size() == pyramidNy.size() );
  assert( pyramidNx.size() == pyramidNz.size() );
  assert( pyramidIndices.size() == pyramidNz.size() );

  unsigned int num_levels = pyramidNx.size();

  pyramidNormal.resize(num_levels);

  for(unsigned int idx = 0; idx < num_levels; ++idx)
  {
    assert( pyramidNx.at(idx).rows == pyramidNy.at(idx).rows );
    assert( pyramidNx.at(idx).cols == pyramidNy.at(idx).cols );
    assert( pyramidNx.at(idx).rows == pyramidNz.at(idx).rows );
    assert( pyramidNx.at(idx).cols == pyramidNz.at(idx).cols );
    assert( pyramidNx.at(idx).rows == pyramidIndices.at(idx).rows );
    assert( pyramidNx.at(idx).cols == pyramidIndices.at(idx).cols );

    pyramidNormal.at(idx) = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>() );

    unsigned int cur_height = pyramidNx.at(idx).rows;
    unsigned int cur_width = pyramidNx.at(idx).cols;

    pyramidNormal.at(idx)->points.clear();

    for(unsigned int i = 0 ; i < cur_height; ++i)
    {
      for(unsigned int j = 0; j < cur_width; ++j)
      {
        if(pyramidIndices.at(idx).at<float>(i,j) > 0)
	{
	  pcl::Normal p_cur;
	  p_cur.normal[0] = pyramidNx.at(idx).at<float>(i,j);
	  p_cur.normal[1] = pyramidNy.at(idx).at<float>(i,j);
	  p_cur.normal[2] = pyramidNz.at(idx).at<float>(i,j);

	  pyramidNormal.at(idx)->points.push_back(p_cur);
	}
      }
    }

    pyramidNormal.at(idx)->width = pyramidNormal.at(idx)->points.size();
    pyramidNormal.at(idx)->height = 1;

  }
}

void createIndicesPyramid(std::vector<cv::Mat> &pyramidIndices, std::vector<pcl::PointIndices::Ptr> &pyramidIndiceSets)
{
  unsigned int num_levels = pyramidIndices.size();

  pyramidIndiceSets.resize(num_levels);

  for(unsigned int idx = 0; idx < num_levels; ++idx)
  {
    pyramidIndiceSets.at(idx) = pcl::PointIndices::Ptr(new pcl::PointIndices() );

    unsigned int cur_height = pyramidIndices.at(idx).rows;
    unsigned int cur_width = pyramidIndices.at(idx).cols;

    pyramidIndiceSets.at(idx)->indices.clear();

    for(unsigned int i = 0 ; i < cur_height; ++i)
    {
      for(unsigned int j = 0; j < cur_width; ++j)
      {
        if(pyramidIndices.at(idx).at<float>(i,j) > 0)
	{
	  int p_idx = i*cur_width + j;
	  pyramidIndiceSets.at(idx)->indices.push_back(p_idx);
	}
      }
    }

  }
}

void upscaleImage(cv::Mat &input, cv::Mat &output, unsigned int width, unsigned int height)
{
  assert( width >= (unsigned int)(2*input.cols) );
  assert( height >= (unsigned int)(2*input.rows) );

  output = cv::Mat_<float>::zeros(height, width);

  for(unsigned int i = 0; i < (unsigned int)input.rows; ++i)
  {
    for(unsigned int j = 0; j < (unsigned int)input.cols; ++j)
    {
      output.at<float>(2*i,2*j) = input.at<float>(i,j);
    }
  }

  //cv::imshow("output",output);
  //cv::waitKey(-1);

  for(unsigned int i = 0; i < (unsigned int)output.rows; i=i+2)
  {
    for(unsigned int j = 1; j < (unsigned int)output.cols-1; j=j+2)
    {
      output.at<float>(i,j) = ( output.at<float>(i,j-1) + output.at<float>(i,j+1) ) / 2.0;
    }
  }
  for(unsigned int i = 1; i < (unsigned int)output.rows-1; i=i+2)
  {
    for(unsigned int j = 0; j < (unsigned int)output.cols; j=j+2)
    {
      output.at<float>(i,j) = ( output.at<float>(i-1,j) + output.at<float>(i+1,j) ) / 2.0;
    }
  }
  for(unsigned int i = 1; i < (unsigned int)output.rows-1; i=i+2)
  {
    for(unsigned int j = 1; j < (unsigned int)output.cols-1; j=j+2)
    {
      output.at<float>(i,j) = ( output.at<float>(i-1,j-1) + output.at<float>(i-1,j+1) + output.at<float>(i+1,j-1) + output.at<float>(i+1,j+1) ) / 4.0;
    }
  }
}

void downscaleImage(cv::Mat &input, cv::Mat &output, unsigned int width, unsigned int height)
{
  assert( 2*width <= (unsigned int)(input.cols) );
  assert( 2*height <= (unsigned int)(input.rows) );

  output = cv::Mat_<float>::zeros(height, width);

  for(unsigned int i = 0; i < (unsigned int)output.rows; ++i)
  {
    for(unsigned int j = 0; j < (unsigned int)output.cols; ++j)
    {
      output.at<float>(i,j) = input.at<float>(2*i,2*j);
    }
  }
}

void scaleImage(std::vector<cv::Mat> &inputPyramid, cv::Mat &input, cv::Mat &output, int inLevel, int outLevel)
{
  assert(input.cols == inputPyramid.at(inLevel).cols);
  assert(input.rows == inputPyramid.at(inLevel).rows);

  if( inLevel < outLevel )
  {
    input.copyTo(output);
    for(int l = inLevel+1; l <= outLevel; ++l)
    {
      cv::Mat temp;
      downscaleImage(output,temp,inputPyramid.at(l).cols,inputPyramid.at(l).rows);
      temp.copyTo(output);
    }
  }
  else if ( inLevel > outLevel )
  {
    input.copyTo(output);
    for(int l = inLevel-1; l >= outLevel; --l)
    {
      cv::Mat temp;
      upscaleImage(output,temp,inputPyramid.at(l).cols,inputPyramid.at(l).rows);
      temp.copyTo(output);
    }
  }
  else
  {
    input.copyTo(output);
  }
}

bool inPoly(std::vector<cv::Point> &poly, cv::Point p)
{
  cv::Point newPoint;
  cv::Point oldPoint;
  cv::Point p1, p2;

  bool inside = false;

  if (poly.size() < 3)
  {
    return(false);
  }

  oldPoint = poly.at(poly.size()-1);
  for(unsigned int i = 0 ; i < poly.size(); i++)
  {
    newPoint = poly.at(i);
    if(newPoint.y > oldPoint.y)
    {
      p1 = oldPoint;
      p2 = newPoint;
    }
    else
    {
      p1 = newPoint;
      p2 = oldPoint;
    }

    if((newPoint.y < p.y) == (p.y <= oldPoint.y)          /* edge "open" at one end */
      && ((long)p.x-(long)p1.x)*(long)(p2.y-p1.y) < ((long)p2.x-(long)p1.x)*(long)(p.y-p1.y))
    {
      inside = !inside;
    }
    oldPoint = newPoint;
  }
  return(inside);
}

void buildPolygonMap(cv::Mat &polygonMap, std::vector<std::vector<cv::Point> > &polygons)
{
  for(int i = 0; i < polygonMap.rows; ++i)
  {
    for(int j = 0; j < polygonMap.cols; ++j)
    {
      unsigned int k = 0;
      while((polygonMap.at<uchar>(i,j) == 0) && (k < polygons.size()))
      {
        if(inPoly(polygons.at(k),cv::Point(j,i)))
          polygonMap.at<uchar>(i,j) = k+1;
        k += 1;
      }
    }
  }
}

void buildCountourMap(cv::Mat &polygonMap, std::vector<std::vector<cv::Point> > &polygons, cv::Scalar color)
{
  for(unsigned int i = 0; i < polygons.size(); ++i)
  {
    for(unsigned int j = 0; j < polygons.at(i).size(); ++j)
    {
      cv::line(polygonMap,polygons.at(i).at(j),polygons.at(i).at((j+1)%polygons.at(i).size()),color,2);
    }
  }
}

float normPDF(float x, float mean, float stddev)
{
  float val = exp(-(x-mean)*(x-mean)/(2*stddev*stddev));
  val /= sqrt(2*3.14)*stddev;
  return val;
}

float normPDF(std::vector<float> x, std::vector<float> mean, cv::Mat stddev)
{
  int dim = mean.size();

  EIGEN_ALIGN16 Eigen::MatrixXf _stddev = Eigen::MatrixXf::Zero(dim,dim);
  EIGEN_ALIGN16 Eigen::VectorXf _x = Eigen::VectorXf::Zero(dim);

  for(int i = 0; i < dim; ++i)
  {
    _x[i] = x.at(i) - mean.at(i);
    for(int j = 0; j < dim; ++j)
    {
      _stddev(i,j) = stddev.at<float>(i,j);
    }
  }

  //std::cerr << "_x.transpose() = " << _x.transpose() << std::endl;
  //std::cerr << "_stddev = " << _stddev << std::endl;
  //std::cerr << "_x = " << _x << std::endl;

  float value = (_x.transpose())*_stddev*_x;
  value /= -2;
  value = exp(value);

  float det = _stddev.determinant();

  value /= sqrt(pow(2*3.14,dim)*det);

  return(value);

}

void addNoise(cv::Mat &image, cv::Mat &nImage, cv::RNG &rng,float min, float max)
{
  nImage = cv::Mat_<float>::zeros(image.rows, image.cols);
  for(int i = 0; i < image.rows; ++i)
  {
    for(int j = 0; j < image.cols; ++j)
    {
      float val = image.at<float>(i,j) + rng.uniform(min,max);
      nImage.at<float>(i,j) = (val < 0 ? 0 : (val > 1 ? 1 : val));
    }
  }
}

void normPDF(cv::Mat &mat, float mean, float stddev, cv::Mat &res)
{
  res = cv::Mat_<float>::zeros(mat.rows,mat.cols);
  res = mat - mean;
  res = res.mul(res);
  float b = - 1.0f / (2 * stddev * stddev);
  res = res * b;

  cv::exp(res,res);
  float a = 1.0f/(stddev*sqrt(2*M_PI));
  res = res * a;
}

void normalizeDist(std::vector<float> &dist)
{
  float total_sum = 0;
  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    total_sum += dist.at(i);
  }

  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    dist.at(i) /= total_sum;
  }
}

float getMean(std::vector<float> dist, float total_num)
{
  float mean = 0;
  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    mean += (dist.at(i)/total_num)*i;
  }
  return mean;
}

float getStd(std::vector<float> dist, float mean, float total_num)
{
  float stdDev = 0;

  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    stdDev += (i-mean)*(i-mean)*(dist.at(i)/total_num);
  }

  stdDev = sqrt(stdDev);

  return stdDev;
}

long commulativeFunctionArgValue(float x, std::vector<float> &A)
{
  long min = 0;
  long max = A.size()-1;
  while(max!=min+1)
  {
    long mid = (min+max)/2;
    if(x <= A.at(mid))
    {
      max = mid;
    }
    else
    {
      min = mid;
    }
  }

  if(A.at(min) >= x)
    return(min);
  else
    return(max);
}

void createContoursFromMasks(std::vector<cv::Mat> &masks, std::vector<std::vector<cv::Point> > &contours)
{
  // to extract contours
  contours.clear();
  contours.resize(masks.size());

  for(unsigned int i = 0; i < masks.size(); ++i)
  {
    std::vector<std::vector<cv::Point> > temp;
    cv::Mat temp_image;
    masks.at(i).copyTo(temp_image);
    cv::findContours(temp_image,temp,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);
    if(temp.size())
    {
      long totalPointsNum = 0;
      for(unsigned int j = 0; j < temp.size(); ++j)
      {
	totalPointsNum += temp.at(j).size();
      }
      contours.at(i).resize(totalPointsNum);
      totalPointsNum = 0;
      for(unsigned int j = 0; j < temp.size(); ++j)
      {
	for(unsigned int k = 0; k < temp.at(j).size(); ++k)
	{
	  contours.at(i).at(totalPointsNum) = temp.at(j).at(k);
	  totalPointsNum += 1;
	}
      }
    }
  }
}

uchar Num01Transitions(cv::Mat s, int j, int i)
{

  uchar p2 = s.at<uchar>(j-1,i);
  uchar p3 = s.at<uchar>(j-1,i+1);
  uchar p4 = s.at<uchar>(j,i+1);
  uchar p5 = s.at<uchar>(j+1,i+1);
  uchar p6 = s.at<uchar>(j+1,i);
  uchar p7 = s.at<uchar>(j+1,i-1);
  uchar p8 = s.at<uchar>(j,i-1);
  uchar p9 = s.at<uchar>(j-1,i-1);

  uchar Nt = 0;

  if((p3-p2) == 1)
    Nt++;
  if((p4-p3) == 1)
    Nt++;
  if((p5-p4) == 1)
    Nt++;
  if((p6-p5) == 1)
    Nt++;
  if((p7-p6) == 1)
    Nt++;
  if((p8-p7) == 1)
    Nt++;
  if((p9-p8) == 1)
    Nt++;
  if((p2-p9) == 1)
    Nt++;

  return Nt;
}

void MConnectivity(cv::Mat &s, uchar *element)
{
  for(int i = 1; i < s.rows-1; ++i)
  {
    for(int j = 1; j < s.cols-1; ++j)
    {
      if(s.at<uchar>(i,j) > 0)
      {
	//s.at<uchar>(i,j) = 0;

	bool remove = true;
	for(int p = 0; p < 8; ++p)
        {
          int new_x = j + dx8[p];
          int new_y = i + dy8[p];

          uchar value = s.at<uchar>(new_y,new_x);

	  if(element[p] < 2)
	  {
	    if(value != element[p])
	    {
	      remove = false;
	    }
	  }
        }

	if(remove)
	{
	  s.at<uchar>(i,j) = 0;
	}
      }
    }
  }
}

V4R_EXPORTS void Skeleton(cv::Mat a, cv::Mat &s)
{
  int width  = a.cols;
  int height = a.rows;

  a.copyTo(s);

  cv::Scalar prevsum(0);

  while(true)
  {

    cv::Mat m = cv::Mat_<uchar>::ones(height,width);

    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {
        if (s.at<uchar>(j, i) == 1)
	{
	  uchar p2 = s.at<uchar>(j-1,i);
	  uchar p3 = s.at<uchar>(j-1,i+1);
	  uchar p4 = s.at<uchar>(j,i+1);
	  uchar p5 = s.at<uchar>(j+1,i+1);
	  uchar p6 = s.at<uchar>(j+1,i);
	  uchar p7 = s.at<uchar>(j+1,i-1);
	  uchar p8 = s.at<uchar>(j,i-1);
	  uchar p9 = s.at<uchar>(j-1,i-1);

	  uchar condA = p2+p3+p4+p5+p6+p7+p8+p9;
          uchar condB = Num01Transitions(s,j,i);
          uchar condC = p2 * p4 * p6;
          uchar condD = p4 * p6 * p8;

          if ((condA >= 2) && (condA <= 6) && (condB == 1) && (condC == 0) && (condD == 0))
             m.at<uchar>(j, i) = 0;
	}

      }
    }

    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {
	s.at<uchar>(j,i) = s.at<uchar>(j,i)*m.at<uchar>(j,i);
      }
    }

    m = cv::Mat_<uchar>::ones(height,width);
    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {

        if (s.at<uchar>(j, i) == 1)
	{
          uchar p2 = s.at<uchar>(j-1,i);
	  uchar p3 = s.at<uchar>(j-1,i+1);
	  uchar p4 = s.at<uchar>(j,i+1);
	  uchar p5 = s.at<uchar>(j+1,i+1);
	  uchar p6 = s.at<uchar>(j+1,i);
	  uchar p7 = s.at<uchar>(j+1,i-1);
	  uchar p8 = s.at<uchar>(j,i-1);
	  uchar p9 = s.at<uchar>(j-1,i-1);

	  uchar condA = p2+p3+p4+p5+p6+p7+p8+p9;
          uchar condB = Num01Transitions(s,j,i);
          uchar condC = p2 * p4 * p8;
          uchar condD = p2 * p6 * p8;

          if ((condA >= 2) && (condA <= 6) && (condB == 1) && (condC == 0) && (condD == 0))
            m.at<uchar>(j, i) = 0;
	}
      }
    }

    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {
	s.at<uchar>(j,i) = s.at<uchar>(j,i)*m.at<uchar>(j,i);
      }
    }

    cv::Scalar newsum = cv::sum(s);

    if (newsum(0) == prevsum(0))
      break;

    prevsum = newsum;

  }

  prevsum = cv::sum(s);

  uchar e1[8] = {2,1,2,1,2,0,0,0};
  uchar e2[8] = {2,1,2,0,0,0,2,1};
  uchar e3[8] = {0,0,2,1,2,1,2,0};
  uchar e4[8] = {2,0,0,0,2,1,2,1};

  while(true)
  {
    MConnectivity(s,e1);
    MConnectivity(s,e2);
    MConnectivity(s,e3);
    MConnectivity(s,e4);

    cv::Scalar newsum = cv::sum(s);

    if (newsum(0) == prevsum(0))
      break;

    prevsum = newsum;
  }
}

float calculateDistance(cv::Point center, cv::Point point)
{
  float distance = sqrt((center.x-point.x)*(center.x-point.x) + (center.y-point.y)*(center.y-point.y));
  return(distance);
}

float calculateDistance(cv::Point center, cv::Point point, float sigma)
{
  float distance = (center.x-point.x)*(center.x-point.x) + (center.y-point.y)*(center.y-point.y);
  distance = exp(-distance/(2*sigma*sigma));
  return(distance);
}

void calculateObjectCenter(cv::Mat mask, cv::Point &center)
{
  assert(mask.type() == CV_8UC1);

  float x_cen = 0;
  float y_cen = 0;
  cv::Scalar area = cv::sum(mask);

  for(int i = 0; i < mask.rows; ++i)
  {
    for(int j = 0; j < mask.cols; ++j)
    {
      uchar value = mask.at<uchar>(i,j);
      if(value > 0)
      {
	x_cen = x_cen + ((float)j)/area(0);
        y_cen = y_cen + ((float)i)/area(0);
      }
    }
  }

  if(mask.at<uchar>(y_cen,x_cen) == 0)
  {
    // search for closes 4-neighbour point
    std::list<cv::Point> points;
    points.push_back(cv::Point(x_cen,y_cen));
    cv::Mat used = cv::Mat_<uchar>::zeros(mask.rows,mask.cols);
    used.at<uchar>(y_cen,x_cen) = 1;
    while(points.size())
    {
      cv::Point p = points.front();
      points.pop_front();
      if(mask.at<uchar>(p.y,p.x) > 0)
      {
        x_cen = p.x;
	y_cen = p.y;
        break;
      }

      for(int i = 0; i < 8; ++i)
      {
        int new_x = p.x + dx8[i];
        int new_y = p.y + dy8[i];

	if((new_x < 0) || (new_y < 0) || (new_x >= used.cols) || (new_y >= used.rows))
	  continue;

	if(used.at<uchar>(new_y,new_x) <= 0)
	{
          points.push_back(cv::Point(new_x,new_y));
	  used.at<uchar>(new_y,new_x) = 1;
	}
      }
    }
  }

  center.x = x_cen;
  center.y = y_cen;
}

void calculateObjectCenter(std::vector<cv::Point> contour, cv::Mat mask, cv::Point &center)
{
  float x_cen = 0;
  float y_cen = 0;

  for(unsigned int i = 0; i < contour.size(); ++i)
  {
    x_cen = x_cen + ((float)contour.at(i).x)/contour.size();
    y_cen = y_cen + ((float)contour.at(i).y)/contour.size();
  }

  if(mask.at<uchar>(y_cen,x_cen) == 0)
  {
    // search for closes 4-neighbour point
    std::list<cv::Point> points;
    points.push_back(cv::Point(x_cen,y_cen));
    cv::Mat used = cv::Mat_<uchar>::zeros(mask.rows,mask.cols);
    used.at<uchar>(y_cen,x_cen) = 1;
    while(points.size())
    {
      cv::Point p = points.front();
      points.pop_front();
      if(mask.at<uchar>(p.y,p.x) > 0)
      {
        x_cen = p.x;
	y_cen = p.y;
        break;
      }

      for(int i = 0; i < 8; ++i)
      {
        int new_x = p.x + dx8[i];
        int new_y = p.y + dy8[i];

	if((new_x < 0) || (new_y < 0) || (new_x >= used.cols) || (new_y >= used.rows))
	  continue;

	if(used.at<uchar>(new_y,new_x) <= 0)
	{
          points.push_back(cv::Point(new_x,new_y));
	  used.at<uchar>(new_y,new_x) = 1;
	}
      }
    }
  }

  center.x = x_cen;
  center.y = y_cen;
}

void get2DNeighbors(const cv::Mat &patches, cv::Mat &neighbors, int patchesNumber)
{
  neighbors = cv::Mat_<bool>(patchesNumber,patchesNumber);
  neighbors.setTo(false);

  //@ep TODO: uncomment?
//   int dr[4] = {-1,-1, 0, 1};
//   int dc[4] = { 0,-1,-1,-1};

  int dr[4] = {-1,0,-1};
  int dc[4] = { 0,-1,-1};

  for(int r = 1; r < patches.rows-1; r++)
  {
    for(int c = 1; c < patches.cols-1; c++)
    {
      // if the patch exist
      if(patches.at<int>(r,c) != -1)
      {

	int patchIdx =  patches.at<int>(r,c);

	//@ep: why we did not use 1,-1 shift???
	for(int i = 0; i < 3; ++i) //@ep: TODO 3->4
	{

	  int nr = r + dr[i];
	  int nc = c + dc[i];

	  int currentPatchIdx =  patches.at<int>(nr,nc);
	  if(currentPatchIdx == -1)
	    continue;

	  if(patchIdx != currentPatchIdx)
	  {
	    neighbors.at<bool>(currentPatchIdx,patchIdx) = true;
            neighbors.at<bool>(patchIdx,currentPatchIdx) = true;
	  }
	}
      }
    }
  }
}

} //namespace v4r


#include <possumwood_sdk/datatypes/enum.h>
#include <possumwood_sdk/datatypes/filename.h>
#include <possumwood_sdk/node_implementation.h>

#include <boost/filesystem.hpp>

#include "datatypes/font.h"
#include "datatypes/vertex_data.inl"

namespace {

dependency_graph::InAttr<std::string> a_text;
dependency_graph::InAttr<possumwood::Font> a_font;
dependency_graph::InAttr<possumwood::Enum> a_horizAlign, a_vertAlign;

dependency_graph::OutAttr<possumwood::VertexData> a_vd;

dependency_graph::State compute(dependency_graph::Values& data) {
	const possumwood::Font& font = data.get(a_font);

	const int horizAlign = data.get(a_horizAlign).intValue();
	const int vertAlign = data.get(a_vertAlign).intValue();

	// and render the text into a new VertexData instance
	possumwood::VertexData vd(GL_TRIANGLES);
	if(!data.get(a_text).empty()) {
		// first, figure out the size of the text
		float totalWidth = 0.0f;
		for(auto& c : data.get(a_text)) {
			auto& g = font.glyph(c);
			totalWidth += (float)g.advance / font.size();
		}

		// construct the array of vertices
		std::vector<Imath::V2f> vertices;
		std::vector<Imath::V2f> uvs;

		float currentPos = 0;
		float vertPos = 0;

		if(horizAlign == 1)
			currentPos = -totalWidth / 2.0f;
		else if(horizAlign == 2)
			currentPos = 0;
		else
			currentPos = -totalWidth;

		if(vertAlign == 1)
			vertPos = -font.size() / 2.0f;
		else if(vertAlign == 2)
			vertPos = -font.size();
		else
			vertPos = 0;

		for(auto& c : data.get(a_text)) {
			auto& g = font.glyph(c);

			Imath::V2f p1(currentPos * font.size() - g.originX, g.originY - g.height + vertPos);
			Imath::V2f p2(currentPos * font.size() - g.originX, g.originY + vertPos);
			Imath::V2f p3(currentPos * font.size() - g.originX + g.width, g.originY + vertPos);
			Imath::V2f p4(currentPos * font.size() - g.originX + g.width, g.originY - g.height + vertPos);

			Imath::V2f uv1((float)g.x / font.width(), (float)(g.y + g.height) / font.height());
			Imath::V2f uv2((float)g.x / font.width(), (float)g.y / font.height());
			Imath::V2f uv3((float)(g.x + g.width) / font.width(), (float)g.y / font.height());
			Imath::V2f uv4((float)(g.x + g.width) / font.width(), (float)(g.y + g.height) / font.height());

			vertices.push_back(p1 / font.size());
			vertices.push_back(p3 / font.size());
			vertices.push_back(p2 / font.size());

			vertices.push_back(p1 / font.size());
			vertices.push_back(p4 / font.size());
			vertices.push_back(p3 / font.size());

			uvs.push_back(uv1);
			uvs.push_back(uv3);
			uvs.push_back(uv2);

			uvs.push_back(uv1);
			uvs.push_back(uv4);
			uvs.push_back(uv3);

			currentPos += (float)g.advance / font.size();
		}

		// and add them to the vertex data
		vd.addVBO<Imath::V2f>("position", vertices.size(), possumwood::VertexData::kStatic,
		                      [vertices](possumwood::Buffer<float>& buffer, const possumwood::ViewportState& viewport) {
			                      std::size_t ctr = 0;
			                      for(auto& v : vertices) {
				                      buffer.element(ctr) = v;
				                      ++ctr;
			                      }
		                      });

		vd.addVBO<Imath::V2f>("uv", uvs.size(), possumwood::VertexData::kStatic,
		                      [uvs](possumwood::Buffer<float>& buffer, const possumwood::ViewportState& viewport) {
			                      std::size_t ctr = 0;
			                      for(auto& v : uvs) {
				                      buffer.element(ctr) = v;
				                      ++ctr;
			                      }
		                      });
	}

	data.set(a_vd, std::move(vd));

	return dependency_graph::State();
}

void init(possumwood::Metadata& meta) {
	meta.addAttribute(a_text, "text");
	meta.addAttribute(a_font, "font");
	meta.addAttribute(a_vd, "vertex_data");

	meta.addAttribute(
	    a_horizAlign, "horiz_align",
	    possumwood::Enum({std::make_pair("Center", 1), std::make_pair("Left", 2), std::make_pair("Right", 3)}));

	meta.addAttribute(
	    a_vertAlign, "vert_align",
	    possumwood::Enum({std::make_pair("Center", 1), std::make_pair("Top", 2), std::make_pair("Bottom", 3)}));

	meta.addInfluence(a_font, a_vd);
	meta.addInfluence(a_text, a_vd);
	meta.addInfluence(a_horizAlign, a_vd);
	meta.addInfluence(a_vertAlign, a_vd);

	meta.setCompute(compute);
}

possumwood::NodeImplementation s_impl("render/vertex_data/text", init);

}  // namespace


#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <armadillo>

namespace py = pybind11;

using namespace arma;

typedef py::array_t<double, py::array::f_style | py::array::forcecast> array_tf;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> array_tc;

cube array_to_cube(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];
    int n_slices = _m_buff.shape[2];

    cube _m_arma((double *)_m_buff.ptr, n_rows, n_cols, n_slices);

    return _m_arma;
}

mat array_to_mat(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];

    mat _m_arma((double *)_m_buff.ptr, n_rows, n_cols);

    return _m_arma;
}

vec array_to_vec(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];

    vec _m_vec((double *)_m_buff.ptr, n_rows);

    return _m_vec;
}

array_tf cube_to_array(cube m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols, m.n_slices});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols * m.n_slices);

    return _m_array;
}

array_tf mat_to_array(mat m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols);

    return _m_array;
}

array_tf vec_to_array(vec m) {

    auto _m_array = array_tf({m.n_rows});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows);

    return _m_array;
}

py::tuple backward_pass(array_tf _Cxx, array_tf _cx, array_tf _Cuu,
                        array_tf _cu, array_tf _Cxu,
                        array_tf _A, array_tf _B,
                        double lmbda, int reg,
                        int dm_state, int dm_act, int nb_steps) {

    // inputs
    cube Cxx = array_to_cube(_Cxx);
    mat cx = array_to_mat(_cx);
    cube Cuu = array_to_cube(_Cuu);
    mat cu = array_to_mat(_cu);
    cube Cxu = array_to_cube(_Cxu);

    cube A = array_to_cube(_A);
    cube B = array_to_cube(_B);

    // outputs
    cube Q(dm_state + dm_act, dm_state + dm_act, nb_steps);
    cube Qxx(dm_state, dm_state, nb_steps);
    cube Qux(dm_act, dm_state, nb_steps);
    cube Quu(dm_act, dm_act, nb_steps);
    mat qx(dm_state, nb_steps);
    mat qu(dm_act, nb_steps);

    cube Qux_reg(dm_act, dm_state, nb_steps);
    cube Quu_reg(dm_act, dm_act, nb_steps);
    cube Quu_inv(dm_act, dm_act, nb_steps);

    cube V(dm_state, dm_state, nb_steps + 1);
    mat v(dm_state, nb_steps + 1);
    vec dV(2);

    cube V_reg(dm_state, dm_state, nb_steps + 1);

    cube K(dm_act, dm_state, nb_steps);
    mat kff(dm_act, nb_steps);

    int _diverge = 0;

    // last time step
    V.slice(nb_steps) = Cxx.slice(nb_steps);
    v.col(nb_steps) = cx.col(nb_steps);

	for(int i = nb_steps - 1; i>= 0; --i)
	{
        Qxx.slice(i) = Cxx.slice(i) + A.slice(i).t() * V.slice(i+1) * A.slice(i);
        Quu.slice(i) = Cuu.slice(i) + B.slice(i).t() * V.slice(i+1) * B.slice(i);
        Qux.slice(i) = (Cxu.slice(i) + A.slice(i).t() * V.slice(i+1) * B.slice(i)).t();

        qu.col(i) = cu.col(i) + B.slice(i).t() * v.col(i+1);
        qx.col(i) = cx.col(i) + A.slice(i).t() * v.col(i+1);

        V_reg.slice(i+1) = V.slice(i+1);
        if (reg==2)
            V_reg.slice(i+1) += lmbda * eye(dm_state, dm_state);

        Qux_reg.slice(i) = (Cxu.slice(i) + A.slice(i).t() * V_reg.slice(i+1) * B.slice(i)).t();

        Quu_reg.slice(i) = Cuu.slice(i) + B.slice(i).t() * V_reg.slice(i+1) * B.slice(i);
        if (reg==1)
            Quu_reg.slice(i) += lmbda * eye(dm_act, dm_act);

        if (!(Quu_reg.slice(i)).is_sympd()) {
            _diverge = i;
            break;
        }

        Quu_inv.slice(i) = inv(Quu_reg.slice(i));
        K.slice(i) = - Quu_inv.slice(i) * Qux_reg.slice(i);
        kff.col(i) = - Quu_inv.slice(i) * qu.col(i);

        dV += join_vert(kff.col(i).t() * qu.col(i), 0.5 * kff.col(i).t() * Quu.slice(i) * kff.col(i));

        v.col(i) = qx.col(i) + K.slice(i).t() * Quu.slice(i) * kff.col(i) +
                   K.slice(i).t() * qu.col(i) + Qux.slice(i).t() * kff.col(i);

        V.slice(i) = Qxx.slice(i) + K.slice(i).t() * Quu.slice(i) * K.slice(i) +
                     K.slice(i).t() * Qux.slice(i) + Qux.slice(i).t() * K.slice(i);
        V.slice(i) = 0.5 * (V.slice(i) + V.slice(i).t());
	}

    // transform outputs to numpy
    array_tf _Qxx = cube_to_array(Qxx);
    array_tf _Qux = cube_to_array(Qux);
    array_tf _Quu = cube_to_array(Quu);

    array_tf _qx = mat_to_array(qx);
    array_tf _qu = mat_to_array(qu);

    array_tf _V = cube_to_array(V);
    array_tf _v = mat_to_array(v);
    array_tf _dV = vec_to_array(dV);

    array_tf _K = cube_to_array(K);
    array_tf _kff = mat_to_array(kff);

    py::tuple output =  py::make_tuple(_Qxx, _Qux, _Quu, _qx, _qu,
                                       _V, _v, _dV, _K, _kff, _diverge);
	return output;
}

PYBIND11_MODULE(core, m)
{
    m.def("backward_pass", &backward_pass);
}


// fontnik
#include "glyph_foundry.hpp"

#include <agg/agg_curves.h>
#include <agg/agg_curves_impl.hpp>

// boost
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>

// std
#include <cmath> // std::sqrt

namespace bg = boost::geometry;
namespace bgm = bg::model;
namespace bgi = bg::index;
typedef bgm::point<float, 2, bg::cs::cartesian> Point;
typedef bgm::box<Point> Box;
typedef std::vector<Point> Points;
typedef std::vector<Points> Rings;
typedef std::pair<Point, Point> SegmentPair;
typedef std::pair<Box, SegmentPair> SegmentValue;
typedef bgi::rtree<SegmentValue, bgi::rstar<16>> Tree;

namespace sdf_glyph_foundry
{
    struct User {
        Rings rings;
        Points ring;
    };

    void CloseRing(Points &ring)
    {
        const Point &first = ring.front();
        const Point &last = ring.back();

        if (first.get<0>() != last.get<0>() ||
            first.get<1>() != last.get<1>())
        {
            ring.push_back(first);
        }
    }

    int MoveTo(const FT_Vector *to, void *ptr)
    {
        User *user = (User*)ptr;
        if (!user->ring.empty()) {
            CloseRing(user->ring);
            user->rings.push_back(user->ring);
            user->ring.clear();
        }
        user->ring.emplace_back(float(to->x) / 64.0, float(to->y) / 64.0);
        return 0;
    }

    int LineTo(const FT_Vector *to, void *ptr)
    {
        User *user = static_cast<User*>(ptr);
        user->ring.emplace_back(float(to->x) / 64.0, float(to->y) / 64.0);
        return 0;
    }

    int ConicTo(const FT_Vector *control,
                const FT_Vector *to,
                void *ptr)
    {
        User *user = static_cast<User*>(ptr);

        if (!user->ring.empty()) {
            Point const& prev = user->ring.back();
            auto dx = prev.get<0>();
            auto dy = prev.get<1>();

            // pop off last point, duplicate of first point in bezier curve
            // WARNING: pop_back invalidates `prev`
            // http://en.cppreference.com/w/cpp/container/vector/pop_back
            user->ring.pop_back();

            agg_fontnik::curve3_div curve(dx,dy,
                                  float(control->x) / 64, float(control->y) / 64,
                                  float(to->x) / 64, float(to->y) / 64);

            curve.rewind(0);
            double x, y;
            unsigned cmd;

            while (agg_fontnik::path_cmd_stop != (cmd = curve.vertex(&x, &y))) {
                user->ring.emplace_back(x, y);
            }
        }

        return 0;
    }

    int CubicTo(const FT_Vector *c1,
                const FT_Vector *c2,
                const FT_Vector *to,
                void *ptr)
    {
        User *user = static_cast<User*>(ptr);

        if (!user->ring.empty()) {

            Point const& prev = user->ring.back();
            auto dx = prev.get<0>();
            auto dy = prev.get<1>();

            // pop off last point, duplicate of first point in bezier curve
            // WARNING: pop_back invalidates `prev`
            // http://en.cppreference.com/w/cpp/container/vector/pop_back
            user->ring.pop_back();

            agg_fontnik::curve4_div curve(dx,dy,
                                  float(c1->x) / 64, float(c1->y) / 64,
                                  float(c2->x) / 64, float(c2->y) / 64,
                                  float(to->x) / 64, float(to->y) / 64);

            curve.rewind(0);
            double x, y;
            unsigned cmd;

            while (agg_fontnik::path_cmd_stop != (cmd = curve.vertex(&x, &y))) {
                user->ring.emplace_back(x, y);
            }
        }

        return 0;
    }

    // point in polygon ray casting algorithm
    bool PolyContainsPoint(const Rings &rings, const Point &p)
    {
        bool c = false;

        for (const Points &ring : rings) {
            auto p1 = ring.begin();
            auto p2 = p1 + 1;

            for (; p2 != ring.end(); p1++, p2++) {
                if (((p1->get<1>() > p.get<1>()) != (p2->get<1>() > p.get<1>())) && (p.get<0>() < (p2->get<0>() - p1->get<0>()) * (p.get<1>() - p1->get<1>()) / (p2->get<1>() - p1->get<1>()) + p1->get<0>())) {
                    c = !c;
                }
            }
        }

        return c;
    }

    double SquaredDistance(const Point &v, const Point &w)
    {
        const double a = v.get<0>() - w.get<0>();
        const double b = v.get<1>() - w.get<1>();
        return a * a + b * b;
    }

    Point ProjectPointOnLineSegment(const Point &p,
                                    const Point &v,
                                    const Point &w)
    {
      const double l2 = SquaredDistance(v, w);
      if (l2 == 0) return v;

      const double t = ((p.get<0>() - v.get<0>()) * (w.get<0>() - v.get<0>()) + (p.get<1>() - v.get<1>()) * (w.get<1>() - v.get<1>())) / l2;
      if (t < 0) return v;
      if (t > 1) return w;

      return Point {
          v.get<0>() + t * (w.get<0>() - v.get<0>()),
          v.get<1>() + t * (w.get<1>() - v.get<1>())
      };
    }

    double SquaredDistanceToLineSegment(const Point &p,
                                        const Point &v,
                                        const Point &w)
    {
        const Point s = ProjectPointOnLineSegment(p, v, w);
        return SquaredDistance(p, s);
    }

    double MinDistanceToLineSegment(const Tree &tree,
                                    const Point &p,
                                    int radius)
    {
        const int squared_radius = radius * radius;

        std::vector<SegmentValue> results;
        tree.query(bgi::intersects(
            Box{
                Point{p.get<0>() - radius, p.get<1>() - radius},
                Point{p.get<0>() + radius, p.get<1>() + radius}
            }),
            std::back_inserter(results));

        double sqaured_distance = std::numeric_limits<double>::infinity();

        for (const auto &value : results) {
            const SegmentPair &segment = value.second;
            const double dist = SquaredDistanceToLineSegment(p,
                                                             segment.first,
                                                             segment.second);
            if (dist < sqaured_distance && dist < squared_radius) {
                sqaured_distance = dist;
            }
        }

        return std::sqrt(sqaured_distance);
    }

    void RenderSDF(glyph_info &glyph,
                         int size,
                         int buffer,
                         float cutoff,
                         FT_Face ft_face)
    {

        if (FT_Load_Glyph (ft_face, glyph.glyph_index, FT_LOAD_NO_HINTING)) {
            return;
        }

        int advance = ft_face->glyph->metrics.horiAdvance / 64;
        int ascender = ft_face->size->metrics.ascender / 64;
        int descender = ft_face->size->metrics.descender / 64;

        glyph.line_height = ft_face->size->metrics.height;
        glyph.advance = advance;
        glyph.ascender = ascender;
        glyph.descender = descender;

        FT_Outline_Funcs func_interface = {
            .move_to = &MoveTo,
            .line_to = &LineTo,
            .conic_to = &ConicTo,
            .cubic_to = &CubicTo,
            .shift = 0,
            .delta = 0
        };

        User user;

        if (ft_face->glyph->format == FT_GLYPH_FORMAT_OUTLINE) {
            // Decompose outline into bezier curves and line segments
            FT_Outline outline = ft_face->glyph->outline;
            if (FT_Outline_Decompose(&outline, &func_interface, &user)) return;

            if (!user.ring.empty()) {
                CloseRing(user.ring);
                user.rings.push_back(user.ring);
            }

            if (user.rings.empty()) {
                return;
            }
        } else {
            return;
        }

        // Calculate the real glyph bbox.
        double bbox_xmin = std::numeric_limits<double>::infinity(),
               bbox_ymin = std::numeric_limits<double>::infinity();

        double bbox_xmax = -std::numeric_limits<double>::infinity(),
               bbox_ymax = -std::numeric_limits<double>::infinity();

        for (const Points &ring : user.rings) {
            for (const Point &point : ring) {
                if (point.get<0>() > bbox_xmax) bbox_xmax = point.get<0>();
                if (point.get<0>() < bbox_xmin) bbox_xmin = point.get<0>();
                if (point.get<1>() > bbox_ymax) bbox_ymax = point.get<1>();
                if (point.get<1>() < bbox_ymin) bbox_ymin = point.get<1>();
            }
        }

        bbox_xmin = std::round(bbox_xmin);
        bbox_ymin = std::round(bbox_ymin);
        bbox_xmax = std::round(bbox_xmax);
        bbox_ymax = std::round(bbox_ymax);

        // Offset so that glyph outlines are in the bounding box.
        for (Points &ring : user.rings) {
            for (Point &point : ring) {
                point.set<0>(point.get<0>() + -bbox_xmin + buffer);
                point.set<1>(point.get<1>() + -bbox_ymin + buffer);
            }
        }

        if (bbox_xmax - bbox_xmin == 0 || bbox_ymax - bbox_ymin == 0) return;

        glyph.left = bbox_xmin;
        glyph.top = bbox_ymax;
        glyph.width = bbox_xmax - bbox_xmin;
        glyph.height = bbox_ymax - bbox_ymin;

        Tree tree;
        float offset = 0.5;
        int radius = 8;
        int radius_by_256 = (256 / radius);

        for (const Points &ring : user.rings) {
            auto p1 = ring.begin();
            auto p2 = p1 + 1;

            for (; p2 != ring.end(); p1++, p2++) {
                const int segment_x1 = std::min(p1->get<0>(), p2->get<0>());
                const int segment_x2 = std::max(p1->get<0>(), p2->get<0>());
                const int segment_y1 = std::min(p1->get<1>(), p2->get<1>());
                const int segment_y2 = std::max(p1->get<1>(), p2->get<1>());

                tree.insert(SegmentValue {
                    Box {
                        Point {segment_x1, segment_y1},
                        Point {segment_x2, segment_y2}
                    },
                    SegmentPair {
                        Point {p1->get<0>(), p1->get<1>()},
                        Point {p2->get<0>(), p2->get<1>()}
                    }
                });
            }
        }

        // Loop over every pixel and determine the positive/negative distance to the outline.
        unsigned int buffered_width = glyph.width + 2 * buffer;
        unsigned int buffered_height = glyph.height + 2 * buffer;
        unsigned int bitmap_size = buffered_width * buffered_height;
        glyph.bitmap.resize(bitmap_size);

        for (unsigned int y = 0; y < buffered_height; y++) {
            for (unsigned int x = 0; x < buffered_width; x++) {
                unsigned int ypos = buffered_height - y - 1;
                unsigned int i = ypos * buffered_width + x;
                Point pt{x + offset, y + offset };
                double d = MinDistanceToLineSegment(tree, pt, radius) * radius_by_256;

                // Invert if point is inside.
                const bool inside = PolyContainsPoint(user.rings, pt);
                if (inside) {
                    d = -d;
                }

                // Shift the 0 so that we can fit a few negative values
                // into our 8 bits.
                d += cutoff * 256;

                // Clamp to 0-255 to prevent overflows or underflows.
                int n = d > 255 ? 255 : d;
                n = n < 0 ? 0 : n;

                glyph.bitmap[i] = static_cast<char>(255 - n);
            }
        }
    }

} // ns sdf_glyph_foundry


//
// Copyright 2013 Juan V. Puertos G-Cluster, Christian Henning
//
// 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
//
#ifndef BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_YCBCR_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_YCBCR_HPP

#include <boost/gil/extension/toolbox/metafunctions/get_num_bits.hpp>

#include <boost/gil.hpp> // FIXME: Include what you use!

#include <boost/mpl/identity.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/vector_c.hpp>

#include <cstdint>

namespace boost{ namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace ycbcr_601_color_space
{
/// \brief Luminance
struct y_t {};
/// \brief Blue chrominance component
struct cb_t {};
/// \brief Red chrominance component
struct cr_t {};
}

namespace ycbcr_709_color_space
{
/// \brief Luminance
struct y_t {};
/// \brief Blue chrominance component
struct cb_t {};
/// \brief Red chrominance component
struct cr_t {};
}
/// \}

/// \ingroup ColorSpaceModel
typedef boost::mpl::vector3< ycbcr_601_color_space::y_t, ycbcr_601_color_space::cb_t, ycbcr_601_color_space::cr_t > ycbcr_601__t;
typedef boost::mpl::vector3< ycbcr_709_color_space::y_t, ycbcr_709_color_space::cb_t, ycbcr_709_color_space::cr_t > ycbcr_709__t;

/// \ingroup LayoutModel
typedef boost::gil::layout<ycbcr_601__t> ycbcr_601__layout_t;
typedef boost::gil::layout<ycbcr_709__t> ycbcr_709__layout_t;

//The channel depth is ALWAYS 8bits ofr YCbCr!
GIL_DEFINE_ALL_TYPEDEFS(8, uint8_t, ycbcr_601_)
GIL_DEFINE_ALL_TYPEDEFS(8, uint8_t, ycbcr_709_)

namespace detail {

// Source:boost/algorithm/clamp.hpp
template<typename T>
constexpr T const& clamp(T const& val,
    typename boost::mpl::identity<T>::type const & lo,
    typename boost::mpl::identity<T>::type const & hi)
{
    // assert ( !p ( hi, lo )); // Can't assert p ( lo, hi ) b/c they might be equal
    auto const p = std::less<T>();
    return p(val, lo) ? lo : p(hi, val) ? hi : val;
}

} // namespace detail

/*
 * 601 Source: http://en.wikipedia.org/wiki/YCbCr#ITU-R_BT.601_conversion
 * 709 Source: http://en.wikipedia.org/wiki/YCbCr#ITU-R_BT.709_conversion
 * (using values coming directly from ITU-R BT.601 recommendation)
 * (using values coming directly from ITU-R BT.709 recommendation)
 */

/**
* @brief Convert YCbCr ITU.BT-601 to RGB.
*/
template<>
struct default_color_converter_impl<ycbcr_601__t, rgb_t>
{
	// Note: the RGB_t channels range can be set later on by the users. We dont want to cast to uint8_t or anything here.
	template < typename SRCP, typename DSTP >
	void operator()( const SRCP& src, DSTP& dst ) const
	{
        typedef typename channel_type< DSTP >::type dst_channel_t;
        convert( src, dst
               , typename boost::is_same< typename mpl::int_< sizeof( dst_channel_t ) >::type
                                        , typename mpl::int_<1>::type
                                        >::type()
               );
	}

private:

    // optimization for bit8 channels
    template< typename Src_Pixel
            , typename Dst_Pixel
            >
    void convert( const Src_Pixel& src
                ,       Dst_Pixel& dst
                , mpl::true_ // is 8 bit channel
                ) const
    {
        using namespace ycbcr_601_color_space;

        typedef typename channel_type< Src_Pixel >::type src_channel_t;
        typedef typename channel_type< Dst_Pixel >::type dst_channel_t;

		src_channel_t y  = channel_convert<src_channel_t>( get_color(src,  y_t()));
		src_channel_t cb = channel_convert<src_channel_t>( get_color(src, cb_t()));
		src_channel_t cr = channel_convert<src_channel_t>( get_color(src, cr_t()));

		// The intermediate results of the formulas require at least 16bits of precission.
		std::int_fast16_t c = y  - 16;
		std::int_fast16_t d = cb - 128;
		std::int_fast16_t e = cr - 128;
		std::int_fast16_t red   = detail::clamp((( 298 * c + 409 * e + 128) >> 8), 0, 255);
		std::int_fast16_t green = detail::clamp((( 298 * c - 100 * d - 208 * e + 128) >> 8), 0, 255);
		std::int_fast16_t blue  = detail::clamp((( 298 * c + 516 * d + 128) >> 8), 0, 255);

		get_color( dst,  red_t() )  = (dst_channel_t) red;
		get_color( dst, green_t() ) = (dst_channel_t) green;
		get_color( dst,  blue_t() ) = (dst_channel_t) blue;
    }

    template< typename Src_Pixel
            , typename Dst_Pixel
            >
    void convert( const Src_Pixel& src
                ,       Dst_Pixel& dst
                , mpl::false_ // is 8 bit channel
                ) const
    {
        using namespace ycbcr_601_color_space;

        typedef typename channel_type< Dst_Pixel >::type dst_channel_t;

        double  y = get_color( src,  y_t() );
        double cb = get_color( src, cb_t() );
        double cr = get_color( src, cr_t() );

        get_color(dst, red_t()) = static_cast<dst_channel_t>(
            detail::clamp(1.6438 * (y - 16.0) + 1.5960 * (cr -128.0), 0.0, 255.0));

        get_color(dst, green_t()) = static_cast<dst_channel_t>(
            detail::clamp(1.6438 * (y - 16.0) - 0.3917 * (cb - 128.0) + 0.8129 * (cr -128.0), 0.0, 255.0));

        get_color(dst, blue_t()) = static_cast<dst_channel_t>(
            detail::clamp(1.6438 * ( y - 16.0 ) - 2.0172 * ( cb -128.0 ), 0.0, 255.0));
    }
};

/*
 * Source: http://en.wikipedia.org/wiki/YCbCr#ITU-R_BT.601_conversion
 * digital Y′CbCr derived from digital R'dG'dB'd 8 bits per sample, each using the full range.
 * with NO footroom wither headroom.
 */
/**
* @brief Convert RGB to YCbCr ITU.BT-601.
*/
template<>
struct default_color_converter_impl<rgb_t, ycbcr_601__t>
{
	template < typename SRCP, typename DSTP >
	void operator()( const SRCP& src, DSTP& dst ) const
	{
        using namespace ycbcr_601_color_space;

        typedef typename channel_type< SRCP >::type src_channel_t;
        typedef typename channel_type< DSTP >::type dst_channel_t;

		src_channel_t red   = channel_convert<src_channel_t>( get_color(src,   red_t()));
		src_channel_t green = channel_convert<src_channel_t>( get_color(src, green_t()));
		src_channel_t blue  = channel_convert<src_channel_t>( get_color(src,  blue_t()));

		double  y =  16.0 + 0.2567 * red  + 0.5041 * green + 0.0979 * blue;
		double cb = 128.0 - 0.1482 * red  - 0.2909 * green + 0.4392 * blue;
		double cr = 128.0 + 0.4392 * red  - 0.3677 * green - 0.0714 * blue;

		get_color( dst,  y_t() ) = (dst_channel_t)  y;
		get_color( dst, cb_t() ) = (dst_channel_t) cb;
		get_color( dst, cr_t() ) = (dst_channel_t) cr;
	}
};

/**
* @brief Convert RGB to YCbCr ITU.BT-709.
*/
template<>
struct default_color_converter_impl<rgb_t, ycbcr_709__t>
{
	template < typename SRCP, typename DSTP >
	void operator()( const SRCP& src, DSTP& dst ) const
	{
        using namespace ycbcr_709_color_space;

        typedef typename channel_type< SRCP >::type src_channel_t;
        typedef typename channel_type< DSTP >::type dst_channel_t;

		src_channel_t red   = channel_convert<src_channel_t>( get_color(src,   red_t()));
		src_channel_t green = channel_convert<src_channel_t>( get_color(src, green_t()));
		src_channel_t blue  = channel_convert<src_channel_t>( get_color(src,  blue_t()));

		double  y =            0.299 * red  +    0.587 * green +    0.114 * blue;
		double cb = 128.0 - 0.168736 * red  - 0.331264 * green +      0.5 * blue;
		double cr = 128.0 +      0.5 * red  - 0.418688 * green - 0.081312 * blue;

		get_color( dst,  y_t() ) = (dst_channel_t)  y;
		get_color( dst, cb_t() ) = (dst_channel_t) cb;
		get_color( dst, cr_t() ) = (dst_channel_t) cr;
	}
};

/**
* @brief Convert RGB to YCbCr ITU.BT-709.
*/
template<>
struct default_color_converter_impl<ycbcr_709__t, rgb_t>
{
	template < typename SRCP, typename DSTP >
	void operator()( const SRCP& src, DSTP& dst ) const
	{
        using namespace ycbcr_709_color_space;

        typedef typename channel_type< SRCP >::type src_channel_t;
        typedef typename channel_type< DSTP >::type dst_channel_t;

		src_channel_t y           = channel_convert<src_channel_t>( get_color(src,  y_t())       );
		src_channel_t cb_clipped  = channel_convert<src_channel_t>( get_color(src, cb_t()) - 128 );
		src_channel_t cr_clipped  = channel_convert<src_channel_t>( get_color(src, cr_t()) - 128 );

		double   red =   y                        +   1.042 * cr_clipped;
		double green =   y - 0.34414 * cb_clipped - 0.71414 * cr_clipped;
		double  blue =   y +   1.772 * cb_clipped;

		get_color( dst,   red_t() ) = (dst_channel_t)   red;
		get_color( dst, green_t() ) = (dst_channel_t) green;
		get_color( dst,  blue_t() ) = (dst_channel_t)  blue;
	}
};

} // namespace gil
} // namespace boost

#endif


//==================================================================================================
/*!
  @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_DETAIL_CONSTANT_EXPX2C1_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_EXPX2C1_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate the constant expx2c1.

    @return The Expx2c1 constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    struct expx2c1_ : boost::dispatch::constant_value_<expx2c1_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,expx2c1_,boost::dispatch::constant_value_<expx2c1_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0x42000000UL, 0x4060000000000000ull);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, expx2c1_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::expx2c1_,expx2c1);
  }

  template<typename T> BOOST_FORCEINLINE auto Expx2c1()
  BOOST_NOEXCEPT_DECLTYPE(detail::expx2c1( boost::dispatch::as_<T>{}))
  {
    return detail::expx2c1( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


// Copyright 2019-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Interoperability helpers connecting internal math types and Eigen.
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_math
 */

#pragma once

#ifndef __cplusplus
#error "This header only usable from C++"
#endif

#include "math/m_api.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace xrt::auxiliary::math {

/*!
 * @brief Wrap an internal quaternion struct in an Eigen type, const overload.
 *
 * Permits zero-overhead manipulation of `const xrt_quat&` by Eigen routines as
 * if it were a `const Eigen::Quaternionf&`.
 */
static inline Eigen::Map<const Eigen::Quaternionf>
map_quat(const struct xrt_quat &q)
{
	return Eigen::Map<const Eigen::Quaternionf>{&q.x};
}

/*!
 * @brief Wrap an internal quaternion struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_quat&` by Eigen routines as if it
 * were a `Eigen::Quaternionf&`.
 */
static inline Eigen::Map<Eigen::Quaternionf>
map_quat(struct xrt_quat &q)
{
	return Eigen::Map<Eigen::Quaternionf>{&q.x};
}

/*!
 * @brief Wrap an internal 3D vector struct in an Eigen type, const overload.
 *
 * Permits zero-overhead manipulation of `const xrt_vec3&` by Eigen routines as
 * if it were a `const Eigen::Vector3f&`.
 */
static inline Eigen::Map<const Eigen::Vector3f>
map_vec3(const struct xrt_vec3 &v)
{
	return Eigen::Map<const Eigen::Vector3f>{&v.x};
}

/*!
 * @brief Wrap an internal 3D vector struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_vec3&` by Eigen routines as
 * if it were a `Eigen::Vector3f&`.
 */
static inline Eigen::Map<Eigen::Vector3f>
map_vec3(struct xrt_vec3 &v)
{
	return Eigen::Map<Eigen::Vector3f>{&v.x};
}

/*!
 * @brief Wrap an internal 3x3 matrix struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_matrix_3x3&` by Eigen routines as
 * if it were a `Eigen::Matrix3f&`.
 */
static inline Eigen::Map<Eigen::Matrix3f>
map_matrix_3x3(struct xrt_matrix_3x3 &m)
{
	return Eigen::Map<Eigen::Matrix3f>(m.v);
}

/*!
 * @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_matrix_4x4&` by Eigen routines as
 * if it were a `Eigen::Matrix4f&`.
 */
static inline Eigen::Map<Eigen::Matrix4f>
map_matrix_4x4(struct xrt_matrix_4x4 &m)
{
	return Eigen::Map<Eigen::Matrix4f>(m.v);
}

/*!
 * @brief Wrap an internal 4x4 matrix f64 struct in an Eigen type, const overload.
 *
 * Permits zero-overhead manipulation of `const xrt_matrix_4x4_f64&` by Eigen routines as if it were a
 * `const Eigen::Matrix4d&`.
 */
static inline Eigen::Map<const Eigen::Matrix4d>
map_matrix_4x4_f64(const struct xrt_matrix_4x4_f64 &m)
{
	return Eigen::Map<const Eigen::Matrix4d>(m.v);
}

/*!
 * @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const overload.
 *
 * Permits zero-overhead manipulation of `xrt_matrix_4x4_f64&` by Eigen routines as if it were a `Eigen::Matrix4d&`.
 */
static inline Eigen::Map<Eigen::Matrix4d>
map_matrix_4x4_f64(struct xrt_matrix_4x4_f64 &m)
{
	return Eigen::Map<Eigen::Matrix4d>(m.v);
}

/*
 *
 * Pose deconstruction helpers.
 *
 */

/*!
 * Return a Eigen type wrapping a pose's orientation (const).
 */
static inline Eigen::Map<const Eigen::Quaternionf>
orientation(const struct xrt_pose &pose)
{
	return map_quat(pose.orientation);
}

/*!
 * Return a Eigen type wrapping a pose's orientation.
 */
static inline Eigen::Map<Eigen::Quaternionf>
orientation(struct xrt_pose &pose)
{
	return map_quat(pose.orientation);
}

/*!
 * Return a Eigen type wrapping a pose's position (const).
 */
static inline Eigen::Map<const Eigen::Vector3f>
position(const struct xrt_pose &pose)
{
	return map_vec3(pose.position);
}

/*!
 * Return a Eigen type wrapping a pose's position.
 */
static inline Eigen::Map<Eigen::Vector3f>
position(struct xrt_pose &pose)
{
	return map_vec3(pose.position);
}

} // namespace xrt::auxiliary::math


// (C) Copyright Shou Hao Ho   2018
// Distributed under the MIT Software License (See accompanying LICENSE file)

#include <boost/range/combine.hpp>
#include "plan.h"

void Plan::assign_coords(const Partition &partition, const std::vector<coord> &coords, const plan_region &bound) {
    RUNTIME_ASSERT(partition.size() == coords.size());

    const Atom* atom;
    coord c;
    for (const auto &entry : boost::combine(partition, coords)) {
        boost::tie(atom, c) = entry;

        auto iter = m_board.find(atom);
        RUNTIME_ASSERT(iter != m_board.end());

        c.x = std::min(bound.first.end, c.x);
        c.x = std::max(bound.first.begin, c.x);
        c.y = std::min(bound.second.end, c.y);
        c.y = std::max(bound.second.begin, c.y);
        iter->second = c;
    }
}

void Plan::recursive_partition(bool split_vertically, partitioning_method method) {
    auto old_partitions = std::move(m_partitions);
    auto old_bounds = std::move(m_partition_bounds);

    m_partitions.reserve(old_partitions.size() * 2);
    m_partition_bounds.reserve(old_bounds.size() * 2);

    auto sort_by_coord = [&](const Atom* lhs, const Atom* rhs) {
        const coord &lhs_c = m_board.at(lhs);
        const coord &rhs_c = m_board.at(rhs);
        return split_vertically ? coord::x_major_lt(lhs_c, rhs_c) :
                                    coord::y_major_lt(lhs_c, rhs_c);
    };

    for (auto entry : boost::combine(old_partitions, old_bounds)) {
        Partition &partition = boost::get<0>(entry);
        const plan_region &region = boost::get<1>(entry);

        if (partition.size() == 0) {
            m_partition_bounds.emplace_back(region);
            m_partitions.emplace_back(std::move(partition));
        }
        else {
            auto mid_iter = partition.begin() + partition.size() / 2;
            std::partial_sort(partition.begin(), mid_iter, partition.end(), sort_by_coord);

            if (split_vertically) {
                double mid_x = (method == partitioning_method::adaptive) ? get_coord(**mid_iter).x :
                                          region.first.begin + (region.first.end - region.first.begin) / 2;
                mid_x = std::max(region.first.begin, mid_x);
                mid_x = std::min(region.first.end, mid_x);
                m_partition_bounds.emplace_back(bound{ region.first.begin, mid_x }, region.second);
                m_partition_bounds.emplace_back(bound{ mid_x, region.first.end }, region.second);
            }
            else {
                double mid_y = (method == partitioning_method::adaptive) ? get_coord(**mid_iter).y :
                                          region.second.begin + (region.second.end - region.second.begin) / 2;
                mid_y = std::max(region.second.begin, mid_y);
                mid_y = std::min(region.second.end, mid_y);
                m_partition_bounds.emplace_back(region.first, bound{ region.second.begin, mid_y });
                m_partition_bounds.emplace_back(region.first, bound{ mid_y, region.second.end });
            }

            m_partitions.emplace_back(mid_iter, partition.end());
            partition.erase(mid_iter, partition.end());
            m_partitions.emplace_back(std::move(partition));
        }
    }
}

void Plan::initial_setup() {
    Partition initial_partiton;
    initial_partiton.resize(m_netlist.num_luts() + m_netlist.num_ffs());
    auto iter = std::transform(m_netlist.begin_luts(), m_netlist.end_luts(), initial_partiton.begin(),
        [](const Atom &lut) { return &lut; });
    std::transform(m_netlist.begin_ffs(), m_netlist.end_ffs(), iter,
        [](const Atom &ff) { return &ff; });

    auto insert_initial_coord = [&](const Atom &atom) {
        m_board.emplace(&atom, coord{ 0.0, 0.0 });
    };

    m_partitions.emplace_back(std::move(initial_partiton));
    m_partition_bounds.emplace_back(bound{ 0.0, static_cast<double>(m_height) },
                                    bound{ 0.0, static_cast<double>(m_width) });

    std::for_each(m_netlist.begin_luts(), m_netlist.end_luts(), insert_initial_coord);
    std::for_each(m_netlist.begin_ffs(), m_netlist.end_ffs(), insert_initial_coord);
    RUNTIME_ASSERT(m_board.size() == m_netlist.num_luts() + m_netlist.num_ffs());
}