#ifndef _PATHFINDINGUTILS_UTILS_HEADER__
#define _PATHFINDINGUTILS_UTILS_HEADER__

#include <boost/filesystem.hpp>
#include <cstdlib>

#include <cpp-utils/adjacentGraph.hpp>
#include <cpp-utils/functional.hpp>
#include <cpp-utils/igraph.hpp>
#include <cpp-utils/SetPlus.hpp>
#include <cpp-utils/serializers.hpp>

#include "GridMap.hpp"
#include "GridMapImage.hpp"
#include "DijkstraSearchAlgorithm.hpp"
#include "NodePath.hpp"
#include "statevisited_e.hpp"

namespace pathfinding::utils {

    using namespace cpp_utils;
    using namespace cpp_utils::graphs;
    using namespace pathfinding::maps;
    using namespace pathfinding::search;

	/**
	 * @brief generate a path from a search state
	 *
	 * @tparam implementation of a ISearchState
	 * @param mapper function converting state into a node over a graph
	 * @return NodePath
	 */
	template <typename STATE>
	NodePath getNodePath(const STATE& state, const function_t<STATE, nodeid_t>& mapper) {
		NodePath result{};
		auto tmp = &state;
		debug("getNodePath is", tmp);
		while (tmp != nullptr) {
			debug("getNodePath is", tmp);
			result.add(mapper(*tmp));
			debug("mapper is", mapper(*tmp));
			tmp = tmp->getParent();
		}
		return result;
	}

    /**
     * @brief get the optimal path over a graph in a simplistic way as **a sequence of vertices**
     *
     * If start == goal the sequence generated has size 1
     *
     * @tparam G type of a paylaod associated to the graph
     * @tparam V type of a payload associated to every vertex
     * @tparam E type of a payload associated to every edge
     * @param graph the graph where to compute the optimal path
     * @param start start of the optimal path
     * @param goal goal of the optimal path
     * @param mapper the function to use to convert E to cost_t
     * @return NodePath an optimal path from @c start to @c goal
     */
    template <typename G, typename V, typename E>
	NodePath getOptimalPathAsVertices(const IImmutableGraph<G, V, E>& graph, nodeid_t start, nodeid_t goal, std::function<cost_t(const E&)> mapper) {
		DijkstraSearchAlgorithm<G, V, E> dijkstra{graph, mapper};
		std::unique_ptr<ISolutionPath<nodeid_t, nodeid_t, nodeid_t>> path = ::std::move(dijkstra.search(start, goal));
		NodePath result{*path};
        finer("path is ", *path, path.get());
        finest("done building path with NodePath, which ", result, &result);
        return result;
	}

    /**
     * @brief get the optimal path over a graph in a simplistic way as **a sequence of edges**
     *
     * If start == goal the sequence generated has size 0
     *
     * @tparam E
     * @param graph
     * @param start
     * @param goal
     * @return vectorplus<Edge<E>>
     */
	template <typename G, typename V, typename E>
	cpp_utils::vectorplus<Edge<E>> getOptimalPathAsEdges(const IImmutableGraph<G, V, E>& graph, nodeid_t start, nodeid_t goal, const std::function<cost_t(const E&)> costFunction) {
		DijkstraSearchAlgorithm<G, V, E> dijkstra{graph, costFunction};
		auto path = dijkstra.search(start, goal);

		cpp_utils::vectorplus<Edge<E>> result{};
		if (path.isEmpty()) {
			return result;
		} else if (path.size() == 1) {
			return result;
		} else {
			for (int i=0; i<(path.size() - 1); ++i) {
				result.add(Edge<E>{path[i], path[i+1], graph.getEdge(path[i], path[i+1])});
			}
			return result;
		}
	}

    /**
     * @brief Loads the perturbated graph starting from the perturbations saved into a file
     *
     * @tparam G the type of the paylaod of the whole graph
     * @tparam V the type of the payload of each vertex
     * @tparam E the type of the edges to load from the file
     * @param path path where the perturbated edges are stored
     * @param originalGraph the original graph before the perturbations
     * @param mapper a function that map edge cost_t in the @c originalGraph into
     * @return AdjacentGraph<G, V, E>* a graph containing the perturbatations. This needs to be freed manually
     */
    template <typename G, typename V, typename E>
    IImmutableGraph<G, V, E>* loadPerturbatedMap(const boost::filesystem::path& path, const IImmutableGraph<G, V, cost_t>& originalGraph, const cpp_utils::function_t<cost_t, E>& mapper) {
        info("perturbated graph file should be located in", path);

        if (!boost::filesystem::exists(path)) {
            throw cpp_utils::exceptions::FileOpeningException{path};
        }

		auto resultTmp = std::unique_ptr<IImmutableGraph<G,V,E>>{originalGraph.mapEdges(mapper)};
        AdjacentGraph<G,V,E>* result = new AdjacentGraph<G, V, E>{*resultTmp};

        FILE* f = fopen(path.native().c_str(), "rb");
        SetPlus<Edge<E>> perturbatedEdges{};
        cpp_utils::serializers::loadFromFile(f, perturbatedEdges);
        fclose(f);

        //apply the new perturbated edges
        for (auto perturbatedEdge : perturbatedEdges) {
            result->changeWeightUndirectedEdge(
                perturbatedEdge.getSourceId(),
                perturbatedEdge.getSinkId(),
                perturbatedEdge.getPayload()
            );
        }

        return result;
    }

    /**
	 * @brief Alters an image (assumed to be a GridMapImage) to add all the perturbations present in `perturbatedGraph`
	 *
	 * @tparam G
	 * @tparam V
	 * @tparam E
	 * @param gridmap
	 * @param baseMapGraph
	 * @param perturbatedGraph
	 * @param worseEdgeColor
	 * @param betterEdgeColor
	 * @return GridMapImage& the @c image altered
	 */
	template <typename G, typename V, typename E>
	PPMImage& addPerturbationsOnMap(PPMImage& image, const IPathFindingMap& map, const IImmutableGraph<G, V, cost_t>& baseMapGraph, const IImmutableGraph<G, V, E>& perturbatedGraph, color_t worseEdgeColor, color_t betterEdgeColor, const std::function<cost_t(const E&)>& costFunction) {
		GridMapImage& gridMapImage = static_cast<GridMapImage&>(image);
		const GridMap& gridMap = static_cast<const GridMap&>(map);

		for (auto it=perturbatedGraph.beginEdges(); it!=perturbatedGraph.endEdges(); ++it) {
			if (!it->getPayload().isPerturbated()) {
				continue;
			}
			xyLoc source = perturbatedGraph.getVertex(it->getSourceId());
			xyLoc sink = perturbatedGraph.getVertex(it->getSinkId());
			int pixel1x, pixel1y, pixel2x, pixel2y;
			switch (xyLoc::getDirection(source, sink)) {
				case Direction::SOUTHEAST:  pixel1x=2; pixel1y=2; pixel2x=0; pixel2y=0; break;
				case Direction::EAST:       pixel1x=2; pixel1y=1; pixel2x=0; pixel2y=1; break;
				case Direction::NORTHEAST:  pixel1x=2; pixel1y=0; pixel2x=0; pixel2y=2; break;

				case Direction::SOUTHWEST:  pixel1x=0; pixel1y=2; pixel2x=2; pixel2y=0; break;
				case Direction::WEST:       pixel1x=0; pixel1y=1; pixel2x=2; pixel2y=1; break;
				case Direction::NORTHWEST:  pixel1x=0; pixel1y=0; pixel2x=2; pixel2y=2; break;

				case Direction::SOUTH:      pixel1x=1; pixel1y=2; pixel2x=1; pixel2y=0; break;
				case Direction::NORTH:      pixel1x=1; pixel1y=0; pixel2x=1; pixel2y=2; break;
				default:
					throw cpp_utils::exceptions::makeImpossibleException("invalid direction!");
			}
			cost_t oldWeight = baseMapGraph.getEdge(it->getSourceId(), it->getSinkId());
			cost_t newWeight = costFunction(perturbatedGraph.getEdge(it->getSourceId(), it->getSinkId()));
			color_t perturbatedColor;
			if (newWeight > oldWeight) {
				//cost has increased
				perturbatedColor = worseEdgeColor;
			} else {
				//cost has decreased
				perturbatedColor = betterEdgeColor;
			}
			gridMapImage.setPixelInGrid(source.x, source.y, pixel1x, pixel1y, perturbatedColor);
			gridMapImage.setPixelInGrid(sink.x, sink.y, pixel2x, pixel2y, perturbatedColor);
		}

		return gridMapImage;
	}

	/**
	 * @brief update the @c image by coloring the states which have been expanded/generated
	 *
	 * @tparam G1 type of the payload of the graph representig the map where we're searching
	 * @tparam V1 type of the payload of eachv vertex representig the map where we're searching
	 * @tparam E1 type of the payload of each edge representig the map where we're searching
	 * @tparam G2 type of the payload of the graph representig the map storing the state of each node
	 * @tparam V2 type of the payload of each vertex representig the map storing the state of each node
	 * @tparam E2 type of the payload of each edge representig the map storing the state of each node
	 * @param image the image to update
	 * @param map the actual map where we are searching
	 * @param graphMap the repersentation of the graph where we are searching
	 * @param stateGraph the graph we use to fetch the state of each vertex
	 * @param mapper function converting V2 into states
	 * @param unvisitedColor color to use to color the vertex we haven't even visited
	 * @param expandedColor color to use to colro the vertices we have expanded at least once
	 * @param generatedColor color to use to colro the vertices we have generated but not expanded
	 * @return PPMImage& the updated image
	 */
	template <typename G1, typename V1, typename E1, typename G2, typename V2, typename E2>
	PPMImage& addExpandedNodesInImage(PPMImage& image, const IPathFindingMap& map, const IImmutableGraph<G1,V1,E1>& graphMap, const IImmutableGraph<G2,V2,E2>& stateGraph, const function_t<V2, statevisited_e>& mapper, const color_t& unvisitedColor, const color_t& expandedColor, const color_t& generatedColor) {
		GridMapImage& gridMapImage = static_cast<GridMapImage&>(image);
		const GridMap& gridMap = static_cast<const GridMap&>(map);

		//now add to the map the expanded nodes
		for (auto it=stateGraph.beginVertices(); it!=stateGraph.endVertices(); ++it) {
			auto data = mapper(it->second);
			xyLoc loc = graphMap.getVertex(it->first);
			if (data == statevisited_e::UNVISITED) {
				//do nothing;
			} else if (data == statevisited_e::GENERATED) {
				gridMapImage.lerpGridCellColor(loc, generatedColor);
			} else if (data == statevisited_e::EXPANDED) {
				gridMapImage.lerpGridCellColor(loc, expandedColor);
			} else {
				throw cpp_utils::exceptions::InvalidScenarioException{"state visited", data};
			}
		}

		return gridMapImage;
	}

	/**
	 * @brief add
	 *
	 * @tparam G
	 * @tparam V
	 * @tparam E
	 * @param image
	 * @param map
	 * @param graph
	 * @param path
	 * @param path
	 * @param start
	 * @param goal
	 * @return PPMImage&
	 */
	template <typename G, typename V, typename E>
	PPMImage& addPathInImage(PPMImage& image, const IPathFindingMap& map, const IImmutableGraph<G,V,E>& graph, const NodePath& path, const color_t& pathColor, const color_t& startColor, const color_t& goalColor) {
		GridMapImage& gridMapImage = static_cast<GridMapImage&>(image);
		const GridMap& gridMap = static_cast<const GridMap&>(map);

		color_t c{};
		critical("putting path of size", path.size());
		for (int i=0; i<path.size(); ++i) {
			if (i == 0) {
				c = startColor;
			} else if (i == path.size()) {
				c = goalColor;
			} else {
				c = pathColor;
			}
			xyLoc loc = graph.getVertex(path.at(i));
			gridMapImage.setGridCellColor(loc, c);
		}
		return gridMapImage;
	}
}

#endif


/**
 *  file HadamardFeedbackDelayNetworkFilter.cpp
 */

#include <ATK/Delay/FeedbackDelayNetworkFilter.h>
#include <ATK/Delay/HadamardMixture.h>

#ifndef EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT

#include <array>
#include <fstream>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

const std::size_t PROCESSSIZE = 1024*64;

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_linel100_delay50_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(0, 50);
  filter.set_ingain(0, 1);
  filter.set_outgain(0, 1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(HadamardFeedbackDelayNetworkFilter_sinus_line100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(0, 50);
  filter.set_ingain(0, 1);
  filter.set_outgain(0, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(HadamardFeedbackDelayNetworkFilter_sinus_line1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(0, 50);
  filter.set_ingain(0, 1);
  filter.set_outgain(0, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_liner100_delay50_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(1, 50);
  filter.set_ingain(1, 1);
  filter.set_outgain(1, 1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(HadamardFeedbackDelayNetworkFilter_sinus_liner100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(1, 50);
  filter.set_ingain(1, 1);
  filter.set_outgain(1, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(HadamardFeedbackDelayNetworkFilter_sinus_liner1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(1, 50);
  filter.set_ingain(1, 1);
  filter.set_outgain(1, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}

const std::size_t OTHERPROCESSSIZE = 9600;

BOOST_AUTO_TEST_CASE(HadamardFeedbackDelayNetworkFilter_sinus_complex_test)
{
  std::vector<double> data(OTHERPROCESSSIZE);
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_hadamard.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(data.data()), OTHERPROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, OTHERPROCESSSIZE, false);
  generator.set_output_sampling_rate(96000);

  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 2>> filter(100000);
  filter.set_input_sampling_rate(96000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(0, 4800);
  filter.set_delay(1, 3600);
  filter.set_delay(2, 2400);
  filter.set_delay(3, 1200);
  filter.set_ingain(0, 1);
  filter.set_ingain(1, .2);
  filter.set_ingain(2, 1);
  filter.set_ingain(3, 1);
  filter.set_outgain(0, 1);
  filter.set_outgain(1, 1);
  filter.set_outgain(2, 1);
  filter.set_outgain(3, .5);
  filter.set_feedback(0, .2);
  filter.set_feedback(1, .8);
  filter.set_feedback(2, .4);
  filter.set_feedback(3, .2);

  filter.process(OTHERPROCESSSIZE);

  std::vector<double> outdata(OTHERPROCESSSIZE);
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_hadamard.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(outdata.data()), OTHERPROCESSSIZE * sizeof(double));
  }

  for (unsigned int i = 0; i < OTHERPROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(outdata[i], filter.get_output_array(0)[i], 0.0001);
  }
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_delay_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  filter.set_delay(0, 10);
  BOOST_CHECK_EQUAL(filter.get_delay(0), 10);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_delay_range_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  BOOST_CHECK_THROW(filter.set_delay(0, 0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_delay_range2_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  BOOST_CHECK_THROW(filter.set_delay(0, 128), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_ingain_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  filter.set_ingain(0, 0.5);
  BOOST_CHECK_EQUAL(filter.get_ingain(0), 0.5);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_outgain_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  filter.set_outgain(0, 0.5);
  BOOST_CHECK_EQUAL(filter.get_outgain(0), 0.5);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_feedback_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  filter.set_feedback(0, 0.5);
  BOOST_CHECK_EQUAL(filter.get_feedback(0), 0.5);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_feedback_range_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  BOOST_CHECK_THROW(filter.set_feedback(0, 1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( HadamardFeedbackDelayNetworkFilter_sinus_feedback_range2_test )
{
  ATK::FeedbackDelayNetworkFilter<ATK::HadamardMixture<double, 3>> filter(128);
  BOOST_CHECK_THROW(filter.set_feedback(0, -1), ATK::RuntimeError);
}

#endif


#pragma once

#include <deal.II/base/tensor.h>
#include <complex>

namespace boltzmann {
namespace helper {

template <int dim>
dealii::Tensor<2, dim, std::complex<double> >
make_complex_tensor(const dealii::Tensor<2, dim, double>& tensor)
{
  dealii::Tensor<2, dim, std::complex<double> > ret;
  for (int i = 0; i < dim; ++i)
    for (int j = 0; j < dim; ++j) {
      ret[i][j] = std::complex<double>(tensor[i][j], 0.);
    }
  return ret;
}

template <int dim>
dealii::Tensor<1, dim, std::complex<double> >
make_complex_tensor(const dealii::Tensor<1, dim, double>& tensor)
{
  dealii::Tensor<1, dim, std::complex<double> > ret;
  for (int i = 0; i < dim; ++i) ret[i] = std::complex<double>(tensor[i], 0.);
  return ret;
}

template <int dim>
dealii::Tensor<0, dim, std::complex<double> >
make_complex_tensor(const dealii::Tensor<0, dim, double>& tensor)
{
  dealii::Tensor<0, dim, std::complex<double> > ret;
  ret = std::complex<double>(tensor, 0.);
  return ret;
}

std::complex<double>
make_complex_tensor(double a)
{
  return std::complex<double>(a, 0);
}

}  // end namespace helper

}  // end namespace Namespace


#ifndef TEXTURE
#define TEXTURE

//#define DEBUG_TRUE

#include "my_png.hpp"
#include "surface_recon.hpp"
#include <math.h>
#include <Eigen/Dense>
#include <ros/ros.h>

struct uv_map {
    double x, y;

    uv_map() {}

    uv_map(double _x, double _y) {
        x = _x;
        y = _y;
    }
};

struct face {

    size_t v1, v2, v3;
    uv_map uv1, uv2, uv3;
    double area;

    face(size_t _v1, size_t _v2, size_t _v3) {
        v1 = _v1;
        v2 = _v2;
        v3 = _v3;

        uv1 = uv_map(0, 0);
        uv2 = uv_map(0, 0);
        uv3 = uv_map(0, 0);
    }

};

struct vertex {
    double x, y, z;

    vertex(double _x, double _y, double _z) {
        x = _x;
        y = _y;
        z = _z;
    }
};

class texture {

    Tree &tree; //CGAL KD-tree to bake textures
    std::vector<Color> &colors;

    std::string output_path;
    std::string base_path;
    my_png img;
    int texture_size;
    vector<vertex> vertices;
    vector<face> faces;
    vector<uv_map> uv_coords;
    size_t uv_idx;

public:

    texture(Tree &tree, std::vector<Color> &colors, std::string output_path, int texture_size, std::string base_path) :
    tree(tree), colors(colors), output_path(output_path), texture_size(texture_size), base_path(base_path){
        img = my_png(texture_size, texture_size); //initialize image for texture
        uv_idx = 0;
        read_off(base_path + "temp3.off");
    }

    void build_png();

    void draw_triangle_up(int x, int y, int t, unsigned char rgb[], int sep);

    void draw_triangle_down(int x, int y, int t, unsigned char rgb[], int sep);

    void draw_quad_and_parametrize(int x, int y, int t, int n);

    void save_obj();

    void read_off(string filename);

    double face_area(const face &f);

    bool sort_face_by_area(const face &f1, const face &f2);

    void idw_interpolation(double xyz[], unsigned char rgb[]);

};

#endif


// Copyright (c) 2017, University of Minnesota
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef ADMM_LINEARSOLVER_H
#define ADMM_LINEARSOLVER_H

#include "SolverLog.hpp"
#include <Eigen/SparseCholesky>
#include <unordered_map>
#include <memory>
#ifdef EIGEN_USE_MKL_VML
#include <Eigen/PardisoSupport>
#endif

namespace admm {

//
//	Linear solver base class
//
class LinearSolver {
public:
	typedef Eigen::Matrix<double,3,1> Vec3;
	typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;
	typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMat;

	SolverLog logger;

	// Updates solver data
	virtual void update_system( const SparseMat &A_ ) = 0;

	// Solve Ax=b subject to constraints
	virtual int solve( VecX &x, const VecX &b ) = 0;

	// Helper class for avoiding divide-by-zero
	static bool is_zero(double x){ return std::abs(x)<std::numeric_limits<double>::min(); }

}; // end class linear solver

//
//	Classic LDLT solver that does not handle constraints (SCA 2016)
//
class LDLTSolver : public LinearSolver {
public:
	typedef Eigen::Matrix<double,3,1> Vec3;
	typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;
	typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMat;

	#ifdef EIGEN_USE_MKL_VML
	typedef Eigen::PardisoLDLT< Eigen::SparseMatrix<double> > Cholesky;
	#else
	typedef Eigen::SimplicialLDLT< Eigen::SparseMatrix<double> > Cholesky;
	#endif

	SparseMat A;
	std::unique_ptr<Cholesky> m_cholesky;

	LDLTSolver(){
		m_cholesky = std::unique_ptr<Cholesky>( new Cholesky() );
	}

	// Does a cholesky factorization on the system matrix
	void update_system( const SparseMat &A_ ){
		int dim = A_.rows();
		if( dim != A_.cols() || dim == 0 ){ throw std::runtime_error("**LDLTSolver Error: Bad dimensions in A"); }
		A = A_;
		m_cholesky->compute(A);
	}

	// Solves for x given A, linear constraints (C), and pinning subspace (P)
	int solve( VecX &x, const VecX &b0 ){
		x = m_cholesky->solve(b0);
		return 1;
	}

}; // end class linear solver

} // ns admm

#endif


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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 <Eigen/Dense>
#include <iostream>
#include <memory>

#include "open3d/Open3D.h"

using namespace open3d;

void PrintHelp() {
    using namespace open3d;

    PrintOpen3DVersion();
    // clang-format off
    utility::LogInfo("Usage:");
    utility::LogInfo(">    RegistrationColoredICP source_pcd target_pcd [--visualize]");
    // clang-format on
    utility::LogInfo("");
}

void VisualizeRegistration(const open3d::geometry::PointCloud &source,
                           const open3d::geometry::PointCloud &target,
                           const Eigen::Matrix4d &Transformation) {
    std::shared_ptr<geometry::PointCloud> source_transformed_ptr(
            new geometry::PointCloud);
    std::shared_ptr<geometry::PointCloud> target_ptr(new geometry::PointCloud);
    *source_transformed_ptr = source;
    *target_ptr = target;
    source_transformed_ptr->Transform(Transformation);
    visualization::DrawGeometries({source_transformed_ptr, target_ptr},
                                  "Registration result");
}

int main(int argc, char *argv[]) {
    using namespace open3d;

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

    if (argc < 3) {
        PrintHelp();
        return 1;
    }

    bool visualize = false;
    if (utility::ProgramOptionExists(argc, argv, "--visualize")) {
        visualize = true;
    }

    // Prepare input
    std::shared_ptr<geometry::PointCloud> source =
            open3d::io::CreatePointCloudFromFile(argv[1]);
    std::shared_ptr<geometry::PointCloud> target =
            open3d::io::CreatePointCloudFromFile(argv[2]);
    if (source == nullptr || target == nullptr) {
        utility::LogWarning("Unable to load source or target file.");
        return -1;
    }

    std::vector<double> voxel_sizes = {0.05, 0.05 / 2, 0.05 / 4};
    std::vector<int> iterations = {50, 30, 14};
    Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();
    for (int i = 0; i < 3; ++i) {
        float voxel_size = voxel_sizes[i];

        auto source_down = source->VoxelDownSample(voxel_size);
        source_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        auto target_down = target->VoxelDownSample(voxel_size);
        target_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
                voxel_size * 2.0, 30));

        auto result = pipelines::registration::RegistrationColoredICP(
                *source_down, *target_down, 0.07, trans,
                pipelines::registration::
                        TransformationEstimationForColoredICP(),
                pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6,
                                                                iterations[i]));
        trans = result.transformation_;

        if (visualize) {
            VisualizeRegistration(*source, *target, trans);
        }
    }

    std::stringstream ss;
    ss << trans;
    utility::LogInfo("Final transformation = \n{}", ss.str());

    return 0;
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/nonlinear_position_estimator.h"

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <algorithm>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include "theia/sfm/global_pose_estimation/pairwise_translation_error.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      rotation_angle_axis.data(),
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation.transpose() * translation;
}

Vector3d GetRotatedFeatureRay(const Camera& camera,
                              const Vector3d& orientation,
                              const Feature& feature) {
  Camera temp_camera = camera;
  temp_camera.SetOrientationFromAngleAxis(orientation);
  // Get the image ray rotated into the world reference frame.
  return camera.PixelToUnitDepthRay(feature).normalized();
}

// Sorts the pairs such that the number of views (i.e. the int) is sorted in
// descending order.
bool CompareViewsPerTrack(const std::pair<TrackId, int>& t1,
                          const std::pair<TrackId, int>& t2) {
  return t1.second > t2.second;
}

}  // namespace

NonlinearPositionEstimator::NonlinearPositionEstimator(
    const NonlinearPositionEstimator::Options& options,
    const Reconstruction& reconstruction)
    : options_(options), reconstruction_(reconstruction) {
  CHECK_GT(options_.num_threads, 0);
  CHECK_GE(options_.min_num_points_per_view, 0);
  CHECK_GT(options_.point_to_camera_weight, 0);
  CHECK_GT(options_.robust_loss_width, 0);

  if (options_.rng.get() == nullptr) {
    rng_ = std::make_shared<RandomNumberGenerator>();
  } else {
    rng_ = options_.rng;
  }
}

bool NonlinearPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions);
  if (view_pairs.empty() || orientations.empty()) {
    VLOG(2) << "Number of view_pairs = " << view_pairs.size()
            << " Number of orientations = " << orientations.size();
    return false;
  }
  triangulated_points_.clear();
  problem_.reset(new ceres::Problem());
  view_pairs_ = &view_pairs;

  // Iterative schur is only used if the problem is large enough, otherwise
  // sparse schur is used.
  static const int kMinNumCamerasForIterativeSolve = 1000;

  // Initialize positions to be random.
  InitializeRandomPositions(orientations, positions);

  // Add the constraints to the problem.
  AddCameraToCameraConstraints(orientations, positions);
  if (options_.min_num_points_per_view > 0) {
    AddPointToCameraConstraints(orientations, positions);
    AddCamerasAndPointsToParameterGroups(positions);
  }

  // Set one camera to be at the origin to remove the ambiguity of the origin.
  positions->begin()->second.setZero();
  problem_->SetParameterBlockConstant(positions->begin()->second.data());

  // Set the solver options.
  ceres::Solver::Summary summary;
  solver_options_.num_threads = options_.num_threads;
  solver_options_.num_linear_solver_threads = options_.num_threads;
  solver_options_.max_num_iterations = options_.max_num_iterations;

  // Choose the type of linear solver. For sufficiently large problems, we want
  // to use iterative methods (e.g., Conjugate Gradient or Iterative Schur);
  // however, we only want to use a Schur solver if 3D points are used in the
  // optimization.
  if (positions->size() > kMinNumCamerasForIterativeSolve) {
    if (options_.min_num_points_per_view > 0) {
      solver_options_.linear_solver_type = ceres::ITERATIVE_SCHUR;
      solver_options_.preconditioner_type = ceres::SCHUR_JACOBI;
    } else {
      solver_options_.linear_solver_type = ceres::CGNR;
      solver_options_.preconditioner_type = ceres::JACOBI;
    }
  } else {
    if (options_.min_num_points_per_view > 0) {
      solver_options_.linear_solver_type = ceres::SPARSE_SCHUR;
    } else {
      solver_options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    }
  }

  ceres::Solve(solver_options_, problem_.get(), &summary);
  LOG(INFO) << summary.FullReport();
  return summary.IsSolutionUsable();
}

void NonlinearPositionEstimator::InitializeRandomPositions(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  std::unordered_set<ViewId> constrained_positions;
  constrained_positions.reserve(orientations.size());
  for (const auto& view_pair : *view_pairs_) {
    constrained_positions.insert(view_pair.first.first);
    constrained_positions.insert(view_pair.first.second);
  }

  positions->reserve(orientations.size());
  for (const auto& orientation : orientations) {
    if (ContainsKey(constrained_positions, orientation.first)) {
      (*positions)[orientation.first] = 100.0 * rng_->RandVector3d();
    }
  }
}

void NonlinearPositionEstimator::AddCameraToCameraConstraints(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  for (const auto& view_pair : *view_pairs_) {
    const ViewId view_id1 = view_pair.first.first;
    const ViewId view_id2 = view_pair.first.second;
    Vector3d* position1 = FindOrNull(*positions, view_id1);
    Vector3d* position2 = FindOrNull(*positions, view_id2);

    // Do not add this view pair if one or both of the positions do not exist.
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction = GetRotatedTranslation(
        FindOrDie(orientations, view_id1), view_pair.second.position_2);

    ceres::CostFunction* cost_function =
        PairwiseTranslationError::Create(translation_direction, 1.0);

    problem_->AddResidualBlock(cost_function,
                               new ceres::HuberLoss(options_.robust_loss_width),
                               position1->data(),
                               position2->data());
  }

  VLOG(2) << problem_->NumResidualBlocks() << " camera to camera constraints "
                                              "were added to the position "
                                              "estimation problem.";
}

void NonlinearPositionEstimator::AddPointToCameraConstraints(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    std::unordered_map<ViewId, Eigen::Vector3d>* positions) {
  const int num_camera_to_camera_constraints = problem_->NumResidualBlocks();
  std::unordered_set<TrackId> tracks_to_add;
  const int num_point_to_camera_constraints =
      FindTracksForProblem(*positions, &tracks_to_add);
  if (num_point_to_camera_constraints == 0) {
    return;
  }

  const double point_to_camera_weight =
      options_.point_to_camera_weight *
      static_cast<double>(num_camera_to_camera_constraints) /
      static_cast<double>(num_point_to_camera_constraints);

  triangulated_points_.reserve(tracks_to_add.size());
  for (const TrackId track_id : tracks_to_add) {
    triangulated_points_[track_id] = 100.0 * rng_->RandVector3d();

    AddTrackToProblem(track_id,
                      orientations,
                      point_to_camera_weight,
                      positions);
  }

  VLOG(2) << num_point_to_camera_constraints << " point to camera constriants "
                                                "were added to the position "
                                                "estimation problem.";
}

int NonlinearPositionEstimator::FindTracksForProblem(
    const std::unordered_map<ViewId, Eigen::Vector3d>& positions,
    std::unordered_set<TrackId>* tracks_to_add) {
  CHECK_NOTNULL(tracks_to_add)->clear();

  std::unordered_map<ViewId, int> tracks_per_camera;
  for (const auto& position : positions) {
    tracks_per_camera[position.first] = 0;
  }

  // Add the tracks that see the most views until each camera has the minimum
  // number of tracks.
  for (const auto& position : positions) {
    const View* view = reconstruction_.View(position.first);
    if (view == nullptr ||
        view->NumFeatures() < options_.min_num_points_per_view) {
      continue;
    }

    // Get the tracks in sorted order so that we add the tracks that see the
    // most cameras first.
    const std::vector<TrackId>& sorted_tracks =
        GetTracksSortedByNumViews(reconstruction_, *view, *tracks_to_add);

    for (int i = 0;
         i < sorted_tracks.size() && tracks_per_camera[position.first] <
                                         options_.min_num_points_per_view;
         i++) {
      // Update the number of point to camera constraints for each camera.
      tracks_to_add->insert(sorted_tracks[i]);
      for (const ViewId view_id :
           reconstruction_.Track(sorted_tracks[i])->ViewIds()) {
        if (!ContainsKey(positions, view_id)) {
          continue;
        }
        ++tracks_per_camera[view_id];
      }
    }
  }

  int num_point_to_camera_constraints = 0;
  for (const auto& tracks_in_camera : tracks_per_camera) {
    num_point_to_camera_constraints += tracks_in_camera.second;
  }
  return num_point_to_camera_constraints;
}

std::vector<TrackId> NonlinearPositionEstimator::GetTracksSortedByNumViews(
    const Reconstruction& reconstruction,
    const View& view,
    const std::unordered_set<TrackId>& existing_tracks) {
  std::vector<std::pair<TrackId, int> > views_per_track;
  views_per_track.reserve(view.NumFeatures());
  const auto& track_ids = view.TrackIds();
  for (const auto& track_id : track_ids) {
    const Track* track = reconstruction.Track(track_id);

    if (track == nullptr || ContainsKey(existing_tracks, track_id)) {
      continue;
    }
    views_per_track.emplace_back(track_id, track->NumViews());
  }

  // Return an empty array if no tracks could be found for this view.
  std::vector<TrackId> sorted_tracks(views_per_track.size());
  if (views_per_track.size() == 0) {
    return sorted_tracks;
  }

  // Sort the tracks by the number of views. Only sort the first few tracks
  // since those are the ones that will be added to the problem.
  const int num_tracks_to_sort =
      std::min(static_cast<int>(views_per_track.size()),
               options_.min_num_points_per_view);
  std::partial_sort(views_per_track.begin(),
                    views_per_track.begin() + num_tracks_to_sort,
                    views_per_track.end(), CompareViewsPerTrack);

  for (int i = 0; i < num_tracks_to_sort; i++) {
    sorted_tracks[i] = views_per_track[i].first;
  }
  return sorted_tracks;
}

void NonlinearPositionEstimator::AddTrackToProblem(
    const TrackId track_id,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    const double point_to_camera_weight,
    std::unordered_map<ViewId, Vector3d>* positions) {
  // For each view in the track add the point to camera correspondences.
  for (const ViewId view_id : reconstruction_.Track(track_id)->ViewIds()) {
    if (!ContainsKey(*positions, view_id)) {
      continue;
    }
    Vector3d& camera_position = FindOrDie(*positions, view_id);
    Vector3d& point = FindOrDie(triangulated_points_, track_id);

    // Rotate the feature ray to be in the global orientation frame.
    const Vector3d feature_ray = GetRotatedFeatureRay(
        reconstruction_.View(view_id)->Camera(),
        FindOrDie(orientations, view_id),
        *reconstruction_.View(view_id)->GetFeature(track_id));

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    ceres::CostFunction* cost_function =
        PairwiseTranslationError::Create(feature_ray, point_to_camera_weight);

    // Add the residual block
    problem_->AddResidualBlock(cost_function,
                               new ceres::HuberLoss(options_.robust_loss_width),
                               camera_position.data(),
                               point.data());
  }
}

void NonlinearPositionEstimator::AddCamerasAndPointsToParameterGroups(
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_GT(triangulated_points_.size(), 0)
      << "Cannot set the Ceres parameter groups for Schur based solvers "
         "because there are no triangulated points.";

  // Create a custom ordering for Schur-based problems.
  solver_options_.linear_solver_ordering.reset(
      new ceres::ParameterBlockOrdering);
  ceres::ParameterBlockOrdering* parameter_ordering =
      solver_options_.linear_solver_ordering.get();
  // Add point parameters to group 0.
  for (auto& point : triangulated_points_) {
    parameter_ordering->AddElementToGroup(point.second.data(), 0);
  }

  // Add camera parameters to group 1.
  for (auto& position : *positions) {
    parameter_ordering->AddElementToGroup(position.second.data(), 1);
  }
}

}  // namespace theia


///////////////////////////////////////////////////////////////////////////////
// non_empty_cells_count.hpp                                                 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_NON_EMPTY_CELLS_COUNT_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_NON_EMPTY_CELLS_COUNT_HPP_ER_2010
#include <boost/range.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/traits.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace contingency_table{
namespace result_of{

    template<typename Keys,typename AccSet>
    struct non_empty_cells_count
        : contingency_table::cells_traits::size_of<Keys,AccSet>{};

}// result_of

  	template<typename Keys,typename AccSet>
    typename contingency_table::result_of::non_empty_cells_count<
        Keys,AccSet>::type
    non_empty_cells_count(AccSet const& acc){
        namespace ns = contingency_table;
        return ns::extract::cells<Keys>( acc ).size() ;
    }

}// contingency_table
}// detail
}// statistics
}// boost

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestValarray
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/container/valarray.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(size)
{
    boost::compute::valarray<float> array;
    BOOST_CHECK_EQUAL(array.size(), size_t(0));

    array.resize(10);
    BOOST_CHECK_EQUAL(array.size(), size_t(10));
}

BOOST_AUTO_TEST_CASE(at)
{
    int data[] = { 1, 2, 3, 4, 5 };
    boost::compute::valarray<int> array(data, 5);
    BOOST_CHECK_EQUAL(array.size(), size_t(5));

    boost::compute::system::finish();
    BOOST_CHECK_EQUAL(int(array[0]), int(1));
    BOOST_CHECK_EQUAL(int(array[1]), int(2));
    BOOST_CHECK_EQUAL(int(array[2]), int(3));
    BOOST_CHECK_EQUAL(int(array[3]), int(4));
    BOOST_CHECK_EQUAL(int(array[4]), int(5));
}

BOOST_AUTO_TEST_CASE(min_and_max)
{
    int data[] = { 5, 2, 3, 7, 1, 9, 6, 5 };
    boost::compute::valarray<int> array(data, 8);
    BOOST_CHECK_EQUAL(array.size(), size_t(8));

    BOOST_CHECK_EQUAL((array.min)(), int(1));
    BOOST_CHECK_EQUAL((array.max)(), int(9));
}

BOOST_AUTO_TEST_CASE(sum)
{
    int data[] = { 1, 2, 3, 4 };
    boost::compute::valarray<int> array(data, 4);
    BOOST_CHECK_EQUAL(array.size(), size_t(4));

    BOOST_CHECK_EQUAL(array.sum(), int(10));
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file tree_augmentation_test.cpp
 * @brief
 * @author Attila Bernath, Piotr Godlewski
 * @version 1.0
 * @date 2013-07-10
 */

#include "test_utils/logger.hpp"

#include "paal/iterative_rounding/treeaug/tree_augmentation.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/test/unit_test.hpp>

using namespace paal;
using namespace paal::ir;
using namespace boost;

template <typename VertexList, typename EdgeProp>
using Graph = adjacency_list<vecS, VertexList, undirectedS,
                             property<vertex_index_t, int>, EdgeProp>;

using EdgeProp =
    property<edge_index_t, std::size_t,
             property<edge_weight_t, double, property<edge_color_t, bool>>>;
using VectorGraph = Graph<vecS, EdgeProp>;
using Edge = graph_traits<VectorGraph>::edge_descriptor;

template <typename Graph, typename TreeMap, typename Cost>
Edge add_edge_to_graph(Graph & g, int u, int v,
        TreeMap tree, bool in_t,
        Cost & cost, double c) {
    bool b;
    Edge e;
    std::tie(e, b) = add_edge(u, v, g);
    assert(b);
    tree[e] = in_t;
    cost[e] = c;
    return e;
}

using Cost = property_map<VectorGraph, edge_weight_t>::type;
using TreeMap = property_map<VectorGraph, edge_color_t>::type;
using SolutionTree = std::set<Edge>;

BOOST_AUTO_TEST_SUITE(tree_augmentation)

BOOST_AUTO_TEST_CASE(tree_augmentation_test) {
    // sample problem
    LOGLN("Sample problem:");
    VectorGraph g(6);
    Cost cost = get(edge_weight, g);
    TreeMap tree_map = get(edge_color, g);

    add_edge_to_graph(g, 0, 1, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 2, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 3, tree_map, true, cost, 0);
    add_edge_to_graph(g, 3, 4, tree_map, true, cost, 0);
    add_edge_to_graph(g, 3, 5, tree_map, true, cost, 0);
    add_edge_to_graph(g, 0, 3, tree_map, false, cost, 1);
    add_edge_to_graph(g, 0, 2, tree_map, false, cost, 1);
    add_edge_to_graph(g, 2, 4, tree_map, false, cost, 1);
    add_edge_to_graph(g, 2, 5, tree_map, false, cost, 1);
    add_edge_to_graph(g, 4, 5, tree_map, false, cost, 1);

    SolutionTree solution_tree;

    auto treeaug(make_tree_aug(g, std::inserter(solution_tree, solution_tree.begin())));
    solve_iterative_rounding(treeaug, tree_augmentation_ir_components<>());
    BOOST_CHECK(!solution_tree.empty());
}

BOOST_AUTO_TEST_CASE(tree_augmentation_list) {
    // boost::listS instead of boost::vecS for vertex storage
    using EdgeProp =
        property<edge_weight_t, double, property<edge_color_t, bool>>;
    using ListGraph = Graph<listS, EdgeProp>;
    using EdgeT = graph_traits<ListGraph>::edge_descriptor;

    std::vector<std::pair<int, int>> edges = { { 0, 1 }, { 1, 2 }, { 1, 3 },
                                               { 3, 4 }, { 3, 5 }, { 0, 3 },
                                               { 0, 2 }, { 2, 4 }, { 2, 5 },
                                               { 4, 5 } };
    std::vector<EdgeProp> edge_properties{ EdgeProp(0, 1), EdgeProp(0, 1),
                                           EdgeProp(0, 1), EdgeProp(0, 1),
                                           EdgeProp(0, 1), EdgeProp(1, 0),
                                           EdgeProp(1, 0), EdgeProp(1, 0),
                                           EdgeProp(1, 0), EdgeProp(1, 0) };
    ListGraph g(edges.begin(), edges.end(), edge_properties.begin(), 6);

    auto index = get(boost::vertex_index, g);
    int idx = 0;
    for (auto v : boost::make_iterator_range(vertices(g))) {
        put(index, v, idx);
        ++idx;
    }

    std::vector<EdgeT> solution_tree;
    tree_augmentation_iterative_rounding(g, std::inserter(solution_tree, solution_tree.begin()));
    BOOST_CHECK(!solution_tree.empty());
}

BOOST_AUTO_TEST_CASE(tree_augmentation_test_parameters) {
    // sample problem
    LOGLN("Sample problem:");
    VectorGraph g(6);
    TreeMap tree_map = get(edge_color, g);

    tree_map[add_edge(0, 1, 0, g).first] = true;
    tree_map[add_edge(1, 2, 1, g).first] = true;
    tree_map[add_edge(1, 3, 2, g).first] = true;
    tree_map[add_edge(3, 4, 3, g).first] = true;
    tree_map[add_edge(3, 5, 4, g).first] = true;
    tree_map[add_edge(0, 3, 5, g).first] = false;
    tree_map[add_edge(0, 2, 6, g).first] = false;
    tree_map[add_edge(2, 4, 7, g).first] = false;
    tree_map[add_edge(2, 5, 8, g).first] = false;
    tree_map[add_edge(4, 5, 9, g).first] = false;

    auto edge_id = get(boost::edge_index, g);
    std::vector<double> costs = {0, 0, 0, 0, 0, 1, 1, 1, 1, 1};
    auto cost = boost::make_iterator_property_map(costs.begin(), edge_id);

    {
        SolutionTree solution_tree;
        auto treeaug(make_tree_aug(g, boost::edge_color_map(tree_map).weight_map(cost),
                std::inserter(solution_tree, solution_tree.begin())));
        solve_iterative_rounding(treeaug, tree_augmentation_ir_components<>());
        BOOST_CHECK(!solution_tree.empty());
    }
    {
        SolutionTree solution_tree;
        tree_augmentation_iterative_rounding(
                g, boost::weight_map(cost),
                std::inserter(solution_tree, solution_tree.begin()));
        BOOST_CHECK(!solution_tree.empty());
    }
}

void run_invalid_test(const VectorGraph & g){
    SolutionTree solution_tree;
    auto treeaug(make_tree_aug(g, std::back_inserter(solution_tree)));
    auto invalid = treeaug.check_input_validity();

    BOOST_CHECK(invalid);
    LOGLN(*invalid);
}

BOOST_AUTO_TEST_CASE(tree_augmentation_invalid_test_1) {
    // invalid problem (wrong number of spanning tree edges)
    LOGLN("Invalid problem (wrong number of spanning tree edges):");
    VectorGraph g(6);
    Cost cost = get(edge_weight, g);
    TreeMap tree_map = get(edge_color, g);

    add_edge_to_graph(g, 0, 1, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 2, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 3, tree_map, false, cost, 0);
    add_edge_to_graph(g, 3, 4, tree_map, false, cost, 0);
    add_edge_to_graph(g, 3, 5, tree_map, false, cost, 0);

    run_invalid_test(g);
}

BOOST_AUTO_TEST_CASE(tree_augmentation_invalid_test_2) {
    // invalid problem (spanning tree not connected)
    LOGLN("Invalid problem (spanning tree not connected):");
    VectorGraph g(6);
    Cost cost = get(edge_weight, g);
    TreeMap tree_map = get(edge_color, g);

    add_edge_to_graph(g, 0, 1, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 2, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 3, tree_map, false, cost, 0);
    add_edge_to_graph(g, 3, 4, tree_map, true, cost, 0);
    add_edge_to_graph(g, 3, 5, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 4, tree_map, false, cost, 0);
    add_edge_to_graph(g, 5, 4, tree_map, true, cost, 0);
    add_edge_to_graph(g, 2, 3, tree_map, false, cost, 0);

    run_invalid_test(g);
}

BOOST_AUTO_TEST_CASE(tree_augmentation_invalid_test_3) {
    // invalid problem (graph not 2-edge-connected)
    LOGLN("Invalid problem (graph not 2-edge-connected):");
    VectorGraph g(6);
    Cost cost = get(edge_weight, g);
    TreeMap tree_map = get(edge_color, g);

    add_edge_to_graph(g, 0, 1, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 2, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 3, tree_map, true, cost, 0);
    add_edge_to_graph(g, 3, 4, tree_map, true, cost, 0);
    add_edge_to_graph(g, 3, 5, tree_map, true, cost, 0);
    add_edge_to_graph(g, 1, 4, tree_map, false, cost, 0);
    add_edge_to_graph(g, 5, 4, tree_map, false, cost, 0);
    add_edge_to_graph(g, 2, 3, tree_map, false, cost, 0);

    run_invalid_test(g);
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include "../src/Hash.hpp"

BOOST_AUTO_TEST_SUITE(HashModulusTest)

BOOST_AUTO_TEST_CASE(HashModulus3)
{
  Hash sut( 3, true, false, false );

  BOOST_CHECK_EQUAL( sut.modulus(), 1'000 );
}

BOOST_AUTO_TEST_CASE(HashModulusDigit)
{
  Hash sut( 8, true, false, false );

  BOOST_CHECK_EQUAL( sut.modulus(), 100'000'000 );
}

BOOST_AUTO_TEST_CASE(HashModulusUpper)
{
  Hash sut( 8, false, true, false );

  BOOST_CHECK_EQUAL( sut.modulus(), 208'827'064'576 );
}

BOOST_AUTO_TEST_CASE(HashModulusLower)
{
  Hash sut( 8, false, false, true );

  BOOST_CHECK_EQUAL( sut.modulus(), 208'827'064'576 );
}

BOOST_AUTO_TEST_CASE(HashModulusAll)
{
  Hash sut( 8, true, true, true );

  BOOST_CHECK_EQUAL( sut.modulus(), 218'340'105'584'896 );
}

BOOST_AUTO_TEST_SUITE_END()


#include <fstream>
#include <string>
#include <sstream>

#include <Eigen/Dense>

#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>

#include <MMM/Motion/Motion.h>
#include <MMM/XMLTools.h>

#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>

#include "utils/mmm/TrajectoryExporter.h"
#include "utils/mmm/ControlPoint.h"
#include "utils/mmm/ControlMatrix.h"
#include "utils/mmm/ControlValue.h"
#include "utils/Estimation.h"

namespace Bipedal
{

void TrajectoryExporter::exportToMMM(const std::string& path)
{
    boost::filesystem::path targetPath(path);
    boost::filesystem::path baseDir = targetPath.parent_path();
    std::string relRobotPath = MMM::XML::make_relative(baseDir.string(), pathToRobot);

    MMM::MotionPtr motion(new MMM::Motion("Walking pattern"));

    motion->setJointOrder(jointNames);

    int size = bodyTrajectory.cols();
    int ndof = bodyTrajectory.rows();

    Eigen::MatrixXf bodyVelocity     = Bipedal::slopeEstimation(bodyTrajectory, timestep);
    Eigen::MatrixXf bodyAcceleratoin = Bipedal::slopeEstimation(bodyVelocity, timestep);

    auto intervalIter = intervals.begin();

    for (int i = 0; i < size; i++)
    {
        while (i >= intervalIter->endIdx)
        {
            intervalIter = std::next(intervalIter);
        }

        // we need rootPos in mm
        MMM::MotionFramePtr frame(new MMM::MotionFrame(ndof));
        Eigen::Matrix4f rootPose = leftFootTrajectory[i];
        frame->setRootPose(rootPose);
        frame->setRootPos(rootPose.block(0, 3, 3, 1));
        frame->joint = bodyTrajectory.col(i);
        frame->joint_vel = bodyVelocity.col(i);
        frame->joint_acc = bodyAcceleratoin.col(i);
        frame->timestep = timestep * i;
        frame->addEntry("CoM",
                        boost::make_shared<ControlPointEntry3f>("CoM",
                                comTrajectory.col(i),
                                comVelocity.col(i),
                                comAcceleration.col(i)
                                                               )
                       );
        frame->addEntry("ZMP",
                        boost::make_shared<ControlPointEntry2f>("ZMP",
                                computedZMPTrajectory.col(i)
                                                               )
                       );
        frame->addEntry("ReferenceZMP",
                        boost::make_shared<ControlPointEntry2f>("ReferenceZMP",
                                referenceZMPTrajectory.col(i)
                                                               )
                       );
        frame->addEntry("LeftFoot",
                        boost::make_shared<ControlMatrixEntry4f>("LeftFoot",
                                leftFootTrajectory[i]
                                                                )
                       );
        frame->addEntry("RightFoot",
                        boost::make_shared<ControlMatrixEntry4f>("RightFoot",
                                rightFootTrajectory[i]
                                                                )
                       );
        frame->addEntry("Chest",
                        boost::make_shared<ControlMatrixEntry4f>("Chest",
                                chestTrajectory[i]
                                                                )
                       );
        frame->addEntry("Pelvis",
                        boost::make_shared<ControlMatrixEntry4f>("Pelvis",
                                pelvisTrajectory[i]
                                                                )
                       );
        frame->addEntry("SupportPhase",
                        boost::make_shared<ControlValueEntry<int>>("SupportPhase",
                                intervalIter->phase
                                                                  )
                       );

        motion->addMotionFrame(frame);
    }

    MMM::ModelPtr model(new MMM::Model());
    model->filename = relRobotPath;
    motion->setModel(model);

    std::ofstream out(path.c_str());
    out << "<MMM>"
        << motion->toXML()
        << "</MMM>";
}

}


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray_math.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"

#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "cuTvOperator.h"
#include "cuTv1dOperator.h"
#include "cuTvPicsOperator.h"
#include "CBSubsetOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include "osMOMSolverD.h"
#include "osMOMSolverD2.h"
#include "hoCuNCGSolver.h"
#include "osMOMSolverD3.h"
#include "osMOMSolverL1.h"
#include "osMOMSolverF.h"
#include "osAHZCSolver.h"
#include "ADMMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <GPUTimer.h>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "dicomWriter.h"
#include "conebeam_projection.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "CT_acquisition.h"
#include "CTSubsetOperator.h"
#include <boost/filesystem.hpp>
#include <boost/regex.hpp>
#include "dicomCTWriter.h"
#include "nonlocalMeans.h"
using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;
namespace fs = boost::filesystem;

std::vector<string> get_dcm_files(std::string dir){

	fs::directory_iterator end_itr;
	const boost::regex filter(".*\.dcm");

	std::vector<std::string> files;
	for (fs::directory_iterator i (dir); i!= end_itr; i++){
		if (!fs::is_regular(i->status())) continue;
		boost::smatch what;

		if (!boost::regex_match(i->path().filename().string(),what,filter)) continue;

		//std::cout  << i->path().filename() << std::endl;
		files.push_back(i->path().string());
	}
	sort(files.begin(),files.end());

	return files;
}

boost::shared_ptr<hoCuNDArray<float>> calculate_weights(hoCuNDArray<float>* projections,hoCuNDArray<float>* photonStats){

    auto  weights = boost::make_shared<hoCuNDArray<float>>(*projections);

    float* weights_ptr = weights->get_data_ptr();
    const size_t elements = weights->get_number_of_elements();

    float * statPtr = photonStats->get_data_ptr();

    const size_t nrows = projections->get_size(0);
    const size_t ncols = projections->get_size(1);
    const size_t nprojs = projections->get_size(2);
#pragma omp parallel for
    for (int proj = 0; proj < nprojs; proj++)
        for (int col = 0; col < ncols; col++ )
            for (int row = 0; row < nrows; row++) {
                size_t i = row + col * nrows + proj * nrows * ncols;
                weights_ptr[i] = std::sqrt(exp(-weights_ptr[i])*statPtr[row+proj*nrows]);
            }

    return weights;

}

int main(int argc, char** argv)
{

	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;

	unsigned int iterations;
	unsigned int subsets;
	float rho,tau;
	float tv_weight, wavelet_weight,huber,sigma,dct_weight;

	bool use_non_negativity,use_weights;
	int reg_iter;
	unsigned int skip_projections;

	po::options_description desc("Allowed options");

	desc.add_options()
			("help", "produce help message")
			("dir", po::value<string>(), "Dicom directory")
			("samples,n",po::value<unsigned int>(),"Number of samples per ray")
	("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
			("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
			("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
			("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
			("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
	("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
			("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
	("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight in spatial dimensions")

			("Wavelet,W",po::value<float>(&wavelet_weight)->default_value(0),"Weight of the wavelet operator")
			("Huber",po::value<float>(&huber)->default_value(0),"Huber weight")
			("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Prevent image from having negative attenuation")
			("sigma",po::value<float>(&sigma)->default_value(0.1),"Sigma for billateral filter")
			("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
			("offset",po::value<float>())

			("tau",po::value<float>(&tau)->default_value(1e-5),"Tau value for solver")
			("reg_iter",po::value<int>(&reg_iter)->default_value(2))
			("skip_projections",po::value<unsigned int>(&skip_projections)->default_value(0))
	("use_weights",po::value<bool>(&use_weights)->default_value(false));

	;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
		else if (a.type() == typeid(bool)) command_line_string << it->second.as<bool>();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}
	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	auto files = read_dicom_projections(get_dcm_files(vm["dir"].as<string>()),skip_projections);
	std::vector<float>& axials =  files->geometry.detectorFocalCenterAxialPosition;
	std::cout << "Axials size " << axials.size() << std::endl;
	float offset;

	if (vm.count("offset")){
		offset = vm["offset"].as<float>();
	} else {
		offset = std::accumulate(axials.begin(),axials.end(),0.0f)/axials.size();
		std::cout << "Mean offset " << offset << std::endl;

	}

	for ( auto & z : axials)
		z -= offset;

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = files->projections.get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	//osLALMSolver<hoCuNDArray<float> solver;
	osMOMSolverD3<hoCuNDArray<float>> solver;
	//osSPSSolver<hoCuNDArray<float>> solver;
	//osMOMSolverL1<hoCuNDArray<float> solver;
	//osAHZCSolver<hoCuNDArray<float> solver;
	//osMOMSolverF<hoCuNDArray<float> solver;
	//ADMMSolver<hoCuNDArray<float> solver;
	solver.set_dump(true);
	//solver.set_stepsize(1);
	//solver.set_beta(0.1);

	//hoCuCgDescentSolver<float> solver;

	//osSPSSolver<hoNDArray<float>> solver;
	//hoCuNCGSolver<float> solver;
	//solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(osSPSSolver<hoCuNDArray<float>>::OUTPUT_VERBOSE);
	solver.set_tau(tau);
	solver.set_non_negativity_constraint(use_non_negativity);
	solver.set_huber(huber);
	solver.set_reg_steps(reg_iter);
	//solver.set_rho(rho);

	if (tv_weight > 0) {

		auto Dx = boost::make_shared<hoCuPartialDerivativeOperator<float, 4>>(0);
		Dx->set_weight(tv_weight);
		Dx->set_domain_dimensions(&is_dims);
		Dx->set_codomain_dimensions(&is_dims);

		auto Dy = boost::make_shared<hoCuPartialDerivativeOperator<float, 4>>(1);
		Dy->set_weight(tv_weight);
		Dy->set_domain_dimensions(&is_dims);
		Dy->set_codomain_dimensions(&is_dims);

		auto Dz = boost::make_shared<hoCuPartialDerivativeOperator<float, 4>>(2);
		Dz->set_weight(tv_weight);
		Dz->set_domain_dimensions(&is_dims);
		Dz->set_codomain_dimensions(&is_dims);

		solver.add_regularization_group({Dx, Dy, Dz});
	}

	auto E = boost::make_shared<CTSubsetOperator<hoCuNDArray> >(subsets);

	E->set_domain_dimensions(&is_dims);
	//E->setup(ps,binning,imageDimensions);

    boost::shared_ptr<hoCuNDArray<float>> host_projections;
    if (use_weights) {
        auto weights = calculate_weights(&files->projections,&files->photonStatistics);
        write_nd_array(weights.get(),"weights.real");
        files->projections *= *weights;
        host_projections = E->setup(files, imageDimensions,weights);

    }else
	    host_projections = E->setup(files,imageDimensions);

	E->set_codomain_dimensions(host_projections->get_dimensions().get());

	solver.set_encoding_operator(E);

	boost::shared_ptr<hoCuNDArray<float>> result;
	{

		GPUTimer tim("Solver");
		result = solver.solve(host_projections.get());
	}

	std::cout << "Penguin" << nrm2(result.get()) << std::endl;

	std::cout << "Result sum " << asum(result.get()) << std::endl;

	//apply_mask(result.get(),mask.get());

	std::cout << "Result sum " << asum(result.get()) << std::endl;
	//saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),command_line_string.str(),iterations);

//	write_nd_array(result.get(),"reconstruction.real");

	write_dicomCT("output",result.get(),imageDimensions,files.get(),offset);

    /*
	cuNDArray<float> tmp(result.get());
	cuNDArray<float> tmp2(result.get());
	nonlocal_means(&tmp,&tmp2,0.01);
	auto result2 = tmp2.to_host();

	write_dicomCT("output2",result2.get(),imageDimensions,files.get(),offset);
*/

}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "DifOdometry_Camera.h"

#include <mrpt/opengl/CBox.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CFrustum.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/system/filesystem.h>

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
using namespace mrpt;
using namespace mrpt::opengl;

void CDifodoCamera::loadConfiguration(const mrpt::config::CConfigFileBase& ini)
{
	fovh = M_PI * 58.6 / 180.0;
	fovv = M_PI * 45.6 / 180.0;
	downsample = 1;
	fast_pyramid = true;
	cam_mode = ini.read_int("DIFODO_CONFIG", "cam_mode", 2, true);
	rows = ini.read_int("DIFODO_CONFIG", "rows", 240, true);
	cols = ini.read_int("DIFODO_CONFIG", "cols", 320, true);
	fps = ini.read_int("DIFODO_CONFIG", "fps", 30, false);
	ctf_levels = ini.read_int("DIFODO_CONFIG", "ctf_levels", 5, true);

	//			Resize Matrices and adjust parameters
	//=========================================================
	m_width = 640 / (cam_mode * downsample);
	m_height = 480 / (cam_mode * downsample);
	repr_level = mrpt::round(log(float(m_width / cols)) / log(2.f));

	// Resize pyramid
	const unsigned int pyr_levels =
		mrpt::round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;
	depth.resize(pyr_levels);
	depth_old.resize(pyr_levels);
	depth_inter.resize(pyr_levels);
	depth_warped.resize(pyr_levels);
	xx.resize(pyr_levels);
	xx_inter.resize(pyr_levels);
	xx_old.resize(pyr_levels);
	xx_warped.resize(pyr_levels);
	yy.resize(pyr_levels);
	yy_inter.resize(pyr_levels);
	yy_old.resize(pyr_levels);
	yy_warped.resize(pyr_levels);
	transformations.resize(pyr_levels);

	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		depth[i].resize(rows_i, cols_i);
		depth_inter[i].resize(rows_i, cols_i);
		depth_old[i].resize(rows_i, cols_i);
		depth[i].fill(0.0f);
		depth_old[i].fill(0.0f);
		xx[i].resize(rows_i, cols_i);
		xx_inter[i].resize(rows_i, cols_i);
		xx_old[i].resize(rows_i, cols_i);
		xx[i].fill(0.0f);
		xx_old[i].fill(0.0f);
		yy[i].resize(rows_i, cols_i);
		yy_inter[i].resize(rows_i, cols_i);
		yy_old[i].resize(rows_i, cols_i);
		yy[i].fill(0.0f);
		yy_old[i].fill(0.0f);
		transformations[i].resize(4, 4);

		if (cols_i <= cols)
		{
			depth_warped[i].resize(rows_i, cols_i);
			xx_warped[i].resize(rows_i, cols_i);
			yy_warped[i].resize(rows_i, cols_i);
		}
	}

	depth_wf.setSize(m_height, m_width);
}

bool CDifodoCamera::openCamera()
{
	const char* deviceURI = openni::ANY_DEVICE;

	rc = openni::OpenNI::initialize();

	printf("Initialization:\n %s\n", openni::OpenNI::getExtendedError());
	rc = device.open(deviceURI);
	if (rc != openni::STATUS_OK)
	{
		printf(
			"SimpleViewer: Device open failed:\n%s\n",
			openni::OpenNI::getExtendedError());
		openni::OpenNI::shutdown();
		return true;
	}

	//				Create Depth channel
	//-----------------------------------------------------
	rc = depth_ch.create(device, openni::SENSOR_DEPTH);

	// Display video modes
	openni::VideoMode vm;
	printf("Camera depth modes are listed:\n");
	for (int i = 0;
		 i < depth_ch.getSensorInfo().getSupportedVideoModes().getSize(); i++)
	{
		vm = depth_ch.getSensorInfo().getSupportedVideoModes()[i];
		cout << "depth mode :" << vm.getResolutionX() << "x"
			 << vm.getResolutionY() << "@" << vm.getFps() << "Hz using format "
			 << vm.getPixelFormat() << endl;
	}

	//					Set video mode
	//------------------------------------------------------
	const unsigned int resh = 640 / cam_mode;
	const unsigned int resv = 480 / cam_mode;
	video_options = depth_ch.getVideoMode();
	video_options.setResolution(resh, resv);
	video_options.setFps(fps);
	rc = depth_ch.setVideoMode(video_options);
	rc = depth_ch.setMirroringEnabled(false);

	video_options = depth_ch.getVideoMode();
	printf(
		"\nResolution (%d, %d) \n", video_options.getResolutionX(),
		video_options.getResolutionY());

	if (rc == openni::STATUS_OK)
	{
		rc = depth_ch.start();
		if (rc != openni::STATUS_OK)
		{
			printf(
				"Couldn't start depth stream:\n%s\n",
				openni::OpenNI::getExtendedError());
			depth_ch.destroy();
		}
	}
	else
		printf(
			"Couldn't find depth stream:\n%s\n",
			openni::OpenNI::getExtendedError());

	return false;
}

void CDifodoCamera::loadFrame()
{
	// Read the newest frame
	openni::VideoFrameRef framed;
	depth_ch.readFrame(&framed);

	const int height = framed.getHeight();
	const int width = framed.getWidth();

	// Store the depth values
	const auto* pDepthRow = (const openni::DepthPixel*)framed.getData();
	int rowSize = framed.getStrideInBytes() / sizeof(openni::DepthPixel);

	for (int yc = height - 1; yc >= 0; --yc)
	{
		const openni::DepthPixel* pDepth = pDepthRow;
		for (int xc = width - 1; xc >= 0; --xc, ++pDepth)
		{
			if (*pDepth < 4500.f) depth_wf(yc, xc) = 0.001f * (*pDepth);

			else
				depth_wf(yc, xc) = 0.f;
		}

		pDepthRow += rowSize;
	}
}

void CDifodoCamera::CreateResultsFile()
{
	try
	{
		// Open file; find the first free file-name.
		char aux[100];
		int nFile = 0;
		bool free_name = false;

		system::createDirectory("./difodo.results");

		while (!free_name)
		{
			nFile++;
			sprintf(aux, "./difodo.results/experiment_%03u.txt", nFile);
			free_name = !system::fileExists(aux);
		}

		// Open log file:
		f_res.open(aux);

		clock.Tic();

		printf(" Saving results to file: %s \n", aux);
	}
	catch (...)
	{
		printf("Exception found trying to create the 'results file' !!\n");
	}
}

void CDifodoCamera::initializeScene()
{
	poses::CPose3D rel_lenspose(0, -0.05, 0, 0, 0, 0);

	global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE(1000000);
	window.resize(1000, 900);
	window.setPos(900, 0);
	window.setCameraZoom(16);
	window.setCameraAzimuthDeg(0);
	window.setCameraElevationDeg(90);
	window.setCameraPointingToPoint(0, 0, 0);
	window.setCameraPointingToPoint(0, 0, 1);

	scene = window.get3DSceneAndLock();

	// Grid (ground)
	CGridPlaneXY::Ptr ground = std::make_shared<CGridPlaneXY>();
	scene->insert(ground);

	// Reference
	CSetOfObjects::Ptr reference = stock_objects::CornerXYZ();
	scene->insert(reference);

	//					Cameras and points
	//------------------------------------------------------

	// DifOdo camera
	CBox::Ptr camera_odo = std::make_shared<CBox>(
		math::TPoint3D(-0.02, -0.1, -0.01), math::TPoint3D(0.02, 0.1, 0.01));
	camera_odo->setPose(cam_pose + rel_lenspose);
	camera_odo->setColor(0, 1, 0);
	scene->insert(camera_odo);

	// Frustum
	opengl::CFrustum::Ptr FOV = std::make_shared<opengl::CFrustum>(
		0.3, 2, 57.3 * fovh, 57.3 * fovv, 1.f, true, false);
	FOV->setColor(0.7, 0.7, 0.7);
	FOV->setPose(cam_pose);
	scene->insert(FOV);

	// Reference cam
	CSetOfObjects::Ptr reference_gt = stock_objects::CornerXYZ();
	reference_gt->setScale(0.2);
	reference_gt->setPose(cam_pose);
	scene->insert(reference_gt);

	// Camera points
	CPointCloudColoured::Ptr cam_points =
		std::make_shared<CPointCloudColoured>();
	cam_points->setPointSize(2);
	cam_points->setPose(cam_pose);
	scene->insert(cam_points);

	//					Trajectories and covariance
	//-------------------------------------------------------------

	// Dif Odometry
	CSetOfLines::Ptr traj_lines_odo = std::make_shared<CSetOfLines>();
	traj_lines_odo->setLocation(0, 0, 0);
	traj_lines_odo->setColor(0, 0.6, 0);
	traj_lines_odo->setLineWidth(6);
	scene->insert(traj_lines_odo);
	CPointCloud::Ptr traj_points_odo = std::make_shared<CPointCloud>();
	traj_points_odo->setColor(0, 0.6, 0);
	traj_points_odo->setPointSize(4);
	scene->insert(traj_points_odo);

	// Ellipsoid showing covariance
	math::CMatrixFloat33 cov3d = est_cov.extractMatrix<3, 3>(0, 0);
	cov3d *= 20.f;

	CEllipsoid3D::Ptr ellip = std::make_shared<CEllipsoid3D>();
	ellip->setCovMatrix(cov3d.cast_double());
	ellip->setQuantiles(2.0);
	ellip->setColor(1.0, 1.0, 1.0, 0.5);
	ellip->enableDrawSolid3D(true);
	ellip->setPose(cam_pose + rel_lenspose);
	scene->insert(ellip);

	// User-interface information
	img::CImage img_legend;
	img_legend.loadFromXPM(legend_xpm);
	COpenGLViewport::Ptr legend = scene->createViewport("legend");
	legend->setViewportPosition(20, 20, 350, 201);
	legend->setImageView(img_legend);

	window.unlockAccess3DScene();
	window.repaint();
}

void CDifodoCamera::updateScene()
{
	poses::CPose3D rel_lenspose(0, -0.05, 0, 0, 0, 0);

	scene = window.get3DSceneAndLock();

	// Reference gt
	CSetOfObjects::Ptr reference_gt = scene->getByClass<CSetOfObjects>(1);
	reference_gt->setPose(cam_pose);

	// Camera points
	CPointCloudColoured::Ptr cam_points =
		scene->getByClass<CPointCloudColoured>(0);
	cam_points->clear();
	cam_points->setPose(cam_pose);
	for (unsigned int y = 0; y < cols; y++)
		for (unsigned int z = 0; z < rows; z++)
			cam_points->push_back(
				depth[repr_level](z, y), xx[repr_level](z, y),
				yy[repr_level](z, y), 1.f - sqrt(weights(z, y)),
				sqrt(weights(z, y)), 0);

	// DifOdo camera
	CBox::Ptr camera_odo = scene->getByClass<CBox>(0);
	camera_odo->setPose(cam_pose + rel_lenspose);

	// Frustum
	CFrustum::Ptr FOV = scene->getByClass<CFrustum>(0);
	FOV->setPose(cam_pose);

	// Difodo traj lines
	CSetOfLines::Ptr traj_lines_odo = scene->getByClass<CSetOfLines>(0);
	traj_lines_odo->appendLine(
		cam_oldpose.x(), cam_oldpose.y(), cam_oldpose.z(), cam_pose.x(),
		cam_pose.y(), cam_pose.z());

	// Difodo traj points
	CPointCloud::Ptr traj_points_odo = scene->getByClass<CPointCloud>(0);
	traj_points_odo->insertPoint(cam_pose.x(), cam_pose.y(), cam_pose.z());

	// Ellipsoid showing covariance
	const auto cov3d = math::CMatrixFloat33(20.f * est_cov.block<3, 3>(0, 0));
	CEllipsoid3D::Ptr ellip = scene->getByClass<CEllipsoid3D>(0);
	ellip->setCovMatrix(cov3d.cast_double());
	ellip->setPose(cam_pose + rel_lenspose);

	window.unlockAccess3DScene();
	window.repaint();
}

void CDifodoCamera::closeCamera()
{
	depth_ch.destroy();
	openni::OpenNI::shutdown();
}

void CDifodoCamera::reset()
{
	// Reset Difodo
	loadFrame();
	if (fast_pyramid) buildCoordinatesPyramidFast();
	else
		buildCoordinatesPyramid();
	loadFrame();
	odometryCalculation();

	cam_pose.setFromValues(0, 0, 1.5, 0, 0, 0);
	cam_oldpose = cam_pose;

	// Reset scene
	scene = window.get3DSceneAndLock();
	CSetOfLines::Ptr traj_lines_odo = scene->getByClass<CSetOfLines>(0);
	traj_lines_odo->clear();
	CPointCloud::Ptr traj_points_odo = scene->getByClass<CPointCloud>(0);
	traj_points_odo->clear();
	window.unlockAccess3DScene();

	updateScene();
}

void CDifodoCamera::writeTrajectoryFile()
{
	mrpt::math::CQuaternionDouble quat;
	mrpt::poses::CPose3D auxpose, transf;
	transf.setFromValues(0, 0, 0, 0.5 * M_PI, -0.5 * M_PI, 0);

	auxpose = cam_pose - transf;
	auxpose.getAsQuaternion(quat);

	f_res << clock.Tac() << " ";
	f_res << cam_pose[0] << " ";
	f_res << cam_pose[1] << " ";
	f_res << cam_pose[2] << " ";
	f_res << quat[2] << " ";
	f_res << quat[3] << " ";
	f_res << -quat[1] << " ";
	f_res << -quat[0] << endl;
}


#ifndef Fitting_MinimizeLogAVarianceAndScalingFactors_hpp
#define Fitting_MinimizeLogAVarianceAndScalingFactors_hpp

/** @file MinimizeLogAVarianceAndScalingFactors.hpp
  * @brief
  * @author C.D. Clark III
  * @date 06/28/17
  */

#include <utility>

#include <boost/math/tools/minima.hpp>
using boost::math::tools::brent_find_minima;

#include <boost/math/tools/roots.hpp>
using boost::math::tools::bracket_and_solve_root;
using boost::math::tools::eps_tolerance;

#include "../../ThresholdCalculator.hpp"

namespace libArrhenius {

/** @class MinimizeLogAVarianceAndScalingFactors
  * @brief
  * @author C.D. Clark III
  */
template<typename Real>
class ArrheniusFit<Real,MinimizeLogAVarianceAndScalingFactors> : public ArrheniusFitBase<Real>
{
  protected:

  public:
    ArrheniusFit() {};
    virtual ~ArrheniusFit() {};

    typedef typename ArrheniusFitBase<Real>::Return Return;

    Return
    exec() const
    {
      BOOST_LOG_TRIVIAL(trace) << "MinimizeLogAVarianceAndScalignFactors: Executing Fit";
      // find Ea that minimizes the variance in corresponding A values.
      // then find A that minimizes scaling factor error

      Return ret;
      ArrheniusIntegral<Real> integrator;
      ThresholdCalculator<decltype(integrator)> calc;
      std::vector<Real*> const &t = this->t;
      std::vector<Real*> const &T = this->T;
      std::vector<size_t> const &N = this->N;

      // search for Ea

      // the cost function for choosing Ea
      // calculates and returns the sum of squared deviations.
      auto Ea_cost = [&](Real Ea){

        // cost is equal to the sum of squared deviations (i.e. proportional to variance)
        integrator.setEa( Ea );
        integrator.setA(1);

        std::vector<Real> logAs(N.size());
        for(size_t i = 0; i < N.size(); ++i)
          logAs[i] = -log( integrator(N[i], t[i], T[i]) );

        // calculate the mean
        Real mean = 0;
        for(size_t i = 0; i < N.size(); ++i)
          mean += logAs[i];
        mean /= N.size();

        // calculate the sum of squared deviations
        Real devs = 0;
        for(size_t i = 0; i < N.size(); ++i)
          devs += (logAs[i] - mean)*(logAs[i] - mean);

       return devs;
      };

      // We need to get a range for Ea before we can run the minimization
      // function. So, we'll do a quick scan for the minimum.
      // If the caller has specified a bound use it.
      // otherwise, calculate one
      Real Ea_lb = 1;
      Real Ea_ub = 2;

      if( this->minEa )
      {
        Ea_lb = this->minEa.get();
      }

      if( this->maxEa )
      {
        Ea_ub = this->maxEa.get();
      }

      int prec;
      boost::uintmax_t maxit;

      if(!this->minEa || !this->maxEa)
      {
      BOOST_LOG_TRIVIAL(trace) << "Searching for upper bound on Ea";
      prec = std::numeric_limits<Real>::digits - 3;

      {
        // If Ea is too large, then the Arrhenius integral will
        // return zero. The point at which this happens depends on the
        // data type being used. Basically, higher-precision types can
        // evaluate the integral at larger Ea's before reaching zero.
        // So, we know Ea can't be larger than the smallest value that
        // gives zero for the integral. This gives us an initial upper bound on Ea.
        bool found_one = false;
        for( size_t i = 0; i < N.size(); i++ )
        {
          eps_tolerance<Real> tol( prec );
          maxit = 100;
          Real guess = 1e2; // a place to start
          Real factor = 2;  // multiplication factor to use when searching for upper bound
          try {
          auto Ea_ub_range = bracket_and_solve_root( [&](Real Ea){
              integrator.setEa(Ea);
              integrator.setA(1);
              return integrator(N[i],t[i],T[i]);}, guess, factor, false, tol, maxit );

              // use the smallest Ea for the upper bound.
              if( !found_one || Ea_ub_range.first < Ea_ub )
                Ea_ub = Ea_ub_range.first;
              found_one = true;
          } catch( ... ) {
            BOOST_LOG_TRIVIAL(warning) <<"WARNING: Could not determine an upper bound on Ea from thermal profile number " << i << ". Skipping";
          }
        }
        if( !found_one )
        {
          BOOST_LOG_TRIVIAL(fatal)<<"ERROR: Could not determine an upper-bound on Ea from any of the thermal profiles.";
          throw std::runtime_error( "ERROR: Could not determine an upper-bound on Ea from any of the thermal profiles.");
        }

        // Now do a quick scan for the minimum.
        // We'll calculate the cost at every half decade for Ea.
        BOOST_LOG_TRIVIAL(trace) << "Scanning for minimum (rough estimate)";

        while( Ea_cost(Ea_ub) == 0 )
          Ea_ub /= 2;

        // scan Ea from 1 to Ea_ub on a log-scale
        Real min_lnEa, max_lnEa, d_lnEa;
        min_lnEa = 0;
        max_lnEa = log(Ea_ub);
        // check if we have limits configured. if so,
        // we should use them if they are more restrictive.
        if( this->minEa && log(this->minEa.get()) > min_lnEa )
          min_lnEa = log(this->minEa.get());
        if( this->maxEa && log(this->maxEa.get()) < max_lnEa )
          max_lnEa = log(this->maxEa.get());
        // discretize
        int num  = static_cast<int>((max_lnEa - min_lnEa) / 0.5); // half log spacing
        d_lnEa = (max_lnEa - min_lnEa) / (num - 1);
        int i_of_min = 0;
        Real min_cost = Ea_cost(exp(min_lnEa));
        for(int i = 1; i < num; ++i)
        {
          Ea_ub = exp(min_lnEa + i*d_lnEa);
          Real cost = Ea_cost(Ea_ub);
          if( cost < min_cost )
          {
            i_of_min = i;
            min_cost = cost;
          }
        }
        if(!this->minEa)
        {
        Ea_lb = exp(min_lnEa + (i_of_min-1)*d_lnEa);
        }
        if(!this->maxEa)
        {
        Ea_ub = exp(min_lnEa + (i_of_min+1)*d_lnEa);
        }
        BOOST_LOG_TRIVIAL(trace) << "Minimum between " << Ea_lb << " and " << Ea_ub;
      }
      }

      maxit = 1000;
      BOOST_LOG_TRIVIAL(trace) << "Searching for Ea with Cost minimization";
      auto Ea_min = brent_find_minima( Ea_cost, Ea_lb, Ea_ub, prec );
      ret.Ea = Ea_min.first;

      // search for A

      // the cost function for choosing A
      // calculates and returns the sum of squared deviations.
      auto A_cost = [&](Real A){

        // cost is equal to the sum of squared deviations (i.e. proportional to variance)
        calc.setEa( ret.Ea.get() );
        calc.setA( A );

        std::vector<Real> thresholds(N.size());
        for(size_t i = 0; i < N.size(); ++i)
          thresholds[i] = calc(N[i], t[i], T[i]);

        // calculate the sum of squared deviations
        Real devs = 0;
        for(size_t i = 0; i < N.size(); ++i)
          devs += (thresholds[i] - 1)*(thresholds[i] - 1);

        return devs;
      };

      // get the range to search for A
      std::vector<Real> As(N.size());
      for(size_t i = 0; i < N.size(); ++i)
        As[i] = 1/integrator(N[i], t[i], T[i]);
      Real A_lb = *std::min_element(As.begin(), As.end());
      Real A_ub = *std::max_element(As.begin(), As.end());

      if(this->minA)
      {
        A_lb = this->minA.get();
      }
      if(this->maxA)
      {
        A_ub = this->maxA.get();
      }

      maxit = 1000;
      BOOST_LOG_TRIVIAL(trace) << "Searching for A with Cost minimization between " << A_lb << " and " << A_ub;
      auto A_min = brent_find_minima( A_cost, A_lb, A_ub, prec );
      ret.A = A_min.first;

      // OR....
      // find zero for log of average threshold
      // both seem to work.
      //{
        //eps_tolerance<Real> tol( prec );
        //maxit = 100;
        //Real guess = 1/integrator(N[0],t[0],T[0]);
        //Real factor = 2;
        //auto A_range = bracket_and_solve_root( [&](Real A){
            //calc.setEa(ret.Ea.get());
            //calc.setA(A);
            //Real sum = 0;
            //for( int i = 0; i < N.size(); ++i )
              //sum += calc(N[i], t[i], T[i]);
            //sum /= N.size();

            //return static_cast<Real>(log(sum));}, guess, factor, false, tol, maxit );

      //}

      return ret;
    }

  protected:
};

}

#endif // include protector


#include <Eigen/Dense>

#include <gpd/lenet.h>

int main(int argc, char* argv[])
{
  // Test with input from file.
  Lenet net(1, "/home/andreas/projects/grasp_pose_detection/catkin_ws/src/gpd/caffe/15channels/bin/");
  std::vector<float> x = net.readFileLineByLineIntoVector("src/gpd/caffe/15channels/txt/rand_input.txt");
  net.forward(x);

  return 0;
}