#define BOOST_TEST_MODULE Table with scalar fields
#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>
#include <flatboobs_test_schema/scalars.hpp>
#include <iostream>

using namespace flatboobs::schema::test;
namespace bdata = boost::unit_test::data;

class Dataset {
public:
  using sample = TestScalars;

  struct iterator {
    TestScalars value;
    iterator() { operator++(); }
    TestScalars &operator*() { return value; }
    TestScalars &operator++() {
      value = value.evolve((std::rand() % 200) - 100, // int_8
                           (std::rand() % 200) - 100, // int_16
                           (std::rand() % 200) - 100, // int_32
                           (std::rand() % 200) - 100, // int_64
                           (std::rand() % 200),       // unit_8
                           (std::rand() % 200),       // uint_16
                           (std::rand() % 200),       // uint_32
                           (std::rand() % 200),       // uint_64

                           (std::rand() / 1000), // float_32
                           (std::rand() / 1000), // float_64

                           (std::rand() % 2), // bool_true
                           (std::rand() % 2)  // bool_false
      );
      return value;
    }
  };

  bdata::size_t size() const { return 10; }
  iterator begin() const { return iterator(); }
};

namespace boost {
namespace unit_test {
namespace data {
namespace monomorphic {
template <> struct is_dataset<Dataset> : boost::mpl::true_ {};
} // namespace monomorphic
} // namespace data
} // namespace unit_test
}; // namespace boost

BOOST_AUTO_TEST_CASE(test_default_values) {

  TestScalars table{};

  BOOST_TEST(table.int_8() == -8);
  BOOST_TEST(table.int_16() == -16);
  BOOST_TEST(table.int_32() == 0);
  BOOST_TEST(table.int_64() == 0);
  BOOST_TEST(table.uint_8() == 0);
  BOOST_TEST(table.uint_16() == 0);
  BOOST_TEST(table.uint_32() == 0);
  BOOST_TEST(table.uint_64() == 0);
  BOOST_TEST(table.float_32() == 0);
  BOOST_TEST(table.float_64() == 0);
  BOOST_TEST(table.bool_true() == true);
  BOOST_TEST(table.bool_false() == false);
}

BOOST_DATA_TEST_CASE(test_copy, Dataset()) {

  TestScalars table{sample};

  BOOST_TEST(table.int_8() == sample.int_8());
  BOOST_TEST(table.int_16() == sample.int_16());
  BOOST_TEST(table.int_32() == sample.int_32());
  BOOST_TEST(table.int_64() == sample.int_64());

  BOOST_TEST(table.uint_8() == sample.uint_8());
  BOOST_TEST(table.uint_16() == sample.uint_16());
  BOOST_TEST(table.uint_32() == sample.uint_32());
  BOOST_TEST(table.uint_64() == sample.uint_64());

  BOOST_TEST(table.float_32() == sample.float_32());
  BOOST_TEST(table.float_64() == sample.float_64());

  BOOST_TEST(table.bool_true() == sample.bool_true());
  BOOST_TEST(table.bool_false() == sample.bool_false());

  BOOST_TEST(table == sample);
}

BOOST_DATA_TEST_CASE(test_evolve, Dataset()) {

  TestScalars lhs{sample};
  TestScalars rhs{sample};
  BOOST_TEST(lhs == rhs);

  lhs = lhs.evolve({},               // int_8:byte=-8;
                   {},               // int_16:short=-16;
                   {},               // int_32:int;
                   lhs.int_64() + 1, // int_64:long;
                   {},               // uint_8:ubyte;
                   {},               // uint_16:ushort;
                   {},               // uint_32:uint;
                   {},               // uint_64:ulong;
                   {},               // float_32:float;
                   {},               // float_64:double;
                   {},               // bool_true:bool=true;
                   {}                // bool_false:bool;
  );
  BOOST_TEST(lhs != rhs);
  BOOST_TEST(lhs.int_64() - 1 == rhs.int_64());

  lhs = lhs.evolve({},               // int_8:byte=-8;
                   {},               // int_16:short=-16;
                   {},               // int_32:int;
                   lhs.int_64() - 1, // int_64:long;
                   {},               // uint_8:ubyte;
                   {},               // uint_16:ushort;
                   {},               // uint_32:uint;
                   {},               // uint_64:ulong;
                   {},               // float_32:float;
                   {},               // float_64:double;
                   {},               // bool_true:bool=true;
                   {}                // bool_false:bool;
  );
  BOOST_TEST(lhs == rhs);
}

BOOST_DATA_TEST_CASE(test_pack_unpack, Dataset()) {

  auto data = flatboobs::pack(sample);
  TestScalars result = flatboobs::unpack<TestScalars>(data);

  BOOST_TEST(result == sample);
}


#include <algorithm>
#include <boost/functional/hash.hpp>
#include <functional>
#include <limits>
#include <queue>
#include <sstream>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <doctest/doctest.h>

#include "solver_15_part1.h"

namespace {

using Coordinate = std::pair<unsigned int, unsigned int>;

class Vertex
{
private:
  Coordinate position_;
  Coordinate parent_position_;// Next vertex back that continues current lowest cost path that was found
  unsigned int g_ = std::numeric_limits<unsigned int>::max();// Current lowest total cost to travel from start to this vertex (including this vertex's cost)
  unsigned int h_;// Pre-computed heuristic - Manhattan distance from vertex to end (equal to shortest path from this vertex to end if all costs are 1)
  unsigned int cost_;// Cost of entering this vertex

  // Lowest g + h = f is the next vertex we consider in the search
  [[nodiscard]] unsigned int f() const
  {
    return g_ + h_;
  };

public:
  explicit Vertex(const Coordinate position, const unsigned int cost, const unsigned int h)
    : position_(position), h_(h), cost_(cost) {}

  // Used for priority queue minimum
  friend bool operator>(const Vertex &lhs, const Vertex &rhs) { return lhs.f() > rhs.f(); }

  [[nodiscard]] Coordinate position() const
  {
    return position_;
  }

  Coordinate &parent_position() { return parent_position_; }

  unsigned int &g() { return g_; }
  [[nodiscard]] unsigned int g() const { return g_; }

  unsigned int &cost() { return cost_; }
};

std::vector<Coordinate> getNeighborCoords(Coordinate center, unsigned int width, unsigned int height)
{
  std::vector<Coordinate> neighbors;

  const auto &x = center.first;
  const auto &y = center.second;

  if (x > 0) {
    neighbors.emplace_back(std::make_pair(x - 1, y));
  }

  if (y > 0) {
    neighbors.emplace_back(std::make_pair(x, y - 1));
  }

  if (x < width - 1) {
    neighbors.emplace_back(std::make_pair(x + 1, y));
  }

  if (y < height - 1) {
    neighbors.emplace_back(std::make_pair(x, y + 1));
  }

  return neighbors;
}

}// namespace

std::string Solver_15_part1::solve(std::istream &is)
{
  std::vector<std::tuple<unsigned int, unsigned int, unsigned int>> inputs;

  unsigned int width = 0;
  unsigned int height = 0;

  // Ingest the map
  {
    unsigned int x = 0;
    unsigned int y = 0;

    char input = 0;
    while (is.get(input)) {
      if (input == ' ') {
        continue;
      }

      if (input == '\n') {
        if (x > width) {
          width = x;
        }
        if (x != 0) {
          y += 1;
        }
        x = 0;
      } else {
        int cost = input - '0';
        if (cost < 0 || cost > 9) {
          throw solver_runtime_error("Input out of range");
        }

        inputs.emplace_back(std::make_tuple(x, y, cost));

        x += 1;
      }
    }
    height = x == 0 ? y : y + 1;
  }

  unsigned int goal_x = width - 1;
  unsigned int goal_y = height - 1;

  Coordinate start = std::make_pair(0, 0);
  Coordinate goal = std::make_pair(goal_x, goal_y);

  // Now that we know the width / height we can precompute h (distance) and populate the full map
  std::unordered_map<Coordinate, Vertex, boost::hash<Coordinate>> cave_map;

  for (const auto &[x, y, cost] : inputs) {
    const Coordinate position = std::make_pair(x, y);

    // Manhattan distance heuristic for 4-movement grid: |goal_x - vertex_x| + |goal_y - vertex_y|
    // We don't need to do abs (||) op, because we assume the goal is always at the bottom right corner
    const auto h = (goal_x - x) + (goal_y - y);

    cave_map.emplace(position, Vertex(position, cost, h));
  }

  std::priority_queue<Vertex, std::vector<Vertex>, std::greater<>> open_list;
  std::unordered_set<Coordinate, boost::hash<Coordinate>> open_set;
  std::unordered_set<Coordinate, boost::hash<Coordinate>> closed_set;

  cave_map.at(start).g() = 0;
  open_list.push(cave_map.at(start));// No need to place it on set too, it will just be immediately erased

  // Use A* search to find guaranteed shortest path
  // Assuming A* w/ Manhattan distance will work here might be incorrect? It solved part 1
  // but brought unexpected problems in part 2.
  while (!open_list.empty()) {
    const Vertex vertex = open_list.top();
    open_list.pop();
    open_set.erase(vertex.position());

    if (vertex.position() == goal) {
      // Goal found! Trace path and end
      unsigned int cost = 0;

      // Don't count start cost
      for (Coordinate c = vertex.position(); c != start; c = cave_map.at(c).parent_position()) {
        cost += cave_map.at(c).cost();
      }

      return std::to_string(cost);
    }

    // Only visit every vertex once
    if (closed_set.find(vertex.position()) != closed_set.end()) {
      continue;
    }
    closed_set.insert(vertex.position());

    // Now look at all vertex's neighbors and update their g if new g through this vertex is lower (even if neighbor's been visited)
    for (const auto &neighbor_coord : getNeighborCoords(vertex.position(), width, height)) {
      auto &neighbor_vertex = cave_map.at(std::make_pair(neighbor_coord.first, neighbor_coord.second));

      const auto new_g = neighbor_vertex.cost() + vertex.g();

      if (new_g < neighbor_vertex.g()) {
        neighbor_vertex.g() = new_g;
        neighbor_vertex.parent_position() = vertex.position();
      }

      if (closed_set.find(neighbor_coord) == closed_set.end() && open_set.find(neighbor_coord) == open_set.end()) {
        open_list.push(neighbor_vertex);
        open_set.insert(neighbor_coord);
      }
    }
  }

  throw solver_runtime_error("Could not find a path to the goal");
}

TEST_CASE("testing solver for day 15 part 1 - path through chiton cave")
{
  Solver_15_part1 solver;

  {
    std::istringstream is(std::string{ R"(
      1163751742
      1381373672
      2136511328
      3694931569
      7463417111
      1319128137
      1359912421
      3125421639
      1293138521
      2311944581
    )" });

    CHECK(solver.solve(is) == "40");
  }

  {
    std::istringstream is(std::string{ R"(
      19999
      19111
      11191
    )" });

    CHECK(solver.solve(is) == "8");
  }

  {
    std::istringstream is(std::string{ R"(
      59999
      19111
      11195
    )" });

    CHECK(solver.solve(is) == "12");
  }
}


/*
 * fluidTracks.cpp
 *
 *  Created on: Jun 17, 2013
 *      Author: kthierbach
 */

#include "fluidTracks.hpp"

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <iostream>

#include "componentsMeasurements.hpp"
#include "connectedComponents.hpp"
#include "graphcut.hpp"
#include "labeling.hpp"
#include "utils/vectorArray2D.hpp"
#include "utils/utilities.hpp"

namespace elib
{

using std::vector;
using std::unordered_map;
using std::string;

FluidTracks::FluidTracks()
{
	divisions = new vector<glm::ivec2>;
	frames = new vector<MaskList<int, glm::ivec2> >();
}
FluidTracks::FluidTracks(Parameters *params, vector<string> *images, vector<string> *flows)
: params(params), images(images), flows(flows)
{
	divisions = new vector<glm::ivec2>;
	frames = new vector<MaskList<int, glm::ivec2> >();
}
FluidTracks::~FluidTracks()
{
	delete divisions;
	delete frames;
}

void FluidTracks::addAppearingObjects(MaskList<int, glm::ivec2> &masks)
{
	ComponentsMeasurements cm;
	ConnectedComponents cc;
	Image<int> label_image;
	std::shared_ptr<Mask<glm::ivec2>> new_objects;
	MaskList<int, glm::ivec2> new_object_masks(initial.getRank(), *(initial.getDimensions()));

	new_objects = masks.getMask(1);
	if(new_objects != nullptr)
	{
		label_image = cc.getComponents(new_objects->toImage());
		cm = ComponentsMeasurements(label_image);
		new_object_masks = cm.getMasks();
		applySizeConstraints(new_object_masks);
		for(auto it=new_object_masks.begin(); it!=new_object_masks.end(); ++it)
		{
			masks.addMask(id_counter++, *(it->second));
		}
		masks.deleteMask(1);
	}
}

void FluidTracks::applySizeConstraints(MaskList<int, glm::ivec2> &masks)
{
	vector<int> for_deletion;
	int size;

	auto it = masks.begin();
	while(it != masks.end())
	{
		size = it->second->getSize();
		if(size < min_object_size || size > max_object_size)
		{
			for_deletion.push_back(it->first);
			++it;
		}
		else
			++it;
	}
	masks.deleteMasks(for_deletion);

}

MaskList<int, glm::ivec2> FluidTracks::assignLabels(const Image<int> &image, MaskList<int, glm::ivec2> &old_labels, MaskList<int, glm::ivec2> &segmentations)
{
	using namespace boost::numeric::ublas;

	MaskList<int, glm::ivec2> labeled_masks(old_labels.getRank(), *old_labels.getDimensions());
	std::vector<int> labels;
	std::shared_ptr<compressed_matrix<int>> adjacency;
	std::vector<int> 	segmentation_index_to_id(segmentations.getSize()),
						old_labels_index_to_id(old_labels.getSize());
	std::shared_ptr<Mask<glm::ivec2>> seg_mask, label_mask;

	int old_labels_index = 0;
	for(auto i=old_labels.getLabels()->begin(); i!=old_labels.getLabels()->end(); ++i, ++old_labels_index)
	{
		old_labels_index_to_id[old_labels_index] = *i;
	}
	int segmentation_index = 0;
	for(auto i=segmentations.getLabels()->begin(); i!=segmentations.getLabels()->end(); ++i, ++segmentation_index)
	{
		segmentation_index_to_id[segmentation_index] = *i;
	}

	adjacency = computeAdjacency(old_labels, segmentations);

	std::vector<bool> visited(segmentations.getSize(), false);
	std::shared_ptr<Mask<glm::ivec2>> mask;

	for (auto it1 = adjacency->begin1(); it1 != adjacency->end1(); ++it1)
	{
		if(!visited[it1.index1()])
		{
			std::pair<std::list<int>,std::list<int>> tmp = getAssociations(*adjacency, it1.index1());

			if(tmp.second.empty()) // add new object or not
			{
				if(include_appearing)
				{
					mask = segmentations.getMask(segmentation_index_to_id[it1.index1()]);
					if(mask != nullptr)
					{
						labeled_masks.addMask(id_counter++, *mask);
					}
					else
					{
						std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
						abort();
					}
				}
				visited[it1.index1()]=true;
			}
			else if(tmp.first.empty() && tmp.second.size()==1) // unique association
			{
				mask = segmentations.getMask(segmentation_index_to_id[it1.index1()]);
				if(mask != nullptr)
				{
					labeled_masks.addMask(old_labels_index_to_id[tmp.second.front()], *mask);
				}
				else
				{
					std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
					abort();
				}
				visited[it1.index1()]=true;
			}
			else if(!tmp.first.empty() && tmp.second.size()==1) // division
			{
				for(auto i = tmp.first.begin(); i!=tmp.first.end(); ++i)
				{
					visited[*i] = true;
					divisions->push_back(glm::ivec2(old_labels_index_to_id[tmp.second.front()], id_counter));
					mask = segmentations.getMask(segmentation_index_to_id[*i]);
					if(mask != nullptr)
					{
						labeled_masks.addMask(id_counter++, *mask);
					}
					else
					{
						std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
						abort();
					}
				}
				visited[it1.index1()]=true;
			}
			else // resolve ambiguities
			{
				MaskList<int, glm::ivec2> 	masks(old_labels.getRank(), *old_labels.getDimensions()),
											tmp_masks;

				mask = segmentations.getMask(segmentation_index_to_id[it1.index1()]);
				if(mask != nullptr)
				{
					visited[it1.index1()]=true;
					masks.addMask(0, *mask);
				}
				else
				{
					std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
					abort();
				}
				int index = 1;
				for(auto i = tmp.first.begin(); i != tmp.first.end(); ++i)
				{
					mask = segmentations.getMask(segmentation_index_to_id[*i]);
					if(mask != nullptr)
					{
						visited[*i]=true;
						masks.addMask(index++, *mask);
					}
					else
					{
						std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
						abort();
					}

				}
				for(auto i = tmp.second.begin(); i != tmp.second.end(); ++i)
				{
					mask = old_labels.getMask(old_labels_index_to_id[*i]);
					if(mask != nullptr)
					{
						masks.addMask(index++, *mask);
					}
					else
					{
						std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
						abort();
					}
				}
				Mask<glm::ivec2> fused = masks.fuse();
				BoundingBox<glm::ivec2> bounding_box = fused.getBoundingBox();
				Image<int> label_image = old_labels.toImage();
				std::shared_ptr<Image<int>> image_part = image.imageTake(bounding_box),
											label_tmp_part = label_image.imageTake(bounding_box);

				ComponentsMeasurements cm = ComponentsMeasurements(*label_tmp_part);
				tmp_masks = cm.getMasks();
				masks = MaskList<int, glm::ivec2>(tmp_masks.getRank(), *tmp_masks.getDimensions());
				for(auto i = tmp.second.begin(); i != tmp.second.end(); ++i)
				{
					mask = tmp_masks.getMask(old_labels_index_to_id[*i]);
					if(mask != nullptr)
					{
						masks.addMask(old_labels_index_to_id[*i], *mask);
					}
					else
					{
						std::cerr << "Error in FluidTracks::assignLabels:" << __LINE__ << std::endl;
						abort();
					}
				}
				Image<int> label_image_part = masks.toImage();
				params->addParameter("NumberLabels", int(tmp.second.size()+2));
				Labeling lbg;
				std::shared_ptr<Image<int>> new_label_image = lbg.labeling(label_image_part, *image_part, *params);
				cm = ComponentsMeasurements(*new_label_image);
				masks = cm.getMasks();
				masks.deleteMask(1);
				detectDivisions(masks);
				masks.setRank(old_labels.getRank());
				masks.setDimensions(*old_labels.getDimensions());
				masks.setOrigin(bounding_box.getUpperLeft());
				labeled_masks.addMasks(masks);
			}
		}
	}
	return labeled_masks;
}

std::shared_ptr<boost::numeric::ublas::compressed_matrix<int>> FluidTracks::computeAdjacency(MaskList<int, glm::ivec2> &old_labels, MaskList<int, glm::ivec2> &segmentations)
{
	using namespace boost::numeric::ublas;

	std::shared_ptr<compressed_matrix<int>> adjacency = std::shared_ptr<compressed_matrix<int>>(new compressed_matrix<int>(segmentations.getSize(), old_labels.getSize()));
	std::vector<int> 	segmentation_index_to_id(segmentations.getSize()),
						old_labels_index_to_id(old_labels.getSize());
	std::shared_ptr<Mask<glm::ivec2>> seg_mask, label_mask;
	int old_labels_index = 0;
	for(auto i=old_labels.getLabels()->begin(); i!=old_labels.getLabels()->end(); ++i, ++old_labels_index)
	{
		old_labels_index_to_id[old_labels_index] = *i;
	}
	int segmentation_index = 0;
	for(auto i=segmentations.getLabels()->begin(); i!=segmentations.getLabels()->end(); ++i, ++segmentation_index)
	{
		segmentation_index_to_id[segmentation_index] = *i;
		old_labels_index = 0;
		for(auto j=old_labels.getLabels()->begin(); j!=old_labels.getLabels()->end(); ++j, ++old_labels_index)
		{
			seg_mask = segmentations.getMask(*i);
			label_mask = old_labels.getMask(*j);
			if(seg_mask->overlap(*label_mask))
			{
				adjacency->insert_element(segmentation_index, old_labels_index,1);
			}
		}
	}
	return adjacency;
}

std::pair<std::list<int>,std::list<int>> FluidTracks::getAssociations(boost::numeric::ublas::compressed_matrix<int> &adjacency, int id)
{
	using namespace boost::numeric::ublas;
	std::pair<std::list<int>,std::list<int>> labels;
	std::list<int> row_labels, column_labels, tmp;
	int current_row, current_column;
	std::vector<bool> visited_rows(adjacency.size1()), visited_columns(adjacency.size2());

	current_row = id;
	row_labels=getRowAssociations(adjacency, id);
	labels.second.insert(labels.second.end(), row_labels.begin(), row_labels.end());
	while(!row_labels.empty())
	{
		current_column =  row_labels.front();
		if(!visited_columns[current_column])
		{
			visited_columns[current_column] = true;
			row_labels.pop_front();
			tmp = getColumnAssociations(adjacency, current_column);
			tmp.remove(current_row);
			labels.first.insert(labels.first.end(), tmp.begin(), tmp.end());
			column_labels.insert(column_labels.end(), tmp.begin(), tmp.end());
		}
		else
		{
			row_labels.pop_front();
		}

		while(!column_labels.empty())
		{
			current_row = column_labels.front();
			if(!visited_rows[current_row])
			{
				visited_rows[current_row] = true;
				column_labels.pop_front();
				tmp = getRowAssociations(adjacency, current_row);
				tmp.remove(current_column);
				labels.second.insert(labels.second.end(), tmp.begin(), tmp.end());
				row_labels.insert(row_labels.end(), tmp.begin(), tmp.end());
			}
			else
			{
				column_labels.pop_front();
			}
		}
	}
	labels.first.sort();
	labels.first.unique();
	labels.second.sort();
	labels.second.unique();

	return labels;
}

std::list<int> FluidTracks::getColumnAssociations(boost::numeric::ublas::compressed_matrix<int> &adjacency, int id)
{
	using namespace boost::numeric::ublas;
	std::list<int> labels;

	matrix_column<compressed_matrix<int>> column(adjacency, id);
	for(auto i=column.begin(); i!=column.end(); ++i)
	{
		labels.push_back(i.index());
	}

	return labels;
}

std::list<int> FluidTracks::getRowAssociations(boost::numeric::ublas::compressed_matrix<int> &adjacency, int id)
{
	using namespace boost::numeric::ublas;
	std::list<int> labels;

	matrix_row<compressed_matrix<int>> row(adjacency, id);
	for(auto i=row.begin(); i!=row.end(); ++i)
	{
		labels.push_back(i.index());
	}

	return labels;
}

void FluidTracks::detectDivisions(MaskList<int, glm::ivec2> &masks)
{
	ConnectedComponents cc;
	ComponentsMeasurements cm;
	Image<int> label_image;
	MaskList<int, glm::ivec2> 	objects(initial.getRank(),*(initial.getDimensions())),
								tmp_masks(initial.getRank(), *(initial.getDimensions()));
	vector<int> for_deletion;
	int max;

	auto it= masks.begin();
	while(it!=masks.end())
	{
		label_image = cc.getComponents(it->second->toImage());
		max = label_image.max();
		if(max > 1)
		{
			for_deletion.push_back(it->first);
			cm = ComponentsMeasurements(label_image);
			objects = cm.getMasks();
			applySizeConstraints(objects);
			if(objects.getSize() == 1)//no real division
			{
				tmp_masks.addMask(it->first, *(objects.begin()->second));
			}
			if(objects.getSize() > 1)
			{
				for(auto it2=objects.begin(); it2!=objects.end(); ++it2)
				{
					divisions->push_back(glm::ivec2(it->first, id_counter));
					tmp_masks.addMask(id_counter++,*(it2->second));
				}
			}
		}
		++it;
	}
	masks.deleteMasks(for_deletion);
	masks.addMasks(tmp_masks);
}

void FluidTracks::track()
{
	ConnectedComponents cc;
	ComponentsMeasurements cm;
	Image<int> old_label;
	std::shared_ptr<Image<int>> image, propagated_label, tmp;
	Labeling lbg;
	MaskList<int, glm::ivec2> masks;

	cc.setLabelOffset(2);
	if(initial_mask_image.compare("") == 0)
	{
		tmp = Image<int>::openImage((*images)[0]);
		if(tmp == nullptr)
		{
			std::cerr << "Image " << (*images)[0] << " not found!" << std::endl;
			abort();
		}
		else
		{
			initial = *tmp;
		}
		Image<int> emptyLabel(initial.getRank(), *(initial.getDimensions()), 16, initial.getChannels());
		propagated_label = graphcut(initial, *params);
		initial = *propagated_label;
		initial = cc.getComponents(initial);
		cm = ComponentsMeasurements(initial);
		masks = cm.getMasks();
		applySizeConstraints(masks);
		masks.relabel(1);
		initial = masks.toImage();
	}
	else /* open initial label image */
	{
		tmp = Image<int>::openImage(initial_mask_image);
		if(tmp == nullptr)
		{
			std::cerr << "Initial image " << initial_mask_image << " not found!" << std::endl;
			abort();
		}
		else
		{
			initial = *tmp;
		}
		cm = ComponentsMeasurements(initial);
		masks = cm.getMasks();
		applySizeConstraints(masks);
		masks.relabel(1);
		initial = masks.toImage();
	}
	old_label = Image<int>(initial);
	cm = ComponentsMeasurements(old_label);
	masks = cm.getMasks();
	frames->push_back(masks);
	id_counter = *(--masks.getLabels()->end())+1;
	VectorArray2D va;
	std::cout << "\r" << "frame: " << 0 << ", #objects: " << masks.getSize() << std::flush;
	for(unsigned int i=1; i<images->size(); ++i)
	{
		std::cout << "\r" << "frame: " << i << ", #objects: " << masks.getSize() << std::flush;
		if(flows->size() != 0)
		{
			va.load((*flows)[i-1].c_str());
			old_label.displaceByVectorField(va);
			cm = ComponentsMeasurements(old_label);
			masks = cm.getMasks();
		}
		image = Image<int>::openImage((*images)[i]);
		if(image == nullptr)
		{
			std::cerr << "Image " << (*images)[i] << " not found!" << std::endl;
			abort();
		}
		propagated_label = graphcut(*image, *params);
		cc.setLabelOffset();
		Image<int> components = cc.getComponents(*propagated_label);
		cm = ComponentsMeasurements(components);
		masks = cm.getMasks();
		applySizeConstraints(masks);
		masks = assignLabels(*image, frames->back(), masks);
		applySizeConstraints(masks);
		masks.deleteSparseRepresentations();
		frames->push_back(masks);
		old_label = masks.toImage();

	}
	std::cout << "\r" << "finished successfully" << std::string(50,' ') << std::endl;
}

Image<int>* FluidTracks::getInitial()
{
	return &initial;
}

vector<MaskList<int, glm::ivec2>>* FluidTracks::getFrames()
{
	return frames;
}

int FluidTracks::getMaxObjectSize() const
{
	return max_object_size;
}

void FluidTracks::setMaxObjectSize(int maxObjectSize)
{
	max_object_size = maxObjectSize;
}

} /* namespace elib */


#pragma once

#include <El.hpp>

#include <boost/filesystem.hpp>

inline void
set_element(El::DistMatrix<El::BigFloat> &block, const int64_t &row,
            const int64_t &column, const El::BigFloat &element)
{
  if(block.IsLocal(row, column))
    {
      block.SetLocal(block.LocalRow(row), block.LocalCol(column),
                     El::BigFloat(element));
    }
}

inline void set_element(El::Matrix<El::BigFloat> &block, const int64_t &row,
                        const int64_t &column, const El::BigFloat &element)
{
  block(row, column) = element;
}

template <typename Matrix>
void read_text_block(Matrix &block, const boost::filesystem::path &block_path)
{
  boost::filesystem::ifstream block_stream(block_path);
  if(!block_stream)
    {
      throw std::runtime_error("Unable to open checkpoint file: '"
                               + block_path.string() + "'");
    }
  int64_t file_height, file_width;
  block_stream >> file_height >> file_width;
  if(!block_stream.good())
    {
      throw std::runtime_error("Corrupted header in file: "
                               + block_path.string());
    }
  if(file_height != block.Height() || file_width != block.Width())
    {
      std::stringstream ss;
      ss << "Incompatible checkpoint file: '" << block_path.string()
         << "'.  Expected dimensions (" << block.Height() << ","
         << block.Width() << "), but found (" << file_height << ","
         << file_width << ")";
      throw std::runtime_error(ss.str());
    }

  std::string element;
  for(int64_t row = 0; row < file_height; ++row)
    for(int64_t column = 0; column < file_width; ++column)
      {
        block_stream >> element;
        set_element(block, row, column, El::BigFloat(element));
      }
  if(!block_stream.good())
    {
      throw std::runtime_error("Corrupted data in file: "
                               + block_path.string());
    }
}

template <typename Matrix>
void read_text_block(Matrix &block,
                     const boost::filesystem::path &block_directory,
                     const std::string &prefix, const size_t &block_index)
{
  read_text_block(block, block_directory
                           / (prefix + std::to_string(block_index) + ".txt"));
}


// Copyright 2005 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/transitive_closure.hpp>
#include "graph_types.hpp"
#include <boost/python.hpp>
#include <list>
#include <iterator>

namespace boost { namespace graph { namespace python {

template<typename Graph>
boost::python::object
transitive_closure
  (const Graph& g,
   vector_property_map<
     typename graph_traits<Graph>::vertex_descriptor,
     typename property_map<Graph, vertex_index_t>::const_type>* g_to_tc_map)
{
  boost::python::object result = Graph::pyconstruct();
  Graph& tc = boost::python::extract<Graph&>(result)();
  if (g_to_tc_map)
    boost::transitive_closure(g, tc, *g_to_tc_map, get(vertex_index, g));
  else
    boost::transitive_closure(g, tc);
  return result;
}

void export_transitive_closure()
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::manage_new_object;
  using boost::python::return_value_policy;

#define UNDIRECTED_GRAPH(Name,Type)
#define DIRECTED_GRAPH(Name,Type)                                       \
  {                                                                     \
    typedef vector_property_map<                                        \
              graph_traits<Type>::vertex_descriptor,                    \
              property_map<Type, vertex_index_t>::const_type>           \
      OrigToCopyMap;                                                    \
                                                                        \
    def("transitive_closure", &transitive_closure<Type>,                \
        (arg("graph"), arg("orig_to_copy") = static_cast<OrigToCopyMap*>(0))); \
  }
#include "graphs.hpp"
}

} } } // end namespace boost::graph::python


#include <iostream>

#include <boost/lexical_cast.hpp>

#include "vertices_io.h"

using namespace std;
using namespace boost;

int main(int argc, char ** argv)
{
    if (argc < 4)
    {
        cerr << "usage select_edges <error-low> <error-hi> <weight-low>\n";
        return 1;
    }
    double err_lo = lexical_cast<double>(argv[1]);
    double err_hi = lexical_cast<double>(argv[2]);
    size_t weight_lo = lexical_cast<size_t>(argv[3]);

    size_t edges_num = 0;
    while (1)
    {
        size_t from = read_vertex_vertex(cin); size_t to = read_vertex(cin);
        double distance, error;
        cin >> distance >> error;
        size_t weight;
        cin >> weight;
        if (!cin)
            break;

        if ((error < err_lo) || (error > err_hi) || (weight < weight_lo))
        {
            continue;
        }

        ++edges_num;
        print_vertex(cout, from);
        cout << " ";
        print_vertex(cout, to);
        cout << " " << distance << " " << error << " " << weight << "\n";
    }

    cerr << edges_num << " edges ok\n";

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_ITL_FWD_INCLUDE
#define ITL_ITL_FWD_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>

namespace itl {

    template <class Real>                class basic_iteration;
    template <class Real, class OStream> class cyclic_iteration;
    template <class Real, class OStream> class noisy_iteration;

    template <typename Solver, typename VectorIn, bool trans> class solver_proxy;

    namespace pc {

	template <typename PC, typename Vector, bool> struct solver;
	template <typename Matrix, typename Value> class identity;
	// template <typename Matrix, typename Value, typename Vector> Vector solve(const identity<Matrix>&, const Vector& x);
	// template <typename Matrix, typename Vector> Vector adjoint_solve(const identity<Matrix>&, const Vector& x);

	template <typename Matrix, typename Value> class diagonal;
	// template <typename Matrix, typename Vector> Vector solve(const diagonal<Matrix>& P, const Vector& x);
	// template <typename Matrix, typename Vector> Vector adjoint_solve(const diagonal<Matrix>& P, const Vector& x);

	template <typename Matrix, typename Factorizer, typename Value> class ilu;
	template <typename Matrix, typename Value> class ilu_0; // Maybe we should declare the default here???
	template <typename Matrix, typename Value> class ilut; // Maybe we should declare the default here???
	// template <typename Matrix, typename Vector> Vector solve(const ilu_0<Matrix>& P, const Vector& x);
	// template <typename Matrix, typename Vector> Vector adjoint_solve(const ilu_0<Matrix>& P, const Vector& x);

	template <typename Matrix, typename Value> class ic_0; // Maybe we should declare the default here???
	// template <typename Matrix, typename Vector> Vector solve(const ic_0<Matrix>& P, const Vector& x);
	// template <typename Matrix, typename Value, typename Vector> Vector adjoint_solve(const ic_0<Matrix, Value>& P, const Vector& x);

    } //  namespace pc

    template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
	       typename Preconditioner, typename Iteration >
    int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
	   const Preconditioner& M, Iteration& iter);

    template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator, double>,
	       typename RightPreconditioner= pc::identity<LinearOperator, double> >
    class cg_solver;

    template < typename LinearOperator, typename Vector,
	       typename Preconditioner, typename Iteration >
    int bicg(const LinearOperator &A, Vector &x, const Vector &b,
	     const Preconditioner &M, Iteration& iter);

    template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	       class Preconditioner, class Iteration >
    int bicgstab(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
		 const Preconditioner& M, Iteration& iter);

    template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	       class Preconditioner, class Iteration >
    int bicgstab_2(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
		   const Preconditioner& M, Iteration& iter);

    template < typename LinearOperator, typename Vector,
	       typename LeftPreconditioner, typename RightPreconditioner,
	       typename Iteration >
    int bicgstab_ell(const LinearOperator &A, Vector &x, const Vector &b,
		     const LeftPreconditioner &L, const RightPreconditioner &R,
		     Iteration& iter, size_t l);

    template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator, double>,
	       typename RightPreconditioner= pc::identity<LinearOperator, double> >
    class bicgstab_ell_solver;

    template <typename Solver, unsigned N, bool Stored= false>
    class repeating_solver;

} // namespace itl

#endif // ITL_ITL_FWD_INCLUDE


#ifndef BOOST_METAPARSE_V1_UTIL_DIGIT_TO_INT_C_HPP
#define BOOST_METAPARSE_V1_UTIL_DIGIT_TO_INT_C_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/metaparse/v1/error/digit_expected.hpp>

#include <boost/mpl/int.hpp>

namespace boost
{
  namespace metaparse
  {
    namespace v1
    {
      namespace util
      {
        template <char C>
        struct digit_to_int_c : error::digit_expected {};

        template <> struct digit_to_int_c<'0'> : boost::mpl::int_<0> {};
        template <> struct digit_to_int_c<'1'> : boost::mpl::int_<1> {};
        template <> struct digit_to_int_c<'2'> : boost::mpl::int_<2> {};
        template <> struct digit_to_int_c<'3'> : boost::mpl::int_<3> {};
        template <> struct digit_to_int_c<'4'> : boost::mpl::int_<4> {};
        template <> struct digit_to_int_c<'5'> : boost::mpl::int_<5> {};
        template <> struct digit_to_int_c<'6'> : boost::mpl::int_<6> {};
        template <> struct digit_to_int_c<'7'> : boost::mpl::int_<7> {};
        template <> struct digit_to_int_c<'8'> : boost::mpl::int_<8> {};
        template <> struct digit_to_int_c<'9'> : boost::mpl::int_<9> {};
      }
    }
  }
}

#endif


#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include "sensor_msgs/PointCloud.h"
#include "tf/message_filter.h"
#include "laser_geometry/laser_geometry.h"
#include <Eigen/Eigen>
#include "std_msgs/String.h"

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node;

  ros::Publisher cloud_pub = node.advertise<sensor_msgs::PointCloud>("goal", 50);

  tf::TransformListener listener;

  ros::Rate rate(10.0);
   std::vector<std::string> list;
   list.clear();
   list.push_back("/goal");

  while (node.ok()){
    tf::StampedTransform transform;
    try{
      sensor_msgs::PointCloud cloud;
      cloud.header.stamp = ros::Time::now();
      cloud.header.frame_id = "/base_link";

       for ( int i=0 ; i < list.size() ; i++)// FOR EACH OBSTACLE
      {
       listener.waitForTransform("/base_link",list[i], ros::Time(0), ros::Duration(10.0) );
       listener.lookupTransform("/base_link",list[i], ros::Time(0), transform);

       geometry_msgs::Point32 point;
       point.x =transform.getOrigin().x();
       point.y =transform.getOrigin().y();
       point.z =transform.getOrigin().z();
       cloud.points.push_back(point);

      //we'll also add an intensity channel to the cloud
       sensor_msgs::ChannelFloat32 intensity_channel;
       intensity_channel.name="intensity";
       intensity_channel.values.push_back(1.0);
       cloud.channels.push_back(intensity_channel);
      }
      cloud_pub.publish(cloud);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
    }

    rate.sleep();
  }
  return 0;
};