#define BOOST_TEST_MODULE SplineTest
/*
 * ########################################################################
 * The contents of this file is free and unencumbered software released into the
 * public domain. For more information, please refer to <http://unlicense.org/>
 * ########################################################################
 */

#include <okruz/bspline/BSplineGenerator.h>
#include <okruz/bspline/Core.h>
#include <okruz/bspline/integration/analytical.h>
#include <okruz/bspline/integration/numerical.h>

#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

using okruz::bspline::BSplineGenerator;
using okruz::bspline::Spline;

using namespace okruz::bspline::support;
using namespace okruz::bspline;

template <typename T>
Spline<T, 0> getOne(const Grid<T> &grid) {
  T onet = static_cast<T>(1);
  Support support(grid, Construction::WHOLE_GRID);
  std::vector<std::array<T, 1>> coeffs(support.size() - 1, {onet});
  return Spline<T, 0>(std::move(support), std::move(coeffs));
}

template <typename T, size_t order>
void testIntegration(T tol) {
  using namespace okruz::bspline::integration;

  using Spline = okruz::bspline::Spline<T, order>;
  using Spline0 = okruz::bspline::Spline<T, 0>;
  const BSplineGenerator generator(std::vector<T>{
      -7.0l,  -6.85l, -6.55l, -6.3l, -6.0l, -5.75l, -5.53l, -5.2l,
      -4.75l, -4.5l,  -3.0l,  -2.5l, -1.5l, -1.0l,  0.0l,   0.5l,
      1.5l,   2.5l,   3.5l,   4.0l,  4.35l, 4.55l,  4.95l,  5.4l,
      5.7l,   6.1l,   6.35l,  6.5l,  6.85l, 7.0l});

  const std::vector<Spline> splines =
      generator.template generateBSplines<order + 1>();
  const Spline0 one = getOne(generator.getGrid());

  const auto f1 = [](const T & /*x*/) { return static_cast<T>(1); };
  const auto fx = [](const T &x) { return x; };

  integration::ScalarProduct sp;
  integration::BilinearForm bfx{operators::X<1>{}};
  integration::BilinearForm bfx2{operators::X<2>{}};
  integration::BilinearForm bfx_dx{operators::X<1>{} * operators::Dx<1>{}};
  integration::BilinearForm bfdx{operators::Dx<1>{}};
  integration::BilinearForm bfdx2{operators::Dx<2>{}};
  integration::BilinearForm bfx2_dx2{operators::X<2>{} * operators::Dx<2>{}};

  for (const auto &s1 : splines) {
    for (const auto &s2 : splines) {
      auto s2dx2 = s2.dx2();
      BOOST_CHECK_SMALL(overlap<T>(s1, s2) - integrate<T>(s1 * s2), tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2) - sp.integrate(s1, s2), tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2) - overlap<T>(one, s1 * s2), tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2.timesx()) - integrate_x<T>(s1, s2),
                        tol);
      BOOST_CHECK_SMALL(overlap<T>(s1.timesx(), s2) - integrate_x<T>(s1, s2),
                        tol);
      BOOST_CHECK_SMALL(bfx.integrate(s1, s2) - integrate_x<T>(s1, s2), tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1, s2.timesx().timesx()) - integrate_x2<T>(s1, s2),
          static_cast<T>(5) * tol);
      BOOST_CHECK_SMALL(bfx2.integrate(s1, s2) - integrate_x2<T>(s1, s2),
                        static_cast<T>(5) * tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1.timesx(), s2.timesx()) - integrate_x2<T>(s1, s2),
          static_cast<T>(5) * tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1.timesx().timesx(), s2) - integrate_x2<T>(s1, s2),
          static_cast<T>(5) * tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2.dx()) - integrate_dx<T>(s1, s2), tol);
      BOOST_CHECK_SMALL(bfdx.integrate(s1, s2) - integrate_dx<T>(s1, s2), tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1.timesx(), s2.dx()) - integrate_x_dx<T>(s1, s2), tol);
      BOOST_CHECK_SMALL(bfx_dx.integrate(s1, s2) - integrate_x_dx<T>(s1, s2),
                        static_cast<T>(2) * tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2.dx().dx()) - integrate_dx2<T>(s1, s2),
                        tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2dx2) - integrate_dx2<T>(s1, s2), tol);
      BOOST_CHECK_SMALL(bfdx2.integrate(s1, s2) - integrate_dx2<T>(s1, s2),
                        tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1.timesx(), s2.dx().dx()) - integrate_x_dx2<T>(s1, s2),
          static_cast<T>(11) * tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1, s2.dx().dx().timesx()) - integrate_x_dx2<T>(s1, s2),
          static_cast<T>(8) * tol);
      BOOST_CHECK_SMALL(overlap<T>(s1.timesx().timesx(), s2.dx().dx()) -
                            integrate_x2_dx2<T>(s1, s2),
                        static_cast<T>(60) * tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2.dx().dx().timesx().timesx()) -
                            integrate_x2_dx2<T>(s1, s2),
                        static_cast<T>(60) * tol);
      BOOST_CHECK_SMALL(
          overlap<T>(s1, s2dx2.timesx().timesx()) - integrate_x2_dx2<T>(s1, s2),
          static_cast<T>(60) * tol);
      BOOST_CHECK_SMALL(
          bfx2_dx2.integrate(s1, s2) - integrate_x2_dx2<T>(s1, s2),
          static_cast<T>(100) * tol);
      BOOST_CHECK_SMALL(overlap<T>(s1, s2) - integrate<2 * order>(f1, s1, s2),
                        static_cast<T>(10) * tol);
      BOOST_CHECK_SMALL(
          integrate_x<T>(s1, s2) - integrate<2 * order>(fx, s1, s2),
          static_cast<T>(10) * tol);
    }

    auto s1_d_order = s1.template dx<order>();
    BOOST_TEST(!s1_d_order.isZero());

    auto s1_d_orderp1 = s1.template dx<order + 1>();
    BOOST_CHECK_SMALL(integrate<T>(s1_d_orderp1), tol);
    BOOST_TEST(s1_d_orderp1.isZero());

    auto s1_d_orderp2 = s1.template dx<order + 2>();
    BOOST_CHECK_SMALL(integrate<T>(s1_d_orderp2), tol);
    BOOST_TEST(s1_d_orderp2.isZero());
  }
}

BOOST_AUTO_TEST_CASE(TestIntegration) {
  constexpr double TOL = 1.0e-15;
  testIntegration<double, 2>(TOL);
  testIntegration<double, 3>(TOL);
  testIntegration<double, 4>(TOL);
  testIntegration<double, 5>(TOL);
  testIntegration<double, 6>(TOL);
  testIntegration<double, 7>(TOL);
  testIntegration<double, 8>(TOL);
  testIntegration<double, 9>(TOL);
  testIntegration<double, 10>(TOL);

  if constexpr (sizeof(long double) != sizeof(double)) {
    constexpr long double TOLL = 1.0e-18l;
    testIntegration<long double, 2>(TOLL);
    testIntegration<long double, 3>(TOLL);
    testIntegration<long double, 4>(TOLL);
    testIntegration<long double, 5>(TOLL);
    testIntegration<long double, 6>(TOLL);
    testIntegration<long double, 7>(TOLL);
    testIntegration<long double, 8>(TOLL);
    testIntegration<long double, 9>(TOLL);
    testIntegration<long double, 10>(TOLL);
  }
}

template <typename T, size_t order>
T lc(T x, const std::vector<T> &coeffs,
     const std::vector<okruz::bspline::Spline<T, order>> &splines) {
  T ret = static_cast<T>(0);
  for (size_t i = 0; i < coeffs.size(); i++) {
    ret += coeffs.at(i) * splines.at(i)(x);
  }
  return ret;
}

template <typename T, size_t order>
void testArithmetic(T tol) {
  std::cout.precision(20);
  static_assert(order >= 2, "For this test, order must be at least 2");
  using Spline = okruz::bspline::Spline<T, order>;
  using Spline6 = okruz::bspline::Spline<T, 2 * order>;
  using Spline0 = okruz::bspline::Spline<T, 0>;
  using Spline1 = okruz::bspline::Spline<T, order - 2>;

  BSplineGenerator<T> generator(std::vector<T>{
      -7.0l,  -6.85l, -6.55l, -6.3l, -6.0l, -5.75l, -5.53l, -5.2l,
      -4.75l, -4.5l,  -3.0l,  -2.5l, -1.5l, -1.0l,  0.0l,   0.5l,
      1.5l,   2.5l,   3.5l,   4.0l,  4.35l, 4.55l,  4.95l,  5.4l,
      5.7l,   6.1l,   6.35l,  6.5l,  6.85l, 7.0l});

  const std::vector<Spline> splines =
      generator.template generateBSplines<order + 1>();
  const Spline0 one = getOne(generator.getGrid());

  const std::vector<T> lcCoeffs{1, 2, 3, 4, 3};
  const std::vector<Spline> lcSplines{
      splines[0], splines[1], splines[splines.size() / 2],
      splines[splines.size() - 2], splines[splines.size() - 1]};
  Spline slc = okruz::bspline::linearCombination(
      lcCoeffs.begin(), lcCoeffs.end(), lcSplines.begin(), lcSplines.end());

  for (T x = slc.front(); x <= slc.back(); x += 0.01L) {
    BOOST_CHECK_SMALL(slc(x) - lc(x, lcCoeffs, lcSplines),
                      static_cast<T>(10) * tol);
  }

  for (const auto &s : splines) {
    Spline s2 = s * static_cast<T>(2);
    Spline sm = -s;
    Spline6 sprod = s * s;
    Spline s22 = s;
    s22 *= static_cast<T>(2);
    Spline shalf = s / static_cast<T>(2);
    Spline shalf2 = s;
    shalf2 /= static_cast<T>(2);
    Spline s5half = s2 + shalf;
    Spline s5half2 = s2;
    s5half2 += shalf;
    Spline s3half = s2 - shalf;
    Spline s3half2 = s2;
    s3half2 -= shalf;
    Spline splusone = s + one;
    Spline1 sdx2 = s.dx().dx();
    Spline1 sdx22 = s.dx2();
    Spline sdx0 = s.template dx<0>();

    for (T x = s.front(); x <= s.back(); x += 0.01L) {
      BOOST_CHECK_SMALL(sm(x) + s(x), tol);
      BOOST_CHECK_SMALL(s2(x) - static_cast<T>(2) * s(x), tol);
      BOOST_CHECK_SMALL(s22(x) - static_cast<T>(2) * s(x),
                        tol);  // Tests *= operator
      BOOST_CHECK_SMALL(shalf(x) - s(x) / static_cast<T>(2),
                        tol);  // Tests / operator
      BOOST_CHECK_SMALL(shalf2(x) - s(x) / static_cast<T>(2),
                        tol);  // Tests /= operator
      BOOST_CHECK_SMALL(
          s5half(x) - static_cast<T>(5) * s(x) / static_cast<T>(2),
          tol);  // Tests + operator
      BOOST_CHECK_SMALL(
          s5half2(x) - static_cast<T>(5) * s(x) / static_cast<T>(2),
          tol);  // Tests += operator
      BOOST_CHECK_SMALL(
          s3half(x) - static_cast<T>(3) * s(x) / static_cast<T>(2),
          tol);  // Tests - operator
      BOOST_CHECK_SMALL(
          s3half2(x) - static_cast<T>(3) * s(x) / static_cast<T>(2),
          tol);  // Tests -= operator
      BOOST_CHECK_SMALL(splusone(x) - s(x) - static_cast<T>(1),
                        tol);                      // Tests + operator
      BOOST_CHECK_SMALL(sdx2(x) - sdx22(x), tol);  // Tests dx method
      BOOST_CHECK_SMALL(s(x) - sdx0(x), tol);      // Tests dx method
    }
  }
  BOOST_TEST(static_cast<T>(1) == one(one.front()));
  BOOST_TEST(static_cast<T>(1) == one(one.back()));
}

BOOST_AUTO_TEST_CASE(TestArithmetic) {
  constexpr double TOL = 1.0e-15;
  testArithmetic<double, 2>(TOL);
  testArithmetic<double, 3>(TOL);
  testArithmetic<double, 4>(TOL);
  testArithmetic<double, 5>(TOL);
  testArithmetic<double, 6>(TOL);
  testArithmetic<double, 7>(TOL);
  testArithmetic<double, 8>(TOL);
  testArithmetic<double, 9>(TOL);
  testArithmetic<double, 10>(TOL);

  if constexpr (sizeof(long double) != sizeof(double)) {
    constexpr long double TOLL = 1.0e-18l;
    testArithmetic<long double, 2>(TOLL);
    testArithmetic<long double, 3>(TOLL);
    testArithmetic<long double, 4>(TOLL);
    testArithmetic<long double, 5>(TOLL);
    testArithmetic<long double, 6>(TOLL);
    testArithmetic<long double, 7>(TOLL);
    testArithmetic<long double, 8>(TOLL);
    testArithmetic<long double, 9>(TOLL);
    testArithmetic<long double, 10>(TOLL);
  }
}


#include "laplacian_inpainting.h"

#include <mutex>

#define EIGEN_STACK_ALLOCATION_LIMIT 0
#include <Eigen/Sparse>

#include <tbb/parallel_for.h>
#include <tbb/task_group.h>

namespace possumwood {
namespace opencv {

namespace {

class Triplets;

class Row {
  public:
	Row() = default;

	void addValue(int64_t row, int64_t col, double value) {
		if(value != 0.0)
			m_values[(row << 32) + col] += value;
	}

  private:
	Row(const Row&) = delete;
	Row& operator=(const Row&) = delete;

	std::map<int64_t, double> m_values;

	friend class Triplets;
};

class Triplets {
  public:
	Triplets(int rows, int cols) : m_rowCount(0), m_rows(rows), m_cols(cols) {
	}

	void addRow(const Row& r) {
		for(auto& v : r.m_values) {
			int32_t row = v.first >> 32;
			int32_t col = v.first & 0xffffffff;

			assert(row < m_rows);
			assert(col < m_cols);

			m_triplets.push_back(Eigen::Triplet<double>(m_rowCount, row * m_cols + col, v.second));
		}

		++m_rowCount;
	}

	std::size_t rows() const {
		return m_rowCount;
	}

	const std::vector<Eigen::Triplet<double>>& triplets() const {
		return m_triplets;
	}

  private:
	std::vector<Eigen::Triplet<double>> m_triplets;

	int m_rowCount, m_rows, m_cols;

	friend class Row;
};

static const cv::Mat kernel = (cv::Mat_<double>(3, 3) << 0.0, -1.0, 0.0, -1.0, 4.0, -1.0, 0.0, -1.0, 0.0);

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -1.0, -1.0,
// 	-1.0,  8.0, -1.0,
// 	-1.0, -1.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -2.0, -1.0,
// 	-2.0, 12.0, -2.0,
// 	-1.0, -2.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(5,5) <<
// 	 0.0,  0.0,  1.0,  0.0,  0.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 1.0, -8.0, 20.0, -8.0,  1.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 0.0,  0.0,  1.0,  0.0,  0.0
// );

float buildMatrices(const cv::Mat& image, const cv::Mat& mask, Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b) {
	Triplets triplets(image.rows, image.cols);
	std::vector<double> values;

	std::size_t validCtr = 0, interpolatedCtr = 0;

	for(int y = 0; y < image.rows; ++y)
		for(int x = 0; x < image.cols; ++x) {
			Row row;

			// masked and/or edge
			if(mask.at<unsigned char>(y, x) > 128) {
				values.push_back(0.0f);

				// convolution
				for(int yi = 0; yi < kernel.rows; ++yi)
					for(int xi = 0; xi < kernel.cols; ++xi) {
						int ypos = y + yi - kernel.rows / 2;
						int xpos = x + xi - kernel.cols / 2;

						// handling of edges - "clip" (or "mirror", commented out for now)
						if(ypos < 0)
							// ypos = -ypos;
							ypos = 0;
						if(ypos >= image.rows)
							// ypos = (image.rows-1) - (ypos-image.rows);
							ypos = image.rows - 1;

						if(xpos < 0)
							// xpos = -xpos;
							xpos = 0;
						if(xpos >= image.cols)
							// xpos = (image.cols-1) - (xpos-image.cols);
							xpos = image.cols - 1;

						row.addValue(ypos, xpos, kernel.at<double>(yi, xi));
					}

				++interpolatedCtr;
			}

			// non-masked
			if(mask.at<unsigned char>(y, x) <= 128) {
				values.push_back(image.at<float>(y, x));
				row.addValue(y, x, 1);

				++validCtr;
			}

			triplets.addRow(row);
		}

	// initialise the sparse matrix
	A = Eigen::SparseMatrix<double>(triplets.rows(), image.rows * image.cols);
	A.setFromTriplets(triplets.triplets().begin(), triplets.triplets().end());

	// and the "b" vector
	assert(values.size() == triplets.rows());
	b = Eigen::VectorXd(values.size());
	for(std::size_t i = 0; i < values.size(); ++i)
		b[i] = values[i];

	return (float)validCtr / ((float)validCtr + (float)interpolatedCtr);
}

dependency_graph::State solve(const cv::Mat& input, const cv::Mat& mask, std::vector<float>& output) {
	assert(input.rows == mask.rows);
	assert(input.cols == mask.cols);
	assert(input.type() == CV_32FC1);
	assert(mask.type() == CV_8UC1);

	output = std::vector<float>(input.rows * input.cols, 0.0f);

	Eigen::SparseMatrix<double> A;
	Eigen::VectorXd b, tmp;

	dependency_graph::State state;

	const float ratio = buildMatrices(input, mask, A, b);

	if(ratio > 0.003) {
		const char* stage = "solver construction";

		Eigen::SparseLU<Eigen::SparseMatrix<double> /*, Eigen::NaturalOrdering<int>*/> chol(A);

		if(chol.info() == Eigen::Success) {
			stage = "analyze pattern";

			chol.analyzePattern(A);

			if(chol.info() == Eigen::Success) {
				stage = "factorize";

				chol.factorize(A);

				if(chol.info() == Eigen::Success) {
					stage = "solve";

					tmp = chol.solve(b);

					assert(tmp.size() == input.rows * input.cols);
					for(int i = 0; i < tmp.size(); ++i) {
						const int row = i / input.cols;
						const int col = i % input.cols;
						const int index = row * mask.cols + col;

						assert((std::size_t)index < output.size());
						output[index] = tmp[i];
					}
				}
			}
		}

		if(chol.info() == Eigen::NumericalIssue)
			state.addWarning("Decomposition failed - Eigen::NumericalIssue at stage " + std::string(stage));
		else if(chol.info() == Eigen::NoConvergence)
			state.addWarning("Decomposition failed - Eigen::NoConvergence at stage " + std::string(stage));
		else if(chol.info() == Eigen::InvalidInput)
			state.addWarning("Decomposition failed - Eigen::InvalidInput at stage " + std::string(stage));
		else if(chol.info() != Eigen::Success)
			state.addWarning("Decomposition failed - unknown error at stage " + std::string(stage));
	}

	return state;
}

}  // namespace

dependency_graph::State inpaint(const std::vector<cv::Mat>& inputs,
                                const std::vector<cv::Mat>& masks,
                                std::vector<cv::Mat>& result) {
	if(inputs.size() != masks.size() || inputs.empty() || masks.empty())
		throw std::runtime_error("Laplacian inpainting - number of inputs and masks has to match.");

	for(std::size_t i = 0; i < inputs.size(); ++i) {
		if(inputs[i].depth() != CV_32F)
			throw std::runtime_error("Laplacian inpainting - input image type has to be CV_32F.");
		if(masks[i].depth() != CV_8U || masks[i].type() != masks[0].type())
			throw std::runtime_error("Laplacian inpainting - mask image type has to be CV_8UC1 or CV_8UC3.");
		if(inputs[i].empty() || masks[i].empty())
			throw std::runtime_error("Laplacian inpainting - empty input image and/or mask.");
		if(inputs[i].size != masks[i].size || inputs[i].size != inputs[0].size)
			throw std::runtime_error("Laplacian inpainting - input and mask image size have to match.");
	}

	// split the inputs and masks per channel
	std::vector<cv::Mat> inputs_1ch, masks_1ch;

	for(std::size_t i = 0; i < inputs.size(); ++i) {
		std::vector<cv::Mat> i_tmp, m_tmp;

		cv::split(inputs[i], i_tmp);
		cv::split(masks[i], m_tmp);

		while(m_tmp.size() < i_tmp.size())
			m_tmp.push_back(m_tmp.back());

		inputs_1ch.insert(inputs_1ch.end(), i_tmp.begin(), i_tmp.end());
		masks_1ch.insert(masks_1ch.end(), m_tmp.begin(), m_tmp.end());
	}

	std::vector<std::vector<float>> x(inputs_1ch.size(), std::vector<float>(inputs[0].rows * inputs[0].cols, 0.0f));

	dependency_graph::State state;

	tbb::task_group tasks;
	std::mutex state_mutex;

	for(std::size_t channel = 0; channel < inputs_1ch.size(); ++channel) {
		tasks.run([channel, &inputs_1ch, &masks_1ch, &x, &state, &state_mutex]() {
			const dependency_graph::State currentState = solve(inputs_1ch[channel], masks_1ch[channel], x[channel]);

			std::lock_guard<std::mutex> guard(state_mutex);
			state.append(currentState);
		});
	}

	tasks.wait();

	result = std::vector<cv::Mat>();

	int channel = 0;
	for(auto& in : inputs) {
		cv::Mat tmp = cv::Mat::zeros(in.rows, in.cols, in.type());

		tbb::parallel_for(0, tmp.rows, [&](int yi) {
			for(int xi = 0; xi < tmp.cols; ++xi)
				for(int c = 0; c < in.channels(); ++c)
					tmp.ptr<float>(yi, xi)[c] = x[c + channel][yi * tmp.cols + xi];
		});

		channel += in.channels();
		result.push_back(tmp);
	}

	return state;
}

}  // namespace opencv
}  // namespace possumwood


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

int
main()
{
  using namespace boost;
  GraphvizGraph g_dot;
  read_graphviz("figs/telephone-network.dot", g_dot);

  typedef adjacency_list < vecS, vecS, undirectedS, no_property,
    property < edge_weight_t, int > > Graph;
  Graph g(num_vertices(g_dot));
  property_map < GraphvizGraph, edge_attribute_t >::type
    edge_attr_map = get(edge_attribute, g_dot);
  graph_traits < GraphvizGraph >::edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = edges(g_dot); ei != ei_end; ++ei) {
    int weight = lexical_cast < int >(edge_attr_map[*ei]["label"]);
    property < edge_weight_t, int >edge_property(weight);
    add_edge(source(*ei, g_dot), target(*ei, g_dot), edge_property, g);
  }

  std::vector < graph_traits < Graph >::edge_descriptor > mst;
  kruskal_minimum_spanning_tree(g, std::back_inserter(mst));

  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
  int total_weight = 0;
  for (int e = 0; e < mst.size(); ++e)
    total_weight += get(weight, mst[e]);
  std::cout << "total weight: " << total_weight << std::endl;

  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  for (int i = 0; i < mst.size(); ++i) {
    Vertex u = source(mst[i], g), v = target(mst[i], g);
    edge_attr_map[edge(u, v, g_dot).first]["color"] = "black";
  }
  std::ofstream out("figs/telephone-mst-kruskal.dot");
  graph_property < GraphvizGraph, graph_edge_attribute_t >::type &
    graph_edge_attr_map = get_property(g_dot, graph_edge_attribute);
  graph_edge_attr_map["color"] = "gray";
  graph_edge_attr_map["style"] = "bold";
  write_graphviz(out, g_dot);

  return EXIT_SUCCESS;
}


/**
 * @file fixed_point.cpp
 * @author Salvatore Cardamone
 * @brief Unit test for FixedPoint class.
 */
#define BOOST_TEST_MODULE FixedPointTest
#include <random>
#include <boost/test/included/unit_test.hpp>
#include "utilities/fixed_point.hpp"

/**
 * This case tests construction using all datatypes used for storage of the
 * fixed point representation, and simultaneously minimum and maximum values for
 * the associated representation.
 */
BOOST_AUTO_TEST_CASE( constructors ) {

  // Q4.4 representation, unsigned
  tycheplusplus::FixedPoint<unsigned char,4> us_char(15.0);
  BOOST_CHECK_EQUAL(us_char.AsDouble(), 15.0);
  // Q3.4 representation, signed
  tycheplusplus::FixedPoint<signed char,4> char_pos( 7.0);
  tycheplusplus::FixedPoint<signed char,4> char_neg(-7.0);
  BOOST_CHECK_EQUAL(char_pos.AsDouble(),  7.0);
  BOOST_CHECK_EQUAL(char_neg.AsDouble(), -7.0);

  // Q8.8 representation, unsigned
  tycheplusplus::FixedPoint<unsigned short,8> us_short(255.0);
  BOOST_CHECK_EQUAL(us_short.AsDouble(), 255.0);
  // Q7.8 representation, signed
  tycheplusplus::FixedPoint<signed short,8> short_pos( 127.0);
  tycheplusplus::FixedPoint<signed short,8> short_neg(-127.0);
  BOOST_CHECK_EQUAL(short_pos.AsDouble(),  127.0);
  BOOST_CHECK_EQUAL(short_neg.AsDouble(), -127.0);

  // Q16.16 representation, unsigned
  tycheplusplus::FixedPoint<unsigned int,16> us_int(65535.0);
  BOOST_CHECK_EQUAL(us_int.AsDouble(), 65535.0);
  // Q15.16 representation, signed
  tycheplusplus::FixedPoint<signed int,16> int_pos( 32767.0);
  tycheplusplus::FixedPoint<signed int,16> int_neg(-32767.0);
  BOOST_CHECK_EQUAL(int_pos.AsDouble(),  32767.0);
  BOOST_CHECK_EQUAL(int_neg.AsDouble(), -32767.0);

  // Q32.32 representation, unsigned
  tycheplusplus::FixedPoint<unsigned long int,32> us_long(4294967296.0);
  BOOST_CHECK_EQUAL(us_long.AsDouble(), 4294967296.0);
  // Q31.32 representation, signed
  tycheplusplus::FixedPoint<signed long int,32> long_pos( 2147483647.0);
  tycheplusplus::FixedPoint<signed long int,32> long_neg(-2147483647.0);
  BOOST_CHECK_EQUAL(long_pos.AsDouble(),  2147483647.0);
  BOOST_CHECK_EQUAL(long_neg.AsDouble(), -2147483647.0);

}

BOOST_AUTO_TEST_CASE( addition ) {

  tycheplusplus::FixedPoint<unsigned char,4> a(5.0), b(6.0);
  auto c = a + b;
  BOOST_CHECK_EQUAL(c.AsDouble(), 11.0);

}

/**
 * Test the exponential functionality, both signed and unsigned with positive
 * and negative arguments.
 */
BOOST_AUTO_TEST_CASE( exponential ) {

  constexpr int n_samples = 2048;

  // We'll use the Mersenne Twister for our PRNG
  std::random_device rd;
  std::mt19937 gen(rd());

  std::uniform_real_distribution<double> us_char_dist(0.0, 1.0);
  for (auto i = 0; i < n_samples; ++i) {
    auto val = us_char_dist(gen);
    tycheplusplus::FixedPoint<unsigned char,4,4> arg(val);
    auto exp_result = exp(arg);
    auto diff = fabs(exp_result.AsDouble() - exp(val));
    BOOST_CHECK_SMALL(diff, 1.0);
  }

  std::uniform_real_distribution<double> us_short_dist(0.0, 2.0);
  for (auto i = 0; i < n_samples; ++i) {
    auto val = us_short_dist(gen);
    tycheplusplus::FixedPoint<unsigned short,8,8> arg(val);
    auto exp_result = exp(arg);
    auto diff = fabs(exp_result.AsDouble() - exp(val));
    BOOST_CHECK_SMALL(diff, 0.2);
  }

  std::uniform_real_distribution<double> us_int_dist(0.0, 3.0);
  for (auto i = 0; i < n_samples; ++i) {
    auto val = us_int_dist(gen);
    tycheplusplus::FixedPoint<unsigned int,16,16> arg(val);
    auto exp_result = exp(arg);
    auto diff = fabs(exp_result.AsDouble() - exp(val));
    BOOST_CHECK_SMALL(diff, 0.01);
  }

  std::uniform_real_distribution<double> us_long_dist(0.0, 4.0);
  for (auto i = 0; i < n_samples; ++i) {
    auto val = us_long_dist(gen);
    tycheplusplus::FixedPoint<unsigned long,32,32> arg(val);
    auto exp_result = exp(arg);
    auto diff = fabs(exp_result.AsDouble() - exp(val));
    BOOST_CHECK_SMALL(diff, 0.0000001);
  }

}


//  Copyright (c) 2018 Cem Bassoy
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <boost/numeric/ublas/tensor/operators_comparison.hpp>
#include <boost/numeric/ublas/tensor/operators_arithmetic.hpp>
#include <boost/numeric/ublas/tensor.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE(test_tensor_comparison/*, * boost::unit_test::depends_on("test_tensor")*/)

using double_extended = boost::multiprecision::cpp_bin_float_double_extended;

using test_types = zip<int,float,double_extended>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture {
  using extents_type = boost::numeric::ublas::extents<>;
  fixture()
    : extents{
        //                extents_type{},    // 0
        extents_type{1,1}, // 1
        extents_type{1,2}, // 2
        extents_type{2,1}, // 3
        extents_type{2,3}, // 4
        extents_type{2,3,1}, // 5
        extents_type{4,1,3}, // 6
        extents_type{1,2,3}, // 7
        extents_type{4,2,3}, // 8
        extents_type{4,2,3,5}} // 9
  {
  }
  std::vector<extents_type> extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;
  using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

  auto check = [](auto const& e)
  {
    auto t  = tensor_type (e);
    auto t2 = tensor_type (e);
    auto v  = value_type  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);

    BOOST_CHECK( t == t  );
    BOOST_CHECK( t != t2 );

    if(t.empty())
      return;

    BOOST_CHECK(!(t < t));
    BOOST_CHECK(!(t > t));
    BOOST_CHECK( t < t2 );
    BOOST_CHECK( t2 > t );
    BOOST_CHECK( t <= t );
    BOOST_CHECK( t >= t );
    BOOST_CHECK( t <= t2 );
    BOOST_CHECK( t2 >= t );
    BOOST_CHECK( t2 >= t2 );
    BOOST_CHECK( t2 >= t );
  };

  for(auto const& e : extents)
    check(e);

  auto e0 = extents.at(0);
  auto e1 = extents.at(1);
  auto e2 = extents.at(2);

  auto b = false;
  BOOST_CHECK_NO_THROW ( b = (tensor_type(e0) == tensor_type(e0)));
  BOOST_CHECK_NO_THROW ( b = (tensor_type(e1) == tensor_type(e2)));
  BOOST_CHECK_NO_THROW ( b = (tensor_type(e0) == tensor_type(e2)));
  BOOST_CHECK_NO_THROW ( b = (tensor_type(e1) != tensor_type(e2)));

  BOOST_CHECK_THROW    ( b = (tensor_type(e1) >= tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    ( b = (tensor_type(e1) <= tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    ( b = (tensor_type(e1) <  tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    ( b = (tensor_type(e1) >  tensor_type(e2)), std::runtime_error  );

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison_with_tensor_expressions, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;
  using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

  auto check = [](auto const& e)
  {
    auto t  = tensor_type (e);
    auto t2 = tensor_type (e);
    auto v  = value_type  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);

    BOOST_CHECK( t == t  );
    BOOST_CHECK( t != t2 );

    if(t.empty())
      return;

    BOOST_CHECK( !(t < t) );
    BOOST_CHECK( !(t > t) );
    BOOST_CHECK( t < (t2+t) );
    BOOST_CHECK( (t2+t) > t );
    BOOST_CHECK( t <= (t+t) );
    BOOST_CHECK( (t+t2) >= t );
    BOOST_CHECK( (t2+t2+2) >= t);
    BOOST_CHECK( 2*t2 > t );
    BOOST_CHECK( t < 2*t2 );
    BOOST_CHECK( 2*t2 > t);
    BOOST_CHECK( 2*t2 >= t2 );
    BOOST_CHECK( t2 <= 2*t2);
    BOOST_CHECK( 3*t2 >= t );

  };

  for(auto const& e : extents)
    check(e);

  auto e0 = extents.at(0);
  auto e1 = extents.at(1);
  auto e2 = extents.at(2);

  auto b = false;
  BOOST_CHECK_NO_THROW (b = tensor_type(e0) == (tensor_type(e0) + tensor_type(e0))  );
  BOOST_CHECK_NO_THROW (b = tensor_type(e1) == (tensor_type(e2) + tensor_type(e2))  );
  BOOST_CHECK_NO_THROW (b = tensor_type(e0) == (tensor_type(e2) + 2) );
  BOOST_CHECK_NO_THROW (b = tensor_type(e1) != (2 + tensor_type(e2)) );

  BOOST_CHECK_NO_THROW (b = (tensor_type(e0) + tensor_type(e0)) == tensor_type(e0) );
  BOOST_CHECK_NO_THROW (b = (tensor_type(e2) + tensor_type(e2)) == tensor_type(e1) );
  BOOST_CHECK_NO_THROW (b = (tensor_type(e2) + 2)               == tensor_type(e0) );
  BOOST_CHECK_NO_THROW (b = (2 + tensor_type(e2))               != tensor_type(e1) );

  BOOST_CHECK_THROW    (b = tensor_type(e1) >= (tensor_type(e2) + tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    (b = tensor_type(e1) <= (tensor_type(e2) + tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    (b = tensor_type(e1) <  (tensor_type(e2) + tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    (b = tensor_type(e1) >  (tensor_type(e2) + tensor_type(e2)), std::runtime_error  );

  BOOST_CHECK_THROW    (b = tensor_type(e1) >= (tensor_type(e2) + 2), std::runtime_error  );
  BOOST_CHECK_THROW    (b = tensor_type(e1) <= (2 + tensor_type(e2)), std::runtime_error  );
  BOOST_CHECK_THROW    (b = tensor_type(e1) <  (tensor_type(e2) + 3), std::runtime_error  );
  BOOST_CHECK_THROW    (b = tensor_type(e1) >  (4 + tensor_type(e2)), std::runtime_error  );

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison_with_scalar, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;
  using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

  auto check = [](auto const& e)
  {

    BOOST_CHECK( tensor_type(e,value_type{2}) == tensor_type(e,value_type{2})  );
    BOOST_CHECK( tensor_type(e,value_type{2}) != tensor_type(e,value_type{1})  );

    if(ublas::empty(e))
      return;

    BOOST_CHECK( !(tensor_type(e,2) <  2) );
    BOOST_CHECK( !(tensor_type(e,2) >  2) );
    BOOST_CHECK(  (tensor_type(e,2) >= 2) );
    BOOST_CHECK(  (tensor_type(e,2) <= 2) );
    BOOST_CHECK(  (tensor_type(e,2) == 2) );
    BOOST_CHECK(  (tensor_type(e,2) != 3) );

    BOOST_CHECK( !(2 >  tensor_type(e,2)) );
    BOOST_CHECK( !(2 <  tensor_type(e,2)) );
    BOOST_CHECK(  (2 <= tensor_type(e,2)) );
    BOOST_CHECK(  (2 >= tensor_type(e,2)) );
    BOOST_CHECK(  (2 == tensor_type(e,2)) );
    BOOST_CHECK(  (3 != tensor_type(e,2)) );

    BOOST_CHECK( !( tensor_type(e,2)+3 <  5) );
    BOOST_CHECK( !( tensor_type(e,2)+3 >  5) );
    BOOST_CHECK(  ( tensor_type(e,2)+3 >= 5) );
    BOOST_CHECK(  ( tensor_type(e,2)+3 <= 5) );
    BOOST_CHECK(  ( tensor_type(e,2)+3 == 5) );
    BOOST_CHECK(  ( tensor_type(e,2)+3 != 6) );

    BOOST_CHECK( !( 5 >  tensor_type(e,2)+3) );
    BOOST_CHECK( !( 5 <  tensor_type(e,2)+3) );
    BOOST_CHECK(  ( 5 >= tensor_type(e,2)+3) );
    BOOST_CHECK(  ( 5 <= tensor_type(e,2)+3) );
    BOOST_CHECK(  ( 5 == tensor_type(e,2)+3) );
    BOOST_CHECK(  ( 6 != tensor_type(e,2)+3) );

    BOOST_CHECK( !( tensor_type(e,2)+tensor_type(e,3) <  5) );
    BOOST_CHECK( !( tensor_type(e,2)+tensor_type(e,3) >  5) );
    BOOST_CHECK(  ( tensor_type(e,2)+tensor_type(e,3) >= 5) );
    BOOST_CHECK(  ( tensor_type(e,2)+tensor_type(e,3) <= 5) );
    BOOST_CHECK(  ( tensor_type(e,2)+tensor_type(e,3) == 5) );
    BOOST_CHECK(  ( tensor_type(e,2)+tensor_type(e,3) != 6) );

    BOOST_CHECK( !( 5 >  tensor_type(e,2)+tensor_type(e,3)) );
    BOOST_CHECK( !( 5 <  tensor_type(e,2)+tensor_type(e,3)) );
    BOOST_CHECK(  ( 5 >= tensor_type(e,2)+tensor_type(e,3)) );
    BOOST_CHECK(  ( 5 <= tensor_type(e,2)+tensor_type(e,3)) );
    BOOST_CHECK(  ( 5 == tensor_type(e,2)+tensor_type(e,3)) );
    BOOST_CHECK(  ( 6 != tensor_type(e,2)+tensor_type(e,3)) );

  };

  for(auto const& e : extents)
    check(e);

}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/**
 * \file
 * \brief Homography and derived class functionality regression tests
 */

#include <test_eigen.h>

#include <vital/exceptions/math.h>
#include <vital/types/homography.h>
#include <vital/types/homography_f2f.h>

#include <Eigen/LU>

// ----------------------------------------------------------------------------
int main(int argc, char** argv)
{
  ::testing::InitGoogleTest( &argc, argv );
  return RUN_ALL_TESTS();
}

// ----------------------------------------------------------------------------
template <typename T>
class homography : public ::testing::Test
{
};

using types = ::testing::Types<float, double>;
TYPED_TEST_CASE(homography, types);

// ----------------------------------------------------------------------------
// Test invert function Eigen::Matrix derived classes
TYPED_TEST(homography, inversion)
{
  using homography_t = typename kwiver::vital::homography_<TypeParam>;

  homography_t invertible;
  homography_t expected_result;
  homography_t noninvertable;
  homography_t ni_result;

  invertible.get_matrix() << 1, 1, 2,
                             3, 4, 5,
                             6, 7, 9;
  expected_result.get_matrix() << -0.5, -2.5,  1.5,
                                  -1.5,  1.5, -0.5,
                                   1.5,  0.5, -0.5;

  homography_t h_inverse( *invertible.inverse() );
  EXPECT_MATRIX_EQ( expected_result.matrix(), h_inverse.matrix() );

  noninvertable.get_matrix() << 1, 2, 3,
                                4, 5, 6,
                                7, 8, 9;
  bool is_invertible;
  noninvertable.get_matrix().computeInverseWithCheck(
    ni_result.get_matrix(), is_invertible );
  EXPECT_FALSE( is_invertible );
}

// ----------------------------------------------------------------------------
// Test mapping a point for a homography/point data type
TYPED_TEST(homography, map_point_zero_div)
{
  using homography_t = typename kwiver::vital::homography_<TypeParam>;
  using vector_t = typename Eigen::Matrix<TypeParam, 2, 1>;

  vector_t test_p{ 1, 1 };
  TypeParam e = Eigen::NumTraits<TypeParam>::dummy_precision();

  // Where [2,2] = 0
  homography_t h_0;
  h_0.get_matrix() << 1.0, 0.0, 1.0,
                      0.0, 1.0, 1.0,
                      0.0, 0.0, 0.0;
  EXPECT_THROW(
    h_0.map_point( test_p ),
    kwiver::vital::point_maps_to_infinity )
    << "Applying point to matrix with 0-value lower-right corner";

  // Where [2,2] = e, which is the approximately-zero threshold
  homography_t h_e;
  h_e.get_matrix() << 1.0, 0.0, 1.0,
                      0.0, 1.0, 1.0,
                      0.0, 0.0,  e ;
  EXPECT_THROW(
    h_e.map_point( test_p ),
    kwiver::vital::point_maps_to_infinity )
    << "Applying point to matrix with e-value lower-right corner";

  // Where [2,2] = 0.5, which should be valid.
  homography_t h_half;
  h_half.get_matrix() << 1.0, 0.0, 1.0,
                         0.0, 1.0, 1.0,
                         0.0, 0.0, 0.5;
  EXPECT_MATRIX_NEAR( ( vector_t{ 4, 4 } ), h_half.map_point( test_p ), e );
}

// ----------------------------------------------------------------------------
TYPED_TEST(homography, map_point)
{
  using homography_t = typename kwiver::vital::homography_<TypeParam>;
  using vector_t = typename Eigen::Matrix<TypeParam, 2, 1>;

  // Identity transformation
  homography_t h;
  vector_t p{ static_cast<TypeParam>( 2.2 ), static_cast<TypeParam>( 5.5 ) };
  EXPECT_EQ( p, h.map_point( p ) );
}

// ----------------------------------------------------------------------------
TEST(f2f_homography, inversion)
{
  // Testing from and to frame swapping during inversion
  kwiver::vital::matrix_3x3d i{ kwiver::vital::matrix_3x3d::Identity() };
  kwiver::vital::f2f_homography h{ i, 0, 10 };
  kwiver::vital::f2f_homography h_inv = h.inverse();

  EXPECT_EQ( 10, h_inv.from_id() );
  EXPECT_EQ( 0, h_inv.to_id() );
}


#ifndef RAYCASTING_HPP_OLVFBMND
#define RAYCASTING_HPP_OLVFBMND

#include <Eigen/Core>

#include <pcl_util/point_types.hpp>

#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/box.hpp"
#include "scrollgrid/ray.hpp"
#include "scrollgrid/scrollgrid3.hpp"
#include "scrollgrid/dense_array3.hpp"

namespace ca
{

/**
 * Axis-aligned bounding box intersection test.
 * Reference:
 * An Efficient and Robust Ray–Box Intersection Algorithm, Williams et al. 2004
 */
template<typename Scalar>
bool aabb_ray_intersect(const ca::scrollgrid::Box<Scalar, 3>& box,
                        ca::scrollgrid::Ray3<Scalar> &r) {
  Scalar tmin = (box.bound(r.sign[0]).x() - r.origin.x()) * r.invdir.x();
  Scalar tmax = (box.bound(1-r.sign[0]).x() - r.origin.x()) * r.invdir.x();

  Scalar tymin = (box.bound(r.sign[1]).y() - r.origin.y()) * r.invdir.y();
  Scalar tymax = (box.bound(1-r.sign[1]).y() - r.origin.y()) * r.invdir.y();

  if ((tmin > tymax) || (tymin > tmax)) return false;
  if (tymin > tmin) tmin = tymin;
  if (tymax < tmax) tmax = tymax;

  Scalar tzmin = (box.bound(r.sign[2]).z() - r.origin.z()) * r.invdir.z();
  Scalar tzmax = (box.bound(1-r.sign[2]).z() - r.origin.z()) * r.invdir.z();

  if ((tmin > tzmax) || (tzmin > tmax)) return false;
  if (tzmin > tmin) tmin = tzmin;
  if (tzmax < tmax) tmax = tzmax;
  if (tmin > r.tmin) r.tmin = tmin;
  if (tmax < r.tmax) r.tmax = tmax;
  return true;
}

/**
 * Trace a straight line from start_pos to end_pos.
 * At each step fun(i, j, k) is called.
 *
 * NOTE start_pos and end_pos should be inside the grid
 *
 * Reference: graphics gems article
 * TODO consider DDA-type raytracing.
 */
template<class TraceFunctor>
void bresenham_trace(const Vec3Ix& start_pos,
                     const Vec3Ix& end_pos,
                     const TraceFunctor& fun) {
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
      fun(x, y, z);
      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {
      // SetP ( grid,x,y,z,end_pos, atMax, count);
      fun(x, y, z);
      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
      fun(x, y, z);
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }
}

/**
 * Simply increment a counter in densearray3 for each step along the way.
 */
template<class GridScalar, class ArrayScalar>
void bresenham_trace_simple(const Vec3Ix& start_pos,
                            const Vec3Ix& end_pos,
                            const ca::ScrollGrid3<GridScalar>& grid3,
                            ca::DenseArray3<ArrayScalar>& array3
                            ) {
  //int ray_ctr = 0;
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {
      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      array3[mem_ix] += 1;
      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {
      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      array3[mem_ix] += 1;
      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {
      grid_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      array3[mem_ix] += 1;
      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }
}

}
#endif /* end of include guard: RAYCASTING_HPP_OLVFBMND */


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>

#include <boost/chrono.hpp>
#include <boost/graph/iteration_macros.hpp>

#include "GraphDefs.h"
#include "timer.h"
#include "GraphUtil.h"
#include "GraphParse.h"
#include "GraphGen.h"

#include <fstream>
#include <string>

#define SHOW_DEBUG false

const int vertexIdx = 0; // Arbitrary index id

void printGraph(const char* title, VertexVector* vertices, EdgeVector* edges, bool printVertices = false) {
	std::cout << std::endl << "=== " << title << std::endl;

	if (printVertices) {
		// Iterate through the vertices and print them out
		for (int i = 0; i < vertices->size(); i++) {
			CGALPoint* v = (*vertices)[i];
			std::cout << "index: " << i << " (" << v->x() << ", " << v->y() << ")" << std::endl;
		}
	}

	// Iterate through the edges and print them out
	std::cout << std::endl << "Edges: " << std::endl;
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		//std::cout << edgeWeightMap->at(e).weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
		std::cout << e->u << " " << e->v << " (" << (*src) << ") (" << (*tar) << ")" << std::endl;
	}

	std::cout << std::endl;
}

void printGDFGraph(const char* fileName, VertexVector* vertices, EdgeVector* edges) {
	std::ofstream myfile;
	myfile.open(fileName, std::ios::out | std::ios::in);

	myfile << "nodedef> name VARCHAR,label VARCHAR,width DOUBLE,height DOUBLE,x DOUBLE,y DOUBLE,color VARCHAR" << std::endl;

	// Iterate through the vertices and print them out
	boost::unordered_map<CGALPoint*, VertexIndex> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* v = (*vertices)[i];
		myfile << i << "," << i << ",10.0,10.0," << (*v).x() << "," << (*v).y() << ",'153,153,153'" << std::endl;
		vertexHandles.emplace(v, i);
	}

	myfile << "edgedef> node1,node2,weight DOUBLE,directed BOOLEAN,color VARCHAR" << std::endl;

	// Iterate through the edges and print them out
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		VertexIndex srcInd = vertexHandles[src];
		VertexIndex tarInd = vertexHandles[tar];
		//EdgeWeight weight = CGAL::squared_distance(*src, *tar);
		EdgeWeight weight = 1.0;
		myfile << srcInd << "," << tarInd << "," << weight << ",false,'128,128,128'" << std::endl;
	}

	myfile.close();
}

TriVertexHandle OppositeOfEdge(TriVertexHandle ev0, TriVertexHandle ev1, TriFaceHandle f) {
	TriVertexHandle v0 = f->vertex(0);
	if (v0 != ev0 && v0 != ev1) {
		return v0;
	}

	TriVertexHandle v1 = f->vertex(1);
	if (v1 != ev0 && v1 != ev1) {
		return v1;
	}

	TriVertexHandle v2 = f->vertex(2);
	if (v2 != ev0 && v2 != ev1) {
		return v2;
	}

	assert(false); // Unreachable
	return NULL;
}

bool IsLocallyDelaunay(CGALPoint* p, CGALPoint* q, CGALPoint* r, CGALPoint* t) {
	if (SHOW_DEBUG) {
		//std::cout << "(" << (*p) << ") (" << (*q) << ") (" << (*r) << ") (" << (*t) << ")" << std::endl;
	}
	CGALCircle c(*p, *q, *r);
	CGAL::Bounded_side side = c.bounded_side(*t);

	// We consider ON_BOUNDARY because if a constraint edge is on the boundary and the DT flip of quadrilateral has the same total angle, i.e. both
	// flips are DT, then we have to record the constraint so that the CDT picks the right flip in the subgraph check.
	return side == CGAL::Bounded_side::ON_UNBOUNDED_SIDE;

	// v is outside or on the circumcircle of f
	// return side == CGAL::Bounded_side::ON_UNBOUNDED_SIDE
		//|| side == CGAL::Bounded_side::ON_BOUNDARY;
}

CDT* computeCdt(VertexVector* vertices, EdgeVector* edges, boost::unordered_map<TriVertexHandle, VertexIndex>** handlesToIndex) {
	CDT* cdt = new CDT();

	(*handlesToIndex) = new boost::unordered_map<TriVertexHandle, VertexIndex>();
	boost::unordered_map<VertexIndex, TriVertexHandle> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* pt = (*vertices)[i];
		TriVertexHandle vHandle = cdt->insert(*pt);
		vertexHandles.emplace(i, vHandle);
		(*handlesToIndex)->emplace(vHandle, i);
	}

	// Insert constraint edges
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* edge = (*edges)[i];
		VertexIndex u = edge->u;
		VertexIndex v = edge->v;
		TriVertexHandle uH = vertexHandles[u];
		TriVertexHandle vH = vertexHandles[v];
		cdt->insert_constraint(uH, vH);
	}

	assert(cdt->is_valid());

	return cdt;
}

// Given 3 colinear points, if the 2 furthest points are constrained, how is this delaunay triangulated?
// It seems the outcome is implementation dependent. CGAL will split the constraint into 2 edges.
// Other implementations might allow for overlapping, collinear edges. Either way, the current plan
// is to assume that an input forest, F, with collinear edges will be replaced by smaller constraint edges.
EdgeVector* newConstraintSetFromCt(CDT* cdt, VertexVector* originalVertices) {
	EdgeVector* newEdgeVector = new EdgeVector();

	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < originalVertices->size(); i++) {
		vertexIndex[*(*originalVertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);

		/*CGALPoint cgal_u = segement.point(0);
		double uxpos = CGAL::to_double(cgal_u.x());
		double uypos = CGAL::to_double(cgal_u.y());

		CGALPoint cgal_v = segement.point(1);
		double vxpos = CGAL::to_double(cgal_v.x());
		double vypos = CGAL::to_double(cgal_v.y());*/

		if (cdt->is_constrained(cgal_e)) {
			// Assumes point coord are unique
			CGALSegment segement = cdt->segment(cgal_e);
			CGALPoint cgal_u = segement.point(0);
			CGALPoint cgal_v = segement.point(1);
			VertexIndex u = vertexIndex[cgal_u];
			VertexIndex v = vertexIndex[cgal_v];

			SimpleEdge* edge = new SimpleEdge(u, v, 0);
			newEdgeVector->push_back(edge);
		}
	}
	return newEdgeVector;
}

EdgeVector* convertCdtToGraph(VertexVector* vertices, CDT* cdt) {
	EdgeVector* edgeVec = new EdgeVector();

	// Map CGALPoint -> VertexIndex
	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < vertices->size(); i++) {
		vertexIndex[*(*vertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);
		CGALPoint cgal_u = segement.point(0);
		CGALPoint cgal_v = segement.point(1);
		VertexIndex u = vertexIndex[cgal_u];
		VertexIndex v = vertexIndex[cgal_v];

		SimpleEdge* edge = new SimpleEdge(u, v, 0);
		edgeVec->push_back(edge);
	}

	return edgeVec;
}

void computeNonLocallyDelaunay(
	VertexVector* vertices,
	EdgeVector* edges,
	EdgeVector** NewEdges,
	EdgeVector** S_Edges) {

	boost::chrono::high_resolution_clock::time_point start;
	boost::chrono::high_resolution_clock::time_point end;
	boost::chrono::milliseconds duration(0);
	boost::chrono::milliseconds total(0);

	// Compute CDT(F)
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdt = computeCdt(vertices, edges, &handlesToIndex);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("CDT(F)", duration);

	// Replace F with NewF
	start = boost::chrono::high_resolution_clock::now();
	(*NewEdges) = newConstraintSetFromCt(cdt, vertices);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("F -> NewF", duration);

	// Compute Non-Locally Delaunay edges
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_set<TriEdge>* S = new boost::unordered_set<TriEdge>();

	TriVertexHandle infiniteVertex = cdt->infinite_vertex();
	TriFaceHandle infiniteFace = cdt->infinite_face();
	int edgeCount = 0;

	for (FiniteEdgeIter iter = cdt->finite_edges_begin(); iter != cdt->finite_edges_end(); ++iter) {
		edgeCount++;

		// typedef std::pair<Face_handle, int> Edge;
		TriEdge e = *iter;
		int eIndex = e.second;

		// Edge shared by faces f0 and f1
		TriFaceHandle f0 = e.first;
		TriFaceHandle f1 = e.first->neighbor(eIndex);

		// Vertex opposite of edge e in f0
		TriVertexHandle opp0 = f0->vertex(eIndex);

		// Vertex of edge e
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		TriVertexHandle opp1 = OppositeOfEdge(e0, e1, f1);

		if (SHOW_DEBUG) {
			// Vertex endpoint v0, v1 of edge e for f0 (doesn't seem to be possible to identify the same edge for f1 in a similar way)
			TriVertexHandle v0f0 = f0->vertex(f0->cw(eIndex));
			TriVertexHandle v1f0 = f0->vertex(f0->ccw(eIndex));

			//std::cout << eIndex << std::endl;
			std::cout << (*handlesToIndex)[v0f0] << " " << (*handlesToIndex)[v1f0] << " (" << *v0f0 << ") (" << *v1f0 << ") " << (*handlesToIndex)[opp0] << " (" << *opp0 << ")" << std::endl;
		}

		bool addToS = false;

		if (opp0 != infiniteVertex && f1 != infiniteFace) {
			TriVertexHandle pH = f1->vertex(0);
			TriVertexHandle qH = f1->vertex(1);
			TriVertexHandle rH = f1->vertex(2);

			if (pH != infiniteVertex && qH != infiniteVertex && rH != infiniteVertex) {
				CGALPoint p = pH->point();
				CGALPoint q = qH->point();
				CGALPoint r = rH->point();
				CGALPoint t = opp0->point();
				if (!IsLocallyDelaunay(&p, &q, &r, &t)) {
					addToS = true;
				}
			}
		}

		if (opp1 != infiniteVertex && f0 != infiniteFace) {
			TriVertexHandle pH = f0->vertex(0);
			TriVertexHandle qH = f0->vertex(1);
			TriVertexHandle rH = f0->vertex(2);

			if (pH != infiniteVertex && qH != infiniteVertex && rH != infiniteVertex) {
				CGALPoint p = pH->point();
				CGALPoint q = qH->point();
				CGALPoint r = rH->point();
				CGALPoint t = opp1->point();
				if (!IsLocallyDelaunay(&p, &q, &r, &t)) {
					addToS = true;
				}
			}
		}

		if (addToS) {
			S->emplace(e);
		}
	}

	(*S_Edges) = new EdgeVector();
	for (boost::unordered_set<TriEdge>::iterator iter = S->begin(); iter != S->end(); ++iter) {
		TriEdge e = *iter;
		int eIndex = e.second;
		TriFaceHandle f0 = e.first;
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		VertexIndex u = (*handlesToIndex)[e0];
		VertexIndex v = (*handlesToIndex)[e1];

		(*S_Edges)->push_back(new SimpleEdge(u, v, 0));
	}

	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("computeNonLocallyDelaunay duration", duration);

	printDuration("Total", total);

	delete S;
	delete cdt;
	delete handlesToIndex;
}

EdgeVector* intersectInputSetWithConstraintSet(EdgeVector* inputSet, EdgeVector* constraintSet) {
	boost::unordered_set<SimpleEdge>* NewEdges_Hashset = new boost::unordered_set<SimpleEdge>();
	for (int i = 0; i < inputSet->size(); i++) {
		SimpleEdge* e = (*inputSet)[i];
		NewEdges_Hashset->emplace((*e));
	}

	EdgeVector* intersect = new EdgeVector();
	for (int i = 0; i < constraintSet->size(); i++) {
		SimpleEdge* se = (*constraintSet)[i];
		if (NewEdges_Hashset->count(*se) > 0) {
			intersect->push_back(new SimpleEdge(se->u, se->v, se->weight));
		}
	}

	return intersect;
}

bool containsEdge(boost::unordered_set<SimpleEdge>* edgeSet, SimpleEdge* edge) {
	SimpleEdge se(edge->u, edge->v, 0);
	return edgeSet->count(se) > 0;
}

// True if A a subgraph of B
bool isSubgraph(VertexVector* vertices, EdgeVector* a, EdgeVector* b) {
	boost::unordered_set<SimpleEdge>* bEdgeSet = createSimpleEdgeSet(b);

	bool result = true;

	// Iterate through the edges
	for (int i = 0; i < a->size(); i++) {
		SimpleEdge* edge = (*a)[i];
		if (!containsEdge(bEdgeSet, edge)) {
			CGALPoint* u = (*vertices)[edge->u];
			CGALPoint* v = (*vertices)[edge->v];
			//EdgeWeight weight = CGAL::squared_distance(*u, *v);
			CGAL::Lazy_exact_nt<CGAL::Gmpq> exactWeight = CGAL::squared_distance(*u, *v);
			std::cout << "b contains edge not in a: " << CGAL::to_double(exactWeight) << "(" << edge->u << "," << edge->v << ")" << " (" << *u << ") (" << *v << ")" << std::endl;
			result = false;
		}
	}

	return result;
}

bool isCdtSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdtS = computeCdt(vertices, edgesS, &handlesToIndex); // CDT of mimimum edge constraint
	EdgeVector* ev_cdtS = convertCdtToGraph(vertices, cdtS);

	//sortByXY(ev_cdtS);
	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_DC\\CdtS.gdf", vertices, ev_cdtS);

	bool res = isSubgraph(vertices, edgesF, ev_cdtS);

	delete ev_cdtS;
	delete cdtS;
	delete handlesToIndex;

	return res;
}

int main(int argc, char* argv[]) {
	const char* vertFile = (argc > 2) ? argv[1] : NULL;
	const char* edgeFile = (argc > 2) ? argv[2] : NULL;
	VertexVector* vertices = NULL;
	EdgeVector* edges = NULL;

	if (vertFile == NULL || edgeFile == NULL) {
		// Random graph
		//createRandomCirclePlaneForest(1000, 1000, 100, &vertices, &edges);
		createRandomMediumLengthPlaneForest(1000, 1000, 100, &vertices, &edges);
		//createRandomNearTriangulation(1000, 1000, 100, &vertices, &edges);
	}
	else {
		// Load graph from file
		parseGraph(vertFile, edgeFile, &vertices, &edges);
	}

	if (SHOW_DEBUG) {
		printGraph("Input", vertices, edges, true);
	}

	/*vertices = new VertexVector();
	vertices->push_back(new CGALPoint(0, 0));
	vertices->push_back(new CGALPoint(1, 0));
	vertices->push_back(new CGALPoint(2, 0));

	edges = new EdgeVector();
	edges->push_back(new SimpleEdge(0, 1, 0));
	edges->push_back(new SimpleEdge(1, 2, 0));
	edges->push_back(new SimpleEdge(2, 0, 0));*/

	// Compute CT(V, E)
	EdgeVector* NewEdges;
	EdgeVector* cdtS;
	computeNonLocallyDelaunay(vertices, edges, &NewEdges, &cdtS);
	EdgeVector* S = intersectInputSetWithConstraintSet(NewEdges, cdtS);
	//std::cout << "Edges in E: " << NewEdges->size() << " Edges in S: " << S->size() << " Ratio: " << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << S->size() << std::endl;
	std::cout << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << std::endl;

	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_Med\\Edges.gdf", vertices, edges);
	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_Med\\NewEdges.gdf", vertices, NewEdges);
	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_Med\\S.gdf", vertices, S);

	if (SHOW_DEBUG) {
		printGraph("computeNonLocallyDelaunay", vertices, cdtS, false);
		printGraph("S", vertices, S, false);
	}

	// Validatation:
	// S ⊆ E
	if (!isSubgraph(vertices, S, NewEdges)) {
		std::cout << "Error: isSubgraph is false" << std::endl;
	}

	// F ⊆ CDT(V, S)
	if (!isCdtSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCdtSubgraph is false" << std::endl;
	}

	deleteEdgeVector(S);
	deleteEdgeVector(cdtS);
	deleteEdgeVector(NewEdges);
	deleteEdgeVector(edges);
	deleteVerticesVector(vertices);

	return 0;
}


/*
Copyright 2017 Jiawei Chiu

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include "base.h"

DEFINE_string(train_filename,
              "/usr/local/google/home/jiawei/cfgpu/riftbolt/ml-1m/train.txt",
              "Filename for training dataset.");
DEFINE_string(test_filename,
              "/usr/local/google/home/jiawei/cfgpu/riftbolt/ml-1m/test.txt",
              "Filename for training dataset.");

DEFINE_int32(log_every_sec, 2, "Report test error every this many seconds.");

DEFINE_string(output_filename, "", "Output file for stats.");

DEFINE_double(learning_rate, 0.005, "Learning rate for SGD.");

DEFINE_double(lambda, 0.1, "Lambda for SGD, used for regularization.");

DEFINE_int32(k, 32, "Number of columns for U and V that we want to recover.");

DEFINE_int32(rng_seed, 3255245, "Seed for rng.");

DEFINE_double(max_time, 100000, "Maximum time in seconds.");

namespace gi {

typedef float FloatType;

using Eigen::MatrixBase;

typedef Eigen::MatrixXd Mat;
typedef Eigen::Map<Mat> MatMap;
typedef Eigen::VectorXd Vec;
typedef Eigen::Map<Vec> VecMap;

typedef Eigen::SparseMatrix<FloatType, Eigen::RowMajor> SpMat;
typedef Eigen::Triplet<FloatType> Triplet;

class RngHelper {
public:
  RngHelper();
  RngHelper(uint_fast32_t seed);
  FloatType Normal();
  void Normal(Mat &m);
  FloatType Uniform();
  void Uniform(Mat &m);

  static RngHelper &Get();

private:
  std::mt19937 rng_;
  std::normal_distribution<FloatType> normal_dist_;
  std::uniform_real_distribution<FloatType> uniform_dist_;
};

RngHelper::RngHelper() : RngHelper(FLAGS_rng_seed) {}

RngHelper::RngHelper(uint_fast32_t seed) : rng_(seed) {}

FloatType RngHelper::Normal() { return normal_dist_(rng_); }

void RngHelper::Normal(Mat &m) {
  int size = m.rows() * m.cols();
  for (int i = 0; i < size; ++i) {
    m.data()[i] = Normal();
  }
}

FloatType RngHelper::Uniform() { return uniform_dist_(rng_); }

void RngHelper::Uniform(Mat &m) {
  int size = m.rows() * m.cols();
  for (int i = 0; i < size; ++i) {
    m.data()[i] = Uniform();
  }
}

RngHelper &RngHelper::Get() {
  static RngHelper rng_helper;
  return rng_helper;
}

SpMat NewSpMat(const string &filename) {
  ifstream is(filename);

  // Read in matrix in CSR format.
  int m;
  int n;
  int nnz;
  CHECK(is >> m >> n >> nnz);
  vector<FloatType> value(nnz);
  vector<int> row_ind(m + 1);
  vector<int> col(nnz);
  for (int i = 0; i < nnz; ++i) {
    CHECK(is >> value[i]);
  }
  for (int i = 0; i < nnz; ++i) {
    CHECK(is >> col[i]);
  }
  for (int i = 0; i <= m; ++i) {
    CHECK(is >> row_ind[i]);
  }

  // Convert to triplets.
  vector<Triplet> triplets;
  for (int i = 0; i < m; ++i) {
    for (int j = row_ind[i]; j < row_ind[i + 1]; ++j) {
      triplets.emplace_back(i, col[j], value[j]);
    }
  }

  // Create our sparse matrix.
  SpMat out(m, n);
  out.reserve(nnz);
  out.setFromTriplets(triplets.begin(), triplets.end());
  out.makeCompressed();

  // Verify that the matrix built matches the input we get.
  CHECK_EQ(out.nonZeros(), nnz);
  CHECK_EQ(out.outerSize(), m);
  CHECK_EQ(out.innerSize(), n);
  for (int i = 0; i <= m; ++i) {
    CHECK_EQ(out.outerIndexPtr()[i], row_ind[i]);
  }
  for (int i = 0; i < nnz; ++i) {
    CHECK_EQ(out.innerIndexPtr()[i], col[i]);
    CHECK_EQ(out.valuePtr()[i], value[i]);
  }

  LOG(INFO) << "Read " << nnz << " entries from " << filename << "\n";
  return out;
}

FloatType Square(FloatType x) { return x * x; }

FloatType RmsError(const SpMat &a, Mat &ut, Mat &vt) {
  FloatType sum = 0;
  for (int q = 0; q < a.outerSize(); ++q) {
    for (SpMat::InnerIterator it(a, q); it; ++it) {
      const int i = it.row();
      const int j = it.col();
      sum += Square(it.value() - ut.col(i).dot(vt.col(j)));
    }
  }
  return sqrt(sum / a.nonZeros());
}

void Main() {
  CHECK(!FLAGS_output_filename.empty());
  CHECK(!FLAGS_train_filename.empty());
  CHECK(!FLAGS_test_filename.empty());

  SpMat a_train = NewSpMat(FLAGS_train_filename);
  SpMat a_test = NewSpMat(FLAGS_test_filename);

  const int k = FLAGS_k;

  // Dimensions check.
  const int m = a_train.rows();
  const int n = a_train.cols();
  CHECK_EQ(m, a_test.rows());
  CHECK_EQ(n, a_test.cols());

  RngHelper &rng_helper = RngHelper::Get();
  Mat ut(k, m);
  Mat vt(k, n);

  // Extra check for dimensions, to be sure.
  CHECK_EQ(a_train.rows(), a_train.outerSize());
  CHECK_EQ(a_train.rows(), ut.cols());
  CHECK_EQ(a_train.cols(), vt.cols());
  CHECK_EQ(ut.rows(), vt.rows());

  // Initialize ut, vt with random numbers.
  rng_helper.Uniform(ut);
  rng_helper.Uniform(vt);
  ut /= static_cast<FloatType>(k);
  vt /= static_cast<FloatType>(k);

  // Collect rmse and timing info over iterations.
  Timer timer;
  vector<int> l_iter;
  vector<FloatType> rmse;
  vector<double> timing;
  double time_elapsed = 0;

  for (int iter = 0;; ++iter) {
    const double elapsed = timer.elapsed();
    if (elapsed > FLAGS_log_every_sec || iter == 0) {
      // Get time elapsed.
      time_elapsed += elapsed;
      timing.push_back(time_elapsed);
      l_iter.push_back(iter);

      // Compute error.
      rmse.push_back(RmsError(a_test, ut, vt));

      // Output.
      LOG(INFO) << "Iter: " << l_iter.back()
                << " Time elapsed: " << timing.back()
                << " sec RMSE: " << rmse.back() << "\n";

      // Reset timer.
      timer.reset();

      if (timing.back() > FLAGS_max_time) {
        break;
      }
    }

    for (int q = 0; q < a_train.outerSize(); ++q) {
      for (SpMat::InnerIterator it(a_train, q); it; ++it) {
        const int i = it.row();
        const int j = it.col();
        const FloatType err = it.value() - ut.col(i).dot(vt.col(j));
        ut.col(i) +=
            FLAGS_learning_rate * (err * vt.col(j) - FLAGS_lambda * ut.col(i));
        vt.col(j) +=
            FLAGS_learning_rate * (err * ut.col(i) - FLAGS_lambda * vt.col(j));
      }
    }
  }

  LOG(INFO) << "Total time taken: " << time_elapsed << "\n";

  CHECK_EQ(timing.size(), rmse.size());
  CHECK_EQ(timing.size(), l_iter.size());
  ofstream os(FLAGS_output_filename);
  os << "iter\ttime\trmse\n";
  for (size_t i = 0; i < timing.size(); ++i) {
    os << l_iter[i] << "\t" << timing[i] << "\t" << rmse[i] << "\n";
  }
}

} // namespace gi

int main(int argc, char *argv[]) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();
  gi::Main();
}


//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
// Copyright 2021 Pranam Lashkari <plashkari628@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_DIFFUSION_HPP
#define BOOST_GIL_IMAGE_PROCESSING_DIFFUSION_HPP

#include "boost/gil/detail/math.hpp"
#include <boost/gil/algorithm.hpp>
#include <boost/gil/color_base_algorithm.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/image_view_factory.hpp>
#include <boost/gil/pixel.hpp>
#include <boost/gil/point.hpp>
#include <boost/gil/typedefs.hpp>
#include <functional>
#include <numeric>
#include <vector>

namespace boost { namespace gil {
namespace conductivity {
struct perona_malik_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return std::exp(-std::abs(value));
        });

        return input;
    }
};

struct gaussian_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return std::exp(-value * value);
        });

        return input;
    }
};

struct wide_regions_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return 1.0 / (1.0 + value * value);
        });

        return input;
    }
};

struct more_wide_regions_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return 1.0 / std::sqrt((1.0 + value * value));
        });

        return input;
    }
};
} // namespace diffusion

/**
    \brief contains discrete approximations of 2D Laplacian operator
*/
namespace laplace_function {
// The functions assume clockwise enumeration of stencil points, as such
// NW   North NE          0 1 2      (-1, -1) (0, -1) (+1, -1)
// West       East   ===> 7   3 ===> (-1, 0)          (+1, 0)
// SW   South SE          6 5 4      (-1, +1) (0, +1) (+1, +1)

/**
    \brief This function makes sure all Laplace functions enumerate
    values in the same order and direction.

    The first element is difference North West direction, second in North,
    and so on in clockwise manner. Leave element as zero if it is not
    to be computed.
*/
inline std::array<gil::point_t, 8> get_directed_offsets()
{
    return {point_t{-1, -1}, point_t{0, -1}, point_t{+1, -1}, point_t{+1, 0},
            point_t{+1, +1}, point_t{0, +1}, point_t{-1, +1}, point_t{-1, 0}};
}

template <typename PixelType>
using stencil_type = std::array<PixelType, 8>;

/**
    \brief 5 point stencil approximation of Laplacian

    Only main 4 directions are non-zero, the rest are zero
*/
struct stencil_5points
{
    double delta_t = 0.25;

    template <typename SubImageView>
    stencil_type<typename SubImageView::value_type> compute_laplace(SubImageView view,
                                                                    point_t origin)
    {
        auto current = view(origin);
        stencil_type<typename SubImageView::value_type> stencil;
        using channel_type = typename channel_type<typename SubImageView::value_type>::type;
        std::array<gil::point_t, 8> offsets(get_directed_offsets());
        typename SubImageView::value_type zero_pixel;
        static_fill(zero_pixel, 0);
        for (std::size_t index = 0; index < offsets.size(); ++index)
        {
            if (index % 2 != 0)
            {
                static_transform(view(origin.x + offsets[index].x, origin.y + offsets[index].y),
                                 current, stencil[index], std::minus<channel_type>{});
            }
            else
            {
                stencil[index] = zero_pixel;
            }
        }
        return stencil;
    }

    template <typename Pixel>
    Pixel reduce(const stencil_type<Pixel>& stencil)
    {
        auto first = stencil.begin();
        auto last = stencil.end();
        using channel_type = typename channel_type<Pixel>::type;
        auto result = []() {
            Pixel zero_pixel;
            static_fill(zero_pixel, channel_type(0));
            return zero_pixel;
        }();

        for (std::size_t index : {1u, 3u, 5u, 7u})
        {
            static_transform(result, stencil[index], result, std::plus<channel_type>{});
        }
        Pixel delta_t_pixel;
        static_fill(delta_t_pixel, delta_t);
        static_transform(result, delta_t_pixel, result, std::multiplies<channel_type>{});

        return result;
    }
};

/**
    \brief 9 point stencil approximation of Laplacian

    This is full 8 way approximation, though diagonal
    elements are halved during reduction.
*/
struct stencil_9points_standard
{
    double delta_t = 0.125;

    template <typename SubImageView>
    stencil_type<typename SubImageView::value_type> compute_laplace(SubImageView view,
                                                                    point_t origin)
    {
        stencil_type<typename SubImageView::value_type> stencil;
        auto out = stencil.begin();
        auto current = view(origin);
        using channel_type = typename channel_type<typename SubImageView::value_type>::type;
        std::array<gil::point_t, 8> offsets(get_directed_offsets());
        for (auto offset : offsets)
        {
            static_transform(view(origin.x + offset.x, origin.y + offset.y), current, *out++,
                             std::minus<channel_type>{});
        }

        return stencil;
    }

    template <typename Pixel>
    Pixel reduce(const stencil_type<Pixel>& stencil)
    {
        using channel_type = typename channel_type<Pixel>::type;
        auto result = []() {
            Pixel zero_pixel;
            static_fill(zero_pixel, channel_type(0));
            return zero_pixel;
        }();
        for (std::size_t index : {1u, 3u, 5u, 7u})
        {
            static_transform(result, stencil[index], result, std::plus<channel_type>{});
        }

        for (std::size_t index : {0u, 2u, 4u, 6u})
        {
            Pixel half_pixel;
            static_fill(half_pixel, channel_type(1 / 2.0));
            static_transform(stencil[index], half_pixel, half_pixel,
                             std::multiplies<channel_type>{});
            static_transform(result, half_pixel, result, std::plus<channel_type>{});
        }

        Pixel delta_t_pixel;
        static_fill(delta_t_pixel, delta_t);
        static_transform(result, delta_t_pixel, result, std::multiplies<channel_type>{});

        return result;
    }
};
} // namespace laplace_function

namespace brightness_function {
using laplace_function::stencil_type;
struct identity
{
    template <typename Pixel>
    stencil_type<Pixel> operator()(const stencil_type<Pixel>& stencil)
    {
        return stencil;
    }
};

// TODO: Figure out how to implement color gradient brightness, as it
// seems to need dx and dy using sobel or scharr kernels

struct rgb_luminance
{
    using pixel_type = rgb32f_pixel_t;
    stencil_type<pixel_type> operator()(const stencil_type<pixel_type>& stencil)
    {
        stencil_type<pixel_type> output;
        std::transform(stencil.begin(), stencil.end(), output.begin(), [](const pixel_type& pixel) {
            float32_t luminance = 0.2126f * pixel[0] + 0.7152f * pixel[1] + 0.0722f * pixel[2];
            pixel_type result_pixel;
            static_fill(result_pixel, luminance);
            return result_pixel;
        });
        return output;
    }
};

} // namespace brightness_function

enum class matlab_connectivity
{
    minimal,
    maximal
};

enum class matlab_conduction_method
{
    exponential,
    quadratic
};

template <typename InputView, typename OutputView>
void classic_anisotropic_diffusion(const InputView& input, const OutputView& output,
                                   unsigned int num_iter, double kappa)
{
    anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                          brightness_function::identity{},
                          conductivity::perona_malik_conductivity{kappa});
}

template <typename InputView, typename OutputView>
void matlab_anisotropic_diffusion(const InputView& input, const OutputView& output,
                                  unsigned int num_iter, double kappa,
                                  matlab_connectivity connectivity,
                                  matlab_conduction_method conduction_method)
{
    if (connectivity == matlab_connectivity::minimal)
    {
        if (conduction_method == matlab_conduction_method::exponential)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else if (conduction_method == matlab_conduction_method::quadratic)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else
        {
            throw std::logic_error("unhandled conduction method found");
        }
    }
    else if (connectivity == matlab_connectivity::maximal)
    {
        if (conduction_method == matlab_conduction_method::exponential)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else if (conduction_method == matlab_conduction_method::quadratic)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else
        {
            throw std::logic_error("unhandled conduction method found");
        }
    }
    else
    {
        throw std::logic_error("unhandled connectivity found");
    }
}

template <typename InputView, typename OutputView>
void default_anisotropic_diffusion(const InputView& input, const OutputView& output,
                                   unsigned int num_iter, double kappa)
{
    anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_9points_standard{},
                          brightness_function::identity{}, conductivity::gaussian_conductivity{kappa});
}

/// \brief Performs diffusion according to Perona-Malik equation
///
/// WARNING: Output channel type must be floating point,
/// otherwise there will be loss in accuracy which most
/// probably will lead to incorrect results (input will be unchanged).
/// Anisotropic diffusion is a smoothing algorithm that respects
/// edge boundaries and can work as an edge detector if suitable
/// iteration count is set and grayscale image view is used
/// as an input
template <typename InputView, typename OutputView,
          typename LaplaceStrategy = laplace_function::stencil_9points_standard,
          typename BrightnessFunction = brightness_function::identity,
          typename DiffusivityFunction = conductivity::gaussian_conductivity>
void anisotropic_diffusion(const InputView& input, const OutputView& output, unsigned int num_iter,
                           LaplaceStrategy laplace, BrightnessFunction brightness,
                           DiffusivityFunction diffusivity)
{
    using input_pixel_type = typename InputView::value_type;
    using pixel_type = typename OutputView::value_type;
    using channel_type = typename channel_type<pixel_type>::type;
    using computation_image = image<pixel_type>;
    const auto width = input.width();
    const auto height = input.height();
    const point_t dims(width, height);
    const auto zero_pixel = []() {
        pixel_type pixel;
        static_fill(pixel, static_cast<channel_type>(0));

        return pixel;
    }();
    computation_image result_image(width + 2, height + 2, zero_pixel);
    auto result = view(result_image);
    computation_image scratch_result_image(width + 2, height + 2, zero_pixel);
    auto scratch_result = view(scratch_result_image);
    transform_pixels(input, subimage_view(result, 1, 1, width, height),
                     [](const input_pixel_type& pixel) {
                         pixel_type converted;
                         for (std::size_t i = 0; i < num_channels<pixel_type>{}; ++i)
                         {
                             converted[i] = pixel[i];
                         }
                         return converted;
                     });

    for (unsigned int iteration = 0; iteration < num_iter; ++iteration)
    {
        for (std::ptrdiff_t relative_y = 0; relative_y < height; ++relative_y)
        {
            for (std::ptrdiff_t relative_x = 0; relative_x < width; ++relative_x)
            {
                auto x = relative_x + 1;
                auto y = relative_y + 1;
                auto stencil = laplace.compute_laplace(result, point_t(x, y));
                auto brightness_stencil = brightness(stencil);
                laplace_function::stencil_type<pixel_type> diffusivity_stencil;
                std::transform(brightness_stencil.begin(), brightness_stencil.end(),
                               diffusivity_stencil.begin(), diffusivity);
                laplace_function::stencil_type<pixel_type> product_stencil;
                std::transform(stencil.begin(), stencil.end(), diffusivity_stencil.begin(),
                               product_stencil.begin(), [](pixel_type lhs, pixel_type rhs) {
                                   static_transform(lhs, rhs, lhs, std::multiplies<channel_type>{});
                                   return lhs;
                               });
                static_transform(result(x, y), laplace.reduce(product_stencil),
                                 scratch_result(x, y), std::plus<channel_type>{});
            }
        }
        using std::swap;
        swap(result, scratch_result);
    }

    copy_pixels(subimage_view(result, 1, 1, width, height), output);
}

}} // namespace boost::gil

#endif


/**
 * @file libfrcima.hpp
 * @author Jorge Agüero Zamora
 * @brief Contains the RCIMA and fast-RCIMA methods
 * @version 0.1
 * @date 2021-06-14
 *
 * @copyright Copyright (c) 2021
 *
 */
#ifndef LIBFRCIMA_H
#define LIBFRCIMA_H

#define LOW_RANK_APPROX_ITERATIONS 3 //!< amount of iteration to approximate low rank matrix

#include <armadillo>
#include <vector>
#include <algorithm>

namespace frcima
{
  using arma::mat;
  using std::vector;

  extern double _calculated_error; //!< global variable to hold calculated error

  /**
 * @brief calculates rank constrained inverse matrix approximation using RCIMA method
 *
 * @param t_data_x the original source images. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param t_data_c the images with a noise. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param rank desired rank constrain, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat rcima(const vector<vector<double>> &t_data_x, const vector<vector<double>> &t_data_c, const size_t rank);

  /**
 * @brief calculates rank constrained inverse matrix approximation using RCIMA method
 *
 * @param t_data_x the original source images. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param t_data_c the images with a noise applied. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param rank desired rank constrain, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat rcima(const mat &X, const mat &C, const size_t rank);

  /**
 * @brief calculates rank constrained inverse matrix approximation using fast-RCIMA algorithm
 *
 * @param t_data_x the original source images. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param t_data_c the images with noise. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param rank desired rank approximation, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat fast_rcima(const vector<vector<double>> &t_data_x, const vector<vector<double>> &t_data_c, const size_t rank);

  /**
 * @brief calculates rank constrained inverse matrix approximation using fast-RCIMA algorithm
 *
 * @param t_data_x the original source images. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param t_data_c the images with noise. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param rank desired rank approximation, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat fast_rcima(const mat &X, const mat &C, const size_t rank);

}

#endif