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

#include "include/Helly.hh"
#include "../model/include/Hypergraphe.hh"
#include "../model/include/HyperVertex.hh"
#include "../model/include/HyperEdge.hh"
#include <boost/foreach.hpp>

Helly::Helly(const boost::shared_ptr<HypergrapheAbstrait>& ptrHypergrapheAbstrait) :
				_ptrHypergrapheAbstrait( ptrHypergrapheAbstrait ) {

}

void
Helly::runAlgorithme() {

	_result.setBooleanResult(true);

	BOOST_FOREACH(auto& x, _ptrHypergrapheAbstrait->getHyperVertexList()) {
		BOOST_FOREACH(auto& y, _ptrHypergrapheAbstrait->getHyperVertexList()) {

			LibType::ListHyperEdge X_xy( allContainXY(x, y) );
			BOOST_FOREACH(auto& v, _ptrHypergrapheAbstrait->getHyperVertexList()) {

				if( voisin(x, v) && voisin(y, v) ) {
					LibType::ListHyperEdge X_xv( allContainXY(x, v) );
					LibType::ListHyperEdge X_yv( allContainXY(y, v) );

					LibType::ListHyperEdge X;
					concatenate(X, X_xy);
					concatenate(X, X_xv);
					concatenate(X, X_yv);

					if( !nonEmptyIntersection(X) ) {
						_result.setBooleanResult(false);
						return;
					}
				}
			}

		}
	}
}

bool
Helly::voisin(boost::shared_ptr<HyperVertex>& v1, boost::shared_ptr<HyperVertex>& v2) {
	BOOST_FOREACH(auto& element1, v1->getHyperEdgeList() ) {
		BOOST_FOREACH(auto& element2, v2->getHyperEdgeList() ) {
			if( element1==element2 ) {
				return true;
			}
		}
	}
	return false;
}

void
Helly::concatenate(LibType::ListHyperEdge& dest, LibType::ListHyperEdge& src) {
	BOOST_FOREACH(auto& e, src) {
		dest.push_back(e);
	}
}

bool
Helly::nonEmptyIntersection(LibType::ListHyperEdge& ensemble) {
	for(unsigned int i=0; i<ensemble.size(); i++) {
		for(unsigned int j=i+1; j<ensemble.size(); j++) {
			if( !nonEmptyBetween(ensemble.at(i), ensemble.at(j)) )
				return false;
		}
	}
	return true;
}

bool
Helly::nonEmptyBetween(boost::shared_ptr<HyperEdge>& e1, boost::shared_ptr<HyperEdge>& e2) {
	BOOST_FOREACH(auto& a, e1->getHyperVertexList()) {
		BOOST_FOREACH(auto& b, e2->getHyperVertexList()) {
			if(a==b)return true;
		}
	}
	return false;
}

LibType::ListHyperEdge&
Helly::allContainXY(boost::shared_ptr<HyperVertex>& v1, boost::shared_ptr<HyperVertex>& v2) {
	LibType::ListHyperEdge * elist = new LibType::ListHyperEdge();
	BOOST_FOREACH(auto& e, _ptrHypergrapheAbstrait->getHyperEdgeList()) {
		if( e->containVertex(v1) && e->containVertex(v2) ) {
			elist->push_back(e);
		}
	}
	return *elist;
}

RStructure
Helly::getResult() const {
	return _result;
}

Helly::~Helly() {

}


#include <iostream>
#include <armadillo>
#include "RkFehl.h"
#include "DuffingOscil.h"
/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
using namespace std;
using namespace arma;

/**
 * Pars has 5 fields:
 *
 * gamma - index 0.
 * h - index 1.
 * tf - index 2.
 * x0 - index 3.
 * y0 - index 4.
 *
 *
 * @param pars
 * @return
 */
JNIEXPORT jdoubleArray JNICALL Java_net_tedkwan_javafem_jni_DuffingOscil_rk4(JNIEnv *env, jobject jobj, jdoubleArray jarray) {
    unsigned int i;
    jboolean isCopy1;

    jdouble* srcArrayElems =
            env -> GetDoubleArrayElements(jarray, &isCopy1);
    jint n = env -> GetArrayLength(jarray);
    vector<double> pars(4);
    pars[2]= srcArrayElems[1];
    pars[3]= srcArrayElems[4];
    double h = srcArrayElems[0];
    pars[0]= srcArrayElems[2];
    pars[1]= srcArrayElems[3];
    double tf = srcArrayElems[5];
    vec x = zeros<vec>(2);
    x(0) = srcArrayElems[6];
    x(1) = srcArrayElems[7];
    //x(2) = 0.75;
    //Rkfun testrk(gamma,h,tf,x);
    RkFehl testrk(pars,"vanderpol",tf,h,x);
    vector<vec> y=testrk.y;
    vector<double> t=testrk.t;

    vector<double> res;
    //jboolean isCopy2;
    unsigned int k=t.size();

    for(int j=0;j<3;j++){
        for (i = 0; i < k; i++) {
        if(j<2){
            res.push_back(y[i](j));
        }
        else{
            res.push_back(t[i]);
        }
    }
//        for(int j=0;j<4;j++){
//        for (i = 0; i < k; i++) {
//        if(j<3){
//            res.push_back(y[i](j));
//        }
//        else{
//            res.push_back(t[i]);
//        }
//    }

    }

    cout << n << endl;

    if (isCopy1 == JNI_TRUE) {
        env -> ReleaseDoubleArrayElements(jarray, srcArrayElems, JNI_ABORT);
    }
    int lenres=res.size();
    jdoubleArray result = env -> NewDoubleArray(lenres);
    env->SetDoubleArrayRegion(result, 0, lenres, &res[0]);
//    if (isCopy2 == JNI_TRUE) {
//        env -> ReleaseDoubleArrayElements(jarray, destArrayElems, 0);
//    }

    return result;
}


//  Copyright John Maddock 2007.
//  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)

#include "required_defines.hpp"

#include "performance_measure.hpp"

#include <boost/math/distributions.hpp>

double probabilities[] = {
   1e-5,
   1e-4,
   1e-3,
   1e-2,
   0.05,
   0.1,
   0.2,
   0.3,
   0.4,
   0.5,
   0.6,
   0.7,
   0.8,
   0.9,
   0.95,
   1-1e-5,
   1-1e-4,
   1-1e-3,
   1-1e-2
};

int int_values[] = {
   1,
   2,
   3,
   5,
   10,
   20,
   50,
   100,
   1000,
   10000,
   100000
};

int small_int_values[] = {
   1,
   2,
   3,
   5,
   10,
   15,
   20,
   30,
   50,
   100,
   150
};

double real_values[] = {
   1e-5,
   1e-4,
   1e-2,
   1e-1,
   1,
   10,
   100,
   1000,
   10000,
   100000
};

#define BOOST_MATH_DISTRIBUTION3_TEST(name, param1_table, param2_table, param3_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" BOOST_STRINGIZE(name) "-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
   unsigned d_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            for(unsigned l = 0; l < d_size; ++l)\
            {\
               result += cdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j], param3_table[k]), random_variable_table[l]);\
            }\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size * d_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_pdf_, name), "dist-" BOOST_STRINGIZE(name) "-pdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
   unsigned d_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            for(unsigned l = 0; l < d_size; ++l)\
            {\
               result += pdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j], param3_table[k]), random_variable_table[l]);\
            }\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size * d_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" BOOST_STRINGIZE(name) "-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
      unsigned d_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               for(unsigned l = 0; l < d_size; ++l)\
               {\
                  result += quantile(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j], param3_table[k]), probability_table[l]);\
               }\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size * d_size);\
   }

#define BOOST_MATH_DISTRIBUTION2_TEST(name, param1_table, param2_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" BOOST_STRINGIZE(name) "-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += cdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j]), random_variable_table[k]);\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_pdf_, name), "dist-" BOOST_STRINGIZE(name) "-pdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += pdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j]), random_variable_table[k]);\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" BOOST_STRINGIZE(name) "-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               result += quantile(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j]), probability_table[k]);\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size);\
   }

#define BOOST_MATH_DISTRIBUTION1_TEST(name, param1_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" BOOST_STRINGIZE(name) "-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += cdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i]), random_variable_table[k]);\
         }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_pdf_, name), "dist-" BOOST_STRINGIZE(name) "-pdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += pdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i]), random_variable_table[k]);\
         }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" BOOST_STRINGIZE(name) "-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               result += quantile(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i]), probability_table[k]);\
            }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * c_size);\
   }

BOOST_MATH_DISTRIBUTION2_TEST(beta, probabilities, probabilities, probabilities, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(binomial, int_values, probabilities, int_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(cauchy, int_values, real_values, int_values, probabilities)
BOOST_MATH_DISTRIBUTION1_TEST(chi_squared, int_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION1_TEST(exponential, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(fisher_f, int_values, int_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(gamma, real_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION3_TEST(hypergeometric, small_int_values, small_int_values, small_int_values, small_int_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(logistic, real_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(lognormal, real_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(negative_binomial, int_values, probabilities, int_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(normal, real_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION1_TEST(poisson, real_values, int_values, probabilities)
BOOST_MATH_DISTRIBUTION1_TEST(students_t, int_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(weibull, real_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(non_central_chi_squared, int_values, int_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION3_TEST(non_central_beta, int_values, int_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION3_TEST(non_central_f, int_values, int_values, real_values, real_values, probabilities)
BOOST_MATH_DISTRIBUTION2_TEST(non_central_t, int_values, small_int_values, real_values, probabilities)

#ifdef TEST_R

#define MATHLIB_STANDALONE 1

extern "C" {
#include "Rmath.h"
}

#define BOOST_MATH_R_DISTRIBUTION3_TEST(name, param1_table, param2_table, param3_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" #name "-R-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
   unsigned d_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            for(unsigned l = 0; l < d_size; ++l)\
            {\
               result += p##name (random_variable_table[l], param1_table[i], param2_table[j], param3_table[k], 1, 0);\
            }\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size * d_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_pdf_, name), "dist-" #name "-R-pdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
   unsigned d_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            for(unsigned l = 0; l < d_size; ++l)\
            {\
               result += d##name (random_variable_table[l], param1_table[i], param2_table[j], param3_table[k], 0);\
            }\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size * d_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" #name "-R-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
      unsigned d_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               for(unsigned l = 0; l < d_size; ++l)\
               {\
                  result += q##name (probability_table[l], param1_table[i], param2_table[j], param3_table[k], 1, 0);\
               }\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size * d_size);\
   }

#define BOOST_MATH_R_DISTRIBUTION2_TEST(name, param1_table, param2_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" #name "-R-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += p##name (random_variable_table[k], param1_table[i], param2_table[j], 1, 0);\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_pdf_, name), "dist-" #name "-R-pdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += d##name (random_variable_table[k], param1_table[i], param2_table[j], 0);\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" #name "-R-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               result += q##name (probability_table[k], param1_table[i], param2_table[j], 1, 0);\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size);\
   }

#define BOOST_MATH_R_DISTRIBUTION1_TEST(name, param1_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" #name "-R-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += p##name (random_variable_table[k], param1_table[i], 1, 0);\
         }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_pdf_, name), "dist-" #name "-R-pdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += d##name (random_variable_table[k], param1_table[i], 0);\
         }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" #name "-R-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               result += q##name (probability_table[k], param1_table[i], 1, 0);\
            }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * c_size);\
   }

double qhypergeo(double r, double n, double N, double p, int i, int j)
{
   if(r > N)
      return std::numeric_limits<double>::quiet_NaN();
   double nr = r;
   double nb = N - r;
   return qhyper(nr, nb, n, p, i, j);
}

double phypergeo(double r, double n, double N, double k, int i, int j)
{
   if((r > N) || (k > n) || (k > r))
      return std::numeric_limits<double>::quiet_NaN();
   double nr = r;
   double nb = N - r;
   return phyper(nr, nb, n, k, i, j);
}

double dhypergeo(double r, double n, double N, double k, int i)
{
   if((r > N) || (k > n) || (k > r))
      return std::numeric_limits<double>::quiet_NaN();
   double nr = r;
   double nb = N - r;
   return dhyper(nr, nb, n, k, i);
}

BOOST_MATH_R_DISTRIBUTION2_TEST(beta, probabilities, probabilities, probabilities, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(binom, int_values, probabilities, int_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(cauchy, int_values, real_values, int_values, probabilities)
BOOST_MATH_R_DISTRIBUTION1_TEST(chisq, int_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION1_TEST(exp, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(f, int_values, int_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(gamma, real_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION3_TEST(hypergeo, small_int_values, small_int_values, small_int_values, small_int_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(logis, real_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(lnorm, real_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(nbinom, int_values, probabilities, int_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(norm, real_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION1_TEST(pois, real_values, int_values, probabilities)
BOOST_MATH_R_DISTRIBUTION1_TEST(t, int_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(weibull, real_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(nchisq, int_values, int_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION3_TEST(nf, int_values, int_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION3_TEST(nbeta, int_values, int_values, real_values, real_values, probabilities)
BOOST_MATH_R_DISTRIBUTION2_TEST(nt, int_values, small_int_values, real_values, probabilities)

#endif

#ifdef TEST_CEPHES

extern "C"{

double bdtr(int k, int n, double p);
double bdtri(int k, int n, double p);

double chdtr(double df, double x);
double chdtri(double df, double p);

double fdtr(int k, int n, double p);
double fdtri(int k, int n, double p);

double nbdtr(int k, int n, double p);
double nbdtri(int k, int n, double p);

}

#define BOOST_MATH_CEPHES_DISTRIBUTION2_TEST(name, param1_table, param2_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" #name "-cephes-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += name##dtr (param1_table[i], param2_table[j], random_variable_table[k]);\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" #name "-cephes-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               result += name##dtri (param1_table[i], param2_table[j], probability_table[k]);\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size);\
   }

#define BOOST_MATH_CEPHES_DISTRIBUTION1_TEST(name, param1_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist, name), "dist-" #name "-cephes-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            result += name##dtr (param1_table[i], random_variable_table[k]);\
         }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dist_quant, name), "dist-" #name "-cephes-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               result += name##dtri (param1_table[i], probability_table[k]);\
            }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * c_size);\
   }
// Cephes inverse doesn't actually calculate the quantile!!!
// BOOST_MATH_CEPHES_DISTRIBUTION2_TEST(b, int_values, int_values, probabilities, probabilities)
BOOST_MATH_CEPHES_DISTRIBUTION1_TEST(ch, int_values, real_values, probabilities)
BOOST_MATH_CEPHES_DISTRIBUTION2_TEST(f, int_values, int_values, real_values, probabilities)
// Cephes inverse doesn't calculate the quantile!!!
// BOOST_MATH_CEPHES_DISTRIBUTION2_TEST(nb, int_values, int_values, probabilities, probabilities)

#endif

#ifdef TEST_DCDFLIB
#include <dcdflib.h>

void cdfbeta( int *which, double *p, double *q, double *x, double *a, double *b, int *status, double *bound)
{
   double y = 1 - *x;
   cdfbet(which, p, q, x, &y, a, b, status, bound);
}

void cdfbinomial( int *which, double *p, double *q, double *x, double *a, double *b, int *status, double *bound)
{
   double y = 1 - *x;
   double cb = 1 - *b;
   cdfbet(which, p, q, x, a, b, &cb, status, bound);
}

void cdfnegative_binomial( int *which, double *p, double *q, double *x, double *a, double *b, int *status, double *bound)
{
   double y = 1 - *x;
   double cb = 1 - *b;
   cdfnbn(which, p, q, x, a, b, &cb, status, bound);
}

void cdfchi_squared( int *which, double *p, double *q, double *x, double *a, int *status, double *bound)
{
   cdfchi(which, p, q, x, a, status, bound);
}

void cdfnon_central_chi_squared( int *which, double *p, double *q, double *x, double *a, double *b, int *status, double *bound)
{
   cdfchn(which, p, q, x, a, b, status, bound);
}

namespace boost{ namespace math{

   template <class T = double> struct f_distribution : public fisher_f_distribution<T>
   { f_distribution(T a, T b) : fisher_f_distribution<T>(a, b) {} };
   template <class T = double>
   struct fnc_distribution : public non_central_f_distribution<T>
   { fnc_distribution(T a, T b, T c) : non_central_f_distribution<T>(a, b, c) {} };
   template <class T = double> struct gam_distribution : public gamma_distribution<T>
   { gam_distribution(T a, T b) : gamma_distribution<T>(a, b) {} };
   template <class T = double> struct nor_distribution : public normal_distribution<T>
   { nor_distribution(T a, T b) : normal_distribution<T>(a, b) {} };
   template <class T = double> struct poi_distribution : public poisson_distribution<T>
   { poi_distribution(T a) : poisson_distribution<T>(a) {} };
   template <class T = double> struct t_distribution : public students_t_distribution<T>
   { t_distribution(T a) : students_t_distribution<T>(a) {} };

   template <class T>
   T cdf(const f_distribution<T>& d, const T& r){  return cdf(static_cast<fisher_f_distribution<T> >(d), r);  }
   template <class T>
   T quantile(const f_distribution<T>& d, const T& r){  return quantile(static_cast<fisher_f_distribution<T> >(d), r);  }
   template <class T>
   T cdf(const fnc_distribution<T>& d, const T& r){  return cdf(static_cast<non_central_f_distribution<T> >(d), r);  }
   template <class T>
   T quantile(const fnc_distribution<T>& d, const T& r){  return quantile(static_cast<non_central_f_distribution<T> >(d), r);  }
   template <class T>
   T cdf(const gam_distribution<T>& d, const T& r){  return cdf(static_cast<gamma_distribution<T> >(d), r);  }
   template <class T>
   T quantile(const gam_distribution<T>& d, const T& r){  return quantile(static_cast<gamma_distribution<T> >(d), r);  }
   template <class T>
   T cdf(const nor_distribution<T>& d, const T& r){  return cdf(static_cast<normal_distribution<T> >(d), r);  }
   template <class T>
   T quantile(const nor_distribution<T>& d, const T& r){  return quantile(static_cast<normal_distribution<T> >(d), r);  }
   template <class T>
   T cdf(const poi_distribution<T>& d, const T& r){  return cdf(static_cast<poisson_distribution<T> >(d), r);  }
   template <class T>
   T quantile(const poi_distribution<T>& d, const T& r){  return quantile(static_cast<poisson_distribution<T> >(d), r);  }
   template <class T>
   T cdf(const t_distribution<T>& d, const T& r){  return cdf(static_cast<students_t_distribution<T> >(d), r);  }
   template <class T>
   T quantile(const t_distribution<T>& d, const T& r){  return quantile(static_cast<students_t_distribution<T> >(d), r);  }

}}

bool check_near(double a, double b)
{
   bool r = ((fabs(a) <= 1e-7) || (fabs(b) <= 1e-7)) ? (fabs(a-b) < 1e-7) : fabs((a - b) / a) < 1e-5;
   return r;
}

#define BOOST_MATH_DCD_DISTRIBUTION3_TEST(name, param1_table, param2_table, param3_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dcd_dist, name), "dist-" BOOST_STRINGIZE(name) "-dcd-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
   unsigned d_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            for(unsigned l = 0; l < d_size; ++l)\
            {\
               int which = 1;\
               double p; double q; \
               double rv = random_variable_table[l];\
               double a = param1_table[i];\
               double b = param2_table[j];\
               double c = param3_table[k];\
               int status = 0;\
               double bound = 0;\
               BOOST_JOIN(cdf, name)(&which, &p, &q, &rv, &a, &b, &c, &status, &bound);\
               result += p;\
               BOOST_ASSERT(\
                  (status != 0) || check_near(p, \
                             cdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j], param3_table[k]), random_variable_table[l])\
               ));\
            }\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size * d_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dcd_dist_quant, name), "dist-" BOOST_STRINGIZE(name) "-dcd-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(param3_table)/sizeof(param3_table[0]);\
      unsigned d_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               for(unsigned l = 0; l < d_size; ++l)\
               {\
                  int which = 2;\
                  double p = probability_table[l];\
                  double q = 1 - p; \
                  double rv;\
                  double a = param1_table[i];\
                  double b = param2_table[j];\
                  double c = param3_table[k];\
                  int status = 0;\
                  double bound = 0;\
                  BOOST_JOIN(cdf, name)(&which, &p, &q, &rv, &a, &b, &c, &status, &bound);\
                  result += rv;\
                  BOOST_ASSERT((status != 0) || (p > 0.99) || check_near(rv, quantile(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j], param3_table[k]), probability_table[l])));\
               }\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size * d_size);\
   }

#define BOOST_MATH_DCD_DISTRIBUTION2_TEST(name, param1_table, param2_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dcd_dist, name), "dist-" BOOST_STRINGIZE(name) "-dcd-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
      for(unsigned j = 0; j < b_size; ++j)\
      {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            int which = 1;\
            double p; double q; \
            double rv = random_variable_table[k];\
            double a = param1_table[i];\
            double b = param2_table[j];\
            int status = 0;\
            double bound = 0;\
            BOOST_JOIN(cdf, name)(&which, &p, &q, &rv, &a, &b, &status, &bound);\
            result += p;\
            BOOST_ASSERT((status != 0) || check_near(p, cdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j]), random_variable_table[k])));\
         }\
      }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * b_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dcd_dist_quant, name), "dist-" BOOST_STRINGIZE(name) "-dcd-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned b_size = sizeof(param2_table)/sizeof(param2_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
         for(unsigned j = 0; j < b_size; ++j)\
         {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               int which = 2;\
               double p = probability_table[k];\
               double q = 1 - p; \
               double rv;\
               double a = param1_table[i];\
               double b = param2_table[j];\
               int status = 0;\
               double bound = 0;\
               BOOST_JOIN(cdf, name)(&which, &p, &q, &rv, &a, &b, &status, &bound);\
               result += rv;\
               BOOST_ASSERT((status != 0) || (p > 0.99) || check_near(rv, quantile(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i], param2_table[j]), probability_table[k])));\
            }\
         }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * b_size * c_size);\
   }

#define BOOST_MATH_DCD_DISTRIBUTION1_TEST(name, param1_table, random_variable_table, probability_table) \
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dcd_dist, name), "dist-" BOOST_STRINGIZE(name) "-dcd-cdf")\
   {\
   double result = 0;\
   unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
   unsigned c_size = sizeof(random_variable_table)/sizeof(random_variable_table[0]);\
   \
   for(unsigned i = 0; i < a_size; ++i)\
   {\
         for(unsigned k = 0; k < c_size; ++k)\
         {\
            int which = 1;\
            double p; double q; \
            double rv = random_variable_table[k];\
            double a = param1_table[i];\
            int status = 0;\
            double bound = 0;\
            BOOST_JOIN(cdf, name)(&which, &p, &q, &rv, &a, &status, &bound);\
            result += p;\
            BOOST_ASSERT((status != 0) || check_near(p, cdf(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i]), random_variable_table[k])));\
         }\
   }\
   \
   consume_result(result);\
   set_call_count(a_size * c_size);\
   }\
   BOOST_MATH_PERFORMANCE_TEST(BOOST_JOIN(dcd_dist_quant, name), "dist-" BOOST_STRINGIZE(name) "-dcd-quantile")\
   {\
      double result = 0;\
      unsigned a_size = sizeof(param1_table)/sizeof(param1_table[0]);\
      unsigned c_size = sizeof(probability_table)/sizeof(probability_table[0]);\
      \
      for(unsigned i = 0; i < a_size; ++i)\
      {\
            for(unsigned k = 0; k < c_size; ++k)\
            {\
               int which = 2;\
               double p = probability_table[k];\
               double q = 1 - p; \
               double rv;\
               double a = param1_table[i];\
               int status = 0;\
               double bound = 0;\
               BOOST_JOIN(cdf, name)(&which, &p, &q, &rv, &a, &status, &bound);\
               result += rv;\
               BOOST_ASSERT((status != 0) || (p > 0.99) || check_near(rv, quantile(boost::math:: BOOST_JOIN(name, _distribution) <>(param1_table[i]), probability_table[k])));\
            }\
      }\
      \
      consume_result(result);\
      set_call_count(a_size * c_size);\
   }

BOOST_MATH_DCD_DISTRIBUTION2_TEST(beta, probabilities, probabilities, probabilities, probabilities) // ??
BOOST_MATH_DCD_DISTRIBUTION2_TEST(binomial, int_values, probabilities, int_values, probabilities) // OK ish
BOOST_MATH_DCD_DISTRIBUTION1_TEST(chi_squared, int_values, real_values, probabilities) // OK
BOOST_MATH_DCD_DISTRIBUTION2_TEST(non_central_chi_squared, int_values, int_values, real_values, probabilities) // Error rates quite high for DCD version?
BOOST_MATH_DCD_DISTRIBUTION2_TEST(f, int_values, int_values, real_values, probabilities) // OK
BOOST_MATH_DCD_DISTRIBUTION3_TEST(fnc, int_values, int_values, real_values, real_values, probabilities) // Error rates quite high for DCD version?
BOOST_MATH_DCD_DISTRIBUTION2_TEST(gam, real_values, real_values, real_values, probabilities) // ??
BOOST_MATH_DCD_DISTRIBUTION2_TEST(negative_binomial, int_values, probabilities, int_values, probabilities) // OK
BOOST_MATH_DCD_DISTRIBUTION2_TEST(nor, real_values, real_values, real_values, probabilities) // OK
BOOST_MATH_DCD_DISTRIBUTION1_TEST(poi, real_values, int_values, probabilities) // OK
BOOST_MATH_DCD_DISTRIBUTION1_TEST(t, int_values, real_values, probabilities) // OK

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@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 "frame_field_deformer.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <vector>

#include <igl/cotmatrix_entries.h>
#include <igl/cotmatrix.h>
#include <igl/vertex_triangle_adjacency.h>

namespace igl
{

class Frame_field_deformer
{
public:

  IGL_INLINE Frame_field_deformer();
  IGL_INLINE ~Frame_field_deformer();

  // Initialize the optimizer
  IGL_INLINE void init(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F, const Eigen::MatrixXd& _D1, const Eigen::MatrixXd& _D2, double _Lambda, double _perturb_rotations, int _fixed = 1);

  // Run N optimization steps
  IGL_INLINE void optimize(int N, bool reset = false);

  // Reset optimization
  IGL_INLINE void reset_opt();

  // Precomputation of all components
  IGL_INLINE void precompute_opt();

  // Precomputation for deformation energy
  IGL_INLINE void precompute_ARAP(Eigen::SparseMatrix<double> & Lff, Eigen::MatrixXd & LfcVc);

  // Precomputation for regularization
  IGL_INLINE void precompute_SMOOTH(Eigen::SparseMatrix<double> & MS, Eigen::MatrixXd & bS);

  // extracts a r x c block from sparse matrix mat into sparse matrix m1
  // (r0,c0) is upper left entry of block
  IGL_INLINE void extractBlock(Eigen::SparseMatrix<double> & mat, int r0, int c0, int r, int c, Eigen::SparseMatrix<double> & m1);

  // computes optimal rotations for faces of m wrt current coords in mw.V
  // returns a 3x3 matrix
  IGL_INLINE void compute_optimal_rotations();

  // global optimization step - linear system
  IGL_INLINE void compute_optimal_positions();

  // compute the output XField from deformation gradient
  IGL_INLINE void computeXField(std::vector< Eigen::Matrix<double,3,2> > & XF);

  // computes in WW the ideal warp at each tri to make the frame field a cross
  IGL_INLINE void compute_idealWarp(std::vector< Eigen::Matrix<double,3,3> > & WW);

  // -------------------------------- Variables ----------------------------------------------------

  // Mesh I/O:

  Eigen::MatrixXd V;                         // Original mesh - vertices
  Eigen::MatrixXi F;                         // Original mesh - faces

  std::vector<std::vector<int> > VT;                   // Vertex to triangle topology
  std::vector<std::vector<int> > VTi;                  // Vertex to triangle topology

  Eigen::MatrixXd V_w;                       // Warped mesh - vertices

  std::vector< Eigen::Matrix<double,3,2> > FF;  	// frame field FF in 3D (parallel to m.F)
  std::vector< Eigen::Matrix<double,3,3> > WW;    // warping matrices to make a cross field (parallel to m.F)
  std::vector< Eigen::Matrix<double,3,2> > XF;  	// pseudo-cross field from solution (parallel to m.F)

  int fixed;

  double perturb_rotations; // perturbation to rotation matrices

  // Numerics
  int nfree,nconst;					          // number of free/constrained vertices in the mesh - default all-but-1/1
  Eigen::MatrixXd C;							            // cotangent matrix of m
  Eigen::SparseMatrix<double> L;					          // Laplacian matrix of m

  Eigen::SparseMatrix<double> M;					          // matrix for global optimization - pre-conditioned
  Eigen::MatrixXd RHS;						            // pre-computed part of known term in global optimization
  std::vector< Eigen::Matrix<double,3,3> > RW;    // optimal rotation-warping matrices (parallel to m.F) -- INCORPORATES WW
  Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver;   // solver for linear system in global opt.

  // Parameters
private:
  double Lambda = 0.1;				        // weight of energy regularization

};

  IGL_INLINE Frame_field_deformer::Frame_field_deformer() {}

  IGL_INLINE Frame_field_deformer::~Frame_field_deformer() {}

  IGL_INLINE void Frame_field_deformer::init(const Eigen::MatrixXd& _V,
                          const Eigen::MatrixXi& _F,
                          const Eigen::MatrixXd& _D1,
                          const Eigen::MatrixXd& _D2,
                          double _Lambda,
                          double _perturb_rotations,
                          int _fixed)
{
  V = _V;
  F = _F;

  assert(_D1.rows() == _D2.rows());

  FF.clear();
  for (unsigned i=0; i < _D1.rows(); ++i)
  {
    Eigen::Matrix<double,3,2> ff;
    ff.col(0) = _D1.row(i);
    ff.col(1) = _D2.row(i);
    FF.push_back(ff);
  }

  fixed = _fixed;
  Lambda = _Lambda;
  perturb_rotations = _perturb_rotations;

  reset_opt();
  precompute_opt();
}

IGL_INLINE void Frame_field_deformer::optimize(int N, bool reset)
{
  //Reset optimization
	if (reset)
    reset_opt();

	// Iterative Local/Global optimization
  for (int i=0; i<N;i++)
  {
    compute_optimal_rotations();
    compute_optimal_positions();
		computeXField(XF);
  }
}

IGL_INLINE void Frame_field_deformer::reset_opt()
{
  V_w = V;

  for (unsigned i=0; i<V_w.rows(); ++i)
    for (unsigned j=0; j<V_w.cols(); ++j)
      V_w(i,j) += (double(rand())/double(RAND_MAX))*10e-4*perturb_rotations;

}

// precomputation of all components
IGL_INLINE void Frame_field_deformer::precompute_opt()
{
  using namespace Eigen;
	nfree = V.rows() - fixed;						    // free vertices (at the beginning ov m.V) - global
  nconst = V.rows()-nfree;						// #constrained vertices
  igl::vertex_triangle_adjacency(V,F,VT,VTi);                // compute vertex to face relationship

  igl::cotmatrix_entries(V,F,C);							     // cotangent matrix for opt. rotations - global

  igl::cotmatrix(V,F,L);

	SparseMatrix<double> MA;						// internal matrix for ARAP-warping energy
	MatrixXd LfcVc;										  // RHS (partial) for ARAP-warping energy
	SparseMatrix<double> MS;						// internal matrix for smoothing energy
	MatrixXd bS;										    // RHS (full) for smoothing energy

	precompute_ARAP(MA,LfcVc);					// precompute terms for the ARAP-warp part
	precompute_SMOOTH(MS,bS);						// precompute terms for the smoothing part
	compute_idealWarp(WW);              // computes the ideal warps
  RW.resize(F.rows());								// init rotation matrices - global

  M =	  (1-Lambda)*MA + Lambda*MS;		// matrix for linear system - global

	RHS = (1-Lambda)*LfcVc + Lambda*bS;	// RHS (partial) for linear system - global
  solver.compute(M);									// system pre-conditioning
  if (solver.info()!=Eigen::Success) {fprintf(stderr,"Decomposition failed in pre-conditioning!\n"); exit(-1);}

	fprintf(stdout,"Preconditioning done.\n");

}

IGL_INLINE void Frame_field_deformer::precompute_ARAP(Eigen::SparseMatrix<double> & Lff, Eigen::MatrixXd & LfcVc)
{
  using namespace Eigen;
	fprintf(stdout,"Precomputing ARAP terms\n");
	SparseMatrix<double> LL = -4*L;
	Lff = SparseMatrix<double>(nfree,nfree);
  extractBlock(LL,0,0,nfree,nfree,Lff);
	SparseMatrix<double> Lfc = SparseMatrix<double>(nfree,nconst);
  extractBlock(LL,0,nfree,nfree,nconst,Lfc);
	LfcVc = - Lfc * V_w.block(nfree,0,nconst,3);
}

IGL_INLINE void Frame_field_deformer::precompute_SMOOTH(Eigen::SparseMatrix<double> & MS, Eigen::MatrixXd & bS)
{
  using namespace Eigen;
	fprintf(stdout,"Precomputing SMOOTH terms\n");

	SparseMatrix<double> LL = 4*L*L;

  // top-left
	MS = SparseMatrix<double>(nfree,nfree);
  extractBlock(LL,0,0,nfree,nfree,MS);

  // top-right
	SparseMatrix<double> Mfc = SparseMatrix<double>(nfree,nconst);
  extractBlock(LL,0,nfree,nfree,nconst,Mfc);

	MatrixXd MfcVc = Mfc * V_w.block(nfree,0,nconst,3);
	bS = (LL*V).block(0,0,nfree,3)-MfcVc;

}

  IGL_INLINE void Frame_field_deformer::extractBlock(Eigen::SparseMatrix<double> & mat, int r0, int c0, int r, int c, Eigen::SparseMatrix<double> & m1)
{
  std::vector<Eigen::Triplet<double> > tripletList;
  for (int k=c0; k<c0+c; ++k)
    for (Eigen::SparseMatrix<double>::InnerIterator it(mat,k); it; ++it)
    {
      if (it.row()>=r0 && it.row()<r0+r)
        tripletList.push_back(Eigen::Triplet<double>(it.row()-r0,it.col()-c0,it.value()));
    }
  m1.setFromTriplets(tripletList.begin(), tripletList.end());
}

IGL_INLINE void Frame_field_deformer::compute_optimal_rotations()
{
  using namespace Eigen;
  Matrix<double,3,3> r,S,P,PP,D;

  for (int i=0;i<F.rows();i++)
	{
		// input tri --- could be done once and saved in a matrix
		P.col(0) = (V.row(F(i,1))-V.row(F(i,0))).transpose();
		P.col(1) = (V.row(F(i,2))-V.row(F(i,1))).transpose();
		P.col(2) = (V.row(F(i,0))-V.row(F(i,2))).transpose();

		P = WW[i] * P;		// apply ideal warp

		// current tri
		PP.col(0) = (V_w.row(F(i,1))-V_w.row(F(i,0))).transpose();
		PP.col(1) = (V_w.row(F(i,2))-V_w.row(F(i,1))).transpose();
		PP.col(2) = (V_w.row(F(i,0))-V_w.row(F(i,2))).transpose();

		// cotangents
		D <<    C(i,2), 0,      0,
    0,      C(i,0), 0,
    0,      0,      C(i,1);

		S = PP*D*P.transpose();
		Eigen::JacobiSVD<Matrix<double,3,3> > svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV );
		Matrix<double,3,3>  su = svd.matrixU();
		Matrix<double,3,3>  sv = svd.matrixV();
		r = su*sv.transpose();

		if (r.determinant()<0)  // correct reflections
		{
			su(0,2)=-su(0,2); su(1,2)=-su(1,2); su(2,2)=-su(2,2);
			r = su*sv.transpose();
		}
		RW[i] = r*WW[i];		// RW INCORPORATES IDEAL WARP WW!!!
	}
}

IGL_INLINE void Frame_field_deformer::compute_optimal_positions()
{
  using namespace Eigen;
	// compute variable RHS of ARAP-warp part of the system
  MatrixXd b(nfree,3);          // fx3 known term of the system
	MatrixXd X;										// result
  int t;		  									// triangles incident to edge (i,j)
	int vi,i1,i2;									// index of vertex i wrt tri t0

  for (int i=0;i<nfree;i++)
  {
    b.row(i) << 0.0, 0.0, 0.0;
    for (int k=0;k<(int)VT[i].size();k++)					// for all incident triangles
    {
      t = VT[i][k];												// incident tri
			vi = (i==F(t,0))?0:(i==F(t,1))?1:(i==F(t,2))?2:3;	// index of i in t
			assert(vi!=3);
			i1 = F(t,(vi+1)%3);
			i2 = F(t,(vi+2)%3);
			b.row(i)+=(C(t,(vi+2)%3)*RW[t]*(V.row(i1)-V.row(i)).transpose()).transpose();
			b.row(i)+=(C(t,(vi+1)%3)*RW[t]*(V.row(i2)-V.row(i)).transpose()).transpose();
    }
  }
  b/=2.0;
	b=-4*b;

	b*=(1-Lambda);		// blend

  b+=RHS;				// complete known term

	X = solver.solve(b);
	if (solver.info()!=Eigen::Success) {printf("Solving linear system failed!\n"); return;}

	// copy result to mw.V
  for (int i=0;i<nfree;i++)
    V_w.row(i)=X.row(i);

}

  IGL_INLINE void Frame_field_deformer::computeXField(std::vector< Eigen::Matrix<double,3,2> > & XF)
{
  using namespace Eigen;
  Matrix<double,3,3> P,PP,DG;
	XF.resize(F.rows());

  for (int i=0;i<F.rows();i++)
	{
		int i0,i1,i2;
		// indexes of vertices of face i
		i0 = F(i,0); i1 = F(i,1); i2 = F(i,2);

		// input frame
		P.col(0) = (V.row(i1)-V.row(i0)).transpose();
		P.col(1) = (V.row(i2)-V.row(i0)).transpose();
		P.col(2) = P.col(0).cross(P.col(1));

		// output triangle brought to origin
		PP.col(0) = (V_w.row(i1)-V_w.row(i0)).transpose();
		PP.col(1) = (V_w.row(i2)-V_w.row(i0)).transpose();
		PP.col(2) = PP.col(0).cross(PP.col(1));

		// deformation gradient
		DG = PP * P.inverse();
		XF[i] = DG * FF[i];
	}
}

// computes in WW the ideal warp at each tri to make the frame field a cross
  IGL_INLINE void Frame_field_deformer::compute_idealWarp(std::vector< Eigen::Matrix<double,3,3> > & WW)
{
  using namespace Eigen;

  WW.resize(F.rows());
	for (int i=0;i<(int)FF.size();i++)
	{
		Vector3d v0,v1,v2;
		v0 = FF[i].col(0);
		v1 = FF[i].col(1);
		v2=v0.cross(v1); v2.normalize();			// normal

		Matrix3d A,AI;								// compute affine map A that brings:
		A <<    v0[0], v1[0], v2[0],				//	first vector of FF to x unary vector
    v0[1], v1[1], v2[1],				//	second vector of FF to xy plane
    v0[2], v1[2], v2[2];				//	triangle normal to z unary vector
		AI = A.inverse();

		// polar decomposition to discard rotational component (unnecessary but makes it easier)
		Eigen::JacobiSVD<Matrix<double,3,3> > svd(AI, Eigen::ComputeFullU | Eigen::ComputeFullV );
		//Matrix<double,3,3>  au = svd.matrixU();
		Matrix<double,3,3>  av = svd.matrixV();
		DiagonalMatrix<double,3>	as(svd.singularValues());
		WW[i] = av*as*av.transpose();
	}
}

}

IGL_INLINE void igl::frame_field_deformer(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::MatrixXd& FF1,
  const Eigen::MatrixXd& FF2,
  Eigen::MatrixXd&       V_d,
  Eigen::MatrixXd&       FF1_d,
  Eigen::MatrixXd&       FF2_d,
  const int              iterations,
  const double           lambda,
  const bool             perturb_initial_guess)
{
  using namespace Eigen;
  // Solvers
  Frame_field_deformer deformer;

  // Init optimizer
  deformer.init(V, F, FF1, FF2, lambda, perturb_initial_guess ? 0.1 : 0);

  // Optimize
  deformer.optimize(iterations,true);

  // Copy positions
  V_d = deformer.V_w;

  // Allocate
  FF1_d.resize(F.rows(),3);
  FF2_d.resize(F.rows(),3);

  // Copy frame field
  for(unsigned i=0; i<deformer.XF.size(); ++i)
  {
    FF1_d.row(i) = deformer.XF[i].col(0);
    FF2_d.row(i) = deformer.XF[i].col(1);
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
#endif


#ifndef GRID_H_
#define GRID_H_

//#include <Python.h>
#include <random>
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <boost/functional/hash.hpp>

#include "config.h"
#include "Game.h"
#include "pole.h"
#include "drive.h"
#include "net/network.hpp"
#include "computation_graph.hpp"

using namespace net;

typedef std::unordered_map<int, std::pair<double,double>> partial_states; // conjunction of ranges
typedef std::vector<partial_states> unsafes;  // disjunction of conjunctions of ranges
typedef std::vector<std::pair<double,double>> whole_states;

template < typename SEQUENCE > struct seq_hash
{
    std::size_t operator() ( const SEQUENCE& seq ) const
    {
        std::size_t hash = 0 ;
        boost::hash_range( hash, seq.begin(), seq.end() ) ;
        return hash ;
    }
};

template < typename SEQUENCE, typename T >
using sequence_to_data_map = std::unordered_map< SEQUENCE, T, seq_hash<SEQUENCE> > ;

int binary_search(std::vector<double> v, double data) {
    auto it = std::upper_bound(v.begin(), v.end(), data);
    if (it == v.begin()) {
    	return 0;
    } else if (it == v.end()) {
    	return v.size()-2;
    } else {
    	std::size_t index = std::distance(v.begin(), it);
        return index-1;
    }
}

class Grid {// abstract a world into grids
private:
	void enumerateStateConfiguration (int i, std::vector<std::vector<int>>& stateconfigs) {
		std::cout << "enumerateStateConfiguration i = " << i << "\n";
		if (i == dimensions.size() - 1) {
			for (int k = 0; k < dimensions[i].size() - 1; k++) {
				std::vector<int> nv;
				nv.push_back(k);
				stateconfigs.push_back(nv);
			}
			return;
		}

		enumerateStateConfiguration (i+1, stateconfigs);

		int n = stateconfigs.size();
		for (int j = 0; j < n; j++) {
			stateconfigs[j].insert (stateconfigs[j].begin(), 0);
		}
		for (int k = 1; k < dimensions[i].size() - 1; k++) {
			for (int j = 0; j < n; j++) {
				std::vector<int> nv;
				nv.insert(nv.begin(), stateconfigs[j].begin(), stateconfigs[j].end());
				nv[0] = k;
				stateconfigs.push_back(nv);
			}
		}
	}

	bool empty_range (std::pair<double,double> arg1, std::pair<double,double> arg2) {
		std::pair<double,double> intersection = { std::max(arg1.first, arg2.first), std::min(arg1.second, arg2.second) };
		return (intersection.second <= intersection.first);
	}

	bool check_range (whole_states st, whole_states pst) {
		for (int i = 0; i < st.size(); i++) {
			if (empty_range (st[i], pst[i]))
				return false;
		}
		return true;
	}

	bool check_range (whole_states st, partial_states pst) {
		for (int i = 0; i < st.size(); i++) {
			if (pst.find(i) != pst.end() && empty_range (st[i], pst[i]))
				return false;
		}
		return true;
	}

	bool check_range (whole_states st, unsafes usfs) {
		for (int i = 0; i < usfs.size(); i++) {
			if (check_range (st, usfs[i]))
				return true;
		}
		return false;
	}

	std::vector<double> stepGame (std::vector<double> gamestate) {
		game->setGameState (gamestate);

		/*Vector vec;
        vec.resize (game->inputs());
        for (int i = 0; i < game->inputs(); i++) {
        	vec[i] = gamestate[i];
        }
        const auto& result = graph->forward(vec);
        // greedy algorithm that generates the next action.
        int row, col;
        result.maxCoeff(&row,&col);
        game->step (row);*/

		double x = gamestate[0];
		double gamma = gamestate[1];
		int row;
  		if (x <= -1.21205854416)
    		row = 0;
  		else
    		if (gamma <= 0.137111499906)
      			row = 2;
    		else
      			if (x <= 0.387143492699)
        			row = 0;
      			else
        			row = 1;
        game->step(row);
        return game->getGameState();
	}

public:
	// e.g. x  : -0.1, 0, 0.1 yields (-0.1, 0), (0, 0.1)
	// e.g. dx : -0.1, 0, 0.1 yields (-0.1, 0), (0, 0.1)
	// e.g. t  : -0.1, 0, 0.1 yields (-0.1, 0), (0, 0.1)
	std::vector<std::vector<double>> dimensions;

	std::unordered_map<int, std::vector<int>> index_to_coords;
	sequence_to_data_map<std::vector<int>, int> coords_to_index;

	std::vector<int> error_states;
	std::unordered_set<int> error_set;
	std::vector<int> init_states;

	// Number of states
	int S;
	// Number of actions
	int A;
	// Pointer to a game instance
	Game* game;
	// Pointer to a neurual network
	ComputationGraph* graph;

	Grid (Game& game, ComputationGraph& graph,
			std::vector<std::vector<double>>& dimensions, whole_states& init_zones, unsafes& error_zones) {
		(this->dimensions).insert((this->dimensions).end(), dimensions.begin(), dimensions.end());
		this->game = &game;
		this->graph = &graph;
		this->A = game.actions();

		std::vector<std::vector<int>> stateconfigs;
		std::cout << "Discretizing state space ...\n";
		enumerateStateConfiguration (0, stateconfigs);
		std::cout << "State space discretized\n";
		S = 0;
		for (int i = 0; i < stateconfigs.size(); i++) {
			std::cout << "(";
			for (int ind : stateconfigs[i])
				std::cout << ind << " ";
			std::cout << ") is mapped to " << i << "\n";
			index_to_coords.insert (std::make_pair (S, stateconfigs[i]));
			coords_to_index.insert (std::make_pair (stateconfigs[i], S));
			S ++;

			whole_states state;
			for (int j = 0; j < stateconfigs[i].size(); j++) {
				double start = dimensions[j][stateconfigs[i][j]];
				double end = dimensions[j][stateconfigs[i][j]+1];
				state.push_back(std::make_pair(start,end));
			}
			if (check_range(state, error_zones)) {
				error_states.push_back(i);
				error_set.insert(i);
			}
			if (check_range(state, init_zones)) {
				init_states.push_back(i);
			}
		}
	}

	// An observation is converted to the state of our world.
	// e.g. (x:-002, dx:0.05, t : 0.1)
	int observation_to_index (std::vector<double> observation) {
		//std::cout << "Transitioning to (";
		//for (int j = 0; j < observation.size(); j++) {
		//	std::cout << observation[j];
		//}
		//std::cout << ")\n";

		std::vector<int> coord;
		for (int i = 0; i < observation.size(); i++) {
			double v = observation[i];
			int ind = binary_search (dimensions[i], v);
			coord.push_back(ind);
		}

		//std::cout << "Transitioning to in coords (";
		//for (int j = 0; j < coord.size(); j++) {
		//	std::cout << coord[j];
		//}
		//std::cout << ")\n";

		int index = coords_to_index[coord];
		return index;
	}

	// A state of our world is mapped back to a range of possible observations.
	std::vector<int> index_to_observation_range (int state) {
		return index_to_coords[state];
	}

	// Build the tranision relations of states in our world.
	void build_transitions (int n_samples, std::unordered_map<int, std::unordered_map<int, double>>& transitionCounts) {
		std::cout << "Building probablisitic transitions about all states\n";
		for (int i = 0; i < S; i++) {
			if (error_set.find(i) != error_set.end()) // No tranisition exploration for unsafe states.
				continue;
			std::vector<int> ranges = index_to_observation_range(i);
			//std::cout << "Transitioning from (";
			//for (int j = 0; j < ranges.size(); j++) {
			//	std::cout << ranges[j];
			//}
			//std::cout << ")\n";
			int iteri = 0;
			unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    		std::mt19937 generator (seed);
    		std::uniform_real_distribution<double> uniform01(0.0, 1.0);
    		std::vector<std::vector<double>> points;
			while (iteri < n_samples) {
				// Sample a point within the range of intersted.
				std::vector<double> point;
				for (int j = 0; j < ranges.size(); j++) {
					double start = dimensions[j][ranges[j]];
					double end = dimensions[j][ranges[j]+1];

					double sv =  (end - start) * uniform01(generator) + start;
					point.push_back(sv);
				}
				//std::cout << "Samping a value in range (";
				//	for (int j = 0; j < point.size(); j++) {
				//	std::cout << point[j];
				//}
				//std::cout << ")\n";
				points.push_back(point);
				iteri++;
			}
			std::unordered_map<int, double> currTransitionCount;
			for (int j = 0; j < n_samples; j++) {
				int s = observation_to_index(stepGame(points[j]));
				//std::cout << "sampled a tranision from " << i << " to " << s << "\n";
				currTransitionCount[s]++;
			}
			for( auto& n : currTransitionCount ) {
        		n.second /= n_samples;
    		}
    		transitionCounts[i] = currTransitionCount;
		}
	}

	void outputTraisitionCounts (std::unordered_map<int, std::unordered_map<int, double>>& transitionCounts) {
		std::string datafile = "./data/optimal_policy";
		std::fstream evl(datafile, std::fstream::out);
		for (int i = 0; i < S; i++) {
			for (int j = 0; j < S; j++) {
				if (transitionCounts[i].find(j) != transitionCounts[i].end())
					evl << i << " " << j << " " << transitionCounts[i][j] << "\n";
				else
					evl << i << " " << j << " 0\n";
			}
		}
		evl.close();
	}

	void outputInitial () {
		std::string datafile = "./data/start";
		std::fstream evl(datafile, std::fstream::out);
		for (int i = 0; i < init_states.size(); i++)
			evl << init_states[i] << "\n";
		evl.close();
	}

	void outputUnsafe () {
		std::string datafile = "./data/unsafe";
		std::fstream evl(datafile, std::fstream::out);
		for (int i = 0; i < error_states.size(); i++)
			evl << error_states[i] << "\n";
		evl.close();
	}

	void createSingleInitial (std::unordered_map<int, std::unordered_map<int, double>>& transitionCounts) {
		std::unordered_map<int, double> currTransitionCount;
		for (int i = 0; i < init_states.size(); i++) {
			currTransitionCount[init_states[i]] = ((double)1.0 / (init_states.size()));
		}
		transitionCounts[S] = currTransitionCount;
		S++;
	}

	void createSingleFinal (std::unordered_map<int, std::unordered_map<int, double>>& transitionCounts) {
		for (int i = 0; i < error_states.size(); i++) {
			std::unordered_map<int, double> currTransitionCount;
			currTransitionCount[S] = 1.0;
			transitionCounts[error_states[i]] = currTransitionCount;
		}
		std::unordered_map<int, double> currTransitionCount;
		currTransitionCount[S] = 1.0;
		transitionCounts[S] = currTransitionCount;
		S++;
	}

	void outputStateSpace () {
		std::string datafile = "./data/state_space";
		std::fstream evl(datafile, std::fstream::out);
		evl << "states\n";
		evl << S << "\n";
		evl << "actions\n";
		evl << A;
		evl.close();
	}

	// Verify the safety of our world.
	double verify (int n_samples, int steps) {
		std::cout << "Verify generated probablisitic system\n";
		outputInitial ();
		outputUnsafe ();

		std::unordered_map<int, std::unordered_map<int, double>> transitionCounts;
		build_transitions (n_samples, transitionCounts);
		// Create a single intial state and final state
		createSingleInitial(transitionCounts);
		createSingleFinal(transitionCounts);
		// export policy from transitionCounts
		outputTraisitionCounts (transitionCounts);

		// Output the state space.
		outputStateSpace();

		double verification_result = 0.0;

		// call prism to construct a probablisitic tranision system.
		/*setenv("PYTHONPATH", ".", 1);
		Py_Initialize();

		PyObject *pName = PyString_FromString("prism");
    	PyObject *pModule = PyImport_Import(pName);
    	Py_DECREF(pName);

    	if (pModule != NULL) {
        	PyObject *pFunc = PyObject_GetAttrString(pModule, "model_check");
        	if (pFunc && PyCallable_Check(pFunc)) {
        		PyObject *pArgs = PyTuple_New(2);
        		PyObject *pValue1 = PyInt_FromLong(steps);
        		PyObject *pValue2 = PyInt_FromLong(S);
        		PyTuple_SetItem(pArgs, 0, pValue1);
        		PyTuple_SetItem(pArgs, 1, pValue2);

            	PyObject *pValue = PyObject_CallObject(pFunc, pArgs);
            	Py_DECREF(pArgs);
            	if (pValue != NULL) {
            		verification_result = PyFloat_AsDouble(pValue);
                	printf("Result of call: %lf\n", verification_result);
                	Py_DECREF(pValue);
            	}
            	else {
                	Py_DECREF(pFunc);
                	Py_DECREF(pModule);
                	PyErr_Print();
                	fprintf(stderr,"Call Prism model_check failed\n");
                	return 0;
            	}
        	} else {
            	if (PyErr_Occurred())
                	PyErr_Print();
            	fprintf(stderr, "Cannot find model_check function\n");
            	return 0;
        	}
        	Py_XDECREF(pFunc);
        	Py_DECREF(pModule);
        } else {
        	PyErr_Print();
        	fprintf(stderr, "Failed to load prism\n");
        	return 0;
        }
        Py_Finalize();*/

        return verification_result;
	}

};

int main_verify(int argc, char** argv)
{
	std::string model(argv[1]);
	Network agent;
	agent.load (model);
	ComputationGraph graph(agent);
	//Pole pole;
	Drive pole;
	std::vector<std::vector<double>> dimensions;
	//double xd[] = {-1.2, -1.0, 0, 1.0, 1.2};
	//double xd[] = {-1.2,-1.0,-0.5,-0.4,-0.2,-0.000001,0.000001,0.2,0.4,0.5,1.0,1.2};
	// -- _that_is_within_safe_angle_ -- double xd[] = {-1.2,-1.0,-0.5,-0.4,-0.3,-0.2,-0.1,-0.000001,0.000001,1.2};
	// -- nice-abstraction - double xd[] = {-1.2,-1.0,-0.9,-0.7,-0.5,-0.3,-0.1,-0.000001,0.000001,1.2};
	//double xd[] = {-1.2,-1.0,-0.9,-0.7,-0.5,-0.3,-0.1,-0.000001,0.000001,1.2};
	double xd[] = {-2.2,-2.0,-1.9,-1.8,-1.7,-1.6,-1.5,-1.4,-1.3,-1.21205854416,-1.1,-1.0,-0.9,-0.8,-0.7,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,-0.008,-0.006,-0.004,-0.002,0,0.002,0.004,0.006,0.008,0.1,0.2,0.387143492699,0.4,0.5,0.6,0.7,0.8,0.9,1.0,1.1,1.2,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2.0,2.2};
  	std::vector<double> dx (xd, xd + sizeof(xd) / sizeof(double));
  	//double dxd[] = {-1.0, -0.2, 0, 0.2, 1.0};
  	//double dxd[] = {-1.0,-0.09,-0.003,-0.002,-0.000001,0.000001,0.002,0.003,0.09,1.0};
  	// -- _that_is_within_safe_angle_ -- double dxd[] = {-1.0,-0.09,-0.005,-0.003,-0.002,-0.000001,0.000001,1.0};
  	// -- nice-abstraction - double dxd[] = {-0.1,-0.09,-0.009,-0.005,-0.003,-0.002,-0.000001,0.000001,0.1};
  	//double dxd[] = {-0.2,-0.1,-0.009,-0.005,-0.003,-0.002,-0.000001,0.000001,0.2};
  	double dxd[] = {-1.3,-1.2,-1.1,-1,-0.9,-0.8,-0.78539815,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,-0.008,-0.006,-0.004,-0.002,0,0.002,0.004,0.006,0.008,0.137111499906,0.2,0.3,0.4,0.5,0.6,0.78539815,0.8,0.9,1,1.1,1.2,1.3};
  	std::vector<double> ddx (dxd, dxd + sizeof(dxd) / sizeof(double));
  	//double thetad[] = {-1.0, -0.026179938765, 0, 0.026179938765, 1.0};
  	//double thetad[] = {-1.0,-0.026179938765,-0.005,-0.003,-0.002,-0.000001,0.000001,0.002,0.003,0.005,0.026179938765,1.0};
  	// -- _that_is_within_safe_angle_ -- double thetad[] = {-1.0,-0.026179938765,-0.01,-0.003,-0.002,-0.000001,0.000001,1.0};
  	// -- nice-abstraction - double thetad[] = {-0.1,-0.026179938765,-0.01,-0.003,-0.002,-0.000001,0.000001,0.1};
  	//*** double thetad[] = {-0.1,-0.026179938765,-0.01,-0.003,-0.002,-0.000001,0.000001,0.1};
  	//*** std::vector<double> dtheta (thetad, thetad + sizeof(thetad) / sizeof(double));
  	//double dthetad[] = {-1.0, -0.02, 0, 0.02, 1.0};
  	//double dthetad[] = {-1.0,-0.1,-0.05,-0.02,-0.01745329251,0.01745329251,0.02,0.05,0.1,1.0};
  	// -- _that_is_within_safe_angle_ -- double dthetad[] = {-1.0,-0.1,-0.05,-0.02,-0.01745329251,0.01745329251,1.0};
  	// -- nice-abstraction - double dthetad[] = {-0.1,-0.05,-0.02,-0.01745329251,0.01745329251,0.1};
  	// *** double dthetad[] = {-0.1,-0.05,-0.02,-0.01745329251,0.01745329251,0.1};
  	// *** std::vector<double> ddtheta (dthetad, dthetad + sizeof(dthetad) / sizeof(double));
	dimensions.push_back(dx);
	dimensions.push_back(ddx);
	//dimensions.push_back(dtheta);
	//dimensions.push_back(ddtheta);

	unsafes error_zones;
	//partial_states e1;
	//e1[0] = std::make_pair(-2.2, -2.0);
	//error_zones.push_back(e1);
	partial_states e2;
	e2[0] = std::make_pair(2.0, 2.2);
	error_zones.push_back(e2);
	//partial_states e1;
	//e1[2] = std::make_pair(-1.0, -0.026179938765);
	//error_zones.push_back(e1);
	//partial_states e2;
	//e2[2] = std::make_pair(0.026179938765, 1.0);
	//error_zones.push_back(e2);
	//partial_states e3;
	//e3[0] = std::make_pair(-1.2, -1.0);
	//error_zones.push_back(e3);
	//partial_states e4;
	//e4[0] = std::make_pair(1.0, 1.2);
	//error_zones.push_back(e4);

	whole_states init_zones;
	init_zones.push_back (std::make_pair(-1,1));
	init_zones.push_back (std::make_pair(-0.78539815,0.78539815));
	//init_zones.push_back (std::make_pair(-0.000001,0.000001));
	//init_zones.push_back (std::make_pair(-0.000001,0.000001));
	//init_zones.push_back (std::make_pair(-0.000001,0.000001));
	//init_zones.push_back (std::make_pair(-0.01745329251,0.01745329251));

	Grid g(pole, graph, dimensions, init_zones, error_zones);
	g.verify (1000, 200);
	return 0;
}

#endif /* GRID_H_ */


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Qbit_.h"

#include "ParsQbit.h"

#include <boost/bind.hpp>

using namespace mathutils; using std::make_shared;

namespace qbit {

///////////
//
// Averaged
//
///////////

Averaged::Averaged()
  : Base(keyTitle,{"rho00","rho11","real(rho10=<sigma>)","imag(\")"})
{
}

const Averaged::Averages Averaged::average_v(NoTime, const LazyDensityOperator& matrix) const
{
  auto averages(initializedAverages());
  averages=matrix(0),matrix(1),real(matrix(1)(0)),imag(matrix(1)(0));
  return averages;
}

namespace {

void sigmaJump(qbit::StateVectorLow& psi, double gamma_perpendicular)
{
  psi(0)=sqrt(2.*gamma_perpendicular)*psi(1);
  psi(1)=0;
}

void sigma_zJump(qbit::StateVectorLow& psi, double gamma_parallel)
{
  double fact=sqrt(2.*gamma_parallel);
  psi(0)*= fact; psi(1)*=-fact;
}

double dummyProba(const qbit::LazyDensityOperator&)
{
  return -1;
}

}

LiouvilleanPhaseNoise::LiouvilleanPhaseNoise(double gamma_perpendicular, double gamma_parallel)
  : structure::ElementLiouvilleanStrategies<1,2>(JumpStrategies(bind(sigmaJump  ,_1,gamma_perpendicular),
                                                                bind(sigma_zJump,_1,gamma_parallel)),
                                                 JumpRateStrategies(dummyProba),
                                                 "LossyQbitWithPhaseNoise",{"excitation loss","phase noise"})
{}

} // qbit

////////////////
//
// Highest level
//
////////////////

QbitBase::QbitBase(const RealFreqs& realFreqs, const ComplexFreqs& complexFreqs)
  : ModeBase(2,realFreqs,complexFreqs,qbit::keyTitle), Averaged()
{}

#define TUPLE_delta(ISIP) RF{"delta",-imag(BOOST_PP_IF(ISIP,get_zI,get_zSch)()),1}
#define TUPLE_eta CF{"eta",get_eta(),1}
#define TUPLE_gammadelta(ISIP) CF{"(gamma,delta)",conj(BOOST_PP_IF(ISIP,get_zI,get_zSch)()),1}
#define TUPLE_gamma RF{"gamma",real(get_zSch()),1}

Qbit::Qbit(const qbit::Pars& p)
  : Exact(dcomp(0,-p.delta)),
    QbitBase{TUPLE_delta(1)}
{}

QbitSch::QbitSch(const qbit::Pars& p)
  : qbit::Hamiltonian<false>(dcomp(0,-p.delta),0),
    QbitBase{TUPLE_delta(0)}
{
  getParsStream()<<"Schroedinger picture.\n";
}

PumpedQbit::PumpedQbit(const qbit::ParsPumped& p)
  : qbit::Hamiltonian<true>(0,dcomp(0,-p.delta),p.eta),
    QbitBase(TUPLE_delta(1),TUPLE_eta)
{
  getParsStream()<<"Pumped.\n";
}

PumpedQbitSch::PumpedQbitSch(const qbit::ParsPumped& p)
  : qbit::Hamiltonian<false>(dcomp(0,-p.delta),p.eta),
    QbitBase(TUPLE_delta(0),TUPLE_eta)
{
  getParsStream()<<"Pumped, Schroedinger picture.\n";
}

LossyQbit::LossyQbit(double delta, double gamma)
  : qbit::Liouvillean(gamma),
    Exact(dcomp(gamma,-delta)),
    QbitBase{TUPLE_gammadelta(1)}
{
  getParsStream()<<"Lossy.\n";
}

LossyQbitSch::LossyQbitSch(double delta, double gamma)
  : qbit::Liouvillean(gamma),
    qbit::Hamiltonian<false>(dcomp(gamma,-delta),0),
    QbitBase{TUPLE_gamma,TUPLE_delta(0)}
{
  getParsStream()<<"Lossy, Schroedinger picture.\n";
}

LossyQbitUIP::LossyQbitUIP(double delta, double gamma)
  : qbit::Liouvillean(gamma),
    qbit::Hamiltonian<true>(dcomp(gamma,0),dcomp(0,-delta),0),
    QbitBase{TUPLE_gamma,TUPLE_delta(1)}
{
  getParsStream()<<"Lossy, Unitary interaction picture.\n";
}

PumpedLossyQbit::PumpedLossyQbit(const qbit::ParsPumpedLossy& p)
  : qbit::Liouvillean(p.gamma),
    qbit::Hamiltonian<true>(0,dcomp(p.gamma,-p.delta),p.eta),
    QbitBase{TUPLE_gammadelta(1),TUPLE_eta}
{
  getParsStream()<<"PumpedLossy.\n";
}

PumpedLossyQbitUIP::PumpedLossyQbitUIP(const qbit::ParsPumpedLossy& p)
  : qbit::Liouvillean(p.gamma),
    qbit::Hamiltonian<true>(dcomp(p.gamma,0),dcomp(0,-p.delta),p.eta),
    QbitBase({TUPLE_gamma,TUPLE_delta(1)},TUPLE_eta)
{
  getParsStream()<<"PumpedLossy, Unitary interaction picture.\n";
}

PumpedLossyQbitSch::PumpedLossyQbitSch(const qbit::ParsPumpedLossy& p)
  : qbit::Liouvillean(p.gamma),
    qbit::Hamiltonian<false>(dcomp(p.gamma,-p.delta),p.eta),
    QbitBase{TUPLE_gammadelta(0),TUPLE_eta}
{
  getParsStream()<<"PumpedLossy, Schroedinger picture.\n";
}

LossyQbitWithPhaseNoise::LossyQbitWithPhaseNoise(double delta, double gamma, double gamma_parallel)
  : Exact(dcomp(gamma,-delta)), // gamma_parallel does not contribute to the Hamiltonian
    qbit::LiouvilleanPhaseNoise(gamma,gamma_parallel),
    QbitBase(RF{"gamma_parallel",gamma_parallel,1},TUPLE_gammadelta(1))
{
  getParsStream()<<"LossyWithPhaseNoise.\n";
}

LossyQbitWithPhaseNoiseUIP::LossyQbitWithPhaseNoiseUIP(double delta, double gamma, double gamma_parallel)
  : qbit::Hamiltonian<true>(dcomp(gamma,0),dcomp(0,-delta),0),
    qbit::LiouvilleanPhaseNoise(gamma,gamma_parallel),
    QbitBase{RF{"gamma_parallel",gamma_parallel,1},TUPLE_gamma,TUPLE_delta(1)}
{
  getParsStream()<<"LossyWithPhaseNoise, Unitary interaction picture.\n";
}

PumpedLossyQbitWithPhaseNoise::PumpedLossyQbitWithPhaseNoise(const qbit::ParsPumpedLossyPhaseNoise& p)
  : qbit::Hamiltonian<true>(0,dcomp(p.gamma,-p.delta),p.eta),
    qbit::LiouvilleanPhaseNoise(p.gamma,p.gamma_parallel),
    QbitBase{RF{"gamma_parallel",p.gamma_parallel,1},TUPLE_gammadelta(1),TUPLE_eta}
{
  getParsStream()<<"PumpedLossyWithPhaseNoise.\n";
}

PumpedLossyQbitWithPhaseNoiseUIP::PumpedLossyQbitWithPhaseNoiseUIP(const qbit::ParsPumpedLossyPhaseNoise& p)
  : qbit::Hamiltonian<true>(dcomp(p.gamma,0),dcomp(0,-p.delta),p.eta),
    qbit::LiouvilleanPhaseNoise(p.gamma,p.gamma_parallel),
    QbitBase{RF{"gamma_parallel",p.gamma_parallel,1},TUPLE_gamma,TUPLE_delta(1)}
{
  getParsStream()<<"PumpedLossyWithPhaseNoise, Unitary interaction picture.\n";
}

#undef  TUPLE_gamma
#undef  TUPLE_gammadelta
#undef  TUPLE_eta
#undef  TUPLE_delta

//////////
//
// Helpers
//
//////////

namespace qbit {

Ptr make(const ParsPumpedLossy& p, QM_Picture qmp)
{
  switch (qmp) {
  case QMP_IP  :
    if (p.gamma==0 && std::abs(p.eta)==0)
      return std::make_shared<Qbit            >(p);
    if (p.gamma==0)
      return std::make_shared<PumpedQbit      >(p);
    if (std::abs(p.eta)==0)
      return std::make_shared<LossyQbit       >(p);
    return std::make_shared<PumpedLossyQbit   >(p);
  case QMP_UIP :
    if (p.gamma==0 && std::abs(p.eta)==0)
      return std::make_shared<QbitUIP         >(p);
    if (p.gamma==0)
      return std::make_shared<PumpedQbitUIP   >(p);
    if (std::abs(p.eta)==0)
      return std::make_shared<LossyQbitUIP    >(p);
    return std::make_shared<PumpedLossyQbitUIP>(p);
  case QMP_SCH :
    ;
  }
  if (p.gamma==0 && std::abs(p.eta)==0)
    return std::make_shared<QbitSch         >(p);
  if (p.gamma==0)
    return std::make_shared<PumpedQbitSch   >(p);
  if (std::abs(p.eta)==0)
    return std::make_shared<LossyQbitSch    >(p);
  return std::make_shared<PumpedLossyQbitSch>(p);
}

Ptr make(const ParsPumpedLossyPhaseNoise& p, QM_Picture qmp)
{
  if (p.gamma_parallel) {
    if (qmp==QMP_UIP) {
      if (std::abs(p.eta))
        return std::make_shared<PumpedLossyQbitWithPhaseNoiseUIP>(p);
      else
        return std::make_shared<      LossyQbitWithPhaseNoiseUIP>(p);
    }
    else {
      if (std::abs(p.eta))
        return std::make_shared<PumpedLossyQbitWithPhaseNoise>(p);
      else
        return std::make_shared<      LossyQbitWithPhaseNoise>(p);
    }
  }
  return make(static_cast<const ParsPumpedLossy&>(p),qmp);
}

const Tridiagonal sigmadagsigmaop()
{
  return mode::nop(make_shared<QbitBase>());
}

StateVector init(dcomp psi1)
{
  StateVector res(2);
  res(0)=sqrt(1-sqrAbs(psi1)); res(1)=psi1;
  return res;
}

} // qbit


#include <scomplex/simplicial_complex.hpp>
#include <scomplex/types.hpp>

#include <iterator>  // for debuging purposes

#include <gudhi/Hasse_complex.h>
#include <gudhi/Simplex_tree.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <cmath>
#include <functional>
#include <memory>  // smart pointers
#include <tuple>

#include <iostream>
namespace gsimp {

// hasse diagram structures
struct hasse_node {
    std::pair< int, size_t > handle;
    std::vector< std::shared_ptr< hasse_node > > cofaces;

    hasse_node(std::pair< int, size_t > _handle) { handle = _handle; }

    void add_coface(std::shared_ptr< hasse_node >& node) {
        cofaces.push_back(node);
    }
};

struct hasse_diag {
    std::vector< std::vector< std::shared_ptr< hasse_node > > > cells;
    std::vector< std::vector< bool > > cells_c;

    hasse_diag(){};

    hasse_diag(simplicial_complex& s_comp) {
        // vector of dimension d
        for (int d = 0; d <= s_comp.dimension(); d++) {
            size_t lv_s = s_comp.get_level_size(d);
            std::vector< std::shared_ptr< hasse_node > > lv(lv_s);
            cells.push_back(lv);

            std::vector< bool > c(lv_s, false);
            cells_c.push_back(c);
        }

        for (int d = s_comp.dimension(); d > 0; d--) {
            for (size_t face_i = 0; face_i < s_comp.get_level_size(d);
                 face_i++) {
                std::shared_ptr< hasse_node > face_ptr =
                    get_face(d, face_i, true);

                std::vector< std::pair< int, size_t > > faces =
                    s_comp.get_bdry_and_ind_index(d, face_i);
                for (auto b_face_i : faces) {
                    size_t b_face = std::get< 1 >(b_face_i);
                    std::shared_ptr< hasse_node > b_face_ptr =
                        get_face(d - 1, b_face, true);
                    b_face_ptr->add_coface(face_ptr);
                }
            }
        }
        // no further need, free up the space
        cells_c.clear();
    }

    std::shared_ptr< hasse_node > get_face(int d, size_t face_i,
                                           bool building = false) {
        std::shared_ptr< hasse_node > face;
        if (building && cells_c[d][face_i])
            face = cells[d][face_i];
        else {
            if (building) cells_c[d][face_i] = true;

            cells[d][face_i] = std::make_shared< hasse_node >(
                std::pair< int, size_t >(d, face_i));
            face = cells[d][face_i];
        }
        return face;
    }

    std::vector< size_t > get_coface_i(int d, size_t face_i) {
        auto node = cells[d][face_i];
        std::vector< size_t > cofaces;
        for (auto v : node->cofaces)
            cofaces.push_back(std::get< 1 >(v->handle));
        return cofaces;
    }
};

struct simplicial_complex::impl {
    // auxiliary types
    struct SimpleOptions : Gudhi::Simplex_tree_options_full_featured {
        typedef size_t Vertex_handle;
    };

    typedef Gudhi::Simplex_tree< SimpleOptions > simp_tree;
    typedef Gudhi::Simplex_tree< SimpleOptions >::Simplex_handle simp_handle;
    typedef std::vector< simp_handle* > level_t;
    typedef std::vector< std::unique_ptr< level_t > > levels_t;

    // member variables
    std::vector< point_t > points;
    simp_tree simplices;
    std::vector< matrix_t > boundary_matrices;
    levels_t levels;

    bool has_hasse;
    hasse_diag incidence;

    impl(std::vector< point_t >& arg_points, std::vector< cell_t >& arg_tris)
        : points(arg_points) {
        // create the simplex tree
        for (auto tri : arg_tris) {
            // removed deduping to try to make this a bit faster
            // ... it did cut time down about 10%, so ...
            simplices.insert_simplex_and_subfaces(tri);
            int d = tri.size() - 1;
            if (simplices.dimension() < d) simplices.set_dimension(d);
        }

        // assign a key to each simplex in each level
        std::vector< size_t > count(simplices.dimension() + 1, 0);
        for (int i = 0; i < simplices.dimension() + 1; ++i) {
            level_t* level = new level_t();
            levels.push_back(std::unique_ptr< level_t >(level));
        }

        auto simplex_range = simplices.complex_simplex_range();
        for (auto s : simplex_range) {
            int d = simplices.dimension(s);
            simplices.assign_key(s, count[d]++);
            levels[d]->push_back(new simp_handle(s));
        }
    }

    ~impl() {
        // get rid of the levels so they won't dangle
        for (int i = levels.size(); i <= 0; --i) levels[i].reset();
    };

    size_t get_level_size(int level) { return levels[level]->size(); }

    // calculate the index of s_1 in the boundary of s_2
    int boundary_index(simp_handle s_1, simp_handle s_2) {
        auto it_1 = simplices.simplex_vertex_range(s_1).begin();
        auto end_1 = simplices.simplex_vertex_range(s_1).end();
        //
        auto it_2 = simplices.simplex_vertex_range(s_2).begin();
        //
        int orient = 0;
        int unmatch = 0;
        //
        for (int p = 0; p <= simplices.dimension(s_2); p++) {
            if (*it_1 != *it_2) {
                orient = p++;
                unmatch++;
            } else {
                if (it_1 != end_1) it_1++;
                it_2++;
            }
        }
        return pow(-1, orient);
    }

    std::vector< size_t > dedupe_vec(std::vector< size_t >& vec) {
        std::set< size_t > no_reps;
        std::vector< size_t > no_reps_list;
        for (size_t el : vec) {
            no_reps.insert(el);
        }
        for (size_t el : no_reps) {
            no_reps_list.push_back(el);
        }
        return no_reps_list;
    }

    void calculate_matrices() {
        boundary_matrices = std::vector< matrix_t >();
        for (int k = 0; k < simplices.dimension(); k++) {
            boundary_matrices.push_back(
                matrix_t(get_level_size(k), get_level_size(k + 1)));
        }
        for (auto s : simplices.complex_simplex_range()) {
            int j = simplices.key(s);
            for (auto bs : simplices.boundary_simplex_range(s)) {
                int i = simplices.key(bs);
                int k = simplices.dimension(bs);
                boundary_matrices[k].coeffRef(i, j) = boundary_index(bs, s);
            }
        }
    }

    std::vector< cell_t > get_level(int level) {
        std::vector< cell_t > level_cells;
        for (auto simp : *levels[level]) {
            cell_t v_simp;
            for (auto v : simplices.simplex_vertex_range(*simp)) {
                v_simp.push_back(v);
            }
            level_cells.push_back(v_simp);
        }
        return level_cells;
    }

    simp_handle index_to_handle(int d, size_t tau) {
        return *(*(levels[d]))[tau];
    }

    size_t handle_to_index(simp_handle tau) { return simplices.key(tau); }

    simp_handle cell_to_handle(cell_t tau) {
        auto sh = simplices.find(tau);
        return sh;
    }

    cell_t handle_to_cell(simp_handle tau) {
        cell_t cell;
        for (auto v : simplices.simplex_vertex_range(tau)) cell.push_back(v);
        return cell;
    }

};  // struct impl

std::vector< std::pair< int, cell_t > > simplicial_complex::get_bdry_and_ind(
    cell_t cell) {
    impl::simp_handle simp = p_impl->cell_to_handle(cell);
    auto c_boundary = p_impl->simplices.boundary_simplex_range(simp);
    std::vector< std::pair< int, cell_t > > boundary_and_indices;
    for (auto face : c_boundary)
        boundary_and_indices.push_back(              //
            std::make_pair< int, cell_t >(           //
                p_impl->boundary_index(face, simp),  //
                p_impl->handle_to_cell(face)));      //
    return boundary_and_indices;
};

std::vector< std::pair< int, size_t > >
simplicial_complex::get_bdry_and_ind_index(int d, size_t cell) {
    impl::simp_handle simp = p_impl->index_to_handle(d, cell);
    auto c_boundary = p_impl->simplices.boundary_simplex_range(simp);
    std::vector< std::pair< int, size_t > > boundary_and_indices;
    for (auto face : c_boundary)
        boundary_and_indices.push_back(              //
            std::make_pair< int, size_t >(           //
                p_impl->boundary_index(face, simp),  //
                p_impl->handle_to_index(face)));     //
    return boundary_and_indices;
};

std::vector< size_t > simplicial_complex::cell_boundary_index(int d,
                                                              size_t cell) {
    impl::simp_handle simp = p_impl->index_to_handle(d, cell);
    auto c_boundary = p_impl->simplices.boundary_simplex_range(simp);
    std::vector< size_t > s_boundary;
    for (auto c : c_boundary) s_boundary.push_back(p_impl->handle_to_index(c));
    return s_boundary;
}

std::vector< cell_t > simplicial_complex::cell_boundary(cell_t cell) {
    impl::simp_handle simp = p_impl->cell_to_handle(cell);
    auto c_boundary = p_impl->simplices.boundary_simplex_range(simp);
    std::vector< cell_t > s_boundary;
    for (auto c : c_boundary) s_boundary.push_back(p_impl->handle_to_cell(c));
    return s_boundary;
}

int simplicial_complex::boundary_inclusion_index(cell_t c1, cell_t c2) {
    return p_impl->boundary_index(p_impl->cell_to_handle(c1),   //
                                  p_impl->cell_to_handle(c2));  //
};
int simplicial_complex::boundary_inclusion_index(int d1, size_t s1,    //
                                                 int d2, size_t s2) {  //
    cell_t c1 = index_to_cell(d1, s1);
    cell_t c2 = index_to_cell(d2, s2);
    return p_impl->boundary_index(p_impl->cell_to_handle(c1),
                                  p_impl->cell_to_handle(c2));
};

std::vector< std::pair< int, cell_t > > simplicial_complex::get_cof_and_ind(
    cell_t cell) {
    std::vector< std::pair< int, cell_t > > c_cofaces;
    for (auto face : get_cofaces(cell))
        c_cofaces.push_back(                                   //
            std::make_pair(                                    //
                boundary_inclusion_index(cell, face), face));  //
    return c_cofaces;
}

std::vector< std::pair< int, size_t > >
simplicial_complex::get_cof_and_ind_index(int d, size_t c) {
    std::vector< std::pair< int, size_t > > c_cofaces;
    for (auto face : get_cofaces_index(d, c))
        c_cofaces.push_back(                                          //
            std::make_pair(                                           //
                boundary_inclusion_index(d, c, d + 1, face), face));  //
    return c_cofaces;
}

int simplicial_complex::get_level_size(int level) {
    return p_impl->get_level_size(level);
}

simplicial_complex::simplicial_complex(std::vector< cell_t >& arg_tris) {
    std::vector< point_t > points = {};
    p_impl = std::make_shared< impl >(points, arg_tris);
}

simplicial_complex::simplicial_complex(std::vector< point_t >& arg_points,
                                       std::vector< cell_t >& arg_tris) {
    p_impl = std::make_shared< impl >(arg_points, arg_tris);
}

simplicial_complex::simplicial_complex(const simplicial_complex& other) {
    p_impl = other.p_impl;
}

simplicial_complex& simplicial_complex::operator=(
    const simplicial_complex& other) {
    p_impl = other.p_impl;
    return *this;
}

simplicial_complex::~simplicial_complex() {}

std::vector< point_t > simplicial_complex::get_points() {
    return p_impl->points;
}

point_t simplicial_complex::get_point(size_t index) {
    return p_impl->points[index];
}

std::vector< cell_t > simplicial_complex::get_level(int level) {
    return p_impl->get_level(level);
}

matrix_t simplicial_complex::get_boundary_matrix(int d) {
    // uninstantiated boundary matrices
    if (p_impl->boundary_matrices.size() == 0) p_impl->calculate_matrices();

    // now they have to be instantiated, get them
    if (0 <= d && d < p_impl->boundary_matrices.size())
        return p_impl->boundary_matrices[d];
    else
        throw No_Boundary();
}

int simplicial_complex::dimension() { return p_impl->simplices.dimension(); }

cell_t simplicial_complex::index_to_cell(int d, size_t ind) {
    auto sh = (*(p_impl->levels[d]))[ind];
    return p_impl->handle_to_cell(*sh);
}

size_t simplicial_complex::cell_to_index(cell_t simp) {
    auto sh = p_impl->simplices.find(simp);
    return p_impl->simplices.key(sh);
}

std::vector< size_t > simplicial_complex::get_cofaces_index(int d,
                                                            size_t face) {
    // codimension 1 faces
    if (!p_impl->has_hasse) {
        calculate_hasse();
        p_impl->has_hasse = true;
    }
    auto s_cofaces = p_impl->incidence.get_coface_i(d, face);
    return s_cofaces;
}

std::vector< cell_t > simplicial_complex::get_cofaces(cell_t face) {
    if (!p_impl->has_hasse) {
        calculate_hasse();
        p_impl->has_hasse = true;
    }
    std::vector< cell_t > s_cofaces;
    auto face_i = cell_to_index(face);
    int d = face.size() - 1;
    // codimension 1 faces
    auto coface_i_v = p_impl->incidence.get_coface_i(d, face_i);
    for (auto v : coface_i_v) {
        auto face_h = (*(p_impl->levels[d + 1]))[v];
        s_cofaces.push_back(p_impl->handle_to_cell(*face_h));
    }
    return s_cofaces;
}

chain_v simplicial_complex::new_v_chain(int d) {
    std::vector< double > v(get_level_size(d), 0);
    return chain_v(d, v);
}
chain_t simplicial_complex::new_chain(int d) {
    vector_t v(get_level_size(d));
    return chain_t(d, v);
}

void simplicial_complex::calculate_hasse() {
    p_impl->has_hasse = true;
    p_impl->incidence = hasse_diag(*this);
}
};  // namespace gsimp