/*
* @file         P-Tucker.cpp
* @main author  Sejoon Oh (sejun6431@gmail.com), Georgia Institute of Technology
* @author       Namyong Park (namyongp@cs.cmu.edu), Carnegie Mellon University
* @author       Lee Sael (saellee@gmail.com), Seoul National University
* @author       U Kang (ukang@snu.ac.kr), Seoul National University
* @version      1.8
* @date         2019-06-07
*
* Scalable Tucker Factorization for Sparse Tensors - Algorithms and Discoveries (ICDE 2018)
*
* This software is free of charge under research purposes.
* For commercial purposes, please contact the main author.
*
* Recent Updates:
	- Orthogonalizing factor matrices & updating core tensor have been added (default: off); uncomment Orthogonalize() if you want to use it.
	- base-1 indexing is not used anymore & output format is improved
	- Eigen library is used instead of Armadillo for generality
* Usage:
*   - make P-Tucker
    - ./P-Tucker [input_tensor_path] [result_directory_path] [tensor_order] [tensor_rank] [number of threads]
        e.g.) ./P-Tucker input.txt result/ 3 10 20;
*/

/////    Header files     /////

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <Eigen/QR>
#include <Eigen/Dense>
#include <omp.h>
using namespace std;
using namespace Eigen;
typedef Matrix<double, Dynamic, Dynamic> MatrixXdd; //Matrix format of Eigen library

#define lambda 0.001						// Default L2 regularization parameter
///////////////////////////////
char* InputPath;
char* ResultPath;
/////////////////////////////////                Variables                  /////////////////////////////////

//Related to input tensor
//Sparse tensor form : (i1, i2, ... , iN, value)
int order;										// Tensor order (e.g., 5)
int *dimensionality, max_dim;					// Tensor dimensionality (e.g., 100x100x100)
int Entries_N;									// Total number of observation entries
double *Entries;								// Containing all values of an input tensor
int *Indices;										// Containing all indices of an input tensor (Vectorized form)
int *WhereX, *CountX;					        // WhereX[n][I_n] contains all entries of a tensor X whose nth mode's index is I_n
double NormX;									// Norm of input tensor
int threadsN;									// Number of threads to be used

int Core_N = 1;									// Total number of nonzeros in a core tensor
int *Core_size, Core_dim;						// Core tensor dimensionality
double *CoreTensor;								// Containing all values of a core tensor
int *CorePermu;									// Containing all indices of a core tensor (Vectorized form)
double MaxCore;									// The maximum value of core tensor
int *WhereG, *CountG;							// WhereG[n][R_n] contains all entries of a core tensor G whose nth mode's index is R_n

double *FactorM; //Factor matrices in vectorized form

//Update-related variables
 //Please see the paper and supplementary material for details
double Fit, pFit = -1;
double *Error_T;
double Error, RMSE, MAE, Test_RMSE;
int *crows, rowcount;
double *tempCore;
int *Mul, *tempPermu;
//For-loop variables
int i, j, k, l;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

double frand(double x, double y) {//return the random value in (x,y) interval
	return ((y - x)*((double)rand() / RAND_MAX)) + x;
}

double abss(double x) {
	return x > 0 ? x : -x;
}

//[Input] Given tensor X
//[Output] Updated WhereX and CountX
//[Function] Assign all non-zeros to the corresponding rows, represented as WhereX and CountX
void assign_index() {
	int *tempX = (int *)malloc(sizeof(int)*max_dim*order);
	int pos = 0, i, j, k, l;
	for (i = 0; i < order; i++) {
		for (j = 0; j < dimensionality[i]; j++) {
			CountX[i*max_dim + j] = tempX[i*max_dim + j] = 0;
		}
	}
	for (i = 0; i < Entries_N; i++) {
		for (j = 0; j < order; j++) {
			k = Indices[pos++];
			CountX[j*max_dim + k]++;
			tempX[j*max_dim + k]++;
		}
	}
	pos = 0;
	int now = 0;
	for (i = 0; i < order; i++) {
		pos = i*max_dim;
		for (j = 0; j < dimensionality[i]; j++) {
			k = CountX[pos];
			CountX[pos] = now;
			tempX[pos++] = now;
			now += k;
		}
		CountX[pos] = now;
		tempX[pos] = now;
	}
	pos = 0;
	for (i = 0; i < Entries_N; i++) {
		for (j = 0; j < order; j++) {
			k = Indices[pos++];
			int now = tempX[j*max_dim + k];
			WhereX[now] = i;
			tempX[j*max_dim + k]++;
		}
	}
}
char tmp[10005];
//[Input] Metadata + input tensor as a sparse tensor format
//[Output] Initialized core tensor G and factor matrices A^{(n)} (n=1...N)
//[Function] Getting all information about an input tensor X / Initialize all factor matrices and core tensor.
void Getting_Input() {
	FILE *fin = fopen(InputPath, "r");
	FILE *fin2 = fopen(InputPath, "r");
	FILE *ftest = fopen(ResultPath, "r");

	double Timee = clock();

	printf("Reading Input...\n");

	dimensionality = (int *)malloc(sizeof(int)*order);
	Core_size = (int *)malloc(sizeof(int)*order);

	for (i = 0; i < order; i++) {
		dimensionality[i] = 0;
		Core_size[i] = Core_dim;
		Core_N *= Core_size[i];
	}

	int len = 0;
	while (fgets(tmp, 10005, fin)) {
		Entries_N++;
	}
	Indices = (int *)malloc(sizeof(int)*Entries_N*order);
	Entries = (double *)malloc(sizeof(double)*Entries_N);
	int pos = 0;
	for (i = 0; i < Entries_N; i++) {
		fgets(tmp, 10005, fin2);
		len = strlen(tmp);
		int k = 0, idx = 0, flag = 0, flag2 = 0;
		double mul = 0.1, val = 0;
		for (j = 0; j < len; j++) {
			if (tmp[j] == ' ' || tmp[j] == '\t') {
				Indices[pos++] = idx;
				if (dimensionality[k] < idx+1) dimensionality[k] = idx+1;
				idx = 0;
				k++;
			}
			else if (tmp[j] >= '0' && tmp[j] <= '9') {
				if (flag == 1) {
					val += mul*(tmp[j] - '0');
					mul /= 10;
				}
				else idx = idx * 10 + tmp[j] - '0';
			}
			else if (tmp[j] == '.') {
				val += idx;
				flag = 1;
			}
			else if (tmp[j] == '-') {
				flag2 = 1;
			}
		}
		if(flag==0) val = idx;
        if(flag2 == 0){
		    Entries[i] = val;
        }else{
            Entries[i] = -1 * val;
        }
		NormX += Entries[i] * Entries[i];
	}
	for (i = 0; i < order; i++) {
		if (max_dim < dimensionality[i]) max_dim = dimensionality[i];
	}
	max_dim++;
	WhereX = (int *)malloc(sizeof(int)*order*Entries_N);
	CountX = (int *)malloc(sizeof(int)*max_dim*order);

	NormX = sqrt(NormX);
	printf("Reading Done.\n\n[METADATA]\nTensor Order: %d\tSize: ", order);
	for (i = 0; i < order; i++) {
		if (i != order - 1) printf("%dx", dimensionality[i]);
		else printf("%d\t", dimensionality[i]);
	}
	printf("Rank: %d\tNNZ : %d\tThreads : %d\tNorm : %lf\n\nInitialize...\n", Core_dim, Entries_N, threadsN, NormX);

	//INITIALIZE
	assign_index();
	FactorM = (double *)malloc(sizeof(double)*order*max_dim*Core_dim);
	for (i = 0; i < order; i++) {
		int row = dimensionality[i], col = Core_size[i];
		for (j = 0; j < row; j++) {
			for (k = 0; k < col; k++) {
				FactorM[i*max_dim*col + j*col + k] = frand(0, 1);
			}
		}
	}

	CoreTensor = (double *)malloc(sizeof(double)*Core_N);
	CorePermu = (int *)malloc(sizeof(int)*Core_N*order);
	pos = 0;
	for (i = 0; i < Core_N; i++) {
		CoreTensor[i] = frand(0, 1);
		if (i == 0) {
			for (j = 0; j < order; j++) CorePermu[j] = 0;
		}
		else {
			for (j = 0; j < order; j++) {
				CorePermu[i*order + j] = CorePermu[(i - 1)*order + j];
			}
			CorePermu[i*order + order - 1]++;  k = order - 1;
			while (CorePermu[i*order + k] >= Core_size[k]) {
				CorePermu[i*order + k] -= Core_size[k];
				CorePermu[i*order + k - 1]++; k--;
			}
		}
	}
	printf("Elapsed Time for I/O and Initializations:\t%lf\n\n", (clock() - Timee) / CLOCKS_PER_SEC);
}

//[Input] Input tensor X, initialized core tensor G, and factor matrices A^{(n)} (n=1...N)
//[Output] Updated factor matrices A^{(n)} (n=1...N)
//[Function] Update all factor matrices by a gradient-based ALS.
void Update_Factor_Matrices() {

	int mult = max_dim*Core_dim;
	for (i = 0; i < order; i++) { //Updating the ith Factor Matrix
		int row_size = dimensionality[i];
		int column_size = Core_size[i];
		int iter;
		rowcount = 0;
		crows = (int *)malloc(sizeof(int)*row_size);
		for (j = 0; j < row_size; j++) { //extracting rows who have non-zeros
			int pos = i*max_dim + j, nnz = CountX[pos + 1] - CountX[pos];
			if (nnz != 0) {
				crows[rowcount++] = j;
			}
		}

        #pragma omp parallel for schedule(dynamic) //in parallel
		for (iter = 0; iter < rowcount; iter++) {
			int j, k, l, ii, jj;
			j = crows[iter]; //Indicating that we are updating the jth row of ith factor matrix
			double *Delta = (double *)malloc(sizeof(double)*column_size);
			double *B = (double *)malloc(sizeof(double)*column_size*column_size);
			double *C = (double *)malloc(sizeof(double)*column_size);

			//Initialize B and C
			int pos = 0;
			for (k = 0; k < column_size; k++) {
				for (l = 0; l < column_size; l++) {
					B[pos] = 0;
					if (k == l) B[pos] = lambda;
					pos++;
				}
				C[k] = 0;
			}

			pos = i*max_dim + j;
			int nnz = CountX[pos + 1] - CountX[pos];
			pos = CountX[pos];
			for (k = 0; k < nnz; k++) { //Updating Delta, B, and C
				int current_input_entry = WhereX[pos + k];
				int pre_val = current_input_entry*order;
				int *cach1 = (int *)malloc(sizeof(int)*order);
				for (l = 0; l < order; l++) cach1[l] = Indices[pre_val++];
				for (l = 0; l < column_size; l++) Delta[l] = 0;
				for (l = 0; l < Core_N; l++) {
					int pre1 = l*order, pre2 = 0;
					int CorePos = CorePermu[pre1 + i];
					double res = CoreTensor[l];
					for (ii = 0; ii < order; ii++) {
						if (ii != i) {
							int mulrow = cach1[ii], mulcol = CorePermu[pre1];
							res *= FactorM[pre2 + mulrow*column_size + mulcol];
						}
						pre1++;
						pre2 += mult;
					}
					Delta[CorePos] += res;
				}
				free(cach1);
				int now = 0;
				double Entry_val = Entries[current_input_entry];
				for (ii = 0; ii < column_size; ii++) {
					double cach = Delta[ii];
					for (jj = 0; jj < column_size; jj++) {
						B[now++] += cach * Delta[jj];
					}
					C[ii] += cach * Entry_val;
				}
			}
			free(Delta);
			//Getting the inverse matrix of [B+lambda*I]
			MatrixXdd A(column_size, column_size);
			pos = 0;
			for (k = 0; k < column_size; k++) {
				for (l = 0; l < column_size; l++) {
					A(k, l) = B[k*column_size + l];
				}
			}
			MatrixXdd BB = A.inverse();
			pos = 0;
			for (k = 0; k < column_size; k++) {
				for (l = 0; l < column_size; l++) {
					B[k*column_size + l] = BB(k, l);
				}
			}

			//Update the jth row of ith Factor Matrix
			int cach = i*mult + j*column_size;
			for (k = 0; k < column_size; k++) {
				double res = 0;
				for (l = 0; l < column_size; l++) {
					res += C[l] * B[l*column_size + k];
				}
				FactorM[cach + k] = res;
			}
            A.resize(0,0); BB.resize(0,0);
			free(B);
			free(C);
		}
		free(crows);
	}
}

//[Input] Input tensor X, core tensor G, and factor matrices A^{(n)} (n=1...N)
//[Output] Fit = 1-||X-X'||/||X|| (Reconstruction error = ||X-X'||)
//[Function] Calculating fit and reconstruction error in a parallel way.
void Reconstruction() {
	RMSE = Error = 0;
	Error_T = (double *)malloc(sizeof(double)*Entries_N);
#pragma omp parallel for schedule(static)
	for (i = 0; i < Entries_N; i++) {
		Error_T[i] = Entries[i];
	}
	int mult = max_dim*Core_dim;
#pragma omp parallel for schedule(static)
	for (i = 0; i < Entries_N; i++) {
		int j, pre_val = i*order;
		double ans = 0;
		int *cach1 = (int *)malloc(sizeof(int)*order);
		for (j = 0; j < order; j++) cach1[j] = Indices[pre_val++];
		for (j = 0; j < Core_N; j++) {
			double temp = CoreTensor[j];
			int k;
			int pos = j*order;
			int val = 0;
			for (k = 0; k < order; k++) {
				int mulrow = cach1[k], mulcol = CorePermu[pos++];
				temp *= FactorM[val + mulrow*Core_dim + mulcol];
				val += mult;
			}
			ans += temp;
		}
		free(cach1);
		Error_T[i] -= ans;
	}
#pragma omp parallel for schedule(static) reduction(+:Error)
	for (i = 0; i < Entries_N; i++) {
		Error += Error_T[i] * Error_T[i];
	}
	RMSE = sqrt(Error / Entries_N);
	if (NormX == 0) Fit = 1;
	else Fit = 1 - sqrt(Error) / NormX;
	free(Error_T);
}

//[Input] Updated factor matrices A^{(n)} (n=1...N)
//[Output] Orthonormal factor matrices A^{(n)} (n=1...N) and updated core tensor G
//[Function] Orthogonalize all factor matrices and update core tensor simultaneously.
void Orthogonalize() {
	Mul = (int *)malloc(sizeof(int)*order);
	Mul[order - 1] = 1;
	for (i = order - 2; i >= 0; i--) {
		Mul[i] = Mul[i + 1] * Core_size[i + 1];
	}
	int pos = 0;
	for (i = 0; i < order; i++) {
        MatrixXdd X(dimensionality[i], Core_size[i]), thinQ(MatrixXdd::Identity(dimensionality[i], Core_size[i]));
		for (k = 0; k < dimensionality[i]; k++) {
			for (l = 0; l < Core_size[i]; l++) {
				X(k, l) = FactorM[i*max_dim*Core_dim + k*Core_dim + l];
			}
		}
        HouseholderQR<MatrixXdd> qr(X);
        thinQ = qr.householderQ() * thinQ;

		for (k = 0; k < dimensionality[i]; k++) {
			for (l = 0; l < Core_size[i]; l++) {
				FactorM[i*max_dim*Core_dim + k*Core_dim + l] = thinQ(k, l);
			}
		}

        MatrixXdd R = thinQ.transpose()*X;
        tempCore = (double *)malloc(sizeof(double)*Core_N);
		tempPermu = (int *)malloc(sizeof(int)*order);
		for (j = 0; j < Core_N; j++) {
			tempCore[j] = 0;
		}
		for (j = 0; j < Core_N; j++) {
			for (k = 0; k <= i - 1; k++) {
				tempPermu[k] = CorePermu[j*order + k];
			}
			for (k = i + 1; k < order; k++) {
				tempPermu[k] = CorePermu[j*order + k];
			}
			for (k = 0; k < Core_size[i]; k++) {
				tempPermu[i] = k;
				int cur = j + (k - CorePermu[j*order + i])*Mul[i];
				tempCore[cur] += CoreTensor[j] * R(k, CorePermu[j*order + i]);
			}
		}
		for (j = 0; j < Core_N; j++) {
			CoreTensor[j] = tempCore[j];
		}
	}
}

//[Input] Input tensor X, initialized core tensor G, and initialized factor matrices A^{(n)} (n=1...N)
//[Output] Updated core tensor G and factor matrices A^{(n)} (n=1...N)
//[Function] Performing main algorithm which updates core tensor and factor matrices iteratively
void PTucker() {
	printf("P-Tucker START!\n\n");

	double Stime = omp_get_wtime();
	int iter = 0;
	double avertime = 0;

	while (1) {

		double itertime = omp_get_wtime(), steptime;
		steptime = itertime;

		printf("[Iteration %d]\n",++iter);

		Update_Factor_Matrices();
		printf("Elapsed Time for Updating Factor Matrices:\t%lf\n", omp_get_wtime() - steptime);
		steptime = omp_get_wtime();

		Reconstruction();
		printf("Elapsed Time for Calculating Recon. Error:\t%lf\n", omp_get_wtime() - steptime);
		steptime = omp_get_wtime();

		avertime += omp_get_wtime() - itertime;
		printf("Fit:\t%lf\nTraining RMSE:\t%lf\nElapsed Time:\t%lf\n\n", Fit, RMSE, omp_get_wtime() - itertime);

		if (iter>=20 || (pFit != -1 && abss(pFit - Fit) <= 0.0001)) break;
		pFit = Fit;
	}

	avertime /= iter;

    /*
    Orthogonalize();
    Reconstruction();
    */

	printf("\nMain iterations are ended.\tFinal Fit : %lf\tAverage iteration time : %lf\tTotal Elapsed time: %lf\n\n", Fit, avertime,omp_get_wtime()-Stime);

}

//[Input] Updated core tensor G and factor matrices A^{(n)} (n=1...N)
//[Output] core tensor G in sparse tensor format and factor matrices A^{(n)} (n=1...N) in full-dense matrix format
//[Function] Writing all factor matrices and core tensor in result path
void Print() {
	printf("\nWriting factor matrices and the core tensor...\n");
	char temp[10005];
	int pos = 0;
	int mult = max_dim*Core_dim;
	for (i = 0; i < order; i++) {
		sprintf(temp, "%s/FACTOR%d", ResultPath, i);
		FILE *fin = fopen(temp, "w");
		for (j = 0; j < dimensionality[i]; j++) {
			for (k = 0; k < Core_size[i]; k++) {
				if(k==Core_size[i]-1) fprintf(fin, "%e", FactorM[i*mult + j*Core_size[i] + k]);
				else fprintf(fin, "%e\t", FactorM[i*mult + j*Core_size[i] + k]);
			}
			fprintf(fin, "\n");
		}
	}
	sprintf(temp, "%s/CORETENSOR", ResultPath);
	FILE *fcore = fopen(temp, "w");
	pos = 0;
	for (i = 0; i < Core_N; i++) {
		for (j = 0; j < order; j++) {
		    fprintf(fcore, "%d\t", CorePermu[pos++]);
		}
		fprintf(fcore, "%e\n", CoreTensor[i]);
	}
}

//[Input] Path of input tensor file, result directory, tensor order, tensor rank, and number of threads
//[Output] Core tensor G and factor matrices A^{(n)} (n=1,...,N)
//[Function] Performing P-Tucker for a given sparse tensor
int main(int argc, char* argv[]) {

	if (argc == 6) {

		InputPath = argv[1];
		ResultPath = argv[2];
		order = atoi(argv[3]);
		Core_dim = atoi(argv[4]);
		threadsN = atoi(argv[5]);

		srand((unsigned)time(NULL));

		Getting_Input();

		PTucker();

		Print();

	}

	else printf("ERROR: Invalid Arguments\n\nUsage: ./P-Tucker [input_tensor_path] [result_directory_path] [tensor_order] [tensor_rank] [number of threads]\ne.g.) ./P-Tucker input.txt result/ 3 10 20\n");
	return 0;
}


#ifndef PARMCB_DETAIL_SIGNED_DIJKSTRA_HPP_
#define PARMCB_DETAIL_SIGNED_DIJKSTRA_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/scoped_array.hpp>
#include <boost/throw_exception.hpp>
#include <boost/functional/hash.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>

#include <parmcb/detail/util.hpp>

namespace std {

    std::ostream& operator <<(std::ostream &out, const std::pair<unsigned long int, bool> &v) {
        out << v.first << (v.second ? "+" : "-");
        return out;
    }

}

namespace parmcb {

    namespace detail {

        template<class Graph, class WeightMap>
        struct SignedDistanceFunctor {
            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::property_traits<WeightMap>::value_type DistanceType;
            typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
            typedef std::pair<Vertex, bool> SignedVertex;

            std::size_t n;
            std::vector<DistanceType> &dist;
            const VertexIndexMapType &index_map;

            SignedDistanceFunctor(std::size_t n, std::vector<DistanceType> &dist, const VertexIndexMapType &index_map) :
                    n(n), dist(dist), index_map(index_map) {
            }

            DistanceType& operator()(const SignedVertex &v) const {
                return dist.at(index_map[v.first] + (v.second ? 0 : n));
            }
        };

        template<class Graph>
        struct SignedPredecessorFunctor {
            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
            typedef std::pair<Vertex, bool> SignedVertex;
            typedef std::tuple<SignedVertex, bool, Edge> Predecessor;

            std::size_t n;
            std::vector<Predecessor> &pred;
            const VertexIndexMapType &index_map;

            SignedPredecessorFunctor(std::size_t n, std::vector<Predecessor> &pred, const VertexIndexMapType &index_map) :
                    n(n), pred(pred), index_map(index_map) {
            }

            Predecessor& operator()(const SignedVertex &v) const {
                return pred.at(index_map[v.first] + (v.second ? 0 : n));
            }
        };

        template<class Graph>
        struct SignedIndexInHeapFunctor {
            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
            typedef std::pair<Vertex, bool> SignedVertex;

            std::size_t n;
            std::vector<std::size_t> &index_in_heap;
            const VertexIndexMapType &index_map;

            SignedIndexInHeapFunctor(std::size_t n, std::vector<std::size_t> &index_in_heap,
                    const VertexIndexMapType &index_map) :
                    n(n), index_in_heap(index_in_heap), index_map(index_map) {
            }

            std::size_t& operator()(const SignedVertex &v) const {
                return index_in_heap.at(index_map[v.first] + (v.second ? 0 : n));
            }
        };

        template<class Graph, class WeightMap>
        struct search_frontier {
            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
            typedef typename boost::property_traits<WeightMap>::value_type WeightType;
            typedef typename boost::property_traits<WeightMap>::value_type DistanceType;
            typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
            typedef std::pair<Vertex, bool> SignedVertex;
            typedef std::tuple<SignedVertex, bool, Edge> Predecessor;
            typedef boost::d_ary_heap_indirect<SignedVertex, 4,
                    boost::function_property_map<parmcb::detail::SignedIndexInHeapFunctor<Graph>, SignedVertex,
                            std::size_t&>,
                    boost::function_property_map<parmcb::detail::SignedDistanceFunctor<Graph, WeightMap>, SignedVertex,
                            DistanceType&>, std::less<DistanceType>> VertexQueue;

            const Graph &g;
            const VertexIndexMapType index_map;
            std::vector<std::size_t> index_in_heap;
            std::vector<DistanceType> dist;
            std::vector<Predecessor> pred;
            boost::function_property_map<parmcb::detail::SignedIndexInHeapFunctor<Graph>, SignedVertex, std::size_t&> index_in_heap_map;
            boost::function_property_map<parmcb::detail::SignedDistanceFunctor<Graph, WeightMap>, SignedVertex,
                    DistanceType&> dist_map;
            boost::function_property_map<parmcb::detail::SignedPredecessorFunctor<Graph>, SignedVertex, Predecessor&> pred_map;
            std::less<DistanceType> compare;
            VertexQueue queue;
            SignedVertex source;

            search_frontier(const Graph &g) :
                    g(g), index_map(boost::get(boost::vertex_index, g)), index_in_heap(2 * boost::num_vertices(g)), dist(
                            2 * boost::num_vertices(g), (std::numeric_limits<DistanceType>::max)()), pred(
                            2 * boost::num_vertices(g), std::make_tuple(std::make_pair(Vertex(), true), false, Edge())), index_in_heap_map(
                            parmcb::detail::SignedIndexInHeapFunctor<Graph>(boost::num_vertices(g), index_in_heap,
                                    index_map)), dist_map(
                            parmcb::detail::SignedDistanceFunctor<Graph, WeightMap>(boost::num_vertices(g), dist,
                                    index_map)), pred_map(
                            parmcb::detail::SignedPredecessorFunctor<Graph>(boost::num_vertices(g), pred, index_map)), compare(), queue(
                            dist_map, index_in_heap_map, compare) {
            }

            SignedVertex poll() {
                SignedVertex u = queue.top();
                queue.pop();
                return u;
            }

            const DistanceType& find_min() {
                SignedVertex u = queue.top();
                return boost::get(dist_map, u);
            }

            bool has_finite_dist(const SignedVertex &u) {
                return u == source || std::get<1>(boost::get(pred_map, u));
            }

            const DistanceType& get_dist(const SignedVertex &u) {
                return boost::get(dist_map, u);
            }

            const Predecessor& get_pred(const SignedVertex &u) {
                return boost::get(pred_map, u);
            }

            void push_source(SignedVertex s) {
                boost::put(dist_map, s, DistanceType());
                queue.push(s);
                source = s;
            }

            SignedVertex get_source() {
                return source;
            }

            void update(SignedVertex w, const DistanceType &c, const SignedVertex &pred, const Edge &pred_e) {
                if (w == source) {
                    return;
                }
                bool visited_w = std::get<1>(boost::get(pred_map, w));

                if (!visited_w) {
                    // first time found
                    boost::put(dist_map, w, c);
                    boost::put(pred_map, w, std::make_tuple(pred, true, pred_e));
                    queue.push(w);
                } else if (compare(c, boost::get(dist_map, w))) {
                    // already reached
                    boost::put(dist_map, w, c);
                    boost::put(pred_map, w, std::make_tuple(pred, true, pred_e));
                    queue.update(w);
                }
            }

        };

    } // detail

    template<class Graph, class WeightMap, class SignedEdges, class HiddenEdges>
    std::tuple<std::set<typename boost::graph_traits<Graph>::edge_descriptor>,
            typename boost::property_traits<WeightMap>::value_type, bool> signed_dijkstra(const Graph &g,
            const WeightMap &weight_map, const SignedEdges &signed_edges, const HiddenEdges &hidden_edges,
            bool use_hidden_edges, const typename boost::graph_traits<Graph>::vertex_descriptor &s, bool s_pos,
            const typename boost::graph_traits<Graph>::vertex_descriptor &t, bool t_pos, bool use_cycle_weight_limit,
            const typename boost::property_traits<WeightMap>::value_type &cycle_weight_limit) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;
        typedef typename boost::property_traits<WeightMap>::value_type DistanceType;
        typedef std::pair<Vertex, bool> SignedVertex;
        typedef std::tuple<SignedVertex, bool, Edge> Predecessor;

        DistanceType distance_inf = (std::numeric_limits<DistanceType>::max)();
        std::less<DistanceType> compare;
        parmcb::detail::closed_plus<DistanceType> combine = parmcb::detail::closed_plus<DistanceType>();

        parmcb::detail::search_frontier<Graph, WeightMap> frontier(g);
        SignedVertex signed_s = std::make_pair(s, s_pos);
        SignedVertex signed_t = std::make_pair(t, t_pos);
        frontier.push_source(signed_s);

        assert(signed_s != signed_t);

        while (!frontier.queue.empty()) {
            SignedVertex signed_u = frontier.poll();
            DistanceType d_u = frontier.get_dist(signed_u);
            auto u = signed_u.first;

            if (use_cycle_weight_limit && !compare(d_u, cycle_weight_limit)) {
                // reached limit
                return std::make_tuple(std::set<Edge> { }, distance_inf, false);
            }

            if (signed_u == signed_t) { // found target
                std::set<Edge> cycle;
                WeightType cycle_weight = WeightType();
                SignedVertex signed_cur = signed_t;
                while (signed_cur != signed_s) {
                    Predecessor p = frontier.get_pred(signed_cur);
                    Edge e = std::get<2>(p);
                    if (!cycle.insert(e).second) {
                        // duplicate edge, discard cycle
                        return std::make_pair(std::set<Edge> { }, distance_inf);
                    } else {
                        cycle_weight += get(weight_map, e);
                    }
                    signed_cur = std::get<0>(p);
                }
                return std::make_tuple(cycle, cycle_weight, true);
            }

            auto eiRange = boost::out_edges(signed_u.first, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto e = *ei;
                if (use_hidden_edges && hidden_edges.find(e) != hidden_edges.end()) {
                    continue;
                }

                auto w = boost::target(e, g);
                if (w == u) {
                    w = boost::source(e, g);
                }
                if (w == u) {
                    // self-loop
                    continue;
                }
                const WeightType c = combine(d_u, get(weight_map, e));

                if (use_cycle_weight_limit && !compare(c, cycle_weight_limit)) {
                    // never insert if more than current minimum
                    continue;
                }

                bool is_signed = (signed_edges.find(e) != signed_edges.end());
                SignedVertex signed_w = std::make_pair(w, is_signed ? (!signed_u.second) : signed_u.second);

                frontier.update(signed_w, c, signed_u, e);
            }

        }

        return std::make_tuple(std::set<Edge> { }, distance_inf, false);
    }

    template<class Graph, class WeightMap, class SignedEdges, class HiddenEdges>
    std::tuple<std::set<typename boost::graph_traits<Graph>::edge_descriptor>,
            typename boost::property_traits<WeightMap>::value_type, bool> bidirectional_signed_dijkstra(const Graph &g,
            const WeightMap &weight_map, const SignedEdges &signed_edges, const HiddenEdges &hidden_edges,
            bool use_hidden_edges, const typename boost::graph_traits<Graph>::vertex_descriptor &s, bool s_pos,
            const typename boost::graph_traits<Graph>::vertex_descriptor &t, bool t_pos, bool use_cycle_weight_limit,
            const typename boost::property_traits<WeightMap>::value_type &cycle_weight_limit) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;
        typedef typename boost::property_traits<WeightMap>::value_type DistanceType;
        typedef std::pair<Vertex, bool> SignedVertex;
        typedef std::tuple<SignedVertex, bool, Edge> Predecessor;

        DistanceType distance_inf = (std::numeric_limits<DistanceType>::max)();
        std::less<DistanceType> compare;
        parmcb::detail::closed_plus<DistanceType> combine = parmcb::detail::closed_plus<DistanceType>();

        SignedVertex signed_s = std::make_pair(s, s_pos);
        parmcb::detail::search_frontier<Graph, WeightMap> f_frontier(g);
        f_frontier.push_source(signed_s);

        SignedVertex signed_t = std::make_pair(t, t_pos);
        parmcb::detail::search_frontier<Graph, WeightMap> b_frontier(g);
        b_frontier.push_source(signed_t);

        assert(signed_s != signed_t);

        std::reference_wrapper<parmcb::detail::search_frontier<Graph, WeightMap>> frontier = std::ref(f_frontier);
        std::reference_wrapper<parmcb::detail::search_frontier<Graph, WeightMap>> other_frontier = std::ref(b_frontier);
        DistanceType best_path = distance_inf;
        bool best_path_set = false;
        SignedVertex best_path_common_vertex;

        while (true) {
            // stopping condition
            if (frontier.get().queue.empty() || other_frontier.get().queue.empty()
                    || (best_path_set
                            && !compare(combine(frontier.get().find_min(), other_frontier.get().find_min()), best_path))) {
                break;
            }

            // frontier scan
            SignedVertex signed_u = frontier.get().poll();
            DistanceType d_u = frontier.get().get_dist(signed_u);
            auto u = signed_u.first;

            if (use_cycle_weight_limit && !compare(d_u, cycle_weight_limit)) {
                // reached limit
                return std::make_tuple(std::set<Edge> { }, distance_inf, false);
            }

            auto eiRange = boost::out_edges(signed_u.first, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto e = *ei;
                if (use_hidden_edges && hidden_edges.find(e) != hidden_edges.end()) {
                    continue;
                }
                auto w = boost::target(e, g);
                if (w == u) {
                    w = boost::source(e, g);
                }
                if (w == u) {
                    // self-loop
                    continue;
                }

                const WeightType c = combine(d_u, get(weight_map, e));
                if (use_cycle_weight_limit && !frontier.get().compare(c, cycle_weight_limit)) {
                    // never insert if more than current minimum
                    continue;
                }

                bool is_signed = (signed_edges.find(e) != signed_edges.end());
                SignedVertex signed_w = std::make_pair(w, is_signed ? (!signed_u.second) : signed_u.second);

                frontier.get().update(signed_w, c, signed_u, e);

                if (other_frontier.get().has_finite_dist(signed_w)) {
                    // check path with w's distance from other frontier
                    DistanceType path_distance = combine(c, other_frontier.get().get_dist(signed_w));
                    if (compare(path_distance, best_path)) {
                        best_path_set = true;
                        best_path = path_distance;
                        best_path_common_vertex = signed_w;
                    }
                }

            }

            // swap frontiers
            std::swap(frontier, other_frontier);
        }

        if (!best_path_set || (use_cycle_weight_limit && !compare(best_path, cycle_weight_limit))) {
            return std::make_tuple(std::set<Edge> { }, distance_inf, false);
        }

        // create path if found
        std::set<Edge> cycle;
        WeightType cycle_weight = WeightType();

        SignedVertex signed_cur = best_path_common_vertex;
        SignedVertex signed_goal = frontier.get().get_source();
        while (signed_cur != signed_goal) {
            Predecessor p = frontier.get().get_pred(signed_cur);
            Edge e = std::get<2>(p);
            if (!cycle.insert(e).second) {
                // duplicate edge, discard cycle
                return std::make_tuple(std::set<Edge> { }, distance_inf, false);
            } else {
                cycle_weight += boost::get(weight_map, e);
            }
            signed_cur = std::get<0>(p);
        }

        signed_cur = best_path_common_vertex;
        signed_goal = other_frontier.get().get_source();
        while (signed_cur != signed_goal) {
            Predecessor p = other_frontier.get().get_pred(signed_cur);
            Edge e = std::get<2>(p);
            if (!cycle.insert(e).second) {
                // duplicate edge, discard cycle
                return std::make_tuple(std::set<Edge> { }, distance_inf, false);
            } else {
                cycle_weight += boost::get(weight_map, e);
            }
            signed_cur = std::get<0>(p);
        }
        return std::make_tuple(cycle, cycle_weight, true);
    }

} // parmcb

#endif


//  Copyright John Maddock 2006.
//  Copyright Paul A. Bristow 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)

#ifndef BOOST_MATH_SPECIAL_FUNCTIONS_IBETA_INVERSE_HPP
#define BOOST_MATH_SPECIAL_FUNCTIONS_IBETA_INVERSE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/special_functions/detail/t_distribution_inv.hpp>

namespace boost{ namespace math{ namespace detail{

//
// Helper object used by root finding
// code to convert eta to x.
//
template <class T>
struct temme_root_finder
{
   temme_root_finder(const T t_, const T a_) : t(t_), a(a_) {}

   boost::math::tuple<T, T> operator()(T x)
   {
      BOOST_MATH_STD_USING // ADL of std names

      T y = 1 - x;
      if(y == 0)
      {
         T big = tools::max_value<T>() / 4;
         return boost::math::make_tuple(static_cast<T>(-big), static_cast<T>(-big));
      }
      if(x == 0)
      {
         T big = tools::max_value<T>() / 4;
         return boost::math::make_tuple(static_cast<T>(-big), big);
      }
      T f = log(x) + a * log(y) + t;
      T f1 = (1 / x) - (a / (y));
      return boost::math::make_tuple(f, f1);
   }
private:
   T t, a;
};
//
// See:
// "Asymptotic Inversion of the Incomplete Beta Function"
// N.M. Temme
// Journal of Computation and Applied Mathematics 41 (1992) 145-157.
// Section 2.
//
template <class T, class Policy>
T temme_method_1_ibeta_inverse(T a, T b, T z, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names

   const T r2 = sqrt(T(2));
   //
   // get the first approximation for eta from the inverse
   // error function (Eq: 2.9 and 2.10).
   //
   T eta0 = boost::math::erfc_inv(2 * z, pol);
   eta0 /= -sqrt(a / 2);

   T terms[4] = { eta0 };
   T workspace[7];
   //
   // calculate powers:
   //
   T B = b - a;
   T B_2 = B * B;
   T B_3 = B_2 * B;
   //
   // Calculate correction terms:
   //

   // See eq following 2.15:
   workspace[0] = -B * r2 / 2;
   workspace[1] = (1 - 2 * B) / 8;
   workspace[2] = -(B * r2 / 48);
   workspace[3] = T(-1) / 192;
   workspace[4] = -B * r2 / 3840;
   terms[1] = tools::evaluate_polynomial(workspace, eta0, 5);
   // Eq Following 2.17:
   workspace[0] = B * r2 * (3 * B - 2) / 12;
   workspace[1] = (20 * B_2 - 12 * B + 1) / 128;
   workspace[2] = B * r2 * (20 * B - 1) / 960;
   workspace[3] = (16 * B_2 + 30 * B - 15) / 4608;
   workspace[4] = B * r2 * (21 * B + 32) / 53760;
   workspace[5] = (-32 * B_2 + 63) / 368640;
   workspace[6] = -B * r2 * (120 * B + 17) / 25804480;
   terms[2] = tools::evaluate_polynomial(workspace, eta0, 7);
   // Eq Following 2.17:
   workspace[0] = B * r2 * (-75 * B_2 + 80 * B - 16) / 480;
   workspace[1] = (-1080 * B_3 + 868 * B_2 - 90 * B - 45) / 9216;
   workspace[2] = B * r2 * (-1190 * B_2 + 84 * B + 373) / 53760;
   workspace[3] = (-2240 * B_3 - 2508 * B_2 + 2100 * B - 165) / 368640;
   terms[3] = tools::evaluate_polynomial(workspace, eta0, 4);
   //
   // Bring them together to get a final estimate for eta:
   //
   T eta = tools::evaluate_polynomial(terms, T(1/a), 4);
   //
   // now we need to convert eta to x, by solving the appropriate
   // quadratic equation:
   //
   T eta_2 = eta * eta;
   T c = -exp(-eta_2 / 2);
   T x;
   if(eta_2 == 0)
      x = 0.5;
   else
      x = (1 + eta * sqrt((1 + c) / eta_2)) / 2;

   BOOST_ASSERT(x >= 0);
   BOOST_ASSERT(x <= 1);
   BOOST_ASSERT(eta * (x - 0.5) >= 0);
#ifdef BOOST_INSTRUMENT
   std::cout << "Estimating x with Temme method 1: " << x << std::endl;
#endif
   return x;
}
//
// See:
// "Asymptotic Inversion of the Incomplete Beta Function"
// N.M. Temme
// Journal of Computation and Applied Mathematics 41 (1992) 145-157.
// Section 3.
//
template <class T, class Policy>
T temme_method_2_ibeta_inverse(T /*a*/, T /*b*/, T z, T r, T theta, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names

   //
   // Get first estimate for eta, see Eq 3.9 and 3.10,
   // but note there is a typo in Eq 3.10:
   //
   T eta0 = boost::math::erfc_inv(2 * z, pol);
   eta0 /= -sqrt(r / 2);

   T s = sin(theta);
   T c = cos(theta);
   //
   // Now we need to purturb eta0 to get eta, which we do by
   // evaluating the polynomial in 1/r at the bottom of page 151,
   // to do this we first need the error terms e1, e2 e3
   // which we'll fill into the array "terms".  Since these
   // terms are themselves polynomials, we'll need another
   // array "workspace" to calculate those...
   //
   T terms[4] = { eta0 };
   T workspace[6];
   //
   // some powers of sin(theta)cos(theta) that we'll need later:
   //
   T sc = s * c;
   T sc_2 = sc * sc;
   T sc_3 = sc_2 * sc;
   T sc_4 = sc_2 * sc_2;
   T sc_5 = sc_2 * sc_3;
   T sc_6 = sc_3 * sc_3;
   T sc_7 = sc_4 * sc_3;
   //
   // Calculate e1 and put it in terms[1], see the middle of page 151:
   //
   workspace[0] = (2 * s * s - 1) / (3 * s * c);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co1[] = { -1, -5, 5 };
   workspace[1] = -tools::evaluate_even_polynomial(co1, s, 3) / (36 * sc_2);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co2[] = { 1, 21, -69, 46 };
   workspace[2] = tools::evaluate_even_polynomial(co2, s, 4) / (1620 * sc_3);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co3[] = { 7, -2, 33, -62, 31 };
   workspace[3] = -tools::evaluate_even_polynomial(co3, s, 5) / (6480 * sc_4);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co4[] = { 25, -52, -17, 88, -115, 46 };
   workspace[4] = tools::evaluate_even_polynomial(co4, s, 6) / (90720 * sc_5);
   terms[1] = tools::evaluate_polynomial(workspace, eta0, 5);
   //
   // Now evaluate e2 and put it in terms[2]:
   //
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co5[] = { 7, 12, -78, 52 };
   workspace[0] = -tools::evaluate_even_polynomial(co5, s, 4) / (405 * sc_3);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co6[] = { -7, 2, 183, -370, 185 };
   workspace[1] = tools::evaluate_even_polynomial(co6, s, 5) / (2592 * sc_4);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co7[] = { -533, 776, -1835, 10240, -13525, 5410 };
   workspace[2] = -tools::evaluate_even_polynomial(co7, s, 6) / (204120 * sc_5);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co8[] = { -1579, 3747, -3372, -15821, 45588, -45213, 15071 };
   workspace[3] = -tools::evaluate_even_polynomial(co8, s, 7) / (2099520 * sc_6);
   terms[2] = tools::evaluate_polynomial(workspace, eta0, 4);
   //
   // And e3, and put it in terms[3]:
   //
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co9[] = {449, -1259, -769, 6686, -9260, 3704 };
   workspace[0] = tools::evaluate_even_polynomial(co9, s, 6) / (102060 * sc_5);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co10[] = { 63149, -151557, 140052, -727469, 2239932, -2251437, 750479 };
   workspace[1] = -tools::evaluate_even_polynomial(co10, s, 7) / (20995200 * sc_6);
   static const BOOST_MATH_INT_TABLE_TYPE(T, int) co11[] = { 29233, -78755, 105222, 146879, -1602610, 3195183, -2554139, 729754 };
   workspace[2] = tools::evaluate_even_polynomial(co11, s, 8) / (36741600 * sc_7);
   terms[3] = tools::evaluate_polynomial(workspace, eta0, 3);
   //
   // Bring the correction terms together to evaluate eta,
   // this is the last equation on page 151:
   //
   T eta = tools::evaluate_polynomial(terms, T(1/r), 4);
   //
   // Now that we have eta we need to back solve for x,
   // we seek the value of x that gives eta in Eq 3.2.
   // The two methods used are described in section 5.
   //
   // Begin by defining a few variables we'll need later:
   //
   T x;
   T s_2 = s * s;
   T c_2 = c * c;
   T alpha = c / s;
   alpha *= alpha;
   T lu = (-(eta * eta) / (2 * s_2) + log(s_2) + c_2 * log(c_2) / s_2);
   //
   // Temme doesn't specify what value to switch on here,
   // but this seems to work pretty well:
   //
   if(fabs(eta) < 0.7)
   {
      //
      // Small eta use the expansion Temme gives in the second equation
      // of section 5, it's a polynomial in eta:
      //
      workspace[0] = s * s;
      workspace[1] = s * c;
      workspace[2] = (1 - 2 * workspace[0]) / 3;
      static const BOOST_MATH_INT_TABLE_TYPE(T, int) co12[] = { 1, -13, 13 };
      workspace[3] = tools::evaluate_polynomial(co12, workspace[0], 3) / (36 * s * c);
      static const BOOST_MATH_INT_TABLE_TYPE(T, int) co13[] = { 1, 21, -69, 46 };
      workspace[4] = tools::evaluate_polynomial(co13, workspace[0], 4) / (270 * workspace[0] * c * c);
      x = tools::evaluate_polynomial(workspace, eta, 5);
#ifdef BOOST_INSTRUMENT
      std::cout << "Estimating x with Temme method 2 (small eta): " << x << std::endl;
#endif
   }
   else
   {
      //
      // If eta is large we need to solve Eq 3.2 more directly,
      // begin by getting an initial approximation for x from
      // the last equation on page 155, this is a polynomial in u:
      //
      T u = exp(lu);
      workspace[0] = u;
      workspace[1] = alpha;
      workspace[2] = 0;
      workspace[3] = 3 * alpha * (3 * alpha + 1) / 6;
      workspace[4] = 4 * alpha * (4 * alpha + 1) * (4 * alpha + 2) / 24;
      workspace[5] = 5 * alpha * (5 * alpha + 1) * (5 * alpha + 2) * (5 * alpha + 3) / 120;
      x = tools::evaluate_polynomial(workspace, u, 6);
      //
      // At this point we may or may not have the right answer, Eq-3.2 has
      // two solutions for x for any given eta, however the mapping in 3.2
      // is 1:1 with the sign of eta and x-sin^2(theta) being the same.
      // So we can check if we have the right root of 3.2, and if not
      // switch x for 1-x.  This transformation is motivated by the fact
      // that the distribution is *almost* symetric so 1-x will be in the right
      // ball park for the solution:
      //
      if((x - s_2) * eta < 0)
         x = 1 - x;
#ifdef BOOST_INSTRUMENT
      std::cout << "Estimating x with Temme method 2 (large eta): " << x << std::endl;
#endif
   }
   //
   // The final step is a few Newton-Raphson iterations to
   // clean up our approximation for x, this is pretty cheap
   // in general, and very cheap compared to an incomplete beta
   // evaluation.  The limits set on x come from the observation
   // that the sign of eta and x-sin^2(theta) are the same.
   //
   T lower, upper;
   if(eta < 0)
   {
      lower = 0;
      upper = s_2;
   }
   else
   {
      lower = s_2;
      upper = 1;
   }
   //
   // If our initial approximation is out of bounds then bisect:
   //
   if((x < lower) || (x > upper))
      x = (lower+upper) / 2;
   //
   // And iterate:
   //
   x = tools::newton_raphson_iterate(
      temme_root_finder<T>(-lu, alpha), x, lower, upper, policies::digits<T, Policy>() / 2);

   return x;
}
//
// See:
// "Asymptotic Inversion of the Incomplete Beta Function"
// N.M. Temme
// Journal of Computation and Applied Mathematics 41 (1992) 145-157.
// Section 4.
//
template <class T, class Policy>
T temme_method_3_ibeta_inverse(T a, T b, T p, T q, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names

   //
   // Begin by getting an initial approximation for the quantity
   // eta from the dominant part of the incomplete beta:
   //
   T eta0;
   if(p < q)
      eta0 = boost::math::gamma_q_inv(b, p, pol);
   else
      eta0 = boost::math::gamma_p_inv(b, q, pol);
   eta0 /= a;
   //
   // Define the variables and powers we'll need later on:
   //
   T mu = b / a;
   T w = sqrt(1 + mu);
   T w_2 = w * w;
   T w_3 = w_2 * w;
   T w_4 = w_2 * w_2;
   T w_5 = w_3 * w_2;
   T w_6 = w_3 * w_3;
   T w_7 = w_4 * w_3;
   T w_8 = w_4 * w_4;
   T w_9 = w_5 * w_4;
   T w_10 = w_5 * w_5;
   T d = eta0 - mu;
   T d_2 = d * d;
   T d_3 = d_2 * d;
   T d_4 = d_2 * d_2;
   T w1 = w + 1;
   T w1_2 = w1 * w1;
   T w1_3 = w1 * w1_2;
   T w1_4 = w1_2 * w1_2;
   //
   // Now we need to compute the purturbation error terms that
   // convert eta0 to eta, these are all polynomials of polynomials.
   // Probably these should be re-written to use tabulated data
   // (see examples above), but it's less of a win in this case as we
   // need to calculate the individual powers for the denominator terms
   // anyway, so we might as well use them for the numerator-polynomials
   // as well....
   //
   // Refer to p154-p155 for the details of these expansions:
   //
   T e1 = (w + 2) * (w - 1) / (3 * w);
   e1 += (w_3 + 9 * w_2 + 21 * w + 5) * d / (36 * w_2 * w1);
   e1 -= (w_4 - 13 * w_3 + 69 * w_2 + 167 * w + 46) * d_2 / (1620 * w1_2 * w_3);
   e1 -= (7 * w_5 + 21 * w_4 + 70 * w_3 + 26 * w_2 - 93 * w - 31) * d_3 / (6480 * w1_3 * w_4);
   e1 -= (75 * w_6 + 202 * w_5 + 188 * w_4 - 888 * w_3 - 1345 * w_2 + 118 * w + 138) * d_4 / (272160 * w1_4 * w_5);

   T e2 = (28 * w_4 + 131 * w_3 + 402 * w_2 + 581 * w + 208) * (w - 1) / (1620 * w1 * w_3);
   e2 -= (35 * w_6 - 154 * w_5 - 623 * w_4 - 1636 * w_3 - 3983 * w_2 - 3514 * w - 925) * d / (12960 * w1_2 * w_4);
   e2 -= (2132 * w_7 + 7915 * w_6 + 16821 * w_5 + 35066 * w_4 + 87490 * w_3 + 141183 * w_2 + 95993 * w + 21640) * d_2  / (816480 * w_5 * w1_3);
   e2 -= (11053 * w_8 + 53308 * w_7 + 117010 * w_6 + 163924 * w_5 + 116188 * w_4 - 258428 * w_3 - 677042 * w_2 - 481940 * w - 105497) * d_3 / (14696640 * w1_4 * w_6);

   T e3 = -((3592 * w_7 + 8375 * w_6 - 1323 * w_5 - 29198 * w_4 - 89578 * w_3 - 154413 * w_2 - 116063 * w - 29632) * (w - 1)) / (816480 * w_5 * w1_2);
   e3 -= (442043 * w_9 + 2054169 * w_8 + 3803094 * w_7 + 3470754 * w_6 + 2141568 * w_5 - 2393568 * w_4 - 19904934 * w_3 - 34714674 * w_2 - 23128299 * w - 5253353) * d / (146966400 * w_6 * w1_3);
   e3 -= (116932 * w_10 + 819281 * w_9 + 2378172 * w_8 + 4341330 * w_7 + 6806004 * w_6 + 10622748 * w_5 + 18739500 * w_4 + 30651894 * w_3 + 30869976 * w_2 + 15431867 * w + 2919016) * d_2 / (146966400 * w1_4 * w_7);
   //
   // Combine eta0 and the error terms to compute eta (Second eqaution p155):
   //
   T eta = eta0 + e1 / a + e2 / (a * a) + e3 / (a * a * a);
   //
   // Now we need to solve Eq 4.2 to obtain x.  For any given value of
   // eta there are two solutions to this equation, and since the distribtion
   // may be very skewed, these are not related by x ~ 1-x we used when
   // implementing section 3 above.  However we know that:
   //
   //  cross < x <= 1       ; iff eta < mu
   //          x == cross   ; iff eta == mu
   //     0 <= x < cross    ; iff eta > mu
   //
   // Where cross == 1 / (1 + mu)
   // Many thanks to Prof Temme for clarifying this point.
   //
   // Therefore we'll just jump straight into Newton iterations
   // to solve Eq 4.2 using these bounds, and simple bisection
   // as the first guess, in practice this converges pretty quickly
   // and we only need a few digits correct anyway:
   //
   if(eta <= 0)
      eta = tools::min_value<T>();
   T u = eta - mu * log(eta) + (1 + mu) * log(1 + mu) - mu;
   T cross = 1 / (1 + mu);
   T lower = eta < mu ? cross : 0;
   T upper = eta < mu ? 1 : cross;
   T x = (lower + upper) / 2;
   x = tools::newton_raphson_iterate(
      temme_root_finder<T>(u, mu), x, lower, upper, policies::digits<T, Policy>() / 2);
#ifdef BOOST_INSTRUMENT
   std::cout << "Estimating x with Temme method 3: " << x << std::endl;
#endif
   return x;
}

template <class T, class Policy>
struct ibeta_roots
{
   ibeta_roots(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   boost::math::tuple<T, T, T> operator()(T x)
   {
      BOOST_MATH_STD_USING // ADL of std names

      BOOST_FPU_EXCEPTION_GUARD

      T f1;
      T y = 1 - x;
      T f = ibeta_imp(a, b, x, Policy(), invert, true, &f1) - target;
      if(invert)
         f1 = -f1;
      if(y == 0)
         y = tools::min_value<T>() * 64;
      if(x == 0)
         x = tools::min_value<T>() * 64;

      T f2 = f1 * (-y * a + (b - 2) * x + 1);
      if(fabs(f2) < y * x * tools::max_value<T>())
         f2 /= (y * x);
      if(invert)
         f2 = -f2;

      // make sure we don't have a zero derivative:
      if(f1 == 0)
         f1 = (invert ? -1 : 1) * tools::min_value<T>() * 64;

      return boost::math::make_tuple(f, f1, f2);
   }
private:
   T a, b, target;
   bool invert;
};

template <class T, class Policy>
T ibeta_inv_imp(T a, T b, T p, T q, const Policy& pol, T* py)
{
   BOOST_MATH_STD_USING  // For ADL of math functions.

   //
   // Handle trivial cases first:
   //
   if(q == 0)
   {
      if(py) *py = 0;
      return 1;
   }
   else if(p == 0)
   {
      if(py) *py = 1;
      return 0;
   }
   else if((a == 1) && (b == 1))
   {
      if(py) *py = 1 - p;
      return p;
   }
   //
   // The flag invert is set to true if we swap a for b and p for q,
   // in which case the result has to be subtracted from 1:
   //
   bool invert = false;
   //
   // Depending upon which approximation method we use, we may end up
   // calculating either x or y initially (where y = 1-x):
   //
   T x = 0; // Set to a safe zero to avoid a
   // MSVC 2005 warning C4701: potentially uninitialized local variable 'x' used
   // But code inspection appears to ensure that x IS assigned whatever the code path.
   T y;

   // For some of the methods we can put tighter bounds
   // on the result than simply [0,1]:
   //
   T lower = 0;
   T upper = 1;
   //
   // Student's T with b = 0.5 gets handled as a special case, swap
   // around if the arguments are in the "wrong" order:
   //
   if((a == 0.5f) && (b >= 0.5f))
   {
      std::swap(a, b);
      std::swap(p, q);
      invert = !invert;
   }
   //
   // Select calculation method for the initial estimate:
   //
   if((b == 0.5f) && (a >= 0.5f))
   {
      //
      // We have a Student's T distribution:
      x = find_ibeta_inv_from_t_dist(a, p, q, &y, pol);
   }
   else if(a + b > 5)
   {
      //
      // When a+b is large then we can use one of Prof Temme's
      // asymptotic expansions, begin by swapping things around
      // so that p < 0.5, we do this to avoid cancellations errors
      // when p is large.
      //
      if(p > 0.5)
      {
         std::swap(a, b);
         std::swap(p, q);
         invert = !invert;
      }
      T minv = (std::min)(a, b);
      T maxv = (std::max)(a, b);
      if((sqrt(minv) > (maxv - minv)) && (minv > 5))
      {
         //
         // When a and b differ by a small amount
         // the curve is quite symmetrical and we can use an error
         // function to approximate the inverse. This is the cheapest
         // of the three Temme expantions, and the calculated value
         // for x will never be much larger than p, so we don't have
         // to worry about cancellation as long as p is small.
         //
         x = temme_method_1_ibeta_inverse(a, b, p, pol);
         y = 1 - x;
      }
      else
      {
         T r = a + b;
         T theta = asin(sqrt(a / r));
         T lambda = minv / r;
         if((lambda >= 0.2) && (lambda <= 0.8) && (r >= 10))
         {
            //
            // The second error function case is the next cheapest
            // to use, it brakes down when the result is likely to be
            // very small, if a+b is also small, but we can use a
            // cheaper expansion there in any case.  As before x won't
            // be much larger than p, so as long as p is small we should
            // be free of cancellation error.
            //
            T ppa = pow(p, 1/a);
            if((ppa < 0.0025) && (a + b < 200))
            {
               x = ppa * pow(a * boost::math::beta(a, b, pol), 1/a);
            }
            else
               x = temme_method_2_ibeta_inverse(a, b, p, r, theta, pol);
            y = 1 - x;
         }
         else
         {
            //
            // If we get here then a and b are very different in magnitude
            // and we need to use the third of Temme's methods which
            // involves inverting the incomplete gamma.  This is much more
            // expensive than the other methods.  We also can only use this
            // method when a > b, which can lead to cancellation errors
            // if we really want y (as we will when x is close to 1), so
            // a different expansion is used in that case.
            //
            if(a < b)
            {
               std::swap(a, b);
               std::swap(p, q);
               invert = !invert;
            }
            //
            // Try and compute the easy way first:
            //
            T bet = 0;
            if(b < 2)
               bet = boost::math::beta(a, b, pol);
            if(bet != 0)
            {
               y = pow(b * q * bet, 1/b);
               x = 1 - y;
            }
            else
               y = 1;
            if(y > 1e-5)
            {
               x = temme_method_3_ibeta_inverse(a, b, p, q, pol);
               y = 1 - x;
            }
         }
      }
   }
   else if((a < 1) && (b < 1))
   {
      //
      // Both a and b less than 1,
      // there is a point of inflection at xs:
      //
      T xs = (1 - a) / (2 - a - b);
      //
      // Now we need to ensure that we start our iteration from the
      // right side of the inflection point:
      //
      T fs = boost::math::ibeta(a, b, xs, pol) - p;
      if(fabs(fs) / p < tools::epsilon<T>() * 3)
      {
         // The result is at the point of inflection, best just return it:
         *py = invert ? xs : 1 - xs;
         return invert ? 1-xs : xs;
      }
      if(fs < 0)
      {
         std::swap(a, b);
         std::swap(p, q);
         invert = !invert;
         xs = 1 - xs;
      }
      T xg = pow(a * p * boost::math::beta(a, b, pol), 1/a);
      x = xg / (1 + xg);
      y = 1 / (1 + xg);
      //
      // And finally we know that our result is below the inflection
      // point, so set an upper limit on our search:
      //
      if(x > xs)
         x = xs;
      upper = xs;
   }
   else if((a > 1) && (b > 1))
   {
      //
      // Small a and b, both greater than 1,
      // there is a point of inflection at xs,
      // and it's complement is xs2, we must always
      // start our iteration from the right side of the
      // point of inflection.
      //
      T xs = (a - 1) / (a + b - 2);
      T xs2 = (b - 1) / (a + b - 2);
      T ps = boost::math::ibeta(a, b, xs, pol) - p;

      if(ps < 0)
      {
         std::swap(a, b);
         std::swap(p, q);
         std::swap(xs, xs2);
         invert = !invert;
      }
      //
      // Estimate x and y, using expm1 to get a good estimate
      // for y when it's very small:
      //
      T lx = log(p * a * boost::math::beta(a, b, pol)) / a;
      x = exp(lx);
      y = x < 0.9 ? T(1 - x) : (T)(-boost::math::expm1(lx, pol));

      if((b < a) && (x < 0.2))
      {
         //
         // Under a limited range of circumstances we can improve
         // our estimate for x, frankly it's clear if this has much effect!
         //
         T ap1 = a - 1;
         T bm1 = b - 1;
         T a_2 = a * a;
         T a_3 = a * a_2;
         T b_2 = b * b;
         T terms[5] = { 0, 1 };
         terms[2] = bm1 / ap1;
         ap1 *= ap1;
         terms[3] = bm1 * (3 * a * b + 5 * b + a_2 - a - 4) / (2 * (a + 2) * ap1);
         ap1 *= (a + 1);
         terms[4] = bm1 * (33 * a * b_2 + 31 * b_2 + 8 * a_2 * b_2 - 30 * a * b - 47 * b + 11 * a_2 * b + 6 * a_3 * b + 18 + 4 * a - a_3 + a_2 * a_2 - 10 * a_2)
                    / (3 * (a + 3) * (a + 2) * ap1);
         x = tools::evaluate_polynomial(terms, x, 5);
      }
      //
      // And finally we know that our result is below the inflection
      // point, so set an upper limit on our search:
      //
      if(x > xs)
         x = xs;
      upper = xs;
   }
   else /*if((a <= 1) != (b <= 1))*/
   {
      //
      // If all else fails we get here, only one of a and b
      // is above 1, and a+b is small.  Start by swapping
      // things around so that we have a concave curve with b > a
      // and no points of inflection in [0,1].  As long as we expect
      // x to be small then we can use the simple (and cheap) power
      // term to estimate x, but when we expect x to be large then
      // this greatly underestimates x and leaves us trying to
      // iterate "round the corner" which may take almost forever...
      //
      // We could use Temme's inverse gamma function case in that case,
      // this works really rather well (albeit expensively) even though
      // strictly speaking we're outside it's defined range.
      //
      // However it's expensive to compute, and an alternative approach
      // which models the curve as a distorted quarter circle is much
      // cheaper to compute, and still keeps the number of iterations
      // required down to a reasonable level.  With thanks to Prof Temme
      // for this suggestion.
      //
      if(b < a)
      {
         std::swap(a, b);
         std::swap(p, q);
         invert = !invert;
      }
      if(pow(p, 1/a) < 0.5)
      {
         x = pow(p * a * boost::math::beta(a, b, pol), 1 / a);
         if(x == 0)
            x = boost::math::tools::min_value<T>();
         y = 1 - x;
      }
      else /*if(pow(q, 1/b) < 0.1)*/
      {
         // model a distorted quarter circle:
         y = pow(1 - pow(p, b * boost::math::beta(a, b, pol)), 1/b);
         if(y == 0)
            y = boost::math::tools::min_value<T>();
         x = 1 - y;
      }
   }

   //
   // Now we have a guess for x (and for y) we can set things up for
   // iteration.  If x > 0.5 it pays to swap things round:
   //
   if(x > 0.5)
   {
      std::swap(a, b);
      std::swap(p, q);
      std::swap(x, y);
      invert = !invert;
      T l = 1 - upper;
      T u = 1 - lower;
      lower = l;
      upper = u;
   }
   //
   // lower bound for our search:
   //
   // We're not interested in denormalised answers as these tend to
   // these tend to take up lots of iterations, given that we can't get
   // accurate derivatives in this area (they tend to be infinite).
   //
   if(lower == 0)
   {
      if(invert && (py == 0))
      {
         //
         // We're not interested in answers smaller than machine epsilon:
         //
         lower = boost::math::tools::epsilon<T>();
         if(x < lower)
            x = lower;
      }
      else
         lower = boost::math::tools::min_value<T>();
      if(x < lower)
         x = lower;
   }
   //
   // Figure out how many digits to iterate towards:
   //
   int digits = boost::math::policies::digits<T, Policy>() / 2;
   if((x < 1e-50) && ((a < 1) || (b < 1)))
   {
      //
      // If we're in a region where the first derivative is very
      // large, then we have to take care that the root-finder
      // doesn't terminate prematurely.  We'll bump the precision
      // up to avoid this, but we have to take care not to set the
      // precision too high or the last few iterations will just
      // thrash around and convergence may be slow in this case.
      // Try 3/4 of machine epsilon:
      //
      digits *= 3;
      digits /= 2;
   }
   //
   // Now iterate, we can use either p or q as the target here
   // depending on which is smaller:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   x = boost::math::tools::halley_iterate(
      boost::math::detail::ibeta_roots<T, Policy>(a, b, (p < q ? p : q), (p < q ? false : true)), x, lower, upper, digits, max_iter);
   policies::check_root_iterations<T>("boost::math::ibeta<%1%>(%1%, %1%, %1%)", max_iter, pol);
   //
   // We don't really want these asserts here, but they are useful for sanity
   // checking that we have the limits right, uncomment if you suspect bugs *only*.
   //
   //BOOST_ASSERT(x != upper);
   //BOOST_ASSERT((x != lower) || (x == boost::math::tools::min_value<T>()) || (x == boost::math::tools::epsilon<T>()));
   //
   // Tidy up, if we "lower" was too high then zero is the best answer we have:
   //
   if(x == lower)
      x = 0;
   if(py)
      *py = invert ? x : 1 - x;
   return invert ? 1-x : x;
}

} // namespace detail

template <class T1, class T2, class T3, class T4, class Policy>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ibeta_inv(T1 a, T2 b, T3 p, T4* py, const Policy& pol)
{
   static const char* function = "boost::math::ibeta_inv<%1%>(%1%,%1%,%1%)";
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T1, T2, T3, T4>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   if(a <= 0)
      return policies::raise_domain_error<result_type>(function, "The argument a to the incomplete beta function inverse must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      return policies::raise_domain_error<result_type>(function, "The argument b to the incomplete beta function inverse must be greater than zero (got b=%1%).", b, pol);
   if((p < 0) || (p > 1))
      return policies::raise_domain_error<result_type>(function, "Argument p outside the range [0,1] in the incomplete beta function inverse (got p=%1%).", p, pol);

   value_type rx, ry;

   rx = detail::ibeta_inv_imp(
         static_cast<value_type>(a),
         static_cast<value_type>(b),
         static_cast<value_type>(p),
         static_cast<value_type>(1 - p),
         forwarding_policy(), &ry);

   if(py) *py = policies::checked_narrowing_cast<T4, forwarding_policy>(ry, function);
   return policies::checked_narrowing_cast<result_type, forwarding_policy>(rx, function);
}

template <class T1, class T2, class T3, class T4>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ibeta_inv(T1 a, T2 b, T3 p, T4* py)
{
   return ibeta_inv(a, b, p, py, policies::policy<>());
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   ibeta_inv(T1 a, T2 b, T3 p)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ibeta_inv(a, b, p, static_cast<result_type*>(0), policies::policy<>());
}

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
   ibeta_inv(T1 a, T2 b, T3 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ibeta_inv(a, b, p, static_cast<result_type*>(0), pol);
}

template <class T1, class T2, class T3, class T4, class Policy>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ibetac_inv(T1 a, T2 b, T3 q, T4* py, const Policy& pol)
{
   static const char* function = "boost::math::ibetac_inv<%1%>(%1%,%1%,%1%)";
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T1, T2, T3, T4>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   if(a <= 0)
      policies::raise_domain_error<result_type>(function, "The argument a to the incomplete beta function inverse must be greater than zero (got a=%1%).", a, pol);
   if(b <= 0)
      policies::raise_domain_error<result_type>(function, "The argument b to the incomplete beta function inverse must be greater than zero (got b=%1%).", b, pol);
   if((q < 0) || (q > 1))
      policies::raise_domain_error<result_type>(function, "Argument q outside the range [0,1] in the incomplete beta function inverse (got q=%1%).", q, pol);

   value_type rx, ry;

   rx = detail::ibeta_inv_imp(
         static_cast<value_type>(a),
         static_cast<value_type>(b),
         static_cast<value_type>(1 - q),
         static_cast<value_type>(q),
         forwarding_policy(), &ry);

   if(py) *py = policies::checked_narrowing_cast<T4, forwarding_policy>(ry, function);
   return policies::checked_narrowing_cast<result_type, forwarding_policy>(rx, function);
}

template <class T1, class T2, class T3, class T4>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ibetac_inv(T1 a, T2 b, T3 q, T4* py)
{
   return ibetac_inv(a, b, q, py, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibetac_inv(RT1 a, RT2 b, RT3 q)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   return ibetac_inv(a, b, q, static_cast<result_type*>(0), policies::policy<>());
}

template <class RT1, class RT2, class RT3, class Policy>
inline typename tools::promote_args<RT1, RT2, RT3>::type
   ibetac_inv(RT1 a, RT2 b, RT3 q, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   return ibetac_inv(a, b, q, static_cast<result_type*>(0), pol);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP


#include <exoplanet/calcEA.h>
#include <exoplanet/contact_points.h>
#include <exoplanet/starry.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

#include <Eigen/Core>
#include <cmath>
#include <vector>

namespace py = pybind11;

// ASCII art from:
// http://patorjk.com/software/taag/#p=display&f=Small&c=c++

namespace driver {

//    _        _
//   | |_  ___| |_ __  ___ _ _ ___
//   | ' \/ -_) | '_ \/ -_) '_(_-<
//   |_||_\___|_| .__/\___|_| /__/
//              |_|

template <typename Scalar, int ExtraFlags = py::array::forcecast>
struct flat_unchecked_array {
  flat_unchecked_array(py::array_t<Scalar, ExtraFlags> &array, bool require_mutable = false) {
    info = array.request();
    if (require_mutable && info.readonly) throw std::invalid_argument("outputs must be writeable");
    data = (Scalar *)info.ptr;
  }

  inline Scalar &operator()(ssize_t index) { return data[index]; }
  inline ssize_t shape(ssize_t index) const { return info.shape[index]; }
  inline ssize_t size() const { return info.size; }
  inline ssize_t ndim() const { return info.ndim; }

  py::buffer_info info;
  Scalar *data;
};

template <typename T>
int sgn(T val) {
  return (T(0) < val) - (val < T(0));
}

//       _
//    __| |_ __ _ _ _ _ _ _  _
//   (_-<  _/ _` | '_| '_| || |
//   /__/\__\__,_|_| |_|  \_, |
//                        |__/

namespace starry {

auto get_cl(py::array_t<double, py::array::c_style> u_in,
            py::array_t<double, py::array::c_style> c_out) {
  auto u = u_in.unchecked<1>();
  auto c = c_out.mutable_unchecked<1>();
  ssize_t N = u.size();
  if (N < 1 || c.size() != N) throw std::invalid_argument("dimension mismatch");

  std::vector<double> a(N);
  a[0] = 1;
  for (ssize_t n = 1; n < N; ++n) a[n] = 0;

  // Compute the a_n coefficients
  double bcoeff;
  int sign;
  for (ssize_t i = 1; i < N; ++i) {
    bcoeff = 1;
    sign = 1;
    for (ssize_t j = 0; j <= i; ++j) {
      a[j] -= u(i) * bcoeff * sign;
      sign *= -1;
      bcoeff *= (double(i - j) / (j + 1));
    }
  }

  // Now, compute the c_n coefficients
  for (ssize_t j = N - 1; j >= std::max<ssize_t>(2, N - 2); --j) {
    c(j) = a[j] / (j + 2);
  }
  for (ssize_t j = N - 3; j >= 2; --j) {
    c(j) = a[j] / (j + 2) + c(j + 2);
  }
  if (N >= 4)
    c(1) = a[1] + 3 * c(3);
  else
    c(1) = a[1];
  if (N >= 3)
    c(0) = a[0] + 2 * c(2);
  else
    c(0) = a[0];

  return c_out;
}

auto get_cl_rev(py::array_t<double, py::array::c_style> bc_in,
                py::array_t<double, py::array::c_style> bu_out) {
  auto bc_ = bc_in.unchecked<1>();
  auto bu = bu_out.mutable_unchecked<1>();
  ssize_t N = bc_.size();
  if (N < 1 || bu.size() != N) throw std::invalid_argument("dimension mismatch");

  std::vector<double> ba(N), bc(N);
  for (ssize_t i = 0; i < N; ++i) {
    bu(i) = 0;
    bc[i] = bc_(i);
  }

  if (N >= 3) {
    // c[0] = a(0) + 2 * c[2];
    ba[0] = bc[0];
    bc[2] += 2 * bc[0];
  } else {
    // c[0] = a(0);
    ba[0] = bc[0];
  }

  if (N >= 4) {
    // c[1] = a(1) + 3 * c[3];
    ba[1] = bc[1];
    bc[3] += 3 * bc[1];
  } else {
    // c[1] = a(1);
    ba[1] = bc[1];
  }

  for (ssize_t j = 2; j <= N - 3; ++j) {
    // c[j] = a(j) / (j + 2) + c[j + 2];
    ba[j] = bc[j] / (j + 2);
    bc[j + 2] += bc[j];
  }
  for (ssize_t j = std::max<ssize_t>(2, N - 2); j <= N - 1; ++j) {
    // c[j] = a(j) / (j + 2);
    ba[j] = bc[j] / (j + 2);
  }

  // Compute the a_n coefficients
  double bcoeff;
  int sign;
  for (ssize_t i = 1; i < N; ++i) {
    bcoeff = 1;
    sign = 1;
    for (ssize_t j = 0; j <= i; ++j) {
      // a(j) -= u[i] * bcoeff * sign;
      bu(i) -= ba[j] * bcoeff * sign;
      sign *= -1;
      bcoeff *= (double(i - j) / (j + 1));
    }
  }

  return bu_out;
}

struct LimbDark {
  LimbDark() : ld(0){};

  auto apply(py::array_t<double, py::array::c_style> cl_in,
             py::array_t<double, py::array::c_style> b_in,
             py::array_t<double, py::array::c_style> r_in,
             py::array_t<double, py::array::c_style> los_in,
             py::array_t<double, py::array::c_style> f_out,
             py::array_t<double, py::array::c_style> dfdcl_out,
             py::array_t<double, py::array::c_style> dfdb_out,
             py::array_t<double, py::array::c_style> dfdr_out) {
    flat_unchecked_array<double, py::array::c_style> b(b_in), r(r_in), los(los_in);
    ssize_t N = b.size();
    if (r.size() != N || los.size() != N) throw std::invalid_argument("dimension mismatch");

    flat_unchecked_array<double, py::array::c_style> f(f_out, true), dfdb(dfdb_out, true),
        dfdr(dfdr_out, true);
    if (f.size() != N || dfdb.size() != N || dfdr.size() != N)
      throw std::invalid_argument("dimension mismatch");

    py::buffer_info cl_info = cl_in.request(), dfdcl_info = dfdcl_out.request();
    ssize_t num_cl = cl_info.size;
    if (dfdcl_info.ndim <= 1 || dfdcl_info.shape[0] != num_cl || dfdcl_info.size != N * num_cl)
      throw std::invalid_argument("invalid dimensions for dfdcl");
    if (dfdcl_info.readonly) throw std::invalid_argument("dfdcl must be writeable");

    Eigen::Map<Eigen::VectorXd> cl((double *)cl_info.ptr, num_cl);
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> dfdcl(
        (double *)dfdcl_info.ptr, num_cl, N);
    dfdcl.setZero();

    // Re-initialize if lmax is wrong
    if (ld.lmax != int(num_cl))
      ld = exoplanet::starry::limbdark::GreensLimbDark<double>(int(num_cl));

    for (ssize_t i = 0; i < N; ++i) {
      f(i) = 0;
      dfdb(i) = 0;
      dfdr(i) = 0;

      if (los(i) > 0) {
        auto b_ = std::abs(b(i));
        auto r_ = std::abs(r(i));
        if (b_ < 1 + r_) {
          ld.compute<true>(b_, r_);
          auto sT = ld.sT;

          // The value of the light curve
          f(i) = sT.dot(cl) - 1;

          // The gradients
          dfdcl.col(i) = sT;
          dfdb(i) = sgn(b(i)) * ld.dsTdb.dot(cl);
          dfdr(i) = sgn(r(i)) * ld.dsTdr.dot(cl);
        }
      }
    }

    return std::make_tuple(f_out, dfdcl_out, dfdb_out, dfdr_out);
  }

  exoplanet::starry::limbdark::GreensLimbDark<double> ld;
};

struct SimpleLimbDark {
  SimpleLimbDark() : ld(0){};

  void allocate_cl(py::array_t<double, py::array::c_style> cl_in) {
    auto cl_array = cl_in.unchecked<1>();
    ssize_t N = cl_array.size();
    if (ld.lmax != int(N)) ld = exoplanet::starry::limbdark::GreensLimbDark<double>(int(N));
    cl.resize(N);
    for (ssize_t n = 0; n < N; ++n) {
      cl(n) = cl_array(n);
    }
  }

  void set_u(py::array_t<double, py::array::c_style> u_in) {
    auto u = u_in.unchecked<1>();
    ssize_t N = u.size();
    if (N < 2) throw std::invalid_argument("invalid dimensions");

    std::vector<double> a(N);
    a[0] = 1;
    for (ssize_t n = 1; n < N; ++n) a[n] = 0;

    // Compute the a_n coefficients
    double bcoeff;
    int sign;
    for (ssize_t i = 1; i < N; ++i) {
      bcoeff = 1;
      sign = 1;
      for (ssize_t j = 0; j <= i; ++j) {
        a[j] -= u(i) * bcoeff * sign;
        sign *= -1;
        bcoeff *= (double(i - j) / (j + 1));
      }
    }

    // Now, compute the c_n coefficients
    cl.resize(N);
    for (ssize_t j = N - 1; j >= std::max<ssize_t>(2, N - 2); --j) {
      cl(j) = a[j] / (j + 2);
    }
    for (ssize_t j = N - 3; j >= 2; --j) {
      cl(j) = a[j] / (j + 2) + cl(j + 2);
    }
    if (N >= 4)
      cl(1) = a[1] + 3 * cl(3);
    else
      cl(1) = a[1];
    if (N >= 3)
      cl(0) = a[0] + 2 * cl(2);
    else
      cl(0) = a[0];

    cl.array() /= M_PI * (cl(0) + 2 * cl(1) / 3.0);

    // Re-initialize if lmax is wrong
    if (ld.lmax != int(N)) ld = exoplanet::starry::limbdark::GreensLimbDark<double>(int(N));
  }

  double apply(double b, double r) {
    auto b_ = std::abs(b);
    auto r_ = std::abs(r);
    if (cl.rows() > 0 && b_ < 1 + r_) {
      ld.compute<false>(b_, r_);
      return double(ld.sT.dot(cl) - 1);
    }
    return 0.0;
  }

  Eigen::VectorXd cl;
  exoplanet::starry::limbdark::GreensLimbDark<double> ld;
};

}  // namespace starry

//    _            _
//   | |_____ _ __| |___ _ _
//   | / / -_) '_ \ / -_) '_|
//   |_\_\___| .__/_\___|_|
//           |_|

namespace kepler {

auto kepler(py::array_t<double, py::array::c_style> M_in,
            py::array_t<double, py::array::c_style> ecc_in,
            py::array_t<double, py::array::c_style> sinf_out,
            py::array_t<double, py::array::c_style> cosf_out) {
  flat_unchecked_array<double, py::array::c_style> M(M_in), ecc(ecc_in);
  flat_unchecked_array<double, py::array::c_style> cosf(cosf_out, true), sinf(sinf_out, true);
  ssize_t N = M.size();
  if (ecc.size() != N || cosf.size() != N || sinf.size() != N)
    throw std::invalid_argument("dimension mismatch");
  for (ssize_t n = 0; n < N; ++n) {
    if (ecc(n) < 0 || ecc(n) > 1)
      throw std::invalid_argument("eccentricity must be in the range [0, 1)");
    exoplanet::calcEA::solve_kepler(M(n), ecc(n), &(cosf(n)), &(sinf(n)));
  }
  return std::make_tuple(cosf_out, sinf_out);
}

auto contact_points(py::array_t<double, py::array::c_style> a_in,
                    py::array_t<double, py::array::c_style> e_in,
                    py::array_t<double, py::array::c_style> cosw_in,
                    py::array_t<double, py::array::c_style> sinw_in,
                    py::array_t<double, py::array::c_style> cosi_in,
                    py::array_t<double, py::array::c_style> sini_in,
                    py::array_t<double, py::array::c_style> L_in,
                    py::array_t<double, py::array::c_style> M_left_out,
                    py::array_t<double, py::array::c_style> M_right_out,
                    py::array_t<int, py::array::c_style> flag_out, double tol) {
  flat_unchecked_array<double, py::array::c_style> a(a_in), e(e_in), cosw(cosw_in), sinw(sinw_in),
      cosi(cosi_in), sini(sini_in), L(L_in);
  flat_unchecked_array<double, py::array::c_style> M_left(M_left_out, true),
      M_right(M_right_out, true);
  flat_unchecked_array<int, py::array::c_style> flag(flag_out, true);
  ssize_t N = a.size();
  if (e.size() != N || cosw.size() != N || sinw.size() != N || cosi.size() != N ||
      sini.size() != N || L.size() != N || M_left.size() != N || M_right.size() != N ||
      flag.size() != N)
    throw std::invalid_argument("dimension mismatch");
  for (ssize_t n = 0; n < N; ++n) {
    auto const solver = exoplanet::contact_points::ContactPointSolver<double>(
        a(n), e(n), cosw(n), sinw(n), cosi(n), sini(n));
    auto const roots = solver.find_roots(L(n), tol);
    flag(n) = std::get<0>(roots);
    M_left(n) = std::get<1>(roots);
    M_right(n) = std::get<2>(roots);
  }
  return std::make_tuple(M_left_out, M_right_out, flag_out);
}

}  // namespace kepler
}  // namespace driver

//              _    _         _ _ _
//    _ __ _  _| |__(_)_ _  __| / / |
//   | '_ \ || | '_ \ | ' \/ _` | | |
//   | .__/\_, |_.__/_|_||_\__,_|_|_|
//   |_|   |__/

PYBIND11_MODULE(driver, m) {
  m.doc() = R"doc(
    The computation engine for the Theano ops
)doc";

  m.def("get_cl", &driver::starry::get_cl, py::arg("u"), py::arg("c").noconvert());
  m.def("get_cl_rev", &driver::starry::get_cl_rev, py::arg("bc"), py::arg("bu").noconvert());

  py::class_<driver::starry::LimbDark>(m, "LimbDark")
      .def(py::init<>())
      .def("apply", &driver::starry::LimbDark::apply, py::arg("cl"), py::arg("b"), py::arg("r"),
           py::arg("los"), py::arg("f").noconvert(), py::arg("dfdcl").noconvert(),
           py::arg("dfdb").noconvert(), py::arg("dfdr").noconvert());

  py::class_<driver::starry::SimpleLimbDark>(m, "SimpleLimbDark")
      .def(py::init<>())
      .def("set_u", &driver::starry::SimpleLimbDark::set_u, py::arg("u"))
      .def("apply", py::vectorize(&driver::starry::SimpleLimbDark::apply), py::arg("b"),
           py::arg("r"))
      .def(py::pickle(
          [](const driver::starry::SimpleLimbDark &obj) {
            int N = obj.cl.rows();
            auto array = py::array_t<double>(N);
            py::buffer_info buf = array.request();
            double *data = (double *)buf.ptr;
            for (int n = 0; n < N; ++n) {
              data[n] = obj.cl(n);
            }
            return py::make_tuple(array);
          },
          [](py::tuple data) {
            if (data.size() != 1) throw std::runtime_error("Invalid state!");
            driver::starry::SimpleLimbDark ld;
            auto array = data[0].cast<py::array_t<double>>();
            ld.allocate_cl(array);
            return ld;
          }));

  m.def("kepler", &driver::kepler::kepler, py::arg("M"), py::arg("ecc"),
        py::arg("sinf").noconvert(), py::arg("cosf").noconvert());
  m.def("contact_points", &driver::kepler::contact_points, py::arg("a"), py::arg("e"),
        py::arg("cosw"), py::arg("sinw"), py::arg("cosi"), py::arg("sini"), py::arg("L"),
        py::arg("M_left").noconvert(), py::arg("M_right").noconvert(), py::arg("flag").noconvert(),
        py::arg("tol"));
}


#include <iostream>
#include <fstream>
#include <string>
#include <boost/tokenizer.hpp>
#include <Eigen/Dense>

#include <omplapp/config.h>
#include <omplapp/apps/SE3RigidBodyPlanning.h>
#include <ompl/base/samplers/UniformValidStateSampler.h>
#include <ompl/tools/benchmark/Benchmark.h>

#include <ompl/geometric/planners/GSE/GSE.h>
#include <ompl/geometric/planners/est/EST.h>
#include <ompl/geometric/planners/kpiece/BKPIECE1.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/informedtrees/BITstar.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/sbl/SBL.h>

using namespace ompl;

base::PlannerPtr myConfiguredGSE(const base::SpaceInformationPtr &si);
void genSphere(Eigen::MatrixXd &sphere);

int main(int,char**){

    app::SE3RigidBodyPlanning setup;
    // load the robot and the environment
    std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/GSE/point.dae";
    std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/GSE/lattice.dae";
    setup.setRobotMesh(robot_fname);
    setup.setEnvironmentMesh(env_fname);

    // define start state
    base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
    start->setX(-2);
    start->setY(-2);
    start->setZ(-2);
    start->rotation().setIdentity();

    // define goal state
    base::ScopedState<base::SE3StateSpace> goal(start);
    goal->setX(10);
    goal->setY(10);
    goal->setZ(10);
    goal->rotation().setIdentity();

    base::RealVectorBounds bounds(3);
    bounds.setHigh(0, 12.1);
    bounds.setHigh(1, 12.1);
    bounds.setHigh(2, 12.1);
    bounds.setLow(0, -2.1);
    bounds.setLow(1, -2.1);
    bounds.setLow(2, -2.1);
    setup.getStateSpace()->as<base::SE3StateSpace>()->setBounds(bounds);

    // set the start & goal states
    setup.setStartAndGoalStates(start, goal);
    // setting collision checking resolution to 1% of the space extent
    setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);

    // run all planners with a uniform valid state sampler on the benchmark problem
    setup.getSpaceInformation()->setValidStateSamplerAllocator(
        [](const base::SpaceInformation *si) -> base::ValidStateSamplerPtr {
            return std::make_shared<base::UniformValidStateSampler>(si);
        });

    // set high because memory usage is not always estimated correctly
    double runtime_limit = 0.06, memory_limit = 100.0, run_count = 500;

    // create the benchmark object and add all the planners we'd like to run
    tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
    tools::Benchmark b(setup, "GSE Benchmark");

    b.addPlannerAllocator(std::bind(&myConfiguredGSE, std::placeholders::_1));
    b.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::RRT>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::BKPIECE1>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::LBKPIECE1>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::KPIECE1>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::SBL>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::EST>(setup.getSpaceInformation()));
    b.addPlanner(std::make_shared<geometric::PRM>(setup.getSpaceInformation()));

    b.benchmark(request);
    b.saveResultsToFile();

    return 0;
}

template<typename M>
M load_csv(const std::string path){
    std::ifstream fin(path.c_str());
    std::string line;
    std::vector<double> values;
    uint cols =0;
    while(getline(fin,line)){
        boost::tokenizer<boost::escaped_list_separator<char>> tok(line);
        for (auto str: tok)
            values.push_back(std::stod(str));
        ++cols;
    }
    return Eigen::Map<M>(values.data(),values.size()/cols,cols);
}

base::PlannerPtr myConfiguredGSE(const base::SpaceInformationPtr &si)
{
    geometric::GSE *planner = new geometric::GSE(si,3);

    Eigen::MatrixXd sphere(1000,3);
    genSphere(sphere);

    std::vector<std::vector<double>> obs{{0,0,0},
                                        {0,-2,0},
                                        {0,2,0},
                                        {-2,0,0},
                                        {2,0,0},
                                        {0,0,-2},
                                        {0,0,2},
                                        {1,1,1}};

    for(auto row:obs){
        Eigen::VectorXd centre = Eigen::Map<Eigen::VectorXd>(row.data(),row.size());
        Eigen::MatrixXd obsi = (0.5*sphere).colwise()+centre;
        planner->addObstacle(obsi);
    }

    si->setup();
    auto projector = si->getStateSpace()->getDefaultProjection();
    planner->setProjector(projector);

    return base::PlannerPtr(planner);
}

// generate Uniform randomn Points on a sphere of raidius 1
// http://corysimon.github.io/articles/uniformdistn-on-sphere/
void genSphere(Eigen::MatrixXd &sphere){
    int N = sphere.rows();
    auto theta = 2*M_PI*(Eigen::ArrayXd::Random(N)*0.5+0.5);
    auto phi = Eigen::ArrayXd::Random(N).acos();

    Eigen::VectorXd x = phi.sin()*theta.cos();
    Eigen::VectorXd y = phi.sin()*theta.sin();
    Eigen::VectorXd z = phi.cos();

    sphere<<x,y,z;
}