#include <algorithm>
#include <iostream>
#include <vector>

#include <benchmark/benchmark.h>

#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/pixel_numeric_operations.hpp>

using namespace boost::gil;

static void std_algo(benchmark::State &state) {
  std::vector<uint16_t> img(1024 * 1024, 0);

  for (auto _ : state)
    // The code to benchmark
    for (auto &&p : img)
      p += 1;

  if (!std::all_of(img.begin(), img.end(),
                   [&state](auto p) { return p == state.iterations(); }))
    state.SkipWithError("std_algo wrong result");
}
BENCHMARK(std_algo);

static void gil_algo_stdfunction(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});

  // Pixel operation
  auto op = std::bind(
      gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>(), _1,
      gray16_pixel_t{1});

  for (auto _ : state)
    // The code to benchmark
    gil::transform_pixels(gil::const_view(img), gil::view(img), op);

  if (!std::all_of(gil::view(img).begin(), gil::view(img).end(),
                   [&state](auto p) { return p == state.iterations(); }))
    state.SkipWithError("gil_algo_stdfunction wrong result");
}
BENCHMARK(gil_algo_stdfunction);

static void gil_algo_lambda(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});

  // Pixel operation
  auto op2 = [second_arg = gray16_pixel_t{1}](gray16_pixel_t value) {
    return gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>()(
        value, second_arg);
  };

  for (auto _ : state)
    // The code to benchmark
    gil::transform_pixels(gil::const_view(img), gil::view(img), op2);

  if (!std::all_of(gil::view(img).begin(), gil::view(img).end(),
                   [&state](auto p) { return p == state.iterations(); }))
    state.SkipWithError("gil_algo_lambda wrong result");
}
BENCHMARK(gil_algo_lambda);

static void gil_algo_lambda_plus(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});

  // Pixel operation
  auto op2 = [second_arg = gray16_pixel_t{1}](gray16_pixel_t value) {
    return value + second_arg;
  };

  for (auto _ : state)
    // The code to benchmark
    gil::transform_pixels(gil::const_view(img), gil::view(img), op2);

  if (!std::all_of(gil::view(img).begin(), gil::view(img).end(),
                   [&state](auto p) { return p == state.iterations(); }))
    state.SkipWithError("gil_algo_lambda_plus wrong result");
}
BENCHMARK(gil_algo_lambda_plus);

static void gil_algo_lambda_copy(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});
  gil::gray16_image_t img2(1024, 1024, gray16_pixel_t{0});

  auto input_view = gil::const_view(img);
  auto output_view = gil::view(img2);
  auto plus =
      gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>{};

  // Pixel operation
  auto op2 = [second_arg = gray16_pixel_t{1}](gray16_pixel_t value) {
    return gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>()(
        value, second_arg);
  };

  for (auto _ : state)
    // The code to benchmark
    gil::transform_pixels(input_view, output_view, op2);

  if (!std::all_of(output_view.begin(), output_view.end(),
                   [&state](auto p) { return p == 1; }))
    state.SkipWithError("gil_algo_lambda_copy wrong result");
}
BENCHMARK(gil_algo_lambda_copy);

static void gil_algo_rangefor(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});
  gil::gray16_image_t img2(1024, 1024, gray16_pixel_t{0});

  auto input_view = gil::const_view(img);
  auto output_view = gil::view(img2);
  auto plus =
      gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>{};

  // Pixel operation
  auto op2 = [second_arg = gray16_pixel_t{1}](gray16_pixel_t value) {
    return gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>()(
        value, second_arg);
  };

  for (auto _ : state) {
    auto d_first = output_view.begin();
    for (auto pixel : input_view) {
      *d_first++ = op2(pixel);
    }
  }

  if (!std::all_of(output_view.begin(), output_view.end(),
                   [&state](auto p) { return p == 1; }))
    state.SkipWithError("gil_algo_rangefor wrong result");
}
BENCHMARK(gil_algo_rangefor);

static void gil_algo_rangefor_plus(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});
  gil::gray16_image_t img2(1024, 1024, gray16_pixel_t{0});

  auto input_view = gil::const_view(img);
  auto output_view = gil::view(img2);
  auto plus =
      gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>{};

  // Pixel operation
  auto op2 = [second_arg = gray16_pixel_t{1}](gray16_pixel_t value) {
    return gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>()(
        value, second_arg);
  };

  for (auto _ : state) {
    auto d_first = output_view.begin();
    for (const auto &pixel : input_view) {
      *d_first++ = pixel + 1;
    }
  }

  if (!std::all_of(output_view.begin(), output_view.end(),
                   [&state](auto p) { return p == 1; }))
    state.SkipWithError("gil_algo_rangefor_plus wrong result");
}
BENCHMARK(gil_algo_rangefor_plus);

static void gil_algo_copy_mem(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;

  gil::gray16_image_t img(1024, 1024, gray16_pixel_t{0});
  gil::gray16_image_t img2(1024, 1024, gray16_pixel_t{0});

  auto input_view = gil::const_view(img);
  auto output_view = gil::view(img2);

  // Pixel operation
  auto op2 = [second_arg = gray16_pixel_t{1}](gray16_pixel_t value) {
    return gil::pixel_plus_t<gray16_pixel_t, gray16_pixel_t, gray16_pixel_t>()(
        value, second_arg);
  };

  for (auto _ : state) {
    auto start = std::addressof(input_view(0, 0));
    auto end = std::addressof(input_view(input_view.width() - 1,
                                         input_view.height() - 1)) +
               1;
    auto d_start = std::addressof(output_view(0, 0));
    while (start != end) {
      *d_start++ = *start++ + 1;
    }
  }

  if (!std::all_of(output_view.begin(), output_view.end(),
                   [&state](auto p) { return p == 1; }))
    state.SkipWithError("gil_algo_copy_mem wrong result");
}
BENCHMARK(gil_algo_copy_mem);

static void gil_algo_pixel_vector(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;
  std::vector<gil::gray16_pixel_t> img(1024 * 1024, gil::gray16_pixel_t(0));

  for (auto _ : state) {
    for (auto &pixel : img)
      pixel.at(std::integral_constant<int, 0>{}) =
          pixel.at(std::integral_constant<int, 0>{}) + 1;
  }

  if (!std::all_of(img.begin(), img.end(),
                   [&state](auto p) { return p == state.iterations(); }))
    state.SkipWithError("gil_algo_pixel_vector wrong result");
}
BENCHMARK(gil_algo_pixel_vector);

static void gil_algo_pixel_vector_reinterpret(benchmark::State &state) {
  using namespace std::placeholders;
  namespace gil = boost::gil;
  std::vector<gil::gray16_pixel_t> img(1024 * 1024, gil::gray16_pixel_t(0));

  for (auto _ : state) {
    auto start = reinterpret_cast<std::uint16_t *>(img.data());
    auto end = start + img.size();
    for (; start != end; ++start)
      *start += 1;
  }

  if (!std::all_of(img.begin(), img.end(),
                   [&state](auto p) { return p == state.iterations(); }))
    state.SkipWithError("gil_algo_pixel_vector wrong result");
}
BENCHMARK(gil_algo_pixel_vector_reinterpret);

BENCHMARK_MAIN();


#include "path_finder.hpp"

#include <algorithm>
#include <queue>
#include <unordered_map>
#include <functional>

#include <boost/functional/hash.hpp>

namespace ppc
{
	PathFinder::PathFinder(mpi::communicator workers, mpi::communicator orientee, const index_type workerID, const index_type numOfWorkers)
		: m_workers{ std::move(workers) }, m_orientee{ std::move(orientee) }, m_workerID{ workerID }, m_numOfWorkers{ numOfWorkers }
	{
	}

	namespace
	{
		using TableRow = std::vector<std::pair<int, std::int8_t>>;

		struct MinOffsetCompare
		{
			MinOffsetCompare(const Map& map, const Area& area, const TableRow& prevRow, const index_type x)
				: m_map{ map }, m_area{ area }, m_prevRow{ prevRow }, m_x{ x }
			{
			}

			const bool operator()(const int offset1, const int offset2) const
			{
				if (m_x == 0 || m_x == m_area.width - 1)
				{
					const auto invalidOffset = (m_x == 0) ? -1 : 1;
					if (offset1 == invalidOffset)
					{
						return false;
					}
					else if (offset2 == invalidOffset)
					{
						return true;
					}
				}

				auto val1 = m_prevRow[m_x + offset1].first;
				auto val2 = m_prevRow[m_x + offset2].first;
				if (val1 < 0)
				{
					return false;
				}
				else if (val2 < 0)
				{
					return true;
				}

				return val1 < val2;
			}

			void x(const index_type value)
			{
				m_x = value;
			}

		private:
			const Map& m_map;
			const Area& m_area;
			const TableRow& m_prevRow;
			index_type m_x;
		};

		struct ZoneValidator
		{
			ZoneValidator(const Map& map, const Area& area)
				: m_map{ map }, m_area{ area }
			{
			}

			const bool operator()(const int x, const int y) const
			{
				const auto zone = m_map[m_area.y + y][m_area.x + x];
				return zone == OPEN || zone == ROAD;
			}

		private:
			const Map& m_map;
			const Area& m_area;
		};

		auto create_auxiliary_row(const ZoneValidator& validator, const index_type width, const index_type startX)
		{
			std::vector<int> auxiliaryRow(width, -1);
			auxiliaryRow[startX] = 0;
			if (startX > 0)
			{
				for (auto x = startX - 1; x >= 0 && validator(x, -1); --x)
				{
					auxiliaryRow[x] = auxiliaryRow[x + 1] + 1;
				}
			}

			for (auto x = startX + 1; x < width && validator(x, -1); ++x)
			{
				auxiliaryRow[x] = auxiliaryRow[x - 1] + 1;
			}

			return auxiliaryRow;
		}

		auto find_entrance_point(const std::vector<int>& pointRow, const TableRow& row, MinOffsetCompare& compare)
		{
			index_type minX = 0;
			int minValue = -1;

			for (auto x = 0u; x < pointRow.size(); ++x)
			{
				if (pointRow[x] < 0)
				{
					continue;
				}

				compare.x(x);
				auto minDir = std::min({ 0, -1, 1 }, compare) * -1;

				if (row[x + minDir].first < 0)
				{
					continue;
				}

				const auto val = pointRow[x] + row[x + minDir].first + (minDir == 0 ? 1 : 2);
				if (minValue < 0 || val < minValue)
				{
					minX = x;
					minValue = val;
				}
			}

			assert(minValue >= 0);
			if (minValue < 0)
			{
				PPC_LOG(info) << "Failed to find entrance point...";
				std::exit(1);
			}
			return minX;
		}

		//TODO: For some reason ostream << index_pair doesn't work with boost::log...
		inline auto to_string(const index_pair& pair)
		{
			return static_cast<const std::ostringstream&>(std::ostringstream{} << "( x = " << pair.first << ", y = " << pair.second << " )").str();
		}

		auto get_neighbours(const index_pair& location, const Area& area, const ZoneValidator& isPassable)
		{
			std::vector<index_pair> neighbours;
			neighbours.reserve(8);

			auto xMin = location.first <= area.x ? 0 : -1;
			auto xMax = location.first >= area.x + area.width - 1 ? 0 : 1;
			auto yMin = location.second <= area.y ? 0 : -1;
			auto yMax = location.second >= area.y + area.height - 1 ? 0 : 1;
			for (auto i = xMin; i <= xMax; ++i)
			{
				for (auto j = yMin; j <= yMax; ++j)
				{
					if ( !(i == 0 && i == j) && isPassable(location.first + i, location.second + j))
					{
						neighbours.push_back({ location.first + i, location.second + j });
					}
				}
			}

			return neighbours;
		}

		inline auto get_cost(const index_pair& from, const index_pair& to)
		{
			if (from.first == to.first || from.second == to.second)
			{
				return 1;
			}
			return 2;
		}

		auto smart_find_entrance_point(const index_pair& start, const Map& map, std::function<bool(const index_pair& location)> locationValidator)
		{
			using PriorityPair = std::pair<index_pair, int>;
			auto priorityCmp = [](const PriorityPair& lhs, const PriorityPair& rhs)
			{
				return lhs.second > rhs.second;
			};

			struct MyHash
			{
				const size_t operator()(const index_pair& location) const
				{
					std::size_t seed = 0;
					boost::hash_combine(seed, location.first);
					boost::hash_combine(seed, location.second);

					return seed;
				}
			};

			std::priority_queue<PriorityPair, std::vector<PriorityPair>, decltype(priorityCmp)> frontier{ priorityCmp };
			frontier.push({ start, 0 });

			std::unordered_map<index_pair, index_pair, MyHash> cameFrom;
			cameFrom[start] = { 0, 0 };	//{ 0, 0 } will always be invalid

			std::unordered_map<index_pair, int, MyHash> costSoFar;
			costSoFar[start] = 0;

			Area area{ 0, 0, start.second + 2, map.width() };
			ZoneValidator isPassable{ map, area };
			index_pair current{};
			while (!frontier.empty())
			{
				current = frontier.top().first;
				frontier.pop();

				if (locationValidator(current))
				{
					break;
				}

				const auto neighbours = get_neighbours(current, area, isPassable);
				for (const auto& neighbour : neighbours)
				{
					const auto newCost = costSoFar[current] + get_cost(current, neighbour);
					if (costSoFar.find(neighbour) == costSoFar.cend() || newCost < costSoFar[neighbour])
					{
						costSoFar[neighbour] = newCost;
						frontier.push({ neighbour, newCost });
						cameFrom[neighbour] = current;
					}
				}
			}

			std::vector<index_pair> pathToEntrance;
			const auto cost = costSoFar[current];
			do
			{
				pathToEntrance.push_back(current);
				current = cameFrom[current];
			} while (current != start);
			std::reverse(pathToEntrance.begin(), pathToEntrance.end());
			return std::make_pair(std::move(pathToEntrance), cost);
		}
	}

	void PathFinder::run(const Map& map, const Area& area, boost::optional<LocationOrientationPair> locationResult, EntranceSearching searching, int numOfSearchLocations)
	{
		if (area.height == 0 || area.width == 0)
		{
			return;
		}

		PPC_LOG(info) << "Path finding for area: " << area;

		auto isPassable = ZoneValidator{ map, area };

		//2D matrix of pair of { weight, turn }
		std::vector<TableRow> table{ area.height,{ area.width, std::make_pair(0, 0) } };
		for (auto x = 0u; x < area.width; ++x)
		{
			table[area.height - 1][x].first = isPassable(x, area.height - 1) ? 0 : -1;
		}

		for (auto y = static_cast<int>(area.height - 2); y >= 0; --y)
		{
			for (auto x = 0u; x < area.width; ++x)
			{
				auto& row = table[y];
				const auto& prev = table[y + 1];

				if (!isPassable(x, y))
				{
					row[x].first = -1;
					continue;
				}

				auto minDir = std::min({ 0, -1, 1 }, MinOffsetCompare{map, area, prev, x});

				const auto prevVal = prev[x + minDir].first;
				if (prevVal >= 0)
				{
					const auto addedWeight = minDir == 0 ? 1 : 2;
					row[x] = { prevVal + addedWeight, minDir };
				}
				else
				{
					row[x] = { -1, 0 };
				}
			}
		}
		PPC_LOG(info) << "Pathing table for the area computed.";
		int minTableEntry = 0;
		{
			auto minIt = std::min_element(table[0].cbegin(), table[0].cend(),
				[](const std::pair<int, std::int8_t>& lhs, const std::pair<int, std::int8_t>& rhs)
				{
					if(lhs.first < 0)
					{
						return false;
					}
					else if (rhs.first < 0)
					{
						return true;
					}
					return lhs.first < rhs.first;
				}
			);
			minTableEntry = static_cast<int>(minIt - table[0].cbegin());
		}

		index_type startX;
		auto currentOrientation = BACKWARDS;
		if (locationResult)
		{
			startX = locationResult->first.first;
			currentOrientation = locationResult->second;
		}
		else
		{
			m_workers.recv(mpi::any_source, mpi::any_tag, startX);
			m_workers.recv(mpi::any_source, mpi::any_tag, currentOrientation);
		}

		const auto startingLocation = index_pair{ startX, area.y - 1 };
		PPC_LOG(info) << "Starting location: " << startingLocation;

		//Finding the entrance
		auto anyLocationValidator = [&startingLocation, &table](const index_pair& location)	//Find the first valid entrance point
		{
			return location.second == startingLocation.second + 1 && table[0][location.first].first >= 0;
		};

		index_pair specificLocation{};
		auto specificLocationValidator = [&specificLocation](const index_pair& location)
		{
			return location == specificLocation;
		};

		std::vector<index_pair> pathToEntrance;
		if (searching == ANY_ENTRANCE)
		{
			pathToEntrance = smart_find_entrance_point(startingLocation, map, anyLocationValidator).first;
		}
		else if (searching == MIN_ENTRANCE)
		{
			specificLocation = { minTableEntry, startingLocation.second + 1 };
			pathToEntrance = smart_find_entrance_point(startingLocation, map, specificLocationValidator).first;
		}
		else if (searching == N_SEARCH)
		{
			specificLocation = { minTableEntry, startingLocation.second + 1 };
			auto pathResult = smart_find_entrance_point(startingLocation, map, specificLocationValidator);

			pathToEntrance = std::move(pathResult.first);
			int minCost = table[0][minTableEntry].first + pathResult.second;

			auto offset = static_cast<int>(area.width / numOfSearchLocations);
			for (auto i = area.x + offset; i < area.width; i += offset)
			{
				const auto tableCost = table[0][i].first;
				if (tableCost < 0)
				{
					continue;
				}

				specificLocation = { i, startingLocation.second + 1 };
				pathResult = smart_find_entrance_point(startingLocation, map, specificLocationValidator);

				const auto totalCost = tableCost + pathResult.second;
				if (totalCost < minCost)
				{
					pathToEntrance = std::move(pathResult.first);
					minCost = totalCost;
				}
			}
		}
		PPC_LOG(info) << "Entrance found: " << pathToEntrance.back();

		//Move to the entrance.
		PPC_LOG(info) << "Moving to entrance";
		auto currentX = startingLocation.first;
		auto currentY = startingLocation.second;
		for (const auto pathPoint : pathToEntrance)
		{
			const auto offsets = std::make_pair(
				static_cast<int>(pathPoint.first) - static_cast<int>(currentX),
				static_cast<int>(pathPoint.second) - static_cast<int>(currentY)
			);
			currentOrientation = moveTo(currentOrientation, offsets);
			currentX = pathPoint.first;
			currentY = pathPoint.second;

			PPC_LOG(info) << "Current position: " << pathPoint;
		}
		//

		PPC_LOG(info) << "Moving down. Starting pos = " << index_pair{ currentX, area.y };
		for (auto i = 0u; i < area.height - 1; ++i)
		{
			auto xOffset = table[i][currentX].second;
			currentX += xOffset;
			currentOrientation = moveTo(currentOrientation, { xOffset, 1 });
			PPC_LOG(info) << "Current pos = " << index_pair{ currentX, area.y + i + 1};
		}

		if (m_workerID != m_numOfWorkers - 1)
		{
			PPC_LOG(info) << "Sending current x and current orientation forward...";
			m_workers.send(m_workerID + 1, 0, currentX);
			m_workers.send(m_workerID + 1, 0, currentOrientation);
		}
	}

	namespace
	{
		constexpr auto ensure_direction(Direction currentOrientation, Direction moveDir)
		{
			return static_cast<Direction>(((moveDir + 4) - currentOrientation) % 4);
		}
	}

	Direction PathFinder::moveTo(Direction currentOrientation, const std::pair<int, int>& offsets)
	{
		const auto offsetX = offsets.first;
		const auto offsetY = offsets.second;

		if(offsetX != 0)
		{
			auto moveDir = get_direction({ offsetX, 0 });
			auto realDir = ensure_direction(currentOrientation, moveDir);
			m_orientee.send(1, tags::MOVE, realDir);
			currentOrientation = moveDir;
		}

		if (offsetY != 0)
		{
			auto moveDir = get_direction({ 0, offsetY });
			auto realDir = ensure_direction(currentOrientation, moveDir);
			m_orientee.send(1, tags::MOVE, realDir);
			currentOrientation = moveDir;
		}

		return currentOrientation;
	}
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// 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 <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/metafunction.hpp>
#include <mpllibs/metamonad/returns.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <boost/mpl/int.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/assert.hpp>

using mpllibs::metamonad::returns;

namespace
{
  MPLLIBS_METAFUNCTION(always13, (T)) ((returns13));

  struct hidden_result
  {
    typedef int13 the_result;
    // no ::type
  };

  struct can_be_evaluated_only_once : returns<hidden_result> {};
}

BOOST_AUTO_TEST_CASE(test_lazy)
{
  using mpllibs::metamonad::lazy;

  using boost::mpl::equal_to;
  using boost::mpl::divides;
  using boost::mpl::if_;
  using boost::mpl::false_;

  // test_evaluation
  BOOST_MPL_ASSERT((equal_to<int13, lazy<divides<int26, int2> >::type>));

  // test_lazyness
  BOOST_MPL_ASSERT((
    equal_to<int26, lazy<non_lazy_plus<returns13, returns13> >::type>
  ));

  // test_evaluation_limit
  BOOST_MPL_ASSERT((
    equal_to<int13, lazy<can_be_evaluated_only_once>::type::the_result>
  ));

  // test_evaluation_limit_of_double_lazy
  BOOST_MPL_ASSERT((
    equal_to<int13, lazy<lazy<can_be_evaluated_only_once> >::type::the_result>
  ));
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/bool.hpp>
#include <boost/hana/detail/assert.hpp>

#include <tuple>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(if_(true, 1, 2) == 1);
    BOOST_HANA_CONSTEXPR_ASSERT(if_(false, 1, 2) == 2);

    BOOST_HANA_CONSTEXPR_ASSERT(
        if_(true_,
            std::make_tuple('t', 'r', 'u', 'e'),
            std::make_tuple('f', 'a', 'l', 's', 'e')
        )
        ==
        std::make_tuple('t', 'r', 'u', 'e')
    );
    //! [main]
}


// ------------------------------------------------------------------------
//  Copyright (C)
//  ETH Zurich - Switzerland
//
//  Kevis-Kokitsi Maninis <kmaninis@vision.ee.ethz.ch>
//  Jordi Pont-Tuset <jponttuset@vision.ee.ethz.ch>
//  July 2016
// ------------------------------------------------------------------------
// This file is part of the COB package presented in:
//    K.K. Maninis, J. Pont-Tuset, P. Arbelaez and L. Van Gool
//    Convolutional Oriented Boundaries
//    European Conference on Computer Vision (ECCV), 2016
// Please consider citing the paper if you use this code.
// ------------------------------------------------------------------------
#ifndef PART2CONT_HPP
#define PART2CONT_HPP

#include <iostream>
#include "containers.hpp"
#include <Eigen/Dense>

contour_container part2cont(const Eigen::Array<label_type,Eigen::Dynamic,Eigen::Dynamic>& lp)
{
    /* Sizes */
    std::size_t sx = lp.rows();
    std::size_t sy = lp.cols();

    /*-------------------------------------------------------------*/
    /*           Create LUT of from pairs of regions to            */
    /*             list of contours and UCM threshold              */
    /*-------------------------------------------------------------*/
    contour_container pairs_contours;

    /* Scan horizontal contours*/
    for (std::size_t xx=1; xx<sx; ++xx)
    {
        for (std::size_t yy=0; yy<sy; ++yy)
        {
            if (lp(xx,yy)!=lp(xx-1,yy))
            {
                /* Build a contour element and the two labels 'touching' it*/
                label_type lab1 = std::min(lp(xx,yy),lp(xx-1,yy));
                label_type lab2 = std::max(lp(xx,yy),lp(xx-1,yy));

                /* Recover the element or build and empty one */
                /* and push the contour piece                 */
                pairs_contours[std::make_pair(lab1,lab2)].push_back(cont_elem(2*xx,2*yy+1));
            }
        }
    }

    /* Scan vertical contours*/
    for (std::size_t xx=0; xx<sx; ++xx)
    {
        for (std::size_t yy=1; yy<sy; ++yy)
        {
            if (lp(xx,yy)!=lp(xx,yy-1))
            {
                /* Build a contour element and the two labels 'touching' it*/
                label_type lab1 = std::min(lp(xx,yy),lp(xx,yy-1));
                label_type lab2 = std::max(lp(xx,yy),lp(xx,yy-1));

                /* Recover the element or build and empty one */
                /* and push the contour piece                 */
                pairs_contours[std::make_pair(lab1,lab2)].push_back(cont_elem(2*xx+1,2*yy));
            }
        }
    }

    return pairs_contours;
}

#endif


//
// Created by Han Zhao on 11/19/15.
//
#include "src/SPNNode.h"
#include "src/SPNetwork.h"
#include "src/utils.h"
#include "src/StreamParamLearning.h"

#include <fstream>
#include <random>

#include <boost/program_options.hpp>

namespace po = boost::program_options;

using namespace std;
using SPN::SPNNode;
using SPN::SumNode;
using SPN::ProdNode;
using SPN::VarNode;
using SPN::SPNNodeType;
using SPN::SPNetwork;
using SPN::StreamParamLearning;
using SPN::StreamProjectedGD;
using SPN::StreamExpoGD;
using SPN::StreamSMA;
using SPN::StreamExpectMax;
using SPN::utils::split;

// The main difference between the online version of these algorithms and the
// streaming version of these algorithms lie in the fact that in the online
// setting we're allowed to revisit each training instance multiple times while
// in the streaming setting each instance should be visited only once.
// Furthermore, in the streaming setting normally the data set is so large that
// we cannot load them all at once into main memory, so instead we load each instance
// at one time.
int main(int argc, char *argv[]) {
    // Positional program parameters.
    std::string model_filename, training_filename, valid_filename, test_filename, stream_algo;
    std::string output_model_filename;
    // Hyperparameter for the streaming expectation maximization algorithm.
    uint seed = 42;
    double proj_eps = 1e-2;
    double lrate = 1e-1;
    // Building command line parser
    po::options_description desc("Please specify the following options");
    desc.add_options()
            // Positional program parameters.
            ("train", po::value<std::string>(&training_filename), "file path of training data")
            ("test", po::value<std::string>(&test_filename), "file path of test data")
            ("model", po::value<std::string>(&model_filename), "file path of SPN")
            ("output_model", po::value<std::string>(&output_model_filename), "file path of SPN to save")
            ("algo", po::value<std::string>(&stream_algo), "streaming algorithm")
            // Hyperparameters for EM.
            ("seed", po::value<uint>(&seed), "random seed")
            ("proj_eps", po::value<double>(&proj_eps), "projection constant")
            ("lrate", po::value<double>(&lrate), "learning rate");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (!vm.count("train") || !vm.count("test") || !vm.count("model") || !vm.count("algo")) {
        std::cout << desc << std::endl;
        return -1;
    }
    std::cout << "Loaded model: " << model_filename << std::endl;
    // Load and simplify the SPN.
    SPNetwork *spn = SPN::utils::load(model_filename);
    spn->init();
    std::cout << "Network statistics after initialization: " << std::endl;
    cout << "Network height: " << spn->height() << endl;
    cout << "Network size: " << spn->size() << endl;
    cout << "Network number of nodes: " << spn->num_nodes() << endl;
    cout << "Network number of edges: " << spn->num_edges() << endl;
    cout << "Network number of varnodes: " << spn->num_var_nodes() << endl;
    cout << "Network number of sumnodes: " << spn->num_sum_nodes() << endl;
    cout << "Network number of prodnodes: " << spn->num_prod_nodes() << endl;
    cout << "**********************************" << endl;
    // Streaming projected gradient descent.
    // Random initialization of model parameters.
    spn->set_random_params(seed);
    const auto &tokens = split(model_filename, '.');
    string data_name = split(tokens[0], '/')[1];
    StreamParamLearning *slearning = nullptr;
    // Select the concrete algorithm to be used.
    if (stream_algo == "pgd") {
        slearning = new StreamProjectedGD(proj_eps, lrate);
    } else if (stream_algo == "eg") {
        slearning = new StreamExpoGD(lrate);
    } else if (stream_algo == "sma") {
        slearning = new StreamSMA(lrate);
    } else if (stream_algo == "em") {
        slearning = new StreamExpectMax(lrate);
    } else {
        std::cerr << "Please choose from pgd, eg, sma or em" << std::endl;
        std::exit(-1);
    }
    std::cout << slearning->algo_name() << std::endl;
    std::cout << "Optimization hyperparameters:" << std::endl;
    std::cout << "Random seed = " << seed << std::endl;
    std::cout << "Projection constant = " << proj_eps << std::endl;
    std::cout << "Learning rate = " << lrate << std::endl;
    std::cout << "-----------------------------------------------" << std::endl;
    std::ifstream fin(training_filename, std::ifstream::in);
    if (!fin) {
        std::cerr << "Failed to open file: " << training_filename << std::endl;
        std::exit(-1);
    }
    std::string line;
    std::vector<double> input_inst;
    std::vector<std::string> terms;
    size_t num_train = 0, num_test = 0;
    std::clock_t t_start = std::clock();
    // In case there is interruption during the training.
    while (std::getline(fin, line)) {
        terms = split(line, ',');
        input_inst.clear();
        for (const std::string &term : terms)
            input_inst.push_back(std::stod(term));
        slearning->fit(input_inst, *spn);
        num_train += 1;
    }
    fin.close();
    std::clock_t t_end = std::clock();
    spn->weight_projection(1e-3);
    std::cout << "Time used for training = " << 1000.0 * (t_end - t_start) / CLOCKS_PER_SEC
              << " milliseconds" << std::endl;
    std::cout << "Number of streaming instances = " << num_train << std::endl;
    // Testing.
    fin.open(test_filename, std::ifstream::in);
    if (!fin) {
        std::cerr << "Failed to open file: " << test_filename << std::endl;
        std::exit(-1);
    }
    t_start = std::clock();
    double tlogp = 0.0;
    while (std::getline(fin, line)) {
        terms = split(line, ',');
        input_inst.clear();
        for (const std::string &term : terms)
            input_inst.push_back(std::stoi(term));
        tlogp += spn->logprob(input_inst);
        num_test += 1;
    }
    tlogp /= num_test;
    t_end = std::clock();
    std::cout << "Time used for testing = " << 1000.0 * (t_end - t_start) / CLOCKS_PER_SEC
              << " milliseconds" << std::endl;
    std::cout << "Number of test instances = " << num_test << std::endl;
    std::cout << "Average log-likelihoods = " << tlogp << std::endl;
    if (vm.count("output_model")) {
        SPN::utils::save(spn, output_model_filename);
    }
    delete spn;
    delete slearning;
    return 0;
}


#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE "test_timestamp"
#define BOOST_AUTO_TEST_MAIN

#include <iostream>
#include <numeric>

#include <boost/test/unit_test.hpp>
#include <boost/test/execution_monitor.hpp>

#include <stream_aligner/TimestampEstimator.hpp>
#include <fstream>
#include <iomanip>

/**
 * helper class for unit testing This class calculates the sample time, hardware
 * time and real time for a given sample number.
 *
 * It also provides some functionallity to create plottable data and performs
 * some standard unit testing.
 * */
class Tester
{
private:
    std::ofstream debugFile;
public:
    /** static latency of the sample **/
    base::Time sample_latency;

    /** maximum of random noise added ontop of the static latency **/
    base::Time sample_latency_max_noise;

    /** maximum noise of hardware timestamp **/
    base::Time hw_time_max_noise;

    /** time of the first sample **/
    base::Time base_time;

    /** real period **/
    base::Time real_period;

    /** Drift of the period in s/s
    I.e. it is the rate of change of period in seconds per second **/
    base::Time period_drift;

    /** output, calculated by classe **/
    base::Time sample_time;

    /** output, calculated by classe **/
    base::Time hw_time;

    /** output, calculated by classe **/
    base::Time real_time;

    /** output: real_period with drift, calculated in update **/
    base::Time actual_period;

public:
    Tester(std::string debugFileName = "")
    {

        /** init random nummer generator **/
        srand48(time(NULL));

        base_time = base::Time::now();

        if(!debugFileName.empty())
            debugFile.open(debugFileName.c_str(), std::ios::out);

        if(debugFile.is_open())
        {
            debugFile << std::setprecision(5) << std::endl;
	        debugFile << "# (hardware-real) (sample-real) (estimate-real) (estimated_period-actual_period) (est-last_est) (real-last_real) period" << std::endl;
        }

    }

    void calculateSamples(int nr)
    {
        base::Time sample_latency_max_noise;
        if (nr > 0)
            sample_latency_max_noise = this->sample_latency_max_noise;
        else
            sample_latency_max_noise = this->real_period * 0.09;

        base::Time sample_latency_noise = base::Time::fromSeconds(drand48() * this->sample_latency_max_noise.toSeconds());

	    base::Time hw_time_noise(base::Time::fromSeconds(drand48() * this->hw_time_max_noise.toSeconds()));

        this->actual_period = this->real_period + this->period_drift * nr;
        this->real_time = this->base_time + this->real_period * nr + this->period_drift * nr * (nr + 1) / 2;
        this->sample_time = this->real_time + this->sample_latency + sample_latency_noise;
        this->hw_time = this->real_time + hw_time_noise;
    }

    void addResultToPlot(base::Time estimated_time, base::Time estimated_period)
    {
        static base::Time last_estimated_time = estimated_time;
        static base::Time last_real_time = real_time;

    	if(debugFile.is_open())
        {
	        debugFile << (hw_time - real_time).toSeconds() / actual_period.toSeconds()
                << " " << (sample_time - real_time - sample_latency).toSeconds() / actual_period.toSeconds()
                << " " << (estimated_time - real_time).toSeconds() / actual_period.toSeconds()
                << " " << (estimated_period - actual_period).toSeconds() / actual_period.toSeconds()
                << " " << (estimated_time - last_estimated_time).toSeconds()
                << " " << (real_time - last_real_time).toSeconds()
                << " " << actual_period.toSeconds()
                << std::endl;
        }
        last_estimated_time = estimated_time;
        last_real_time = real_time;
    }

    void checkResult(base::Time estimated_time, base::Time estimated_period)
    {
        addResultToPlot(estimated_time, estimated_period);

        if (((estimated_time - real_time).toSeconds()) > this->actual_period.toSeconds() / 10)
        {
            std::cout<<"estimated time: "<<estimated_time.toString()<<" real time: "<<real_time.toString()<<"\n";
        }

        BOOST_CHECK_SMALL((estimated_time - real_time).toSeconds(), this->actual_period.toSeconds() / 10);
    }
};

/** type of announcement methods **/
enum LOSS_ANNOUNCE_METHODS
{
    USE_NONE,
    USE_UPDATE_LOSS,
    USE_INDEX
};

void test_timestamper_impl(int hardware_order, bool has_initial_period, bool has_drift, int init, double loss_rate, LOSS_ANNOUNCE_METHODS loss_announce_method = USE_NONE)
{
    /** this test case tries to emulate the values of an hokuyo laser scanner **/
    static const int COUNT = 10;

    std::stringstream csv_filename;
    csv_filename << "test_timestamper";
    if (hardware_order < 0)
        csv_filename << "__hw_before";
    else if (hardware_order > 0)
        csv_filename << "__hw_after";
    if (has_initial_period)
        csv_filename << "__initial_period";
    if (has_drift)
        csv_filename << "__period_drift";
    if (init)
        csv_filename << "__init" << init;
    if (loss_rate)
        csv_filename << "__loss";
    csv_filename << ".csv";

    Tester data(csv_filename.str());
    data.real_period = base::Time::fromSeconds(0.025);

    if (hardware_order != 0)
    {
        data.sample_latency = base::Time::fromSeconds(0.02);
        if (init < 900)
            init = 900;
    }

    if (has_drift)
    {
        /** experimental value. In these tests, the filter does not cope anymore
        if the period drift is higher than that **/
        data.period_drift = base::Time::fromSeconds(3e-5);
    }
    data.sample_latency_max_noise = base::Time::fromSeconds(0.005);
    data.hw_time_max_noise = base::Time::fromMicroseconds(50);

    base::Time initial_period;
    if (has_initial_period)
        initial_period = base::Time::fromSeconds(0.025);

    /** estimator for testing **/
    stream_aligner::TimestampEstimator estimator(base::Time::fromSeconds(20), initial_period);

    //std::cout<<"== START == "<<estimator.getStatus()<<"\n";

    for (int i = 0; i < COUNT; ++i)
    {
	    data.calculateSamples(i);

        if (hardware_order < 0)
            estimator.updateReference(data.hw_time);

        if (drand48() < loss_rate)
        {
            if (loss_announce_method == USE_UPDATE_LOSS)
                estimator.updateLoss();
            continue;
        }

        base::Time estimated_time;
        if (loss_announce_method == USE_INDEX)
            estimated_time = estimator.update(data.sample_time, i);
        else
            estimated_time = estimator.update(data.sample_time);

        if (hardware_order > 0)
            estimator.updateReference(data.hw_time);

        base::Time period;
        if (estimator.haveEstimate())
            period = estimator.getPeriod();

        if (i < init)
            data.addResultToPlot(estimated_time, period);
        else
            data.checkResult(estimated_time, period);
    }
    //stream_aligner::TimestampStatus status = estimator.getStatus();
    //std::cout<<"== END == "<<status<<"\n";
};

BOOST_AUTO_TEST_CASE(test_perfect_stream)
{
    base::Time time = base::Time::now();
    base::Time step = base::Time::fromSeconds(0.01);

    stream_aligner::TimestampEstimator estimator(base::Time::fromSeconds(2), 0);
    for (int i = 0; i < 10000; ++i)
    {
        time = time + step;
        //std::cout<<"time: "<<time.toString()<<"\n";
        BOOST_REQUIRE_CLOSE(time.toSeconds(), estimator.update(time).toSeconds(), 0.0000001);
        BOOST_REQUIRE_EQUAL(0, estimator.getLostSampleCount());
    }

    //std::cout<<"estimated period:" <<estimator.getPeriod().toSeconds() <<"\n";
    BOOST_REQUIRE_CLOSE(step.toSeconds(), estimator.getPeriod().toSeconds(), 1e-6);

    stream_aligner::TimestampStatus status = estimator.getStatus();
    std::cout<<"== END == "<<status<<"\n";
}

BOOST_AUTO_TEST_CASE(test_timestamper__plain)
{ test_timestamper_impl(0, false, false, 1000, 0); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_before__initial_period)
{ test_timestamper_impl(-1, true, false, 0, 0); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_before)
{ test_timestamper_impl(-1, false, false, 1000, 0); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_after__initial_period)
{ test_timestamper_impl(1, true, false, 0, 0); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_after)
{ test_timestamper_impl(1, false, false, 1000, 0); }
BOOST_AUTO_TEST_CASE(test_timestamper__initial_period)
{ test_timestamper_impl(0, true, false, 0, 0); }

BOOST_AUTO_TEST_CASE(test_timestamper__hw_before__initial_period__loss_updateLoss)
{ test_timestamper_impl(-1, true, false, 0, 0.01, USE_UPDATE_LOSS); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_before__loss_updateLoss)
{ test_timestamper_impl(-1, false, false, 1000, 0.01, USE_UPDATE_LOSS); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_after__initial_period__loss_updateLoss)
{ test_timestamper_impl(1, true, false, 0, 0.01, USE_UPDATE_LOSS); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_after__loss_updateLoss)
{ test_timestamper_impl(1, false, false, 1000, 0.01, USE_UPDATE_LOSS); }
BOOST_AUTO_TEST_CASE(test_timestamper__initial_period__loss_updateLoss)
{ test_timestamper_impl(0, true, false, 0, 0.01, USE_UPDATE_LOSS); }
BOOST_AUTO_TEST_CASE(test_timestamper__loss_updateLoss)
{ test_timestamper_impl(0, false, false, 1000, 0.01, USE_UPDATE_LOSS); }

BOOST_AUTO_TEST_CASE(test_timestamper__hw_before__initial_period__loss_index)
{ test_timestamper_impl(-1, true, false, 0, 0.01, USE_INDEX); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_before__loss_index)
{ test_timestamper_impl(-1, false, false, 1000, 0.01, USE_INDEX); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_after__initial_period__loss_index)
{ test_timestamper_impl(1, true, false, 0, 0.01, USE_INDEX); }
BOOST_AUTO_TEST_CASE(test_timestamper__hw_after__loss_index)
{ test_timestamper_impl(1, false, false, 1000, 0.01, USE_INDEX); }
BOOST_AUTO_TEST_CASE(test_timestamper__initial_period__loss_index)
{ test_timestamper_impl(0, true, false, 0, 0.01, USE_INDEX); }
BOOST_AUTO_TEST_CASE(test_timestamper__loss_index)
{ test_timestamper_impl(0, false, false, 1000, 0.01, USE_INDEX); }


#include <skelly_sim.hpp>

#include "cnpy.hpp"
#include <Eigen/Core>
#include <fstream>
#include <iostream>
#include <mpi.h>
#include <system.hpp>

#ifdef NDEBUG
#undef NDEBUG
#include <cassert>
#define NDEBUG
#else
#include <cassert>
#endif

#include <body.hpp>
#include <fiber.hpp>

template <typename DerivedA, typename DerivedB>
bool allclose(
    const Eigen::DenseBase<DerivedA> &a, const Eigen::DenseBase<DerivedB> &b,
    const typename DerivedA::RealScalar &rtol = Eigen::NumTraits<typename DerivedA::RealScalar>::dummy_precision(),
    const typename DerivedA::RealScalar &atol = Eigen::NumTraits<typename DerivedA::RealScalar>::epsilon()) {
    return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all();
}

int main(int argc, char *argv[]) {
    MPI_Init(&argc, &argv);
    std::string config_file("test_body.toml");
    toml::value config = toml::parse(config_file);
    Params params(config.at("params"));
    toml::array &body_configs = config.at("bodies").as_array();
    Body body(body_configs.at(0).as_table(), params);

    System::init(config_file);
    FiberContainer &fc = *System::get_fiber_container();
    BodyContainer &bc = *System::get_body_container();
    for (auto &fiber : fc.fibers) {
        auto [i_body, i_site] = fiber.binding_site_;
        auto &body = *bc.bodies[i_body];

        if (i_site > 0)
            std::cout << i_body << " " << i_site << " [" << body.nucleation_sites_ref_.col(i_site).transpose() << "] ["
                      << fiber.x_.col(0).transpose() << "]" << std::endl;
    }

    MPI_Finalize();

    std::cout << "Test passed\n";
    return 0;
}


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

std::ostream nullout(nullptr);

class IntcodeComputer {
    public:
        deque<int> inputs;
        int output = 0;
        vector<int> backup;
        vector<int> memory;
        bool halted = true;

        void load(istream& is);
        void run();    // Run the computer (until it halts or blocks on input)
        void resume(); // Resumes after being blocked
        bool hasHalted() const { return halted; }; // has the computer halted.

    private:
        std::ostream & debug = nullout; // cout;

        int pc = 0;

        int * param(int parameter, bool must_be_positional = false);
        string param_string(int parameter) const;
};

// Returns the value
int * IntcodeComputer::param(int parameter, bool must_be_positional) {
    int offset = pow(10, parameter + 1);
    int mode = memory[pc] / offset % 10;

    if (must_be_positional && mode != 0) {
        assert(must_be_positional);
    }

    if (mode == 0) { // position
        int address = memory[pc + parameter];
        return &memory[address];
    }
    if (mode == 1) { // immediate
        return &memory[pc + parameter];
    }
    assert(false);
}

// Debug string
string IntcodeComputer::param_string(int parameter) const {
    int offset = pow(10, parameter + 1);
    int mode = memory[pc] / offset % 10;
    if (mode == 0) { // position
        int address = memory[pc + parameter];
        return "&" + to_string(address) + "(" + to_string(memory[address]) + ")";
    }
    if (mode == 1) { // immediate
        int value = memory[pc + parameter];
        return to_string(value);
    }
    assert(false);
}

void IntcodeComputer::run() {
    this->memory = this->backup; // Reset the software
    this->pc = 0;
    halted = false;

    resume();
}

void IntcodeComputer::resume() {
    int last_pc = -1;
    while (pc >= 0 && pc < memory.size()) {
        assert(pc != last_pc); // Stuck in loop
        last_pc = pc;

        int op = memory[pc] % 100;
        switch (op) {
            case 1: { // add
                debug << "ADD " << param_string(3) << " = " << param_string(1) << " + " << param_string(2) << endl;

                int * src1 = param(1);
                int * src2 = param(2);
                int * dest = param(3, true);
                *dest = *src1 + *src2;
                pc += 4;
                break;
            }
            case 2: {// mul
                debug << "MUL " << param_string(3) << " = " << param_string(1) << " * " << param_string(2) << endl;

                int * src1 = param(1);
                int * src2 = param(2);
                int * dest = param(3, true);
                *dest = *src1 * *src2;
                pc += 4;
                break;
            }
            case 3: { // input
                debug << "INPUT " << param_string(1) << endl;

                if (inputs.size() <= 0 ) {
                    // Block (a resume() call has to be made)
                    return;
                }

                int * dest = param(1, true);
                *dest = inputs.front();
                inputs.pop_front();

                pc += 2;
                break;
            }
            case 4: { // output
                debug << "OUTPUT " << param_string(1) << endl;
                int * src = param(1);
                output = *src;
                pc += 2;
                break;
            }

            case 5: { // jump-if-true
                debug << "IF " << param_string(1) << " = TRUE then pc = " << param_string(2) << endl;
                int * cond = param(1);
                if (*cond != 0) {
                    pc = *param(2);
                } else {
                    pc += 3;
                }
                break;
            }

            case 6: { // jump-if-false
                debug << "IF " << param_string(1) << " = FALSE then pc = " << param_string(2) << endl;
                int * cond = param(1);
                if (*cond == 0) {
                    pc = *param(2);
                } else {
                    pc += 3;
                }
                break;
            }

            case 7: { // less than
                debug << param_string(3) << " = " << param_string(1) << " < " << param_string(2) << endl;
                int * src1 = param(1);
                int * src2 = param(2);
                int * dest = param(3, true);

                *dest = (*src1 < *src2) ? 1 : 0;

                pc += 4;
                break;
            }

            case 8: { // equals
                debug << param_string(3) << " = " << param_string(1) << " == " << param_string(2) << endl;
                int * src1 = param(1);
                int * src2 = param(2);
                int * dest = param(3, true);

                *dest = (*src1 == *src2) ? 1 : 0;

                pc += 4;
                break;
            }

            case 99: // break
                halted = true;
                return;
            default:
                halted = true;
                cout << "Unhandled op:" << op << " modes:" << (memory[pc] / 100) << endl;
                return;
        };
    }

    cout << "No more instructions without halting" << endl;
}

void IntcodeComputer::load(istream& in) {
    // Read the program
    this->backup.clear();

    string line;
    while (getline(in, line, ',')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }

        backup.push_back(stoi(line));
    }
}

int run(vector<int> phrases) {
    ifstream file ("2019/7.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    IntcodeComputer original;
    original.load(file);

    // Make 5 copies.
    vector<IntcodeComputer> computers;
    computers.push_back(original);
    computers.push_back(original);
    computers.push_back(original);
    computers.push_back(original);
    computers.push_back(original);

    file.close();

    int max_thrust = 0;
    string max_settings = "";

    do {
        for (int i = 0; i < computers.size(); i++) {
            computers[i].inputs.push_back(phrases[i]);
            computers[i].run();
        }

        // Keep running the computers while they wait for input
        int last_signal = 0;
        while (!computers[computers.size() - 1].hasHalted()) {
            for (int i = 0; i < computers.size(); i++) {
                computers[i].inputs.push_back(last_signal);
                computers[i].resume();

                last_signal = computers[i].output;
            }
        }

        if (last_signal > max_thrust) {
            max_thrust = last_signal;
            max_settings = to_string(phrases[0]) + to_string(phrases[1]) + to_string(phrases[2]) + to_string(phrases[3]) + to_string(phrases[4]);
        }
    } while ( next_permutation(phrases.begin(), phrases.end()) );

    return max_thrust;
}

int main() {
    int answer1 = run({0, 1, 2, 3, 4});
    int answer2 = run({5, 6, 7, 8, 9});

    cout << "Answer 7.1: " << answer1 << endl;
    cout << "Answer 7.2: " << answer2 << endl;
}


/**
 * Author	: Michael Fonder
 * Year		: 2016
 **/

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <unordered_map>
#include <unordered_set>
#include <set>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/video.hpp>

#include "dynamicVectorContainer.hpp"
#include "dynamicVectorReference.hpp"
#include "imuState.hpp"
#include "MSCKF.hpp"

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

using namespace cv;
using namespace std;

int main(int argc, char* argv[])
{
	FileStorage fs;
	Mat distCoeffs;
	Mat cameraMatrix;

	//******************************************************************************

	string filename = "distCoeffs";
	fs.open(filename+".yml", FileStorage::READ);
	fs[filename] >> distCoeffs;

	filename = "cameraMatrix";
	fs.open(filename+".yml", FileStorage::READ);
	fs[filename] >> cameraMatrix;

	filename = "featurePos";
	fs.open(filename+".yml", FileStorage::READ);
	Mat featurePos;
	fs[filename] >> featurePos;
	cout <<featurePos  << endl;

	filename = "data";
	fs.open(filename+".yml", FileStorage::READ);
	Mat data;
	fs[filename] >> data;
	cout << filename+".yml" << endl;
	data = data.t();

	Mat ba, bg, IMU_noise, p_CI, q_CI, MSCKF_params, init_uncertainty;
	filename = "parameters";
	fs.open(filename+".yml", FileStorage::READ);
	fs["ba"] >> ba;
	fs["bg"] >> bg;
	fs["IMU_noise"] >> IMU_noise;
	fs["init_uncertainty"] >> init_uncertainty;
	fs["p_CI"] >> p_CI;
	fs["q_CI"] >> q_CI;
	fs["MSCKF_params"] >> MSCKF_params;
	cout << filename+".yml" << endl;
	cout << ba << bg << IMU_noise << p_CI << q_CI << MSCKF_params << endl;

	Measurement newMeasurement;
	IMUstate imustate;
	cout << "datatestbegin *********************************" << endl;

	VideoCapture capture("flight_cut.avi"); // attempt to open it as a video file or image sequence
//	VideoCapture capture("test_syn.avi"); // attempt to open it as a video file or image sequence

	if (!capture.isOpened()){  // returns true if video capturing has been initialized already
		cerr << "Failed to open the video file or image sequence...\n" << endl;
		return 1;
	}
	Mat frame;
	bool finished = false;
	size_t timeIMU=0, timeVideo=0;
	MSCKF filter;
	filter.trueFeaturesPos = featurePos;
	filter.camera.distCoeffs = distCoeffs;
	filter.camera.intrisic = cameraMatrix;
	filter.setCameraParams(p_CI, q_CI);
	filter.setFilterParams(MSCKF_params);
	ba.copyTo(filter.imustate->ba);
	bg.copyTo(filter.imustate->bg);
	filter.imustate->setQ(IMU_noise);
	filter.imustate->resetCovar(init_uncertainty);
	capture >> frame;

	Mat log_p = Mat::zeros(1,6,CV_32FC1);
	Mat local = Mat::zeros(1,6,CV_32FC1);

	Mat log_q = Mat::zeros(1,7,CV_32FC1);
	Mat localq = Mat::zeros(1,7,CV_32FC1);

	while(!finished)
	{
		capture >> frame;
		if (frame.empty())
		{
			finished = true;
			continue;
		}
// 		if(timeVideo > 150)
// 			break;

		while(float(timeIMU)*0.01 <= float(timeVideo)/30)
		{
//			data.rowRange(6,9).col(timeIMU).copyTo(filter.truePos);
//			data.rowRange(9,13).col(timeIMU).copyTo(filter.trueQuat);

			data.rowRange(0,3).col(timeIMU).copyTo(newMeasurement.a_I);
			//newMeasurement.a_I.row(0) *= -1;
			data.rowRange(3,6).col(timeIMU).copyTo(newMeasurement.omega);
			//newMeasurement.omega.rowRange(0,2)*=-1.0;
			filter.propagateIMUStateAndCovar(newMeasurement, float(timeIMU)*0.01);
			local.colRange(0,3) = filter.imustate->p_G.t();
			local.at<float>(0,3) = 	filter.imustate->covar.at<float>(12,12);
			local.at<float>(0,4) = 	filter.imustate->covar.at<float>(13,13);
			local.at<float>(0,5) = 	filter.imustate->covar.at<float>(14,14);
			log_p.push_back(local);

			localq.colRange(0,4) = filter.imustate->q_IG.t();
			localq.at<float>(0,4) = 	filter.imustate->covar.at<float>(0,0);
			localq.at<float>(0,5) = 	filter.imustate->covar.at<float>(1,1);
			localq.at<float>(0,6) = 	filter.imustate->covar.at<float>(2,2);
			log_q.push_back(localq);
			++timeIMU;
		}

		cvtColor(frame, frame, CV_BGR2GRAY);
		if(!(timeVideo%2))
		{
			cout << "augment " << timeVideo << endl;
			filter.augmentState(frame, float(timeVideo)/30);
			cout << "correct " << timeVideo << endl;
			filter.update();
		}
		++timeVideo;
		FileStorage logFilep("log_p.yml", FileStorage::WRITE);
		logFilep << "log_p" << log_p;
		FileStorage logFileq("log_q.yml", FileStorage::WRITE);
		logFileq << "log_q" << log_q;
		FileStorage covar_IMU("covar_IMU.yml", FileStorage::WRITE);
		covar_IMU << "covar_IMU" << filter.imustate->covar;

		cout << "Estimated attitude : " << filter.getOrientation().t() << endl;
		cout << "Estimated position : " << filter.getPosition().t() << endl;

	}

	cout << filter.getPosition() << endl;

    return 0;
}


#include <costar_task_plan/dmp_trajectory_distribution.h>

#include <Eigen/Dense>
#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <math.h>

using namespace Eigen;

#define SHOW_SAMPLED_VALUES 0
#define DEFAULT_SIGMA 0.005
//#define DEFAULT_SIGMA 0.0000000001

namespace costar {

  /**
   * Initialize a trajectory distribution with given params
   */
  DmpTrajectoryDistribution::DmpTrajectoryDistribution(unsigned int dim_, unsigned int nbasis_, RobotKinematicsPtr robot_)
    : dim(dim_),
    checker(0),
    nbasis(nbasis_),
    robot(robot_),
    dist((dim_*nbasis_) + POSE_FEATURES_SIZE,1),
    verbose(false),
    dmp_list(dim_),
    dmp_goal(dim_),
    k_gain(100),
    d_gain(20),
    tau(2.0),
    goal_threshold(dim_,0.01),
    dmp_velocity_multiplier(0.1),
    attached(false),
    collision_detection_step(4)
  {

    assert(dim == robot->getDegreesOfFreedom());

    // set up anything else?
    nvars = dist.ns[0].mu.size();

    for (unsigned int i = 0; i < dim; ++i) {
      dmp_list[i].k_gain = k_gain;
      dmp_list[i].d_gain = d_gain;
      dmp_list[i].weights.resize(nbasis);
    }
  }

  /** check for collisions */
  void DmpTrajectoryDistribution::setCollisionChecker(CostarPlanner *c) {
    checker = c;
  }

  void DmpTrajectoryDistribution::addNoise(double d) {
    for (int j = 0; j < nvars; ++j) {
      //dist.ns[0].P(j,j) += d;
      if (j < POSE_RPY_SIZE) {
        dist.ns[0].P(j,j) += d;
      }
      else {
        dist.ns[0].P(j,j) += 100*d;
      }
    }
    dist.Update();
  }

  void DmpTrajectoryDistribution::addNoise(std::vector<double> sigma) {
    if (sigma.size() < nvars) {
      if (verbose) {
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Noise argument for trajectory search initialization was the wrong size!" << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Should be: " << dim << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Was: " << sigma.size() << std::endl;
      }
      for (int j = 0; j < nvars; ++j) {
        if (j < POSE_RPY_SIZE) {
          dist.ns[0].P(j,j) = DEFAULT_SIGMA;
        }
        else {
          dist.ns[0].P(j,j) = 100*DEFAULT_SIGMA;
        }
      }

    } else {
      for (int j = 0; j < nvars; ++j) {
        dist.ns[0].P(j,j) = sigma[j];
      }
    }
    dist.Update();
  }

  /**
   * use a model of a skill
   * compute the grasp pose from that skill and use it to plan for now
   */
  void DmpTrajectoryDistribution::attachObjectFromSkill(Skill &skill) {
    attachObjectFrame(skill.getInitializationFinalPose());
  }

  /**
   * set an attached object
   */
  void DmpTrajectoryDistribution::attachObjectFrame(const Pose &pose) {
    attachedObjectFrame = pose;
    attached = true;
  }

  /*
   * remove object
   */
  void DmpTrajectoryDistribution::detachObjectFrame() {
    attached = false;
  }

  void DmpTrajectoryDistribution::initializePose(TestFeatures &features, const Skill &skill, bool initBegin) {
    Pose p1 = features.lookup(skill.getInitializationFeature());
    //std::cout << p1 << std::endl;
    if (not initBegin) {
      p1 = p1 * skill.getInitializationFinalPose();
    } else {
      p1 = p1 * skill.getInitializationStartPose();
    }

    if (skill.hasAttachedObject()) {
      if (attached) {
        std::cout << "Attached frame loaded from DISTRIBUTION\n";
        //std::cout << attachedObjectFrame << "\n";
        p1 = p1 * attachedObjectFrame.Inverse();
      } else if (features.hasAttachedObjectFrame()) {
        std::cout << "Attached frame loaded from FEATURES\n";
        //std::cout << features.getAttachedObjectFrame() << "\n";
        p1 = p1 * features.getAttachedObjectFrame().Inverse();
      } else {
        std::cerr << __FILE__ << ":" << __LINE__ << ": This skill requires an attached object and none was provided!\n";
        assert(false);
      }
    }

    unsigned int idx = 0;

    dist.ns[0].mu[POSE_FEATURE_X] = p1.p.x();
    dist.ns[0].mu[POSE_FEATURE_Y] = p1.p.y();
    dist.ns[0].mu[POSE_FEATURE_Z] = p1.p.z();

    double roll, pitch, yaw;
    p1.M.GetRPY(roll, pitch, yaw);

    dist.ns[0].mu[POSE_FEATURE_YAW] = yaw;
    dist.ns[0].mu[POSE_FEATURE_PITCH] = pitch;
    dist.ns[0].mu[POSE_FEATURE_ROLL] = roll;
  }

  /**
   * initialize
   * set up the distribution based on a skill and an environment
   */
  void DmpTrajectoryDistribution::initialize(TestFeatures &features, const Skill &skill, bool initBegin, std::vector<double> sigma) {

    initializePose(features,skill,initBegin);

    for (int j = POSE_RPY_SIZE; j < nvars; ++j) {
      dist.ns[0].mu[j] = 0;//(double)(j % nbasis) / (double)nbasis;
    }

    /*for (unsigned int j = 0; j < dist.ns[0].mu.size(); ++j) {
      std::cout<<dist.ns[0].mu(j)<<"\n";
      }*/

    if (sigma.size() < nvars) {
      if (verbose) {
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Noise argument for trajectory search initialization was the wrong size!" << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Should be: " << dim << std::endl;
        std::cerr << "[GRID/TRAJECTORY DISTRIBUTION] Was: " << sigma.size() << std::endl;
      }
      for (int j = 0; j < nvars; ++j) {
        if (j < POSE_RPY_SIZE) {
          dist.ns[0].P(j,j) = DEFAULT_SIGMA;
        }
        else {
          dist.ns[0].P(j,j) = 10*DEFAULT_SIGMA;
        }
      }

    } else {
      for (int j = 0; j < nvars; ++j) {
        dist.ns[0].P(j,j) = sigma[j];
      }
    }
    dist.Update();

  }

  /**
   * sample
   * Pull a random trajectory from the gmm
   * Convert it into a KDL trajectory
   * NON-CONST becuse Gmm::sample is likewise non-const
   */
  unsigned int DmpTrajectoryDistribution::sample(
      const std::vector<JointTrajectoryPoint> &start_pts,
      std::vector<EigenVectornd> &params,
      std::vector<JointTrajectory> &trajs, unsigned int nsamples) {

    using KDL::Vector;
    using KDL::Rotation;

    int ik_tries = 0;

    if (nsamples == 0) {
      nsamples = params.size();
    } else {
      params.resize(nsamples);
    }
    trajs.resize(nsamples);

    int sample = 0;
    while (sample < nsamples) {

      //EigenVectornd vec(nvars);
      //vec.resize(nvars);

      params[sample].resize(nvars);
      dist.Sample(params[sample]);

#if SHOW_SAMPLED_VALUES
      std::cout << "Sampled: ";
      for (int j = 0; j < dim; ++j) {
        std::cout << params[sample][j] << " ";
      }
      std::cout << std::endl;
#endif

      // convert the first six into a pose
      Vector v1 = Vector(params[sample][POSE_FEATURE_X],params[sample][POSE_FEATURE_Y],params[sample][POSE_FEATURE_Z]);
      Rotation r1 = Rotation::RPY(params[sample][POSE_FEATURE_ROLL],params[sample][POSE_FEATURE_PITCH],params[sample][POSE_FEATURE_YAW]);
      Pose p(r1,v1);

      robot->updateHint(start_pts[sample].positions);
      robot->updateVelocityHint(start_pts[sample].velocities);
      int ik_result = robot->IkPos(p,q);

      if (ik_result < 0) {
        ++ik_tries;
        if (ik_tries > 2*nsamples) {
          std::cerr << __FILE__ << ":" << __LINE__ << ": We are really having trouble with IK!\n";
          //for (unsigned int i = 0; i < POSE_RPY_SIZE; ++i) {
          //  std::cout << params[sample][i];
          //}
          std::cout << std::endl;
          std::cout << "XYZ = " << v1 << std::endl;
          std::cout << "RPY = " << params[sample][POSE_FEATURE_ROLL] << ", "
            << params[sample][POSE_FEATURE_PITCH] << ", " << params[sample][POSE_FEATURE_YAW] << "\n";
          std::cout << "Max tries reached: " << 10*nsamples << "\n";
          break;
        }
        continue;
      }

      unsigned int idx = 0;
      //std::cout << "GOAL = ";
      for (; idx < dim; ++idx) {
        dmp_goal[idx] = q(idx);
        //std::cout << q(idx) << " ";
      }
      //std::cout << std::endl;

      for (unsigned int i = 0; i < dim; ++i) {
        for (unsigned int j = 0; j < nbasis; ++j) {
          dmp_list[i].weights[j] = params[sample][idx++];
        }
      }

      unsigned char at_goal;
      dmp::DMPTraj plan;

      //std::cout << sample << "\n" << start_pts.size() << "\n";
      //std::cout << "pos " << start_pts[0].positions.size() << "\n";
      //for (const double &d: start_pts[0].positions) {
      //  std::cout << d << " ";
      //}
      //std::cout << std::endl;

      dmp::generatePlan(dmp_list,
                        start_pts[sample].positions,
                        start_pts[sample].velocities,
                        0,dmp_goal,goal_threshold,-1,tau,0.1,5,plan,at_goal);

      if (verbose) {
        std::cout << "--------------------------" << std::endl;

        std::cout << "Using joints = ";
        for (const double &q: robot->getJointPos()) {
          std::cout << q << " ";
        }
        std::cout << std::endl;        std::cout << "at goal: " << (unsigned int)at_goal << std::endl;
        std::cout << "points: " << plan.points.size() << std::endl;
      }

      trajs[sample].points.resize(plan.points.size());
      for (unsigned int j = 0; j < plan.points.size(); ++j) {
        trajs[sample].points[j].positions = plan.points[j].positions;
        trajs[sample].points[j].velocities = plan.points[j].velocities;
        for (double &v: trajs[sample].points[j].velocities) {
          v *= dmp_velocity_multiplier;
        }
      }

      if (checker) {
        bool collision = !checker->TryTrajectory(trajs[sample],collision_detection_step);

        if (collision) {
          //std::cout << "COLLISION DETECTED!\n";
          ++ik_tries;
          if (ik_tries > 2*nsamples) {
            std::cerr << __FILE__ << ":" << __LINE__ << ": We are really having trouble with collisions!\n";
            break;
          } else {
            continue;
          }
        }
      }

      ++sample;
    }

    //std::cout << "sampled " << sample << "\n";
    return sample;
  }

  /* ==================================================================== */
  /* ==================================================================== */
  /*             BELOW THIS: SAME AS TRAJECTORY DISTRIBUTION              */
  /* ==================================================================== */
  /* ==================================================================== */

  /** is there an object attached */
  bool DmpTrajectoryDistribution::hasAttachedObject() const {
    return attached;
  }

  /** get the attached object frame */
  const Pose &DmpTrajectoryDistribution::getAttachedObjectFrame() const {
    return attachedObjectFrame;
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void DmpTrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      unsigned int nsamples,
      double diagonal_noise)
  {
    update(params,ps,nsamples,diagonal_noise,def_step_size);
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void DmpTrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      unsigned int nsamples)
  {
    update(params,ps,nsamples,diagonal_sigma,def_step_size);
  }

  /**
   * update
   * take a set of trajectories and samples
   * use the trajectories to reweight the distribution
   */
  void DmpTrajectoryDistribution::update(
      std::vector<EigenVectornd> &params,
      std::vector<double> &ps,
      unsigned int full_nsamples,
      double diagonal_noise,
      double step_size)
  {

    unsigned int nsamples = full_nsamples;
    double psum = 0;
    for (unsigned int i = 0; i < full_nsamples; ++i) {
      assert(not std::isnan(ps[i]));
      if (params[i].size() == 0) {
        std::cout << __FILE__ << ":" << __LINE__ << ": Not enough params; skipping the rest.\n";
        nsamples = i;
        break;
      }
      psum += ps[i];
    }

    if (psum == 0) {
      return;
    }

    if (dist.k == 1) {

      // one cluster only
      // compute mean

      //std::cout << "BEFORE:\n";
      //std::cout << dist.ns[0].P << "\n";

      dist.ns[0].mu *= (1 - step_size); //setZero();
      dist.ns[0].P *= (1 - step_size); //setZero();

      for (unsigned int i = 0; i < nsamples; ++i) {
        //std::cout << "mu rows = " << dist.ns[0].mu.rows() << ", vec rows = " << vec.rows() << std::endl;
        //std::cout << "mu cols = " << dist.ns[0].mu.cols() << ", vec cols = " << vec.cols() << std::endl;
        double wt = step_size * ps[i] / psum;
        assert (not std::isnan(wt));
        dist.ns[0].mu += params[i] * wt;
      }

      for (unsigned int i = 0; i < nsamples; ++i) {
        double wt = step_size * ps[i] / psum;
        //std::cout << wt << ", " << ps[i] << ", " << psum << std::endl;
        //dist.ns[0].P += wt * (params[i] - dist.ns[0].mu) * (params[i] - dist.ns[0].mu).transpose();
        dist.ns[0].P += (wt * (params[i] - dist.ns[0].mu)).array().square().matrix().asDiagonal();// * (params[i] - dist.ns[0].mu);
      }

      //std::cout << "AFTER:\n";
      //std::cout << dist.ns[0].P << "\n";

    } else {

      // set up weighted data
      // and then fit GMM again

      std::vector<std::pair<EigenVectornd,double> > data(nsamples);//ps.size());
      for (unsigned int i = 0; i < nsamples; ++i) {
        data[0].first = params[i];
        data[0].second = ps[i] / psum;
      }

      dist.Fit(data);

    }

    for (unsigned int i = 0; i < dist.k; ++i) {
      dist.ns[0].P += diagonal_noise * Matrix<double,Dynamic,Dynamic>::Identity(nvars,nvars);
    }

    dist.Update();
  }

  /**
   * set the skip between calling collision detection
   */
  unsigned int DmpTrajectoryDistribution::setCollisionDetectionStep(unsigned int step) {
    collision_detection_step = step;
  }

}


// Copyright Oleg Maximenko 2014.
// 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)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <cmath>
#include <boost/array.hpp>
#include <boost/noncopyable.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/mpl/if.hpp>
#include <boost/parameter.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <svgpp/detail/adapt_context.hpp>
#include <svgpp/policy/transform_events.hpp>
#include <svgpp/policy/transform.hpp>

namespace svgpp
{

namespace detail
{

template<class Context, class Number, class EventsPolicy>
class passthrough_transform_adapter
{
public:
  typedef Number number_type;

  passthrough_transform_adapter(Context & context)
    : context(context)
  {
  }

  Context & get_output_context() const { return context; }

  void on_exit_attribute() const
  {
  }

  void transform_matrix(const boost::array<number_type, 6> & matrix)
  {
    EventsPolicy::transform_matrix(context, matrix);
  }

  void transform_translate(number_type tx, number_type ty)
  {
    EventsPolicy::transform_translate(context, tx, ty);
  }

  void transform_translate(number_type tx)
  {
    EventsPolicy::transform_translate(context, tx);
  }

  void transform_scale(number_type sx, number_type sy)
  {
    EventsPolicy::transform_scale(context, sx, sy);
  }

  void transform_scale(number_type scale)
  {
    EventsPolicy::transform_scale(context, scale);
  }

  void transform_rotate(number_type angle)
  {
    EventsPolicy::transform_rotate(context, angle);
  }

  void transform_rotate(number_type angle, number_type cx, number_type cy)
  {
    EventsPolicy::transform_rotate(context, angle, cx, cy);
  }

  void transform_skew_x(number_type angle)
  {
    EventsPolicy::transform_skew_x(context, angle);
  }

  void transform_skew_y(number_type angle)
  {
    EventsPolicy::transform_skew_y(context, angle);
  }

protected:
  Context & context;
};

template<class Context, class Number, class EventsPolicy>
class join_transform_adapter: boost::noncopyable
{
public:
  typedef Number number_type;

  join_transform_adapter(Context & context)
    : context(context)
  {
    matrix[0] = 1; matrix[2] = 0; matrix[4] = 0;
    matrix[1] = 0; matrix[3] = 1; matrix[5] = 0;
  }

  Context & get_output_context() const { return context; }

  void on_exit_attribute() const
  {
    EventsPolicy::transform_matrix(context, matrix);
  }

  void transform_matrix(const boost::array<number_type, 6> &matrix2)
  {
    boost::array<number_type, 6> new_matrix;
    for(size_t i = 0; i < 2; ++i)
    {
      for(size_t j = 0; j < 3; ++j)
        new_matrix[i + j * 2] = matrix[i] * matrix2[j * 2] + matrix[i + 2] * matrix2[j * 2 + 1];
      new_matrix[i + 4] += matrix[i + 4];
    }
    matrix = new_matrix;
  }

protected:
  Context & context;

private:
  boost::array<number_type, 6> matrix;
};

template<class Base>
class matrix_only_transform_adapter: public Base
{
public:
  typedef typename Base::number_type number_type;

  template<class Context>
  matrix_only_transform_adapter(Context & context)
    : Base(context)
  {
  }

  void transform_translate(number_type tx, number_type ty)
  {
    const boost::array<number_type, 6> matrix = {{1, 0, 0, 1, tx, ty}};
    this->transform_matrix(matrix);
  }

  void transform_scale(number_type sx, number_type sy)
  {
    const boost::array<number_type, 6> matrix = {{sx, 0, 0, sy, 0, 0}};
    this->transform_matrix(matrix);
  }

  void transform_rotate(number_type angle)
  {
    BOOST_STATIC_ASSERT(boost::is_floating_point<number_type>::value);
    number_type cosa = std::cos(angle * boost::math::constants::degree<number_type>()),
      sina = std::sin(angle * boost::math::constants::degree<number_type>());
    const boost::array<number_type, 6> matrix = {{cosa, sina, -sina, cosa, 0, 0}};
    this->transform_matrix(matrix);
  }

  void transform_skew_x(number_type angle)
  {
    BOOST_STATIC_ASSERT(boost::is_floating_point<number_type>::value);
    const boost::array<number_type, 6> matrix =
      {{1, 0, std::tan(angle * boost::math::constants::degree<number_type>()), 1, 0, 0}};
    this->transform_matrix(matrix);
  }

  void transform_skew_y(number_type angle)
  {
    BOOST_STATIC_ASSERT(boost::is_floating_point<number_type>::value);
    const boost::array<number_type, 6> matrix =
      {{1, std::tan(angle * boost::math::constants::degree<number_type>()), 0, 1, 0, 0}};
    this->transform_matrix(matrix);
  }
};

template<class Context, class TransformPolicy, class EventsPolicy, class Number>
struct transform_adapter_base
{
  typedef typename boost::mpl::if_c<
    TransformPolicy::join_transforms,
      matrix_only_transform_adapter<
        join_transform_adapter<Context, Number, EventsPolicy>
      >,
      typename boost::mpl::if_c<
        TransformPolicy::only_matrix_transform,
          matrix_only_transform_adapter<
            passthrough_transform_adapter<Context, Number, EventsPolicy>
          >,
          passthrough_transform_adapter<Context, Number, EventsPolicy>
        >::type
      >::type type;
};

} // namespace detail

template<
  class Context,
  class TransformPolicy = typename policy::transform::by_context<Context>::type,
  class EventsPolicy = policy::transform_events::default_policy<Context>,
  class Number = double,
  class Enable = void>
class transform_adapter;

template<
  class Context,
  class TransformPolicy,
  class EventsPolicy,
  class Number
>
class transform_adapter<Context, TransformPolicy, EventsPolicy, Number,
  typename boost::enable_if_c<
       TransformPolicy::no_shorthands
    || TransformPolicy::only_matrix_transform
    || TransformPolicy::join_transforms>::type
>:
  public detail::transform_adapter_base<Context, TransformPolicy, EventsPolicy, Number>::type
{
  typedef typename detail::transform_adapter_base<Context, TransformPolicy, EventsPolicy, Number>::type base;

public:
  typedef Number number_type;

  transform_adapter(Context & context)
    : base(context)
  {}

  using base::transform_translate;
  using base::transform_scale;
  using base::transform_rotate;

  void transform_translate(number_type tx)
  {
    transform_translate(tx, 0);
  }

  void transform_scale(number_type scale)
  {
    transform_scale(scale, scale);
  }

  void transform_rotate(number_type angle, number_type cx, number_type cy)
  {
    transform_translate(cx, cy);
    transform_rotate(angle);
    transform_translate(-cx, -cy);
  }
};

template<
  class Context,
  class TransformPolicy,
  class EventsPolicy,
  class Number
>
class transform_adapter<Context, TransformPolicy, EventsPolicy, Number,
  typename boost::disable_if_c<
       TransformPolicy::no_shorthands
    || TransformPolicy::only_matrix_transform
    || TransformPolicy::join_transforms>::type
>:
  public detail::transform_adapter_base<Context, TransformPolicy, EventsPolicy, Number>::type
{
  typedef typename detail::transform_adapter_base<Context, TransformPolicy, EventsPolicy, Number>::type base;

public:
  transform_adapter(Context & context)
    : base(context)
  {}
};

namespace detail
{

template<class TransformPolicy>
struct need_transform_adapter:
  boost::mpl::bool_<
       TransformPolicy::join_transforms
    || TransformPolicy::no_rotate_about_point
    || TransformPolicy::no_shorthands
    || TransformPolicy::only_matrix_transform>
{};

template<class OriginalContext, class Enable = void>
struct transform_adapter_if_needed
{
private:
  struct adapter_stub
  {
    template<class Context> adapter_stub(Context const &) {}
  };

public:
  typedef adapter_stub type;
  typedef OriginalContext adapted_context;
  typedef OriginalContext & adapted_context_holder;

  static OriginalContext & adapt_context(OriginalContext & context, adapter_stub &)
  {
    return context;
  }

  static void on_exit_attribute(type const &) {}
};

template<class OriginalContext>
struct transform_adapter_if_needed<OriginalContext,
  typename boost::enable_if<need_transform_adapter<typename detail::unwrap_context<OriginalContext, tag::transform_policy>::policy> >::type>
{
private:
  typedef typename detail::unwrap_context<OriginalContext, tag::transform_policy>::policy transform_policy;
  typedef typename detail::unwrap_context<OriginalContext, tag::transform_events_policy>::policy original_transform_events_policy;
  typedef typename detail::unwrap_context<OriginalContext, tag::number_type>::policy number_type;

public:
  typedef transform_adapter<
    typename original_transform_events_policy::context_type,
    transform_policy,
    original_transform_events_policy,
    number_type
  > type;

  typedef const adapted_context_wrapper<
    OriginalContext,
    type,
    tag::transform_events_policy,
    policy::transform_events::forward_to_method<type>
  > adapted_context;
  typedef adapted_context adapted_context_holder;

  static adapted_context adapt_context(OriginalContext & context, type & adapter)
  {
    return adapted_context(context, adapter);
  }

  static void on_exit_attribute(type & adapter)
  {
    adapter.on_exit_attribute();
  }
};

} // namespace detail

}