/**
 * @file dataloader_tests.cpp
 * @author Kartik Dutt
 *
 * Tests for various functionalities of dataloader.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN // Do not define this anywhere else.
#include <dataloader/dataloader.hpp>
#include <boost/test/unit_test.hpp>

using namespace boost::unit_test;

BOOST_AUTO_TEST_SUITE(DataLoadersTest);

/**
 * Simple test for Dataloader.
 */
BOOST_AUTO_TEST_CASE(CSVDataLoaderTest)
{
  // Check that dataloader loads only known datasets.
  BOOST_REQUIRE_THROW(DataLoader<>("no-dataset", true), std::runtime_error);

  // Check Load CSV Function for Dataloader.
  Utils::DownloadFile("/datasets/iris.csv", "./../data/iris.csv");
  // Check the file has been downloaded.
  BOOST_REQUIRE(Utils::PathExists("./../data/iris.csv"));

  DataLoader<> irisDataloader;
  irisDataloader.LoadCSV("./../data/iris.csv", true, true, 0.5, false, false,
      0, -1, 1, -1);

  // Check for length and columns of training dataset.
  BOOST_REQUIRE_EQUAL(irisDataloader.TrainLabels().n_cols, 75);
  BOOST_REQUIRE_EQUAL(irisDataloader.TrainLabels().n_rows, 3);

  // Check for validation data as well.
  BOOST_REQUIRE_EQUAL(irisDataloader.ValidFeatures().n_cols, 75);
  BOOST_REQUIRE_EQUAL(irisDataloader.ValidFeatures().n_rows, 4);

  // Check for validation dataset using tuples.
  BOOST_REQUIRE_EQUAL(std::get<1>(irisDataloader.ValidSet()).n_cols, 75);
  BOOST_REQUIRE_EQUAL(std::get<1>(irisDataloader.ValidSet()).n_rows, 3);

  // Check for training dataset using tuples.
  BOOST_REQUIRE_EQUAL(std::get<0>(irisDataloader.TrainSet()).n_cols, 75);
  BOOST_REQUIRE_EQUAL(std::get<0>(irisDataloader.TrainSet()).n_rows, 4);

  Utils::RemoveFile("./../data/iris.csv");
}

/**
 * Simple test for MNIST Dataloader.
 */
BOOST_AUTO_TEST_CASE(MNISTDataLoaderTest)
{
  DataLoader<> dataloader("mnist", true, 0.80);

  // Check for correct dimensions.
  BOOST_REQUIRE_EQUAL(dataloader.TrainFeatures().n_rows, 784);
  BOOST_REQUIRE_EQUAL(dataloader.TestFeatures().n_rows, 784);
  BOOST_REQUIRE_EQUAL(dataloader.ValidFeatures().n_rows, 784);

  // Check for correct dimensions.
  BOOST_REQUIRE_EQUAL(dataloader.TrainFeatures().n_cols, 8400);
  BOOST_REQUIRE_EQUAL(dataloader.ValidFeatures().n_cols, 33600);
  BOOST_REQUIRE_EQUAL(dataloader.TestFeatures().n_cols, 28000);

  // Check if we can access both features and labels using
  // TrainSet tuple and ValidSet tuple.
  BOOST_REQUIRE_EQUAL(std::get<0>(dataloader.TrainSet()).n_cols, 8400);
  BOOST_REQUIRE_EQUAL(std::get<1>(dataloader.TrainSet()).n_rows, 1);
  BOOST_REQUIRE_EQUAL(std::get<0>(dataloader.ValidSet()).n_cols, 33600);
  BOOST_REQUIRE_EQUAL(std::get<1>(dataloader.ValidSet()).n_rows, 1);

  // Clean up.
  Utils::RemoveFile("./../data/mnist-dataset/mnist_all.csv");
  Utils::RemoveFile("./../data/mnist-dataset/mnist_all_centroids.csv");
  Utils::RemoveFile("./../data/mnist-dataset/mnist_train.csv");
  Utils::RemoveFile("./../data/mnist-dataset/mnist_test.csv");
  Utils::RemoveFile("./../data/mnist.tar.gz");
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * gist.cpp
 *
 *  Created on: Sep 11, 2012
 *      Author: lbossard
 */

#include "gist.hpp"

#include <numeric>

#include <opencv2/imgproc/imgproc.hpp>
#include <boost/shared_ptr.hpp>

#include "cpp/vision/image_utils.hpp"

namespace vision
{
namespace features
{
namespace global
{

namespace
{

	cv::Mat prepareCvImgGrey(const cv::Mat& bgr_img)
	{

		cv::Mat gray_mat = bgr_img;

		if (gray_mat.type() != CV_8U)
		{
			cv::cvtColor(bgr_img, gray_mat, CV_BGR2GRAY);
		}

		if (!gray_mat.isContinuous())
		{
			gray_mat = gray_mat.clone();
		}

		gray_mat.convertTo(gray_mat, CV_32F);

		return gray_mat;
	}

	struct BgrChannels
	{
		cv::Mat r_channel;
		cv::Mat g_channel;
		cv::Mat b_channel;

		void create(int rows, int cols)
		{
			r_channel.create(rows, cols, CV_32F);
			g_channel.create(rows, cols, CV_32F);
			b_channel.create(rows, cols, CV_32F);
		}

		void from_bgr_image(const cv::Mat_<cv::Vec3f>& bgr_img)
		{
			static const int from_to[] = { 0,2,  1,1,  2,0};

			this->create(bgr_img.rows, bgr_img.cols);
			cv::Mat out[] = { r_channel, g_channel, b_channel };

			cv::mixChannels(&bgr_img, 1,
							out, 3,
							from_to, 3 );
		}
	};

	void mat2GistColorImage(const BgrChannels& bgr_mat, color_image_t& img_t)
	{

		img_t.width = bgr_mat.r_channel.cols;
		img_t.height = bgr_mat.r_channel.rows;
		img_t.c1 = reinterpret_cast<float*>(bgr_mat.r_channel.data);     // r
		img_t.c2 = reinterpret_cast<float*>(bgr_mat.g_channel.data);     // g
		img_t.c3 = reinterpret_cast<float*>(bgr_mat.b_channel.data);     // b
	}

	void mat2ImagePtr(const cv::Mat_<float>& grey_mat, image_t& img_t)
	{
		img_t.width = grey_mat.cols;
		img_t.height = grey_mat.rows;
		img_t.stride = grey_mat.step.p[0];
		img_t.data = reinterpret_cast<float*>(grey_mat.data);

	}

	struct MallocDeleter
	{
	    void operator()(void* x)
	    {
	        free(x);
	    }
	};

}

void Gist::extractColor(const cv::Mat& img, cv::Mat_<float>& gist, const unsigned int n_blocks, const unsigned int a, const unsigned int b, const unsigned int c)
{
	std::vector<int> orientations_per_scale(3);
	orientations_per_scale[0] = a;
	orientations_per_scale[1] = b;
	orientations_per_scale[2] = c;
//	extractScaleTabColor(img, gist, n_blocks, orientations_per_scale);

}

void Gist::extract(const cv::Mat& img, cv::Mat_<float>& gist, const unsigned int n_blocks, const unsigned int a, const unsigned int b, const unsigned int c)
{
	std::vector<int> orientations_per_scale(3);
	orientations_per_scale[0] = a;
	orientations_per_scale[1] = b;
	orientations_per_scale[2] = c;
//	extractScaleTab(img, gist, n_blocks, orientations_per_scale);

}

//void Gist::extractScaleTabColor(const cv::Mat& img, cv::Mat_<float>& gist, const unsigned int n_blocks, const std::vector<int>& orientations_per_scale)
//{
//	const int num_scales = orientations_per_scale.size();
//	const int total_bin_count = std::accumulate(orientations_per_scale.begin(), orientations_per_scale.end(), 0);
//
//	BgrChannels img_channels;
//	color_image_t gist_img;
//
//
//	// prepare img
//	cv::Mat_<cv::Vec3f> float_img;
//	img.convertTo(float_img, CV_32F); // makes it continuous?
//	assert(float_img.isContinuous());
//	img_channels.from_bgr_image(float_img);
//	mat2GistColorImage(img_channels, gist_img);
//
//	// compute gist
//	boost::shared_ptr<float> descriptor(
//			::color_gist_scaletab(&gist_img, n_blocks, num_scales, &orientations_per_scale[0]),
//			MallocDeleter()
//			 );
//	if (descriptor == NULL)
//	{
//		gist.create(0,0);
//		return;
//	}
//
//	// copy to output
//	const int descriptor_dim = n_blocks * n_blocks * total_bin_count * 3;
//	gist.create(1, descriptor_dim);
//	::memcpy(gist.data, descriptor.get(), descriptor_dim * sizeof(float));
//
//}

//void Gist::extractScaleTab(const cv::Mat& img, cv::Mat_<float>& gist, const unsigned int n_blocks, const std::vector<int>& orientations_per_scale)
//{
//	const int num_scales = orientations_per_scale.size();
//	const int total_bin_count = std::accumulate(orientations_per_scale.begin(), orientations_per_scale.end(), 0);
//
//	// prepare img
//	cv::Mat gray_img = img;
//	image_t gist_img;
//	gray_img = prepareCvImgGrey(gray_img);
//	mat2ImagePtr(gray_img, gist_img);
//
//	// compute gist
//	boost::shared_ptr<float> descriptor(
//			::bw_gist_scaletab(&gist_img, n_blocks, num_scales , &orientations_per_scale[0]),
//			MallocDeleter());
//	if (descriptor == NULL)
//	{
//		gist.create(0,0);
//		return;
//	}
//
//	// copy to output
//	const std::size_t descriptor_dim = n_blocks * n_blocks * total_bin_count;
//	gist.create(1, descriptor_dim);
//	::memcpy(gist.data, descriptor.get(), descriptor_dim * sizeof(float));
//
//}

} /* namespace global */
} /* namespace features */
} /* namespace vision */


#ifndef BART_DOMAIN_FINITE_ELEMENT_HPP_
#define BART_DOMAIN_FINITE_ELEMENT_HPP_

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_values.h>

#include "domain/finite_element/finite_element_i.hpp"

namespace bart::domain::finite_element {

/*! \brief Base implementation of the FiniteElement class using Dealii.
 *
 * This class is almost entirley a wrapper for various Dealii objects and many calls are just passed through to the
 * underlying classes.
 *
 * @tparam dim spatial dimension
 */
template<int dim>
class FiniteElement : public FiniteElementI<dim> {
 public:
  using typename FiniteElementI<dim>::DealiiVector, typename FiniteElementI<dim>::Tensor;
  virtual ~FiniteElement() = default;

  // Basic Finite Element data
  [[nodiscard]] auto dofs_per_cell() const -> int override { return finite_element_->dofs_per_cell; }
  [[nodiscard]] auto n_cell_quad_pts() const -> int override { return cell_quadrature_->size(); }
  [[nodiscard]] auto n_face_quad_pts() const -> int override { return face_quadrature_->size(); }

  auto SetCell(const domain::CellPtr<dim> &to_set) -> bool override;
  auto SetFace(const domain::CellPtr<dim> &to_set, const domain::FaceIndex face) -> bool override;

  [[nodiscard]] auto ShapeValue(const int cell_degree_of_freedom,
                                const int cell_quadrature_point) const -> double override {
    return values_->shape_value(cell_degree_of_freedom, cell_quadrature_point);
  }

  [[nodiscard]] auto FaceShapeValue(const int cell_degree_of_freedom,
                                    const int face_quadrature_point) const -> double override {
    return face_values_->shape_value(cell_degree_of_freedom, face_quadrature_point);
  }

  [[nodiscard]] auto ShapeGradient(const int cell_degree_of_freedom,
                                   const int cell_quadrature_point) const -> Tensor override {
    return values_->shape_grad(cell_degree_of_freedom, cell_quadrature_point);
  }

  [[nodiscard]] auto Jacobian(const int cell_quadrature_point) const -> double override {
    return values_->JxW(cell_quadrature_point);
  }

  [[nodiscard]] auto FaceJacobian(const int face_quadrature_point) const -> double override {
    return face_values_->JxW(face_quadrature_point);
  }

  [[nodiscard]] auto FaceNormal() const -> Tensor override {
    return face_values_->normal_vector(0);
  }

  [[nodiscard]] auto ValueAtQuadrature(const DealiiVector& values_at_dofs) const -> std::vector<double> override;
  [[nodiscard]] auto ValueAtFaceQuadrature(const DealiiVector& values_at_dofs) const -> std::vector<double> override;

  // Dependencies
  auto finite_element() -> dealii::FiniteElement<dim, dim>* override { return finite_element_.get(); };
  auto values() -> dealii::FEValues<dim>* override { return values_.get(); };
  auto face_values() -> dealii::FEFaceValues<dim>* override {return face_values_.get(); };
  auto neighbor_face_values() -> dealii::FEFaceValues<dim>* override {return neighbor_face_values_.get(); };
  auto cell_quadrature() -> dealii::QGauss<dim>* { return cell_quadrature_.get(); };
  auto face_quadrature() -> dealii::QGauss<dim - 1>* { return face_quadrature_.get(); };

 protected:
  std::shared_ptr<dealii::FiniteElement<dim, dim>> finite_element_;
  std::shared_ptr<dealii::FEValues<dim>> values_;
  std::shared_ptr<dealii::FEFaceValues<dim>> face_values_;
  std::shared_ptr<dealii::FEFaceValues<dim>> neighbor_face_values_;
  std::shared_ptr<dealii::QGauss<dim>> cell_quadrature_;
  std::shared_ptr<dealii::QGauss<dim - 1>> face_quadrature_;

  bool values_reinit_called_{ false };
  bool face_values_reinit_called_{ false };
  bool neighbor_face_values_reinit_called_{ false };
  using FiniteElementI<dim>::values;
  using FiniteElementI<dim>::face_values;
};

} // namespace bart::domain::finite_element

#endif // BART_DOMAIN_FINITE_ELEMENT_HPP_


#include <iostream>
#include <cstdint>
#include <chrono>
#include "colored_kDataFrame.hpp"
#include "parallel_hashmap/phmap.h"
#include "kDataFrame.hpp"
#include <boost/algorithm/string/join.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/functional/hash.hpp>

using boost::adaptors::transformed;
using boost::algorithm::join;
using namespace std;
using namespace phmap;

typedef std::chrono::high_resolution_clock Time;

class Combo {

public:
    Combo() = default;

    std::vector<std::pair<uint32_t, uint32_t>> combs;

    void combinations(int n) {
        this->combs.clear();
        this->comb(n, this->r, this->arr);
    }

private:
    int *arr = new int[2];
    int r = 2;

    void comb(int n, int r, int *arr) {
        for (int i = n; i >= r; i--) {
            // choose the first element
            arr[r - 1] = i;
            if (r > 1) { // if still needs to choose
                // recursive into smaller problem
                comb(i - 1, r - 1, arr);

            } else {
                this->combs.emplace_back(std::make_pair(arr[0] - 1, arr[1] - 1));
            }
        }
    }

};

namespace kSpider{
void pairwise(string index_prefix) {

    // Read colors
    string colors_map = index_prefix + "colors.intvectors";
    ifstream input(colors_map.c_str());
    int size;
    input >> size;
    flat_hash_map<uint64_t, vector<uint32_t>> color_to_ids = flat_hash_map<uint64_t, std::vector<uint32_t>>(size);
    for (int i = 0; i < size; i++) {
        uint64_t color, colorSize;
        input >> color >> colorSize;
        uint32_t sampleID;
        color_to_ids[color] = std::vector<uint32_t>(colorSize);
        for (int j = 0; j < colorSize; j++) {
            input >> sampleID;
            color_to_ids[color][j] = sampleID;
        }
    }

    flat_hash_map<uint32_t, uint32_t> colorsCount;

//    auto *ckf = colored_kDataFrame::load(index_prefix);
    auto *kf = kDataFrame::load(index_prefix);
//    auto *kf = ckf->getkDataFrame();

    auto it = kf->begin();
    while (it != kf->end()) {
        colorsCount[it.getCount()]++;
        it++;
    }

    // Free some memory
    delete kf;

    flat_hash_map<uint32_t, uint32_t> groupID_to_kmerCount;
    for (const auto &record : color_to_ids) {
        uint32_t colorCount = colorsCount[record.first];
        for (auto group_id: record.second) {
            groupID_to_kmerCount[group_id] += colorCount;
        }
    }

    std::ofstream fstream_kmerCount;
    fstream_kmerCount.open(index_prefix + "_kSpider_seqToKmersNo.tsv");
    fstream_kmerCount << "ID\tseq\tkmers\n";
    uint64_t counter = 0;
    for(const auto & item : groupID_to_kmerCount){
        fstream_kmerCount << ++counter << '\t' << item.first << '\t' << item.second << '\n';
    }
    fstream_kmerCount.close();

    Combo combo = Combo();
    flat_hash_map<std::pair<uint32_t, uint32_t>, uint32_t, boost::hash<pair<uint32_t, uint32_t>>> edges;
    for (const auto &item : color_to_ids) {
        combo.combinations(item.second.size());
        for (auto const &seq_pair : combo.combs) {
            uint32_t _seq1 = item.second[seq_pair.first];
            uint32_t _seq2 = item.second[seq_pair.second];
            _seq1 > _seq2 ? edges[{_seq1, _seq2}] += colorsCount[item.first] : edges[{_seq2,
                                                                                _seq1}] += colorsCount[item.first];
        }
    }

    std::ofstream myfile;

    myfile.open(index_prefix + "_kSpider_pairwise.tsv");

    myfile << "ID" << '\t' << "seq1" << '\t' << "seq2" << '\t' << "shared_kmers" << '\n';

    uint64_t line_count = 0;

    for (const auto &edge : edges) {
        myfile << ++line_count << '\t' << edge.first.first << '\t' << edge.first.second << '\t' << edge.second << '\n';
    }

    myfile.close();

}
}


#include <boost/test/unit_test.hpp>
#include <boost/format.hpp>
#include <fstream>

#include <madym/tests/mdm_test_utils.h>
#include <madym/t1_methods/mdm_T1FitterBase.h>
#include <mdm_version.h>
#include <madym/mdm_AIF.h>
#include <madym/dce_models/mdm_DCEModelGenerator.h>

namespace fs = boost::filesystem;

BOOST_AUTO_TEST_SUITE(test_mdm_tools)

BOOST_AUTO_TEST_CASE(test_madym_lite) {
	BOOST_TEST_MESSAGE("======= Testing tool: madym_DCE_lite =======");
	//Need to generate input data files. To do this, load in calibration
	//data

	//Read in dyn times
	int nTimes;
	std::string timesFileName(mdm_test_utils::calibration_dir() + "dyn_times.dat");
	std::ifstream timesFileStream(timesFileName, std::ios::in | std::ios::binary);
	timesFileStream.read(reinterpret_cast<char*>(&nTimes), sizeof(int));
	std::vector<double> dynTimes(nTimes);
	for (double &t : dynTimes)
		timesFileStream.read(reinterpret_cast<char*>(&t), sizeof(double));
	timesFileStream.close();

	//Read in AIF parameters
	int injectionImage;
	double hct;
	double dose;

	std::string aifFileName(mdm_test_utils::calibration_dir() + "aif.dat");
	std::ifstream aifFileStream(aifFileName, std::ios::in | std::ios::binary);
	aifFileStream.read(reinterpret_cast<char*>(&injectionImage), sizeof(int));
	aifFileStream.read(reinterpret_cast<char*>(&hct), sizeof(double));
	aifFileStream.read(reinterpret_cast<char*>(&dose), sizeof(double));
	aifFileStream.close();

	//Read (noisy) ETM times series from calibration data
	int nParams;
	std::vector<double> Ct(nTimes);
	std::string modelFileName = mdm_test_utils::calibration_dir() + "ETM_noise.dat";

	std::ifstream modelFileStream(modelFileName, std::ios::in | std::ios::binary);
	modelFileStream.read(reinterpret_cast<char*>(&nParams), sizeof(int));

	std::vector<double> trueParams(nParams);
	for (double &p : trueParams)
		modelFileStream.read(reinterpret_cast<char*>(&p), sizeof(double));
	for (double &c : Ct)
		modelFileStream.read(reinterpret_cast<char*>(&c), sizeof(double));
	modelFileStream.close();

	//Read IAUC values
	int nIAUC;
	std::string iaucFileName = mdm_test_utils::calibration_dir() + "ETM_IAUC.dat";
	std::ifstream iaucFileStream(iaucFileName, std::ios::in | std::ios::binary);
	iaucFileStream.read(reinterpret_cast<char*>(&nIAUC), sizeof(int));
	std::vector<double> IAUCTimes(nIAUC);
	std::vector<double> IAUCVals(nIAUC);
	for (double &it : IAUCTimes)
		iaucFileStream.read(reinterpret_cast<char*>(&it), sizeof(double));

	for (double &iv : IAUCVals)
		iaucFileStream.read(reinterpret_cast<char*>(&iv), sizeof(double));
	iaucFileStream.close();

	std::string IAUC_str = std::to_string((int)IAUCTimes[0]);
	for (int i = 1; i < nIAUC; i++)
		IAUC_str += ("," + std::to_string((int)IAUCTimes[i]));

	//Create a temporary directory where we'll run these tests, which we can then cleanup
	//easily at the end
	std::string test_dir = mdm_test_utils::temp_dir();

	//Write out the concentration times
	std::string inputDataFile = test_dir + "/Ct_input.dat";
	std::ofstream ifs(inputDataFile, std::ios::out);
	BOOST_REQUIRE_MESSAGE(ifs.is_open(), "Failed to write out Ct values for madym_DCE_lite");

	for (const auto c : Ct)
		ifs << c << " ";
	ifs.close();

	//Writeout the dynamic time
	std::string dynTimesFile = test_dir + "/dyn_times.dat";
	std::ofstream dfs(dynTimesFile, std::ios::out);
	BOOST_REQUIRE_MESSAGE(dfs.is_open(), "Failed to write out dyn times values for madym_DCE_lite");

	for (const auto t : dynTimes)
		dfs << t << " ";
	dfs.close();

	//Call madym_lite to fit ETM
	std::string Ct_output_dir = test_dir + "/madym_DCE_lite/";
	std::string outputName = "madym_analysis.dat";
	std::stringstream cmd;
	cmd << mdm_test_utils::tools_exe_dir() << "madym_DCE_lite"
		<< " -m ETM"
		<< " --data " << inputDataFile
		<< " -n " << nTimes
		<< " -I " << IAUC_str
		<< " -i " << injectionImage
		<< " -o " << Ct_output_dir
		<< " -O " << outputName
		<< " --Ct"
		<< " -t " << dynTimesFile;

	BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

	int error;
	try
	{
		error = std::system(cmd.str().c_str());
	}
	catch (...)
	{
		BOOST_CHECK_MESSAGE(false, "Running madym_DCE_lite failed");
		return;
	}

	BOOST_CHECK_MESSAGE(!error, "Error returned from madym_DCE_lite tool");

	//Load in the fitted parameters from the output file
	std::string outputDataFile = Ct_output_dir + "ETM_" + outputName;
	std::ifstream ofs(outputDataFile, std::ios::in);
	BOOST_REQUIRE_MESSAGE(ofs.is_open(), "Failed to read in fitted values for ETM");

	int fit_errors, enhancing;
	double model_fit, ktrans_fit, ve_fit, vp_fit, tau_fit;
	std::vector<double> IAUC_fit(nIAUC);
	ofs >> fit_errors;
	ofs >> enhancing;
	ofs >> model_fit;
	for (auto &i : IAUC_fit)
		ofs >> i;
	ofs >> ktrans_fit;
	ofs >> ve_fit;
	ofs >> vp_fit;
	ofs >> tau_fit;
	ofs.close();

	//Check the model parameters have fitted correctly
	double tol = 1.0;
	BOOST_TEST_MESSAGE(boost::format("Fitted ktrans (%1.2f, %2.2f)")
		% ktrans_fit % trueParams[0]);
	BOOST_CHECK_CLOSE(ktrans_fit, trueParams[0], tol);
	BOOST_TEST_MESSAGE(boost::format("Fitted Ve (%1.2f, %2.2f)")
		% ve_fit % trueParams[1]);
	BOOST_CHECK_CLOSE(ve_fit, trueParams[1], tol);
	BOOST_TEST_MESSAGE(boost::format("Fitted Vp (%1.2f, %2.2f)")
		% vp_fit % trueParams[2]);
	BOOST_CHECK_CLOSE(vp_fit, trueParams[2], tol);
	BOOST_TEST_MESSAGE(boost::format("Fitted tau (%1.2f, %2.2f)")
		% tau_fit % trueParams[3]);
	BOOST_CHECK_CLOSE(tau_fit, trueParams[3], tol);

	//Check model fit, error codes and enhancing
	BOOST_TEST_MESSAGE(boost::format("Model residuals = %1%")
		% model_fit);
	BOOST_CHECK_SMALL(model_fit, 0.01);
	BOOST_TEST_MESSAGE("No error code");
	BOOST_CHECK(!fit_errors);
	BOOST_TEST_MESSAGE("Enhancing");
	BOOST_CHECK(enhancing);

	for (int i = 0; i < nIAUC; i++)
	{
		BOOST_TEST_MESSAGE("IAUC " + std::to_string(IAUCTimes[i]));
		BOOST_CHECK_CLOSE(IAUC_fit[i], IAUCVals[i], tol);
	}

	//Tidy up
	fs::remove(inputDataFile);
	fs::remove(dynTimesFile);
	fs::remove_all(Ct_output_dir);
}

BOOST_AUTO_TEST_SUITE_END() //


#pragma once

#include <cstdio>
#include <cstdlib>

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

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

using namespace std;

class Network{
    typedef struct _Edge {
        int u, v;
        double p;
    } _Edge;

    void clearPointers(){
        neighbor = nullptr;
        neighbor_reverse = nullptr;
        neighbor_ptr = nullptr;
        neighbor_reverse_ptr = nullptr;
        probability = nullptr;
        inDegree = nullptr;
        outDegree = nullptr;
        s_contri_order = nullptr;
        threshold = nullptr;
        c_threshold = nullptr;
        s_contri = nullptr;
    }

    template <typename T>
    void freeSpace(T *ptr){
        if(ptr != nullptr)
            delete [] ptr;
    }

    void importRelation(string path){
        _Edge *edge = new _Edge[100000000];
        char line[50];
        int u, v, tmp;
        // set<int> nodeSet;

        FILE *fd = fopen(path.c_str(), "r");
        while(NULL != fgets(line, 50, fd)){
            vector<string> inStr;
            boost::split(inStr, line, boost::is_any_of(" "));
            u = stoi(inStr[0]);
            v = stoi(inStr[1]);
            edge[edgeNum].u = u;
            edge[edgeNum].v = v;
            inDegree[v]++;
            outDegree[u]++;
            if(type == "VIC")
                edge[edgeNum].p = stod(inStr[2]);
            edgeNum++;
        }
        fclose(fd);

        neighbor = new int[edgeNum];
        neighbor_reverse = new int[edgeNum];
        probability = new double[edgeNum];
        sort(edge, edge+edgeNum, [](const _Edge &a, const _Edge &b)-> bool {
            if(a.u == b.u)
                return a.v < b.v;
            return a.u < b.u;
        });
        for(int i = 0;i < edgeNum;i++){
            neighbor[i] = edge[i].v;
            probability[i] = edge[i].p;
        }

        sort(edge, edge+edgeNum, [](const _Edge &a, const _Edge &b)-> bool {
            if(a.v == b.v)
                return a.u < b.u;
            return a.v < b.v;
        });
        for(int i = 0;i < edgeNum;i++)
            neighbor_reverse[i] = edge[i].u;

        memcpy(neighbor_ptr, outDegree, vertexNum*sizeof(int));
        memcpy(neighbor_reverse_ptr, inDegree, vertexNum*sizeof(int));
        for(int i = 1;i < vertexNum;i++){
            neighbor_ptr[i] += neighbor_ptr[i-1];
            neighbor_reverse_ptr[i] += neighbor_reverse_ptr[i-1];
        }

        delete [] edge;
    }

    inline double getProbability(int i, int j) const {
        if(i)   return probability[neighbor_ptr[i-1]+j];
        return probability[j];
    }

public:
    int vertexNum, edgeNum;
    int *neighbor, *neighbor_reverse;
    double *probability;

    vector<int> sorted_degree;
    int *neighbor_ptr, *neighbor_reverse_ptr;
    int *inDegree, *outDegree;
    int *s_contri_order;
    double *threshold, *c_threshold, *s_contri;
    string type, path, s_contri_path;
    double IC_prob;
    bool is_s_contri;

    Network(){
        clearPointers();
        is_s_contri = false;
    }

    Network(string path, string type, int vertexNum){
        clearPointers();
        cout << "import " << path << " " << type << endl;
        this->path = path;
        this->type = type;
        this->vertexNum = vertexNum;
        this->edgeNum = 0;
        is_s_contri = false;
        inDegree = new int[vertexNum];
        outDegree = new int[vertexNum];
        neighbor_ptr = new int[vertexNum];
        neighbor_reverse_ptr = new int[vertexNum];
        sorted_degree.clear();

        importRelation(path);

        if(type == "IC")
            IC_prob = .1;
        else if(type == "LT"){
            threshold = new double[vertexNum];
            c_threshold = new double[vertexNum];
        }
        else if(type != "WC" && type != "VIC")
            printf("Invalid model\n");
    }

    Network(const Network &network){
        clearPointers();
        path = network.path;
        type = network.type;
        vertexNum = network.vertexNum;
        edgeNum = network.edgeNum;
        is_s_contri = network.is_s_contri;
        neighbor = new int[edgeNum];
        neighbor_reverse = new int[edgeNum];
        memcpy(neighbor, network.neighbor, edgeNum*sizeof(int));
        memcpy(neighbor_reverse, network.neighbor_reverse, edgeNum*sizeof(int));
        neighbor_ptr = new int[vertexNum];
        neighbor_reverse_ptr = new int[vertexNum];
        memcpy(neighbor_ptr, network.neighbor_ptr, vertexNum*sizeof(int));
        memcpy(neighbor_reverse_ptr, network.neighbor_reverse_ptr, vertexNum*sizeof(int));
        probability = new double[edgeNum];
        memcpy(probability, network.probability, edgeNum*sizeof(double));
        sorted_degree = network.sorted_degree;
        IC_prob = network.IC_prob;
        s_contri_path = network.s_contri_path;
        inDegree = new int[vertexNum];
        outDegree = new int[vertexNum];
        memcpy(inDegree, network.inDegree, vertexNum*sizeof(int));
        memcpy(outDegree, network.outDegree, vertexNum*sizeof(int));
        if(network.s_contri != nullptr){
            s_contri_order = new int[vertexNum];
            s_contri = new double[vertexNum];
            memcpy(s_contri_order, network.s_contri_order, vertexNum*sizeof(int));
            memcpy(s_contri, network.s_contri, vertexNum*sizeof(double));
        }
        if(type == "LT"){
            threshold = new double[vertexNum];
            c_threshold = new double[vertexNum];
            memcpy(threshold, network.threshold, vertexNum*sizeof(double));
            memcpy(c_threshold, network.c_threshold, vertexNum*sizeof(double));
        }
    }

    ~Network(){
        freeSpace(neighbor);
        freeSpace(neighbor_reverse);
        freeSpace(neighbor_ptr);
        freeSpace(neighbor_reverse_ptr);
        freeSpace(probability);
        freeSpace(inDegree);
        freeSpace(outDegree);
        freeSpace(s_contri);
        freeSpace(s_contri_order);
        freeSpace(threshold);
        freeSpace(c_threshold);
    }

    inline int getNeighbor(int i, int j) const {
        if(i)   return neighbor[neighbor_ptr[i-1]+j];
        return neighbor[j];
    }

    inline int getReverseNeighbor(int i, int j) const {
        if(i)   return neighbor_reverse[neighbor_reverse_ptr[i-1]+j];
        return neighbor_reverse[j];
    }

    void setICProb(double prob){
        IC_prob = prob;
    }

    void sortByDegree(){
        vector<pair<int,int>> tempMap;
        for(int i = 0;i < vertexNum;i++)
            tempMap.push_back(make_pair(i,outDegree[i]));
        sort(tempMap.begin(), tempMap.end(), [](const pair<int,int> &a, const pair<int,int> &b)-> bool {
            return a.second > b.second;
        });
        sorted_degree.clear();
        for(pair<int,int> p: tempMap)
            sorted_degree.push_back(p.first);
    }

    void setSContri(string path){
        s_contri = new double[vertexNum];
        s_contri_order = new int[vertexNum];
        memset(s_contri, 0, vertexNum);
        char line[256];
        int index = 0, node;
        double value;
        FILE *fd = fopen(path.c_str(), "r");
        vector<string> inStr;
        while(NULL != fgets(line, 256, fd)){
            boost::split(inStr, line, boost::is_any_of(" "));
            s_contri[node] = stod(inStr[1]);
            s_contri_order[index] = stoi(inStr[0]);
            index++;
        }
        fclose(fd);
        is_s_contri = true;
        s_contri_path = path;
    }

    void showSContri(){
        for(int i = 0;i < vertexNum;i++)
            printf("%d %lf\n", i, s_contri[i]);
    }

    void showData(){
        for(int i = 0;i < vertexNum;i++){
            for(int j = 0;j < outDegree[i];j++){
                printf("%d - %d\n", i, getNeighbor(i,j));
            }
        }
        printf("Edge number: %d\n", edgeNum);
    }

    double getProb(int cseed, int cseede) const {
        if(type == "IC")
            return IC_prob;
        else if(type == "VIC"){
            int index = [this, cseed, cseede]()-> int {
                for(int i = 0;i < outDegree[cseed];i++)
                    if(getNeighbor(cseed, i) == cseede)
                        return i;
                return -1;
            }();
            if(index < 0){
                cout << "error occurs in getProb" << endl;
                exit(1);
            }
            return getProbability(cseed, index);
        }
        else if(type == "WC")
            return 1./inDegree[cseede];
        else if(type == "LT")
            return 0.;
        else
            printf("Invalid model\n");
            return 0.;
    }
};


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IF_MINUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IF_MINUS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/unary_minus.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( if_minus_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::fundamental_<A0>, X>
                          , bs::pack_<bd::unspecified_<A1>, X>
                          , bs::pack_<bd::unspecified_<A1>, X>
                          )
   {
     BOOST_FORCEINLINE A1 operator()(A0 const& a0, A1 const& a1, A1 const& a2) const
      {
        return a1 - if_else_zero(a0, a2);
      }
   };

} } }

#endif


//
// Created by James Noeckel on 10/6/20.
//
#include <opencv2/opencv.hpp>
#include "imgproc/graph_cut.h"
#include "math/GaussianMixture.h"
/*#include <boost/filesystem.hpp>
#include <boost/regex.hpp>*/
#include "utils/timingMacros.h"

//#define WIDTH 64
//#define HEIGHT 64
#define MIN_PROB 1e-11f

const std::vector<std::vector<std::pair<int, bool>>> viewIndices = {{{1, false}, {2, false}, {7, false}, {13, false}, {14, false}},
                                                   {{34, false}, {35, false}, {44, true}, {45, true}, {54, false}, {55, false}, {56, false}, {64, true}, {65, true}, {66, true}},
                                                   {{19, false}, {35, false}, {44, true}, {45, true}, {54, false}, {55, false}, {56, false}, {64, true}, {65, true}, {66, true}},
                                                   {{31, true}, {39, false}, {40, false}, {49, true}, {50, true}, {51, true}, {59, false}, {60, false}, {61, false}, {69, true}}};
const std::vector<int> parts = {0, 1, 3, 4};

int main(int argc, char **argv) {
    using namespace Eigen;
    if (argc != 5) {
        std::cout << "usage: " << argv[0] << " smoothness sigma components variance" << std::endl;
        return 1;
    }
    float smoothness = std::stof(argv[1]);
    float sigma = std::stof(argv[2]);
    int components = std::stoi(argv[3]);
    float variance = std::stof(argv[4]);
    std::cout << "smoothness: " << smoothness << std::endl;
    std::cout << "sigma: " << sigma << std::endl;
    std::cout << "components: " << components << std::endl;
    std::cout << "variance: " << variance << std::endl;
    for (int partInd=0; partInd<parts.size(); ++partInd) {
        std::vector<std::string> imageFnames;
        std::vector<std::string> occlusionMaskFnames;
        int partId = parts[partInd];
        for (auto pair : viewIndices[partInd]) {
            imageFnames.push_back("../test_data/segmentation2/part_" + std::to_string(partId) + "_view_" +
                                  std::to_string(pair.first) + (pair.second ? "_backside" : "") + "_warped.png");
            occlusionMaskFnames.push_back("../test_data/segmentation2/part_" + std::to_string(partId) + "_view_" +
                                          std::to_string(pair.first) + (pair.second ? "_backside" : "") +
                                          "_mask_warped_eroded.png");
        }

        /*or (int partId : parts) {
            boost::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
            const boost::regex img_filter( "part_" + std::to_string(partId) + "_view_\\d+_warped\\.png" );
            const boost::regex mask_filter( "part_" + std::to_string(partId) + "_view_\\d+_mask_warped_eroded\\.png" );
            for (boost::filesystem::directory_iterator i("../test_data/segmentation/"); i != end_itr; ++i) {
                // Skip if not a file
                if (!boost::filesystem::is_regular_file(i->status())) continue;
                if (i->path().extension() != ".png") continue;

                boost::smatch what;
                if (boost::regex_match(i->path().filename().string(), what, img_filter)) {
                    imageFnames.push_back(i->path().string());
                }
                if (boost::regex_match(i->path().filename().string(), what, mask_filter)) {
                    occlusionMaskFnames.push_back(i->path().string());
                }
            }
            if (imageFnames.size() != occlusionMaskFnames.size()) {
                std::cout << "unequal file counts for part " << partId << " (" << imageFnames.size() << "-" << occlusionMaskFnames.size() << std::endl;
                return 1;
            }
            if (imageFnames.empty()) {
                std::cout << "no files matched" << std::endl;
                return 1;
            }
            std::sort(imageFnames.begin(), imageFnames.end());
            std::sort(occlusionMaskFnames.begin(), occlusionMaskFnames.end());*/

        cv::Mat initMask = cv::imread("../test_data/segmentation2/part_" + std::to_string(partId) + "_initMask.png",
                                      cv::IMREAD_GRAYSCALE) / 255;
        if (initMask.empty()) {
            std::cout << "failed to load initMask " << partId << std::endl;
            return 1;
        }

        std::vector<cv::Mat> images;
        std::vector<cv::Mat> occlusionMasks;

        for (int index = 0; index < imageFnames.size(); ++index) {
            images.push_back(cv::imread(imageFnames[index]));
            cv::cvtColor(images.back(), images.back(), cv::COLOR_BGR2Luv, 0);
            occlusionMasks.push_back(cv::imread(occlusionMaskFnames[index], cv::IMREAD_GRAYSCALE) / 255);
            if (images.back().empty() || occlusionMasks.back().empty()) {
                std::cout << "failed to load image or mask " << imageFnames[index] << std::endl;
                return 1;
            }
            if (images.back().rows != initMask.rows || images.back().cols != initMask.cols) {
                std::cout << "image and mask sizes do not match: " << images.back().size << " vs " << initMask.size
                          << std::endl;
                return 1;
            }
            if (occlusionMasks.back().rows != initMask.rows || occlusionMasks.back().cols != initMask.cols) {
                std::cout << "mask sizes do not match: " << occlusionMasks.back().size << " vs " << initMask.size
                          << std::endl;
                return 1;
            }
        }
        for (int iteration = 0; iteration < 2; ++iteration) {
            std::vector<std::vector<GaussianMixture>> gmms(images.size(), std::vector<GaussianMixture>(2));
            for (size_t i = 0; i < images.size(); ++i) {
                for (int cluster = 0; cluster <= 1; ++cluster) {
                    const auto &img = images[i];
                    std::vector<int> indices;
                    indices.reserve(img.total());
                    //add all pixels that are both inside the known subset and unoccluded in this view to the training set
                    for (int p = 0; p < img.total(); ++p) {
                        if (initMask.at<uchar>(p) == cluster && occlusionMasks[i].at<uchar>(p)) {
                            indices.push_back(p);
                        }
                    }
                    size_t n = indices.size();
                    std::cout << "learning view " << i << " cluster " << cluster << " with " << n << " points"
                              << std::endl;
                    MatrixX3d colors(n, 3);
                    for (size_t j = 0; j < n; ++j) {
                        int p = indices[j];
                        const auto &col = img.at<cv::Vec3b>(p);
                        colors.row(j) = RowVector3d(col[0], col[1], col[2]);
                    }
                    gmms[i][cluster] = GaussianMixture(components, 3, variance);
                    int iters = gmms[i][cluster].learn(colors);
                    /*if (!gmms[i][cluster].success()) {
                        std::cout << "warning: gmm " << i << " cluster " << cluster << " failed to learn from " << n
                                  << " points after " << iters << " iterations" << std::endl;
                        cv::imshow("mask", occlusionMasks[i]);
                        cv::waitKey();
                        cv::destroyWindow("mask");
    //                    std::cout << initMask << std::endl;
                        cv::imshow("initMask", initMask == cluster);
                        cv::waitKey();
                        cv::destroyWindow("initMask");
                    }*/
                }
            }
            std::vector<cv::Mat> avgProbImgs(2);
            avgProbImgs[0] = cv::Mat::zeros(initMask.rows, initMask.cols, CV_32FC1);
            avgProbImgs[1] = cv::Mat::zeros(initMask.rows, initMask.cols, CV_32FC1);
            cv::Mat viewCount = cv::Mat::ones(initMask.rows, initMask.cols, CV_32FC1) * 0.0001;
            for (size_t imgInd = 0; imgInd < images.size(); ++imgInd) {
                std::vector<cv::Mat> debugImgs(2);
                debugImgs[0] = cv::Mat::zeros(initMask.rows, initMask.cols, CV_32FC1);
                debugImgs[1] = cv::Mat::zeros(initMask.rows, initMask.cols, CV_32FC1);
                for (int i = 0; i < initMask.rows; ++i) {
                    for (int j = 0; j < initMask.cols; ++j) {
                        if (occlusionMasks[imgInd].at<uchar>(i, j)) {
                            for (int cluster = 0; cluster <= 1; ++cluster) {
                                if (!gmms[imgInd][cluster].success()) continue;
                                const auto &col = images[imgInd].at<cv::Vec3b>(i, j);
                                RowVector3d color(col[0], col[1], col[2]);
                                float logp = static_cast<float>(gmms[imgInd][cluster].logp_data(color)(0));
                                float prob = std::exp(logp);
                                if (prob > 1) {
                                    std::cout << "warning: probability " << prob << " at " << i << ", " << j << ": "
                                              << cluster << std::endl;
                                }
                                debugImgs[cluster].at<float>(i, j) = prob;
                                avgProbImgs[cluster].at<float>(i, j) += prob;
                                viewCount.at<float>(i, j) += 1;
                            }
                        }
                    }
                }
                for (int cluster = 0; cluster <= 1; ++cluster) {
                    if (!gmms[imgInd][cluster].success()) continue;
                    cv::Mat debugImgConverted;
                    double minVal;
                    double maxVal;
                    cv::Point minLoc;
                    cv::Point maxLoc;
                    cv::minMaxLoc(debugImgs[cluster], &minVal, &maxVal, &minLoc, &maxLoc);
                    debugImgs[cluster].convertTo(debugImgConverted, CV_8UC1, 255.0 / maxVal);
                    for (int pix = 0; pix < initMask.total(); ++pix) {
                        if (!occlusionMasks[imgInd].at<uchar>(pix)) {
                            debugImgConverted.at<uchar>(pix) = 127;
                        }
                    }
                    cv::imwrite("part_" + std::to_string(partId) + "_prob_" + std::to_string(cluster) + "_view_" +
                                std::to_string(imgInd) + "_iteration_" + std::to_string(iteration) + ".png", debugImgConverted);
                }
            }
            avgProbImgs[0] /= viewCount;
            avgProbImgs[1] /= viewCount;

            for (int cluster = 0; cluster <= 1; ++cluster) {
                cv::Mat probImgConverted;
                double minVal;
                double maxVal;
                cv::Point minLoc;
                cv::Point maxLoc;
                cv::minMaxLoc(avgProbImgs[cluster], &minVal, &maxVal, &minLoc, &maxLoc);
                std::cout << "minval " << cluster << ": " << minVal << "; maxval: " << maxVal << std::endl;
                std::cout << "view count at maxval: " << viewCount.at<float>(maxLoc) << std::endl;
                avgProbImgs[cluster].convertTo(probImgConverted, CV_8UC1, 255.0 / maxVal);
                cv::imwrite("part_" + std::to_string(partId) + "_prob_" + std::to_string(cluster) + "_iteration_" + std::to_string(iteration) + ".png",
                            probImgConverted);
            }

            cv::Mat energy(initMask.rows, initMask.cols, CV_32FC2);
            for (int row = 0; row < initMask.rows; ++row) {
                for (int col = 0; col < initMask.cols; ++col) {
                    float energy0;
                    float energy1 = -std::log(std::max(MIN_PROB, avgProbImgs[1].at<float>(row, col)));
                    if (row == 0 || col == 0 || row == initMask.rows - 1 || col == initMask.cols - 1) {
                        energy0 = -1000000 * std::log(MIN_PROB);
                    } else {
                        energy0 = -std::log(std::max(MIN_PROB, avgProbImgs[0].at<float>(row, col)));
                    }
                    energy.at<cv::Vec<float, 2>>(row, col) = cv::Vec<float, 2>(energy0, energy1);
                }
            }
            cv::Mat labels(initMask.rows, initMask.cols, CV_8UC1);
            std::cout << "running maxflow" << std::endl;
            DECLARE_TIMING(flow);
            START_TIMING(flow);
//        float maxflow = graph_cut(energy, labels, smoothness, avgProbImgs, sigma);
            float maxflow = graph_cut(energy, labels, smoothness, images, sigma, occlusionMasks);
//        float maxflow = graph_cut(energy, labels, smoothness);
            STOP_TIMING(flow);
            PRINT_TIMING(flow);
            std::cout << "solution with " << maxflow << " flow" << std::endl;
            cv::imwrite("part_" + std::to_string(partId) + "_graphCutOutput_iteration_" + std::to_string(iteration) + ".png", labels * 255);
            initMask = labels;
        }
    }
    return 0;

    /*cv::Mat energy(HEIGHT, WIDTH, CV_32FC2);
    cv::Mat labels(HEIGHT, WIDTH, CV_8UC1);
    for (int i=0; i<HEIGHT; ++i) {
        for (int j=0; j<WIDTH; ++j) {
            float probi = std::max(0.0f, (std::abs(i - HEIGHT/2.0f) - WIDTH / 8.0f)/WIDTH*4);
            float probj = std::max(0.0f, (std::abs(j - HEIGHT/2.0f) - WIDTH / 8.0f)/WIDTH*4);
            float prob = std::max(probi, probj);
            energy.at<cv::Vec<float, 2>>(i, j) = cv::Vec<float, 2>(prob, 1.0f-prob);
        }
    }
    float maxflow = graph_cut(energy, labels, 5);
    std::cout << "solution with " << maxflow << " flow" << std::endl;
    cv::imwrite("graphCutTest.png", labels*255);
    return 0;*/
}


#include <limits>
#include <Eigen/Geometry>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>

#include <igl/exact_geodesic.h>
#include <igl/is_border_vertex.h>

namespace py = pybind11;
using namespace Eigen;

PYBIND11_MODULE(_igl_ext, m) {
    using namespace pybind11::literals;

    m.def("exact_geodesic", []
        (const MatrixXd& V,
         const MatrixXi& F,
         const VectorXi& src_ix)
        -> VectorXd
        {
            VectorXi src_face_ix;
            VectorXi target_ix = VectorXi::LinSpaced(V.rows(), 0, V.rows()-1);
            VectorXi target_face_ix;
            MatrixXd dists;

            igl::exact_geodesic(V, F,
                    src_ix, src_face_ix,
                    target_ix, target_face_ix,
                    dists);

            Map<RowVectorXd> dists_flat(dists.data(), dists.size());
            return dists_flat;
        },
        py::call_guard<py::gil_scoped_release>(),
        py::arg("verts"), py::arg("tris"), py::arg("src_vert_indices")
    );

    m.def("is_border_vertex", []
        (const MatrixXi& F)
        -> Matrix<bool, Dynamic, 1>
        {
            std::vector<bool> b = igl::is_border_vertex(F);
            Matrix<bool, Dynamic, 1> bm(b.size());
            for (size_t i=0; i < b.size(); i++) {
                bm(i) = b[i];
            }
            return bm;
        },
        py::call_guard<py::gil_scoped_release>(),
        py::arg("tris")
    );
}


#pragma once

#include <dr_eigen/ros.hpp>
#include <dr_ensenso_msgs/Calibrate.h>
#include <dr_ensenso_msgs/FinalizeCalibration.h>
#include <dr_ensenso_msgs/InitializeCalibration.h>
#include <dr_msgs/GetPose.h>
#include <dr_msgs/SendPose.h>
#include <dr_ros/node.hpp>
#include <dr_ros/service_client.hpp>

#include <estd/result.hpp>

#include <ros/node_handle.h>

#include <Eigen/Eigen>

namespace dr {

struct InitializeCalibrationConfig {
	/// If true, the camera is attached to the moving frame; if false, it is attached to the fixed frame.
	/// Additionally, the calibration plate is assumed to be attached to the other frame.
	bool camera_moving;

	/// Name of the moving frame (usually the name of the robot end effector).
	std::string moving_frame;

	/// Name of the fixed frame (usually the name of the robot base frame).
	std::string fixed_frame;

	/// Initial guess of the pose of the camera, can be used to speed up the calibration optimization.
	/// Depending on the value of camera_moving, this pose is defined in either moving_frame or fixed_frame.
	std::optional<Eigen::Isometry3d> camera_guess;

	/// Initial guess of the pattern, can be used to speed up the calibration optimization.
	/// Depending on the value of camera_moving, this pose is defined in either moving_frame or fixed_frame.
	std::optional<Eigen::Isometry3d> pattern_guess;

	/// Directory in which calibration information will be stored (leave empty to disable).
	std::string dump_dir;
};

class EnsensoCalibratorNode: public Node {
public:
	struct CalibrationResult {
		/// Calibrated pose of the camera in either moving_frame or fixed_frame (depending on camera_moving).
		Eigen::Isometry3d camera_pose;

		/// Calibrated pose of the pattern in either moving_frame or fixed_frame (depending on camera_moving).
		Eigen::Isometry3d pattern_pose;

		/// The residual error of the calibration.
		double residual_error;
	};

private:
	/// List of ros services.
	struct Services {
		/// Service for initializing a calibration sequence.
		dr::ServiceClient<dr_ensenso_msgs::InitializeCalibration> initialize_calibration;

		/// Service for recording a calibration pattern.
		dr::ServiceClient<dr_msgs::SendPose> record_calibration;

		/// Service for finalizing a calibration sequence.
		dr::ServiceClient<dr_ensenso_msgs::FinalizeCalibration> finalize_calibration;

		/// Service for getting the calibration on the camera.
		dr::ServiceClient<dr_msgs::GetPose> get_calibration;

		/// Service client for performing a workspace calibration.
		dr::ServiceClient<dr_ensenso_msgs::Calibrate> workspace_calibration;
	} services_;

	bool store_calibration_ = true;

public:
	EnsensoCalibratorNode(
		std::string const & initialize_calibration_service, ///< Service name for the initialization of the calibration.
		std::string const & record_calibration_service,     ///< Service name for the detect calibration pattern service.
		std::string const & finalize_calibration_service,   ///< Service name for finalizing the calibration.
		std::string const & get_calibration_service,        ///< Service name for getting the calibration on the camera.
		std::string const & workspace_calibration_service,  ///< Service name for performing a workspace calibration.
		bool wait_for_services = false,                     ///< If true, waits for the services to come alive.
		bool store_calibration = true                       ///< If true, stores the calibration in the Ensenso.
	);

	/// Initializes a calibration sequence, clearing any state from previous calibration sequences.
	estd::result<void, estd::error> initializeCalibration(InitializeCalibrationConfig const & config);

	/// Records a single calibration pattern.
	estd::result<void, estd::error> recordCalibration(Eigen::Isometry3d const & robot_pose);

	/// Finalizes the calibration, returning the result.
	estd::result<CalibrationResult, estd::error> finalizeCalibration();

	/// Retrieves the calibration from the camera, returning the result.
	estd::result<Eigen::Isometry3d, estd::error> getCalibration();

	/// Performs a workspace calibration.
	estd::result<Eigen::Isometry3d, estd::error> workspaceCalibration(
		Eigen::Isometry3d const & pattern_pose, ///< Pose of the pattern, defined in the frame to calibrate to.
		std::string const & frame_id,           ///< The name of the frame to calibrate to.
		int samples                             ///< Number of sampels to average the results over, useful for reducing possible noise.
	);
};

}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef SAMPLER_HH
#define SAMPLER_HH

#include "random.hh"
#include <functional>
#include <boost/mpl/if.hpp>

namespace graph_tool
{
using namespace std;
using namespace boost;

// Discrete sampling via vose's alias method.

// See http://www.keithschwarz.com/darts-dice-coins/ for a very clear
// explanation.

template <class Value, class KeepReference = mpl::true_>
class Sampler
{
public:
    Sampler(const vector<Value>& items,
            const vector<double>& probs)
        : _items(items), _probs(probs), _alias(items.size())
    {
        double S = 0;
        for (size_t i = 0; i < _probs.size(); ++i)
            S += _probs[i];

        for (size_t i = 0; i < _probs.size(); ++i)
        {
            _probs[i] *= _probs.size() / S;
            if (_probs[i] < 1)
                _small.push_back(i);
            else
                _large.push_back(i);
        }

        while (!(_small.empty() || _large.empty()))
        {
            size_t l = _small.back();
            size_t g = _large.back();
            _small.pop_back();
            _large.pop_back();

            _alias[l] = g;
            _probs[g] = (_probs[l] + _probs[g]) - 1;
            if (_probs[g] < 1)
                _small.push_back(g);
            else
                _large.push_back(g);
        }

        // fix numerical instability
        for (size_t i = 0; i < _large.size(); ++i)
            _probs[_large[i]] = 1;
        for (size_t i = 0; i < _small.size(); ++i)
            _probs[_small[i]] = 1;
        _large.clear();
        _small.clear();
    }

    Sampler() {}

    template <class RNG>
    const Value& sample(RNG& rng)
    {
        uniform_int_distribution<size_t> sample(0, _probs.size() - 1);
        size_t i = sample(rng);

        bernoulli_distribution coin(_probs[i]);
        if (coin(rng))
            return _items[i];
        else
            return _items[_alias[i]];
    }

    size_t size() const { return _items.size(); }

private:

    typedef typename mpl::if_<KeepReference,
                              const vector<Value>&,
                              vector<Value> >::type items_t;
    items_t _items;
    vector<double> _probs;
    vector<size_t> _alias;
    vector<size_t> _small;
    vector<size_t> _large;
};

// uniform sampling from containers

template <class Container, class RNG>
const typename Container::value_type& uniform_sample(const Container& v, RNG& rng)
{
    std::uniform_int_distribution<size_t> i_rand(0, v.size() - 1);
    return v[i_rand(rng)];
}

} // namespace graph_tool

#endif // SAMPLER_HH


#ifndef BART_SRC_SOLVER_EIGENVALUE_SPECTRAL_RADIUS_I_HPP_
#define BART_SRC_SOLVER_EIGENVALUE_SPECTRAL_RADIUS_I_HPP_

#include <deal.II/lac/petsc_matrix_base.h>

namespace bart::solver::eigenvalue {

class SpectralRadiusI {
 public:
  using MatrixBase = dealii::PETScWrappers::MatrixBase;
  virtual ~SpectralRadiusI() = default;
  /*! \brief Calculate the spectral radius and corresponding eigenvector.
   *
   * @return pair containing the spectral radius and eigenvector.
   */
  virtual auto SpectralRadius(const MatrixBase&) -> std::pair<double, std::vector<double>> = 0;
};

} // namespace bart::solver::eigenvalue

#endif //BART_SRC_SOLVER_EIGENVALUE_SPECTRAL_RADIUS_I_HPP_


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef MSSM_TWOLOOPHIGGS_H
#define MSSM_TWOLOOPHIGGS_H

#include <Eigen/Core>

/**
 * @file mssm_twoloophiggs.hpp
 * @brief function declarations for 2-loop MSSM Higgs self-energies
 *        and tadpoles
 *
 * Notation:
 *
 * mt2    : squared DR-bar top mass in the MSSM
 * mb2    : squared DR-bar bottom mass in the MSSM
 * mtau2  : squared DR-bar tau mass in the MSSM
 * mg     : DR-bar gluino mass in the MSSM
 * mA2    : squared DR-bar CP-odd Higgs mass in the MSSM
 * mst12  : squared DR-bar lightest stop mass
 * mst22  : squared DR-bar heaviest stop mass
 * msb12  : squared DR-bar lightest sbottom mass
 * msb22  : squared DR-bar heaviest sbottom mass
 * mstau12: squared DR-bar lightest stau mass
 * mstau22: squared DR-bar heaviest stau mass
 * msv2   : squared DR-bar tau sneutrino mass
 *
 * sxt    : sine of DR-bar stop mixing angle in the MSSM
 * cxt    : cosine of DR-bar stop mixing angle in the MSSM
 * sxb    : sine of DR-bar sbottom mixing angle in the MSSM
 * cxb    : cosine of DR-bar sbottom mixing angle in the MSSM
 * sintau : sine of DR-bar stau mixing angle in the MSSM
 * costau : cosine of DR-bar stau mixing angle in the MSSM
 *
 * gs     : DR-bar strong gauge coupling g3 in the MSSM
 * mu     : DR-bar mu-parameter in the MSSM (arXiv:0907.4682)
 * tanb   : DR-bar tan(beta) = vu/vd in the MSSM
 * cotb   : DR-bar 1/tan(beta) in the MSSM
 * vev2   : squared DR-bar vev^2 = (vu^2 + vd^2) in the MSSM
 *
 * scheme : DR-bar scheme (0) or on-shell scheme (1)
 */

namespace flexiblesusy {
namespace mssm_twoloophiggs {

// tadpoles

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_at_as_mssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2,
   double mu, double tanb, double vev2, double gs);

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_at_at_mssm(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_ab_as_mssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2,
   double mu, double cotb, double vev2, double gs);

Eigen::Matrix<double, 2, 1> tadpole_higgs_2loop_atau_atau_mssm(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2);

// self-energies

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_as_mssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_at_mssm(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_ab_as_mssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_atau_atau_mssm(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_at_as_mssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_at_at_mssm(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_ab_as_mssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs);

Eigen::Matrix<double, 2, 2> self_energy_pseudoscalar_2loop_atau_atau_mssm(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2);

// self-energies with tadpoles added

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_as_mssm_with_tadpoles(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_at_at_mssm_with_tadpoles(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_ab_as_mssm_with_tadpoles(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs, int scheme = 0);

Eigen::Matrix<double, 2, 2> self_energy_higgs_2loop_atau_atau_mssm_with_tadpoles(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2, int scheme = 0);

double self_energy_pseudoscalar_2loop_at_as_mssm_with_tadpoles(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double mu,
   double tanb, double vev2, double gs);

double self_energy_pseudoscalar_2loop_at_at_mssm_with_tadpoles(
   double mt2, double mb2, double mA2, double mst12,
   double mst22, double msb12, double msb22,
   double sxt, double cxt, double sxb, double cxb,
   double scale2, double mu, double tanb, double vev2);

double self_energy_pseudoscalar_2loop_ab_as_mssm_with_tadpoles(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double mu,
   double cotb, double vev2, double gs);

double self_energy_pseudoscalar_2loop_atau_atau_mssm_with_tadpoles(
   double mtau2, double mA2, double msv2, double mstau12,
   double mstau22, double sintau, double costau, double scale2,
   double mu, double tanb, double vev2);

} // namespace mssm_twoloophiggs
} // namespace flexiblesusy

#endif