#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <time.h>
#include <math.h>
#include <dirent.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <iomanip>
#include <limits>
#include <string>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/math/distributions/normal.hpp>

#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/flann/flann.hpp"

#include "FastEMD/emd_hat.hpp"
#include "FastEMD/emd_hat_signatures_interface.hpp"
#include "RubnerEMD/emd.hpp"
#include "FastEMD/tictoc.hpp"

using namespace cv;
using namespace std;

namespace fs = boost::filesystem;

// TODO: config
/* CONFIGURATIONS */
bool use_fast_emd = 1;
float gd_dist = 350;
/* END CONFIGURATIONS */

void help();
void print_progress(time_t start, int total, int& completed, int per_cout);
void print_keypoints(const vector<KeyPoint>& kp);

float calc_emd(int first, int second);
double calc_fast_emd(int first, int second);
float calc_scalar_manitude(const Mat& desc, int index);
void calc_contrast(const vector<KeyPoint>& kp);
void calc_dist_mat(const Mat& desc_1, const Mat& desc_2, bool gd = false);
float euclid_dist(const float *v1, const float *v2, int dim1, int dim2);
float emd_dist(feature_t *f1, feature_t *f2);
double fast_emd_dist(feature_tt *f1, feature_tt *f2);

Mat dist_mat;
vector<Mat> img_descriptors;
vector<vector<KeyPoint> > img_keypoints;
vector<string> img_paths;

int main(int argc, char *argv[]) {
	if (argc < 4) {
		help();
		return -1;
	}

	const char *index_path = argv[1];
	const char *queries_path = argv[2];
	const char *out_path = argv[3];
	if (argc > 5)
		gd_dist = atof(argv[5]);

	cout << "Reading queries from " << queries_path << endl;
	vector<string> query_paths;
	string query_path;
	int query_count;
	ifstream queries_file(queries_path);
	queries_file >> query_count;
	for (int i = 0; i < query_count; ++i) {
		queries_file >> query_path;
		query_paths.push_back(query_path);
	}

	cout << "Reading index from " << index_path << endl;
	FileStorage fs(index_path, FileStorage::READ);
	FileNode imgs = fs["index"];
	FileNodeIterator it;
	int img_count = 0;
	vector<pair<int, int> > query_ids;
	for (it = imgs.begin(); it != imgs.end(); ++it, ++img_count) {
		string img_path;
		vector<KeyPoint> img_keypoint;
		Mat img_descriptor;

		(*it)["path"] >> img_path;
		read((*it)["keypoints"], img_keypoint);
		(*it)["descriptors"] >> img_descriptor;

		img_paths.push_back(img_path);
		img_keypoints.push_back(img_keypoint);
		img_descriptors.push_back(img_descriptor);

		for (int i = 0; i < query_count; ++i) {
			if (img_path == query_paths[i])
				query_ids.push_back(pair<int, int>(img_count, i));
		}
	}

	query_count = query_ids.size();

	cout << "Calcing emds..." << endl;

	int total_process = query_count * img_count;
	int completed_process = 0;
	time_t start = time(0);

	string img_path_1, img_path_2;
	int query_id;

	ofstream ofile(out_path);

	tictoc timer;
	timer.tic();
	// calculate emd distances of each pair of images
	for (int i = 0; i < query_count; ++i) {
		query_id = query_ids[i].first;
		img_path_1 = query_paths[query_ids[i].second];

		for (int j = 0; j < img_count; ++j) {
			if (query_id == j)
				continue;

			img_path_2 = img_paths[j];

			double emd_dist;
			if (use_fast_emd)
				emd_dist = calc_fast_emd(query_id, j);
			else
				emd_dist = calc_emd(query_id, j);

			// cout << setprecision(3) << fixed;
			// timer.clear();
			// timer.tic();
			// emd_dist = calc_fast_emd(i, j);
			// timer.toc();
			// cout << "fast: " << timer.totalTimeSec() << endl;
			// cout << img_path_1 << " " << img_path_2 << " " << emd_dist << endl;

			// timer.clear();
			// timer.tic();
			// emd_dist = calc_emd(i, j);
			// timer.toc();
			// cout << "rubner: " << timer.totalTimeSec() << endl;
			// cout << img_path_1 << " " << img_path_2 << " " << emd_dist << endl;

			ofile << setprecision(3) << fixed;
			ofile << img_path_1 << " " << img_path_2 << " " << emd_dist << endl;

			// print_progress(start, total_process, completed_process, 20);
		}
	}
	timer.toc();
	cout << "Time in seconds: " << timer.totalTimeSec() << endl;

	ofile.close();

	return 0;
}

void help() {
	cout << "Usage: ./feature-selection <index_path> <queries_path> <out_path> <fast_thresh=350>" << endl;
}

void print_progress(time_t start, int total, int& completed, int per_count) {
	++completed;
	if (completed % per_count != 0)
		return;
	int remaining_process = total - completed;
	time_t current = time(0);
	float elapsed_time = difftime(current, start);
	float remaining_time = elapsed_time * remaining_process / completed;
	cout << fixed << setprecision(3);
	cout << "Remaining time: ";
	cout << remaining_time / 60 << "min" << endl;
}

void print_keypoints(const vector<KeyPoint>& kp) {
	KeyPoint k;
	for (int i = 0; i < (int) kp.size(); ++i) {
		k = kp[i];
		cout << "(" << k.pt.x << "," << k.pt.y << "): " << k.response << ", " << k.size << endl;
	}
}

float calc_emd(int first, int second) {
	vector<KeyPoint> *keypoints_1, *keypoints_2;
	Mat *desc_1, *desc_2;

	keypoints_1 = &img_keypoints[first];
	desc_1 = &img_descriptors[first];

	keypoints_2 = &img_keypoints[second];
	desc_2 = &img_descriptors[second];

	calc_dist_mat(*desc_1, *desc_2);

	int feature_count_1 = keypoints_1->size();
	int feature_count_2 = keypoints_2->size();

	signature_t signature_1;
	signature_t signature_2;

	signature_1.n = feature_count_1;
	signature_2.n = feature_count_2;

	signature_1.Features = new feature_t[feature_count_1];
	signature_2.Features = new feature_t[feature_count_2];

	signature_1.Weights = new float[feature_count_1];
	signature_2.Weights = new float[feature_count_2];

	for (int i = 0; i < feature_count_1; ++i) {
		signature_1.Weights[i] = calc_scalar_manitude(*desc_1, i);
		signature_1.Features[i] = i;
	}

	for (int i = 0; i < feature_count_2; ++i) {
		signature_2.Weights[i] = calc_scalar_manitude(*desc_2, i);
		signature_2.Features[i] = i;
	}

	float dist = emd(&signature_1, &signature_2, emd_dist, 0, 0);

	delete[] signature_1.Features;
	delete[] signature_1.Weights;
	delete[] signature_2.Features;
	delete[] signature_2.Weights;

	// cout << "EMD: " << dist << endl;

	return dist;
}

double calc_fast_emd(int first, int second) {
	vector<KeyPoint> *keypoints_1, *keypoints_2;
	Mat *desc_1, *desc_2;

	keypoints_1 = &img_keypoints[first];
	desc_1 = &img_descriptors[first];

	keypoints_2 = &img_keypoints[second];
	desc_2 = &img_descriptors[second];

	calc_dist_mat(*desc_1, *desc_2, true);

	int feature_count_1 = keypoints_1->size();
	int feature_count_2 = keypoints_2->size();

	signature_tt<double> signature_1;
	signature_tt<double> signature_2;

	signature_1.n = feature_count_1;
	signature_2.n = feature_count_2;

	signature_1.Features = new feature_tt[feature_count_1];
	signature_2.Features = new feature_tt[feature_count_2];

	signature_1.Weights = new double[feature_count_1];
	signature_2.Weights = new double[feature_count_2];

	for (int i = 0; i < feature_count_1; ++i) {
		signature_1.Weights[i] = calc_scalar_manitude(*desc_1, i);
		signature_1.Features[i] = i;
	}

	for (int i = 0; i < feature_count_2; ++i) {
		signature_2.Weights[i] = calc_scalar_manitude(*desc_2, i);
		signature_2.Features[i] = i;
	}

	double dist = emd_hat_signature_interface<double>(&signature_1, &signature_2, fast_emd_dist, -1);

	delete[] signature_1.Features;
	delete[] signature_1.Weights;
	delete[] signature_2.Features;
	delete[] signature_2.Weights;

	// cout << "EMD: " << dist << endl;

	return dist / 1000;
}

float calc_scalar_manitude(const Mat& desc, int index) {
	const float* d = desc.ptr<float>(index);
	int cols = desc.cols;
	float magnitude = 0;
	for (int i = 0; i < cols; ++i)
		magnitude += d[i] * d[i];
	return sqrt(magnitude);
}

void calc_dist_mat(const Mat& desc_1, const Mat& desc_2, bool gd) {
	float dist;
	int rows = desc_1.rows;
	int cols = desc_2.rows;
	int dim1 = desc_1.cols;
	int dim2 = desc_2.cols;
	dist_mat = Mat::zeros(rows, cols, CV_32F);
	for (int i = 0; i < rows; ++i) {
		float *row = dist_mat.ptr<float>(i);
		const float *v1 = desc_1.ptr<float>(i);
		for (int j = 0; j < cols; ++j) {
			const float *v2 = desc_2.ptr<float>(j);
			dist = euclid_dist(v1, v2, dim1, dim2);
			if (gd)
				row[j] = gd_dist < dist ? gd_dist : dist;
			else
				row[j] = dist;
		}
	}
}

float euclid_dist(const float *v1, const float *v2, int dim1, int dim2) {
	int max_dim = max(dim1, dim2);
	float d1, d2;
	float dist = 0;
	for (int i = 0; i < max_dim; ++i) {
		if (i < dim1)
			d1 = v1[i];
		else
			d1 = 0;

		if (i < dim2)
			d2 = v2[i];
		else
			d2 = 0;

		dist += pow(d1 - d2, 2);
	}
	return sqrt(dist);
}

float emd_dist(feature_t *f1, feature_t *f2) {
	float dist = dist_mat.at<float>(*f1, *f2);
	return dist;
}

double fast_emd_dist(feature_tt *f1, feature_tt *f2) {
	double dist = dist_mat.at<float>(*f1, *f2);
	return dist;
}


#include <iostream>
#include <armadillo>
#include <vector>
#include <string>
#include "fringe_topofit.hpp"

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

    std::vector<std::string> files(4);
    files[0] = "test1.dat";
    files[1] = "test2.dat";
    files[2] = "test3.dat";

    std::complex<float> cJ(0.0, 1.0);

    for(int ii=0; ii<3; ii++)
    {
        arma::fvec alldata;
        alldata.load(files[ii], arma::raw_binary);

        int nifg = (alldata.size() - 4)/2;
        arma::fvec bperp(alldata.memptr() + 4, nifg, false);
        arma::fvec ph(alldata.memptr()+nifg+4, nifg, false);

        arma::cx_fvec cph(nifg);
        arma::cx_fvec resid(nifg);

        for(int kk=0; kk<nifg; kk++)
            cph[kk] = std::exp(cJ * ph[kk]);

        float wvl = alldata[0];
        float rng = alldata[1];
        float inc = alldata[2];
        float delz = alldata[3];

        float Kmod, Cph;

        topofit worker(20.0, 0.1, wvl, nifg);

        float coh = worker.fit(cph.memptr(), bperp.memptr(), rng, inc, resid.memptr(), Kmod, Cph);
        std::cout << "File: " << files[ii] << " K: " << Kmod
                << " C: " << Cph << " coh: " << coh
                << " K_true: " << delz/(wvl*rng*sin(inc*M_PI/180.0)/4.0/M_PI)<< "\n";
    }
}


#include "Tensor.h"
#include "AnisGeodesic.h"
#include <iostream>

#include <Eigen/Dense>
using namespace Eigen;

using namespace GeoProperty;

void Tensor::makeTensor(const Vec3& d1,const Vec3& d2,Tensor &t)
{
	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for (unsigned i = 0; i < 2; i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			hesMat(i, j) = t.mat[i][j];
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;

	t.mag1 = abs(egval[ind1]); t.mag2 = abs(egval[ind2]);
	t.dir1 = egvec(ind1, 0) * d1 + egvec(ind1, 1) * d2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * d1 + egvec(ind2, 1) * d2; t.dir2.normalize();
}

void Tensor::averageTensor(Tensor &t1,Tensor &t2,Tensor &t)
{
	double rat1= 0.5;
	double rat2= 0.5;

	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;

	Vec2 v1_,v2_;
	v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
	v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);

	t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
	t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
	t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for (unsigned i = 0; i < 2; i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			hesMat(i, j) = rat1*t1.mat[i][j] + rat2*t2.mat[i][j];
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if (abs(egval[0]) > abs(egval[1])) //////////////////////////////
		swap(ind1, ind2);

	double nl = (abs(t1.mag1) + abs(t1.mag2) + abs(t2.mag1) + abs(t2.mag2));
	if (nl > 1e-10)
		nl /= (2.0*(abs(egval[ind1]) + abs(egval[ind2])));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::averageTensor(std::vector<Tensor> &ts,Tensor &t, double rat)
{
	std::vector<double> rats(ts.size(),rat/double(ts.size()-1)); rats[0]=1-rat;
	Tensor& t1=ts.front();
	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;
	if(ts.size()==1) {t=t1; return;}

	Vec2 v1_,v2_;
	for(unsigned i=1;i<ts.size();i++)
	{
		Tensor& t2=ts[i];
		v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
		v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);
		t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
		t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
		t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;
	}

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for(unsigned i=0;i<2;i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			for (unsigned k = 0; k < ts.size(); k++)
			{
				hesMat(i, j) += rats[k] * ts[k].mat[i][j];
			}
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if (abs(egval[0]) < abs(egval[1]))
		swap(ind1, ind2);

	double nl = 0;
	for (unsigned i = 0; i<ts.size(); i++)
		nl += rats[i] * abs(ts[i].mag1) + abs(ts[i].mag2);
	if (nl>1e-10)
		nl /= ts.size()*(abs(egval[ind1]) + abs(egval[ind2]));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::averageTensor(Tensor &t1,Tensor &t2,Tensor &t, double rat)
{
	double rat1= 1-rat;

	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;

	Vec2 v1_,v2_;
	v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
	v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);

	t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
	t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
	t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for(unsigned i=0;i<2;i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			hesMat(i, j) = rat1*t1.mat[i][j] + rat*t2.mat[i][j];
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if ((abs(egval[0]) < abs(egval[1])) && (t1.mag1 >= t1.mag2) || (abs(egval[0]) > abs(egval[1])) && (t1.mag1<t1.mag2))
		swap(ind1, ind2);

	double nl = (abs(t1.mag1) + abs(t1.mag2) + abs(t2.mag1) + abs(t2.mag2));
	if (nl>1e-10)
		nl /= (2.0*(abs(egval[ind1]) + abs(egval[ind2])));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::averageTensor(std::vector<Tensor> &ts,Tensor &t)
{
	if (ts.empty()) return;

	Tensor& t1=ts.front();
	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;

	if(ts.size()==1)
	{
		t=t1; return;
	}

	for(unsigned i=1;i<ts.size();i++)
	{
		Tensor& t2=ts[i];

		Vec2 v1(t2.dir1.dot(t1.dir1), t2.dir1.dot(t1.dir2));
		Vec2 v2(t2.dir2.dot(t1.dir1), t2.dir2.dot(t1.dir2));

		t2.mat[0][0] = t2.mag1*v1.x*v1.x + t2.mag2*v2.x*v2.x;
		t2.mat[0][1] = t2.mat[1][0] = t2.mag1*v1.x*v1.y + t2.mag2*v2.x*v2.y;
		t2.mat[1][1] = t2.mag1*v1.y*v1.y + t2.mag2*v2.y*v2.y;
	}

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for (unsigned k = 0; k < ts.size(); k++)
	{
		hesMat(0, 0) += ts[k].mat[0][0];
		hesMat(0, 1) += ts[k].mat[0][1];
		hesMat(1, 0) += ts[k].mat[1][0];
		hesMat(1, 1) += ts[k].mat[1][1];
	}
	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if (abs(egval[0]) < abs(egval[1]))
		swap(ind1, ind2);

	double nl = 0;
	for (unsigned i = 0; i<ts.size(); i++)
		nl += abs(ts[i].mag1) + abs(ts[i].mag2);
	if (nl>1e-10)
		nl /= ts.size()*(abs(egval[ind1]) + abs(egval[ind2]));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::computeTriangleCurvature(MyMesh* orgMesh,std::vector<Tensor>& faceTensor)
{
	/*
	int fNum = orgMesh->getFaces().size();
	faceTensor.clear(); faceTensor.resize(fNum);
	const auto& fts = orgMesh->getFIter();
#pragma omp parallel for
	for(int i=0;i<fNum;i++){
		auto f_it = fts[i];
		MyMesh::VertexIter v[3]={f_it->vertex_iter(0),f_it->vertex_iter(1),f_it->vertex_iter(2)};

		// local coord system for single triangle;
		Vec3 axix = v[1]->coordinate()- v[0]->coordinate(); axix.normalize();
		Vec3 axiy = v[2]->coordinate()- v[0]->coordinate();
		Vec3 norm = axix.cross(axiy); norm.normalize();
		axiy = norm.cross(axix); axiy.normalize();

		std::vector<Tensor> ts(3);
		for(unsigned i=0;i<3;i++){
			ts[i].dir1 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(0));
			ts[i].dir2 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(1));
			ts[i].mag1 = v[0]->magnitude(0);
			ts[i].mag2 = v[0]->magnitude(1);
		}

		Tensor t;
		averageTensor(ts,t);
		faceTensor[i] =t;
/*
	//second method;  which seems wrong, but whose result is close to method 1;
		double anis[2];
		Jacobi jcb(2);
		for(unsigned i=0;i<2;i++){
			std::vector<std::vector<double> > mat(2,std::vector<double>(2,0));
			for(unsigned j=0;j<3;j++){
				Vec3 projVec = v_its[j]->direction(i); projVec.normalize();
				projVec -= projVec.dot(norm) * norm; projVec.normalize();

				double x = v_its[j]->direction(i).dot(u);
				double y = v_its[j]->direction(i).dot(v);

				mat[0][0] += v_its[j]->magnitude(i)*x*x;
				mat[0][1] += v_its[j]->magnitude(i)*x*y;
				mat[1][1] += v_its[j]->magnitude(i)*y*y;
			}
			mat[1][0] = mat[0][1];

			jcb.setMatrix(mat);
			jcb.run();

			std::vector<double> egvalue = jcb.getEigenvalues();

			std::vector<std::vector<double> > egvector = jcb.getEigenvectors();
			unsigned ind=0;
			if(egvalue[0]<egvalue[1]) ind=1 ;
			Vec3 td = u*egvector[ind][0] + v*egvector[ind][1]; td.normalize();

			anis[i]= *std::max_element(egvalue.begin(),egvalue.end());
		}
		faceAnis.push_back(abs(anis[1]-anis[0]));
*/
//	}

	faceTensor.clear();
	Tensor t;
	for(auto f_it = orgMesh->getFaces().begin(); f_it!=orgMesh->getFaces().end(); f_it++)
	{
		MyMesh::VertexIter v[3]={f_it->vertex_iter(0),f_it->vertex_iter(1),f_it->vertex_iter(2)};

		// local coord system for single triangle;
		Vec3 axix = v[1]->coordinate()- v[0]->coordinate(); axix.normalize();
		Vec3 axiy = v[2]->coordinate()- v[0]->coordinate();
		Vec3 norm = axix.cross(axiy); norm.normalize();
		axiy = norm.cross(axix); axiy.normalize();

		std::vector<Tensor> ts(3);
		for(unsigned i=0;i<3;i++)
		{
			ts[i].dir1 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(0));
			ts[i].dir2 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(1));
			ts[i].mag1 = v[i]->magnitude(0);
			ts[i].mag2 = v[i]->magnitude(1);
		}

		averageTensor(ts,t);
		faceTensor.push_back(t);
	}
}

Tensor GeoProperty::lineSearchHessian(Vec3& v, Tensor& tensor)
{
	double x = v.dot(tensor.dir1);
	double y = v.dot(tensor.dir2);

	/*
	double denom = pow(x,2)*tensor.mag1+pow(y,2)*tensor.mag2;
	double denomSqrt = sqrt(denom);
	double upscale = 1e5;

	tensor.mat[0][0]=tensor.mag1;
	tensor.mat[1][1]=tensor.mag2;
	tensor.mat[0][1]=tensor.mat[1][0]=0;
	Tensor res;
	for (int i = 0; i < 2; i++){
	for (int j = 0; j < 2; j++){
	double t1 = 0.0 , t2 = 0.0;
	for (int k = 0; k < 2; k++){
	t1 += tensor.mat[i][k] * vecProj[k];
	t2 += tensor.mat[j][k] * vecProj[k];
	}
	res.mat[i][j] = (0.5 * tensor.mat[i][j] * upscale) / (denomSqrt * upscale) -
	(0.5 * t1 * upscale * t2) / (denom * upscale * denomSqrt);
	}
	}
	*/

	//make it as a metric tensor

	double a = tensor.mag1;
	double d = tensor.mag2;

	double dis = a*x*x + d*y*y;
	double dis23 = sqrt(dis) * dis;

	Tensor res;
	if (dis23>1e-8)
	{
		res.mat[0][0] = a / dis - a*a*x*x / dis23;
		res.mat[0][1] = -a*d*x*y / dis23;
		res.mat[1][0] = res.mat[0][1];
		res.mat[1][1] = d / dis - d*d*y*y / dis23;
		Tensor::makeTensor(tensor.dir1, tensor.dir2, res);
	}
	return res;
}

// double Tensor::compareTensors(Tensor& t1, Tensor& t2, std::vector<Vec2> vs)
// {
// 	//compute the difference of two tensors.... it is hard to define the difference..
// /*
// 	U1 = {{a1, b1}, {c1, d1}} ;
// 	U2 = {{a2, b2}, {c2, d2}};
// 	V = {Cos[#], Sin[#]} &;
// 	Sum[Power[Sqrt[V[x].U1.V[x]] - Sqrt[V[x].U2.V[x]], 2], {x, 0, Pi,
// 		Pi/360}]
// */
// 	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
// 	t1.mat[0][1]=t1.mat[1][0]=0;
//
// 	Vec2 v1_,v2_;
// 	v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
// 	v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);
// 	t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
// 	t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
// 	t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;
//
// 	//Sum[Power[Sqrt[V[x].U1.V[x]] - Sqrt[V[x].U2.V[x]], 2]
// 	double ds=0;
// 	for(unsigned i=0;i<vs.size();i++)
// 	{
// 		ds+= pow(
// 			sqrt(abs((vs[i].x*t1.mat[0][0] + vs[i].y*t1.mat[1][0])*vs[i].x + (vs[i].x*t1.mat[0][1] + vs[i].y*t1.mat[1][1])*vs[i].y)) -
// 			sqrt(abs((vs[i].x*t2.mat[0][0] + vs[i].y*t2.mat[1][0])*vs[i].x + (vs[i].x*t2.mat[0][1] + vs[i].y*t2.mat[1][1])*vs[i].y))
// 			,2);
// 	}
// /*
// 	for(unsigned i=0;i<vs.size();i++){
// 		ds+= abs(
// 			sqrt(abs((vs[i].x*t1.mat[0][0] + vs[i].y*t1.mat[1][0])*vs[i].x + (vs[i].x*t1.mat[0][1] + vs[i].y*t1.mat[1][1])*vs[i].y)) -
// 			sqrt(abs((vs[i].x*t2.mat[0][0] + vs[i].y*t2.mat[1][0])*vs[i].x + (vs[i].x*t2.mat[0][1] + vs[i].y*t2.mat[1][1])*vs[i].y))
// 			);
// 	}
// */
// 	return ds;
// }


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <cassert>
#include <time.h>
#include <queue>

//#include <boost/heap/fibonacci_heap.hpp>

#include "CRS.h"

using namespace std;

CRS data;
bool* relaxed;
unsigned* totalDist;

class ComparePair {
public:
  bool operator()(pair<unsigned,unsigned> n1, pair<unsigned,unsigned> n2) {
    if(n1.second > n2.second)
      return true;
    else
      return false;
  }
};
priority_queue<pair<unsigned, unsigned>, vector<pair<unsigned, unsigned> >, ComparePair> work;

double abs(double val) {
  if(val < 0)
    return -1.0 * val;
  else
    return val;
}

unsigned min(unsigned a, unsigned b) {
  return (a < b) ? a : b;
}

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

  if((argc != 3) && (argc != 5)) {
    cout << "ERROR: incorrect input parameters!\n";
    cout << argv[0] << " <input file name> <source vertex>\n-- OR --\n";
    cout << argv[0] << " <input file name> <source vertex> -out <output file name>" << endl;
    exit(1);
  }

  ifstream in(argv[1]);
  unsigned source = atoi(argv[2]);
  bool genOutput = false;
  ofstream out(argv[4]);
  if(argc == 5) {
    genOutput = true;
  }

  if(!in.is_open()) {
    cout << "ERROR: Can't open file " << argv[1] << endl;
    exit(1);
  }
  cout << "Running on " << argv[1] << " with source vertex " << source << endl;
  time_t t1, t2, t3;
  t1 = time(NULL);

  // Get vertices and edges
  unsigned numVertices, numEdges;
  in >> numVertices >> ws >> numEdges >> ws;

  totalDist = new unsigned[numVertices];
  relaxed = new bool[numVertices];
  for(int i = 0; i < numVertices; i++) {
    relaxed[i] = false;
    totalDist[i] = -1;
  }
  cout << "EdgeDist initialized to " << totalDist[0] << endl;
  totalDist[source] = 0;
  work.push(make_pair(source, 0));

  int src, dest, weight;
  data.row->resize(numVertices);
  while(in.good()) {
    in >> src >> ws >> dest >> ws >> weight >> ws;
    //cout << "---" << src << " " << dest << " " << weight << endl;
    data.add(src, dest, weight);
  }
  //data.print();
  t2 = time(NULL);
  cout << "Number of rows: " << data.row->size() << endl;

  cout << "Running Dijkstra's Algorithm\n";
  while(!work.empty()) {
    pair<unsigned, unsigned> top = work.top();
    work.pop();

    //cout << "Top: " << top.first << ", " << top.second << endl;

    int vertex = top.first;
    if(relaxed[vertex])
      continue;

    // Check for disconnected graphs!
    if(totalDist[vertex] == -1) {
      cout << "Disconnected graph detected, break!\n";
      break;
    }

    relaxed[vertex] = true;

    deque<pair<unsigned, unsigned> >* row = data.getRow(vertex);
    if(row != NULL) {
      for(deque<pair<unsigned,unsigned> >::iterator it = row->begin(); it!=row->end(); it++) {
	if(!relaxed[it->first]) {
	  // update distance
	  int dist = totalDist[vertex]+it->second;
	  if(dist < totalDist[it->first]) {
	    totalDist[it->first] = dist;
	    //cout << "  Checking " << it->first << ", minDist = " << minDist << endl;
	    work.push(make_pair(it->first, dist));
	  }
	}
      }
    }
  }

  t3 = time(NULL);

  cout << "Setup time: " << (t2 - t1) << " seconds\n";
  cout << "Dijkstra SSSP time: " << (t3 - t2) << " seconds\n";

  if(genOutput) {
    for(int i = 0; i < numVertices; i++) {
      out <<  i << "," << totalDist[i] << endl;
    }
  }
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* Test_matmul.cpp - Testing the functionality of multiplying an encrypted
 * vector by a plaintext matrix, either over the extension- or the
 * base-field/ring.
 */
#include <cassert>
#include <NTL/lzz_pXFactoring.h>
#include "multiAutomorph.h"
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

// defined in debugging.cpp
void decryptAndPrint(ostream& s, const Ctxt& ctxt, const FHESecKey& sk,
		     const EncryptedArray& ea, long flags);

static void checkAuto(const Ctxt& cOrig, const Ctxt& cAuto, long amt,
                      EncryptedArray& ea, const FHESecKey& sk)
{
  NewPlaintextArray v1(ea), v2(ea);
  Ctxt cTmp = cOrig;
  cTmp.smartAutomorph(amt);

  ea.decrypt(cTmp, sk, v1);
  ea.decrypt(cAuto, sk, v2);

  if (!equals(ea, v1, v2)) { // check that we've got the right answer
    cout << " k = "<<amt<<"failed, Grrr@*\n";
    exit(0);
  }
}

class AutoTester: public AutomorphHandler {
public:
  const Ctxt& cOrig;
  FHESecKey& sk;
  EncryptedArray& ea;

    AutoTester(const Ctxt& c, FHESecKey& k, EncryptedArray& e, bool v=false):
    cOrig(c), sk(k), ea(e) {}

  // cPtr points to the original ciphertext, ctxtx is after automorphism
  bool handle(std::unique_ptr<Ctxt>& ctxt, long amt) override {
    // check that ctxt is indeed the original ctxt after automorphism
    checkAuto(cOrig, *ctxt, amt, ea, sk);

    return true;
  }
};

void  TestIt1(FHESecKey& secretKey, EncryptedArray& ea, bool verbose=false)
{
  const FHEcontext& context = ea.getContext();
  const FHEPubKey& publicKey = secretKey;

  // choose a random plaintext vector
  NewPlaintextArray v(ea);
  random(ea, v);

  // encrypt the random vector
  Ctxt ctxt(publicKey);
  ea.encrypt(ctxt, publicKey, v);
  ctxt.square();
  ctxt.cube();

  AutoTester test(ctxt, secretKey, ea);
  for (long i=0; i<=ea.dimension(); i++) {
    const AutGraph& tree = publicKey.getTree4dim(i);
    multiAutomorph(ctxt, tree, test);
  }
  cout << "  All tests using handler passed successfully\n";
}

void  TestIt2(FHESecKey& secretKey, EncryptedArray& ea, bool verbose=false)
{
  const FHEcontext& context = ea.getContext();
  const FHEPubKey& publicKey = secretKey;

  // choose a random plaintext vector
  NewPlaintextArray v(ea);
  random(ea, v);

  // encrypt the random vector
  Ctxt ctxt(publicKey);
  ea.encrypt(ctxt, publicKey, v);
  ctxt.square();
  ctxt.cube();

  Ctxt tmp(ZeroCtxtLike, ctxt);
  for (long i=0; i<=ea.dimension(); i++) {
    const AutGraph& tree = publicKey.getTree4dim(i);
    std::unique_ptr<AutoIterator> it(AutoIterator::build(ctxt, tree));
    while (long val = it->next(tmp))
      checkAuto(ctxt, tmp, val, ea, secretKey);
  }
  cout << "  All tests using iterator passed successfully\n";
}

/* Testing the new automorphism

 * Usage: Test_newAutomorph_x [optional params]
 *
 *  m defines the cyclotomic polynomial Phi_m(X) [default=2047]
 *    another useful setting to test is m=4369
 *  p is the plaintext base [default=2]
 *  L is the # of primes in the modulus chain [default=4]
 *  verbose print extra info [default=0]
 */
int main(int argc, char *argv[])
{
  ArgMapping amap;

  long p=2;
  amap.arg("p", p, "plaintext base");
  long m=2047;
  amap.arg("m", m, "defines the cyclotomic polynomial Phi_m(X)");
  amap.note("another useful setting to test is m=4369, p=2");
  long L=15;
  amap.arg("L", L, "# of levels in the modulus chain");
  bool verbose=false;
  amap.arg("verbose", verbose, "print extra information");
  amap.parse(argc, argv);

  cout << "*** "<<argv[0]
       << ": m=" << m
       << ", p=" << p
       << ", L=" << L
       << endl;

  FHEcontext context(m, p, 1);
  buildModChain(context, L, /*c=*/3);

  FHESecKey secretKey(context);
  secretKey.GenSecKey(/*w=*/64); // A Hamming-weight-w secret key

  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey); // compute key-switching matrices that we need
  EncryptedArray ea(context, context.alMod);

  if (verbose) {
    context.zMStar.printout();
    cout << endl;
    for (long i=0; i<=ea.dimension(); i++) {
      cout << "Tree("<<i<<") =\n";
      const AutGraph& tree = secretKey.getTree4dim(i);
      for (auto x: tree) {
        cout << "  "<< x.first<<": ";
        cout << x.second << endl;
      }
    }
  }

  resetAllTimers();
  TestIt1(secretKey, ea, verbose);
  if (verbose) {
    printAllTimers();
    cout << endl;
  }

  resetAllTimers();
  TestIt2(secretKey, ea, verbose);
  if (verbose) {
    printAllTimers();
    cout << endl;
  }
}


#include <boost/test/unit_test.hpp>
#include "functions/derivatives.hh"
#include "functions/streaming.hh"
#include "pointwise_equal.hh"

BOOST_AUTO_TEST_CASE(derivative_test) {
  return;
  using namespace manifolds;
  BOOST_CHECK_EQUAL(Derivative(Sin()), Cos());
  auto f = Cos()(x * x) * y;
  auto d = Derivative(f);
  auto check = 2_c * -x * Sin()(x * x) * y;
  PointwiseEqual(d, check);

  D<2> d2;
  auto ddf = d2(f);
  auto ddf2 = Derivative(d);
  PointwiseEqual(ddf, ddf2);
  auto fd = FullDerivative(f);
  auto fd_check = GetFunctionMatrix(GetRow(d, Cos()(x * x)));
  PointwiseEqual(fd, fd_check);
  auto fd_check_d = Derivative(fd);
  auto fd_check_check = GetFunctionMatrix(
      GetRow(-2_c * Sin()(x * x) * y + -4_c * x * x * Cos()(x * x) * y,
             -2_c * x * Sin()(x * x)));
  PointwiseEqual(fd_check, fd_check_check);
  PointwiseEqual(fd_check_d, fd_check_check);
  auto p = Pow()(x, x);
  auto pd = Derivative(p);
  auto pd_check = (Log()(x) + 1_c) * Pow()(x, x);
  PointwiseEqual(pd, pd_check, pointwise_default_num_points,
                 pointwise_default_tolerance, 0.001);

  auto m = D<3>()(Log()(x));
  auto m_check = Derivative(Derivative(Derivative(Log()(x))));
  BOOST_CHECK_EQUAL(m, m_check);
}


#pragma once

#include <vector>
#include <Eigen/Eigen>
#include "types/SensorValues.hpp"

/* Complementary Kalman Filter (CKF), estimates robot torso pitch and roll
   (forward lean and right side lean) using gyro and accelerometer */

class CKF {
   public:

      CKF();

      void update(const SensorValues &sensorValues);

      void resetFilter();

      float getSideLean();

      float getForwardLean();

   private:

      Eigen::Vector4f state; // roll, pitch, roll_velocity, pitch_velocity
      Eigen::Matrix<float, 4, 4> var;
      Eigen::Matrix<float, 4, 4> Q; // Process noise
      Eigen::Matrix<float, 4, 4> R; // Observation noise

};


/**
 * Copyright (c) 2011, 2012
 * Claudio Kopper <claudio.kopper@icecube.wisc.edu>
 * and the IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * $Id: I3CLSimVectorTransformMatrix.cxx 108199 2013-07-12 21:33:08Z nwhitehorn $
 *
 * @file I3CLSimVectorTransformMatrix.cxx
 * @version $Revision: 108199 $
 * @date $Date: 2013-07-12 15:33:08 -0600 (Fri, 12 Jul 2013) $
 * @author Claudio Kopper
 */

#include <icetray/serialization.h>
#include <clsim/function/I3CLSimVectorTransformMatrix.h>

#include <typeinfo>
#include <cmath>
#include <math.h>
#include <sstream>
#include <stdexcept>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include "clsim/I3CLSimHelperToFloatString.h"
using namespace I3CLSimHelper;

I3CLSimVectorTransformMatrix::
I3CLSimVectorTransformMatrix(
    const I3Matrix &matrix,
    bool renormalize)
:
matrix_(matrix),
renormalize_(renormalize)
{
    if ((matrix_.size1() != 3) || (matrix_.size2() != 3))
        throw std::range_error("matrix must be 3x3!");
}

I3CLSimVectorTransformMatrix::I3CLSimVectorTransformMatrix() {;}

I3CLSimVectorTransformMatrix::~I3CLSimVectorTransformMatrix()
{;}

bool I3CLSimVectorTransformMatrix::HasNativeImplementation() const
{
    return true;
}

std::vector<double> I3CLSimVectorTransformMatrix::ApplyTransform(const std::vector<double> &vec) const
{
    if (vec.size() != 3)
        throw std::range_error("vector must contain excatly 3 elements!");

    boost::numeric::ublas::vector<double> uvec_in(3);
    std::copy(vec.begin(), vec.end(), uvec_in.begin());
    boost::numeric::ublas::vector<double> uvec_out(3);

    boost::numeric::ublas::noalias(uvec_out) = boost::numeric::ublas::prod(matrix_, uvec_in);

    std::vector<double> out_vec(3, NAN);

    double norm=1.;

    if (renormalize_) {
        norm=0.;
        for (std::size_t i=0;i<3;++i)
        {
            norm += (uvec_out(i)*uvec_out(i));
        }
        norm = std::sqrt(norm);

        for (std::size_t i=0;i<3;++i)
        {
            out_vec[i] = uvec_out(i)/norm;
        }
    } else {
        for (std::size_t i=0;i<3;++i)
        {
            out_vec[i] = uvec_out(i);
        }
    }

    return out_vec;
}

std::string I3CLSimVectorTransformMatrix::GetOpenCLFunction(const std::string &functionName) const
{
    // the OpenCL interface takes a pointer to a float4, but ignores the fourth component

    std::string funcDef =
    std::string("inline void ") + functionName + std::string("(float4 *vec)");

    std::string funcBody = std::string() +
    "{\n";

    funcBody = funcBody +
    "    *vec = (float4)\n"
    "    (\n"
    "        (" + ToFloatString(matrix_(0,0)) + "*(*vec).x)+(" + ToFloatString(matrix_(0,1)) + "*(*vec).y)+(" + ToFloatString(matrix_(0,2)) + "*(*vec).z),\n"
    "        (" + ToFloatString(matrix_(1,0)) + "*(*vec).x)+(" + ToFloatString(matrix_(1,1)) + "*(*vec).y)+(" + ToFloatString(matrix_(1,2)) + "*(*vec).z),\n"
    "        (" + ToFloatString(matrix_(2,0)) + "*(*vec).x)+(" + ToFloatString(matrix_(2,1)) + "*(*vec).y)+(" + ToFloatString(matrix_(2,2)) + "*(*vec).z),\n"
    "        (*vec).w\n"
    "    );\n";

    if (renormalize_) {
        funcBody = funcBody +
        "#ifdef USE_NATIVE_MATH\n"
        "    (*vec).xyz = fast_normalize((*vec).xyz);\n"
        "#else\n"
        "    const float norm = rsqrt((*vec).x*(*vec).x + (*vec).y*(*vec).y + (*vec).z*(*vec).z);\n"
        "    (*vec).xyz = (*vec).xyz*norm;\n"
        "#endif\n";
    }

    funcBody = funcBody +
    "}\n";

    return funcDef + ";\n\n" + funcDef + "\n" + funcBody;
}

bool I3CLSimVectorTransformMatrix::CompareTo(const I3CLSimVectorTransform &other) const
{
    try
    {
        const I3CLSimVectorTransformMatrix &other_ = dynamic_cast<const I3CLSimVectorTransformMatrix &>(other);

        if ((other_.matrix_.size1() != matrix_.size1()))
            return false;
        if ((other_.matrix_.size2() != matrix_.size2()))
            return false;
        for (std::size_t i=0;i<matrix_.size1();++i)
            for (std::size_t j=0;j<matrix_.size2();++j)
            {
                if ((other_.matrix_(i,j) != matrix_(i,j)))
                    return false;
            }

        if ((other_.renormalize_ != renormalize_))
            return false;

        return true;
    }
    catch (const std::bad_cast& e)
    {
        // not of the same type, treat it as non-equal
        return false;
    }

}

template <class Archive>
void I3CLSimVectorTransformMatrix::serialize(Archive &ar, unsigned version)
{
    if (version>i3clsimvectortransformmatrix_version_)
        log_fatal("Attempting to read version %u from file but running version %u of I3CLSimVectorTransformMatrix class.",version,i3clsimvectortransformmatrix_version_);

    ar & make_nvp("I3CLSimVectorTransform", base_object<I3CLSimVectorTransform>(*this));
    ar & make_nvp("matrix", matrix_);
    ar & make_nvp("renormalize", renormalize_);
}

I3_SERIALIZABLE(I3CLSimVectorTransformMatrix);


#pragma once
#include <Eigen/Eigen>

class XXZ
{
private:
	int n_;
	double J_;
	double Delta_;

public:
	XXZ(int n, double J, double Delta)
		: n_(n), J_(J), Delta_(Delta)
	{
	}

	template<class State>
	typename State::T operator()(const State& smp) const
	{
		typename State::T s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			int zz = smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += J_*Delta_*zz; //zz
			s += J_*(1-zz)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		return s;
	}

	std::vector<std::array<int,2> > offDiagonals(const Eigen::VectorXi& s) const
	{
		std::vector< std::array<int, 2> > res;
		for(int i = 0; i < n_; i++)
		{
			if( s(i) != s((i+1)%n_) )
				res.push_back(std::array<int,2>{ i,(i+1)%n_ });
		}
		return res;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int zz = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col] += J_*Delta_*zz;
			m[col ^ x] += J_*(1 - zz);
		}
		return m;
	}
};


/***************************************************************************
   Copyright 2015 Ufora Inc.

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

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

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

#include <boost/random.hpp>
#include "../lassert.hpp"

namespace Ufora {
namespace math {
namespace Random {

template<class scalar_type>
class Normal {
public:
		Normal(uint32_t inSeed = 1u) :
				mSampler(mGen, mNormal)
			{
			mGen.seed(boost::uint32_t(inSeed));
			}

		scalar_type	operator()(void)
			{
			return mSampler();
			}

		template<class T>
		T operator()(T value)
			{
			return mSampler(value);
			}

		typedef typename boost::variate_generator<boost::mt19937&, boost::normal_distribution<scalar_type> >::result_type result_type;

		result_type min() const
			{
			return mSampler.min();
			}

		result_type max() const
			{
			return mSampler.max();
			}
private:
		boost::normal_distribution<scalar_type> mNormal;

		boost::mt19937 mGen;

	    boost::variate_generator<boost::mt19937&, boost::normal_distribution<scalar_type> > mSampler;
};

template<class scalar_type,
	bool = std::is_integral<scalar_type>::value,
	bool = std::is_floating_point<scalar_type>::value>
class Uniform;

template<class scalar_type>
class Uniform<scalar_type, false, true> {
public:
		Uniform(uint32_t inSeed = 1u) :
				mSampler(mGen, mUniform)
			{
			mGen.seed(boost::uint32_t(inSeed));
			}

		scalar_type	operator()(void)
			{
			return mSampler();
			}

		template<class T>
		scalar_type operator()(T value)
			{
			return mSampler(value);
			}

		typedef typename boost::variate_generator<boost::mt19937&, boost::uniform_real<scalar_type> >::result_type result_type;

		result_type min()
			{
			return mSampler.min();
			}

		result_type max()
			{
			return mSampler.max();
			}
private:
		boost::uniform_real<scalar_type> mUniform;

		boost::mt19937 mGen;

		boost::variate_generator<boost::mt19937&, boost::uniform_real<scalar_type> > mSampler;
};

template<class scalar_type>
class Uniform<scalar_type, true, false> {
public:
		Uniform(uint32_t inSeed = 1u) :
				mSampler(mGen, mUniform)
			{
			mGen.seed(boost::uint32_t(inSeed));
			}

		scalar_type	operator()(void)
			{
			return mSampler();
			}

		template<class T>
		scalar_type operator()(T value)
			{
			return mSampler(value);
			}

		typedef typename boost::variate_generator<boost::mt19937&, boost::uniform_int<scalar_type> >::result_type result_type;

		result_type min()
			{
			return mSampler.min();
			}

		result_type max()
			{
			return mSampler.max();
			}
private:
		boost::uniform_int<scalar_type> mUniform;

		boost::mt19937 mGen;

	    boost::variate_generator<boost::mt19937&, boost::uniform_int<scalar_type> > mSampler;
};

template<class T, class scalar_type>
decltype(*T().begin()) pickRandomlyFromSet(
								const T& values,
								Uniform<scalar_type>& random
								)
	{
	int which = values.size();

	while (which >= values.size())
		which = random() * values.size();

	auto it = values.begin();

	while (which)
		{
		it++;
		which--;
		}

	return *it;
	}

template<class T, class scalar_type, class weight_function_type>
decltype(*T().begin()) pickRandomlyFromSetWithWeight(
								const T& values,
								Uniform<scalar_type>& random,
								weight_function_type weightFunction
								)
	{
	double totalWeight = 0;

	for (auto val: values)
		totalWeight += weightFunction(val);

	lassert(totalWeight > 0.0);

	double w = totalWeight * random();

	for (auto val: values)
		if (weightFunction(val) >= w)
			return val;
		else
			w -= weightFunction(val);

	lassert(false);
	}

}; //namespace Random
}; //namespace math
}; //namespace Ufora


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <Eigen/Dense>
#include <ct/core/core.h>
#include "Quadrotor.hpp"

namespace ct {
namespace models {

class QuadrotorLinear final : public ct::core::LinearSystem<quadrotor::nStates, quadrotor::nControls>
{
public:
    typedef ct::core::StateVector<quadrotor::nStates> state_vector_t;
    typedef ct::core::ControlVector<quadrotor::nControls> control_vector_t;

    typedef Eigen::Matrix<double, quadrotor::nStates, quadrotor::nStates> state_matrix_t;
    typedef Eigen::Matrix<double, quadrotor::nStates, quadrotor::nControls> state_control_matrix_t;

    QuadrotorLinear* clone() const override { return new QuadrotorLinear(*this); }
    const state_matrix_t& getDerivativeState(const state_vector_t& x,
        const control_vector_t& u,
        const ct::core::Time t = 0.0) override
    {
        A_ = A_quadrotor(x, u);
        return A_;
    }

    const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
        const control_vector_t& u,
        const ct::core::Time t = 0.0) override
    {
        B_ = B_quadrotor(x, u);
        return B_;
    }

private:
    state_matrix_t A_;
    state_control_matrix_t B_;
};
}
}


// g++ -O3 -DNDEBUG -DMATSIZE=<x> benchmark.cpp -o benchmark && time ./benchmark
#include <Eigen/Array>

#ifndef MATSIZE
#define MATSIZE 3
#endif

using namespace std;
USING_PART_OF_NAMESPACE_EIGEN

#ifndef REPEAT
#define REPEAT 40000000
#endif

#ifndef SCALAR
#define SCALAR double
#endif

int main(int argc, char *argv[])
{
    Matrix<SCALAR,MATSIZE,MATSIZE> I = Matrix<SCALAR,MATSIZE,MATSIZE>::Ones();
    Matrix<SCALAR,MATSIZE,MATSIZE> m;
    for(int i = 0; i < MATSIZE; i++)
        for(int j = 0; j < MATSIZE; j++)
        {
            m(i,j) = (i+MATSIZE*j);
        }
    asm("#begin");
    for(int a = 0; a < REPEAT; a++)
    {
        m = Matrix<SCALAR,MATSIZE,MATSIZE>::Ones() + 0.00005 * (m + (m*m));
    }
    asm("#end");
    cout << m << endl;
    return 0;
}


# include <cstdlib>
# include <cmath>
# include <sys/time.h>
#include <boost/multi_array.hpp>

# include "matrix.hpp"
# include "brain.hpp"

# define CONV_UP  1.0e-6  /* parametro de convergencia Jacobi */
# define CONV_DO  1.0e2   /* parametro para chequear si Jacobi explota */
# define ITER_MAX 100

# define BTAG 0
# define ATAG 1

# if not defined SOLVER_H
# define SOLVER_H

/* hold grid dimensions and indexes for both serial and
 * parallel cases */
struct dimensions_t {
    int total;
    int local;
    int rest;
    int nx_rest;
    int nx_local;
    std::pair<int, int> local_i;
    int start_i;
    int end_i;
};

class Solver {

private:

    /* number of parallel processes */
    int num_tasks;

    /* discrete grid dimensions */
    int nx,ny,nz;
    dimensions_t dims;

    /* Jacobi matrix coefficients */
    boost_array2d_t *coeffs;
    int dim_coeffs;
    int s_order;

    /* space and time step values */
    double dX;
    double dT;

    /* timing */
    struct timeval tp;
    int t_elap;
    double t1, t2;

    /* preparar vectores *after y *before */
    void prepare_solution(const int, const int,
            boost_array3d_t& solution, boost_array2d_t& before, boost_array2d_t& after);

public:

  /* variables to keep track of timing */
  double init_time;
  double linear_time;
  double nonlinear_time;

  Solver(const int nx, const int ny, const int nz,
    const double dX, const double dT);

  ~Solver();

  static dimensions_t get_local_dimensions(const int nx, const int ny, const int nz);

  /* solver initialization */
  void start(const int task_id, BrainModel& brain, const int method);

  /* solution methods */
  void lie_trotter(const double current_time,
                const int task_id,
                boost_array3d_t& solution,
                BrainModel& brain_model);

  void expl(const int task_id, const int num_tasks,
            boost_array3d_t& solution, BrainModel& brain);

};

# endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "per_face_normals.h"
#include <Eigen/Geometry>

#define SQRT_ONE_OVER_THREE 0.57735026918962573
template <typename DerivedV, typename DerivedF, typename DerivedZ, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  const Eigen::MatrixBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  N.resize(F.rows(),3);
  // loop over faces
  int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
  for(int i = 0; i < Frows;i++)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
    N.row(i) = v1.cross(v2);//.normalized();
    typename DerivedV::Scalar r = N.row(i).norm();
    if(r == 0)
    {
      N.row(i) = Z;
    }else
    {
      N.row(i) /= r;
    }
  }
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  using namespace Eigen;
  Matrix<typename DerivedN::Scalar,3,1> Z(0,0,0);
  return per_face_normals(V,F,Z,N);
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals_stable(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  using namespace Eigen;
  typedef Matrix<typename DerivedV::Scalar,1,3> RowVectorV3;
  typedef typename DerivedV::Scalar Scalar;

  const size_t m = F.rows();

  N.resize(F.rows(),3);
  // Grad all points
  for(size_t f = 0;f<m;f++)
  {
    const RowVectorV3 p0 = V.row(F(f,0));
    const RowVectorV3 p1 = V.row(F(f,1));
    const RowVectorV3 p2 = V.row(F(f,2));
    const RowVectorV3 n0 = (p1 - p0).cross(p2 - p0);
    const RowVectorV3 n1 = (p2 - p1).cross(p0 - p1);
    const RowVectorV3 n2 = (p0 - p2).cross(p1 - p2);

    // careful sum
    for(int d = 0;d<3;d++)
    {
      // This is a little _silly_ in terms of complexity, but its recursive
      // implementation is clean looking...
      const std::function<Scalar(Scalar,Scalar,Scalar)> sum3 =
        [&sum3](Scalar a, Scalar b, Scalar c)->Scalar
      {
        if(fabs(c)>fabs(a))
        {
          return sum3(c,b,a);
        }
        // c < a
        if(fabs(c)>fabs(b))
        {
          return sum3(a,c,b);
        }
        // c < a, c < b
        if(fabs(b)>fabs(a))
        {
          return sum3(b,a,c);
        }
        return (a+b)+c;
      };

      N(f,d) = sum3(n0(d),n1(d),n2(d));
    }
    // sum better not be sure, or else NaN
    N.row(f) /= N.row(f).norm();
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// Nonsense template
namespace igl{template<> void per_face_normals<Eigen::Matrix<double, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 2, 0, -1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&){} }
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, -1, 1, 1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 1, 3, 1, 1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 1, -1, -1>, Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1>, Eigen::Matrix<float, -1, -1, 1, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<class Eigen::Matrix<double,-1,3,0,-1,3>,class Eigen::Matrix<int,-1,-1,0,-1,-1>,class Eigen::Matrix<double,-1,-1,0,-1,-1> >(class Eigen::MatrixBase<class Eigen::Matrix<double,-1,3,0,-1,3> > const &,class Eigen::MatrixBase<class Eigen::Matrix<int,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<double,-1,-1,0,-1,-1> > &);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 3, 0, 2, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 2, 3, 0, 2, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_DIVCEIL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIVCEIL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing divceil capabilities

    Computes the ceil of the division of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divceil(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = ceil(x/y);
    @endcode

    for integral types, if y is null, it returns @ref Valmax (resp. @ref Valmin)
    if x is positive (resp. negative), and 0 if x is null.
    Take care also that dividing @ref Valmin by -1 for signed integral types has
    undefined behaviour.

    @see  divides, rec, divs, divfloor, divround, divround2even, divfix

  **/
  const boost::dispatch::functor<tag::divceil_> divceil = {};
} }
#endif

#include <boost/simd/function/scalar/divceil.hpp>
#include <boost/simd/function/simd/divceil.hpp>

#endif


#include <memory>
#include <vector>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/camera.h>
#include <aslam/cameras/distortion-fisheye.h>

#include <geometric-vision/five-point-pose-estimator.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

class VariableCameraAngle : public ::testing::TestWithParam<double> {};

INSTANTIATE_TEST_CASE_P(
    OpengvPoseEstimationFivePoint, VariableCameraAngle,
    ::testing::Values(0, M_PI / 24.0, M_PI / 8.0, -M_PI / 8.0, -M_PI / 24.0));

TEST_P(VariableCameraAngle, PinholeCameraFivePointPoseInterface) {
  opengv_pose_estimation::FivePointPoseEstimator pose_estimator;

  typedef aslam::FisheyeDistortion DistortionType;
  typedef aslam::PinholeCamera CameraType;
  const double distortion_param = 0.90;
  const double fu = 200;
  const double fv = 200;
  const unsigned int ru = 640;
  const unsigned int rv = 480;
  const double cu = ru / 2;
  const double cv = rv / 2;
  Eigen::VectorXd distortion_params(1);
  distortion_params << distortion_param;
  std::shared_ptr<CameraType> camera;
  aslam::Distortion::UniquePtr distortion(
      new DistortionType(distortion_params));

  Eigen::VectorXd intrinsics(4);
  intrinsics << fu, fv, cu, cv;

  camera = std::shared_ptr<CameraType>(
      new CameraType(intrinsics, ru, rv, distortion));

  constexpr double kVariationAngle = M_PI / 36;  // 5 deg.
  Eigen::Quaterniond G_q_C_a(
      Eigen::AngleAxisd(
          GetParam() - kVariationAngle, Eigen::Vector3d::UnitY()));
  Eigen::Quaterniond G_q_C_b(
      Eigen::AngleAxisd(
          GetParam() + kVariationAngle, Eigen::Vector3d::UnitY()));
  Eigen::Matrix3d G_R_C_a = G_q_C_a.toRotationMatrix();
  Eigen::Matrix3d G_R_C_b = G_q_C_b.toRotationMatrix();
  Eigen::Vector3d G_p_C_a(1, 2.5, 3);
  Eigen::Vector3d G_p_C_b(1, 2, 3);

  const int num_of_points = 100;
  const int num_of_outliers = 10;
  Eigen::Matrix2Xd measurements_a;
  Eigen::Matrix2Xd measurements_b;
  Eigen::Matrix3Xd landmark_positions;
  measurements_a.resize(Eigen::NoChange, num_of_points + num_of_outliers);
  measurements_b.resize(Eigen::NoChange, num_of_points + num_of_outliers);
  landmark_positions.resize(Eigen::NoChange, num_of_points + num_of_outliers);
  for (int i = 0; i < num_of_points + num_of_outliers; ++i) {
    // We need to vary the depth -- plane may not be enough to recover
    // the camera pose.
    Eigen::Vector3d C_p_fa = camera->createRandomVisiblePoint(i % 5 + 1);
    Eigen::Vector2d keypoint_measurement_a;
    camera->project3(C_p_fa, &keypoint_measurement_a);
    measurements_a.col(i) = keypoint_measurement_a;

    // Add outliers.
    if (i >= num_of_points) {
      measurements_a.col(i) =
          keypoint_measurement_a +
          (Eigen::Vector2d() << (i + 10) % 8, -10).finished();
    }

    Eigen::Matrix<double, 3, 1> G_landmark_position =
        G_R_C_a * C_p_fa + G_p_C_a;

    Eigen::Vector3d C_p_fb =
        G_R_C_b.transpose() * (G_landmark_position - G_p_C_b);
    Eigen::Vector2d keypoint_measurement_b;
    camera->project3(C_p_fb, &keypoint_measurement_b);
    measurements_b.col(i) = keypoint_measurement_b;

    // Add outliers.
    if (i >= num_of_points) {
      measurements_b.col(i) = keypoint_measurement_b +
                              (Eigen::Vector2d() << (i + 5) % 7, 10).finished();
    }
  }

  Eigen::Quaterniond expected_rotation = G_q_C_a.inverse() * G_q_C_b;

  pose::Transformation estimated_transform;
  std::vector<int> inlier_matches;
  constexpr double kPixelSigma = 0.8;
  constexpr double kFocalLength = 100;
  const double kRansacThreshold = 1.0 - cos(atan(kPixelSigma / kFocalLength));
  pose_estimator.Compute(
      measurements_a, measurements_b, kRansacThreshold, 500, camera,
      &estimated_transform, &inlier_matches);

  EXPECT_GT(inlier_matches.size(), num_of_points * 0.90);

  EXPECT_NEAR_EIGEN(
      estimated_transform.getRotation().toImplementation().coeffs(),
      expected_rotation.coeffs(), 1e-2);
}

MAPLAB_UNITTEST_ENTRYPOINT


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <numeric>
#include <queue>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) (a).begin(), (a).end()
#define AALL(a, n) (a), ((a) + (n))
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define MODNUM (INT(1e9 + 7))
#define MOD(x) ((x) % MODNUM)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

const int INF = 2e9;
const double EPS = 1e-10;
const double PI = acos(-1.0);

const int dx[] = {-1, 0, 1, 0};
const int dy[] = {0, -1, 0, 1};

template <typename T>
int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T>
void chmax(T& m, T x) {
	m = max(m, x);
}

template <typename T>
void chmin(T& m, T x) {
	m = min(m, x);
}

template <typename T>
T square(T x) {
	return x * x;
}

inline int toInt(string s) {
	int v;
	istringstream sin(s);
	sin >> v;
	return v;
}

int top_digit(int n) {
	while(n / 10 > 0) {
		n /= 10;
	}
	return n;
}

int bottom_digit(int n) {
	return n % 10;
}

int kirisage(int n) {
	int ret = 1;
	while(ret * 10 < n) {
		ret *= 10;
	}
	return ret;
}

int count(int n, int y, int x) {
	int ret = 0;

	RANGE(i, 1, n + 1) {
		int top = top_digit(i);
		int bottom = bottom_digit(i);
		if(top == y && bottom == x) {
			ret++;
		}
	}
	return ret;
}
int main() {
	int n;
	scanf("%d", &n);
	VI2D num(10, VI(10, 0));
	RANGE(i, 1, 10) {
		RANGE(j, 1, 10) { num[i][j] = count(n, i, j); }
	}
	ll result = 0;
	RANGE(i, 1, 10) {
		RANGE(j, 1, 10) { result += (num[i][j] * num[j][i]); }
	}

	printf("%lld\n", result);
	return 0;
}


/* plantcalc -- Power plant modelling
 * (c) 2012 Michał Górny
 * Released under the terms of the 2-clause BSD license
 */

#ifdef HAVE_CONFIG_H
#	include "config.h"
#endif

#include "linearequationsolver.hxx"
#include "../equations/linearequation.hxx"
#include "../exceptions/contradictionerror.hxx"
#include "../variable.hxx"

#include <algorithm>
#include <cassert>
#include <cmath>
#include <map>
#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <Eigen/LU>

namespace
{
	class col_index
	{
		int _i;

	public:
		col_index()
			: _i(-1)
		{
		}

		void operator=(int x)
		{
			_i = x;
		}

		operator int() const
		{
			return _i;
		}
	};
}

LinearEquationSolver::LinearEquationSolver(EquationSystem& eqs, double epsilon)
	: _eqs(eqs), _epsilon(epsilon)
{
}

bool LinearEquationSolver::iterate()
{
	EquationSystem& eqs = _eqs;
	typedef std::vector<LinearEquation*> linear_eq_list;
	linear_eq_list lineqs;

	// collect linear equations
	for (EquationSystem::iterator it = eqs.begin(); it != eqs.end(); ++it)
	{
		LinearEquation* e = dynamic_cast<LinearEquation*>(*it);

		if (e)
			lineqs.push_back(e);
	}

	const int num_eqs = lineqs.size();
	if (num_eqs == 0)
		return false;

	int curr_var_index = 0;

	typedef std::map<Variable*, col_index> variable_map;
	variable_map varmap;

	// gather variable counts
	linear_eq_list::iterator it;
	for (it = lineqs.begin(); it != lineqs.end();)
	{
		LinearEquation& e = **it;
		LinearEquation::list_type::iterator jt;

		bool accepted = true;

		for (jt = e._vars.begin(); jt != e._vars.end(); ++jt)
		{
			LinearEquation::list_elem_type& cv = *jt;

			Variable* unk_var = 0;

			if (!cv.variable1->is_set())
				unk_var = cv.variable1;

			if (cv.variable2 && !cv.variable2->is_set())
			{
				// if there's no usable coefficient, drop that eq
				if (unk_var)
				{
					it = lineqs.erase(it);
					accepted = false;
					break;
				}
				unk_var = cv.variable2;
			}

			if (unk_var)
			{
				col_index& col = varmap[unk_var];

				if (col == -1)
					col = curr_var_index++;
			}
		}

		if (accepted)
			++it;
	}

	const int num_vars = varmap.size();
	Eigen::MatrixXd coeff_matrix
		= Eigen::MatrixXd::Zero(num_eqs + 1, num_vars);
	Eigen::VectorXd value_vector(num_eqs + 1);

	int row;
	for (it = lineqs.begin(), row = 0; it != lineqs.end(); ++it)
	{
		LinearEquation& e = **it;
		LinearEquation::list_type::iterator jt;

		double val = 0;

		for (jt = e._vars.begin(); jt != e._vars.end(); ++jt)
		{
			LinearEquation::list_elem_type& cv = *jt;

			Variable* unk_var = 0;
			double coeff = cv.coefficient;

			if (!cv.variable1->is_set())
				unk_var = cv.variable1;
			else
				coeff *= *cv.variable1;

			if (cv.variable2)
			{
				if (!cv.variable2->is_set())
				{
					assert(!unk_var);
					unk_var = cv.variable2;
				}
				else
					coeff *= *cv.variable2;
			}

			if (!unk_var)
				val -= coeff;
			else
			{
				col_index& col = varmap[unk_var];
				assert(col < num_vars);

				coeff_matrix(row, col) = coeff;
			}
		}

		value_vector(row) = val;
		++row;
	}

	Eigen::FullPivLU<Eigen::MatrixXd> lu = coeff_matrix.fullPivLu();

	Eigen::VectorXd sol_vector = lu.solve(value_vector);
	Eigen::VectorXd null_vector = lu.kernel().rowwise().sum();

	double error = (coeff_matrix * sol_vector - value_vector).norm();

	if (std::abs(error) > _epsilon)
		throw ContradictionError();

	std::vector<bool> keep_equations(num_eqs, false);

	variable_map::iterator vt;
	for (vt = varmap.begin(); vt != varmap.end(); ++vt)
	{
		Variable& v = *(*vt).first;
		col_index& col = (*vt).second;

		if (null_vector(col) > 0.5)
		{
			// find all equations with the variable and remove them
			// so they will remain in the original system of equations

			for (int i = 0; i < num_eqs; ++i)
			{
				if (std::abs(coeff_matrix(i, col)) > _epsilon)
					keep_equations[i] = true;
			}

			continue;
		}

		v.set_value(sol_vector(col));
	}

	// remove the solved equations
	// they should be in the same order in both vectors
	EquationSystem::iterator et = eqs.begin();
	for (linear_eq_list::size_type i = 0; i < lineqs.size(); ++i)
	{
		if (keep_equations[i])
			continue;

		Equation* eq = lineqs[i];

		assert(et != eqs.end());
		while (*et != eq)
		{
			++et;
			assert(et != eqs.end());
		}
		et = eqs.erase(et);
	}

	return false;
}


#ifndef DYN_GCA_INCLUDED
#define DYN_GCA_INCLUDED

#include <boost/multi_array.hpp>

using namespace std;

/**
 * @file   dyn_gca.hpp
 * @brief  ODE system for group SIS with group selection mechanism for transmission rates
 *
 * @author  LHD
 * @since   2021-02-06
 */

struct Sparam {
    const double beta;
    const double gamma;
    const double rho;
    const double b;
    const double c;
    const double mu;
    const int dim1;
    const int dim2;
}; // parameter structure

//********** function dydt definition **************************************************************
int dydt(double t, const double y[], double f[], void * param) {
// ODE system for interacting contagions

    // Cast parameters
    Sparam& p = *static_cast<Sparam* >(param);

    // Create multi_array reference to y and f
    typedef boost::multi_array_ref<const double,2> CSTmatref_type;
    typedef boost::multi_array_ref<double,2> matref_type;
    typedef CSTmatref_type::index indexref;
    CSTmatref_type yref(y,boost::extents[p.dim1][p.dim2]);
    matref_type fref(f,boost::extents[p.dim1][p.dim2]);

    // Calculate mean-field coupling and observed fitness landscape
    vector<double> Zvec(p.dim1, 0.0); vector<double> popvec(p.dim1, 0.0); double R=0.0;
    for(int l=0; l<p.dim1; ++l) {
        for(int i=0; i<p.dim2; ++i) {
            Zvec[l] += exp(p.b*i-p.c*l)*yref[l][i]; //should this really be proportional to C_i,l?
            popvec[l] += yref[l][i];
            R += p.rho*i*yref[l][i];
        }
        if(popvec[l]>0.0) Zvec[l] /= popvec[l];
    }
    double Z = accumulate(Zvec.begin(), Zvec.end(), 0.0);
    int n=p.dim2-1;
    int L=p.dim1-1;

    // Compute derivatives
    for(int l=0; l<p.dim1; ++l) {
        for(int i=0; i<p.dim2; ++i) {
            fref[l][i] = -1.0*p.gamma*i*yref[l][i] - p.beta*l*(i+R)*(n-i)*yref[l][i];
            if(i>0) fref[l][i] += p.beta*l*((i-1)+R)*(n-i+1)*yref[l][i-1];
            if(i<n) fref[l][i] += p.gamma*(i+1)*yref[l][i+1];
            if(l>0) fref[l][i] += p.rho*yref[l-1][i]*(Zvec[l]/Zvec[l-1]+p.mu) - p.rho*yref[l][i]*(Zvec[l-1]/Zvec[l]+p.mu);
            if(l<L) fref[l][i] += p.rho*yref[l+1][i]*(Zvec[l]/Zvec[l+1]+p.mu) - p.rho*yref[l][i]*(Zvec[l+1]/Zvec[l]+p.mu);
        }
    }

    return GSL_SUCCESS;

} //********** end function dydt definition ********************************************************

#endif // DYN_GCA_HPP_INCLUDED


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Use of the iterative solvers in ViennaCL with Boost.uBLAS
*
*/

//
// include necessary system headers
//
#include <iostream>

//
// Necessary to obtain a suitable performance in ublas
#ifndef NDEBUG
 #define NDEBUG
#endif

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

//
// ViennaCL includes
//
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

using namespace boost::numeric;

int main()
{
  typedef float       ScalarType;

  //
  // Set up some ublas objects
  //
  ublas::vector<ScalarType> rhs;
  ublas::vector<ScalarType> rhs2;
  ublas::vector<ScalarType> ref_result;
  ublas::vector<ScalarType> result;
  ublas::compressed_matrix<ScalarType> ublas_matrix;

  //
  // Read system from file
  //
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //std::cout << "done reading matrix" << std::endl;

  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }
  //std::cout << "done reading rhs" << std::endl;

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ref_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }
  //std::cout << "done reading result" << std::endl;

  //
  // set up ILUT preconditioners for ViennaCL and ublas objects. Other preconditioners can also be used (see manual)
  //
  viennacl::linalg::ilut_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilut(ublas_matrix, viennacl::linalg::ilut_tag());
  viennacl::linalg::ilu0_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());
  viennacl::linalg::block_ilu_precond< ublas::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          ublas_block_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());

  //
  // Conjugate gradient solver:
  //
  std::cout << "----- CG Test -----" << std::endl;

  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag());
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_ilut);
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_ilu0);
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_block_ilu0);
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;

  //
  // Stabilized BiConjugate gradient solver:
  //
  std::cout << "----- BiCGStab Test -----" << std::endl;

  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag());          //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_ilut); //with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_ilu0); //with preconditioner

  //
  // GMRES solver:
  //
  std::cout << "----- GMRES Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag());   //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_ilut);//with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_ilu0);//with preconditioner

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return 0;
}


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

#ifndef BOOST_MATH_HYPOT_INCLUDED
#define BOOST_MATH_HYPOT_INCLUDED

#include <cmath>
#include <boost/limits.hpp>
#include <algorithm> // swap

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; using ::fabs; }
#endif

namespace boost{ namespace math{

template <class T>
T hypot(T x, T y)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif

   //
   // normalize x and y, so that both are positive and x >= y:
   //
   x = (std::fabs)(x);
   y = (std::fabs)(y);

   // special case, see C99 Annex F:
   if(std::numeric_limits<T>::has_infinity
      && ((x == std::numeric_limits<T>::infinity())
      || (y == std::numeric_limits<T>::infinity())))
      return std::numeric_limits<T>::infinity();

   if(y > x)
      (std::swap)(x, y);
   //
   // figure out overflow and underflow limits:
   //
   T safe_upper = (std::sqrt)((std::numeric_limits<T>::max)()) / 2;
   T safe_lower = (std::sqrt)((std::numeric_limits<T>::min)());
   static const T one = 1;
   //
   // Now handle special cases:
   //
   if(x >= safe_upper)
   {
      if(y <= one)
      {
         // y is neligible:
         return x;
      }
      return (std::sqrt)(x) * (std::sqrt)(y) * (std::sqrt)(x/y + y/x);
   }
   else if(y <= safe_lower)
   {
      if((x >= one) || (y == 0))
      {
         // y is negligible:
         return x;
      }
      return (std::sqrt)(x) * (std::sqrt)(y) * (std::sqrt)(x/y + y/x);
   }
   //
   // If we get here then x^2+y^2 will not overflow or underflow:
   //
   return (std::sqrt)(x*x + y*y);
}

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


#include <Eigen/Core>
#include <Eigen/LU>
#include "gtest/gtest.h"

#include "solver/qp_solver.h"
#include "util/random.h"

namespace GraphSfM {

// A rigged QP minimization problem with a known output.
//
// 1/2 * x' * P * x + q' * x + r
//     [  5  -2  -1 ]
// P = [ -2   4   3 ]
//     [ -1   3   5 ]
//
// q = [ 2  -35  -47 ]^t
// r = 5
//
// Minimizing this unbounded problem should result in:
//   x = [ 3  5  7 ]^t
TEST(QPSolver, Unbounded) {
  static const double kTolerance = 1e-4;

  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1,
    -2, 4, 3,
    -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  options.max_num_iterations = 100;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);
  Eigen::VectorXd solution;
  ASSERT_TRUE(qp_solver.Solve(&solution));

  // Verify the solution is near (3, 5, 7).
  const Eigen::Vector3d gt_solution(3, 5, 7);
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(solution(i), gt_solution(i), kTolerance);
  }

  // Check that the residual is near optimal.
  const double residual =
      0.5 * solution.dot(P * solution) + solution.dot(q) + r;
  const double gt_residual =
      0.5 * gt_solution.dot(P * gt_solution) + gt_solution.dot(q) + r;
  EXPECT_NEAR(residual, gt_residual, kTolerance);
}

TEST(QPSolver, LooseBounds) {
  static const double kTolerance = 1e-4;

  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1,
    -2, 4, 3,
    -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  options.max_num_iterations = 100;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);

  // Set a lower bound that should not affect the output.
  Eigen::VectorXd lower_bound(3);
  lower_bound << 0, 0, 0;
  qp_solver.SetLowerBound(lower_bound);

  // Set an upper bound that should not affect the output.
  Eigen::VectorXd upper_bound(3);
  upper_bound << 10, 10, 10;
  qp_solver.SetUpperBound(upper_bound);
  Eigen::VectorXd solution;

  ASSERT_TRUE(qp_solver.Solve(&solution));

  // Verify the solution is near (3, 5, 7).
  const Eigen::Vector3d gt_solution(3, 5, 7);
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(solution(i), gt_solution(i), kTolerance);
  }

  // Check that the residual is near optimal.
  const double residual =
      0.5 * solution.dot(P * solution) + solution.dot(q) + r;
  const double gt_residual =
      0.5 * gt_solution.dot(P * gt_solution) + gt_solution.dot(q) + r;
  EXPECT_NEAR(residual, gt_residual, kTolerance);
}

TEST(QPSolver, TightBounds) {
  static const double kTolerance = 1e-4;

  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1,
    -2, 4, 3,
    -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  options.absolute_tolerance = 1e-8;
  options.relative_tolerance = 1e-8;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);

  // Set a lower bound that constrains the output.
  Eigen::VectorXd lower_bound(3);
  lower_bound << 5, 7, 9;
  qp_solver.SetLowerBound(lower_bound);

  // Set an upper bound that constrains the output.
  Eigen::VectorXd upper_bound(3);
  upper_bound << 10, 12, 14;
  qp_solver.SetUpperBound(upper_bound);

  Eigen::VectorXd solution;
  ASSERT_TRUE(qp_solver.Solve(&solution));

  // Verify the solution is near (5, 7, 9).
  const Eigen::Vector3d gt_solution(5, 7, 9);
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(solution(i), gt_solution(i), kTolerance);
  }

  // Check that the residual is near optimal.
  const double residual =
      0.5 * solution.dot(P * solution) + solution.dot(q) + r;
  const double gt_residual =
      0.5 * gt_solution.dot(P * gt_solution) + gt_solution.dot(q) + r;
  EXPECT_NEAR(residual, gt_residual, kTolerance);
}

TEST(QPSolver, InvalidBounds) {
  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1,
    -2, 4, 3,
    -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);

  // Set the upper bound as the lower bound.
  Eigen::VectorXd lower_bound(3);
  lower_bound << 5, 7, 9;
  qp_solver.SetUpperBound(lower_bound);

  // Set the lower bound as the upper bound, making the valid solution space
  // non-existant.
  Eigen::VectorXd upper_bound(3);
  upper_bound << 10, 12, 14;
  qp_solver.SetLowerBound(upper_bound);

  Eigen::VectorXd solution;
  EXPECT_FALSE(qp_solver.Solve(&solution));
}

}  // namespace GraphSfM


#include <iostream>
#include <sys/time.h>
#include <Eigen/Core>
#include <fstream>

#include "celerite/celerite.h"
#include "celerite/carma.h"
#include "celerite/utils.h"
#include "../include/KF.h"
#include "../include/ndsho.h"
#include "../include/dsho.h"

using namespace Eigen;
using namespace std;

#define TWOPI 6.283185307179586

// This program benchmarks a single DSHO using celerite and gpstate

// Timer for the benchmark.
double get_timestamp ()
{
  struct timeval now;
  gettimeofday (&now, NULL);
  return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
}

// Function to read a whitespace separated file
std::vector<double> load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ' ')) {
            //cout << std::stod(cell) << endl;
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return values;
}

int main (int argc, char* argv[])
{
  srand(42);

  // we will benchmark models with up to N_max DSHO components
  size_t N_max = 64;
  //N_max=1;
  if (argc >= 2) N_max = atoi(argv[1]);
  // dfor each sample size, timeing will be done niter times
  size_t niter = 5;
  if (argc >= 3) niter = atoi(argv[2]);

  std::random_device rd;
  std::mt19937 rng(rd());
  std::normal_distribution<double> gaussian(0.0,0.05);

  int vecsize = 10000;
	// map data to VectorXds
	VectorXd yi(vecsize);
  VectorXd times = Eigen::VectorXd::Random(vecsize);
  times.array() += 1.0;
  times.array() *= (10*0.5);
  std::sort(times.data(), times.data() + times.size());
  double omega0 = TWOPI;

  for (int j=0; j<times.size(); j++){
      //times(j) = 0.001*double(j);
      yi(j) = sin(omega0 * times(j)) + gaussian(rng);
  }

	// this is the observational error vector
	VectorXd yierr = VectorXd::Ones(yi.size());
	yierr.array() *= 0.05;

  // Generate some fake data.
  //size_t N_points = 4096;
  //Eigen::VectorXd x = Eigen::VectorXd::Random(N_points),
  //                yerr = Eigen::VectorXd::Random(N_points),
  //                y, diag;
  //yerr.array() *= 0.1;
  //yerr.array() += 1.0;
  //diag = yerr.array() * yerr.array();
  //std::sort(x.data(), x.data() + x.size());
  //y = sin(x.array());

  //std::vector<double> values = load_csv("two_comp_dsho.txt");
  //std::vector<double> values = load_csv("GPtest10_dsho_trim.txt");

  //VectorXd times = Map<VectorXd, 0, InnerStride<2> > (values.data(), 10000);
  //VectorXd yi = Map<VectorXd, 0, InnerStride<2> > (values.data()+1, 10000);
  //VectorXd yierr = VectorXd::Ones(yi.size());
  //yierr *= 0.01;
  VectorXd diagi = yierr.array()*yierr.array();

  //set up the single DSHO parameters we use as a base
  //double omega0 = 0.785398;
  double Q = 10.0;
  double varf = 0.05;

  double celerite_time = 0.0;
  double gpstate_time = 0.0;
  double strt;

  Eigen::VectorXd alpha_real, beta_real;

  for (size_t N = 1; N <= N_max; N +=1) {
    // define arrays
    // The DSHO is a CARMA model, with parameters as written below
    Eigen::VectorXd omega0_arr(N), Q_arr(N), varf_arr(N);
    Eigen::VectorXd alpha_complex_real_arr(N), alpha_complex_imag_arr(N), beta_complex_real_arr(N), beta_complex_imag_arr(N);
    double log_likelihood=0.0, celerite_ll=0.0;

    int nterms = 3;

    for (size_t i=0; i<N; i++)
    {
      if (i == 0)
      {
        omega0_arr(i) = omega0; //0.785398; //0.1234234;
      }
      else
      {
        omega0_arr(i) = omega0 + static_cast<double>(i);
      }
      Q_arr(i) = Q;
      varf_arr(i) = varf;
      Eigen::VectorXd carma_arparams(nterms);
      Eigen::VectorXd carma_maparams(nterms-1);
      carma_arparams << omega0_arr(i)*omega0_arr(i), omega0_arr(i)/Q, 1.0;
      carma_maparams << 1.0, 0.0;

      // version that used to work!
      double temp = std::sqrt(4.0*Q_arr(i)*Q_arr(i) - 1.0);
      //double S0 = varf_arr(i)* std::pow(Q_arr(i),-2) * std::sqrt(M_PI) / std::sqrt(2);
      double S0 = varf_arr(i)* std::pow(omega0_arr(i),-4) * std::sqrt(M_PI) / std::sqrt(2);
      alpha_complex_real_arr(i) = S0 * omega0_arr(i) * Q_arr(i);
      alpha_complex_imag_arr(i) = S0 * omega0_arr(i) * Q_arr(i) / temp;
      beta_complex_real_arr(i) = 0.5 * omega0_arr(i) / Q_arr(i);
      beta_complex_imag_arr(i) = 0.5 * temp * omega0_arr(i) / Q_arr(i);
    }

    celerite::solver::CholeskySolver<double> solver;

    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      //int flag = solver.compute(alpha_real_arr, beta_real_arr, alpha_complex_real_arr, alpha_complex_imag_arr, beta_complex_real_arr, beta_complex_imag_arr, x, diag);
      //celerite_ll = -0.5*(solver.dot_solve(y) + solver.log_determinant() + x.rows() * log(2.0 * M_PI));

      solver.compute(0.0, alpha_real, beta_real, alpha_complex_real_arr, alpha_complex_imag_arr, beta_complex_real_arr, beta_complex_imag_arr, times, diagi);
      celerite_ll = -0.5*(solver.dot_solve(yi) + solver.log_determinant() + times.rows() * log(2.0 * M_PI));
      celerite_time += get_timestamp()-strt;
    }

    //gpstate::n_dsho::N_DSHOSolver ndsho(x,y,yerr,omega0_arr, Q_arr, varf_arr);
    gpstate::n_dsho::N_DSHOSolver ndsho(times,yi,yierr,omega0_arr, Q_arr, varf_arr);
    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      log_likelihood = ndsho.KF_log_likelihood();
      gpstate_time += get_timestamp()-strt;
    }

    // Print the results.
    std::cout << N;
    std::cout << " ";
    std::cout << celerite_time / niter;
    std::cout << " ";
    std::cout << gpstate_time / niter;
    std::cout << " ";
    std::cout << celerite_ll;
    std::cout << " ";
    std::cout << log_likelihood;
    std::cout << " ";

    std::cout << "\n";
  }
}


//  Copyright (c) 2006 Xiaogang Zhang, 2015 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the y < 0 case.
//  Updated 2015 to use Carlson's latest methods.
//

#ifndef BOOST_MATH_ELLINT_RC_HPP
#define BOOST_MATH_ELLINT_RC_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/constants/constants.hpp>
#include <iostream>

// Carlson's degenerate elliptic integral
// R_C(x, y) = R_F(x, y, y) = 0.5 * \int_{0}^{\infty} (t+x)^{-1/2} (t+y)^{-1} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rc_imp(T x, T y, const Policy& pol)
{
    BOOST_MATH_STD_USING

    static const char* function = "boost::math::ellint_rc<%1%>(%1%,%1%)";

    if(x < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument x must be non-negative but got %1%", x, pol);
    }
    if(y == 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument y must not be zero but got %1%", y, pol);
    }

    // for y < 0, the integral is singular, return Cauchy principal value
    T prefix, result;
    if(y < 0)
    {
        prefix = sqrt(x / (x - y));
        x = x - y;
        y = -y;
    }
    else
       prefix = 1;

    if(x == 0)
    {
       result = constants::half_pi<T>() / sqrt(y);
    }
    else if(x == y)
    {
       result = 1 / sqrt(x);
    }
    else if(y > x)
    {
       result = atan(sqrt((y - x) / x)) / sqrt(y - x);
    }
    else
    {
       if(y / x > 0.5)
       {
          T arg = sqrt((x - y) / x);
          result = (boost::math::log1p(arg) - boost::math::log1p(-arg)) / (2 * sqrt(x - y));
       }
       else
       {
          result = log((sqrt(x) + sqrt(x - y)) / sqrt(y)) / sqrt(x - y);
       }
    }
    return prefix * result;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rc_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y), pol), "boost::math::ellint_rc<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y)
{
   return ellint_rc(x, y, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RC_HPP


#ifndef STATS_STATS_HPP
#define STATS_STATS_HPP

#include <Eigen/Dense>
#include <unordered_map>
#include <string>
#include <math.h>
#ifdef BAZEL
#include "Models/Model.hpp"
#else
#include "../Models/Model.hpp"
#endif

using namespace Eigen;
using namespace std;

namespace Stats {
//public:
    float ChiSquaredTest(MatrixXf, MatrixXf);
    float ChiToPValue(float, int);
    float WaldTest(float mle, float var, float candidate);
    float FisherExactTest(MatrixXf);
    float BonCorrection(float, int);
    float get_ts(float beta, float var, float sigma);
    float get_qs(float ts, int N, int q);
};

class StatsBasic : public Model{
protected:
    bool shouldCorrect;
    int genoType;

    // algorithm use
    float progress;
    bool isRunning;
    bool shouldStop;

    void checkGenoType();
public:
    virtual void setAttributeMatrix(const string&, MatrixXf*);

    void BonferroniCorrection();

    MatrixXf getBeta();

    virtual void assertReadyToRun();
    virtual void run() {};
    virtual void setUpRun();
    virtual void finishRun();

    StatsBasic();
    StatsBasic(const unordered_map<string, string>&);

    // algorithm replacement
    float getProgress();
    bool getIsRunning();
    void stop();

    virtual ~StatsBasic(){};
};

#endif //STATS_STATS_HPP


#include <boost/math/distributions/logistic.hpp>