#include "vi-map/landmark-quality-metrics.h"

#include <limits>
#include <vector>

#include <Eigen/Dense>
#include <aslam/common/statistics/statistics.h>
#include <gflags/gflags.h>
#include <vi-map/landmark.h>
#include <vi-map/vi-map.h>

DEFINE_double(
    vi_map_landmark_quality_min_observation_angle_deg, 5,
    "Minimum angle disparity of observers for a landmark to be "
    "well constrained.");

DEFINE_uint64(
    vi_map_landmark_quality_min_observers, 4,
    "Minimum number of observers for a landmark to be "
    "well constrained.");

DEFINE_double(
    vi_map_landmark_quality_max_distance_from_closest_observer, 40,
    "Maximum distance from closest observer for a landmark to be "
    "well constrained [m].");

DEFINE_double(
    vi_map_landmark_quality_min_distance_from_closest_observer, 0.05,
    "Minimum distance from closest observer for a landmark to be "
    "well constrained [m].");

namespace vi_map {
LandmarkWellConstrainedSettings::LandmarkWellConstrainedSettings()
    : max_distance_to_closest_observer(
          FLAGS_vi_map_landmark_quality_max_distance_from_closest_observer),
      min_distance_to_closest_observer(
          FLAGS_vi_map_landmark_quality_min_distance_from_closest_observer),
      min_observation_angle_deg(
          FLAGS_vi_map_landmark_quality_min_observation_angle_deg),
      min_observers(FLAGS_vi_map_landmark_quality_min_observers) {}

bool isLandmarkWellConstrained(//在优化的时候调用
    const vi_map::VIMap& map, const vi_map::Landmark& landmark)
    {
  constexpr bool kReEvaluateQuality = false;//因为只是查看这个地图点的质量，所以不用再去重新评估这个地图点的质量
  return isLandmarkWellConstrained(map, landmark, kReEvaluateQuality);
}

    bool isLandmarkWellConstrained(//进行地图点质量的判定
            const vi_map::VIMap& map, const vi_map::Landmark& landmark,
            bool re_evaluate_quality)
    {
        //在重新三角化时，这里会默认设成kbad，而在关键帧选择以后重新评估点质量时，这里应该是good和bad都有
        vi_map::Landmark::Quality quality = landmark.getQuality();
        // Localization landmarks are always good.
        if (quality == vi_map::Landmark::Quality::kLocalizationSummaryLandmark) {
            return true;
        }
        if (re_evaluate_quality) {//在重新三角化时，kReEvaluateQuality会默认设成true,也就是重新评估质量
            quality = vi_map::Landmark::Quality::kUnknown;
        }

        if (quality != vi_map::Landmark::Quality::kUnknown) {
            return quality == vi_map::Landmark::Quality::kGood;
        }

        // Use default settings.
        LandmarkWellConstrainedSettings settings;

        const vi_map::KeypointIdentifierList& backlinks = landmark.getObservations();//得到这个地图点的所有观测信息
        if (backlinks.size() < settings.min_observers) {//FLAGS_vi_map_landmark_quality_min_observers去赋值这个最小观测
            statistics::StatsCollector stats("Landmark has too few backlinks");
            stats.IncrementOne();
            return false;
        }

        const Eigen::Vector3d& p_G_fi = map.getLandmark_G_p_fi(landmark.id());//应该是vio三角化的点吧
        Aligned<std::vector, Eigen::Vector3d> G_normalized_incidence_rays;
        G_normalized_incidence_rays.reserve(backlinks.size());
        double signed_distance_to_closest_observer =
                std::numeric_limits<double>::max();
        for (const vi_map::KeypointIdentifier& backlink : backlinks)
        {
            const vi_map::Vertex& vertex = map.getVertex(backlink.frame_id.vertex_id);

            const pose::Transformation T_G_C =//得到这帧在世界坐标系下的位姿
                    map.getVertex_T_G_I(backlink.frame_id.vertex_id) *
                    map.getSensorManager()
                            .getNCameraForMission(vertex.getMissionId())
                            .get_T_C_B(backlink.frame_id.frame_index)
                            .inverse();

            const Eigen::Vector3d p_C_fi = map.getLandmark_p_C_fi(//当前这个地图点在这帧中的相机坐标系中的位置
                    landmark.id(), vertex, backlink.frame_id.frame_index);
            const Eigen::Vector3d G_incidence_ray = T_G_C.getPosition() - p_G_fi;//这个只是全局坐标系下相机的位置和这个地图点位置的差值

            const double distance = G_incidence_ray.norm();//求相机到这个点的绝对距离
            const double signed_distance = distance * (p_C_fi(2) < 0.0 ? -1.0 : 1.0);//带符号的距离
            signed_distance_to_closest_observer =//求最小的带符号的距离
                    std::min(signed_distance_to_closest_observer, signed_distance);

            if (distance > 0) {
                G_normalized_incidence_rays.emplace_back(G_incidence_ray / distance);//这是为了之后算cos，也就是两帧和他们观测到的路标点之间算夹角时用到
            }
        }

        return isLandmarkWellConstrained(
                G_normalized_incidence_rays, signed_distance_to_closest_observer,
                settings);
    }
}  // namespace vi_map


/*
 *  assemble.cpp
 *  cufflinks
 *
 *  Created by Cole Trapnell on 3/23/09.
 *  Copyright 2009 Cole Trapnell. All rights reserved.
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <map>
#include <list>
#include <vector>
#include <iostream>

#include <lemon/topology.h>

//#include <lemon/graph_utils.h>
#include <lemon/bipartite_matching.h>

#include <boost/ref.hpp>
// DON'T move this, or mystery compiler errors will result. Affects gcc >= 4.1
#include <boost/graph/vector_as_graph.hpp>

#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/graph_traits.hpp>
//#include <boost/numeric/ublas/io.hpp>

#include <boost/version.hpp>

#if (BOOST_VERSION < 103800)
#include <boost/vector_property_map.hpp>
#else
#include <boost/property_map/vector_property_map.hpp>
#endif

#include <boost/math/distributions/normal.hpp> // for normal_distribution
using boost::math::normal; // typedef provides default type is double.

#include "transitive_closure.h"
#include "transitive_reduction.h"

#include "common.h"
#include "assemble.h"

#include "bundles.h"
//#include "filters.h"
//#include "genes.h"
#include "scaffolds.h"
#include "clustering.h"
#include "matching_merge.h"
#include "graph_optimize.h"

using namespace boost;
using namespace std;

bool mate_graphs(const HitBundle& bundle, BundleStats* stats);

typedef lemon::SmartBpUGraph ReachGraph;

long long weight_of_merge(Scaffold& lhs,
						  Scaffold& rhs,
						  double source_psi,
						  double target_psi)
{
	//double expected_cov_diff = max(1.0, 0.1 * (source_doc + target_doc));
	//normal cov_norm(0, expected_cov_diff);

	normal cov_test_norm(0, 1.0);

	double score = 0.0;

	// HACK: This early breakout prevents spliced reads that cross exactly one
	// intron from being matched up if they both cross the same intron.
	// Otherwise, we get phasing problems, as introns aren't matched up long
	// distance.  Ugh..
	if (Scaffold::overlap_in_genome(lhs, rhs, 0))
	{
		bool lh_intron = lhs.has_intron();
		bool rh_intron = rhs.has_intron();

		if (lh_intron && rh_intron)
		{
			vector<pair<int, int> > lh_gaps = lhs.gaps();
			vector<pair<int, int> > rh_gaps = rhs.gaps();
			if (lh_gaps.size() == 1 && lh_gaps == rh_gaps)
				return 999999999;
		}
	}

	if (source_psi > 0 && target_psi > 0 )
	{
		double test_stat = log(1.0 - abs(source_psi - target_psi));
		score = test_stat;
		assert (score <= 0.0);
	}
	else
	{
		return 999999999;
	}

	assert (score <= 0.0);
	if (score >= -1e-6)
		score = -1e-6;
	int weight = (int)(score * -1e6);
	return weight;
}

typedef map<DAGNode, pair<int, int> > DagToBp;
void create_reachability_bp_graph(DAG& dag,
                                  ReachGraph& reach_graph,
                                  vector<ReachGraph::BNode> b_to_a,
                                  DagToBp& dag_to_bp,
                                  const adjacency_list<>& TC,
                                  const vector<bool>& scaffold_mask)
{
	//typedef graph_traits<DAG>::vertex_descriptor Vertex;
	HitsForNodeMap hits_for_node = get(vertex_name, dag);

	//fprintf (stdout, "\tclosure edges:\t\t\t\%d\n", num_edges(TC));
	graph_traits < adjacency_list<> >::vertex_iterator v, vend;

	b_to_a.resize(num_vertices(TC));

	for (tie(v, vend) = vertices(TC); v != vend; ++v)
	{
		DagToBp::iterator itr = dag_to_bp.find(*v);
		if (itr == dag_to_bp.end())
		{
			ReachGraph::ANode A = reach_graph.addANode();
			int a = reach_graph.aNodeId(A);

			ReachGraph::BNode B = reach_graph.addBNode();
			int b = reach_graph.bNodeId(B);
			b_to_a[b] = A;
			dag_to_bp[*v] = make_pair(a, b);

		}
	}

	reach_graph.reserveEdge(num_edges(TC));
	reach_graph.reserveANode(num_vertices(TC));
	reach_graph.reserveBNode(num_vertices(TC));

	graph_traits < adjacency_list<> >::edge_iterator i, end;
	for (tie(i, end) = edges(TC); i != end; ++i)
	{
		int a_id = -1;
		int b_id = -1;
		DAGNode s = source(*i, TC);
		DAGNode t = target(*i, TC);

		DagToBp::iterator itr = dag_to_bp.find(s);
		if (itr == dag_to_bp.end())
		{
			assert (false);
		}
		else
		{
			a_id = itr->second.first;
		}

		itr = dag_to_bp.find(t);

		if (itr == dag_to_bp.end())
		{
			assert(false);
		}
		else
		{
			b_id = itr->second.second;
		}

		if (in_degree(s, dag) == 0  /* virtual "source"? */||
            out_degree(t, dag) == 0 /* virtual "sink"?*/)
			continue;

		assert (a_id != -1);
		assert (b_id != -1);

		ReachGraph::ANode a_node = reach_graph.nodeFromANodeId(a_id);
		ReachGraph::BNode b_node = reach_graph.nodeFromBNodeId(b_id);
		ReachGraph::ANode a_for_b = b_to_a[b_id];

		assert (a_for_b != a_node);

		if (scaffold_mask[a_id] &&
			scaffold_mask[b_id])
		{
			reach_graph.addEdge(a_node,
								b_node);
		}
	}
}

void add_weights_to_reachability_bp_graph(ReachGraph& bp,
									   const HitsForNodeMap& hits_for_node,
									   const vector<Scaffold>& hits,
									   const vector<Scaffold>& scaffolds,
									   ReachGraph::UEdgeMap<long long>& weights)
{
	// number of reads of reads spliced in to each scaffold
	vector<double> spliced_in(scaffolds.size(), 0.0);

	// number spliced in / length of scaffold
	vector<double> density(scaffolds.size(), 0.0);

	// number of reads spliced in to scaffold + those that just overlap it
	//vector<double> overlapping(scaffolds.size(), 0.0);

	for (size_t i = 0; i < scaffolds.size(); ++i)
	{
		for (size_t j = 0; j < hits.size(); ++j)
		{
			if (!hits[j].is_ref() && scaffolds[i].contains(hits[j]))
			{
				if (Scaffold::compatible(scaffolds[i],hits[j]))
				{
					spliced_in[i]++;
				}
			}
		}
		density[i] = spliced_in[i] / scaffolds[i].length();
	}

	// percent spliced in = density / (total density of overlapping scaffolds)
	vector<double> psi(scaffolds.size(), 0.0);
	vector<double> local_density(scaffolds.size(), 0.0);
	for (size_t i = 0; i < scaffolds.size(); ++i)
	{
		double total_density = 0.0;
		int num_overlaps = 0;
		double compatible_density = 0.0;
		for (size_t j = 0; j < scaffolds.size(); ++j)
		{
			if (Scaffold::overlap_in_genome(scaffolds[i],scaffolds[j], 0))
			{
				total_density += density[j];
				num_overlaps++;
				if (Scaffold::compatible(scaffolds[i], scaffolds[j]))
				{
					compatible_density += density[j];
				}
			}
		}
		if (total_density)
			psi[i] = compatible_density / total_density;
		local_density[i] = compatible_density;
	}

	for (ReachGraph::UEdgeIt i(bp); i!=lemon::INVALID; ++i)
	{
		ReachGraph::ANode a = bp.source(i);
		ReachGraph::BNode b = bp.target(i);
		DAGNode a_dag = bp.aNodeId(a);
		int a_id_for_b = bp.aNodeId(b);
		ReachGraph::ANode a_for_b = bp.nodeFromANodeId(a_id_for_b);
		assert (a_for_b != lemon::INVALID);
		DAGNode b_dag = a_id_for_b;

		Scaffold* a_scaff = hits_for_node[a_dag];
		Scaffold* b_scaff = hits_for_node[b_dag];

		size_t aidx = a_scaff - &scaffolds[0];
		size_t bidx = b_scaff - &scaffolds[0];

		double a_psi = psi[aidx];

		double b_psi = psi[bidx];

		//assert (a_psi != 1.0);
		//assert (b_psi != 1.0);

		long long weight = weight_of_merge(*a_scaff,
										   *b_scaff,
										   a_psi,
										   b_psi);

		if (weight < 0)
			weight = 10000000;

		//fprintf(stderr, "Similarity between %d, %d = %.20lf\n", bp.aNodeId(a), a_id_for_b, weight);
		weights[i] = weight;
	}
}

void holdout_transitivity_hazards(vector<Scaffold>& hits,
								  vector<Scaffold>& hazards)
{
	vector<pair<int,int> > introns;
	for (size_t i = 0; i < hits.size(); ++i)
	{
		const vector<AugmentedCuffOp>& ops = hits[i].augmented_ops();
		for (size_t j = 0; j < ops.size(); ++j)
		{
			const AugmentedCuffOp& op = ops[j];
			if (op.opcode == CUFF_INTRON)
				introns.push_back(make_pair(op.g_left(), op.g_right()));
		}
	}

	sort(introns.begin(), introns.end());
	vector<pair<int, int> >::iterator new_end = unique(introns.begin(),
													   introns.end());
	introns.erase(new_end, introns.end());

	vector<pair<int, int> > evil_introns;
	for (size_t i = 0; i < introns.size(); ++i)
	{
		for (size_t j = i + 1; j < introns.size(); ++j)
		{
			if (overlap_in_genome(introns[i].first, introns[i].second,
								  introns[j].first, introns[j].second))
			{
				evil_introns.push_back(introns[i]);
				evil_introns.push_back(introns[j]);
			}
		}
	}

	sort(evil_introns.begin(), evil_introns.end());
	new_end = unique(evil_introns.begin(), evil_introns.end());
	evil_introns.erase(new_end, evil_introns.end());

	vector<Scaffold> filtered_hits;
	for (size_t i = 0; i < hits.size(); ++i)
	{
		bool overlaps_evil_intron = false;
		const vector<AugmentedCuffOp>& ops = hits[i].augmented_ops();
		for (size_t j = 0; j < ops.size(); ++j)
		{
			const AugmentedCuffOp& op = ops[j];
			if (op.opcode == CUFF_UNKNOWN)
			{
				for (size_t k = 0; k < evil_introns.size(); ++k)
				{

					if (overlap_in_genome(op.g_left(), op.g_right(),
										  evil_introns[k].first, evil_introns[k].second))
					{
						overlaps_evil_intron = true;
					}
				}
			}
		}
		if (overlaps_evil_intron)
		{
//            if (hits[i].has_intron())
//            {
//                fprintf(stderr, "&&& Holding out intron-containing hazard at %d-%d\n", hits[i].left(), hits[i].right());
//            }
			hazards.push_back(hits[i]);
		}
		else
		{
			filtered_hits.push_back(hits[i]);
		}
	}

	verbose_msg( "%s\tHeld out %lu scaffolds as transitivity hazards\n", bundle_label->c_str(), hazards.size());

	hits = filtered_hits;
}

bool make_scaffolds(int bundle_left,
					int bundle_length,
					vector<Scaffold>& hits,
					vector<Scaffold>& scaffolds)
{
	if (hits.empty())
		return true;

	bool intron_hits = false;
	for (size_t i = 0; i < hits.size(); ++i)
	{
		if (hits[i].has_intron())
		{
			intron_hits = true;
			break;
		}
	}

	if (!intron_hits)
	{
		verbose_msg( "%s\tNo introns in bundle, collapsing all hits to single transcript\n", bundle_label->c_str());
		scaffolds.push_back(Scaffold(hits));
		fill_gaps(scaffolds, 2 * olap_radius);
	}
	else
	{
		verbose_msg( "%s\tBundle has spliced reads\n", bundle_label->c_str());

		vector<Scaffold> hazards;
		holdout_transitivity_hazards(hits, hazards);

        vector<Scaffold> split_hazards;
        // Cleave the partials at their unknowns to minimize FPKM dilation on
        // the low end of the expression profile.
        for (size_t i = 0; i < hazards.size(); ++i)
        {
            vector<Scaffold> c;
            hazards[i].get_complete_subscaffolds(c);
            split_hazards.insert(split_hazards.end(), c.begin(), c.end());
        }

		vector<Scaffold> orig_hits = hits;

        hits.insert(hits.end(), split_hazards.begin(), split_hazards.end());

        compress_fragments(hits);

        verbose_msg( "%s\tAssembling bundle with %lu hits\n", bundle_label->c_str(), hits.size());

		vector<float> depth_of_coverage(bundle_length,0);
		map<pair<int,int>, float> intron_depth_of_coverage;
		compute_doc(bundle_left,
					hits,
					depth_of_coverage,
					intron_depth_of_coverage,
					false);

		normal norm(0, 0.1);

		vector<const MateHit*> prev_chaff;
		while (true)
		{
			static size_t MAX_BUNDLE_ALIGNMENTS = 0xFFFF;

			sort(hits.begin(), hits.end(), scaff_lt);

            //fprintf(stderr, "\tCurrent bundle has %d non-constitutive fragments\n", hits.size());

			DAG bundle_dag;

			if (hits.empty())
				return true;
			verbose_msg( "%s\tCalculating scaffold densities\n", bundle_label->c_str());
			vector<double> scaff_doc;
			record_doc_for_scaffolds(bundle_left,
									 hits,
									 depth_of_coverage,
									 intron_depth_of_coverage,
									 scaff_doc);
			verbose_msg( "%s\tCreating compatibility graph\n", bundle_label->c_str());

			if (!create_overlap_dag(hits, bundle_dag))
			{
				break;
			}

            HitsForNodeMap hits_for_node = get(vertex_name, bundle_dag);

            compress_overlap_dag_paths(bundle_dag, hits);

            if (hits.size() >= MAX_BUNDLE_ALIGNMENTS)
			{
				verbose_msg( "%s\tWarning: bundle too large, skipping assembly\n", bundle_label->c_str());
				return false;
			}

            pair<DAGNode, DAGNode> terminal = add_terminal_nodes(bundle_dag);
            DAGNode source = terminal.first;
            DAGNode sink = terminal.second;

			ReachGraph bp;

			verbose_msg( "%s\tConstructing reachability graph\n", bundle_label->c_str());

			vector<ReachGraph::BNode> b_to_a;
			adjacency_list<> TC;

			transitive_closure(bundle_dag, TC);
			DagToBp dag_to_bp;

            // TODO: deprecate dependence of create_reachability_bp_graph() on scaffold_mask
			vector<bool> scaffold_mask(num_vertices(bundle_dag), true);

			create_reachability_bp_graph(bundle_dag, bp, b_to_a, dag_to_bp, TC, scaffold_mask);

			ReachGraph::UEdgeMap<long long> cov_weights(bp);
			add_weights_to_reachability_bp_graph(bp, hits_for_node, orig_hits, hits, cov_weights);

            verbose_msg( "%s\tPerforming weighted matching\n", bundle_label->c_str());

            typedef lemon::MinCostMaxBipartiteMatching<ReachGraph,ReachGraph::UEdgeMap<long long> > Matcher;
            Matcher matcher(bp, cov_weights);
            matcher.run();

            vector<vector<DAGNode> > chains;
            make_chains_from_matching<Matcher>(bp, matcher, chains);

			verbose_msg( "%s\tFound %d distinct chains\n", bundle_label->c_str(), (int)chains.size());

			vector<vector<DAGNode> > paths;
			extend_chains_to_paths(bundle_dag, chains, TC, source, sink, paths);

			verbose_msg( "%s\tCreating scaffolds for %d paths\n", bundle_label->c_str(), (int)paths.size());

			vector<Scaffold> new_scaffs;
			make_scaffolds_from_paths(bundle_dag, paths, new_scaffs);

			verbose_msg( "%s\tCollapsing scaffolds\n", bundle_label->c_str());

			collapse_contained_transfrags(new_scaffs);
			hits = new_scaffs;
		}

		scaffolds = hits;

		// One last collapse attempt...
		vector<Scaffold> new_scaffs = scaffolds;

		verbose_msg( "%s\tPerforming final collapse round\n", bundle_label->c_str());

		fill_gaps(new_scaffs, 2 * olap_radius);

		scaffolds = new_scaffs;

        // Cleave the partials at their unknowns to minimize FPKM dilation on
        // the low end of the expression profile.
        vector<Scaffold> completes;
        for (size_t i = 0; i < scaffolds.size(); ++i)
        {
            vector<Scaffold> c;
            scaffolds[i].get_complete_subscaffolds(c);
            completes.insert(completes.end(), c.begin(), c.end());
        }

        verbose_msg( "Extracted %lu contiguous transfrags from %lu scaffolds\n", completes.size(), scaffolds.size());

        new_scaffs = completes;
        sort(new_scaffs.begin(), new_scaffs.end(), scaff_lt);

		collapse_contained_transfrags(new_scaffs);
		sort(new_scaffs.begin(), new_scaffs.end(), scaff_lt);
		scaffolds = new_scaffs;
	}

    // TODO: refactor into subroutine.  This routine shouldn't actually be
    // necessary, and should be eliminated after thorough testing that it really
    // isn't needed
	for (size_t i = 0; i < scaffolds.size(); ++i)
	{
		//assert(!scaffolds[i].has_unknown());

		const vector<const MateHit*>& supporting = scaffolds[i].mate_hits();
		CuffStrand s = CUFF_STRAND_UNKNOWN;
		for (size_t j = 0; j < supporting.size(); ++j)
		{
//			assert (supporting[j]->strand() == CUFF_STRAND_UNKNOWN ||
//					s == supporting[j]->strand());
			if (supporting[j]->strand() != CUFF_STRAND_UNKNOWN)
				s = supporting[j]->strand();
		}
        if (scaffolds[i].strand() == CUFF_STRAND_UNKNOWN)
            scaffolds[i].strand(s);
	}
    // end refactor

	return true;
}


#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>
#include <rx/ranges.hpp>
#include <utility>

using namespace RX_NAMESPACE;

// This example is derived from the 'rangeless' library's example.
//
// See: https://github.com/ast-al/rangeless/blob/gh-pages/test/calendar.cpp

namespace greg = boost::gregorian;
using date_t = greg::date;

static void make_calendar(uint16_t year, uint8_t num_months_horizontally, std::ostream& os) {
    static constexpr std::array s_month_names = {
        "Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec"};
    seq(date_t(year, greg::Jan, 1), greg::date_duration(1))
        | until([year](date_t x) { return x.year() != year; })
        | group_adjacent_by(
            [](const date_t& d) { return std::make_pair(d.month(), d.week_number()); })
        | transform([&](const auto &wk_dates) -> std::pair<date_t::month_type, std::string> {
              const auto left_pad_amt = size_t(3 * ((wk_dates.get().day_of_week() + 7 - 1) % 7));
              return {wk_dates.get().month(),
                      wk_dates
                          | foldl(
                              std::string(left_pad_amt, ' '),
                              [](std::string ret_wk, const date_t& d) {
                                  return std::move(ret_wk) + (d.day() < 10 ? "  " : " ")
                                         + std::to_string(d.day());
                              })};
          })                                                       //
        | group_adjacent_by([](const auto& x) { return x.first; }) // group by month
        | in_groups_of(num_months_horizontally)                    //
        | for_each([&](const auto& group) {
              for (int row = -2; row < 6; ++row) {
                  for (const auto& mo : group | transform(to_vector())) {
                      os << std::setiosflags(std::ios::left) << std::setw(25)
                         << (row == -2 ? std::string{"        "} + s_month_names.at(mo.front().first - 1u)
                                       : row == -1 ? " Mo Tu We Th Fr Sa Su"
                                                   : size_t(row) < mo.size()
                                                         ? mo[row].second // formatted week
                                                         : "                     ");
                  }
                  os << std::endl;
              }
          });
}

int main() {
    make_calendar(2019, 3, std::cout);
    return 0;
}


//
//=======================================================================
// Copyright (c) 2004 Kristopher Beevers
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//

#ifndef BOOST_GRAPH_ASTAR_SEARCH_HPP
#define BOOST_GRAPH_ASTAR_SEARCH_HPP

#include <functional>
#include <vector>
#include <boost/limits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>

namespace boost {

  template <class Heuristic, class Graph>
  struct AStarHeuristicConcept {
    void constraints()
    {
      function_requires< CopyConstructibleConcept<Heuristic> >();
      h(u);
    }
    Heuristic h;
    typename graph_traits<Graph>::vertex_descriptor u;
  };

  template <class Graph, class CostType>
  class astar_heuristic : public std::unary_function<
    typename graph_traits<Graph>::vertex_descriptor, CostType>
  {
  public:
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    astar_heuristic() {}
    CostType operator()(Vertex u) { return static_cast<CostType>(0); }
  };

  template <class Visitor, class Graph>
  struct AStarVisitorConcept {
    void constraints()
    {
      function_requires< CopyConstructibleConcept<Visitor> >();
      vis.initialize_vertex(u, g);
      vis.discover_vertex(u, g);
      vis.examine_vertex(u, g);
      vis.examine_edge(e, g);
      vis.edge_relaxed(e, g);
      vis.edge_not_relaxed(e, g);
      vis.black_target(e, g);
      vis.finish_vertex(u, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits<Graph>::vertex_descriptor u;
    typename graph_traits<Graph>::edge_descriptor e;
  };

  template <class Visitors = null_visitor>
  class astar_visitor : public bfs_visitor<Visitors> {
  public:
    astar_visitor() {}
    astar_visitor(Visitors vis)
      : bfs_visitor<Visitors>(vis) {}

    template <class Edge, class Graph>
    void edge_relaxed(Edge e, Graph& g) {
      invoke_visitors(this->m_vis, e, g, on_edge_relaxed());
    }
    template <class Edge, class Graph>
    void edge_not_relaxed(Edge e, Graph& g) {
      invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed());
    }
  private:
    template <class Edge, class Graph>
    void tree_edge(Edge e, Graph& g) {}
    template <class Edge, class Graph>
    void non_tree_edge(Edge e, Graph& g) {}
  };
  template <class Visitors>
  astar_visitor<Visitors>
  make_astar_visitor(Visitors vis) {
    return astar_visitor<Visitors>(vis);
  }
  typedef astar_visitor<> default_astar_visitor;

  namespace detail {

    template <class AStarHeuristic, class UniformCostVisitor,
              class UpdatableQueue, class PredecessorMap,
              class CostMap, class DistanceMap, class WeightMap,
              class ColorMap, class BinaryFunction,
              class BinaryPredicate>
    struct astar_bfs_visitor
    {

      typedef typename property_traits<CostMap>::value_type C;
      typedef typename property_traits<ColorMap>::value_type ColorValue;
      typedef color_traits<ColorValue> Color;
      typedef typename property_traits<DistanceMap>::value_type distance_type;

      astar_bfs_visitor(AStarHeuristic h, UniformCostVisitor vis,
                        UpdatableQueue& Q, PredecessorMap p,
                        CostMap c, DistanceMap d, WeightMap w,
                        ColorMap col, BinaryFunction combine,
                        BinaryPredicate compare, C zero)
        : m_h(h), m_vis(vis), m_Q(Q), m_predecessor(p), m_cost(c),
          m_distance(d), m_weight(w), m_color(col),
          m_combine(combine), m_compare(compare), m_zero(zero) {}

      template <class Vertex, class Graph>
      void initialize_vertex(Vertex u, Graph& g) {
        m_vis.initialize_vertex(u, g);
      }
      template <class Vertex, class Graph>
      void discover_vertex(Vertex u, Graph& g) {
        m_vis.discover_vertex(u, g);
      }
      template <class Vertex, class Graph>
      void examine_vertex(Vertex u, Graph& g) {
        m_vis.examine_vertex(u, g);
      }
      template <class Vertex, class Graph>
      void finish_vertex(Vertex u, Graph& g) {
        m_vis.finish_vertex(u, g);
      }
      template <class Edge, class Graph>
      void examine_edge(Edge e, Graph& g) {
        if (m_compare(get(m_weight, e), m_zero))
          throw negative_edge();
        m_vis.examine_edge(e, g);
      }
      template <class Edge, class Graph>
      void non_tree_edge(Edge, Graph&) {}

      template <class Edge, class Graph>
      void tree_edge(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);

        if(m_decreased) {
          m_vis.edge_relaxed(e, g);
          put(m_cost, target(e, g),
              m_combine(get(m_distance, target(e, g)),
                        m_h(target(e, g))));
        } else
          m_vis.edge_not_relaxed(e, g);
      }

      template <class Edge, class Graph>
      void gray_target(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);

        if(m_decreased) {
          put(m_cost, target(e, g),
              m_combine(get(m_distance, target(e, g)),
                        m_h(target(e, g))));
          m_Q.update(target(e, g));
          m_vis.edge_relaxed(e, g);
        } else
          m_vis.edge_not_relaxed(e, g);
      }

      template <class Edge, class Graph>
      void black_target(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);

        if(m_decreased) {
          m_vis.edge_relaxed(e, g);
          put(m_cost, target(e, g),
              m_combine(get(m_distance, target(e, g)),
                        m_h(target(e, g))));
          m_Q.push(target(e, g));
          put(m_color, target(e, g), Color::gray());
          m_vis.black_target(e, g);
        } else
          m_vis.edge_not_relaxed(e, g);
      }

      AStarHeuristic m_h;
      UniformCostVisitor m_vis;
      UpdatableQueue& m_Q;
      PredecessorMap m_predecessor;
      CostMap m_cost;
      DistanceMap m_distance;
      WeightMap m_weight;
      ColorMap m_color;
      BinaryFunction m_combine;
      BinaryPredicate m_compare;
      bool m_decreased;
      C m_zero;

    };

  } // namespace detail

  template <typename VertexListGraph, typename AStarHeuristic,
            typename AStarVisitor, typename PredecessorMap,
            typename CostMap, typename DistanceMap,
            typename WeightMap, typename ColorMap,
            typename VertexIndexMap,
            typename CompareFunction, typename CombineFunction,
            typename CostInf, typename CostZero>
  inline void
  astar_search_no_init
    (VertexListGraph &g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     AStarHeuristic h, AStarVisitor vis,
     PredecessorMap predecessor, CostMap cost,
     DistanceMap distance, WeightMap weight,
     ColorMap color, VertexIndexMap /*index_map*/,
     CompareFunction compare, CombineFunction combine,
     CostInf /*inf*/, CostZero zero)
  {
    typedef typename graph_traits<VertexListGraph>::vertex_descriptor
      Vertex;
    typedef boost::vector_property_map<std::size_t> IndexInHeapMap;
    IndexInHeapMap index_in_heap;
    typedef d_ary_heap_indirect<Vertex, 4, IndexInHeapMap, CostMap, CompareFunction>
      MutableQueue;
    MutableQueue Q(cost, index_in_heap, compare);

    detail::astar_bfs_visitor<AStarHeuristic, AStarVisitor,
        MutableQueue, PredecessorMap, CostMap, DistanceMap,
        WeightMap, ColorMap, CombineFunction, CompareFunction>
      bfs_vis(h, vis, Q, predecessor, cost, distance, weight,
              color, combine, compare, zero);

    breadth_first_visit(g, s, Q, bfs_vis, color);
  }

  // Non-named parameter interface
  template <typename VertexListGraph, typename AStarHeuristic,
            typename AStarVisitor, typename PredecessorMap,
            typename CostMap, typename DistanceMap,
            typename WeightMap, typename VertexIndexMap,
            typename ColorMap,
            typename CompareFunction, typename CombineFunction,
            typename CostInf, typename CostZero>
  inline void
  astar_search
    (VertexListGraph &g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     AStarHeuristic h, AStarVisitor vis,
     PredecessorMap predecessor, CostMap cost,
     DistanceMap distance, WeightMap weight,
     VertexIndexMap index_map, ColorMap color,
     CompareFunction compare, CombineFunction combine,
     CostInf inf, CostZero zero)
  {

    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typename graph_traits<VertexListGraph>::vertex_iterator ui, ui_end;
    for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
      put(color, *ui, Color::white());
      put(distance, *ui, inf);
      put(cost, *ui, inf);
      put(predecessor, *ui, *ui);
      vis.initialize_vertex(*ui, g);
    }
    put(distance, s, zero);
    put(cost, s, h(s));

    astar_search_no_init
      (g, s, h, vis, predecessor, cost, distance, weight,
       color, index_map, compare, combine, inf, zero);

  }

  namespace detail {
    template <class VertexListGraph, class AStarHeuristic,
              class CostMap, class DistanceMap, class WeightMap,
              class IndexMap, class ColorMap, class Params>
    inline void
    astar_dispatch2
      (VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       AStarHeuristic h, CostMap cost, DistanceMap distance,
       WeightMap weight, IndexMap index_map, ColorMap color,
       const Params& params)
    {
      dummy_property_map p_map;
      typedef typename property_traits<CostMap>::value_type C;
      astar_search
        (g, s, h,
         choose_param(get_param(params, graph_visitor),
                      make_astar_visitor(null_visitor())),
         choose_param(get_param(params, vertex_predecessor), p_map),
         cost, distance, weight, index_map, color,
         choose_param(get_param(params, distance_compare_t()),
                      std::less<C>()),
         choose_param(get_param(params, distance_combine_t()),
                      closed_plus<C>()),
         choose_param(get_param(params, distance_inf_t()),
                      std::numeric_limits<C>::max BOOST_PREVENT_MACRO_SUBSTITUTION ()),
         choose_param(get_param(params, distance_zero_t()),
                      C()));
    }

    template <class VertexListGraph, class AStarHeuristic,
              class CostMap, class DistanceMap, class WeightMap,
              class IndexMap, class ColorMap, class Params>
    inline void
    astar_dispatch1
      (VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       AStarHeuristic h, CostMap cost, DistanceMap distance,
       WeightMap weight, IndexMap index_map, ColorMap color,
       const Params& params)
    {
      typedef typename property_traits<WeightMap>::value_type D;
      std::vector<D> distance_map;
      std::vector<D> cost_map;
      std::vector<default_color_type> color_map;

      detail::astar_dispatch2
        (g, s, h,
         choose_param(cost, vector_property_map<D, IndexMap>(index_map)),
         choose_param(distance, vector_property_map<D, IndexMap>(index_map)),
         weight, index_map,
         choose_param(color, vector_property_map<default_color_type, IndexMap>(index_map)),
         params);
    }
  } // namespace detail

  // Named parameter interface
  template <typename VertexListGraph,
            typename AStarHeuristic,
            typename P, typename T, typename R>
  void
  astar_search
    (VertexListGraph &g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     AStarHeuristic h, const bgl_named_params<P, T, R>& params)
  {

    detail::astar_dispatch1
      (g, s, h,
       get_param(params, vertex_rank),
       get_param(params, vertex_distance),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
       get_param(params, vertex_color),
       params);

  }

} // namespace boost

#endif // BOOST_GRAPH_ASTAR_SEARCH_HPP


#ifndef INCLUDES_HPP_
#define INCLUDES_HPP_

#include <iostream>

#include <string>
#include <fstream>
#include <sstream>
#include <map>
#include <iomanip>
#include <list>
#include <vector>
#include <stdlib.h>
#include <algorithm>
#include <inttypes.h>
#include <parallel/algorithm>
#include <iostream>
#include <algorithm>
#include <vector>
#include <set>
#include <cmath>
#include <cstdint>
#include <omp.h>

#include <opencv2/core/core.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;
using namespace cv;
using std::vector;
using std::ifstream;
using std::string;

using std::set;
using std::max;
using std::min;
using std::cin;
using std::cout;
using std::endl;
using std::string;
using std::list;
using std::vector;
using std::map;
using std::pair;

typedef uint64_t int_type_t;
typedef uint16_t offset_int_type_t;
typedef uint16_t uint_dist_type;

#include <sys/stat.h>
#include <sys/time.h>
#include <chrono>

template<class T>
T FromString(const std::string& s)
{
	std::istringstream stream (s);
	T t;
	stream >> t;
	return t;
}

template<class T>
string ToString(T arg)
{
	std::ostringstream s;

	s << arg;

	return s.str();

}

#endif /* INCLUDES_HPP_ */


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include "BezierScheme.cpp"

void PrintTo(BezierSequence const& bezseq, ::std::ostream *os)
{
    *os << std::endl;

    for(auto& bez : bezseq) {
        *os << bez.source().adjoint() << std::endl;
        *os << " -> " << bez.target().adjoint() << std::endl;
    }
}

TEST(BezierScheme, Raw)
{
    BezierScheme scheme;

    scheme.moveTo(Eigen::Vector3d(1.0, 0.5, -1.0));
    scheme.lineTo(Eigen::Vector3d(-1.0, 0.5, 1.0));

    scheme.moveTo(Eigen::Vector3d(-0.5, 1.0, -1.0));
    scheme.lineTo(Eigen::Vector3d(-0.5, -1.0, 1.0));

    scheme.moveTo(Eigen::Vector3d(-1.0, -0.5, -1.0));
    scheme.qbezierTo(Eigen::Vector3d(0.5, -0.5, 0.0),
                     Eigen::Vector3d(0.5, 1.0, 1.0) );

    auto raw = scheme.getRaw();
    ASSERT_EQ(raw.size(), 3);
    EXPECT_EQ(raw[0].size(), 1);
    EXPECT_EQ(raw[0][0].source(), Eigen::Vector3d(1.0, 0.5, -1.0));
    EXPECT_EQ(raw[0][0].target(), Eigen::Vector3d(-1.0, 0.5, 1.0));
    EXPECT_EQ(raw[1].size(), 1);
    EXPECT_EQ(raw[1][0].source(), Eigen::Vector3d(-0.5, 1.0, -1.0));
    EXPECT_EQ(raw[1][0].target(), Eigen::Vector3d(-0.5, -1.0, 1.0));
    EXPECT_EQ(raw[2].size(), 1);
    EXPECT_EQ(raw[2][0].source(), Eigen::Vector3d(-1.0, -0.5, -1.0));
    EXPECT_EQ(raw[2][0].target(), Eigen::Vector3d(0.5, 1.0, 1.0));
}

TEST(BezierScheme, Cutout)
{
    using BezVarType = BezierVariant<Eigen::Vector3d, 0, 1, 2, 3>;

    BezVarType bez0(
        std::integral_constant<size_t,1>(),
        Eigen::Vector3d(0.0, 0.0, 0.0),
        Eigen::Vector3d(1.0, 0.0, 0.0) );
    BezVarType bez1(
        std::integral_constant<size_t,2>(),
        Eigen::Vector3d(1.0, 0.0, 0.0),
        Eigen::Vector3d(1.0, 1.0, 1.0),
        Eigen::Vector3d(0.0, 1.0, 0.0) );
    BezVarType bez2(
        std::integral_constant<size_t,3>(),
        Eigen::Vector3d(0.0, 1.0, 0.0),
        Eigen::Vector3d(-1.0, 1.0, 1.0),
        Eigen::Vector3d(-1.0, -1.0, 1.0),
        Eigen::Vector3d(0.0, -1.0, 0.0) );

    BezierSequence bezseq = {bez0, bez1, bez2};

    std::set<BezierCutter> cutters;

    /*** First test ***/
    std::vector<BezierSequence> result;

    cutters.insert(BezierCutter{1, 0.25});
    cutters.insert(BezierCutter{1, 0.75});
    cutoutBezSeq(bezseq, cutters, result);

    ASSERT_EQ(result.size(), 3);
    ASSERT_EQ(result[0].size(), 2);
    EXPECT_EQ(result[0][0].source(), bez0.source());
    EXPECT_EQ(result[0][0].target(), bez0.target());
    EXPECT_EQ(result[0][1].source(), bez1.source());
    EXPECT_EQ(result[0][1].target(), bez1.eval(0.25));
    ASSERT_EQ(result[1].size(), 1);
    EXPECT_EQ(result[1][0].source(), bez1.eval(0.25));
    EXPECT_LT((result[1][0].target() - bez1.eval(0.75)).norm(), 1e-12)
        << result[1][0].target().adjoint()
        << std::endl
        << bez1.eval(0.75).adjoint()
        << std::endl;
    ASSERT_EQ(result[2].size(), 2);
    EXPECT_LT((result[2][0].source() - bez1.eval(0.75)).norm(), 1e-12)
        << result[2][0].source().adjoint()
        << std::endl
        << bez1.eval(0.75).adjoint()
        << std::endl;
    EXPECT_EQ(result[2][0].target(), bez1.target());
    EXPECT_EQ(result[2][1].source(), bez2.source());
    EXPECT_EQ(result[2][1].target(), bez2.target());

    /*** Second test ***/
    result.clear();
    cutters.insert(BezierCutter{0, 0.5});
    cutoutBezSeq(bezseq, cutters, result);
    ASSERT_EQ(result.size(), 4);
    ASSERT_EQ(result[0].size(), 1);
    EXPECT_EQ(result[0][0].source(), bez0.source());
    EXPECT_EQ(result[0][0].target(), bez0.eval(0.5));
    ASSERT_GE(result[1].size(), 2);
    EXPECT_EQ(result[1][0].source(), bez0.eval(0.5));
    EXPECT_EQ(result[1][0].target(), bez0.target());
    EXPECT_EQ(result[1][1].source(), bez1.source());
    EXPECT_EQ(result[1][1].target(), bez1.eval(0.25));
    if(result[1].size() > 2) {
        ADD_FAILURE();
        for(auto& bez : result[1]) {
            std::cout << bez.source().adjoint()
                      << std::endl << " -> "
                      << bez.target().adjoint()
                      << std::endl;
        }
    }
    ASSERT_EQ(result[2].size(), 1);
    EXPECT_EQ(result[2][0].source(), bez1.eval(0.25));
    EXPECT_LT((result[2][0].target() - bez1.eval(0.75)).norm(), 1e-12)
        << result[2][0].target().adjoint()
        << std::endl
        << bez1.eval(0.75).adjoint()
        << std::endl;
    ASSERT_EQ(result[3].size(), 2);
    EXPECT_LT((result[3][0].source() - bez1.eval(0.75)).norm(), 1e-12)
        << result[2][0].source().adjoint()
        << std::endl
        << bez1.eval(0.75).adjoint()
        << std::endl;
    EXPECT_EQ(result[3][0].target(), bez1.target());
    EXPECT_EQ(result[3][1].source(), bez2.source());
    EXPECT_EQ(result[3][1].target(), bez2.target());

    /*** Third test ***/
    result.clear();
    cutters.insert(BezierCutter{2, 0.5});
    cutoutBezSeq(bezseq, cutters, result);
    ASSERT_EQ(result.size(), 5);
    ASSERT_EQ(result[0].size(), 1);
    EXPECT_EQ(result[0][0].source(), bez0.source());
    EXPECT_EQ(result[0][0].target(), bez0.eval(0.5));
    ASSERT_GE(result[1].size(), 2);
    EXPECT_EQ(result[1][0].source(), bez0.eval(0.5));
    EXPECT_EQ(result[1][0].target(), bez0.target());
    EXPECT_EQ(result[1][1].source(), bez1.source());
    EXPECT_EQ(result[1][1].target(), bez1.eval(0.25));
    if(result[1].size() > 2) {
        ADD_FAILURE();
        for(auto& bez : result[1]) {
            std::cout << bez.source().adjoint()
                      << std::endl << " -> "
                      << bez.target().adjoint()
                      << std::endl;
        }
    }
    ASSERT_EQ(result[2].size(), 1);
    EXPECT_EQ(result[2][0].source(), bez1.eval(0.25));
    EXPECT_LT((result[2][0].target() - bez1.eval(0.75)).norm(), 1e-12)
        << result[2][0].target().adjoint()
        << std::endl
        << bez1.eval(0.75).adjoint()
        << std::endl;
    ASSERT_EQ(result[3].size(), 2);
    EXPECT_LT((result[3][0].source() - bez1.eval(0.75)).norm(), 1e-12)
        << result[2][0].source().adjoint()
        << std::endl
        << bez1.eval(0.75).adjoint()
        << std::endl;
    EXPECT_EQ(result[3][0].target(), bez1.target());
    EXPECT_EQ(result[3][1].source(), bez2.source());
    EXPECT_EQ(result[3][1].target(), bez2.eval(0.5));
    ASSERT_EQ(result[4].size(), 1);
    EXPECT_EQ(result[4][0].source(), bez2.eval(0.5));
    EXPECT_EQ(result[4][0].target(), bez2.target());
}

TEST(BezierScheme, ProjectOneParOne)
{
    BezierScheme scheme;

    scheme.moveTo(Eigen::Vector3d(1.0, 0.5, -1.0));
    scheme.lineTo(Eigen::Vector3d(-1.0, 0.5, 1.0));

    scheme.moveTo(Eigen::Vector3d(-0.5, 1.0, -1.0));
    scheme.lineTo(Eigen::Vector3d(-0.5, -1.0, 1.0));

    scheme.moveTo(Eigen::Vector3d(-1.0, -0.5, -1.0));
    scheme.qbezierTo(Eigen::Vector3d(0.5, -0.5, 0.0),
                     Eigen::Vector3d(0.5, 1.0, 1.0) );

    Eigen::Matrix<double,2,3> prmat{};
    prmat << Eigen::Matrix2d::Identity(), Eigen::Vector2d(0, 0);
    auto result = scheme.getProject(prmat).get();

    if(result.size() != 6) {
        std::cout << "Size: " << result.size() << std::endl;
        for(auto& bezseq : result) {
            PrintTo(bezseq, &std::cout);
        }
        FAIL();
    }

    ASSERT_EQ(result[0].size(), 1);
    EXPECT_EQ(result[0][0].source(), Eigen::Vector3d(1.0, 0.5, -1.0))
        << result[0][0].source().adjoint();
    ASSERT_EQ(result[1].size(), 1);
    EXPECT_EQ(result[1][0].target(), Eigen::Vector3d(-1.0, 0.5, 1.0))
        << result[1][0].target().adjoint();
    ASSERT_EQ(result[2].size(), 1);
    EXPECT_EQ(result[2][0].source(), Eigen::Vector3d(-0.5, 1.0, -1.0))
        << result[2][0].target().adjoint();
    ASSERT_EQ(result[3].size(), 1);
    EXPECT_EQ(result[3][0].target(), Eigen::Vector3d(-0.5, -1.0, 1.0))
        << "[" << result[3][0].target().adjoint() << "]" << std::endl
        << " != " << "[" << Eigen::RowVector3d(-0.5, -1.0, 1.0) << "]"
        << std::endl;
    ASSERT_EQ(result[4].size(), 1);
    EXPECT_EQ(result[4][0].source(), Eigen::Vector3d(-1.0, -0.5, -1.0))
        << "[" << result[4][0].target().adjoint() << "]" << std::endl
        << " != " << "[" << Eigen::Vector3d(-1.0, -0.5, -1.0) << "]"
        << std::endl;
    ASSERT_EQ(result[5].size(), 1);
    EXPECT_EQ(result[5][0].target(), Eigen::Vector3d(0.5, 1.0, 1.0))
        << result[5][0].target().adjoint();
}

TEST(BezierScheme, Terminate)
{
    BezierScheme scheme{};

    // Draw the same Bezier curve twice.
    scheme.moveTo(
        Eigen::Vector3d(0.0, 0.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 0.0, 0.0),
        Eigen::Vector3d(1.0, 1.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 2.0, 0.0),
        Eigen::Vector3d(0.0, 2.0, 0.0));
    scheme.moveTo(
        Eigen::Vector3d(0.0, 0.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 0.0, 0.0),
        Eigen::Vector3d(1.0, 1.0, 0.0));

    Eigen::Matrix<double,2,3> prmat{};
    prmat << Eigen::Matrix2d::Identity(), Eigen::Vector2d(0, 0);

    // This computation takes very long time.
    auto fut_vec = scheme.getProject(prmat);

    std::cout << "Waiting 100 [ms]" << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    std::cout << "Terminating" << std::endl;
    scheme.terminate();
    std::cout << "Terminated" << std::endl;
    auto vec = fut_vec.get();

    EXPECT_TRUE(vec.empty());
}

TEST(BezierScheme, Redraw)
{
    Eigen::Matrix<double,2,3> prmat{};
    prmat << Eigen::Matrix2d::Identity(), Eigen::Vector2d(0, 0);

    BezierScheme scheme{};

    // Draw the same paths twice.
    // First draw:
    scheme.moveTo(
        Eigen::Vector3d(0.0, -1.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, -1.0, 0.0),
        Eigen::Vector3d(1.0, 0.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 1.0, 0.0),
        Eigen::Vector3d(0.0, 1.0, 0.0));
    scheme.moveTo(
        Eigen::Vector3d(-1.0, -1.0, 0.0));
    scheme.lineTo(
        Eigen::Vector3d(-1.0, 1.0, 0.0));

    // Second draw:
    scheme.moveTo(
        Eigen::Vector3d(0.0, -1.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, -1.0, 0.0),
        Eigen::Vector3d(1.0, 0.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 1.0, 0.0),
        Eigen::Vector3d(0.0, 1.0, 0.0));
    scheme.moveTo(
        Eigen::Vector3d(-1.0, -1.0, 0.0));
    scheme.lineTo(
        Eigen::Vector3d(-1.0, 1.0, 0.0));

    // This computation takes very long time.
    auto fut_vec = scheme.getProject(prmat);
    std::vector<BezierSequence> bezseq = scheme.moveRaw();

    std::cout << "Waiting 100 [ms]" << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    // Draw the same paths twice again.
    // First draw:
    scheme.moveTo(
        Eigen::Vector3d(0.0, -1.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, -1.0, 0.0),
        Eigen::Vector3d(1.0, 0.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 1.0, 0.0),
        Eigen::Vector3d(0.0, 1.0, 0.0));
    scheme.moveTo(
        Eigen::Vector3d(-1.0, -1.0, 0.0));
    scheme.lineTo(
        Eigen::Vector3d(-1.0, 1.0, 0.0));

    // Second draw:
    scheme.moveTo(
        Eigen::Vector3d(0.0, -1.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, -1.0, 0.0),
        Eigen::Vector3d(1.0, 0.0, 0.0));
    scheme.qbezierTo(
        Eigen::Vector3d(1.0, 1.0, 0.0),
        Eigen::Vector3d(0.0, 1.0, 0.0));
    scheme.moveTo(
        Eigen::Vector3d(-1.0, -1.0, 0.0));
    scheme.lineTo(
        Eigen::Vector3d(-1.0, 1.0, 0.0));

    auto fut_vec2 = scheme.getProject(prmat);
    std::vector<BezierSequence> bezseq2 = scheme.moveRaw();

    std::cout << "Waiting 100 [ms]" << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    std::cout << "Terminating" << std::endl;
    scheme.terminate();
    std::cout << "Terminated" << std::endl;
    auto vec = fut_vec.get();
    auto vec2 = fut_vec2.get();

    EXPECT_TRUE(vec.empty());
    EXPECT_TRUE(vec2.empty());
}


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#ifndef VISR_PML_BIQUAD_PARAMETER_HPP_INCLUDED
#define VISR_PML_BIQUAD_PARAMETER_HPP_INCLUDED

#include "matrix_parameter_config.hpp" // might be a forward declaration

#include <librbbl/biquad_coefficient.hpp>

#include <libvisr/typed_parameter_base.hpp>
#include <libvisr/parameter_type.hpp>

#include <boost/property_tree/ptree_fwd.hpp>

#include <algorithm>
#include <array>
#include <initializer_list>
#include <iosfwd>
#include <stdexcept>
#include <vector>

namespace visr
{
namespace pml
{

namespace // unnamed
{
/**
 * Type trait to assign a unique type to each concrete BiquadParameterMatrix type.
 */
template<typename ElementType> struct BiquadMatrixParameterType{};

template<> struct BiquadMatrixParameterType<float>
{
  static constexpr const ParameterType ptype() { return detail::compileTimeHashFNV1( "BiquadFloatMatrix" ); }
};
template<> struct BiquadMatrixParameterType<double>
{
  static constexpr const ParameterType ptype() { return detail::compileTimeHashFNV1( "BiquadDoubleMatrix" ); }
};
} // unnamed

template<typename CoeffType >
class BiquadParameterMatrix: public TypedParameterBase<BiquadParameterMatrix<CoeffType>, MatrixParameterConfig, BiquadMatrixParameterType<CoeffType>::ptype() > ,
  public rbbl::BiquadCoefficientMatrix<CoeffType>
{
public:
  /**
   * Inherit all BiquadCoefficientMatrix constructors
   */
  using rbbl::BiquadCoefficientMatrix<CoeffType>::BiquadCoefficientMatrix;

  explicit BiquadParameterMatrix( MatrixParameterConfig const & config );

  explicit BiquadParameterMatrix( ParameterConfigBase const & config );

  virtual ~BiquadParameterMatrix() override;
};

} // namespace pml
} // namespace visr

DEFINE_PARAMETER_TYPE( visr::pml::BiquadParameterMatrix<float>, visr::pml::BiquadParameterMatrix<float>::staticType(), visr::pml::MatrixParameterConfig )
DEFINE_PARAMETER_TYPE( visr::pml::BiquadParameterMatrix<double>, visr::pml::BiquadParameterMatrix<double>::staticType(), visr::pml::MatrixParameterConfig )

#endif // VISR_PML_BIQUAD_PARAMETER_HPP_INCLUDED


// Copyright (c) 2018 Steven Watanabe
//
// 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/units2/quantity.hpp>
#include <boost/units2/unit.hpp>
#include <boost/units2/def.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_UNITS2_DEF(length);
BOOST_UNITS2_DEF(meter, length);

using boost::units2::quantity;

BOOST_AUTO_TEST_CASE(test_quantity)
{
    quantity<meter> x{1.0};
}


// Copyright 2019 Hans Dembinski
//
// 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/histogram/axis/integer.hpp>
#include <boost/histogram/make_profile.hpp>

int main() {
  using namespace boost::histogram;

  struct accumulator {
    void operator()(double) {}
    void operator()(weight_type<double>, double) {}
  };

  auto h = make_histogram_with(dense_storage<accumulator>(), axis::integer<>(0, 5));

  // accumulator requires sample
  h(0, weight(1));

  auto values = {1, 2};
  h.fill(values, weight(1));
}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2015 Bryce Adelstein Lelbach aka wash
//
//  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 <mdspan>

#include <boost/core/lightweight_test.hpp>

using std::experimental::dyn;
using std::experimental::detail::index_into_dynamic_dims;

int main()
{
    BOOST_TEST_EQ((index_into_dynamic_dims(0               )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(0, 3            )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(0, 3,   5       )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(0, 3,   5,   9  )), 0);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, dyn, 5,   9  )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, dyn, 5,   9  )), 1);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, dyn, 5,   9  )), 1);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, 3,   dyn, 9  )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, 3,   dyn, 9  )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, 3,   dyn, 9  )), 1);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, 3,   5,   dyn)), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, 3,   5,   dyn)), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, 3,   5,   dyn)), 0);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, 3,   dyn, dyn)), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, 3,   dyn, dyn)), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, 3,   dyn, dyn)), 1);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, dyn, 5,   dyn)), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, dyn, 5,   dyn)), 1);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, dyn, 5,   dyn)), 1);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, dyn, dyn, 9  )), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, dyn, dyn, 9  )), 1);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, dyn, dyn, 9  )), 2);

    BOOST_TEST_EQ((index_into_dynamic_dims(0, dyn, dyn, dyn)), 0);
    BOOST_TEST_EQ((index_into_dynamic_dims(1, dyn, dyn, dyn)), 1);
    BOOST_TEST_EQ((index_into_dynamic_dims(2, dyn, dyn, dyn)), 2);

    return boost::report_errors();
}


#pragma once

#include "indices_2d.hxx"
#include "../math/circular_modulus.hxx"

#include <boost/circular_buffer.hpp>
#include <cstdint>
#include <functional>

namespace usagi
{
  namespace container
  {
    template
    < typename T >
    class circular_tile
      : boost::circular_buffer< boost::circular_buffer< T > >
    {
    public:
      using type = circular_tile<T>;

      using cols_type = boost::circular_buffer< T >;
      using rows_type = boost::circular_buffer< cols_type >;

      using operate_cols_functor_signature = auto ( cols_type& ) -> void;
      using operate_cols_functor_type = std::function< operate_cols_functor_signature >;

      using apply_functor_signature = auto ( T& ) -> void;
      using apply_functor_type = std::function< apply_functor_signature >;

      using on_arris_resize_functor_signature = auto ( const type& ref, std::size_t new_size ) -> void;
      using on_arris_resize_functor_type = std::function< on_arris_resize_functor_signature >;

    protected:

      inline static auto make_cols( const std::size_t size, const T& default_value = T() )
      { return cols_type( size, default_value ); }

      auto make_cols( const T& default_value = T() )
      { return make_cols( rows_type::front().size(), default_value ); }

      template < typename DATA_TYPE = std::initializer_list<T> >
      auto make_cols( const DATA_TYPE& data )
      {
        cols_type cols( rows_type::front().size() );
        for ( const auto& datum : data )
          cols.push_back( datum );
        return cols;
      }

      template < typename VALUE_TYPE, typename MESSAGE_TYPE >
      inline static auto throw_if_zero( const VALUE_TYPE value, const MESSAGE_TYPE& message )
      {
        if ( value == 0 )
          throw std::logic_error( message );
      }

    public:

      on_arris_resize_functor_type on_cols_resize_before;
      on_arris_resize_functor_type on_cols_resize_after;

      on_arris_resize_functor_type on_rows_resize_before;
      on_arris_resize_functor_type on_rows_resize_after;

      circular_tile( const std::size_t cols_size, const std::size_t rows_size, const T& default_value = T() )
        : rows_type( rows_size, make_cols( cols_size, default_value ) )
        , on_cols_resize_before( get_nothing_functor_on_arris_resize() )
        , on_cols_resize_after ( get_nothing_functor_on_arris_resize() )
        , on_rows_resize_before( get_nothing_functor_on_arris_resize() )
        , on_rows_resize_after ( get_nothing_functor_on_arris_resize() )
      {
        throw_if_zero( cols_size, "cols_size == 0" );
        throw_if_zero( rows_size, "rows_size == 0" );
      }

      circular_tile( const std::size_t size, const T& default_value = T() )
        : circular_tile( size, size, default_value )
      { }

      static inline auto get_nothing_functor_on_arris_resize()
      { return []( const type&, std::size_t ){ }; }

      auto cols_size() const { return this->front().size(); }
      auto rows_size() const { return rows_type::size(); }

      auto size() const { return cols_size() * rows_size(); }

      auto resize_rows( const std::size_t size, const T& default_value = T() )
      {
        on_rows_resize_before( *this, size );

        throw_if_zero( size, "the new rows size is 0; circular_tile cannot use 0 size rows." );
        rows_type::resize( size, make_cols( default_value ) );

        on_rows_resize_after( *this, size );
      }

      auto resize_cols( const std::size_t size, const T& default_value = T() )
      {
        on_cols_resize_before( *this, size );

        throw_if_zero( size, "the new cols size is 0; circular_tile cannot use 0 size cols." );
        for ( auto i = rows_type::begin(), e = rows_type::end(); i != e; ++i )
          i->resize( size, default_value );

        on_cols_resize_after( *this, size );
      }

      auto resize( const std::size_t cols_size, const std::size_t rows_size, const T& default_value = T() )
      {
        resize_rows( rows_size, default_value );
        resize_cols( cols_size, default_value );
      }

      auto resize ( const std::size_t size, const T& default_value = T() )
      { resize( size, size, default_value ); }

      auto begin() const -> decltype( rows_type::begin() ) { return rows_type::begin(); }
      auto end()   const -> decltype( rows_type::end() ){ return rows_type::end();   }

      auto get_center_index_x()
      { return cols_size() >> 1; }

      auto get_center_index_y()
      { return rows_size() >> 1; }

      auto get_center_indices()
      { return indices_2d{ get_center_index_x(), get_center_index_y() }; }

      template < typename INDEX_TYPE >
      decltype(auto) operator()( const INDEX_TYPE col, const INDEX_TYPE row )
      { return (*this)[ math::circular_modulus( row, rows_size() ) ][ math::circular_modulus( col, cols_size() ) ]; }

      template < typename INDICES_TYPE >
      decltype(auto) operator()( const INDICES_TYPE& indices )
      { return operator()( indices.x, indices.y ); }

      template < typename INDEX_TYPE >
      decltype(auto) from_center( const INDEX_TYPE delta_col, const INDEX_TYPE delta_row )
      { return operator()( get_center_index_x() + delta_col, get_center_index_y() + delta_row ); }

      template < typename INDICES_TYPE >
      decltype(auto) from_center( const INDICES_TYPE& delta_indices )
      { return operator()( get_center_index_x() + delta_indices.x, get_center_index_y() + delta_indices.y() ); }

      auto operate_cols( const operate_cols_functor_type& f )
      {
        for ( auto i = rows_type::begin(), e = rows_type::end(); i != e; ++i )
          f( *i );
      }

      auto apply( const apply_functor_type& f )
      {
        for ( auto ir = rows_type::begin(), er = rows_type::end(); ir != er; ++ir )
          for ( auto ic = ir->begin(), ec = ir->end(); ic != ec; ++ic )
            f( *ic );
      }

      auto shift_to_right( const T& default_value = T() )
      { operate_cols( [&]( auto& c ){ c.push_back( default_value ); } ); }

      auto shift_to_left( const T& default_value = T() )
      { operate_cols( [&]( auto& c ){ c.push_front( default_value ); } ); }

      auto shift_to_top( const T& default_value = T() )
      { rows_type::push_front( make_cols( default_value ) ); }

      auto shift_to_bottom( const T& default_value = T() )
      { rows_type::push_back( make_cols( default_value ) ); }

      auto throw_if_ne_rows_size( const std::size_t size )
      {
        if ( size != rows_size() )
          throw std::runtime_error( "size != rows_size()" );
      }

      auto throw_if_ne_cols_size( const std::size_t size )
      {
        if ( size != cols_size() )
          throw std::runtime_error( "size != cols_size()" );
      }

      template < typename DATA_TYPE = std::initializer_list<T> >
      auto shift_to_right( const DATA_TYPE& data )
      {
        throw_if_ne_rows_size( data.size() );

        auto i = data.begin();
        operate_cols( [&]( auto& c ){ c.push_back( *i++ ); } );
      }

      template < typename DATA_TYPE = std::initializer_list<T> >
      auto shift_to_left( const DATA_TYPE& data )
      {
        throw_if_ne_rows_size( data.size() );

        auto i = data.begin();
        operate_cols( [&]( auto& c ){ c.push_front( *i++ ); } );
      }

      template < typename DATA_TYPE = std::initializer_list<T> >
      auto shift_to_top( const DATA_TYPE& data )
      {
        throw_if_ne_cols_size( data.size() );

        rows_type::push_front( make_cols( data ) );
      }

      template < typename DATA_TYPE = std::initializer_list<T> >
      auto shift_to_bottom( const DATA_TYPE& data )
      {
        throw_if_ne_cols_size( data.size() );

        rows_type::push_back( make_cols( data ) );
      }
    };

    template < typename T >
    static decltype(auto) operator<<( std::ostream& o , const circular_tile<T>& tiles )
    {
      for ( const auto& cols : tiles )
      {
        for ( const auto& value : cols )
          o << value << " ";
        o << "\n";
      }

      return o;
    }
  }
}


#ifndef CORE_H
#define CORE_H

#include <Eigen/Core>
#include <memory>
#include <vector>

class MainWindow;
namespace sequential_line_search
{
    class SequentialLineSearchOptimizer;
}

class Core
{
public:
    Core() {}

    static Core& getInstance()
    {
        static Core core;
        return core;
    }

    std::shared_ptr<sequential_line_search::SequentialLineSearchOptimizer> optimizer;

    MainWindow* mainWindow;

    // x in [0, 1]^2
    double evaluateObjectiveFunction(const Eigen::VectorXd& x) const;
};

#endif // CORE_H


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/hi.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/bitwise_cast.hpp>

STF_CASE_TPL (" return_type",  STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::hi;

  STF_EXPR_IS( hi(T()), (bd::as_integer_t<T,unsigned>));
}

STF_CASE_TPL (" real_hi",  (float))
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::hi;

  using iT = bd::as_integer_t<T,unsigned>;

  T  val = bs::splat<T>(bs::bitwise_cast<T>(0x1234FFFF));
  T  res = bs::splat<T>(bs::bitwise_cast<T>(0x00001234));
  iT ref = bs::bitwise_cast<iT>(res);

  STF_EQUAL( hi(val), ref );
}

STF_CASE_TPL (" real_hi",  (double))
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::hi;

  using iT = bd::as_integer_t<T,unsigned>;

  T  val = bs::splat<T>(bs::bitwise_cast<T>(0x12345678FFFFFFFFLL));
  T  res = bs::splat<T>(bs::bitwise_cast<T>(0x0000000012345678LL));
  iT ref = bs::bitwise_cast<iT>(res);

  STF_EQUAL( hi(val), ref );
}

STF_CASE_TPL(" integer_hi", STF_INTEGRAL_TYPES )
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::hi;
  using bs::splat;

  using iT = bd::as_integer_t<T,unsigned>;

  T  val = splat<T>(~0);
  iT ref = splat<T>( (T(1)<< (sizeof(T)*(CHAR_BIT/2))) - 1);

  STF_EQUAL( hi(val), ref );
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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)

#ifndef BOOST_GEOMETRY_CORE_CLOSURE_HPP
#define BOOST_GEOMETRY_CORE_CLOSURE_HPP

#include <boost/mpl/assert.hpp>
#include <boost/mpl/int.hpp>
#include <boost/range.hpp>
#include <boost/type_traits/remove_const.hpp>

#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

namespace boost { namespace geometry
{

/*!
\brief Enumerates options for defining if polygons are open or closed
\ingroup enum
\details The enumeration closure_selector describes options for if a polygon is
    open or closed. In a closed polygon the very first point (per ring) should
    be equal to the very last point.
    The specific closing property of a polygon type is defined by the closure
    metafunction. The closure metafunction defines a value, which is one of the
    values enumerated in the closure_selector

\qbk{
[heading See also]
[link geometry.reference.core.closure The closure metafunction]
}
*/
enum closure_selector
{
    /// Rings are open: first point and last point are different, algorithms
    /// close them explicitly on the fly
    open = 0,
    /// Rings are closed: first point and last point must be the same
    closed = 1,
    /// (Not yet implemented): algorithms first figure out if ring must be
    /// closed on the fly
    closure_undertermined = -1
};

namespace traits
{

/*!
    \brief Traits class indicating if points within a
        ring or (multi)polygon are closed (last point == first point),
        open or not known.
    \ingroup traits
    \par Geometries:
        - ring
    \tparam G geometry
*/
template <typename G>
struct closure
{
    static const closure_selector value = closed;
};

} // namespace traits

#ifndef DOXYGEN_NO_DETAIL
namespace core_detail { namespace closure
{

struct closed
{
    static const closure_selector value = geometry::closed;
};

/// Metafunction to define the minimum size of a ring:
/// 3 for open rings, 4 for closed rings
template <closure_selector Closure>
struct minimum_ring_size {};

template <>
struct minimum_ring_size<geometry::closed> : boost::mpl::int_<4> {};

template <>
struct minimum_ring_size<geometry::open> : boost::mpl::int_<3> {};

}} // namespace detail::point_order
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch
{

template <typename Tag, typename Geometry>
struct closure
{
    BOOST_MPL_ASSERT_MSG
        (
            false, NOT_IMPLEMENTED_FOR_THIS_GEOMETRY_TYPE
            , (types<Geometry>)
        );
};

template <typename Box>
struct closure<point_tag, Box> : public core_detail::closure::closed {};

template <typename Box>
struct closure<box_tag, Box> : public core_detail::closure::closed {};

template <typename Box>
struct closure<segment_tag, Box> : public core_detail::closure::closed {};

template <typename LineString>
struct closure<linestring_tag, LineString>
    : public core_detail::closure::closed {};

template <typename Ring>
struct closure<ring_tag, Ring>
{
    static const closure_selector value
        = geometry::traits::closure<Ring>::value;
};

// Specialization for polygon: the closure is the closure of its rings
template <typename Polygon>
struct closure<polygon_tag, Polygon>
{
    static const closure_selector value = core_dispatch::closure
        <
            ring_tag,
            typename ring_type<polygon_tag, Polygon>::type
        >::value ;
};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_meta{value, closure (clockwise\, counterclockwise),
    \meta_geometry_type}
\tparam Geometry \tparam_geometry
\ingroup core

\qbk{[include reference/core/closure.qbk]}
*/
template <typename Geometry>
struct closure
{
    static const closure_selector value = core_dispatch::closure
        <
            typename tag<Geometry>::type,
            typename boost::remove_const<Geometry>::type
        >::value;
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_CORE_CLOSURE_HPP


#include "Stitcher.h"

#include <iostream>
#include <sstream>
#include <limits>

#include <Eigen/LU>
#include <Eigen/Geometry>

#include <open3d/pipelines/registration/GlobalOptimization.h>
#include <open3d/pipelines/color_map/ColorMapOptimization.h>

Stitcher::Stitcher()
{
}

Stitcher::~Stitcher()
{

}

void Stitcher::setup()
{
	volume_ = std::make_unique<open3d::pipelines::integration::ScalableTSDFVolume>(4.0 / 512, 0.04, open3d::pipelines::integration::TSDFVolumeColorType::RGB8);
}

bool printRotMatrix(const Eigen::Matrix4d &mat)
{
	double theta = -std::asin(mat(2, 0));
	double psi = std::atan2(mat(2, 1)/std::cos(theta), mat(2, 2)/std::cos(theta));
	double phi = std::atan2(mat(1, 0)/std::cos(theta), mat(0, 0)/std::cos(theta));

	std::cout << theta << ", " << psi << ", " << phi << std::endl;

	return (std::abs(theta) > 0.1 || std::abs(psi) > 0.1 || std::abs(phi) > 0.1);
}

void printRotMatrixQ(const Eigen::Matrix4d& mat)
{
	Eigen::Matrix3d rot = mat.block<3,3>(0, 0);
	Eigen::Quaterniond q(rot);
	std::cout << mat(0, 3) << ", " << mat(1, 3) << ", " << mat(2, 3) << ", " << q.x() << ", " << q.y() << ", " << q.z() << ", " << std::endl;
}

void Stitcher::addNewImage(const open3d::geometry::Image& colorImg, const open3d::geometry::Image& depthImg)
{
	std::unique_lock<std::mutex> lock(mutex_);

	open3d::camera::PinholeCameraIntrinsic intrinsic = open3d::camera::PinholeCameraIntrinsic(
		open3d::camera::PinholeCameraIntrinsicParameters::PrimeSenseDefault);
	//intrinsic.SetIntrinsics(640, 480, 524.0, 524.0, 316.7, 238.5);	// from https://www.researchgate.net/figure/ntrinsic-parameters-of-Kinect-RGB-camera_tbl2_305108995
	//intrinsic.SetIntrinsics(640, 480, 517.3, 516.5, 318.6, 255.3);		// from Freiburg test data set
	//intrinsic.SetIntrinsics(640, 480, 537.408, 537.40877, 321.897, 236.29);		// from Calibration of my own camera
	//intrinsic.SetIntrinsics(640, 480, 533.82, 533.82, 320.55, 232.35);		// from Calibration of my own camera
	intrinsic.SetIntrinsics(640, 480, 542.7693, 544.396, 318.79, 239.99);		// from Calibration of my own camera

	//open3d::io::WriteImageToPNG("color.png", colorImg_);
	//open3d::io::WriteImageToPNG("depth.png", depthImg_);

	Eigen::Matrix4d odo_init = Eigen::Matrix4d::Identity();
	auto depthFlt = depthImg.ConvertDepthToFloatImage(depthScale_, std::numeric_limits<float>::max());

	/*{
		float *ptr = depthFlt->PointerAt<float>(0, 0);
		int size = depthImg.width_ * depthImg.height_;
		float maxVal{ std::numeric_limits<float>::lowest() }, minVal{ std::numeric_limits<float>::max() };
		for (int i = 0; i < size; i++)
		{
			const float& val = *ptr;
			maxVal = std::max(maxVal, val);
			minVal = std::min(minVal, val);
			ptr++;
		}

		std::cout << "Min: " << minVal << ", max: " << maxVal << std::endl;
	}*/

	open3d::geometry::RGBDImage source(colorImg, *depthFlt);

	images_.push_back(source);

	if (!oldRGBDImage_.IsEmpty())
	{
		std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> rgbd_odo =
			open3d::pipelines::odometry::ComputeRGBDOdometry(
				oldRGBDImage_, source, intrinsic, odo_init,
				open3d::pipelines::odometry::RGBDOdometryJacobianFromHybridTerm(),
				open3d::pipelines::odometry::OdometryOption({ 20,10,5 }, 0.2));

		std::cout << "Matching ";
		if (std::get<0>(rgbd_odo))
			std::cout << "successful";
		else
			std::cout << "unsuccessful";
		std::cout << std::endl;
		//std::cout << " " << std::get<1>(rgbd_odo) << std::endl;

		//bool doIntegrate = printRotMatrix(std::get<1>(rgbd_odo)) && (transvec_.size() < 2);
		printRotMatrixQ(std::get<1>(rgbd_odo));

		if (std::get<0>(rgbd_odo))
		{
			Eigen::Matrix4d extrinsic = std::get<1>(rgbd_odo);

			//Eigen::Matrix4d extrinsicInv = extrinsic.inverse();
			transvec_.push_back(extrinsic);
			infovec_.push_back(std::get<2>(rgbd_odo));

			pos_ = extrinsic * pos_;
			Eigen::Matrix4d posInv = pos_.inverse();

			volume_->Integrate(source, intrinsic, pos_);
		}

	}
	else
	{
		Eigen::Matrix4d identity = Eigen::Matrix4d::Identity();
		pos_ = identity;
		volume_->Integrate(source, intrinsic, identity);
		infovec_.push_back(Eigen::Matrix6d::Identity());
		transvec_.push_back(identity);
	}

	posvec_.push_back(pos_);

	oldRGBDImage_ = source;

	/*{
		std::ostringstream ostr;
		ostr << "mesh_online_" << transvec_.size() << ".ply";
		auto mesh = volume_->ExtractTriangleMesh();
		open3d::io::WriteTriangleMesh(ostr.str(),
			*mesh);
	}*/
}

void Stitcher::saveVolume()
{
	auto mesh = volume_->ExtractTriangleMesh();
	open3d::io::WriteTriangleMesh("mesh_online.ply",
		*mesh);
	/*{
		std::ostringstream ostr;
		ostr << "mesh_online_" << variant_ << ".ply";
		auto mesh = volume_->ExtractTriangleMesh();
		open3d::io::WriteTriangleMesh(ostr.str(),
			*mesh);
	}*/

	std::cout << "Online mesh saved" << std::endl;

	const size_t keyFrameInterval = 3;
	const size_t maxKeyFrameDistance = 9;
	open3d::camera::PinholeCameraIntrinsic intrinsic = open3d::camera::PinholeCameraIntrinsic(
		open3d::camera::PinholeCameraIntrinsicParameters::PrimeSenseDefault);

	open3d::pipelines::registration::PoseGraph poseGraph;
	Eigen::Matrix4d transOdometry = Eigen::Matrix4d::Identity(), transOdometryInv;
	poseGraph.nodes_.push_back(open3d::pipelines::registration::PoseGraphNode(transOdometry));

	for (size_t i = 0; i < images_.size() - 1; i++)
	{
		for (size_t j = i + 1; j < images_.size(); j++)
		{
			bool isNeighbour = (j == i + 1);
			bool doLoopClosure = (i % keyFrameInterval == 0 && j % keyFrameInterval == 0 && (j-i) <= maxKeyFrameDistance);

			const open3d::geometry::RGBDImage& source = images_[i];
			const open3d::geometry::RGBDImage& target = images_[j];

			if (isNeighbour)
			{
				//Eigen::Matrix4d odo_init = Eigen::Matrix4d::Identity();
				//std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> rgbd_odo =
				//	open3d::pipelines::odometry::ComputeRGBDOdometry(
				//		source, target, intrinsic, odo_init,
				//		open3d::pipelines::odometry::RGBDOdometryJacobianFromHybridTerm(),
				//		open3d::pipelines::odometry::OdometryOption());
				//
				//transOdometry = std::get<1>(rgbd_odo) * transOdometry;
				transOdometry = posvec_[j];
				transOdometryInv = transOdometry.inverse();
				poseGraph.nodes_.push_back(open3d::pipelines::registration::PoseGraphNode(transOdometryInv));
				poseGraph.edges_.push_back(open3d::pipelines::registration::PoseGraphEdge(i, j,
//					std::get<1>(rgbd_odo), std::get<2>(rgbd_odo), false));
					transvec_[j], infovec_[j], false));
			}
			else if (doLoopClosure)
			{
				Eigen::Matrix4d odo_init = Eigen::Matrix4d::Identity();
				std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> rgbd_odo =
					open3d::pipelines::odometry::ComputeRGBDOdometry(
						source, target, intrinsic, odo_init,
						open3d::pipelines::odometry::RGBDOdometryJacobianFromHybridTerm(),
						open3d::pipelines::odometry::OdometryOption({ 20,10,5 }, 0.1));
				if (std::get<0>(rgbd_odo))	// if success==true
				{
					poseGraph.edges_.push_back(open3d::pipelines::registration::PoseGraphEdge(i, j,
						std::get<1>(rgbd_odo), std::get<2>(rgbd_odo), true));
				}
			}
		}
	}

	open3d::utility::SetVerbosityLevel(open3d::utility::VerbosityLevel::Debug);

	// Global optimization
	open3d::pipelines::registration::GlobalOptimization(poseGraph);

	open3d::utility::SetVerbosityLevel(open3d::utility::VerbosityLevel::Error);

	// Integrate
	open3d::pipelines::integration::ScalableTSDFVolume optVolume(2.0 / 512, 0.04, open3d::pipelines::integration::TSDFVolumeColorType::RGB8);

	for (size_t i = 0; i < poseGraph.nodes_.size(); i++)
	{
		auto pose = poseGraph.nodes_[i].pose_;
		Eigen::Matrix4d poseInv = pose.inverse();
		optVolume.Integrate(images_[i], intrinsic, poseInv);
	}

	// Simplify
	auto optMesh = optVolume.ExtractTriangleMesh();
	auto simplMesh = optMesh->SimplifyQuadricDecimation(static_cast<int>(optMesh->triangles_.size() / 2), std::numeric_limits<double>::infinity(), 1.0);
	open3d::io::WriteTriangleMesh("mesh_opt.ply",
		*simplMesh);

	std::cout << "Optimized mesh saved" << std::endl;

	// Subdivide the mesh to allow for finer color resolution
	auto subdivMesh = simplMesh->SubdivideLoop(1);

	// Optimize color map
	open3d::pipelines::color_map::ColorMapOptimizationOption option(true);
	open3d::camera::PinholeCameraTrajectory camera;
	for (size_t i = 0; i < poseGraph.nodes_.size(); i++)
	{
		auto pose = poseGraph.nodes_[i].pose_;
		Eigen::Matrix4d poseInv = pose.inverse();
		open3d::camera::PinholeCameraParameters cameraParams;
		cameraParams.intrinsic_ = intrinsic;
		cameraParams.extrinsic_ = poseInv;
		camera.parameters_.push_back(cameraParams);
	}
	open3d::geometry::RGBDImagePyramid rgbdImages;
	for (const auto& img : images_)
	{
		rgbdImages.push_back(std::make_shared<open3d::geometry::RGBDImage>(img));
	}
	open3d::pipelines::color_map::ColorMapOptimization(*subdivMesh, rgbdImages, camera, option);

	open3d::io::WriteTriangleMesh("mesh_color_opt.ply",
		*subdivMesh);

}

void Stitcher::reset()
{
	std::unique_lock<std::mutex> lock(mutex_);

	/*{
		std::ostringstream ostr;
		ostr << "mesh_online_" << variant_ << ".ply";
		auto mesh = volume_->ExtractTriangleMesh();
		open3d::io::WriteTriangleMesh(ostr.str(),
			*mesh);
	}*/

	std::cout << "Reset" << std::endl;

	oldRGBDImage_.Clear();
	pos_ = Eigen::Matrix4d::Identity();

	setup();
	//volume_->Reset();

	images_.clear();
	posvec_.clear();
	transvec_.clear();
	infovec_.clear();
}


#include <utility/Configuration_Chain.hpp>
#include <utility/Configurations.hpp>
#include <data/Spin_System.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>

#include <Eigen/Dense>

#include <random>
#include <iostream>
#include <string>
#include <vector>

namespace Utility
{
	namespace Configuration_Chain
	{
		void Add_Noise_Temperature(std::shared_ptr<Data::Spin_System_Chain> c, int idx_1, int idx_2, scalar temperature)
		{
			for (int img = idx_1 + 1; img <= idx_2 - 1; ++img)
			{
				Configurations::Add_Noise_Temperature(*c->images[img], temperature, img);
			}
		}

		void Homogeneous_Rotation(std::shared_ptr<Data::Spin_System_Chain> c, int idx_1, int idx_2)
		{
			int nos = c->images[0]->nos;
			int noi = idx_2 - idx_1 + 1;

			scalar angle, rot_angle;
			Vector3 axis, rot_axis, a, b, temp;

			for (int i = 0; i < nos; ++i)
			{
				a = (*c->images[idx_1]->spins)[i];
				b = (*c->images[idx_2]->spins)[i];

				rot_angle = Engine::Manifoldmath::dist_greatcircle(a, b);
				rot_axis = a.cross(b);

				// If they are not strictly parallel we can rotate
				if (rot_axis.norm() > 1e-8)
				{
					rot_axis.normalize();

					for (int img = idx_1+1; img < idx_2; ++img)
					{
						angle = (img-idx_1)*rot_angle/noi;
						Engine::Vectormath::rotate(a, rot_axis, angle, temp);

						(*c->images[img]->spins)[i] = temp;
					}
				}
				// Otherwise we simply leave the spin untouched
				else
				{
					for (int img = idx_1+1; img < idx_2; ++img)
					{
						(*c->images[img]->spins)[i] = a;
					}
				}
			}

		}

		void Homogeneous_Rotation(std::shared_ptr<Data::Spin_System_Chain> c, vectorfield A, vectorfield B)
		{
			(*c->images[0]->spins) = A;
			(*c->images[c->noi - 1]->spins) = B;

			int nos = c->images[0]->nos;

			scalar angle, rot_angle;
			Vector3 axis, rot_axis, a, b, temp;

			for (int i = 0; i < c->images[0]->nos; ++i)
			{
				a = A[i];
				b = B[i];

				rot_angle = Engine::Manifoldmath::dist_greatcircle(a, b);
				rot_axis = a.cross(b);

				// If they are not strictly parallel we can rotate
				if (rot_axis.norm() > 1e-8)
				{
					rot_axis.normalize();

					for (int img = 1; img < c->noi - 1; ++img)
					{
						angle = (img)*rot_angle / (c->noi - 1);
						Engine::Vectormath::rotate(a, rot_axis, angle, temp);

						(*c->images[img]->spins)[i] = temp;
					}
				}
				// Otherwise we simply leave the spin untouched
				else
				{
					for (int img = 1; img < c->noi - 1; ++img)
					{
						(*c->images[img]->spins)[i] = a;
					}
				}
			}

			/*
				do i=1,NOS
				  Start(i,:)  = Start(i,:)/length(Start(i,:))
				  Finish(i,:) = Finish(i,:)/length(Finish(i,:))

				  IMAGES(idx_start,i,:)  = Start(i,:)
				  IMAGES(idx_finish,i,:) = Finish(i,:)

				  r = max(-1.0,min(1.0, dot_product(Start(i,:), Finish(i,:)) )) !! this prevents NaNs from ocurring
				  rot_angle(i)  = acos(r)
				  rot_axis(i,:) = cross_product(Start(i,:), Finish(i,:))
				  if (abs(length(rot_axis(i,:))) > 1e-18) rot_axis(i,:) = rot_axis(i,:)/length(rot_axis(i,:))
				enddo
			*/

			/*
				do i=idx_start+1,idx_finish-1
				  !! loop over spins in image
				  do j=1,NOS
					 angle = (i-1)*rot_angle(j)/(idx_finish-idx_start)
					 IMAGES(i,j,:) = rotate_spin(Start(j,:),rot_axis(j,:),angle)

				  enddo
				enddo
			*/
		}
	}//end namespace Configuration_Chain
}//end namespace Utility


#include "ayla/serialization/glm_serializer.hpp"

#include "ayla/serialization/boost/explicit_instantiation_macros.hpp"
#include "ayla/config.hpp"

#include <boost/serialization/nvp.hpp>

namespace boost {
namespace serialization {

template<class Archive>
void serialize(Archive &ar, glm::vec2& vector, const unsigned int version) {
	ar & make_nvp("x", vector.x);
	ar & make_nvp("y", vector.y);
}

template<class Archive>
void serialize(Archive &ar, glm::vec3& vector, const unsigned int version) {
	ar & make_nvp("x", vector.x);
	ar & make_nvp("y", vector.y);
	ar & make_nvp("z", vector.z);
}

template<class Archive>
void serialize(Archive &ar, glm::vec4& vector, const unsigned int version) {
	ar & make_nvp("x", vector.x);
	ar & make_nvp("y", vector.y);
	ar & make_nvp("z", vector.z);
	ar & make_nvp("w", vector.w);
}

template <class Archive>
void serialize(Archive &ar, glm::mat4& matrix, const unsigned int version) {
	ar & make_nvp("m00", matrix[0][0]);
	ar & make_nvp("m01", matrix[0][1]);
	ar & make_nvp("m02", matrix[0][2]);
	ar & make_nvp("m03", matrix[0][3]);
	ar & make_nvp("m10", matrix[1][0]);
	ar & make_nvp("m11", matrix[1][1]);
	ar & make_nvp("m12", matrix[1][2]);
	ar & make_nvp("m13", matrix[1][3]);
	ar & make_nvp("m20", matrix[2][0]);
	ar & make_nvp("m21", matrix[2][1]);
	ar & make_nvp("m22", matrix[2][2]);
	ar & make_nvp("m23", matrix[2][3]);
	ar & make_nvp("m30", matrix[3][0]);
	ar & make_nvp("m31", matrix[3][1]);
	ar & make_nvp("m32", matrix[3][2]);
	ar & make_nvp("m33", matrix[3][3]);
}

EXPLICIT_INSTANTIATION_SERIALIZE_FUNC_P(glm::vec2, AYLA_API);
EXPLICIT_INSTANTIATION_SERIALIZE_FUNC_P(glm::vec3, AYLA_API);
EXPLICIT_INSTANTIATION_SERIALIZE_FUNC_P(glm::vec4, AYLA_API);
EXPLICIT_INSTANTIATION_SERIALIZE_FUNC_P(glm::mat4, AYLA_API);

}
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 by Rokko Developers https://github.com/t-sakashita/rokko
*
* 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/python.hpp>

#include "rokko_dense.h"
#include "rokko_sparse.h"
#include "utility/frank_matrix_c.h"

enum rokko {
  grid_col_major = rokko_grid_col_major, grid_row_major = rokko_grid_row_major,
  matrix_col_major = rokko_matrix_col_major, matrix_row_major = rokko_matrix_row_major
};

class wrap_rokko_localized_matrix {
	rokko_localized_matrix* raw;
public:
	wrap_rokko_localized_matrix(int dim1, int dim2, int matrix_major) {
		raw = new rokko_localized_matrix();
		rokko_localized_matrix_construct(raw, dim1, dim2, matrix_major);
	}
	rokko_localized_matrix* get_raw(void) {
		return raw;
	}

	void print(void) {
		rokko_localized_matrix_print(*raw);
	}

	~wrap_rokko_localized_matrix(void) {
		rokko_localized_matrix_destruct(raw);
		delete raw;
	}
};

class wrap_rokko_localized_vector {
	rokko_localized_vector* raw;
public:
	wrap_rokko_localized_vector(int dim1) {
		raw = new rokko_localized_vector();
		rokko_localized_vector_construct(raw, dim1);
	}
	rokko_localized_vector* get_raw(void) {
		return raw;
	}

	double get(int i) {
		return rokko_localized_vector_get(*raw, i);
	}

	~wrap_rokko_localized_vector(void) {
		rokko_localized_vector_destruct(raw);
		delete raw;
	}
};

class wrap_rokko_serial_dense_solver {
	rokko_serial_dense_solver* raw;
public:
	wrap_rokko_serial_dense_solver(char* solver_name, int argc, char** argv) {
		raw = new rokko_serial_dense_solver();
		rokko_serial_dense_solver_construct(raw, solver_name, argc, argv);
	}
	rokko_serial_dense_solver* get_raw(void) {
		return raw;
	}

	void diagonalize_localized_matrix(wrap_rokko_localized_matrix* mat, wrap_rokko_localized_vector* eigvals, wrap_rokko_localized_matrix* eigvecs) {
		rokko_serial_dense_solver_diagonalize_localized_matrix(raw, mat->get_raw(), eigvals->get_raw(), eigvecs->get_raw());
	}

	~wrap_rokko_serial_dense_solver(void) {
		rokko_serial_dense_solver_destruct(raw);
		delete raw;
	}
};

static void wrap_rokko_frank_matrix_generate_localized_matrix(wrap_rokko_localized_matrix* mat)
{
	rokko_frank_matrix_generate_localized_matrix(mat->get_raw());
}

#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER) || defined(ROKKO_HAVE_PARALLEL_SPARSE_SOLVER)

class wrap_rokko_grid {
	rokko_grid* raw;
public:
	wrap_rokko_grid(boost::python::object comm, int grid_major) {
		boost::python::object comm_func = comm.attr("py2f");
		boost::python::object comm_f_obj = comm_func();
		int comm_f = boost::python::extract<int>(comm_f_obj);
		raw = new rokko_grid();
		rokko_grid_construct_f(raw, comm_f, grid_major);
	}
	rokko_grid* get_raw(void) {
		return raw;
	}

	int get_myrank(void) {
		return rokko_grid_get_myrank(*raw);
	}

	int get_nprocs(void) {
		return rokko_grid_get_nprocs(*raw);
	}

	~wrap_rokko_grid(void) {
		rokko_grid_destruct(raw);
		delete raw;
	}
};

class wrap_rokko_distributed_matrix;

class wrap_rokko_parallel_dense_solver {
	rokko_parallel_dense_solver* raw;
public:
	wrap_rokko_parallel_dense_solver(char* solver_name, int argc, char** argv) {
		raw = new rokko_parallel_dense_solver();
		rokko_parallel_dense_solver_construct(raw, solver_name, argc, argv);
	}
	rokko_parallel_dense_solver* get_raw(void) {
		return raw;
	}

	void diagonalize_distributed_matrix(wrap_rokko_distributed_matrix*, wrap_rokko_localized_vector*, wrap_rokko_distributed_matrix*);

	~wrap_rokko_parallel_dense_solver(void) {
		rokko_parallel_dense_solver_destruct(raw);
		delete raw;
	}
};

class wrap_rokko_distributed_matrix {
	rokko_distributed_matrix* raw;
public:
	wrap_rokko_distributed_matrix(int dim1, int dim2, wrap_rokko_grid* grid, wrap_rokko_parallel_dense_solver* solver, int matrix_major) {
		raw = new rokko_distributed_matrix();
		rokko_distributed_matrix_construct(raw, dim1, dim2, *grid->get_raw(), *solver->get_raw(), matrix_major);
	}
	rokko_distributed_matrix* get_raw(void) {
		return raw;
	}

	void print(void) {
		rokko_distributed_matrix_print(*raw);
	}

	void set_local(int local_i, int local_j, double value) {
		rokko_distributed_matrix_set_local(raw, local_i, local_j, value);
	}

	double get_local(int local_i, int local_j) {
		return rokko_distributed_matrix_get_local(*raw, local_i, local_j);
	}

	void set_global(int global_i, int global_j, double value) {
		rokko_distributed_matrix_set_global(raw, global_i, global_j, value);
	}

	double get_global(int global_i, int global_j) {
		return rokko_distributed_matrix_get_global(*raw, global_i, global_j);
	}

	int get_m_local(void) {
		return rokko_distributed_matrix_get_m_local(*raw);
	}

	int get_n_local(void) {
		return rokko_distributed_matrix_get_n_local(*raw);
	}

	int get_m_global(void) {
		return rokko_distributed_matrix_get_m_global(*raw);
	}

	int get_n_global(void) {
		return rokko_distributed_matrix_get_n_global(*raw);
	}

	int get_nprocs(void) {
		return rokko_distributed_matrix_get_nprocs(*raw);
	}

	int get_myrank(void) {
		return rokko_distributed_matrix_get_myrank(*raw);
	}

	int translate_l2g_row(int local_i) {
		return rokko_distributed_matrix_translate_l2g_row(*raw, local_i);
	}

	int translate_l2g_col(int local_j) {
		return rokko_distributed_matrix_translate_l2g_col(*raw, local_j);
	}

	int translate_g2l_row(int global_i) {
		return rokko_distributed_matrix_translate_g2l_row(*raw, global_i);
	}

	int translate_g2l_col(int global_j) {
		return rokko_distributed_matrix_translate_g2l_col(*raw, global_j);
	}

	~wrap_rokko_distributed_matrix(void) {
		rokko_distributed_matrix_destruct(raw);
		delete raw;
	}
};

void wrap_rokko_parallel_dense_solver::diagonalize_distributed_matrix(wrap_rokko_distributed_matrix* mat, wrap_rokko_localized_vector* eigvals, wrap_rokko_distributed_matrix* eigvecs)
{
	rokko_parallel_dense_solver_diagonalize_distributed_matrix(raw, mat->get_raw(), eigvals->get_raw(), eigvecs->get_raw());
}

void wrap_rokko_gather(wrap_rokko_distributed_matrix* matrix, double* array, int root)
{
	return rokko_gather(matrix->get_raw(), array, root);
}

void wrap_rokko_scatter(double* global_array, wrap_rokko_distributed_matrix* matrix, int root)
{
	rokko_scatter(global_array, matrix->get_raw(), root);
}

void wrap_rokko_all_gather(wrap_rokko_distributed_matrix* matrix, double* array)
{
	rokko_all_gather(matrix->get_raw(), array);
}

static void wrap_rokko_frank_matrix_generate_distributed_matrix(wrap_rokko_distributed_matrix* mat)
{
	rokko_frank_matrix_generate_distributed_matrix(mat->get_raw());
}

class wrap_rokko_distributed_crs_matrix;

class wrap_rokko_parallel_sparse_solver {
	rokko_parallel_sparse_solver* raw;
public:
	wrap_rokko_parallel_sparse_solver(char* solver_name, int argc, char** argv) {
		raw = new rokko_parallel_sparse_solver();
		rokko_parallel_sparse_solver_construct(raw, solver_name, argc, argv);
	}
	rokko_parallel_sparse_solver* get_raw(void) {
		return raw;
	}

	void diagonalize_distributed_crs_matrix(wrap_rokko_distributed_crs_matrix*, int, int, int, double);

	double eigenvalue(int i) {
		return rokko_parallel_sparse_solver_eigenvalue(raw, i);
	}

	void eigenvector(int i, boost::python::list& vec) {
		int len_vec = boost::python::len(vec);
		double raw_vec[len_vec];
		rokko_parallel_sparse_solver_eigenvector(raw, i, raw_vec);
		for (int i = 0; i < len_vec; i++) {
			vec[i] = raw_vec[i];
		}
	}

	int num_conv(void) {
		return rokko_parallel_sparse_solver_num_conv(raw);
	}

	~wrap_rokko_parallel_sparse_solver(void) {
		rokko_parallel_sparse_solver_destruct(raw);
		delete raw;
	}
};

class wrap_rokko_distributed_crs_matrix {
	rokko_distributed_crs_matrix* raw;
public:
	wrap_rokko_distributed_crs_matrix(int dim1, int dim2, wrap_rokko_parallel_sparse_solver* solver) {
		raw = new rokko_distributed_crs_matrix();
		rokko_distributed_crs_matrix_construct(raw, dim1, dim2, *solver->get_raw());
	}
	rokko_distributed_crs_matrix* get_raw(void) {
		return raw;
	}

	void insert(int row, int col_size, boost::python::list& cols, boost::python::list& values) {
		int len_cols = boost::python::len(cols);
		int raw_cols[len_cols];
		for (int i = 0; i < len_cols; i++) {
			raw_cols[i] = boost::python::extract<int>(cols[i]);
		}

		int len_values = boost::python::len(values);
		double raw_values[len_values];
		for (int i = 0; i < len_cols; i++) {
			raw_values[i] = boost::python::extract<double>(values[i]);
		}

		rokko_distributed_crs_matrix_insert(raw, row, col_size, raw_cols, raw_values);
	}

	void complete(void) {
		rokko_distributed_crs_matrix_complete(raw);
	}

	int num_local_rows(void) {
		return rokko_distributed_crs_matrix_num_local_rows(raw);
	}

	int start_row(void) {
		return rokko_distributed_crs_matrix_start_row(raw);
	}

	int end_row(void) {
		return rokko_distributed_crs_matrix_end_row(raw);
	}

	void print(void) {
		rokko_distributed_crs_matrix_print(raw);
	}

	~wrap_rokko_distributed_crs_matrix(void) {
		rokko_distributed_crs_matrix_destruct(raw);
		delete raw;
	}
};

void wrap_rokko_parallel_sparse_solver::diagonalize_distributed_crs_matrix(wrap_rokko_distributed_crs_matrix* mat, int num_evals, int block_size, int max_iters, double tol)
{
	rokko_parallel_sparse_solver_diagonalize_distributed_crs_matrix(raw, mat->get_raw(), num_evals, block_size, max_iters, tol);
}

#endif

BOOST_PYTHON_MODULE(rokko_ext) {
  using namespace boost::python;

  enum_<rokko>("rokko")
    .value("grid_col_major", grid_col_major)
    .value("grid_row_major", grid_row_major)
    .value("matrix_col_major", matrix_col_major)
    .value("matrix_row_major", matrix_row_major);

  class_<wrap_rokko_serial_dense_solver>("rokko_serial_dense_solver", init<char*, int, char**>())
    .def("diagonalize_localized_matrix",
         &wrap_rokko_serial_dense_solver::diagonalize_localized_matrix);
  class_<wrap_rokko_localized_vector>("rokko_localized_vector", init<int>())
    .def("get", &wrap_rokko_localized_vector::get);
  class_<wrap_rokko_localized_matrix>("rokko_localized_matrix", init<int, int, int>())
    .def("show", &wrap_rokko_localized_matrix::print);
  def("rokko_frank_matrix_generate_localized_matrix",
      &wrap_rokko_frank_matrix_generate_localized_matrix);

#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER) || defined(ROKKO_HAVE_PARALLEL_SPARSE_SOLVER)
  class_<wrap_rokko_grid>("rokko_grid", init<boost::python::object, int>())
    .def("get_myrank", &wrap_rokko_grid::get_myrank)
    .def("get_nprocs", &wrap_rokko_grid::get_nprocs);
  class_<wrap_rokko_parallel_dense_solver>("rokko_parallel_dense_solver",
                                           init<char*, int, char**>())
    .def("diagonalize_distributed_matrix",
         &wrap_rokko_parallel_dense_solver::diagonalize_distributed_matrix);
  class_<wrap_rokko_distributed_matrix>("rokko_distributed_matrix",
    init<int, int, wrap_rokko_grid*, wrap_rokko_parallel_dense_solver*, int>())
    .def("show", &wrap_rokko_distributed_matrix::print)
    .def("set_local", &wrap_rokko_distributed_matrix::set_local)
    .def("get_local", &wrap_rokko_distributed_matrix::get_local)
    .def("set_global", &wrap_rokko_distributed_matrix::set_global)
    .def("get_global", &wrap_rokko_distributed_matrix::get_global)
    .def("get_m_local", &wrap_rokko_distributed_matrix::get_m_local)
    .def("get_n_local", &wrap_rokko_distributed_matrix::get_n_local)
    .def("get_m_global", &wrap_rokko_distributed_matrix::get_m_global)
    .def("get_n_global", &wrap_rokko_distributed_matrix::get_n_global)
    .def("get_nprocs", &wrap_rokko_distributed_matrix::get_nprocs)
    .def("get_myrank", &wrap_rokko_distributed_matrix::get_myrank)
    .def("translate_l2g_row", &wrap_rokko_distributed_matrix::translate_l2g_row)
    .def("translate_l2g_col", &wrap_rokko_distributed_matrix::translate_l2g_col)
    .def("translate_g2l_row", &wrap_rokko_distributed_matrix::translate_g2l_row)
    .def("translate_g2l_col", &wrap_rokko_distributed_matrix::translate_g2l_col);
  class_<wrap_rokko_parallel_sparse_solver>("rokko_parallel_sparse_solver",
                                            init<char*, int, char**>())
    .def("diagonalize_distributed_crs_matrix",
         &wrap_rokko_parallel_sparse_solver::diagonalize_distributed_crs_matrix)
    .def("eigenvalue", &wrap_rokko_parallel_sparse_solver::eigenvalue)
    .def("eigenvector", &wrap_rokko_parallel_sparse_solver::eigenvector)
    .def("num_conv", &wrap_rokko_parallel_sparse_solver::num_conv);
  class_<wrap_rokko_distributed_crs_matrix>("rokko_distributed_crs_matrix",
                                            init<int, int, wrap_rokko_parallel_sparse_solver*>())
    .def("insert", &wrap_rokko_distributed_crs_matrix::insert)
    .def("complete", &wrap_rokko_distributed_crs_matrix::complete)
    .def("num_local_rows", &wrap_rokko_distributed_crs_matrix::num_local_rows)
    .def("start_row", &wrap_rokko_distributed_crs_matrix::start_row)
    .def("end_row", &wrap_rokko_distributed_crs_matrix::end_row)
    .def("show", &wrap_rokko_distributed_crs_matrix::print);
/*
  With a callback:
  def("rokko_distributed_matrix_generate_function", &rokko_distributed_matrix_generate_function);
*/
  def("rokko_gather", &wrap_rokko_gather);
  def("rokko_scatter", &wrap_rokko_scatter);
  def("rokko_all_gather", &wrap_rokko_all_gather);

  def("rokko_frank_matrix_generate_distributed_matrix",
      &wrap_rokko_frank_matrix_generate_distributed_matrix);
#endif
}


#include <stdlib.h>
#include <boost/format.hpp>
#include <iostream>

#include <pcl/common/transforms.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/passthrough.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/surface/marching_cubes.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/surface/poisson.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/impl/texture_mapping.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>

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

#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/opencv.hpp>

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

#include <pcl/surface/mls.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h>
#include <pcl/surface/vtk_smoothing/vtk_utils.h>
#include <pcl/visualization/common/actor_map.h>
#include <vtkFillHolesFilter.h>

#include "Frame3D/Frame3D.h"

#include <algorithm>  // std::max
#include <sys/stat.h> // mkdir
#include <sstream>

using namespace std;

int main(int argc, char *argv[]) {
    if (argc < 2) {
      std::cout << "./final [file]" << std::endl;
      return 0;
    }

    srand(time(NULL));

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr texturedCloud;

    pcl::PolygonMesh triangles;

    cout << pcl::io::loadPolygonFile(argv[1], triangles) << endl;

	// Sample code for visualization.

	// Show viewer
	std::cout << "Finished texturing" << std::endl;
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

	// Add colored point cloud to viewer, because it does not support colored
	// meshes
	//~ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> rgb(texturedCloud);
	//~ viewer->addPointCloud<pcl::PointXYZRGBNormal>(texturedCloud, rgb, "cloud");
	//~ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");

	// Add mesh
	viewer->setBackgroundColor(1, 1, 1);
	viewer->addPolygonMesh(triangles, "meshes", 0);
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

	// Keep viewer open
	while (!viewer->wasStopped()) {
	  viewer->spinOnce(100);
	  boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

    return 0;
  }