/*
(c) 2012 Fengtao Fan
*/
#include "ANNSearchSampling.h"
#include "SimpleGraph.h"
#include "PointSet.h"
#include "GIComplex.h"

#include <iostream>
#include <vector>
#include <cmath>
#include <sstream>

#include <boost/program_options.hpp>

////////////////
void ConstructRipsGraph(const float eps_val, const PointSet &inPts,  SimpleGraph &outRipsGraph)
{
//	Construct Rips graph on the input points
	float sqDistance = eps_val * eps_val;
	ANNSearch::BuildDistanceGraph_ann(outRipsGraph, inPts, sqDistance);
	return;
}
void ComputeEdgeWeights(const PointSet &inPts, SimpleGraph &outRipsGraph)
{
	float segLength = 0.f;
	LongVector tempDiff;
	//
	for (unsigned int i = 0; i < outRipsGraph.vecNode.size(); i++)
	{
		for (std::map<int, float>::iterator mIter = outRipsGraph.vecNode[i].edgeWeights.begin();
				mIter != outRipsGraph.vecNode[i].edgeWeights.end();
				mIter++)
		{
			tempDiff = (*inPts._PointSet)[i] - (*inPts._PointSet)[mIter->first];
			segLength = norm(tempDiff);
			mIter->second = segLength;
		}
	}
	return;
}
void DeltaSparseSampling_EuclideanDistance(const PointSet &inPts,  const float delta_dist,
					std::map<int, int> &SubPointIndex,
					std::vector<int> &ColorMapping)
{
	//
	float sqDistance = 0.0;
	//
	// Perform the delta-sampling delta-sparse subsampling
	//
	sqDistance = delta_dist * delta_dist;
	ColorMapping.resize(inPts._PointSet->size());
	ANNSearch::Subsampling_EuclideanDistance(sqDistance,
											inPts,
											SubPointIndex,
											ColorMapping);

	return;
}
void DeltaSparseSampling_GraphDistance( const SimpleGraph &eps_rips_graph,
										const float delta_dist,
										std::map<int, int> &SubPointIndex,
										std::vector<int> &ColorMapping)
{
	//
	// Perform the delta-sampling delta-sparse subsampling
	//
	ColorMapping.resize(eps_rips_graph.vecNode.size());
	ANNSearch::Subsampling_GraphDistance(delta_dist,
										 eps_rips_graph,
										 SubPointIndex,
										 ColorMapping);
	return;
}
/////////////////

void ComputeBiColoredGraph(const float eps_sampling_dist, const float delta_dist, const bool graph_distance_flag, const PointSet &inPts,
						std::map<int, int> &SubPointIndex, SimpleGraph &biColoredGraph)
{
	SimpleGraph RipsGraph;
	std::vector<int> ColorMapping;

	ConstructRipsGraph(eps_sampling_dist, inPts, RipsGraph);

	//std::cout << "org comp " << RipsGraph.CheckComponents() << std::endl;
	// generate the batch file for creating rips using this rips graph
	// Delat-Sparse-Sampling and construct rips graph on sub-sampling points
	if (graph_distance_flag)
	{
		ComputeEdgeWeights(inPts, RipsGraph);
		DeltaSparseSampling_GraphDistance(RipsGraph, delta_dist, SubPointIndex, ColorMapping);
	}
	else
		DeltaSparseSampling_EuclideanDistance(inPts, delta_dist, SubPointIndex, ColorMapping);

	// construct the bi-colored graph from the org rips graph
	ANNSearch::SetColorMappingAndExtractColoredGraph(ColorMapping, RipsGraph, biColoredGraph);

	//std::cout << "bic comp " << biColoredGraph.CheckComponents() << std::endl;
	biColoredGraph.color_number = (int) SubPointIndex.size();
	return;
}

void ComputeBiColoredGraph(const char* pFileName, const float delta_dist, std::vector<std::vector<float> > &pts,
						std::map<int, int> &SubPointIndex, SimpleGraph &biColoredGraph)
{
	SimpleGraph orgGraph;
	std::vector<int> ColorMapping;

	orgGraph.ReadWeightedGaph(pFileName, pts);

	//std::cout << "org comp " << RipsGraph.CheckComponents() << std::endl;
	// generate the batch file for creating rips using this rips graph
	// Delat-Sparse-Sampling and construct rips graph on sub-sampling points

	DeltaSparseSampling_GraphDistance(orgGraph, delta_dist, SubPointIndex, ColorMapping);

	// construct the bi-colored graph from the org rips graph
	ANNSearch::SetColorMappingAndExtractColoredGraph(ColorMapping, orgGraph, biColoredGraph);

	//std::cout << "bic comp " << biColoredGraph.CheckComponents() << std::endl;
	biColoredGraph.color_number = (int)SubPointIndex.size();
	//RipsGraph.WriteBackToFile("test_rip.txt");
	return;
}

void WriteOFFformatComplex(const char* pFileName, const std::vector<std::vector<float> > &pts, std::map<int, int> &subPointIndices, GIComplex &gic)
{
	std::cout << "Writing < " << pFileName << " >" << std::endl;
	//
	std::ofstream ofile;
	ofile.open(pFileName, std::ifstream::out);
  	//
 	std::stringstream sstr(std::stringstream::in | std::stringstream::out);
	if (ofile.is_open())
	{
		if (!pts.empty())
		{
			// write points out
			std::vector<int> vecSubPts(subPointIndices.size());
			for (std::map<int, int>::iterator mIter = subPointIndices.begin();
				mIter != subPointIndices.end(); mIter++)
			{
				vecSubPts[mIter->second] = mIter->first;
			}
			//
			sstr << pts.front().size() << " " << vecSubPts.size() << std::endl;
			for (unsigned int i = 0; i < vecSubPts.size(); i++)
			{
				for (unsigned v = 0; v < pts.front().size(); v++)
				{
					sstr << pts[vecSubPts[i]][v] << " ";
				}
				sstr << std::endl;
			}
		}
		else
		{
			sstr << "0 " << subPointIndices.size() << std::endl;
			std::vector<int> subVertexIndices(subPointIndices.size());
			for (std::map<int, int>::iterator mIter = subPointIndices.begin();
				mIter != subPointIndices.end(); mIter++)
				subVertexIndices[mIter->second] = mIter->first;
			for (unsigned int i = 0; i < subVertexIndices.size(); i++)
				sstr << subVertexIndices[i] << std::endl;
		}
		//
		// now add edges
		//
		for (int i = 1; i < gic.dim + 1; i++)
		{
			//depth = i + 1; // want to visit all simplices at this dimension
			if (gic.Simplicies_Cnt[i] != 0)
			{// visit each simplex in dimension i
				for (int vid = 0; vid < gic.Simplicies_Cnt[0]; vid++)
				{
					if (gic.head_circular_list_in_each_dim[vid][i - 1]) // i-1 as vertex are stored in an array
					{// the circular list is not empty
						GICSimplicialTreeNode_ptr pIter(gic.head_circular_list_in_each_dim[vid][i - 1]);
						do
						{// visit each simplex
							sstr << i + 1<< " ";
							GICSimplicialTreeNode_ptr pParentIter(pIter);
							//
							do
							{
								sstr << pParentIter->last_label << " ";
								//
								pParentIter = pParentIter->parent_ptr;
							}while (pParentIter);
							sstr << std::endl;
							// move ahead
							pIter = pIter->next_circular_ptr;
						}while (pIter != gic.head_circular_list_in_each_dim[vid][i - 1]);
					} // if head
				}// for vid
			} // if si
		}// for i dim
		sstr << std::endl;
		//
		//ofile << sstr.rdbuf();
		ofile.write(sstr.str().c_str(), sstr.str().size());
		//
		ofile.close();
		sstr.clear();
	}
	else
	{
		std::cout << "Can NOT open file " << pFileName << std::endl;
		exit(0);
	}
	std::cout << "---- Done ---- " << std::endl << std::endl;
	//
}
void WriteOFFformatComplex(const char* pFileName, const PointSet &pts, std::map<int, int> &subPointIndices, GIComplex &gic)
{
	std::cout << "Writing < " << pFileName << " >" << std::endl;
	//
	std::ofstream ofile;
	ofile.open(pFileName, std::ifstream::out);
  	//
 	std::stringstream sstr(std::stringstream::in | std::stringstream::out);
	if (ofile.is_open())
	{
		// write points out
		std::vector<int> vecSubPts(subPointIndices.size());
		for (std::map<int, int>::iterator mIter = subPointIndices.begin();
			mIter != subPointIndices.end(); mIter++)
		{
			vecSubPts[mIter->second] = mIter->first;
		}
		//
		sstr << pts._dimension << " " << vecSubPts.size() << std::endl;
		for (unsigned int i = 0; i < vecSubPts.size(); i++)
		{
			for (unsigned v = 0; v < pts._dimension; v++)
			{
				sstr << (*pts._PointSet)[vecSubPts[i]][v] << " ";
			}
			sstr << std::endl;
		}
		//
		// now add edges
		//
		for (int i = 1; i < gic.dim + 1; i++)
		{
			//depth = i + 1; // want to visit all simplices at this dimension
			if (gic.Simplicies_Cnt[i] != 0)
			{// visit each simplex in dimension i
				for (int vid = 0; vid < gic.Simplicies_Cnt[0]; vid++)
				{
					if (gic.head_circular_list_in_each_dim[vid][i - 1]) // i-1 as vertex are stored in an array
					{// the circular list is not empty
						GICSimplicialTreeNode_ptr pIter(gic.head_circular_list_in_each_dim[vid][i - 1]);
						do
						{// visit each simplex
							sstr << i + 1<< " ";
							GICSimplicialTreeNode_ptr pParentIter(pIter);
							//
							do
							{
								sstr << pParentIter->last_label << " ";
								//
								pParentIter = pParentIter->parent_ptr;
							}while (pParentIter);
							sstr << std::endl;
							// move ahead
							pIter = pIter->next_circular_ptr;
						}while (pIter != gic.head_circular_list_in_each_dim[vid][i - 1]);
					} // if head
				}// for vid
			} // if si
		}// for i dim
		sstr << std::endl;
		//
		//ofile << sstr.rdbuf();
		ofile.write(sstr.str().c_str(), sstr.str().size());
		//
		ofile.close();
		sstr.clear();
	}
	else
	{
		std::cout << "Can NOT open file " << pFileName << std::endl;
		exit(0);
	}
	std::cout << "---- Done ---- " << std::endl << std::endl;
	//
}
/***********************************************/
char strLicenseGIC[] = "THIS SOFTWARE IS PROVIDED \"AS-IS\". THERE IS NO WARRANTY OF ANY KIND. "
"NEITHER THE AUTHORS NOR THE OHIO STATE UNIVERSITY WILL BE LIABLE FOR "
"ANY DAMAGES OF ANY KIND, EVEN IF ADVISED OF SUCH POSSIBILITY. \n"
"\n"
"This software was developed (and is copyrighted by) the Jyamiti group at "
"The Ohio State University. Please do not redistribute this software. "
"This program is for academic research use only. This software uses the "
"Boost library (www.boost.org) and Ann library "
"(www.cs.umd.edu/~mount/ANN/) which are covered under their own licenses.\n"
"\n"
"The Boost library's license "
"(which applies to the Boost library ONLY and NOT to this program itself) is "
"as follows:\n"
"\n"
"LICENSE\n"
"---------------------------------------------------------------------------\n"
"Boost Software License - Version 1.0 - August 17th, 2003\n"
"\n"
"Permission is hereby granted, free of charge, to any person or organization "
"obtaining a copy of the software and accompanying documentation covered by "
"this license (the \"Software\") to use, reproduce, display, distribute, "
"execute, and transmit the Software, and to prepare derivative works of the "
"Software, and to permit third-parties to whom the Software is furnished to "
"do so, all subject to the following: \n"
"\n"
"The copyright notices in the Software and this entire statement, including "
"the above license grant, this restriction and the following disclaimer, "
"must be included in all copies of the Software, in whole or in part, and "
"all derivative works of the Software, unless such copies or derivative "
"works are solely in the form of machine-executable object code generated by "
"a source language processor. \n"
"\n"
"THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR "
"IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, "
"FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT "
"SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE "
"FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, "
"ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER "
"DEALINGS IN THE SOFTWARE. \n"
"---------------------------------------------------------------------------\n"
"\n"
"ANN library's license "
"(which applies to the ANN library ONLY and NOT to this program itself) is "
"as follows: \n"
"\n"
"LICENSE\n"
"---------------------------------------------------------------------------\n"
"The ANN Library (all versions) is provided under the terms and "
"conditions of the GNU Lesser General Public Library, which is stated "
"below.  It can also be found at: \n"
"\n"
"   http:////www.gnu.org/copyleft/lesser.html \n"
"---------------------------------------------------------------------------\n";
/**********************************************************************/

bool ParseCommand(int argc, char** argv,
				int &input_type,
				std::string &InputFile,
				std::string &OutputFile,
				bool &graph_distance_flag,
				float &eps_dist,
				float &delta_dist,
				int &max_dimension)
{
	try
	{
		// Define the program options description
//		namespace po = boost::program_options;
		boost::program_options::options_description desc("GIComplex Usage");
		desc.add_options()
			(",h", "Help information;")
			(",l", "License information;")
			("type", boost::program_options::value<int>(&input_type)->required(), "Input data type: point cloud (0) or weighted graph (1);")
			(",I", boost::program_options::value<std::string>(&InputFile)->required(), "Input file name;")
			(",O", boost::program_options::value<std::string>(&OutputFile)->required(), "Output file name prefix;")
			(",g", boost::program_options::value<bool>(&graph_distance_flag)->default_value(false), "Subsample by Euclidean distance (false) or graph distance (true); for weighted graph input, it's always (true);")
			("epsilon", boost::program_options::value<float>(&eps_dist), "Parameter for building the Rips graph on input samples; Not used for weighted graph input;")
			("delta", boost::program_options::value<float>(&delta_dist)->required(), "Parameter for generating subsamples from input samples;")
			("dim", boost::program_options::value<int>(&max_dimension)->required(), "The maximum dimension of simplices in output complex;");

		// Parser map
		boost::program_options::variables_map vm;
		try
		{
			boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);

			//
			if (vm.count("-h"))
			{
				std::cout << desc << std::endl;
			}
			//
			if (vm.count("-l"))
			{
				std::cout << strLicenseGIC << std::endl;
			}
			//
			boost::program_options::notify(vm);
		}
		catch(boost::program_options::required_option& e)
		{
			std::cerr<< "ERROR: " << e.what() << std::endl;
			return false;
		}
		catch(boost::program_options::error& e)
		{
			std::cerr<< "ERROR: " << e.what() << std::endl;
			return false;
		}
	}
	catch(std::exception& e)
	{
		std::cerr << "Unhandled Exception reached the top of main: "
					<< e.what() << ", application will now exit" << std::endl;
		return false;

	}
	return true;
}

/*********************/
/*int main(int argc, char **argv)
{
	//
	std::string InputFileName;
	std::string OutputFileName;
	int input_type = 0;
	float eps_dist = .05f;
	float delta_dist = 0.9f;
	bool graph_distance_flag = true;
	int max_dimension = 3;

	//
	if (ParseCommand(argc, argv, input_type, InputFileName, OutputFileName, graph_distance_flag, eps_dist, delta_dist, max_dimension))
	{
		if (!input_type)
		{
			PointSet pts;
			//pts.SampleCrossLine(0.02, 1.0);
			pts.ReadPointsFromFile(InputFileName.c_str());
			std::map<int, int> SubPointIndex;
			SimpleGraph biColoredGraph;
			//
			std::cout << std::endl << "Construct graph induced compolex " << std::endl << std::endl;
			//
			ComputeBiColoredGraph(eps_dist, delta_dist, graph_distance_flag, pts, SubPointIndex, biColoredGraph);
			//
			std::string outfile_name(OutputFileName);
			//
			GIComplex gic(max_dimension, biColoredGraph.color_number, &biColoredGraph);
			gic.Construction();
			//
			outfile_name = outfile_name + "_stat.txt";
			gic.WriteStatisticsToFile(outfile_name.c_str());
			//
			outfile_name = OutputFileName + "_complex.txt";
			WriteOFFformatComplex(outfile_name.c_str(), pts, SubPointIndex, gic);
		}
		else
		{// read weighted graph
			std::vector<std::vector<float> > pts;
			std::map<int, int> SubPointIndex;
			SimpleGraph biColoredGraph;
			//
			ComputeBiColoredGraph(InputFileName.c_str(), delta_dist, pts, SubPointIndex, biColoredGraph);
			//
			//
			std::cout << std::endl << "Construct graph induced compolex " << std::endl << std::endl;
			//
			std::string outfile_name(OutputFileName);
			//
			GIComplex gic(max_dimension, biColoredGraph.color_number, &biColoredGraph);
			gic.Construction();
			//
			outfile_name = outfile_name + "_stat.txt";
			gic.WriteStatisticsToFile(outfile_name.c_str());
			//
			outfile_name = OutputFileName + "_complex.txt";
			WriteOFFformatComplex(outfile_name.c_str(), pts, SubPointIndex, gic);
		}
		//pts.WriteBackToFile("test.txt");
		//biColoredGraph.WriteBackToFile("test_graph.txt");
	}
	//
	return 0;
}*/


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

#include <doctest/doctest.h>

#include <boost/algorithm/string/trim.hpp>
#include <climits>
#include <coati/utils.hpp>
#include <filesystem>

namespace coati::utils {

/**
 * \brief Hamming distance between two codons
 *
 * Number of positions in which the two codons are different.
 *
 * @param[in] cod1 uint8_t encoded codon.
 * @param[in] cod2 uint8_t encoded codon.
 *
 * \return hamming distance between two codons (int).
 */
int cod_distance(uint8_t cod1, uint8_t cod2) {
    int distance = 0;

    distance += (((cod1 & 48) >> 4) == ((cod2 & 48) >> 4) ? 0 : 1);
    distance += (((cod1 & 12) >> 2) == ((cod2 & 12) >> 2) ? 0 : 1);
    distance += ((cod1 & 3) == (cod2 & 3) ? 0 : 1);

    return distance;
}

/**
 * \brief Cast codon to position in codon list (AAA->0, AAAC->1 ... TTT->63)
 *
 * @param[in] codon std::string codon.
 *
 * \return encoded codon as its position in codon list.
 */
int cod_int(const std::string& codon) {
    unsigned char pos0 = codon[0];
    unsigned char pos1 = codon[1];
    unsigned char pos2 = codon[2];

    return (nt4_table[pos0] << 4) | (nt4_table[pos1] << 2) | nt4_table[pos2];
}

/**
 * \brief Read substitution rate matrix from a CSV file
 *
 * Read from file a branch length and a codon substitution rate matrix.
 *  File is expected to have 4067 lines; 1 with branch length and 4096 with
 *  the following structure: codon,codon,value (e.g. AAA,AAA,0.0015).
 *
 * @param[in] file std::string path to input file.
 *
 * \return codon substitution matrix (coati::Matrixf).
 */
coati::Matrixf parse_matrix_csv(const std::string& file) {
    float br_len{NAN};
    Matrix64f Q;
    std::ifstream input(file);
    if(!input.good()) {
        throw std::invalid_argument("Error opening file " + file + ".");
    }

    std::string line;
    // Read branch length
    getline(input, line);
    br_len = stof(line);

    std::vector<std::string> vec{"", "", ""};
    int count = 0;

    while(std::getline(input, line)) {
        std::stringstream ss(line);
        getline(ss, vec[0], ',');
        getline(ss, vec[1], ',');
        getline(ss, vec[2], ',');
        Q(cod_int(vec[0]), cod_int(vec[1])) = stof(vec[2]);
        count++;
    }

    input.close();

    if(count != 4096) {
        throw std::invalid_argument(
            "Error reading substitution rate CSV file. Exiting!");
    }

    Q = Q * br_len;
    Q = Q.exp();

    coati::Matrix<coati::float_t> P(64, 64, Q);

    return P;
}

/// @private
TEST_CASE("parse_matrix_csv") {
    std::ofstream outfile;
    coati::Matrix<coati::float_t> P(
        mg94_p(0.0133, 0.2, {0.308, 0.185, 0.199, 0.308}));

    const std::vector<std::string> codons = {
        "AAA", "AAC", "AAG", "AAT", "ACA", "ACC", "ACG", "ACT", "AGA", "AGC",
        "AGG", "AGT", "ATA", "ATC", "ATG", "ATT", "CAA", "CAC", "CAG", "CAT",
        "CCA", "CCC", "CCG", "CCT", "CGA", "CGC", "CGG", "CGT", "CTA", "CTC",
        "CTG", "CTT", "GAA", "GAC", "GAG", "GAT", "GCA", "GCC", "GCG", "GCT",
        "GGA", "GGC", "GGG", "GGT", "GTA", "GTC", "GTG", "GTT", "TAA", "TAC",
        "TAG", "TAT", "TCA", "TCC", "TCG", "TCT", "TGA", "TGC", "TGG", "TGT",
        "TTA", "TTC", "TTG", "TTT"};

    outfile.open("test-marg-matrix.csv");
    REQUIRE(outfile);

    float Q[4096]{0.0f};
    for(auto i = 0; i < 640; i++) {
        Q[mg94_indexes[i]] = mg94Q[i];
    }

    outfile << "0.0133" << std::endl;  // branch length
    for(auto i = 0; i < 64; i++) {
        for(auto j = 0; j < 64; j++) {
            outfile << codons[i] << "," << codons[j] << "," << Q[i * 64 + j]
                    << std::endl;
        }
    }

    outfile.close();
    coati::Matrix<coati::float_t> P_test(
        parse_matrix_csv("test-marg-matrix.csv"));
    for(auto i = 0; i < 64; i++) {
        for(auto j = 0; j < 64; j++) {
            CHECK(P(i, j) == doctest::Approx(P_test(i, j)));
        }
    }
    CHECK(std::filesystem::remove("test-marg-matrix.csv"));
}

/**
 * \brief Setup command line options
 *
 * @param[in] app CLI::App command line arguments parser from CLI11.
 * @param[in,out] args coati::args_t to store the input parameters.
 * @param[in] command std::string one of coati commands (i.e. alignpair or msa).
 *
 */
void set_cli_options(CLI::App& app, coati::args_t& args,
                     const Command& command) {
    app.add_option("input", args.aln.data.path,
                   "Input file (FASTA/PHYLIP/JSON accepted)")
        ->required();
    if(command == Command::MSA) {
        app.add_option("tree", args.aln.tree, "Newick phylogenetic tree")
            ->required()
            ->check(CLI::ExistingFile);
        app.add_option("reference", args.aln.ref, "Reference sequence")
            ->required();
    }
    app.add_option("-m,--model", args.aln.model, "Substitution model");
    if(command == Command::ALIGNPAIR || command == Command::SAMPLE) {
        app.add_option("-t,--time", args.aln.br_len,
                       "Evolutionary time/branch length")
            ->check(CLI::PositiveNumber);
    }
    if(command == Command::ALIGNPAIR) {
        app.add_option("-l,--weight", args.aln.weight_file,
                       "Write alignment score to file");
        app.add_flag("-s,--score", args.aln.score, "Score alignment");
    }
    app.add_option("-o,--output", args.aln.output, "Alignment output file");
    app.add_option("-g,--gap-open", args.aln.gap.open, "Gap opening score")
        ->check(CLI::PositiveNumber);
    app.add_option("-e,--gap-extend", args.aln.gap.extend,
                   "Gap extension score")
        ->check(CLI::PositiveNumber);
    app.add_option("-w,--omega", args.aln.omega,
                   "Nonsynonymous-synonymous bias")
        ->check(CLI::PositiveNumber);
    app.add_option("-p,--pi", args.aln.pi, "Nucleotide frequencies (A C G T)")
        ->expected(4);
    app.add_option("-k,--gap-len", args.aln.gap.len, "Set gap unit size");
    app.add_option("-x,--sigma", args.aln.sigma,
                   "GTR sigma parameters (AC AG AT CG CT GT)")
        ->expected(6);
    if(command == Command::SAMPLE) {
        // app.add_option("-T,--temperature", args.temperature, "Sampling
        // temperature");
        app.add_option("-n,--sample-size", args.sample_size, "Sample size");
        app.add_option("-d, --seed", args.seeds,
                       "Space separated list of seed(s) used for sampling");
        //->expected(Nmin,Nmax);
    }
}

/**
 * \brief Setup command line options for coati format.
 *
 * @param[in] app CLI::App command line arguments parser from CLI11.
 * @param[in,out] args coati::args_t to store the input parameters.
 * @param[in] command std::string one of coati commands (i.e. alignpair or msa).
 *
 */
void set_cli_options_format(CLI::App& app, coati::args_t& args,
                            const Command& command) {
    app.add_option("input", args.aln.data.path,
                   "Input file (FASTA/PHYLIP/JSON accepted)")
        ->required();
    app.add_option("-o,--output", args.aln.output, "Alignment output file");
    auto phase = app.add_flag("-p,--preserve-phase", args.preserve_phase,
                              "Preserve phase");
    app.add_option("-c,--padding", args.padding,
                   "Padding char to format preserve phase")
        ->needs(phase);
}

/**
 * \brief Encode two sequences as vector<unsigned char>
 *
 * Encode ancestor (ref) sequence as codon \& phase, descendant as nucleotide.
 *  ref: AAA \& position 0 -> 0, AAA \& 1 -> 1, ... , TTT \& 3 -> 191.
 *  des: A -> 0, C -> 1, G -> 2, T -> 3.
 *
 * @param[in] anc std::string sequence of ancestor (reference).
 * @param[in] des std::string sequence of descendant.
 *
 * \return two sequences (ancestor \& descendant) encoded
 *  (coati::sequence_pair_t).
 */
sequence_pair_t marginal_seq_encoding(const std::string& anc,
                                      const std::string& des) {
    sequence_pair_t ret(2);
    ret[0].reserve(anc.length());
    ret[1].reserve(des.length());

    // encode phase & codon: AAA0->0, AAA1->1, AAA2->2, AAC0->3, ... , TTT3->191
    for(auto it = anc.cbegin(); it != anc.cend(); it++) {
        auto c0 = static_cast<unsigned char>(
                      nt4_table[static_cast<unsigned char>(*it)])
                  << 4;
        it++;
        auto c1 = static_cast<unsigned char>(
                      nt4_table[static_cast<unsigned char>(*it)])
                  << 2;
        it++;
        auto c2 = static_cast<unsigned char>(
            nt4_table[static_cast<unsigned char>(*it)]);
        auto cod = (c0 | c1 | c2) * 3;
        ret[0].push_back(cod);
        ret[0].push_back(cod + 1);
        ret[0].push_back(cod + 2);
    }

    //  using nt4_table that converts A->0, C->1, G->2, T->3
    for(auto nuc : des) {
        ret[1].push_back(nt4_table[static_cast<unsigned char>(nuc)]);
    }

    return ret;
}

/// @private
TEST_CASE("marginal_seq_encoding") {
    std::string anc = "AAAGGGTTT", des = "ACGT-";
    auto result = marginal_seq_encoding(anc, des);

    CHECK(result[0][0] == static_cast<unsigned char>(0));
    CHECK(result[0][1] == static_cast<unsigned char>(1));
    CHECK(result[0][2] == static_cast<unsigned char>(2));
    CHECK(result[0][3] == static_cast<unsigned char>(126));
    CHECK(result[0][4] == static_cast<unsigned char>(127));
    CHECK(result[0][5] == static_cast<unsigned char>(128));
    CHECK(result[0][6] == static_cast<unsigned char>(189));
    CHECK(result[0][7] == static_cast<unsigned char>(190));
    CHECK(result[0][8] == static_cast<unsigned char>(191));
    CHECK(result[1][0] == static_cast<unsigned char>(0));
    CHECK(result[1][1] == static_cast<unsigned char>(1));
    CHECK(result[1][2] == static_cast<unsigned char>(2));
    CHECK(result[1][3] == static_cast<unsigned char>(3));
    CHECK(result[1][4] == static_cast<unsigned char>(4));
}

/**
 * \brief Set subtitution matrix or FST according to model
 *
 * @param[in,out] aln coati::alignment_t alignment information.
 */
void set_subst(alignment_t& aln) {
    Matrixf P(64, 64);

    if(!aln.rate.empty()) {
        aln.model = "user_marg_model";
        P = parse_matrix_csv(aln.rate);
        aln.subst_matrix = marginal_p(P, aln.pi);
    } else if(aln.model.compare("m-ecm") == 0) {
        P = ecm_p(aln.br_len, aln.omega);
        aln.subst_matrix = marginal_p(P, aln.pi);
    } else if(aln.model.compare("m-coati") == 0) {  // m-coati
        P = mg94_p(aln.br_len, aln.omega, aln.pi);
        aln.subst_matrix = marginal_p(P, aln.pi);
    } else if(aln.model.compare("coati") == 0) {
        aln.subst_fst = mg94(aln.br_len, aln.omega, aln.pi);
    } else if(aln.model.compare("dna") == 0) {
        aln.subst_fst = dna(aln.br_len, aln.omega, aln.pi);
    } else if(aln.model.compare("ecm") == 0) {
        aln.subst_fst = ecm(aln.br_len, aln.omega);
        aln.pi = {0.2676350, 0.2357727, 0.2539630, 0.2426323};
    } else {
        throw std::invalid_argument("Mutation model unknown.");
    }
}

/**
 * \brief Extract file type from path.
 *
 * Extracts extension and filename from both file.ext and ext:file.foo
 * Trims whitespace as well.
 *
 * @param[in] path std::string path to input file.
 *
 * \return coati::file_type_t object containing the path and extension.
 */
file_type_t extract_file_type(std::string path) {
    constexpr auto npos = std::string::npos;

    // trim whitespace
    boost::algorithm::trim(path);

    // Format ext:path
    auto colon = path.find_first_of(':');
    if(colon != npos && colon > 1) {
        auto filepath = path.substr(colon + 1);
        auto ext = "." + path.substr(0, colon);
        return {std::move(filepath), std::move(ext)};
    }
    std::filesystem::path fpath{path};
    return {std::move(path), fpath.extension()};
}

/// @private
TEST_CASE("extract_file_type") {
    auto test = [](std::string filename, file_type_t expected) {
        CAPTURE(filename);
        auto test = extract_file_type(filename);
        CHECK(test.path == expected.path);
        CHECK(test.type_ext == expected.type_ext);
    };

    test("foo.bar", {"foo.bar", ".bar"});
    test("my:foo.bar", {"foo.bar", ".my"});
    test(".bar", {".bar", ""});
    test(".", {".", ""});
    test("..", {"..", ""});
    test("my:.foo.bar", {".foo.bar", ".my"});
    test(".foo.bar", {".foo.bar", ".bar"});
    test("", {"", ""});
    test(std::string{}, {{}, {}});
    test("foo:-", {"-", ".foo"});
    test("foo:bar", {"bar", ".foo"});
    test("bar:", {"", ".bar"});
    test("c:foo.bar", {"c:foo.bar", ".bar"});

    test(" \f\n\r\t\vfoo.bar \f\n\r\t\v", {"foo.bar", ".bar"});
    test(" \f\n\r\t\vmy:foo.bar \f\n\r\t\v", {"foo.bar", ".my"});
    test(" \f\n\r\t\v.bar \f\n\r\t\v", {".bar", ""});
    test(" \f\n\r\t\v", {{}, {}});
}

/**
 * \brief Read sequences and names for any supported format.
 *
 * @param[in] aln coati::alignment_t alignment information.
 *
 * \return coati::data_t object.
 */
data_t read_input(alignment_t& aln) {
    if(aln.output.empty()) {  // default output: json format & stdout
        aln.output = "json:-";
    }
    data_t input_data;
    coati::file_type_t in_type = coati::utils::extract_file_type(aln.data.path);

    // call reader depending on file type
    if(in_type.type_ext == ".fa" || in_type.type_ext == ".fasta") {
        input_data = read_fasta(aln.data.path, aln.is_marginal());
        input_data.out_file = coati::utils::extract_file_type(aln.output);
        return input_data;
    } else if(in_type.type_ext == ".phy") {
        input_data = read_phylip(aln.data.path, aln.is_marginal());
        input_data.out_file = coati::utils::extract_file_type(aln.output);
        return input_data;
    } else if(in_type.type_ext == ".json") {
        input_data = read_json(aln.data.path, aln.is_marginal());
        input_data.out_file = coati::utils::extract_file_type(aln.output);
        return input_data;
    } else {
        if(aln.data.path.empty()) {
            aln.data.path = "(empty)";
        }
        throw std::invalid_argument("Invalid input " + aln.data.path.string() +
                                    ".");
    }
}

/// @private
TEST_CASE("read_input") {
    std::ofstream outfile;
    coati::alignment_t aln;

    SUBCASE("fasta") {
        std::string filename{"test-read-input.fasta"};
        outfile.open(filename);
        REQUIRE(outfile);
        outfile << "; comment line" << std::endl;
        outfile << ">1" << std::endl << "CTCTGGATAGTC" << std::endl;
        outfile << ">2" << std::endl << "CTATAGTC" << std::endl;
        outfile.close();

        aln.data.path = filename;
        coati::data_t fasta = read_input(aln);
        CHECK(std::filesystem::remove(filename));

        CHECK(fasta.names[0] == "1");
        CHECK(fasta.names[1] == "2");
        CHECK(fasta.seqs[0] == "CTCTGGATAGTC");
        CHECK(fasta.seqs[1] == "CTATAGTC");
    }
    SUBCASE("phylip") {
        std::string filename{"test-read-input.phy"};
        outfile.open(filename);
        REQUIRE(outfile);
        outfile << "2 12" << std::endl;
        outfile << "1\tCTCTGGATAGTC" << std::endl;
        outfile << "2\tCTCTGGATAGTC" << std::endl;
        outfile.close();

        aln.data.path = filename;
        coati::data_t phylip = read_input(aln);
        CHECK(std::filesystem::remove(filename));

        CHECK(phylip.names[0] == "1");
        CHECK(phylip.names[1] == "2");
        CHECK(phylip.seqs[0] == "CTCTGGATAGTC");
        CHECK(phylip.seqs[1] == "CTCTGGATAGTC");
    }
    SUBCASE("json") {
        std::string filename{"test-read-input.json"};
        outfile.open(filename);
        REQUIRE(outfile);
        outfile << "{\"data\":{\"names\":[\"a\",\"b\"],\"seqs\":"
                   "[\"CTCTGGATAGTC\",\"CTCTGGATAGTC\"]}}"
                << std::endl;
        outfile.close();

        aln.data.path = filename;
        coati::data_t json = read_input(aln);
        CHECK(std::filesystem::remove(filename));

        CHECK(json.names[0] == "a");
        CHECK(json.names[1] == "b");
        CHECK(json.seqs[0] == "CTCTGGATAGTC");
        CHECK(json.seqs[1] == "CTCTGGATAGTC");
    }
    SUBCASE("ext") {
        std::string filename("test-read-input.ext");
        aln.data.path = filename;
        REQUIRE_THROWS_AS(read_input(aln), std::invalid_argument);
    }
}

/**
 * \brief Write sequences and names in any suppported format.
 *
 * @param[in] data coati::data_t sequences, names, fsts, and weight information.
 * @param[in] aln_path coati::VectorFstStdArc FST object with alignment.
 *
 * \return coati::data_t object.
 */
bool write_output(data_t& data, const VectorFstStdArc& aln_path) {
    // call writer depending on file type
    if(data.out_file.type_ext == ".fa" || data.out_file.type_ext == ".fasta") {
        return write_fasta(data, aln_path);
    } else if(data.out_file.type_ext == ".phy") {
        return write_phylip(data, aln_path);
    } else if(data.out_file.type_ext == ".json") {
        return write_json(data, aln_path);
    } else {
        throw std::invalid_argument("Invalid output " + data.out_file.path +
                                    ".");
    }
}

/// @private
TEST_CASE("write_output") {
    coati::data_t data;
    std::vector<std::string> names = {"anc", "des"};
    std::vector<std::string> sequences = {
        "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTAC"
        "GTACGTACGTACGTACGTACGTACGTACGTACGT",
        "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTAC"
        "GTACGTACGTACGTACGTACGTACGTACGTACGT"};  // length > 100 to test new line

    SUBCASE("fasta") {
        data = coati::data_t("", names, sequences);
        data.out_file.path = "test-write-output-fasta.fasta";
        data.out_file.type_ext = ".fasta";
        REQUIRE(write_output(data));

        std::ifstream infile(data.out_file.path);
        std::string s;
        infile >> s;
        CHECK(s == ">anc");
        infile >> s;
        CHECK(s ==
              "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT"
              "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT");
        infile >> s;
        CHECK(s == ">des");
        infile >> s;
        CHECK(s ==
              "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT"
              "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT");
        CHECK(std::filesystem::remove("test-write-output-fasta.fasta"));
    }

    SUBCASE("phylip") {
        data = coati::data_t("", names, sequences);
        data.out_file.path = "test-write-output-phylip.phy";
        data.out_file.type_ext = ".phy";
        REQUIRE(write_output(data));

        std::ifstream infile(data.out_file.path);
        std::string s1, s2;
        infile >> s1 >> s2;
        CHECK(s1 == "2");
        CHECK(s2 == "104");
        infile >> s1 >> s2;
        CHECK(s1 == "anc");
        CHECK(s2 ==
              "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT"
              "ACGTACGTACGTACGTACGTACGTACGTA");
        infile >> s1 >> s2;
        CHECK(s1 == "des");
        CHECK(s2 ==
              "ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT"
              "ACGTACGTACGTACGTACGTACGTACGTA");
        // infile >> s1;  // blank line
        infile >> s1 >> s2;
        CHECK(s1 == "CGTACGTACGT");
        CHECK(s2 == "CGTACGTACGT");
        CHECK(std::filesystem::remove("test-write-output-phylip.phy"));
    }

    SUBCASE("json") {
        data = coati::data_t("", names, sequences);
        data.out_file.path = "test-write-output-phylip.json";
        data.out_file.type_ext = ".json";
        REQUIRE(write_json(data));

        std::ifstream infile(data.out_file.path);
        std::string s1;
        infile >> s1;
        CHECK(s1 ==
              "{\"data\":{\"names\":[\"anc\",\"des\"],\"seqs\":"
              "[\"ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTA"
              "CGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT\","
              "\"ACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGTAC"
              "GTACGTACGTACGTACGTACGTACGTACGTACGTACGTACGT\"]}}");
    }

    SUBCASE("ext") {
        data = coati::data_t("", names, sequences);
        data.out_file.path = "test-write-output-ext.ext";
        data.out_file.type_ext = ".ext";
        REQUIRE_THROWS_AS(write_output(data), std::invalid_argument);
    }
}

/**
 * \brief Convert alignment FST to std::string sequences.
 *
 * @param[in] data coati::data_t sequences, names, fst, weight information.
 * @param[in] aln coati::alignment_t alignment object.
 */
void fst_to_seqs(coati::data_t& data, const VectorFstStdArc& aln) {
    fst::SymbolTable symbols;
    fill_symbol_table(symbols);

    std::string seq1, seq2;
    fst::StateIterator<fst::StdFst> siter(aln);  // FST state iterator
    for(int i = 0; i < (aln.NumStates() - 1); siter.Next(), i++) {
        fst::ArcIteratorData<fst::StdArc> info;
        aln.InitArcIterator(siter.Value(), &info);
        seq1.append(symbols.Find(info.arcs[0].ilabel));
        seq2.append(symbols.Find(info.arcs[0].olabel));
    }

    data.seqs.clear();
    data.seqs.resize(2);
    data.seqs[0] = seq1;
    data.seqs[1] = seq2;

    // map all epsilons (<eps>) to gaps (-)
    while(data.seqs[0].find("<eps>") != std::string::npos) {
        data.seqs[0].replace(data.seqs[0].find("<eps>"), 5, "-");
    }
    while(data.seqs[1].find("<eps>") != std::string::npos) {
        data.seqs[1].replace(data.seqs[1].find("<eps>"), 5, "-");
    }
}

}  // namespace coati::utils


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_TIME_CONVERSIONS
#define BOOST_ASTRONOMY_TIME_CONVERSIONS

#include <string>
#include <iostream>
#include <exception>
#include <boost/astronomy/time/parser.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

using namespace std;
using namespace boost::gregorian;
using namespace boost::posix_time;

/**
 * Universal time (UT), and therefore the local civil time in any
 * part of the world, is related to the apparent motion of the Sun
 * around the Earth.
 *
 * Sidereal Time (ST) is a time scale that is based on Earth's rate
 * of rotation measured relative to the fixed stars.
 */

/**
 * The Greenwich Sidereal Time(GST) is the sidereal time correct for observations
 * made on the Greenwich meridian, longitude 0◦.
 */
decimal_hour GST(ptime t)
{
    //Get date from UT
    date d = t.date();

    //Get Julian Day Number
    double JD = d.julian_day(); //Ambiguity in Julian precision.

    double S = JD - 2451545.0;

    double T = S/36525.0;

    double T0 = 6.697374558 + (2400.051336 * T) + (0.000025862 * T * T);

    //Reduce the result to the range 0 to 24 by adding or subtracting multiples of 24
    T0 = T0 - 24.0 * floor(T0/24.0);

    //Convert UT to decimal hours
    double UT = ((t.time_of_day().seconds())/60.0 + t.time_of_day().minutes())/60.0 + t.time_of_day().hours();\

    //Multiply UT by 1.002737909
    double A = UT * 1.002737909;

    T0 += A;

    //Add this to T0 and reduce to the range 0 to 24 if necessary by subtracting or adding 24. This is the GST.
    T0 = T0 - 24.0 * floor(T0/24.0);

    //Return GST in decimal hours
    return {T0};
}

enum class DIRECTION {WEST, EAST};

decimal_hour compute_LST(double longitude, DIRECTION direction, double gst)
{
  //Convert longitude to hours
  double long_hours = longitude / 15.0;

  switch(direction)
  {
    case DIRECTION::WEST:
      //Multiply with direction
      long_hours = -1 * long_hours;
      break;
    case DIRECTION::EAST:
      //Multiply with direction
      long_hours = 1 * long_hours;
      break;
  }

  long_hours = long_hours + gst;

  //Bring the result into the range 0 to 24 by adding or subtracting 24 if necessary.
  //This is the local sidereal time (LST).
  long_hours = long_hours - 24.0 * floor(long_hours/24.0);

  return {long_hours};
}

//Local Sidereal Time (LST)
decimal_hour LST(double longitude, DIRECTION direction, ptime t)
{
  double gst = GST(t).get();

  if(longitude == 0)
    return {gst};

  return compute_LST(longitude,direction,gst);
}

decimal_hour LST(double longitude, DIRECTION direction, double gst)
{
  if(longitude == 0)
    return {gst};

  return compute_LST(longitude,direction,gst);
}

#endif //BOOST_ASTRONOMY_TIME_CONVERSIONS


/*
 * Copyright (c) 2017
 * FZI Forschungszentrum Informatik, Karlsruhe, Germany (www.fzi.de)
 * KIT, Institute of Measurement and Control, Karlsruhe, Germany (www.mrt.kit.edu)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its contributors
 *    may be used to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

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

namespace util_eigen_quadratic_matrices {
/* Extend convenience typedefs of predefined Eigen::Matrix.
 */
using Matrix6d = Eigen::Matrix<double, 6, 6>;
using Vector6d = Eigen::Matrix<double, 6, 1>;

/** @brief Removes the i-th row and the i-th column from a symmetric matrix. Specialization for dynamic-size matrices.
 * @param[in] MxM quadratic input Matrix
 * @param[in] Index of row and column that will be removed
 * @return (M-1) x (M-1) quadratic output Matrix*/
template <typename T>
inline Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> removeRowCol(
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& input, const int n) {
    const int size = static_cast<int>(input.rows());
    if (input.rows() != input.cols()) {
        throw std::invalid_argument("Method is only defined for quadratic matrices.");
    }
    if (size == 0) {
        throw std::invalid_argument(
            "Method is not applicable to matrices of size 0x0. Can not remove rows and columns if there are none.");
    }
    if (size == 1) {
        return Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(0, 0);
    }
    // Shortcut if n = 0 or n = (size -1) to avoid failing Assertion in older Eigen Versions.
    // In older Eigen Versions, the comma-initializer can not handle zero-size vectors or matrices.
    // This is fixed in newer Eigen Versions (>3.2.10 ?).
    if (n == 0) {
        return input.bottomRightCorner(size - 1, size - 1);
    }
    if (n == (size - 1)) {
        return input.topLeftCorner(size - 1, size - 1);
    }
    return (Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(size - 1, size - 1) << input.block(0, 0, n, n),
            input.block(0, n + 1, n, size - n - 1),
            input.block(n + 1, 0, size - n - 1, n),
            input.block(n + 1, n + 1, size - n - 1, size - n - 1))
        .finished();
}

/** @brief Checks if a quadratic matrix is symmetric or not.
 * @param[in] MxM quadratic input Matrix
 * @return true if the matrix is symmetric or false otherwise.*/
template <typename T, int Size>
inline bool isSymmetric(const Eigen::Matrix<T, Size, Size>& input) {
    if (input.rows() != input.cols()) {
        throw std::invalid_argument("Method is only defined for quadratic matrices.");
    }
    for (int i = 1; i < input.rows(); i++) {
        if (!(input.diagonal(i).isApprox(input.diagonal(-i)))) {
            return false;
        }
    }
    return true;
}

/** @brief Checks if a quadratic matrix is positive (semi) definite or not.
 * @param[in] MxM quadratic input Matrix
 * @return true if the matrix is positive (semi) definite or false otherwise.*/
template <typename T, int Size>
inline bool isPosSemiDefinit(const Eigen::Matrix<T, Size, Size>& input) {
    if (input.rows() != input.cols()) {
        throw std::invalid_argument("Method is only defined for quadratic matrices.");
    }
    if (input.rows() == 0) {
        return false; // special case for 0D-zero-elements Matrix. Prevents Seg-Fault.
    }
    Eigen::LDLT<Eigen::Matrix<T, Size, Size>> chol(input);
    return (chol.isPositive());
}
} // namespace util_eigen_quadratic_matrices


#ifndef PA_MATH_SCALAR_FIELD_HPP
#define PA_MATH_SCALAR_FIELD_HPP

#include <boost/multi_array.hpp>

#include <pa/math/types.hpp>
#include <pa/export.hpp>

namespace pa
{
struct PA_EXPORT scalar_field
{
  boost::multi_array<scalar, 3> data    {};
  vector3                       offset  {};
  vector3                       size    {};
  vector3                       spacing {};
};
}

#endif


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

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

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // scaling a std::vector
    std::vector<float> in = {1,2,3,4};
    std::vector<float> out = {2,4,6,8};
    scal<float>(in,2); // in = 2*in
    BOOST_CHECK (in == out);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm (
           make_rowmajor_matrix_local_load<float>("./sample_4x4"));

    // checking scal() operation
    auto row = make_row_vector<float> (cm,1);
    scal<float>(row,5); // row = 5 * row
    cm.to_rowmajor().save("./out_4x4");

    // checking whether the above operation successfully taken place
    BOOST_CHECK (system("diff ./out_4x4 ./ref_4x4") == 0);
    system("rm -f ./out_4x4");
}


/*
 * ConstantMesh.cpp
 *
 *  Created on: 12.07.2017
 *      Author: thies
 */

#include <deal.II/base/memory_consumption.h>
#include <deal.II/base/quadrature.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/numerics/matrix_tools.h>

#include <base/ConstantMesh.h>

namespace wavepi {
namespace base {
using namespace dealii;

template <int dim>
ConstantMesh<dim>::ConstantMesh(std::vector<double> times, FE_Q<dim> fe, Quadrature<dim> quad,
                                std::shared_ptr<Triangulation<dim>> tria)
    : SpaceTimeMesh<dim>(times, fe, quad), triangulation(tria) {
  dof_handler = std::make_shared<DoFHandler<dim>>();
  dof_handler->initialize(*tria, this->fe);
}

template <int dim>
std::shared_ptr<SparseMatrix<double>> ConstantMesh<dim>::get_mass_matrix(size_t time_index) {
  if (!mass_matrix) {
    get_sparsity_pattern(time_index);

    mass_matrix = std::make_shared<SparseMatrix<double>>(*sparsity_pattern);
    dealii::MatrixCreator::create_mass_matrix(*dof_handler, quad, *mass_matrix);
  }

  return mass_matrix;
}

template <int dim>
std::shared_ptr<SparseMatrix<double>> ConstantMesh<dim>::get_laplace_matrix(size_t time_index) {
  if (!laplace_matrix) {
    get_sparsity_pattern(time_index);

    laplace_matrix = std::make_shared<SparseMatrix<double>>(*sparsity_pattern);
    dealii::MatrixCreator::create_laplace_matrix(*dof_handler, quad, *laplace_matrix);
  }

  return laplace_matrix;
}

template <int dim>
std::shared_ptr<AffineConstraints<double>> ConstantMesh<dim>::get_constraint_matrix(size_t idx __attribute((unused))) {
  if (!constraints) {
    constraints = std::make_shared<AffineConstraints<double>>();
    DoFTools::make_hanging_node_constraints(*dof_handler, *constraints);
    constraints->close();
  }

  return constraints;
}

template <int dim>
std::shared_ptr<SparsityPattern> ConstantMesh<dim>::get_sparsity_pattern(size_t time_index) {
  if (!sparsity_pattern) {
    get_constraint_matrix(time_index);

    DynamicSparsityPattern dsp(dof_handler->n_dofs(), dof_handler->n_dofs());
    DoFTools::make_sparsity_pattern(*dof_handler, dsp, *constraints, true);

    sparsity_pattern = std::make_shared<SparsityPattern>();
    sparsity_pattern->copy_from(dsp);
  }

  return sparsity_pattern;
}

template <int dim>
size_t ConstantMesh<dim>::n_dofs(size_t time_index __attribute((unused))) {
  return dof_handler->n_dofs();
}

template <int dim>
std::shared_ptr<DoFHandler<dim>> ConstantMesh<dim>::get_dof_handler(size_t time_index __attribute((unused))) {
  return dof_handler;
}

template <int dim>
std::shared_ptr<Triangulation<dim>> ConstantMesh<dim>::get_triangulation(size_t time_index __attribute((unused))) {
  return triangulation;
}

template <int dim>
std::shared_ptr<DoFHandler<dim>> ConstantMesh<dim>::transfer(size_t source_time_index __attribute((unused)),
                                                             size_t target_time_index __attribute((unused)),
                                                             std::initializer_list<Vector<double>*> vectors
                                                             __attribute((unused))) {
  // LogStream::Prefix p("ConstantMesh");
  // deallog << "Mesh transfer: " << source_time_index << " → " << target_time_index << ", taking "
  //         << vectors.size() << " vector(s) along" << std::endl;

  return dof_handler;
}

template <int dim>
size_t ConstantMesh<dim>::memory_consumption() const {
  size_t mem =
      MemoryConsumption::memory_consumption(*dof_handler) + MemoryConsumption::memory_consumption(dof_handler) +
      MemoryConsumption::memory_consumption(*triangulation) + MemoryConsumption::memory_consumption(triangulation) +
      MemoryConsumption::memory_consumption(times) + MemoryConsumption::memory_consumption(sparsity_pattern) +
      MemoryConsumption::memory_consumption(mass_matrix);

  if (mass_matrix) mem += MemoryConsumption::memory_consumption(*mass_matrix);

  if (sparsity_pattern) mem += MemoryConsumption::memory_consumption(*sparsity_pattern);

  return mem;
}

template class ConstantMesh<1>;
template class ConstantMesh<2>;
template class ConstantMesh<3>;

}  // namespace base
} /* namespace wavepi */


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/abs.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/five.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/sqrt_2.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( abs_cplx,   BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::abs;
  using nt2::tag::abs_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef native<cT,ext_t>                vcT;
  typedef typename nt2::dry<T>             dT;
  typedef native<dT,ext_t>                vdT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Inf<vT>(),nt2::Nan<vT>())), nt2::Inf<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Inf<vT>(), nt2::Zero<vT>())), nt2::Inf<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Minf<vT>(), nt2::Zero<vT>())), nt2::Inf<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Nan<vT>(), nt2::Zero<vT>())), nt2::Nan<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Inf<vdT>())   ), nt2::Inf<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Inf<vdT>())   ), nt2::Inf<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Minf<vdT>())  ), nt2::Inf<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Nan<vdT>())   ), nt2::Nan<vT>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Mone<vT>(), nt2::Zero<vT>())), nt2::One<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::One<vT>(), nt2::Zero<vT>())), nt2::One<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Valmax<vT>(), nt2::Zero<vT>())), nt2::Valmax<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Valmin<vT>(), nt2::Zero<vT>())), nt2::Valmax<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Zero<vT>(), nt2::Zero<vT>())), nt2::Zero<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::One<vT>(), nt2::One<vT>())), nt2::Sqrt_2<vT>(), 1);
  NT2_TEST_ULP_EQUAL(nt2::abs(vcT(nt2::Four<vT>(), nt2::Three<vT>())), nt2::Five<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Mone<vdT>())  ), nt2::One<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::One<vdT>())   ), nt2::One<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Valmax<vdT>())), nt2::Valmax<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Valmin<vdT>())), nt2::Valmax<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Zero<vdT>())  ), nt2::Zero<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::One<vdT>())   ), nt2::One<vT>(), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::abs(vdT(nt2::Four<vdT>())  ), nt2::Four<vT>(), 0.5);
}


// --------------------------------------------------------------------------
// Copyright (C) Karlsruhe Institute of Technology, 2017
//
// 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/>.
// --------------------------------------------------------------------------

#include "src/ca.h"

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <fstream>
#include <iostream>

using Time = boost::posix_time::ptime;
using TimeDuration = boost::posix_time::time_duration;

// init f_a and f_b attributes to 1 by default
CA::CA(Instance _instance)
    : instance(_instance),
      tmp_bids(BidSetAux(instance.getBids())),
      tmp_asks(BidSetAux(instance.getAsks())),
      x(_instance.getBids().N(), 0),
      y(_instance.getBids().N(), _instance.getAsks().N()) {}

CA::CA(Instance _instance, RelevanceMode mode)
    : instance(_instance),
      x(_instance.getBids().N(), 0),
      y(_instance.getBids().N(), _instance.getAsks().N()) {
  std::vector<double> f_b, f_a;
  switch (mode) {
    case RelevanceMode::UNIFORM: {
      f_b = std::vector<double>(instance.L(), 1.);
      f_a = std::vector<double>(instance.L(), 1.);
      break;
    }
    case RelevanceMode::SCARCITY: {
      auto capacity = instance.getAsks().computeQPerResource();
      auto demand = instance.getBids().computeQPerResource();
      for (unsigned int k = 0; k < instance.L(); ++k) {
        f_b.push_back(1. / capacity[k]);
        f_a.push_back(1. / demand[k]);
      }
      break;
    }
    case RelevanceMode::RELATIVE_SCARCITY: {
      auto capacity = instance.getAsks().computeQPerResource();
      auto demand = instance.getBids().computeQPerResource();
      for (unsigned int k = 0; k < instance.L(); ++k) {
        f_b.push_back(std::abs((demand[k] - capacity[k]) * 1. / demand[k]));
        f_a.push_back(std::abs((demand[k] - capacity[k]) * 1. / capacity[k]));
      }
      break;
    }
    default:
      break;
  }
  tmp_bids = BidSetAux(instance.getBids(), f_b);
  tmp_asks = BidSetAux(instance.getAsks(), f_a);
}

void CA::run() {
  resetAllocation();
  // solve WDP and measure time
  Time t0(boost::posix_time::microsec_clock::local_time());
  computeAllocation();
  Time t1(boost::posix_time::microsec_clock::local_time());
  long usec = (t1 - t0).total_microseconds();
  // compute pricing and stats
  computeKPricing(0.5);
  computeStatistics();
  stats.setTimeWdp(usec / 1000.);
}

void CA::resetAllocation() { resetBase(); }

bool CA::noSideEffects() { return noSideEffectsBase(); }

void CA::resetBase() {
  // reset variables if new allocation must be calculated
  x = std::vector<int>(instance.getBids().N(), 0);
  y = boost::numeric::ublas::zero_matrix<int>(instance.getBids().N(),
                                              instance.getAsks().N());

  bid_index = std::vector<int>();
  ask_index = std::vector<int>();
  for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
    bid_index.push_back(i);
  }
  for (unsigned int j = 0; j < instance.getAsks().N(); ++j) {
    ask_index.push_back(j);
  }

  // initialise all prices to 0
  for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
    price_buyer[i] = 0.;
  }
  for (unsigned int j = 0; j < instance.getAsks().N(); ++j) {
    price_seller[j] = 0.;
  }

  stats = Stats();
}

bool CA::noSideEffectsBase() {
  for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
    if (x[i]) return false;
    if (price_buyer[i]) return false;
    for (unsigned int j = 0; j < instance.getAsks().N(); ++j) {
      if (y(i, j)) return false;
      if (price_seller[j]) return false;
    }
  }
  if (!std::is_sorted(bid_index.begin(), bid_index.end())) return false;
  if (!std::is_sorted(ask_index.begin(), ask_index.end())) return false;

  if (stats.getAvgUnitPrice() || stats.getMeanUtility() ||
      stats.getNumGoodsTraded() || stats.getNumWinners() ||
      stats.getStddevUtility() || stats.getTimeWdp() || stats.getWelfare())
    return false;

  return true;
}

void CA::computeKPricing(double kappa) {
  // compute prices
  for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
    for (unsigned int j = 0; j < instance.getAsks().N(); ++j) {
      if (y(i, j) > 0) {
        price_buyer[i] = instance.getAsks().V()[j] * kappa +
                         instance.getBids().V()[i] * (1 - kappa);
        price_seller[j] = price_buyer[i];
        break;
      }
    }
  }
}

void CA::computeStatistics() {
  // return welfare / (n + m);
  // only consider winners in calculations
  double welfare = 0.;
  unsigned int num_goods_traded = 0;
  unsigned int num_winners = 0;
  double mean_utility = 0.;
  double stddev_utility = 0.;
  double avg_unit_price = 0.;

  // ========================================================== //
  // compute social welfare, numer of winners and number of units of goods
  // traded buyers
  for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
    if (x[i] > 0) {
      welfare += (instance.getBids().V()[i] - price_buyer[i]);
      ++num_winners;
      for (unsigned int k = 0; k < instance.L(); ++k) {
        num_goods_traded += instance.getBids().Q()(i, k);
      }
      // sellers
      for (unsigned int j = 0; j < instance.getAsks().N(); ++j) {
        if (y(i, j) > 0) {
          welfare += (price_seller[j] - instance.getAsks().V()[j]);
          ++num_winners;
          break;
        }
      }
    }
  }
  // ========================================================== //
  if (num_winners != 0) {
    // compute average and stddev for utility
    mean_utility = welfare / num_winners;
    // buyers
    for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
      if (x[i] > 0) {
        stddev_utility +=
            (instance.getBids().V()[i] - price_buyer[i] - mean_utility) *
            (instance.getBids().V()[i] - price_buyer[i] - mean_utility);
        // sellers
        for (unsigned int j = 0; j < instance.getAsks().N(); ++j) {
          if (y(i, j) > 0) {
            stddev_utility +=
                (price_seller[j] - instance.getAsks().V()[j] - mean_utility) *
                (price_seller[j] - instance.getAsks().V()[j] - mean_utility);
            break;
          }
        }
      }
    }
    stddev_utility = std::sqrt(stddev_utility / num_winners);
    // ========================================================== //
    // compute average price
    for (unsigned int i = 0; i < instance.getBids().N(); ++i) {
      avg_unit_price += price_buyer[i];
    }
    avg_unit_price /= num_goods_traded;
  }
  // ========================================================== //
  // set fields in stats object
  stats.setWelfare(welfare);
  stats.setNumGoodsTraded(num_goods_traded);
  stats.setNumWinners(num_winners);
  stats.setMeanUtility(mean_utility);
  stats.setStddevUtility(stddev_utility);
  stats.setAvgUnitPrice(avg_unit_price);
}

void CA::printResults(std::string mechanism_name) {
  // print summary to standard output
  stats.printFriendly(std::cout, mechanism_name);
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file find_test.cpp
/// \brief Test for find functions.

#include <algorithm>
#include <iterator>
#include <set>
#include <vector>
#include <boost/test/minimal.hpp>
#include "mcrl2/data/data_specification.h"
#include "mcrl2/data/find.h"
#include "mcrl2/data/standard_utility.h"
#include "mcrl2/data/function_sort.h"

using namespace mcrl2;
using namespace mcrl2::core;
using namespace mcrl2::data;

inline
variable nat(const std::string& name)
{
  return variable(identifier_string(name), sort_nat::nat());
}

inline
variable pos(const std::string& name)
{
  return variable(identifier_string(name), sort_pos::pos());
}

inline
variable bool_(const std::string& name)
{
  return variable(identifier_string(name), sort_bool::bool_());
}

void test_search_variable()
{
  variable b = bool_("b");
  variable c = bool_("c");
  data::variable_vector v;
  v.push_back(b);
  v.push_back(c);
  std::set<variable> s = data::find_all_variables(v);
  BOOST_CHECK(s.size() == 2);
  BOOST_CHECK(s.find(b) != s.end());
  BOOST_CHECK(s.find(c) != s.end());
  BOOST_CHECK(data::search_variable(v, b));
  BOOST_CHECK(data::search_variable(v, c));

  data::variable_list l(v.begin(), v.end());
  s = data::find_all_variables(l);
  BOOST_CHECK(s.size() == 2);
  BOOST_CHECK(s.find(b) != s.end());
  BOOST_CHECK(s.find(c) != s.end());
  BOOST_CHECK(data::search_variable(v, b));
  BOOST_CHECK(data::search_variable(v, c));
}

int test_main(int argc, char* argv[])
{
  variable n1 = nat("n1");
  variable n2 = nat("n2");
  variable n3 = nat("n3");
  variable n4 = nat("n4");

  variable b1 = bool_("b1");
  variable b2 = bool_("b2");
  variable b3 = bool_("b3");
  variable b4 = bool_("b4");

  variable p1 = pos("p1");
  variable p2 = pos("p2");
  variable p3 = pos("p3");
  variable p4 = pos("p4");

  std::set<variable> S;
  S.insert(b1);
  S.insert(p1);
  S.insert(n1);

  std::vector<variable> V;
  V.push_back(b1);
  V.push_back(p1);
  V.push_back(n1);

  sort_expression_vector domain = atermpp::make_vector< sort_expression >(sort_pos::pos(), sort_bool::bool_());
  sort_expression sexpr = function_sort(domain, sort_nat::nat());
  variable q1(identifier_string("q1"), sexpr);

  data_expression x = sort_bool::and_(equal_to(n1, n2), not_equal_to(n2, n3));
  data_expression y = sort_bool::or_(equal_to(p1, p2), sort_bool::and_(x, b2));

  //--- search_variable ---//
  BOOST_CHECK(search_variable(x, n1));
  BOOST_CHECK(search_variable(x, n2));
  BOOST_CHECK(search_variable(x, n3));
  BOOST_CHECK(!search_variable(x, n4));
  BOOST_CHECK(search_variable(S, n1));
  BOOST_CHECK(!search_variable(S, n2));
  BOOST_CHECK(search_variable(V, n1));
  BOOST_CHECK(!search_variable(V, n2));

  //--- find_all_variables ---//
  std::set<variable> v = find_all_variables(x);
  BOOST_CHECK(std::find(v.begin(), v.end(), n1) != v.end());
  BOOST_CHECK(std::find(v.begin(), v.end(), n2) != v.end());
  BOOST_CHECK(std::find(v.begin(), v.end(), n3) != v.end());

  std::set<variable> vS = find_all_variables(S);
  std::set<variable> vV = find_all_variables(V);
  BOOST_CHECK(vS == vV);

  //--- find_free_variables ---//
  v = find_free_variables(x);
  BOOST_CHECK(std::find(v.begin(), v.end(), n1) != v.end());
  BOOST_CHECK(std::find(v.begin(), v.end(), n2) != v.end());
  BOOST_CHECK(std::find(v.begin(), v.end(), n3) != v.end());
  v = find_free_variables(data_expression(n1));
  BOOST_CHECK(std::find(v.begin(), v.end(), n1) != v.end());

  //--- find_sort_expressions ---//
  std::set<sort_expression> e = find_sort_expressions(q1);
  BOOST_CHECK(std::find(e.begin(), e.end(), sort_nat::nat())   != e.end());
  BOOST_CHECK(std::find(e.begin(), e.end(), sort_pos::pos())   != e.end());
  BOOST_CHECK(std::find(e.begin(), e.end(), sort_bool::bool_()) != e.end());
  BOOST_CHECK(std::find(e.begin(), e.end(), sexpr)              != e.end());

  std::set<sort_expression> eS = find_sort_expressions(S);
  std::set<sort_expression> eV = find_sort_expressions(V);
  BOOST_CHECK(eS == eV);

  std::set<sort_expression> Z;
  find_sort_expressions(q1, std::inserter(Z, Z.end()));
  find_sort_expressions(S, std::inserter(Z, Z.end()));

  test_search_variable();

  //--- data_specification ---//
  // TODO: discuss whether this test should fail or not
  //BOOST_CHECK(search_data_expression(data_specification().constructors(), sort_bool::true_()));

  return 0;
}


//==================================================================================================
/**
  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_IS_SIMD_LOGICAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

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

  BOOST_DISPATCH_OVERLOAD(is_simd_logical_
                         , (typename A0,typename X)
                         , bd::cpu_
                         , bs::pack_<bd::integer_<A0>,X>
                         )
  {
    BOOST_FORCEINLINE bool operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return bs::all(logical_or(is_equal(a0, Allbits<A0>()), is_eqz(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD(is_simd_logical_
                         , (typename A0,typename X)
                         , bd::cpu_
                         , bs::pack_<bd::floating_<A0>,X>
                         )
  {
    BOOST_FORCEINLINE bool operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0, unsigned>;
      return is_simd_logical(bitwise_cast<iA0>(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD(is_simd_logical_
                         , (typename A0,typename X)
                         , bd::cpu_
                         , bs::pack_<bs::logical_<A0>,X>
                         )

  {
    BOOST_FORCEINLINE bool operator()(A0 const&) const
    {
      return true;
    }
  };
} } }

#endif


#include <iostream>
#include <iomanip>   // setpercision
#include <armadillo>
#include <vector>
#include <sys/stat.h>	//mkdir
#include <sys/types.h>	//mkdir
#include <string>

#include "Sto_IHT.h"
#include "Functions.h"
#include "Bayes_Sto_IHT.h"
#include "Parallel_AMP.h"

using namespace std;
using namespace arma;

// ---------------------------------------------------
// ---------------------------------------------------
// ----------- function: calculate metrics -----------
// ---------------------------------------------------
performance_metrics calculate_metrics(const vector<trial_info> MC_runs, const unsigned int max_iter){
	performance_metrics metrics;
	if (MC_runs.size() ==0 ){
		return metrics;
	}

	// calculate means
	metrics.time_avg = 0;
	metrics.iter_avg = 0;
	metrics.time_per_iter_avg = 0;
	metrics.success_avg = 0;
	for (unsigned int mc = 0; mc < MC_runs.size(); mc++){
		if (MC_runs[mc].time == -1){
			continue;
		}
		if (MC_runs[mc].iterations < max_iter){
			metrics.time_avg += MC_runs[mc].time;
			metrics.iter_avg += MC_runs[mc].iterations;
			metrics.time_per_iter_avg += MC_runs[mc].time/MC_runs[mc].iterations;
		}
		metrics.success_avg += double(MC_runs[mc].iterations < max_iter);
	}
	metrics.time_avg    = metrics.time_avg/MC_runs.size();
	metrics.iter_avg    = metrics.iter_avg/MC_runs.size();
	metrics.success_avg = metrics.success_avg/MC_runs.size();
	metrics.time_per_iter_avg    = metrics.time_per_iter_avg/MC_runs.size();

	// calculate standard deviations
	metrics.time_std = 0;
	metrics.iter_std = 0;
	metrics.success_std = 0;
	metrics.time_per_iter_std = 0;
	for (unsigned int mc = 0; mc < MC_runs.size(); mc++){
		if (MC_runs[mc].time == -1){
			continue;
		}
		if (MC_runs[mc].iterations < max_iter){
			metrics.time_std += pow(MC_runs[mc].time,2);
			metrics.iter_std += pow(MC_runs[mc].iterations,2) ;
			metrics.time_per_iter_std += pow(MC_runs[mc].time/MC_runs[mc].iterations,2) ;
		}
		metrics.success_std += pow(double(MC_runs[mc].iterations < max_iter),2);
	}
	metrics.time_std    = metrics.time_std/MC_runs.size();
	metrics.iter_std    = metrics.iter_std/MC_runs.size();
	metrics.success_std = metrics.success_std/MC_runs.size();
	metrics.time_per_iter_std    = metrics.time_per_iter_std/MC_runs.size();

	metrics.time_std    -= pow(metrics.time_avg,2);
	metrics.iter_std    -= pow(metrics.iter_avg,2);
	metrics.success_std -= pow(metrics.success_avg,2);
	metrics.time_per_iter_std    -= pow(metrics.time_per_iter_avg,2);

	metrics.time_std = sqrt(metrics.time_std);
	metrics.iter_std = sqrt(metrics.iter_std);
	metrics.time_per_iter_std = sqrt(metrics.time_per_iter_std);
	metrics.success_std = sqrt(metrics.success_std);
	return metrics;
}

// ---------------------------------------------------
// ---------------------------------------------------
// ----------- function: run_mc_trials ---------------
// ---------------------------------------------------
vector<performance_metrics> run_mc_trials(const vector <string> alg_names, const unsigned int sig_dim, const unsigned int sparsity, const unsigned int meas_num, const unsigned int max_iter, const double gamma, const double tol , const vec prob_vec, simulation_parameters simulation_params, const int num_mc_runs, const int SEED){

	vector<performance_metrics> MC_metrics;

	simulation_parameters simulation_params_non_parallel = simulation_params;
	simulation_params_non_parallel.num_cores = 1;

	// performance metrics for different methods in MC trials
    vector <vector<trial_info>> alg_infos(alg_names.size());

	for (int mc = 0; mc < num_mc_runs; mc++){
		if (SEED == -1){
			arma_rng::set_seed_random();
			srand(time(NULL));
		}else{
			arma_rng::set_seed(SEED);
			srand(SEED);
		}

		// Generate sparse signal
		vec support(sig_dim,fill::zeros);
		support(span(0,sparsity-1)).ones();
		random_shuffle(support.begin(),support.end());

		const vec x = support;				// Bernoulli signal

		// Generate Measurement matrix
		const mat A =normalise( randn(meas_num,sig_dim) );

		// compute measurements
		const vec y = A*x;

		double time;
		unsigned int num_iters;

        for (unsigned int alg = 0; alg < alg_names.size(); alg++) {
            if(alg_names[alg].compare("Bayesian Sto_IHT") == 0){
                // Solve in Parallel with Bayesian update rules
                time = omp_get_wtime();
                const vec x_hat_bayesian = bayesian_Sto_IHT(A, y, sparsity, prob_vec, max_iter, gamma, tol, num_iters, simulation_params);
                alg_infos[alg].push_back(trial_info(omp_get_wtime() - time,num_iters ));
            }
            else if(alg_names[alg].compare("Sto_IHT") == 0){
                // solve with non-parallel StoIHT
                time = omp_get_wtime();
                const vec x_hat = parallel_Sto_IHT(A, y, sparsity, prob_vec, max_iter, gamma, tol, num_iters, simulation_params_non_parallel);
                alg_infos[alg].push_back(trial_info(omp_get_wtime() - time,num_iters ));
            }
            else if(alg_names[alg].compare("Parallel Sto_IHT") == 0){
                // Solve with Parallel StoIHT
                time = omp_get_wtime();
                const vec x_hat_parallel = parallel_Sto_IHT(A, y, sparsity, prob_vec, max_iter, gamma, tol, num_iters, simulation_params);
                alg_infos[alg].push_back(trial_info(omp_get_wtime() - time,num_iters ));
            }
            else if(alg_names[alg].compare("Tally Sto_IHT") == 0){
                // Solve in Parallel with tally score
                time = omp_get_wtime();
                const vec x_hat_tally = tally_Sto_IHT(A, y, sparsity, prob_vec, max_iter, gamma, tol, num_iters, simulation_params);
                alg_infos[alg].push_back(trial_info(omp_get_wtime() - time,num_iters ));
            }
            else if(alg_names[alg].compare("AMP") == 0){
                // Solve with AMP
                time = omp_get_wtime();
                const vec x_hat_AMP = AMP(A, y, sparsity, max_iter, tol, num_iters,simulation_params);
                alg_infos[alg].push_back(trial_info(omp_get_wtime() - time,num_iters ));
            }
            else if(alg_names[alg].compare("Parallel AMP") == 0){
                // Solve with R_MP_AMP
                time = omp_get_wtime();
                const vec x_hat_R_MP_AMP = R_MP_AMP(A, y, sparsity, max_iter, tol, num_iters,simulation_params);
                alg_infos[alg].push_back(trial_info(omp_get_wtime() - time,num_iters ));
            }
            else{
                cout << "Unknown Algorithm name: " << alg_names[alg] << endl << endl;
                throw;
            }

        }
	}

    for (unsigned int alg = 0; alg < alg_names.size(); alg++) {
        MC_metrics.push_back(calculate_metrics(alg_infos[alg],max_iter));
    }
	return MC_metrics;
}

// ---------------------------------------------------
// ---------------------------------------------------
// ----------- function: set_slow_cores ---------
// ---------------------------------------------------
// NOTE: Thread #0 is never selected to be slow
// Because it is already performing extra calculation to check the convergence criteria
// This way it is easier to interpret the results
void set_slow_cores(uvec &slow_cores, const simulation_parameters simulation_params){
	vec cores(simulation_params.num_cores-1,fill::zeros);
	// generate slow core indices
	slow_cores.reset();
	if (simulation_params.num_slow_cores > 0){
		cores.zeros();
		cores(span(0,simulation_params.num_slow_cores-1)).ones();
		random_shuffle(cores.begin(),cores.end());
		slow_cores = find(cores!=0) + 1;
	}
	return;
}

// ---------------------------------------------------
// ---------------------------------------------------
// ----------- function: save_results ----------------
// ---------------------------------------------------
void save_results(const vector<string> alg_names, const vector <vector<performance_metrics>> sweep_metrics,
	const string parameter_to_sweep, const vec parameter_to_sweep_values,
	const unsigned int sig_dim, const unsigned int sparsity, const unsigned int meas_num,
	const unsigned int max_iter, simulation_parameters simulation_params, const int num_mc_runs){
	// create directory
	string dir = "../results";
	mkdir(dir.c_str(),0777);

	ofstream ofs;
	ofs.open (dir + "/simulation_env.txt", std::ofstream::out);
	ofs << "# of Processors Available: \t" << omp_get_num_procs() << endl << endl;
	ofs << "# of MC trials: \t" << num_mc_runs << endl << endl;
	ofs << "Signal Dimension: \t" << sig_dim << endl<<endl;
	ofs << "Sparsity: \t\t" << sparsity << endl<<endl;
	ofs << "# of measurements: \t" << meas_num << endl<<endl;
	ofs << "Max. # of Iters: \t" << max_iter << endl<<endl;
	ofs << "Percentage of Slow Cores: \t" << simulation_params.slow_cores_ratio * 100;
	ofs << " (each sleeping for " << simulation_params.sleep_slow_cores/1000 << " ms )" << endl<<endl;
	ofs << "Parameter to Sweep: " << parameter_to_sweep << ':' << endl;
	ofs << parameter_to_sweep_values.t() << endl;

	ofs.close();

	char delimiter = '\t';
	for (unsigned int i = 0; i < alg_names.size(); i++){
		if (sweep_metrics[0][i].time_avg < 0){
			continue;
		}
		ofs.open (dir + "/" + alg_names[i] + ".txt", std::ofstream::out);
		ofs << parameter_to_sweep << "\t time_avg\ttime_std\tsuccess_avg\tsuccess_std\ttime_per_iter_avg\ttime_per_iter_std\titer_avg\titer_std" << endl;
		for (unsigned int param = 0; param <parameter_to_sweep_values.n_elem; param++ ){
			ofs << parameter_to_sweep_values[param]         << delimiter
                << 1000* sweep_metrics[param][i].time_avg 	<< delimiter
				<< 1000* sweep_metrics[param][i].time_std 	<< delimiter
				<< sweep_metrics[param][i].success_avg 		<< delimiter
				<< sweep_metrics[param][i].success_std 		<< delimiter
				<< 1000* sweep_metrics[param][i].time_per_iter_avg  << delimiter
				<< 1000* sweep_metrics[param][i].time_per_iter_std  << delimiter
				<< sweep_metrics[param][i].iter_avg  		<< delimiter
				<< sweep_metrics[param][i].iter_std 		<< delimiter
				<< endl ;

		}
		ofs.close();

	}
}

// ---------------------------------------------------
// ---------------------------------------------------
// ----------- function: print_results----------------
// ---------------------------------------------------
void print_results(const vector<string> alg_names, const vector <vector<performance_metrics>> sweep_metrics,
	const string parameter_to_sweep, const vec parameter_to_sweep_values,
	const unsigned int sig_dim, const unsigned int sparsity, const unsigned int meas_num,
	const unsigned int max_iter, simulation_parameters simulation_params, const int num_mc_runs){
	cout << setprecision(5);
	string delimiter = "\t\t";
	cout << endl;
    cout << parameter_to_sweep << delimiter << "time_avg" << delimiter << "success_avg"
        << delimiter << "time/iter_avg"  << delimiter<< "iter_avg" << endl;
	for (unsigned int i = 0; i < alg_names.size(); i++){
		if (sweep_metrics[0][i].time_avg < 0){
			continue;
		}
		cout << alg_names[i] << ":" << endl;
		for (unsigned int param = 0; param <parameter_to_sweep_values.n_elem; param++ ){
			cout<< parameter_to_sweep_values[param]         << delimiter
                << 1000* sweep_metrics[param][i].time_avg 	<< delimiter
				<< sweep_metrics[param][i].success_avg 		<< delimiter
				<< 1000* sweep_metrics[param][i].time_per_iter_avg  << delimiter
				<< sweep_metrics[param][i].iter_avg  		<< delimiter
				<< endl ;

		}
		cout << endl;

	}
	cout << endl;
	cout << "NOTE: Zero (if any) means the success rate of the corresponding algorithm is zero." << endl;
	cout << "(increase maxIter, decrease sparsity, increase MC trials, ....)" << endl;
}

// ---------------------------------------------------
// ---------------------------------------------------
// ----------- function: run_experiments--------------
// ---------------------------------------------------
void run_experiments(const vector <string> alg_names, unsigned int sig_dim, unsigned int sparsity, unsigned int meas_num, const unsigned int max_iter, const double gamma, const double tol , const vec prob_vec, simulation_parameters simulation_params,
	 const int num_mc_runs, const int SEED){
	cout << setprecision(3);
    string parameter_to_sweep = "Cores";
    cout << " Parameter to Sweep: " << parameter_to_sweep << ':' << endl;

    vec parameter_to_sweep_values = floor(linspace(1,10,5));

    vector <vector<performance_metrics>> sweep_metrics(parameter_to_sweep_values.n_elem);

    for (unsigned int param = 0; param <parameter_to_sweep_values.n_elem; param++ ){
        simulation_params.num_cores = int(parameter_to_sweep_values[param]);
        simulation_params.num_slow_cores = int(simulation_params.slow_cores_ratio * simulation_params.num_cores);
        cout << simulation_params.num_cores << '\t' << flush;
        sweep_metrics[param] =  run_mc_trials(alg_names, sig_dim, sparsity, meas_num, max_iter, gamma, tol , prob_vec, simulation_params,  num_mc_runs,  SEED);
    }
    cout << endl;
    // Print/Save
    print_results(alg_names, sweep_metrics, parameter_to_sweep, parameter_to_sweep_values, sig_dim, sparsity,  meas_num, max_iter, simulation_params, num_mc_runs);
    save_results(alg_names, sweep_metrics, parameter_to_sweep, parameter_to_sweep_values, sig_dim, sparsity,  meas_num, max_iter, simulation_params, num_mc_runs);

	return;
}


/**
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Tests for the MassKernels Operations
 */

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

#include <stdexcept>
#include <string>

#include "MassKernels.h"
#include "MeshConfig.h"
#include "MeshSourceStructGenConfig.h"
#include "CupCfdAoSMesh.h"
#include "EuclideanVector.h"

#include "PartitionerNaiveConfig.h"
#include "PartitionerConfig.h"

#include <cstdlib>

#include <iostream>

using namespace cupcfd::fvm;

namespace utf = boost::unit_test;
namespace euc = cupcfd::geometry::euclidean;
namespace meshgeo = cupcfd::geometry::mesh;

// Setup
BOOST_AUTO_TEST_CASE(setup)
{
    int argc = boost::unit_test::framework::master_test_suite().argc;
    char ** argv = boost::unit_test::framework::master_test_suite().argv;

    MPI_Init(&argc, &argv);
}

// === FluxMassDolfynFaceLoop ===
// Test 1: Test it runs without error on a small mesh
BOOST_AUTO_TEST_CASE(FluxMassDolfynFaceLoop_test1)
{
	cupcfd::error::eCodes status;
    cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// Create a small test mesh
    cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;

	// Setup the source config
    meshgeo::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0);

	// Setup the config to use for building
    meshgeo::MeshConfig<int, double,int> meshConfig(partConfig, meshSourceConfig);

    meshgeo::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Setup
	int nCells = mesh->properties.lTCells;
	// int nOwnedCells = mesh->properties.lOCells;
	// int nGhostCells = mesh->properties.lGhCells;
	int nBnds = mesh->properties.lBoundaries;
	int nFaces = mesh->properties.lFaces;
	// int nRegions = mesh->properties.lRegions;

	cupcfd::geometry::euclidean::EuclideanVector<double,3> * dudx =
			(cupcfd::geometry::euclidean::EuclideanVector<double,3> *) malloc(sizeof(cupcfd::geometry::euclidean::EuclideanVector<double,3>) * nCells);

	cupcfd::geometry::euclidean::EuclideanVector<double,3> * dvdx =
			(cupcfd::geometry::euclidean::EuclideanVector<double,3> *) malloc(sizeof(cupcfd::geometry::euclidean::EuclideanVector<double,3>) * nCells);

	cupcfd::geometry::euclidean::EuclideanVector<double,3> * dwdx =
			(cupcfd::geometry::euclidean::EuclideanVector<double,3> *) malloc(sizeof(cupcfd::geometry::euclidean::EuclideanVector<double,3>) * nCells);

	cupcfd::geometry::euclidean::EuclideanVector<double,3> * dpdx =
			(cupcfd::geometry::euclidean::EuclideanVector<double,3> *) malloc(sizeof(cupcfd::geometry::euclidean::EuclideanVector<double,3>) * nCells);

	double * denCell = (double *) malloc(sizeof(double) * nCells);
	double * denBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * uCell = (double *) malloc(sizeof(double) * nCells);
	double * vCell = (double *) malloc(sizeof(double) * nCells);
	double * wCell = (double *) malloc(sizeof(double) * nCells);
	double * massFlux = (double *) malloc(sizeof(double) * nFaces);
	double * p = (double *) malloc(sizeof(double) * nCells);
	double * ar = (double *) malloc(sizeof(double) * nCells);
	double * su = (double *) malloc(sizeof(double) * nCells);
	double * rface = (double *) malloc(sizeof(double) * nFaces * 2);

	double small = 1E-18;
	int icinl;
	int icout;
	int icsym;
	int icwal;
	bool solveTurbEnergy = false;
	bool solveTurbDiss = false;
	bool solveVisc = true;
	bool solveEnthalpy = false;

	double * teCell = (double *) malloc(sizeof(double) * nCells);
	double * teBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * edCell = (double *) malloc(sizeof(double) * nCells);
	double * edBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * viseffCell = (double *) malloc(sizeof(double) * nCells);
	double * viseffBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * tCell = (double *) malloc(sizeof(double) * nCells);
	double * tBoundary = (double *) malloc(sizeof(double) * nBnds);

	status = FluxMassDolfynFaceLoop(*mesh,
			dudx, nCells,
			dvdx, nCells,
			dwdx, nCells,
			dpdx, nCells,
			denCell, nCells,
			denBoundary, nBnds,
			uCell, nCells,
			vCell, nCells,
			wCell, nCells,
			massFlux, nFaces,
			p, nCells,
			ar, nCells,
			su, nCells,
			rface, nFaces * 2,
			small, &icinl, &icout, &icsym, &icwal,
			solveTurbEnergy, solveTurbDiss, solveVisc, solveEnthalpy,
			teCell, nCells,
			teBoundary, nBnds,
			edCell, nCells,
			edBoundary, nBnds,
			viseffCell, nCells,
			viseffBoundary, nBnds,
			tCell, nCells,
			tBoundary, nBnds);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	free(dudx);
	free(dvdx);
	free(dwdx);
	free(dpdx);
	free(denCell);
	free(denBoundary);
	free(uCell);
	free(vCell);
	free(wCell);
	free(massFlux);
	free(p);
	free(ar);
	free(su);
	free(rface);
	delete(mesh);
}

BOOST_AUTO_TEST_CASE(cleanup)
{
    MPI_Finalize();
}


#include "Day04-SecureContainer.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/optional.hpp>

#include <vector>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2019
{
namespace Day04
{

std::vector<int> convertIntToDigitVector(int number)
{
    std::vector<int> digits;

    while (number > 0)
    {
        digits.push_back(number % 10);
        number /= 10;
    }

    std::reverse(digits.begin(), digits.end());

    return digits;
}

bool areDigitsIncreasing(const std::vector<int>& digits)
{
    boost::optional<int> previousDigit{};

    for (const auto digit : digits)
    {
        if (digit < previousDigit)
        {
            return false;
        }

        previousDigit = digit;
    }

    return true;
}

bool hasTwoAdjacentEqualDigits(const std::vector<int>& digits)
{
    boost::optional<int> previousDigit{};

    for (const auto digit : digits)
    {
        if (digit == previousDigit)
        {
            return true;
        }

        previousDigit = digit;
    }

    return false;
}

bool hasTwoAdjacentEqualDigitsNotPartOfLargerGroup(const std::vector<int>& digits)
{
    for (size_t i = 0; i < digits.size() - 1; ++i)
    {
        size_t equalGroupEndIndex = i;
        for (; equalGroupEndIndex < digits.size() - 1 && digits.at(equalGroupEndIndex) == digits.at(equalGroupEndIndex + 1); ++equalGroupEndIndex)
        {
        }

        if (equalGroupEndIndex == i + 1)
        {
            return true;
        }

        i = equalGroupEndIndex;
    }

    return false;
}

unsigned numDifferentValidPasswords(int lowerBound, int upperBound, std::function<bool(std::vector<int>)> validatorFunc)
{
    unsigned numValidPasswords = 0;

    for (int candidate = lowerBound; candidate <= upperBound; ++candidate)
    {
        std::vector<int> digits = convertIntToDigitVector(candidate);

        if (validatorFunc(digits))
        {
            ++numValidPasswords;
        }
    }

    return numValidPasswords;
}

unsigned numDifferentValidPasswordsSimple(int lowerBound, int upperBound)
{
    return numDifferentValidPasswords(lowerBound, upperBound,
                                      [](const auto& digits)
                                      {
                                          return areDigitsIncreasing(digits) && hasTwoAdjacentEqualDigits(digits);
                                      });
}

unsigned numDifferentValidPasswordsLargerGroupIsNotMatching(int lowerBound, int upperBound)
{
    return numDifferentValidPasswords(lowerBound, upperBound,
                                      [](const auto& digits)
                                      {
                                          return areDigitsIncreasing(digits) && hasTwoAdjacentEqualDigitsNotPartOfLargerGroup(digits);
                                      });
}

}
}
}


#include "Vertex.h"
#include <iostream>
#include <string>
#include <armadillo>
#pragma once

using namespace std;

Vertex::Vertex(double vx, double vy, double vz) {
	x = vx;
	y = vy;
	z = vz;
	Vertice = {vx,vy,vz,1.0f};
}

Vertex::Vertex() {

}
double Vertex::getX() {
	return Vertice[0];
}

double Vertex::getY() {
	return Vertice[1];
}

double Vertex::getZ() {
	return Vertice[2];
}

void Vertex::SetVertice(arma::dvec V) {
	Vertice = { V[0], V[1], V[2] , 1.0 };
	x = Vertice[0];
	y = Vertice[1];
	z = Vertice[2];
}
Vertex::~Vertex()
{
}

arma::drowvec Vertex::get_value() {
	arma::drowvec Aux = {Vertice[0],Vertice[1],Vertice[2] };
	return Aux;
}
void Vertex::ToString()
{
	cout << "x: " + std::to_string(Vertice[0]) + " y: " + std::to_string(Vertice[1]) + " z: " + std::to_string(Vertice[2]) << endl;
}


// Copyright (C) 2007-2012 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Nick Edmonds
//           Douglas Gregor
//           Andrew Lumsdaine

#include <am++/am++.hpp>
#include <am++/mpi_transport.hpp>

#include <boost/graph/use_mpi.hpp>
#include <boost/config.hpp>
#include <boost/throw_exception.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/test/minimal.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/lexical_cast.hpp>
#include <ctime>

#define IS_MPI_TRANSPORT 1

using namespace boost;

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

int test_main(int argc, char** argv)
{
  amplusplus::environment env = amplusplus::mpi_environment(argc, argv);
  amplusplus::transport trans = env.create_transport();

  trans.set_nthreads(1);

  int n = 10000;
  double p = 3e-3;
  int seed = std::time(0);

  if (argc > 1) n = lexical_cast<int>(argv[1]);
  if (argc > 2) p = lexical_cast<double>(argv[2]);
  if (argc > 3) seed = lexical_cast<int>(argv[3]);

  typedef adjacency_list<vecS,
                         distributedS<vecS>,
                         directedS> Graph;

  int rank = trans.rank();
  int numprocs = trans.size();
  bool i_am_root = rank == 0;

  // broadcast the seed
//   boost::mpi::broadcast(world, seed, 0);
  { amplusplus::scoped_epoch epoch(trans); }

  // Random number generator
  minstd_rand gen;

  if (i_am_root) {
    std::cout << "n = " << n << ", p = " << p << ", seed = " << seed
              << "\nBuilding graph with the iterator constructor... ";
    std::cout.flush();
  }

  parallel::variant_distribution<> distrib
    = parallel::block<graph_traits<Graph>::vertices_size_type>(trans, n);

  // Build a graph using the iterator constructor, where each of the
  // processors has exactly the same information.
  gen.seed(seed);
  Graph g1(erdos_renyi_iterator<minstd_rand, Graph>(gen, n, p),
           erdos_renyi_iterator<minstd_rand, Graph>(),
           n, trans, distrib);

  { amplusplus::scoped_epoch epoch(trans); }

  sleep(5);

  // Build another, identical graph using add_edge
  if (i_am_root) {
    std::cout << "done.\nBuilding graph with add_edge from the root...";
    std::cout.flush();
  }

  gen.seed(seed);
  Graph g2(n, trans, distrib);

  {
    amplusplus::scoped_epoch epoch(trans);

    if (i_am_root) {
      // The root will add all of the edges, some percentage of which
      // will require an immediate response from the owner of the edge.
      for (erdos_renyi_iterator<minstd_rand, Graph> first(gen, n, p), last;
	   first != last; ++first) {
	add_edge(vertex(first->first, g2), vertex(first->second, g2), g2);

	std::cout << rank << ": add_edge " << vertex(first->first, g2).local << "@" << vertex(first->first, g2).owner
		  << " -> " << vertex(first->second, g2).local << "@" << vertex(first->second, g2).owner << std::endl;
      }
    }
  }

  if (i_am_root) {
    std::cout << "synchronizing...";
    std::cout.flush();
  }

  // Verify that the two graphs are indeed identical.
  if (i_am_root) {
    std::cout << "done.\nVerifying graphs...";
    std::cout.flush();
  }

  // Check the number of vertices
  if (num_vertices(g1) != num_vertices(g2)) {
    std::cerr << g1.processor() << ": g1 has " << num_vertices(g1)
              << " vertices, g2 has " << num_vertices(g2) << " vertices.\n";
    exit(-1);
  }

  // Check the number of edges
  if (num_edges(g1) != num_edges(g2)) {
    std::cerr << g1.processor() << ": g1 has " << num_edges(g1)
              << " edges, g2 has " << num_edges(g2) << " edges.\n";
    exit(-1);
  }

  // Check the in-degree and out-degree of each vertex
  graph_traits<Graph>::vertex_iterator vfirst1, vlast1, vfirst2, vlast2;
  tie(vfirst1, vlast1) = vertices(g1);
  tie(vfirst2, vlast2) = vertices(g2);
  for(; vfirst1 != vlast1 && vfirst2 != vlast2; ++vfirst1, ++vfirst2) {
    if (out_degree(*vfirst1, g1) != out_degree(*vfirst2, g2)) {
      std::cerr << g1.processor() << ": out-degree mismatch ("
                << out_degree(*vfirst1, g1) << " vs. "
                << out_degree(*vfirst2, g2) << ").\n";
      exit(-1);
    }
  }

  { amplusplus::scoped_epoch epoch(trans); }

  // TODO: Check the actual edge targets

  // Build another, identical graph using add_edge
  if (i_am_root) {
    std::cout << "done.\nBuilding graph with add_edge from everywhere...";
    std::cout.flush();
  }

  gen.seed(seed);
  Graph g3(n, trans, distrib);
  {
    // Each processor will take a chunk of incoming edges and add
    // them. Some percentage of the edge additions will require an
    // immediate response from the owner of the edge. This should
    // create a lot of traffic when building the graph, but should
    // produce a graph identical to the other graphs.
    typedef graph_traits<Graph>::edges_size_type edges_size_type;

    erdos_renyi_iterator<minstd_rand, Graph> first(gen, n, p);
    edges_size_type chunk_size = edges_size_type(p*n*n)/numprocs;
    edges_size_type start = chunk_size * rank;
    edges_size_type remaining_edges =
      (rank < numprocs - 1? chunk_size
       : edges_size_type(p*n*n) - start);

    // Spin the generator to the first edge we're responsible for
    for (; start; ++first, --start)
      ;

    {
      amplusplus::scoped_epoch epoch(trans);

      for (; remaining_edges; --remaining_edges, ++first) {
	add_edge(vertex(first->first, g3), vertex(first->second, g3), g3);
      }
    }
  }

  if (i_am_root) {
    std::cout << "synchronizing...";
    std::cout.flush();
  }

  // Verify that the two graphs are indeed identical.
  if (i_am_root) {
    std::cout << "done.\nVerifying graphs...";
    std::cout.flush();
  }

  // Check the number of vertices
  if (num_vertices(g1) != num_vertices(g3)) {
    std::cerr << g1.processor() << ": g1 has " << num_vertices(g1)
              << " vertices, g3 has " << num_vertices(g3) << " vertices.\n";
    exit(-1);
  }

  // Check the number of edges
  if (num_edges(g1) != num_edges(g3)) {
    std::cerr << g1.processor() << ": g1 has " << num_edges(g1)
              << " edges, g3 has " << num_edges(g3) << " edges.\n";
    exit(-1);
  }

  // Check the in-degree and out-degree of each vertex
  tie(vfirst1, vlast1) = vertices(g1);
  tie(vfirst2, vlast2) = vertices(g3);
  for(; vfirst1 != vlast1 && vfirst2 != vlast2; ++vfirst1, ++vfirst2) {
    if (out_degree(*vfirst1, g1) != out_degree(*vfirst2, g3)) {
      std::cerr << g1.processor() << ": out-degree mismatch ("
                << out_degree(*vfirst1, g1) << " vs. "
                << out_degree(*vfirst2, g3) << ").\n";
      exit(-1);
    }
  }

  // TODO: Check the actual edge targets

  if (i_am_root) {
    std::cout << "done.\n";
    std::cout.flush();
  }

  return 0;
}