#ifndef TMC_UNIT_TESTS
#define BOOST_TEST_MODULE lib_test_border
#endif
#include <boost/test/included/unit_test.hpp>
#include <boost/property_tree/json_parser.hpp>
#include "sample_creator.h"
#include "graph_creator.h"

BOOST_AUTO_TEST_SUITE( border )

BOOST_AUTO_TEST_CASE( basics ) {
    std::string names_buffer("| this is comment\n"
                             "target_feature.| one more comment\n"
                             "\n"
                             "   feature1: continuous.   \n"
                             "feature2: continuous.   | third comment\n"
                             "feature3: ignore.\n"
                             "feature4: ignore.\n"
                             "target_feature: v1, v2.\n"
                             "case weight: continuous.\n"
                             "feature5: ignore.\n"
                             "\n"
                             "  | one more comment here\n"
                             "\n");
    std::stringstream ss_names((std::stringstream(names_buffer)));
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(ss_names);

    std::string data_buffer1("1,8,34,44,v2,1,77\n"
                             "2,2,34,44,v1,2,77\n"
                             "2,5,34,44,v2,3,77\n"
                             "3,7,34,44,v2,1,77\n"
                             "3,9,34,44,v1,2,77\n"
                             "4,1,34,44,v2,3,77\n"
                             "4,5,34,44,v2,1,77\n"
                             "5,8,34,44,v1,2,77\n"
                             "6,0,34,44,v2,3,77\n"
                             "7,1,34,44,v2,3,77\n"
                             "7,5,34,44,v2,3,77\n"
                             "8,2,34,44,v2,3,77\n"
    );

    std::stringstream ss_buffer1((std::stringstream(data_buffer1)));

    SampleCreator sample_creator1;
    sample_creator1.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample1 = sample_creator1.from_stream(ss_buffer1);
    BOOST_TEST_MESSAGE("Sample1: " << *pSample1);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;
    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample1);
    auto pGraph1 = pGraphCreator->get_graph();

    SampleCreator sample_creator2;
    const std::shared_ptr<Border> pLower = sample_creator2.lower_border(pSample1, pGraph1);
    const std::shared_ptr<Border> pUpper = sample_creator2.upper_border(pSample1, pGraph1);
    BOOST_TEST_MESSAGE("Lower border: " << *pLower);
    BOOST_TEST_MESSAGE("Upper border: " << *pUpper);

    BOOST_CHECK(pSample1->dim() ==pLower->dim());
    BOOST_CHECK(pSample1->get_neg_pos_counts() ==pLower->get_neg_pos_counts());
    BOOST_CHECK(pSample1->get_neg_pos_counts() ==pUpper->get_neg_pos_counts());

    BOOST_CHECK_EQUAL(pLower->size(), 4);
    BOOST_CHECK_EQUAL(pUpper->size(), 4);

    BOOST_CHECK(pLower->contains((*pSample1)[0]));
    BOOST_CHECK(pLower->contains((*pSample1)[1]));
    BOOST_CHECK(pLower->contains((*pSample1)[5]));
    BOOST_CHECK(pLower->contains((*pSample1)[8]));

    BOOST_CHECK(pUpper->contains((*pSample1)[4]));
    BOOST_CHECK(pUpper->contains((*pSample1)[7]));
    BOOST_CHECK(pUpper->contains((*pSample1)[10]));
    BOOST_CHECK(pUpper->contains((*pSample1)[11]));

    BOOST_CHECK_GE(*pUpper, *pLower);
    BOOST_CHECK_LE(*pLower, *pUpper);
    BOOST_CHECK(!(*pUpper <= *pLower));
    BOOST_CHECK(!(*pLower >= *pUpper));
    BOOST_CHECK(pLower->has_no_intersection_with(*pUpper));
    BOOST_CHECK(pUpper->has_no_intersection_with(*pLower));
}

BOOST_AUTO_TEST_CASE( ptree ) {
    std::string names_buffer("| this is comment\n"
                             "target_feature.| one more comment\n"
                             "\n"
                             "   feature1: continuous.   \n"
                             "feature2: continuous.   | third comment\n"
                             "feature3: ignore.\n"
                             "feature4: ignore.\n"
                             "target_feature: v1, v2.\n"
                             "case weight: continuous.\n"
                             "feature5: ignore.\n"
                             "\n"
                             "  | one more comment here\n"
                             "\n");
    std::stringstream ss_names((std::stringstream(names_buffer)));
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(ss_names);

    std::string data_buffer1("1,8,34,44,v2,1,77\n"
                             "2,2,34,44,v1,2,77\n"
                             "2,5,34,44,v2,3,77\n"
                             "3,7,34,44,v2,1,77\n"
                             "3,9,34,44,v1,2,77\n"
                             "4,1,34,44,v2,3,77\n"
                             "4,5,34,44,v2,1,77\n"
                             "5,8,34,44,v1,2,77\n"
                             "6,0,34,44,v2,3,77\n"
                             "7,1,34,44,v2,3,77\n"
                             "7,5,34,44,v2,3,77\n"
                             "8,2,34,44,v2,3,77\n"
    );

    std::stringstream ss_buffer1((std::stringstream(data_buffer1)));

    SampleCreator sample_creator1;
    sample_creator1.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample1 = sample_creator1.from_stream(ss_buffer1);
    BOOST_TEST_MESSAGE("Sample1: " << *pSample1);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;
    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample1);
    auto pGraph1 = pGraphCreator->get_graph();

    SampleCreator sample_creator2;
    const std::shared_ptr<Border> pLower = sample_creator2.lower_border(pSample1, pGraph1);
    const std::shared_ptr<Border> pUpper = sample_creator2.upper_border(pSample1, pGraph1);
    BOOST_TEST_MESSAGE("Lower border: " << *pLower);
    BOOST_TEST_MESSAGE("Upper border: " << *pUpper);

    boost::property_tree::ptree pt;
    pLower->dump_to_ptree(pt);

    BOOST_CHECK_EQUAL(pt.size(), 5);

    std::stringstream ss;
    boost::property_tree::json_parser::write_json(ss, pt);
    BOOST_TEST_MESSAGE("Property tree as json:\n" << ss.str());

    const std::shared_ptr<Border> pLower1 = std::make_shared<Border>(pt);
    BOOST_TEST_MESSAGE("Read from ptree: " << *pLower1);
    BOOST_CHECK_EQUAL(pLower->dim(), pLower1->dim());
    BOOST_CHECK_EQUAL(pLower->size(), pLower1->size());
    BOOST_CHECK(pLower->get_neg_pos_counts() == pLower1->get_neg_pos_counts());
    for (size_t i = 0; i < pLower->size(); ++i)
        BOOST_CHECK((*pLower)[i]->get_data() == (*pLower1)[i]->get_data());

    for (double x = 0; x <=10; x+=0.5)
        for  (double y = 0; y <=10; y+=0.5) {
            if (pLower->point_above(std::vector<double>({x, y})) !=
                pLower1->point_above(std::vector<double>({x, y})) )
                BOOST_TEST_FAIL("Deserialized and serialized border yields another result for the point"
                                        << " x=" << x << ", y=" << y);
        }
}

BOOST_AUTO_TEST_CASE( point_check_2D ) {
    std::string names_buffer("| this is comment\n"
                             "target_feature.| one more comment\n"
                             "\n"
                             "   feature1: continuous.   \n"
                             "feature2: continuous.   | third comment\n"
                             "feature3: ignore.\n"
                             "feature4: ignore.\n"
                             "target_feature: v1, v2.\n"
                             "case weight: continuous.\n"
                             "feature5: ignore.\n"
                             "\n"
                             "  | one more comment here\n"
                             "\n");
    std::stringstream ss_names((std::stringstream(names_buffer)));
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(ss_names);

    std::string data_buffer1("1,8,34,44,v2,1,77\n"
                             "2,2,34,44,v1,2,77\n"
                             "2,5,34,44,v2,3,77\n"
                             "3,7,34,44,v2,1,77\n"
                             "3,9,34,44,v1,2,77\n"
                             "4,1,34,44,v2,3,77\n"
                             "4,5,34,44,v2,1,77\n"
                             "5,8,34,44,v1,2,77\n"
                             "6,0,34,44,v2,3,77\n"
                             "7,1,34,44,v2,3,77\n"
                             "7,5,34,44,v2,3,77\n"
                             "8,2,34,44,v2,3,77\n"
    );

    std::stringstream ss_buffer1((std::stringstream(data_buffer1)));

    SampleCreator sample_creator1;
    sample_creator1.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample1 = sample_creator1.from_stream(ss_buffer1);
    BOOST_TEST_MESSAGE("Sample1: " << *pSample1);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;
    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample1);
    auto pGraph1 = pGraphCreator->get_graph();

    SampleCreator sample_creator2;
    const std::shared_ptr<Border> pLower = sample_creator2.lower_border(pSample1, pGraph1);
    const std::shared_ptr<Border> pUpper = sample_creator2.upper_border(pSample1, pGraph1);
    BOOST_TEST_MESSAGE("Lower border: " << *pLower);
    BOOST_TEST_MESSAGE("Upper border: " << *pUpper);

    BOOST_CHECK(!pLower->point_above(std::vector<double>({0,0})));
    BOOST_CHECK(!pLower->point_above(std::vector<double>({3,1.5})));
    BOOST_CHECK(!pLower->point_above(std::vector<double>({0,10})));
    BOOST_CHECK(pLower->point_above(std::vector<double>({1,8})));
    BOOST_CHECK(pLower->point_above(std::vector<double>({2,8})));
    BOOST_CHECK(pLower->point_above(std::vector<double>({5,5})));
    BOOST_CHECK(pLower->point_above(std::vector<double>({5,2})));

    BOOST_CHECK(!pUpper->point_below(std::vector<double>({10,10})));
    BOOST_CHECK(!pUpper->point_below(std::vector<double>({6,6})));
    BOOST_CHECK(!pUpper->point_below(std::vector<double>({4,8.5})));
    BOOST_CHECK(!pUpper->point_below(std::vector<double>({9,1})));
    BOOST_CHECK(pUpper->point_below(std::vector<double>({0,0})));
    BOOST_CHECK(pUpper->point_below(std::vector<double>({3,9})));
    BOOST_CHECK(pUpper->point_below(std::vector<double>({3,8})));
    BOOST_CHECK(pUpper->point_below(std::vector<double>({4,3})));

    BOOST_CHECK_EQUAL(pLower->consistent(), true);
    BOOST_CHECK_EQUAL(pUpper->consistent(), true);

    for (double x = 0; x <=10; x+=0.5)
       for  (double y = 0; y <=10; y+=0.5) {
           if (pUpper->point_above(std::vector<double>({x, y}), false) !=
                   pUpper->point_above(std::vector<double>({x, y}), true))
               BOOST_TEST_FAIL("Fast and slow imps. for point_above produce different results for"
               << " x=" << x << ", y=" << y);
           if (pLower->point_below(std::vector<double>({x, y}), false) !=
                   pLower->point_below(std::vector<double>({x, y}), true))
               BOOST_TEST_FAIL("Fast and slow imps. for point_below produce different results for"
                                       << " x=" << x << ", y=" << y);
       }
}

BOOST_AUTO_TEST_CASE( point_check_multiD ) {
    std::string names_buffer("| this is comment\n"
                             "target_feature.| one more comment\n"
                             "\n"
                             "   feature1: continuous.   \n"
                             "feature2: continuous.   | third comment\n"
                             "feature3: continuous.\n"
                             "feature4: continuous.\n"
                             "target_feature: v1, v2.\n"
                             "case weight: continuous.\n"
                             "feature5: ignore.\n"
                             "\n"
                             "  | one more comment here\n"
                             "\n");
    std::stringstream ss_names((std::stringstream(names_buffer)));
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(ss_names);

    std::string data_buffer1("1,8,1,3,v2,1,77\n"
                             "2,2,2,4,v1,2,77\n"
                             "2,5,3,3,v2,3,77\n"
                             "3,7,4,2,v2,1,77\n"
                             "3,9,3,1,v1,2,77\n"
                             "4,1,2,2,v2,3,77\n"
                             "4,5,1,3,v2,1,77\n"
                             "5,8,2,4,v1,2,77\n"
                             "6,0,3,3,v2,3,77\n"
                             "7,1,4,2,v2,3,77\n"
                             "7,5,3,1,v2,3,77\n"
                             "8,2,2,2,v2,3,77\n"
    );

    std::stringstream ss_buffer1((std::stringstream(data_buffer1)));

    SampleCreator sample_creator1;
    sample_creator1.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample1 = sample_creator1.from_stream(ss_buffer1);
    BOOST_TEST_MESSAGE("Sample1: " << *pSample1);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;
    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample1);
    auto pGraph1 = pGraphCreator->get_graph();

    SampleCreator sample_creator2;
    const std::shared_ptr<Border> pLower = sample_creator2.lower_border(pSample1, pGraph1);
    const std::shared_ptr<Border> pUpper = sample_creator2.upper_border(pSample1, pGraph1);
    BOOST_TEST_MESSAGE("Lower border: " << *pLower);
    BOOST_TEST_MESSAGE("Upper border: " << *pUpper);

    BOOST_CHECK(!pLower->point_above(std::vector<double>({1,8,1,2})));
    BOOST_CHECK(pLower->point_above(std::vector<double>({1,8,1,3})));

    BOOST_CHECK(pUpper->point_below(std::vector<double>({2,5,3,3})));
    BOOST_CHECK(!pUpper->point_below(std::vector<double>({2,5,3,4})));

    BOOST_CHECK_EQUAL(pLower->consistent(), true);
    BOOST_CHECK_EQUAL(pUpper->consistent(), true);

    for (double x = 0; x <=10; x+=0.5)
        for  (double y = 0; y <=10; y+=0.5)
            for (double z = 0; z <=10; z+=0.5)
                for  (double t = 0; t <=10; t+=0.5) {
            if (pUpper->point_above(std::vector<double>({x,y,z,t}), false) !=
                pUpper->point_above(std::vector<double>({x,y,z,t}), true))
                BOOST_TEST_FAIL("Fast and slow imps. for point_above produces different results for"
                                        << " x=" << x << ", y=" << y << " z=" << z << ", t=" << t);
            if (pLower->point_below(std::vector<double>({x,y,z,t}), false) !=
                pLower->point_below(std::vector<double>({x,y,z,t}), true))
                BOOST_TEST_FAIL("Fast and slow imps. for point_below produces different results for"
                                        << " x=" << x << ", y=" << y << " z=" << z << ", t=" << t);
        }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef JITANA_DATA_FLOW_HPP
#define JITANA_DATA_FLOW_HPP

#include <algorithm>
#include <vector>

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/properties.hpp>
#include <boost/type_erasure/any_cast.hpp>
#include <boost/range/iterator_range.hpp>
#include "jitana/analysis/monotonic_dataflow.hpp"
#include "jitana/vm_graph/edge_filtered_graph.hpp"
#include "jitana/vm_core/insn_info.hpp"
#include "jitana/vm_core/virtual_machine.hpp"

namespace jitana {
struct insn_data_flow_edge_property {
	register_idx reg_idx;
};

inline void print_graphviz_attr(std::ostream& os,
		const insn_data_flow_edge_property& prop) {
	os << "color=red, fontcolor=red";
	os << ", label=\"" << prop.reg_idx << "\"";
}

void compute_data_flow(virtual_machine& vm, insn_graph& g) {
	if (num_vertices(g) == 0) {
		return;
	}

	using elem = std::pair<insn_vertex_descriptor, register_idx>;
	using set = std::vector<elem>;
	using set_map = std::vector<set>;
	set_map inset_map(num_vertices(g));
	set_map outset_map(num_vertices(g));

	std::vector < std::vector < register_idx >> uses_map(num_vertices(g));
	std::vector < std::vector < register_idx >> defs_map(num_vertices(g));
	for (const auto& v : boost::make_iterator_range(vertices(g))) {
		defs_map[v] = defs(g[v].insn);
		uses_map[v] = uses(g[v].insn);
	}

	auto flow_func = [&](insn_vertex_descriptor v, const set& inset,
			set& outset) {
		outset = inset;

		// Kill.
			for (auto x : defs_map[v]) {
				auto e = std::remove_if(
						begin(outset), end(outset),
						[&](const elem& f) {return f.second == x;});
				outset.erase(e, end(outset));
			}

			// Gen.
			for (auto x : defs_map[v]) {
				outset.emplace_back(v, x);
			}
			std::sort(begin(outset), end(outset));
			outset.erase(std::unique(begin(outset), end(outset)), end(outset));
		};
	auto comb_op = [](set& x, const set& y) {
		set temp;
		std::set_union(begin(x), end(x), begin(y), end(y),
				std::back_inserter(temp));
		x = temp;
	};
	auto cfg = make_edge_filtered_graph<insn_control_flow_edge_property>(g);
	monotonic_dataflow(cfg, inset_map, outset_map, comb_op, flow_func);

	// Update the graph with the data flow information.
	remove_edge_if(make_edge_type_pred<insn_data_flow_edge_property>(g), g);
	for (const auto& v : boost::make_iterator_range(vertices(g))) {
		for (const auto& e : inset_map[v]) {
			if (e.first != v
					&& std::binary_search(begin(uses_map[v]), end(uses_map[v]),
							e.second)) {
				insn_data_flow_edge_property edge_prop;
				edge_prop.reg_idx = e.second;
				add_edge(e.first, v, edge_prop, g);
			}
		}
	}
}
}

#endif


/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE MNISTSimulator test suite
#include <boost/test/included/unit_test.hpp>
#include <SmartPeak/simulator/MNISTSimulator.h>
#include <SmartPeak/test_config.h>

#include <iostream>

using namespace SmartPeak;
using namespace std;

BOOST_AUTO_TEST_SUITE(MNISTSimulator1)

template<typename TensorT>
class MNISTSimulatorExt : public MNISTSimulator<TensorT>
{
public:
	void simulateTrainingData(Eigen::Tensor<TensorT, 4>& input_data, Eigen::Tensor<TensorT, 4>& output_data, Eigen::Tensor<TensorT, 3>& time_steps) override {}
  void simulateValidationData(Eigen::Tensor<TensorT, 4>& input_data, Eigen::Tensor<TensorT, 4>& output_data, Eigen::Tensor<TensorT, 3>& time_steps) override {}
  void simulateEvaluationData(Eigen::Tensor<TensorT, 4>& input_data, Eigen::Tensor<TensorT, 3>& time_steps) override {}
};

BOOST_AUTO_TEST_CASE(constructor)
{
  MNISTSimulatorExt<float>* ptr = nullptr;
  MNISTSimulatorExt<float>* nullPointer = nullptr;
	ptr = new MNISTSimulatorExt<float>();
  BOOST_CHECK_NE(ptr, nullPointer);
}

BOOST_AUTO_TEST_CASE(destructor)
{
  MNISTSimulatorExt<float>* ptr = nullptr;
	ptr = new MNISTSimulatorExt<float>();
  delete ptr;
}

BOOST_AUTO_TEST_CASE(ReverseInt)
{
  MNISTSimulatorExt<float> datasimulator;

  BOOST_CHECK_EQUAL(datasimulator.ReverseInt(0), 0);
  BOOST_CHECK_EQUAL(datasimulator.ReverseInt(1), 16777216);
}

BOOST_AUTO_TEST_CASE(ReadMNIST)
{
  MNISTSimulatorExt<float> datasimulator;

  // MNIST metadata
  const std::size_t input_size = 784;
  const std::size_t training_data_size = 10; //60000;
  const std::size_t validation_data_size = 10; //10000;
  std::string training_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-images.idx3-ubyte");
  std::string training_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-labels.idx1-ubyte");

  // Read training data
  Eigen::Tensor<float, 2> mnist_train(training_data_size, input_size);
  datasimulator.ReadMNIST(training_data_filename, mnist_train, false);

  // Read in the training labels
  Eigen::Tensor<float, 2> mnist_train_labels(training_data_size, 1);
  datasimulator.ReadMNIST(training_labels_filename, mnist_train_labels, true);

  std::vector<float> labels_expected = { 5,0,4,1,9,2,1,3,1,4 };
  for (int i = 0; i < training_data_size; ++i) {
    // UNCOMMENT to convince yourself that the images are what they should be...
    //std::cout << "Reshape:\n" << mnist_train.reshape(Eigen::array<Eigen::Index, 3>({ training_data_size,28,28 })).chip(i, 0) << std::endl;
    //std::cout << "Label:\n" << mnist_train_labels.chip(i, 0) << std::endl;
    BOOST_CHECK_EQUAL(mnist_train_labels(i, 0), labels_expected.at(i));
  }
}

BOOST_AUTO_TEST_CASE(readData)
{
  MNISTSimulatorExt<float> datasimulator;

  // MNIST metadata
  const std::size_t input_size = 784;
  const std::size_t training_data_size = 10; //60000;
  const std::size_t validation_data_size = 10; //10000;
  std::string training_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-images.idx3-ubyte");
  std::string training_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-labels.idx1-ubyte");
  std::string validation_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-images.idx3-ubyte");
  std::string validation_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-labels.idx1-ubyte");

  // Read training data
  datasimulator.readData(training_data_filename, training_labels_filename, true, training_data_size, input_size);

  // Read validation data
  datasimulator.readData(validation_data_filename, validation_labels_filename, false, validation_data_size, input_size);

  std::vector<float> train_labels_expected = { 5,0,4,1,9,2,1,3,1,4 };
  std::vector<float> test_labels_expected = { 7,2,1,0,4,1,4,9,5,9 };
  for (int i = 0; i < training_data_size; ++i) {
    // UNCOMMENT to convince yourself that the images are what they should be...
    //std::cout << "Reshape train images:\n" << datasimulator.training_data.reshape(Eigen::array<Eigen::Index, 3>({ training_data_size,28,28 })).chip(i, 0) << std::endl;
    //std::cout << "Labels train:\n" << train_labels_expected.at(i) << std::endl;
    BOOST_CHECK_EQUAL(datasimulator.training_labels(i, train_labels_expected.at(i)), 1);
    //std::cout << "Reshape test images:\n" << datasimulator.validation_data.reshape(Eigen::array<Eigen::Index, 3>({ validation_data_size,28,28 })).chip(i, 0) << std::endl;
    //std::cout << "Labels test:\n" << test_labels_expected.at(i) << std::endl;
    BOOST_CHECK_EQUAL(datasimulator.validation_labels(i, test_labels_expected.at(i)), 1);
  }
}

BOOST_AUTO_TEST_CASE(smoothLabels)
{
  MNISTSimulatorExt<float> datasimulator;

  // MNIST metadata
  const std::size_t input_size = 784;
  const std::size_t training_data_size = 10; //60000;
  const std::size_t validation_data_size = 10; //10000;
  std::string training_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-images.idx3-ubyte");
  std::string training_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-labels.idx1-ubyte");
  std::string validation_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-images.idx3-ubyte");
  std::string validation_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-labels.idx1-ubyte");

  // Read training data
  datasimulator.readData(training_data_filename, training_labels_filename, true, training_data_size, input_size);

  // Read validation data
  datasimulator.readData(validation_data_filename, validation_labels_filename, false, validation_data_size, input_size);

  // Test
  datasimulator.smoothLabels(0.1, 0.9);
}

BOOST_AUTO_TEST_CASE(unitScaleData)
{
  MNISTSimulatorExt<float> datasimulator;

  // MNIST metadata
  const std::size_t input_size = 784;
  const std::size_t training_data_size = 10; //60000;
  const std::size_t validation_data_size = 10; //10000;
  std::string training_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-images.idx3-ubyte");
  std::string training_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-labels.idx1-ubyte");
  std::string validation_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-images.idx3-ubyte");
  std::string validation_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-labels.idx1-ubyte");

  // Read training data
  datasimulator.readData(training_data_filename, training_labels_filename, true, training_data_size, input_size);

  // Read validation data
  datasimulator.readData(validation_data_filename, validation_labels_filename, false, validation_data_size, input_size);

  // Test
  datasimulator.unitScaleData();
}

BOOST_AUTO_TEST_CASE(centerUnitScaleData)
{
  MNISTSimulatorExt<float> datasimulator;

  // MNIST metadata
  const std::size_t input_size = 784;
  const std::size_t training_data_size = 10; //60000;
  const std::size_t validation_data_size = 10; //10000;
  std::string training_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-images.idx3-ubyte");
  std::string training_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-labels.idx1-ubyte");
  std::string validation_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-images.idx3-ubyte");
  std::string validation_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-labels.idx1-ubyte");

  // Read training data
  datasimulator.readData(training_data_filename, training_labels_filename, true, training_data_size, input_size);

  // Read validation data
  datasimulator.readData(validation_data_filename, validation_labels_filename, false, validation_data_size, input_size);

  // Test
  datasimulator.centerUnitScaleData();
}

BOOST_AUTO_TEST_CASE(corruptTrainingData)
{
  MNISTSimulatorExt<float> datasimulator;

  // MNIST metadata
  const std::size_t input_size = 784;
  const std::size_t training_data_size = 10; //60000;
  const std::size_t validation_data_size = 10; //10000;
  std::string training_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-images.idx3-ubyte");
  std::string training_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("train-labels.idx1-ubyte");
  std::string validation_data_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-images.idx3-ubyte");
  std::string validation_labels_filename = SMARTPEAK_GET_TEST_DATA_PATH("t10k-labels.idx1-ubyte");

  // Read training data
  datasimulator.readData(training_data_filename, training_labels_filename, true, training_data_size, input_size);

  // Read validation data
  datasimulator.readData(validation_data_filename, validation_labels_filename, false, validation_data_size, input_size);

  // Test
  datasimulator.corruptTrainingData(0.25);
}

BOOST_AUTO_TEST_SUITE_END()


//! file bitset2d.hpp
/** This is a 2 dimentional bitset (aka bit-matrix) that is slightly faster than
 * vector<bitset> and slightly more convenient than keeping track of the
 * index offsets using bitset
 **/

#pragma once

#include <cassert>
#include "vector2d.hpp"
#include <boost/dynamic_bitset.hpp>

namespace std{
  template<typename Symmetry = Asymmetric>
  class bitset2d : public Something2d<bool, boost::dynamic_bitset<unsigned>, Symmetry>
  {
  protected:
    using Parent = Something2d<bool, boost::dynamic_bitset<unsigned>, Symmetry>;
    using GrandPa = typename Parent::Parent;
    using Coords = typename Parent::Coords;

  public:
    bitset2d& set(const Coords& coords, bool val = true)
    {
      GrandPa::set(Parent::linearize(coords), val); return *this;
    }
    bitset2d& reset(const Coords& coords, bool val = false)
    {
      return set(coords, val);
    }
    bitset2d& flip(const Coords& coords)
    {
      GrandPa::flip(Parent::linearize(coords)); return *this;
    }
    bool test(const Coords& coords) const
    {
      return GrandPa::test(Parent::linearize(coords));
    }
    bool test_set(const Coords& coords, bool val = true)
    {
      return GrandPa::test_set(Parent::linearize(coords), val);
    }
    size_t count() const
    {
      return GrandPa::count();
    }
  };
  typedef bitset2d<Symmetric> symmetric_bitset2d;

/*
  class _bitset2d : public boost::dynamic_bitset<unsigned>
  {
  protected:
    using Parent = boost::dynamic_bitset<unsigned>;
    using Coords = pair<size_t, size_t>;
    //! translate 2d coordinates into 1d coordinates
    virtual size_t linearize(const Coords& coords) const = 0;

  public:
    virtual ~_bitset2d(){}
    _bitset2d(size_t num_bits = 0): boost::dynamic_bitset<unsigned>(num_bits) {}
    //! NOTE: coordinates are always (col, row) as in (x, y)
    Parent::reference operator[](const Coords& coords)
    {
      return Parent::operator[](linearize(coords));
    }
    bool operator[](const Coords& coords) const
    {
      return Parent::operator[](linearize(coords));
    }
    _bitset2d& set(const Coords& coords, bool val = true)
    {
      Parent::set(linearize(coords), val); return *this;
    }
    _bitset2d& reset(const Coords& coords, bool val = false)
    {
      return set(coords, val);
    }
    _bitset2d& flip(const Coords& coords)
    {
      Parent::flip(linearize(coords)); return *this;
    }
    bool test(const Coords& coords) const
    {
      return Parent::test(linearize(coords));
    }
    bool test_set(const Coords& coords, bool val = true)
    {
      return Parent::test_set(linearize(coords), val);
    }
    void resize(const size_t cols, const size_t rows)
    {
      Parent::resize(linearize({cols - 1, rows - 1}) + 1);
    }
    virtual Coords size() const = 0;
  };// class bitset2d

  template<typename T>
  class bitset2d : public _bitset2d
  {
  protected:
    using Parent = _bitset2d;
    using Parent::Coords;

    size_t columns;

    size_t linearize(const Coords& coords) const
    {
      return coords.second * columns + coords.first;
    }
  public:
    bitset2d(): _bitset2d() {}

    // reserve size such that we can access the last index (cols-1,rows-1)
    bitset2d(const size_t cols, const size_t rows):
      columns(cols),
      _bitset2d(linearize(cols - 1, rows - 1) + 1)
    {}

    void resize(const size_t cols, const size_t rows)
    {
      Parent::resize(cols, rows);
      columns = cols;
    }
    Coords size() const
    {
      assert(columns > 0);
      return Coords(columns, Parent::Parent::size() / columns);
    }
  };

  //! a symmetric version of the 2d bitset that only uses half the space
  class symmetric_bitset2d : public _bitset2d
  {
  protected:
    using Parent = _bitset2d;
    using Parent::Coords;

    //! translate 2d coordinates into 1d coordinates
    size_t linearize(const Coords& coords) const
    {
      if(coords.first <= coords.second)
        return (coords.second * (coords.second + 1)) / 2  + coords.first;
      else
        return (coords.first * (coords.first + 1)) / 2  + coords.second;
    }

  public:
    symmetric_bitset2d(): _bitset2d() {}

    symmetric_bitset2d(const size_t rows_and_cols):
      _bitset2d(linearize({rows_and_cols, rows_and_cols}))
    {}

    size_t rows() const
    {
      return sqrt(Parent::Parent::size() * 2 + 0.25) - .5;
    }
    Coords size() const
    {
      const size_t root = rows();
      return Coords(root, root);
    }
  };// class bitset2d

*/

}// namespace


/******************************************************************************
 * Copyright (c) 2016-2017 Sergey Alexandrov
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 ******************************************************************************/

#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include <boost/filesystem.hpp>

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

#include <radical/radiometric_response.h>

#include "grabbers/grabber.h"

#include "utils/arrange_images_in_grid.h"
#include "utils/plot_histogram.h"
#include "utils/plot_radiometric_response.h"
#include "utils/program_options.h"

#include "calibration.h"
#include "dataset.h"
#include "dataset_collection.h"
#include "engel_calibration.h"

#if HAVE_CERES
#include "debevec_calibration.h"
#define DEFAULT_METHOD "debevec"
#else
#define DEFAULT_METHOD "engel"
#endif

class Options : public OptionsBase {
 public:
  std::string data_source = "";
  std::string output;
  double convergence_threshold = 1e-5;
  std::string calibration_method = DEFAULT_METHOD;
  bool no_visualization = false;
  std::string save_dataset = "";
  DatasetCollection::Parameters dc;
  unsigned int verbosity = 1;
  unsigned int min_samples = 5;
  bool interactive = false;
  double smoothing = 50;
  bool print = false;

 protected:
  void addOptions(boost::program_options::options_description& desc) override {
    namespace po = boost::program_options;
    desc.add_options()("output,o", po::value<std::string>(&output),
                       "Output filename with calibrated response function (default: camera model name + \".\" + camera "
                       "serial number + \".crf\" suffix)");
    desc.add_options()("threshold,t", po::value<double>(&convergence_threshold),
                       "Threshold for energy update after which convergence is declared (default: 1e-5)");
    desc.add_options()("method,m", po::value<std::string>(&calibration_method)->default_value(calibration_method),
                       "Calibration method to use");
    desc.add_options()("min-samples", po::value<unsigned int>(&min_samples)->default_value(min_samples),
                       "Min number of samples per intensity level (only for debevec method)");
    desc.add_options()("smoothing", po::value<double>(&smoothing)->default_value(smoothing),
                       "Smoothing lambda (only for debevec method)");
    desc.add_options()("no-visualization", po::bool_switch(&no_visualization),
                       "Do not visualize the calibration process and results");
    desc.add_options()("verbosity,v", po::value<unsigned int>(&verbosity)->default_value(verbosity),
                       "Verbosity level (0 - silent, 1 - normal, 2 - verbose)");
    desc.add_options()("interactive", po::bool_switch(&interactive),
                       "Wait for a keypress after each optimization iteration");
    desc.add_options()("print", po::bool_switch(&print), "Print calibrated response function to stdout");

    boost::program_options::options_description dcopt("Data collection");
    dcopt.add_options()("exposure-min", po::value<int>(&dc.exposure_min),
                        "Minimum exposure (default: depends on the camera)");
    dcopt.add_options()("exposure-max", po::value<int>(&dc.exposure_max),
                        "Maximum exposure (default: depends on the camera)");
    dcopt.add_options()("factor,f", po::value<float>(&dc.exposure_factor),
                        "Multiplication factor for exposure (default: to cover desired exposure range in 30 steps)");
    dcopt.add_options()("average,a",
                        po::value<unsigned int>(&dc.num_average_frames)->default_value(dc.num_average_frames),
                        "Number of consecutive frames to average into each image");
    dcopt.add_options()("images,i", po::value<unsigned int>(&dc.num_images)->default_value(dc.num_images),
                        "Number of images to take at each exposure setting");
    dcopt.add_options()("lag,l",
                        po::value<unsigned int>(&dc.exposure_control_lag)->default_value(dc.exposure_control_lag),
                        "Number of frames to skip after changing exposure setting");
    dcopt.add_options()("valid-min",
                        po::value<unsigned int>(&dc.valid_intensity_min)->default_value(dc.valid_intensity_min),
                        "Minimum valid intensity value of the sensor");
    dcopt.add_options()("valid-max",
                        po::value<unsigned int>(&dc.valid_intensity_max)->default_value(dc.valid_intensity_max),
                        "Maximum valid intensity value of the sensor");
    dcopt.add_options()("bloom-radius", po::value<unsigned int>(&dc.bloom_radius)->default_value(dc.bloom_radius),
                        "Radius of the blooming effect");
    dcopt.add_options()("save-dataset,s", po::value<std::string>(&save_dataset),
                        "Save collected dataset in the given directory");
    desc.add(dcopt);
  }

  void addPositional(boost::program_options::options_description& desc,
                     boost::program_options::positional_options_description& positional) override {
    namespace po = boost::program_options;
    desc.add_options()("data-source", po::value<std::string>(&data_source),
                       R"(Data source, either a camera ("asus", "intel"), or a path to dataset)");
    positional.add("data-source", -1);
  }

  void printHelp() override {
    std::cout << "Usage: calibrate_radiometric_response [options] <data-source>" << std::endl;
    std::cout << "" << std::endl;
    std::cout << "Calibrate radiometric response of a camera. Two algorithms are available:" << std::endl;
    std::cout << " * Engel et al. (A Photometrically Calibrated Benchmark For Monocular Visual Odometry)" << std::endl;
    std::cout << " * Debevec and Malik (Recovering High Dynamic Range Radiance Maps from Photographs)" << std::endl;
    std::cout << "" << std::endl;
    std::cout << "Working range of the sensor can be specified with --valid-min/--valid-max options." << std::endl;
    std::cout << "Pixels with intensity values outside this range do not contribute to the energy and" << std::endl;
    std::cout << "irradiance computation, however radiometric response is estimated for them as well." << std::endl;
    std::cout << "" << std::endl;
  }

  void validate() override {
    if (dc.valid_intensity_max > 255) {
      throw boost::program_options::error("maximum valid intensity can not exceed 255");
    }
    if (dc.valid_intensity_min > 255) {
      throw boost::program_options::error("minimum valid intensity can not exceed 255");
    }
  }
};

int main(int argc, const char** argv) {
  Options options;
  if (!options.parse(argc, argv))
    return 1;

  auto imshow = [&options](const cv::Mat& image, int w = -1) {
    if (!options.no_visualization) {
      cv::imshow("Calibration", image);
      cv::waitKey(w);
    }
  };

  auto data = Dataset::load(options.data_source);
  if (data) {
    if (data->getNumImages() < 2) {
      std::cerr << "Loaded dataset contains less than 2 images, calibration is not possible" << std::endl;
      return 1;
    }
    if (!options("output")) {
      auto dir = boost::filesystem::canonical(options.data_source);
      options.output = (dir / dir.filename()).string() + ".crf";
    }
    if (options.verbosity)
      std::cout << "Loaded dataset (" << data->getNumImages() << " images) from: " << options.data_source << std::endl;

    auto hist = data->computeIntensityHistogram();
    imshow(utils::plotHistogram(hist.rowRange(options.dc.valid_intensity_min, options.dc.valid_intensity_max), 2, 256));
  } else {
    grabbers::Grabber::Ptr grabber;

    try {
      grabber = grabbers::createGrabber(options.data_source);
    } catch (grabbers::GrabberException&) {
      std::cerr << "Failed to create a grabber"
                << (options.data_source != "" ? " for camera " + options.data_source : "") << std::endl;
      return 1;
    }

    if (!options("output"))
      options.output = grabber->getCameraUID() + ".crf";
    if (!options("exposure-min"))
      options.dc.exposure_min = grabber->getExposureRange().first;
    if (!options("exposure-max"))
      options.dc.exposure_max = grabber->getExposureRange().second;
    if (!options("factor"))
      options.dc.exposure_factor = std::pow(1.0f * options.dc.exposure_max / options.dc.exposure_min, 1.0f / 30);

    cv::Mat frame;

    // Disable AEC but enable AWB so that colors are balanced
    grabber->setAutoExposureEnabled(false);
    grabber->setAutoWhiteBalanceEnabled(true);

    // Wait
    grabber->grabFrame(frame);
    imshow(frame, 400);

    // Disable AWB
    grabber->setAutoWhiteBalanceEnabled(false);

    // Change exposure to requested min value, wait some time until it works
    for (size_t i = 0; i < 12; ++i) {
      grabber->grabFrame(frame);
      imshow(frame, 400);
      grabber->setExposure(options.dc.exposure_min);
    }

    DatasetCollection data_collection(grabber, options.dc);

    cv::Mat histogram(480, 640, CV_8UC3);
    while (grabber->hasMoreFrames()) {
      grabber->grabFrame(frame);
      if (data_collection.addFrame(frame))
        break;
      auto hist = data_collection.getDataset()->computeIntensityHistogram();
      histogram.setTo(0);
      utils::plotHistogram(hist.rowRange(options.dc.valid_intensity_min, options.dc.valid_intensity_max), histogram);
      imshow(arrangeImagesInGrid({frame, histogram}, {1, 2}), 30);
    }

    data = data_collection.getDataset();

    if (options.save_dataset != "") {
      if (options.verbosity)
        std::cout << "Saving dataset to: " << options.save_dataset << std::endl;
      data->save(options.save_dataset);
      std::ofstream file(options.save_dataset + "/DESCRIPTION.txt");
      if (file.is_open()) {
        file << "Camera: " << grabber->getCameraUID() << std::endl;
        file << "Resolution: " << data->getImageSize() << std::endl;
        file << "Exposure range: " << options.dc.exposure_min << " " << options.dc.exposure_max << std::endl;
        file << "Exposure factor: " << options.dc.exposure_factor << std::endl;
        file << "Images per exposure time: " << options.dc.num_images << std::endl;
        file << "Frames averaged into an image: " << options.dc.num_average_frames << std::endl;
        file.close();
      }
    }
  }

  Calibration::Ptr calibration;

  if (options.calibration_method == "engel") {
    auto cal = std::make_shared<EngelCalibration>();
    cal->setConvergenceThreshold(options.convergence_threshold);
    calibration = cal;
  } else if (options.calibration_method == "debevec") {
#if HAVE_CERES
    auto cal = std::make_shared<DebevecCalibration>();
    cal->setMinSamplesPerIntensityLevel(options.min_samples);
    cal->setSmoothingLambda(options.smoothing);
    calibration = cal;
#else
    std::cerr << "Debevec calibration is not supported because the project was compiled without Ceres.\n";
    return 2;
#endif
  } else {
    std::cerr << "Unknown calibration method: " << options.calibration_method
              << ". Please specify \"engel\" or \"debevec\".\n";
    return 1;
  }

  auto limshow = [=](const cv::Mat& image) {
    cv::imshow("Calibration", image);
    cv::waitKey(options.interactive ? -1 : 1);
  };

  calibration->setValidPixelRange(static_cast<unsigned char>(options.dc.valid_intensity_min),
                                  static_cast<unsigned char>(options.dc.valid_intensity_max));
  calibration->setVerbosity(options.verbosity);
  if (!options.no_visualization)
    calibration->setVisualizeProgress(limshow);

  cv::Mat response = calibration->calibrate(*data);

  if (options.verbosity)
    std::cout << "Done, writing response to: " << options.output << std::endl;
  radical::RadiometricResponse rr(response);
  rr.save(options.output);

  imshow(utils::plotRadiometricResponse(rr));

  if (options.print) {
    std::vector<cv::Mat> response_channels;
    cv::split(response, response_channels);
    for (const auto& ch : response_channels) {
      for (int i = 0; i < static_cast<int>(ch.total()); ++i)
        std::cout << ch.at<float>(i) << " ";
      std::cout << std::endl;
    }
  }

  return 0;
}


//          Copyright Carl Philipp Reh 2006 - 2019.
// 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 SGE_CG_PARAMETER_MATRIX_DETAIL_SET_HPP_INCLUDED
#define SGE_CG_PARAMETER_MATRIX_DETAIL_SET_HPP_INCLUDED

#include <sge/cg/parameter/object_fwd.hpp>
#include <sge/cg/parameter/detail/pp_types.hpp>
#include <sge/cg/parameter/matrix/detail/set_double.hpp>
#include <sge/cg/parameter/matrix/detail/set_float.hpp>
#include <sge/cg/parameter/matrix/detail/set_int.hpp>
#include <fcppt/math/size_type.hpp>
#include <fcppt/math/matrix/object_impl.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
#include <fcppt/config/external_end.hpp>

namespace sge::cg::parameter::matrix::detail
{

#define SGE_CG_PARAMETER_MATRIX_DETAIL_SET(seq, _, type) \
  template <fcppt::math::size_type R, fcppt::math::size_type C, typename S> \
  void set( \
      sge::cg::parameter::object const &_parameter, \
      fcppt::math::matrix::object<type, R, C, S> const &_matrix) \
  { \
    sge::cg::parameter::matrix::detail::BOOST_PP_CAT(set_, type)( \
        _parameter, _matrix.storage().data()); \
  }

BOOST_PP_SEQ_FOR_EACH(SGE_CG_PARAMETER_MATRIX_DETAIL_SET, _, SGE_CG_PARAMETER_DETAIL_PP_TYPES)

}

#endif


#include "ros/ros.h"
#include <string>
#include <sstream>
#include <fstream>
#include <array>
#include <vector>
#include <queue>
#include <mutex>
#include <thread>
#include <Eigen/Dense>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <nav_msgs/Path.h>

class PosePublisherNode{
    public:
        PosePublisherNode(ros::NodeHandle n) : n_(n) {
            // Create private node handle
            ros::NodeHandle nh("~");
            std::string pose_csv_file, init_rot_file;
            std::string pose_topic, pose_frame;

            nh.param<std::string>("pose_topic", pose_topic, "/cheetah/groundtruth/pose");
            nh.param<std::string>("pose_frame", pose_frame, "/odom");
            nh.param<std::string>("pose_csv_file", pose_csv_file, "/data/ground_truth.csv");
            nh.param<std::string>("init_rot_file", init_rot_file, "/data/init_rot_file.csv");
            nh.param<double>("publish_rate", publish_rate_, 1000);
            nh.param<int>("pose_skip", pose_skip_, 1000);

            // load poses from csv files

            std::cout<<"reading poses from csv"<<std::endl;
            pose_from_csv_ = readPoseFromCSV(pose_csv_file,init_rot_file);
            first_pose_ = pose_from_csv_.front();
            std::cout<<"done reading, it contains: "<<pose_from_csv_.size()<<" of poses."<<std::endl;
            std::cout<<"first pose is: "<<first_pose_[0]<<", "<<first_pose_[1]<<", "<<first_pose_[2]<<std::endl;
            pose_frame_ = pose_frame;

            pose_pub_ = n_.advertise<geometry_msgs::PoseWithCovarianceStamped>(pose_topic, 1000);
            this->pose_publishing_thread_ = std::thread([this]{this->posePublishingThread();});
        }

    private:
        ros::NodeHandle n_;
        ros::Publisher pose_pub_;
        std::string pose_frame_;
        uint32_t seq_ = 0;
        double publish_rate_;
        int pose_skip_;
        std::queue<std::array<float,3>> pose_from_csv_;
        std::array<float,3> first_pose_;
        std::vector<geometry_msgs::PoseStamped> poses_;
        std::mutex poses_mutex_;
        std::thread pose_publishing_thread_;

        // Pose message callback
        void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){
            if ((int)msg->header.seq%pose_skip_!=0) { return; }
            geometry_msgs::PoseStamped pose;
            pose.header = msg->header;
            pose.pose = msg->pose.pose;
            std::lock_guard<std::mutex> lock(poses_mutex_);
            poses_.push_back(pose);
        }

        std::queue<std::array<float,3>> readPoseFromCSV(std::string pose_csv_file,std::string init_rot_file){
            Eigen::Matrix3f init_rot;
            std::string temp_rot;
            std::ifstream init_file(init_rot_file);
            if(init_file.is_open()){
                for(int i=0; i<3; ++i){
                    for(int j=0; j<3; ++j){
                        std::getline(init_file,temp_rot,',');
                        init_rot(i,j) = std::stof(temp_rot);
                    }
                }
            }
            init_file.close();

            std::cout<<"loaded init tf: "<<init_rot<<std::endl;
            std::ifstream csv_file(pose_csv_file);
            std::queue<std::array<float,3>> pose_output;

            if(csv_file.is_open()){
                std::string line;

                while(std::getline(csv_file, line))
                {
                    std::stringstream ss(line);
                    Eigen::Vector3f temp_pose;
                    std::array<float,3> temp_pose_after_tf;
                    int i = 0;
                    std::string temp_str;
                    while(std::getline(ss, temp_str, ',')){
                        temp_pose[i] = std::stof(temp_str);
                        i++;
                    }
                    temp_pose = init_rot * temp_pose;
                    for(int j=0; j<3; j++){
                        temp_pose_after_tf[j] = temp_pose[j];
                    }

                    pose_output.push(temp_pose_after_tf);
                }
                csv_file.close();
            }
            return pose_output;
        }

        // Publishes path
        void posePublish() {
            if(!pose_from_csv_.empty()){
                std::array<float,3> cur_pose = pose_from_csv_.front();
                pose_from_csv_.pop();

                geometry_msgs::PoseWithCovarianceStamped pose_msg;
                pose_msg.header.seq = seq_;
                pose_msg.header.stamp = ros::Time::now();
                pose_msg.header.frame_id = pose_frame_;
                pose_msg.pose.pose.position.x = cur_pose[0] - first_pose_[0];
                pose_msg.pose.pose.position.y = cur_pose[1] - first_pose_[1];
                pose_msg.pose.pose.position.z = cur_pose[2] - first_pose_[2];
                pose_msg.pose.pose.orientation.w = 0;
                pose_msg.pose.pose.orientation.x = 0;
                pose_msg.pose.pose.orientation.y = 0;
                pose_msg.pose.pose.orientation.z = 0;
                // std::cout<<"publishing: "<<pose_msg.pose.pose.position.x<<", "<<pose_msg.pose.pose.position.y<<", "<<pose_msg.pose.pose.position.z<<std::endl;
                pose_pub_.publish(pose_msg);
                seq_++;
            }
        }

        // Path publishing thread
        void posePublishingThread(){
            // Loop and publish data
            ros::Rate loop_rate(publish_rate_);
            while(ros::ok()){
                posePublish();
                loop_rate.sleep();
            }
        }
};

int main(int argc, char **argv) {
    // Initialize ROS node
    ros::init(argc, argv, "pose_publisher");
    ros::NodeHandle n;
    PosePublisherNode pose_publisher_node(n);
    ros::spin();
    return 0;
}


// Program that evaluates the visual utility of a set of images. By
// default evaluates the window at the center of the image.
//
// Input file:
// <imageFilename>
//
// Output file:
// <imageFilename>,<score>
//
// Usage:
// EvalVUOfImages [options]

#include "visual_utility/VisualUtilityEstimator.h"
#include "visual_utility/VisualUtilityROSParams.h"
#include <ros/ros.h>
#include <fstream>
#include <iostream>
#include <boost/scoped_ptr.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <string>
#include <vector>
#include <gflags/gflags.h>

DEFINE_int32(winH, 128, "Height of the cannoncial window");
DEFINE_int32(winW, 64, "Width of the cannoncial window");

DEFINE_string(input, "", "Input file to use. STDIN if not specified");
DEFINE_string(output, "", "Output file to write to. STDOUT if not specified");

DEFINE_double(thresh, 0.0, "Threshold for accepting an example. Used for stats");
DEFINE_bool(print_stats, false, "Optionally print the statistics at the end");

using namespace std;
using namespace visual_utility;
using namespace boost;
using namespace cv;

int main(int argc, char **argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);

  ros::init(argc, argv, "EvalVUOfImages",
            ros::init_options::AnonymousName);

  if (argc != 1) {
    ROS_FATAL("Usage: EvalVUOfImages [options]");
    return 1;
  }

  // Create the visual utility estimator we want to use
  ros::NodeHandle handle("~");
  scoped_ptr<TransformEstimator> transformEstimator(
    CreateTransformEstimator(handle));
  scoped_ptr<VisualUtilityEstimator> vuEstimator(
    CreateVisualUtilityEstimator(handle, *transformEstimator));

  // Open the file for reading the image names
  istream* inputStream = &std::cin;
  ifstream fileInputStream;
  if (!FLAGS_input.empty()) {
    fileInputStream.open(FLAGS_input.c_str(), ios_base::in);
    if (!fileInputStream.good()) {
      ROS_ERROR_STREAM("Could not open " << FLAGS_input);
      return 1;
    }
    inputStream = &fileInputStream;
  }

  // Open the file for output
  ostream* outputStream = &std::cout;
  ofstream fileOutputStream;
  if (!FLAGS_output.empty()) {
    fileOutputStream.open(FLAGS_output.c_str(),
                         ios_base::out | ios_base::trunc);
    if (!fileOutputStream.good()) {
      ROS_ERROR_STREAM("Could not open " << FLAGS_output);
      return 1;
    }
    outputStream = &fileOutputStream;
  }

  // Process one file at a time
  int nPositive = 0;
  int nExamples = 0;
  while(inputStream->good() && !inputStream->eof()) {
    string imageFile;
    (*inputStream) >> imageFile;

    if (imageFile.empty()) {
      break;
    }

    Mat image = imread(imageFile);

    if (image.empty()) {
      ROS_ERROR_STREAM("Could not open image: " << imageFile);
      continue;
    }

    vector<Rect> roi(1, Rect((image.cols - FLAGS_winW) / 2,
                             (image.rows - FLAGS_winH) / 2,
                             FLAGS_winW,
                             FLAGS_winH));
    vector<pair<double, Rect> > answer;
    vuEstimator->CalculateVisualUtility(image, roi, 0.0, &answer);

    (*outputStream) << imageFile << "," << answer[0].first << endl;

    nExamples++;
    if(answer[0].first > FLAGS_thresh) {
      nPositive++;
    }
  }

  fileOutputStream.close();
  fileInputStream.close();

  if (FLAGS_print_stats) {
    cout << "Accuracy: " << ((double)nPositive) / nExamples
         << " (" << nPositive << "/" << nExamples << ")" << std::endl;
  }

  return 0;
}


/*
 * Copyright (C) 2019 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#ifndef IGNITION_GAZEBO_SYSTEMS_MULTICOPTER_CONTROL_LEEVELOCITYCONTROLLER_HH_
#define IGNITION_GAZEBO_SYSTEMS_MULTICOPTER_CONTROL_LEEVELOCITYCONTROLLER_HH_

#include <Eigen/Geometry>
#include <memory>
#include "ignition/gazebo/config.hh"

#include "Common.hh"
#include "LeeVelocityController.hh"

namespace ignition
{
namespace gazebo
{
// Inline bracket to help doxygen filtering.
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
namespace systems
{
namespace multicopter_control
{
  /// \brief Data structure containing various parameters for the Lee velocity
  /// controller
  struct LeeVelocityControllerParameters
  {
    Eigen::Vector3d velocityGain;
    Eigen::Vector3d attitudeGain;
    Eigen::Vector3d angularRateGain;
    Eigen::Vector3d maxLinearAcceleration;
  };

  /// This controller is inspired by the LeePositionController from RotorS
  /// https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_control/include/rotors_control/lee_position_controller.h
  /// The controller can be used to command linear velocity and yaw angle
  /// velocity expressed in the body frame.
  class LeeVelocityController
  {
    /// \brief Factory function to create LeeVelocityController objects
    /// \param[in] _controllerParams Controller parameteres
    /// \param[in] _vehicleParams Vehicle parameteres
    /// \returns nullptr if the input parameters were valid, otherwise an
    /// pointer to an instance of LeeVelocityController
    public: static std::unique_ptr<LeeVelocityController> MakeController(
                const LeeVelocityControllerParameters &_controllerParams,
                const VehicleParameters &_vehicleParams);

    /// \brief Calculate rotor velocities given the current frame data and the
    /// commanded velocity
    /// \param[in] _frameData Frame data including pose and linear and angular
    /// velocities
    /// \param[in] _cmdVel Commanded velocity
    /// \param[out] _rotorVelocities Computed rotor velocities.
    public: void CalculateRotorVelocities(
                 const FrameData &_frameData,
                 const EigenTwist &_cmdVel,
                 Eigen::VectorXd &_rotorVelocities) const;

    /// \brief Private constructor. Use MakeController to create an instance of
    /// this class
    private: LeeVelocityController() = default;

    /// \brief Compute desired linear acceleration given the current frame data
    /// and commanded linear velocity
    private: Eigen::Vector3d ComputeDesiredAcceleration(
                 const FrameData &_frameData, const EigenTwist &_cmdVel) const;

    /// \brief Compute desired angular acceleration given the current frame
    /// data, comanded angular velocity and desired linear acceleration
    private: Eigen::Vector3d ComputeDesiredAngularAcc(
                 const FrameData &_frameData, const EigenTwist &_cmdVel,
                 const Eigen::Vector3d &_acceleration) const;

    /// \brief Initialize controller parameters
    /// \return True if successful.
    private: bool InitializeParameters();

    /// \brief Velocity controller parameters
    private: LeeVelocityControllerParameters controllerParameters;

    /// \brief Parameters of the whole vehicle
    private: VehicleParameters vehicleParameters;

    /// \brief Attitude gain normalized by the inverse of the inertia matrix
    private: Eigen::Vector3d normalizedAttitudeGain;

    /// \brief Angular gain normalized by the inverse of the inertia matrix
    private: Eigen::Vector3d normalizedAngularRateGain;

    /// \brief Holds the matrix that maps angular acceleration and thrust to
    /// rotor velocities
    private: Eigen::MatrixX4d angularAccToRotorVelocities;
  };
}  // namespace multicopter_control
}  // namespace systems
}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
}  // namespace gazebo
}  // namespace ignition

#endif


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "niftkSequentialCpuQds.h"
#include "niftkQDSCommon.h"
#include <queue>
#include <boost/gil/gil_all.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/video/tracking.hpp>
#include <opencv/highgui.h>
#include <boost/static_assert.hpp>
#include <stdexcept>

#ifdef _OPENMP
#include <omp.h>
#endif

// helps with debugging: writes out some intermediate images
#define DUMP_IMAGES

BOOST_STATIC_ASSERT((sizeof(niftk::RefPoint) == sizeof(boost::gil::dev2n16_pixel_t)));

namespace niftk
{

// Used to store match candidates in the priority queue
struct Match
{
  RefPoint  p0;
  RefPoint  p1;
  float     corr;

  bool operator<(const Match& rhs) const
  {
    return corr < rhs.corr;
  }
};

//-----------------------------------------------------------------------------
static bool CheckBorder(const Match& m, int bx, int by, int w, int h)
{
  if ((m.p0.x < bx    ) ||
      (m.p0.x > w - bx) ||
      (m.p0.y < by    ) ||
      (m.p0.y > h - by) ||
      (m.p1.x < bx    ) ||
      (m.p1.x > w - bx) ||
      (m.p1.y < by    ) ||
      (m.p1.y > h - by))
  {
    return false;
  }

  return true;
}

//-----------------------------------------------------------------------------
SequentialCpuQds::SequentialCpuQds(int width, int height)
  : m_Width(width), m_Height(height), m_MaxDisparity(std::max(width, height) / 7)
{
  m_LeftImg.recreate(width, height);
  m_RightImg.recreate(width, height);

  m_LeftIntegral.recreate(width + 1, height + 1);
  m_RightIntegral.recreate(width + 1, height + 1);
  m_LeftSquaredIntegral.recreate(width + 1, height + 1);
  m_RightSquaredIntegral.recreate(width + 1, height + 1);

  m_LeftTexture.recreate(width, height);
  m_RightTexture.recreate(width, height);

  m_LeftRefMap.recreate(width, height);
  m_RightRefMap.recreate(width, height);

  // the ipl images are used only for a few opencv calls
  // otherwise these simply reference the gil images on which all other processing happens
  cvInitImageHeader(&m_LeftIpl, cvSize(m_LeftImg.width(), m_LeftImg.height()), IPL_DEPTH_8U, 1);
  cvSetData(&m_LeftIpl, &boost::gil::view(m_LeftImg)(0, 0)[0], (char*) &boost::gil::view(m_LeftImg)(0, 1)[0] - (char*) &boost::gil::view(m_LeftImg)(0, 0)[0]);
  cvInitImageHeader(&m_RightIpl, cvSize(m_RightImg.width(), m_RightImg.height()), IPL_DEPTH_8U, 1);
  cvSetData(&m_RightIpl, &boost::gil::view(m_RightImg)(0, 0)[0], (char*) &boost::gil::view(m_RightImg)(0, 1)[0] - (char*) &boost::gil::view(m_RightImg)(0, 0)[0]);

  cvInitImageHeader(&m_LeftIntegralIpl, cvSize(m_LeftIntegral.width(), m_LeftIntegral.height()), IPL_DEPTH_32S, 1);
  cvSetData(&m_LeftIntegralIpl, &boost::gil::view(m_LeftIntegral)(0, 0)[0], (char*) &boost::gil::view(m_LeftIntegral)(0, 1)[0] - (char*) &boost::gil::view(m_LeftIntegral)(0, 0)[0]);
  cvInitImageHeader(&m_RightIntegralIpl, cvSize(m_RightIntegral.width(), m_RightIntegral.height()), IPL_DEPTH_32S, 1);
  cvSetData(&m_RightIntegralIpl, &boost::gil::view(m_RightIntegral)(0, 0)[0], (char*) &boost::gil::view(m_RightIntegral)(0, 1)[0] - (char*) &boost::gil::view(m_RightIntegral)(0, 0)[0]);

  cvInitImageHeader(&m_LeftSquaredIntegralIpl, cvSize(m_LeftSquaredIntegral.width(), m_LeftSquaredIntegral.height()), IPL_DEPTH_64F, 1);
  cvSetData(&m_LeftSquaredIntegralIpl, &boost::gil::view(m_LeftSquaredIntegral)(0, 0)[0], (char*) &boost::gil::view(m_LeftSquaredIntegral)(0, 1)[0] - (char*) &boost::gil::view(m_LeftSquaredIntegral)(0, 0)[0]);
  cvInitImageHeader(&m_RightSquaredIntegralIpl, cvSize(m_RightSquaredIntegral.width(), m_RightSquaredIntegral.height()), IPL_DEPTH_64F, 1);
  cvSetData(&m_RightSquaredIntegralIpl, &boost::gil::view(m_RightSquaredIntegral)(0, 0)[0], (char*) &boost::gil::view(m_RightSquaredIntegral)(0, 1)[0] - (char*) &boost::gil::view(m_RightSquaredIntegral)(0, 0)[0]);

  // some defaults that seem to work
  m_PropagationParams.N = 2;
  m_PropagationParams.Ct = 0.6f;
  m_PropagationParams.WinSizeX = 9;
  m_PropagationParams.WinSizeY = 9;
  m_PropagationParams.Dg = 1;
  m_PropagationParams.Tt = 200;
  m_PropagationParams.BorderX = std::max(m_PropagationParams.WinSizeX, m_PropagationParams.WinSizeY) + m_PropagationParams.N + m_PropagationParams.Dg + 3;
  m_PropagationParams.BorderY = m_PropagationParams.BorderX;
}

//-----------------------------------------------------------------------------
SequentialCpuQds::~SequentialCpuQds()
{
  // lucky us, all buffers are managed by gil or stl, so nothing to do
}

//-----------------------------------------------------------------------------
int SequentialCpuQds::GetWidth() const
{
  return m_Width;
}

//-----------------------------------------------------------------------------
int SequentialCpuQds::GetHeight() const
{
  return m_Height;
}

//-----------------------------------------------------------------------------
CvPoint SequentialCpuQds::GetMatch(int x, int y) const
{
  if ((x < 0) || (y < 0) || (x >= GetWidth()) || (y >= GetHeight()))
    throw std::runtime_error("Ref coordinate out of bounds");

  const boost::gil::dev2n16c_pixel_t& r = boost::gil::const_view(m_LeftRefMap)(x, y);
  return cvPoint(r[0], r[1]);
}

//-----------------------------------------------------------------------------
IplImage* SequentialCpuQds::CreateDisparityImage() const
{
  IplImage* dispimg = cvCreateImage(cvSize(GetWidth(), GetHeight()), IPL_DEPTH_8U, 4);

  // gil view that wraps the ipl image
  auto dst = boost::gil::interleaved_view(dispimg->width, dispimg->height, (boost::gil::rgba8_pixel_t*) dispimg->imageData, dispimg->widthStep);

  for (int y = 0; y < dispimg->height; ++y)
  {
    for (int x = 0; x < dispimg->width; ++x)
    {
      // two-channel pixel in the refmap. values point into the right image.
      const boost::gil::dev2n16c_pixel_t& r = boost::gil::const_view(m_LeftRefMap)(x, y);

      // output rgba pixel
      auto & p = dst(x, y);

      if (r[0] <= 0)
      {
        p[0] = 255;
        p[1] = 0;
        p[2] = 0;
        p[3] = 255;
        continue;
      }

      float   dx = (float) (x - (int) r[0]);
      float   dy = (float) (y - (int) r[1]);
      float   d = std::sqrt(dx*dx + dy*dy);
      d = std::max(d, 0.0f);
      d = std::min(d, (float) m_MaxDisparity);

      float   sd = /*255 -*/ 255.0f * (std::min(d - 0, (float) m_MaxDisparity) / ((float) m_MaxDisparity - 0));
      p[0] = (unsigned char) sd;
      p[1] = (unsigned char) sd;
      p[2] = (unsigned char) sd;
      p[3] = 255;
    }
  }

  return dispimg;
}

//-----------------------------------------------------------------------------
void SequentialCpuQds::InitSparseFeatures()
{
  CvSize  templateSize = cvSize((m_LeftIpl.width / 20) | 0x1, (m_LeftIpl.width / 20) | 0x1);
  double  minSeparationDistance = m_LeftIpl.width / 20;

  m_SparseFeaturesLeft.resize(NUM_MAX_FEATURES);

  // Detect features in the left image
  // note: opencv ignores eigenimage and tempimage!
  int   foundNumFeatures = m_SparseFeaturesLeft.size();
  cvGoodFeaturesToTrack(&m_LeftIpl, 0, 0, &m_SparseFeaturesLeft[0], &foundNumFeatures, 0.01, minSeparationDistance);
  m_SparseFeaturesLeft.resize(foundNumFeatures);

  // lets say we expect horizontal disparity
  //  and guess that on average we have 30 pixels.
  // so lets help the LKT, otherwise it sometimes finds lots of dodgy matches
  m_SparseFeaturesRight = m_SparseFeaturesLeft;
  for (unsigned int i = 0; i < m_SparseFeaturesRight.size(); ++i)
  {
    //m_SparseFeaturesRight[i].x -= m_LeftIpl.width / 12;
  }

  m_FeatureStatus.resize(m_SparseFeaturesLeft.size());

  // match features from the current left frame to the current right frame
  cvCalcOpticalFlowPyrLK(&m_LeftIpl, &m_RightIpl, 0, 0,
      &m_SparseFeaturesLeft[0], &m_SparseFeaturesRight[0], foundNumFeatures,
      templateSize, 3, &m_FeatureStatus[0], 0,
      cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.0003), 0 | CV_LKFLOW_INITIAL_GUESSES);

#ifndef NDEBUG
  // help with debugging this
  // throw out all failed points, otherwise it's a mess trying to figure out which features were tracked and which were not
  for (int i = (int)m_SparseFeaturesLeft.size() - 1; i >= 0; --i)
  {
    if (m_FeatureStatus[i] == 0)
    {
      m_SparseFeaturesLeft.erase(m_SparseFeaturesLeft.begin() + i);
      m_SparseFeaturesRight.erase(m_SparseFeaturesRight.begin() + i);
      m_FeatureStatus.erase(m_FeatureStatus.begin() + i);
    }
  }
#endif
}

//-----------------------------------------------------------------------------
void SequentialCpuQds::QuasiDensePropagation()
{
  // we keep view objects around, for easier writing
  boost::gil::dev2n16_view_t      leftRef  = boost::gil::view(m_LeftRefMap);
  boost::gil::dev2n16_view_t      rightRef = boost::gil::view(m_RightRefMap);
  const boost::gil::gray8c_view_t leftTex  = boost::gil::const_view(m_LeftTexture);
  const boost::gil::gray8c_view_t rightTex = boost::gil::const_view(m_RightTexture);

  // Seed list
  std::priority_queue<Match, std::vector<Match>, std::less<Match> >   seeds;

  // Build a list of seeds from the starting features
  for (unsigned int i = 0; i < m_SparseFeaturesLeft.size(); i++)
  {
    if (m_FeatureStatus[i] != 0)
    {
      // Calculate correlation and store match in Seeds.
      Match m;
      // FIXME: check for negative coords! our guestimate in InitSparseFeatures() might have pushed these off
      m.p0 = RefPoint(static_cast<unsigned short>(m_SparseFeaturesLeft[i].x), static_cast<unsigned short>(m_SparseFeaturesLeft[i].y));
      m.p1 = RefPoint(static_cast<unsigned short>(m_SparseFeaturesRight[i].x), static_cast<unsigned short>(m_SparseFeaturesRight[i].y));

      // Check if too close to boundary.
      if (!CheckBorder(m, m_PropagationParams.BorderX, m_PropagationParams.BorderY, m_LeftImg.width(), m_LeftImg.height()))
      {
        continue;
      }

      // Calculate the correlation threshold
      m.corr = Zncc_C1(m.p0.x, m.p0.y, m.p1.x, m.p1.y, m_PropagationParams.WinSizeX,
                       boost::gil::const_view(m_LeftImg), boost::gil::const_view(m_RightImg),
                       boost::gil::const_view(m_LeftIntegral), boost::gil::const_view(m_RightIntegral),
                       boost::gil::const_view(m_LeftSquaredIntegral), boost::gil::const_view(m_RightSquaredIntegral));

      // Can we add it to the list
      if (m.corr > m_PropagationParams.Ct)
      {
        // FIXME: Check if this is unique (or assume it is due to prior supression)
        seeds.push(m);
        leftRef (m.p0.x, m.p0.y) = (boost::gil::dev2n16_pixel_t) m.p1;
        rightRef(m.p1.x, m.p1.y) = (boost::gil::dev2n16_pixel_t) m.p0;
      }
    }
  }

  // Do the propagation part
  while (!seeds.empty())
  {
    std::priority_queue<Match, std::vector<Match>, std::less<Match> >     localseeds;

    // Get the best seed at the moment
    Match m = seeds.top();
    seeds.pop();

    // Ignore the border
    if (!CheckBorder(m, m_PropagationParams.BorderX, m_PropagationParams.BorderY, m_LeftImg.width(), m_LeftImg.height()))
    {
      continue;
    }

    // For all neighbours in image 1
    for (int y = -m_PropagationParams.N; y <= m_PropagationParams.N; ++y)
    {
      for (int x = -m_PropagationParams.N; x <= m_PropagationParams.N; ++x)
      {
        RefPoint p0(m.p0.x + x, m.p0.y + y);

        // Check if its unique in ref
        if (leftRef(p0.x, p0.y)[0] != 0)
        {
          continue;
        }

        // Check the texture descriptor for a boundary
        if (leftTex(p0.x, p0.y) > m_PropagationParams.Tt)
        {
          continue;
        }

        // For all candidate matches.
        for (int wy = -m_PropagationParams.Dg; wy <= m_PropagationParams.Dg; ++wy)
        {
          for (int wx = -m_PropagationParams.Dg; wx <= m_PropagationParams.Dg; ++wx)
          {
            RefPoint p1(m.p1.x + x + wx, m.p1.y + y + wy);

            // Check if its unique in ref
            if (rightRef(p1.x, p1.y)[0] != 0)
            {
              continue;
            }

            // Check the texture descriptor for a boundary
            if (rightTex(p1.x, p1.y) > m_PropagationParams.Tt)
            {
              continue;
            }

            // Calculate ZNCC and store local match.
            float corr = Zncc_C1(p0.x, p0.y, p1.x, p1.y, m_PropagationParams.WinSizeX,
                            boost::gil::const_view(m_LeftImg), boost::gil::const_view(m_RightImg),
                            boost::gil::const_view(m_LeftIntegral), boost::gil::const_view(m_RightIntegral),
                            boost::gil::const_view(m_LeftSquaredIntegral), boost::gil::const_view(m_RightSquaredIntegral));

            // push back if this is valid match
            if (corr > m_PropagationParams.Ct)
            {
              Match nm;
              nm.p0 = p0;
              nm.p1 = p1;
              nm.corr = corr;
              localseeds.push(nm);
            }
          }
        }
      }
    }

    // Get seeds from the local
    while (!localseeds.empty())
    {
      Match lm = localseeds.top();
      localseeds.pop();

      // Check if its unique in both ref and dst.
      if (leftRef(lm.p0.x, lm.p0.y)[0] != 0)
      {
        continue;
      }
      if (rightRef(lm.p1.x, lm.p1.y)[0] != 0)
      {
        continue;
      }

      // Unique match
      leftRef (lm.p0.x, lm.p0.y) = (boost::gil::dev2n16_pixel_t) lm.p1;
      rightRef(lm.p1.x, lm.p1.y) = (boost::gil::dev2n16_pixel_t) lm.p0;

      // Add to the seed list
      seeds.push(lm);
    }
  } // while (global) seed list is not empty
}

//-----------------------------------------------------------------------------
void SequentialCpuQds::Process(const IplImage* left, const IplImage* right)
{
  assert(left  != 0);
  assert(right != 0);

  assert(left->nChannels == right->nChannels);
  assert(left->width  == right->width);
  assert(left->height == right->height);

  if ((left->width  != GetWidth()) ||
      (left->height != GetHeight()))
  {
    throw std::runtime_error("Image size does not match");
  }

  // the opencv channel layout is BGR by default
  // but lucky us, the greyscale conversion is not weighted
  // so it doesnt matter whether we say RGB or BGR.
  switch (left->nChannels)
  {
    case 3:
      cvCvtColor(left,  &m_LeftIpl,  CV_RGB2GRAY);
      cvCvtColor(right, &m_RightIpl, CV_RGB2GRAY);
      break;
    case 4:
      cvCvtColor(left,  &m_LeftIpl,  CV_RGBA2GRAY);
      cvCvtColor(right, &m_RightIpl, CV_RGBA2GRAY);
      break;

    default:
      throw std::runtime_error("Unknown image format");
  }

  // run an initial sparse feature matching step
  InitSparseFeatures();

  // build texture homogeneity reference maps.
  BuildTextureDescriptor(boost::gil::const_view(m_LeftImg),  boost::gil::view(m_LeftTexture));
  BuildTextureDescriptor(boost::gil::const_view(m_RightImg), boost::gil::view(m_RightTexture));

#ifdef DUMP_IMAGES
  {
    IplImage  ipl;
    cvInitImageHeader(&ipl, cvSize((int) m_LeftTexture.width(), (int) m_LeftTexture.height()), IPL_DEPTH_8U, 1, IPL_ORIGIN_TL, 4);
    cvSetData(&ipl, (void*) &boost::gil::view(m_LeftTexture)(0, 0), (char*) &boost::gil::const_view(m_LeftTexture)(0, 1) - (char*) &boost::gil::const_view(m_LeftTexture)(0, 0));
    cvSaveImage("sequentialqds-left-texture.png", &ipl);
  }
#endif

  // generate the integral images for fast variable window correlation calculations
  cvIntegral(&m_LeftIpl,  &m_LeftIntegralIpl,  &m_LeftSquaredIntegralIpl);
  cvIntegral(&m_RightIpl, &m_RightIntegralIpl, &m_RightSquaredIntegralIpl);

  // we need to reset these, so that we get a fresh stereo matching
  boost::gil::fill_pixels(boost::gil::view(m_LeftRefMap),  boost::gil::dev2n16_pixel_t(0, 0));
  boost::gil::fill_pixels(boost::gil::view(m_RightRefMap), boost::gil::dev2n16_pixel_t(0, 0));

  QuasiDensePropagation();
}

} // namespace


//
// Copyright (c) 2019-2021 Ruben Perez Hidalgo (rubenperez038 at gmail dot com)
//
// 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/mysql/detail/protocol/null_bitmap_traits.hpp"
#include "test_common.hpp"
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic/collection.hpp>
#include <array>

using boost::mysql::detail::null_bitmap_traits;
using boost::mysql::detail::stmt_execute_null_bitmap_offset;
using boost::mysql::detail::binary_row_null_bitmap_offset;
using namespace boost::unit_test;
using namespace boost::mysql::test;

BOOST_AUTO_TEST_SUITE(test_null_bitmap_traits)

// byte_count
struct byte_count_sample
{
    std::size_t offset;
    std::size_t num_fields;
    std::size_t expected_value;
};

std::ostream& operator<<(std::ostream& os, const byte_count_sample& val)
{
    return os << "(offset=" << val.offset << ", num_fields=" << val.num_fields << ")";
}

constexpr byte_count_sample all_byte_count_samples [] {
    { stmt_execute_null_bitmap_offset, 0, 0 },
    { stmt_execute_null_bitmap_offset, 1, 1 },
    { stmt_execute_null_bitmap_offset, 2, 1 },
    { stmt_execute_null_bitmap_offset, 3, 1 },
    { stmt_execute_null_bitmap_offset, 4, 1 },
    { stmt_execute_null_bitmap_offset, 5, 1 },
    { stmt_execute_null_bitmap_offset, 6, 1 },
    { stmt_execute_null_bitmap_offset, 7, 1 },
    { stmt_execute_null_bitmap_offset, 8, 1 },
    { stmt_execute_null_bitmap_offset, 9, 2 },
    { stmt_execute_null_bitmap_offset, 10, 2 },
    { stmt_execute_null_bitmap_offset, 11, 2 },
    { stmt_execute_null_bitmap_offset, 12, 2 },
    { stmt_execute_null_bitmap_offset, 13, 2 },
    { stmt_execute_null_bitmap_offset, 14, 2 },
    { stmt_execute_null_bitmap_offset, 15, 2 },
    { stmt_execute_null_bitmap_offset, 16, 2 },
    { stmt_execute_null_bitmap_offset, 17, 3 },

    { binary_row_null_bitmap_offset, 0, 1 },
    { binary_row_null_bitmap_offset, 1, 1 },
    { binary_row_null_bitmap_offset, 2, 1 },
    { binary_row_null_bitmap_offset, 3, 1 },
    { binary_row_null_bitmap_offset, 4, 1 },
    { binary_row_null_bitmap_offset, 5, 1 },
    { binary_row_null_bitmap_offset, 6, 1 },
    { binary_row_null_bitmap_offset, 7, 2 },
    { binary_row_null_bitmap_offset, 8, 2 },
    { binary_row_null_bitmap_offset, 9, 2 },
    { binary_row_null_bitmap_offset, 10, 2 },
    { binary_row_null_bitmap_offset, 11, 2 },
    { binary_row_null_bitmap_offset, 12, 2 },
    { binary_row_null_bitmap_offset, 13, 2 },
    { binary_row_null_bitmap_offset, 14, 2 },
    { binary_row_null_bitmap_offset, 15, 3 },
    { binary_row_null_bitmap_offset, 16, 3 },
    { binary_row_null_bitmap_offset, 17, 3 },
};

BOOST_DATA_TEST_CASE(byte_count, data::make(all_byte_count_samples))
{
    null_bitmap_traits traits (sample.offset, sample.num_fields);
    BOOST_TEST(traits.byte_count() == sample.expected_value);
}

// is_null
struct is_null_sample
{
    std::size_t offset;
    std::size_t pos;
    bool expected;
};

std::ostream& operator<<(std::ostream& os, const is_null_sample& value)
{
    return os << "(offset=" << value.offset << ", pos=" << value.pos << ")";
}

constexpr is_null_sample all_is_null_samples [] {
    { stmt_execute_null_bitmap_offset, 0, false },
    { stmt_execute_null_bitmap_offset, 1, false },
    { stmt_execute_null_bitmap_offset, 2, true },
    { stmt_execute_null_bitmap_offset, 3, false },
    { stmt_execute_null_bitmap_offset, 4, true },
    { stmt_execute_null_bitmap_offset, 5, true },
    { stmt_execute_null_bitmap_offset, 6, false },
    { stmt_execute_null_bitmap_offset, 7, true },
    { stmt_execute_null_bitmap_offset, 8, true },
    { stmt_execute_null_bitmap_offset, 9, true },
    { stmt_execute_null_bitmap_offset, 10, true },
    { stmt_execute_null_bitmap_offset, 11, true },
    { stmt_execute_null_bitmap_offset, 12, true },
    { stmt_execute_null_bitmap_offset, 13, true },
    { stmt_execute_null_bitmap_offset, 14, true },
    { stmt_execute_null_bitmap_offset, 15, true },
    { stmt_execute_null_bitmap_offset, 16, false },

    { binary_row_null_bitmap_offset, 0, true },
    { binary_row_null_bitmap_offset, 1, false },
    { binary_row_null_bitmap_offset, 2, true },
    { binary_row_null_bitmap_offset, 3, true },
    { binary_row_null_bitmap_offset, 4, false },
    { binary_row_null_bitmap_offset, 5, true },
    { binary_row_null_bitmap_offset, 6, true },
    { binary_row_null_bitmap_offset, 7, true },
    { binary_row_null_bitmap_offset, 8, true },
    { binary_row_null_bitmap_offset, 9, true },
    { binary_row_null_bitmap_offset, 10, true },
    { binary_row_null_bitmap_offset, 11, true },
    { binary_row_null_bitmap_offset, 12, true },
    { binary_row_null_bitmap_offset, 13, true },
    { binary_row_null_bitmap_offset, 14, false },
    { binary_row_null_bitmap_offset, 15, false },
    { binary_row_null_bitmap_offset, 16, false },
};

BOOST_DATA_TEST_CASE(is_null, data::make(all_is_null_samples))
{
    std::array<std::uint8_t, 3> content { 0xb4, 0xff, 0x00 }; // 0b10110100, 0b11111111, 0b00000000
    null_bitmap_traits traits (sample.offset, 17); // 17 fields
    bool actual = traits.is_null(content.data(), sample.pos);
    BOOST_TEST(actual == sample.expected);
}

BOOST_AUTO_TEST_CASE(is_null_one_field_stmt_execute_first_bit_zero)
{
    std::uint8_t value = 0x00;
    null_bitmap_traits traits (stmt_execute_null_bitmap_offset, 1);
    BOOST_TEST(!traits.is_null(&value, 0));
}

BOOST_AUTO_TEST_CASE(is_null_one_field_stmt_execute_first_bit_one)
{
    std::uint8_t value = 0x01;
    null_bitmap_traits traits (stmt_execute_null_bitmap_offset, 1);
    BOOST_TEST(traits.is_null(&value, 0));
}

BOOST_AUTO_TEST_CASE(is_null_one_field_binary_row_third_bit_zero)
{
    std::uint8_t value = 0x00;
    null_bitmap_traits traits (binary_row_null_bitmap_offset, 1);
    BOOST_TEST(!traits.is_null(&value, 0));
}

BOOST_AUTO_TEST_CASE(is_null_one_field_binary_row_third_bit_one)
{
    std::uint8_t value = 0x04; // 0b00000100
    null_bitmap_traits traits (binary_row_null_bitmap_offset, 1);
    BOOST_TEST(traits.is_null(&value, 0));
}

// set_null
struct set_null_sample
{
    std::size_t offset;
    std::size_t pos;
    std::array<std::uint8_t, 3> expected;

    constexpr set_null_sample(
        std::size_t offset,
        std::size_t pos,
        const std::array<std::uint8_t, 3>& expected
    ) :
        offset(offset),
        pos(pos),
        expected(expected)
    {
    };
};

std::ostream& operator<<(std::ostream& os, const set_null_sample& value)
{
    return os << "(offset=" << value.offset << ", pos=" << value.pos << ")";
}

constexpr set_null_sample all_set_null_samples [] {
    { stmt_execute_null_bitmap_offset, 0, {0x01, 0, 0} },
    { stmt_execute_null_bitmap_offset, 1, {0x02, 0, 0} },
    { stmt_execute_null_bitmap_offset, 2, {0x04, 0, 0} },
    { stmt_execute_null_bitmap_offset, 3, {0x08, 0, 0} },
    { stmt_execute_null_bitmap_offset, 4, {0x10, 0, 0} },
    { stmt_execute_null_bitmap_offset, 5, {0x20, 0, 0} },
    { stmt_execute_null_bitmap_offset, 6, {0x40, 0, 0} },
    { stmt_execute_null_bitmap_offset, 7, {0x80, 0, 0} },
    { stmt_execute_null_bitmap_offset, 8, {0,  0x01, 0} },
    { stmt_execute_null_bitmap_offset, 9, {0,  0x02, 0} },
    { stmt_execute_null_bitmap_offset, 10, {0, 0x04, 0} },
    { stmt_execute_null_bitmap_offset, 11, {0, 0x08, 0} },
    { stmt_execute_null_bitmap_offset, 12, {0, 0x10, 0} },
    { stmt_execute_null_bitmap_offset, 13, {0, 0x20, 0} },
    { stmt_execute_null_bitmap_offset, 14, {0, 0x40, 0} },
    { stmt_execute_null_bitmap_offset, 15, {0, 0x80, 0} },
    { stmt_execute_null_bitmap_offset, 16, {0, 0, 0x01} },

    { binary_row_null_bitmap_offset, 0, {0x04, 0, 0} },
    { binary_row_null_bitmap_offset, 1, {0x08, 0, 0} },
    { binary_row_null_bitmap_offset, 2, {0x10, 0, 0} },
    { binary_row_null_bitmap_offset, 3, {0x20, 0, 0} },
    { binary_row_null_bitmap_offset, 4, {0x40, 0, 0} },
    { binary_row_null_bitmap_offset, 5, {0x80, 0, 0} },
    { binary_row_null_bitmap_offset, 6,  {0,  0x01, 0} },
    { binary_row_null_bitmap_offset, 7,  {0,  0x02, 0} },
    { binary_row_null_bitmap_offset, 8,  {0,  0x04, 0} },
    { binary_row_null_bitmap_offset, 9,  {0,  0x08, 0} },
    { binary_row_null_bitmap_offset, 10, {0,  0x10, 0} },
    { binary_row_null_bitmap_offset, 11, {0,  0x20, 0} },
    { binary_row_null_bitmap_offset, 12, {0,  0x40, 0} },
    { binary_row_null_bitmap_offset, 13, {0,  0x80, 0} },
    { binary_row_null_bitmap_offset, 14, {0, 0, 0x01} },
    { binary_row_null_bitmap_offset, 15, {0, 0, 0x02} },
    { binary_row_null_bitmap_offset, 16, {0, 0, 0x04} },
};

BOOST_DATA_TEST_CASE(set_null, data::make(all_set_null_samples))
{
    std::array<std::uint8_t, 4> expected_buffer {}; // help detect buffer overruns
    std::memcpy(expected_buffer.data(), sample.expected.data(), 3);
    std::array<std::uint8_t, 4> actual_buffer {};
    null_bitmap_traits traits (sample.offset, 17); // 17 fields
    traits.set_null(actual_buffer.data(), sample.pos);
    BOOST_TEST(expected_buffer == actual_buffer);
}

BOOST_AUTO_TEST_CASE(set_null_one_field_stmt_execute)
{
    std::uint8_t value = 0;
    null_bitmap_traits traits (stmt_execute_null_bitmap_offset, 1);
    traits.set_null(&value, 0);
    BOOST_TEST(value == 1);
}

BOOST_AUTO_TEST_CASE(set_null_one_field_binary_row)
{
    std::uint8_t value = 0;
    null_bitmap_traits traits (binary_row_null_bitmap_offset, 1);
    traits.set_null(&value, 0);
    BOOST_TEST(value == 4);
}

BOOST_AUTO_TEST_CASE(set_null_multified_stmt_execute)
{
    std::array<std::uint8_t, 4> expected_buffer { 0xb4, 0xff, 0x00, 0x00 };
    std::array<std::uint8_t, 4> actual_buffer {};
    null_bitmap_traits traits (stmt_execute_null_bitmap_offset, 17); // 17 fields
    for (std::size_t pos: {2, 4, 5, 7, 8, 9, 10, 11, 12, 13, 14, 15})
    {
        traits.set_null(actual_buffer.data(), pos);
    }
    BOOST_TEST(expected_buffer == actual_buffer);
}

BOOST_AUTO_TEST_CASE(set_null_multified_binary_row)
{
    std::array<std::uint8_t, 4> expected_buffer { 0xb4, 0xff, 0x00, 0x00 };
    std::array<std::uint8_t, 4> actual_buffer {};
    null_bitmap_traits traits (binary_row_null_bitmap_offset, 17); // 17 fields
    for (std::size_t pos: {0, 2, 3, 5, 6, 7, 8, 9, 10, 11, 12, 13})
    {
        traits.set_null(actual_buffer.data(), pos);
    }
    BOOST_TEST(expected_buffer == actual_buffer);
}

BOOST_AUTO_TEST_SUITE_END() // test_null_bitmap_traits


#include <iostream>
#include <typeinfo>
#include "DNest4.h"
#include "Data.h"
#include "Model.h"
#include "ConditionalPrior.h"
#include <Eigen/Core>

using namespace std;
using namespace DNest4;

#include "default_priors.h"

const bool obs_after_HARPS_fibers = false;
const bool GP = false;
const bool hyperpriors = false;
const bool trend = false;

Model::Model()
:planets(5, 0, true, PlanetConditionalPrior())
,active_regions(3, 1, true, SOAPConditionalPrior())
,mu(Data::get_instance().get_t().size())
,C(Data::get_instance().get_t().size(), Data::get_instance().get_t().size())
,star() //erijiriejr
,ccf(),ccf_active_region()
{
    double ymin = Data::get_instance().get_y_min();
    double ymax = Data::get_instance().get_y_max();
    double topslope = Data::get_instance().topslope();

    Cprior = new Uniform(ymin, ymax);
    if(trend)
    	slope_prior = new Uniform(-topslope, topslope);

    star.prot = 25.05;

    Eigen::MatrixXd c = Data::get_instance().get_ccfdata();
    //cout << c.rows() << "x" << c.cols() << endl;

    initialize_star_quiet();

}

int main(int argc, char** argv)
{
	Data::get_instance().load("BL2009_dataset1.kms.rv", "kms");

	Sampler<Model> sampler = setup<Model>(argc, argv);
	//sampler.run();

	return 0;
}