/*
 * hd.cpp
 *
 * Author: P. Wild (pwild@cosy.sbg.ac.at)
 *
 * Calculates hamming distance of iris codes
 *
 */
#include "version.h"
#include <cstdio>
#include <map>
#include <vector>
#include <string>
#include <cstring>
#include <algorithm>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

using namespace std;
using namespace cv;

/** no globbing in win32 mode **/
int _CRT_glob = 0;

/** Algorithms **/
static const int ALG_MINHD = 0, ALG_MAXHD = 1, ALG_SSF = 2;
/** Program modes **/
static const int MODE_MAIN = 1, MODE_HELP = 2;

static const int htlut[256] = {0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8};

/*
 * Print command line usage for this program
 */
void printUsage() {
    printVersion();
	printf("+-----------------------------------------------------------------------------+\n");
	printf("| hd - calculates hamming distance of iris codes                              |\n");
	printf("|                                                                             |\n");
	printf("| MODES                                                                       |\n");
	printf("|                                                                             |\n");
	printf("| (# 1) HD calculation of the input images (cross comparison)                 |\n");
	printf("| (# 2) usage                                                                 |\n");
	printf("|                                                                             |\n");
	printf("| ARGUMENTS                                                                   |\n");
	printf("|                                                                             |\n");
	printf("+------+------------+---+---+-------------------------------------------------+\n");
	printf("| Name | Parameters | # | ? | Description                                     |\n");
	printf("+------+------------+---+---+-------------------------------------------------+\n");
	printf("| -i   | infile1    | 1 | N | source/reference iris codes (use * as wildcard, |\n");
	printf("|      | infile2    |   |   | all other files may refer to n-th * with ?n)    |\n");
	printf("| -s   | param+     | 1 | Y | min max: min/max (0 0) number of bit shifts     |\n");
	printf("|      |            |   | Y | img: shifted src (?n = n-th * in infile1,* =any)|\n");
    printf("| -ss  | shiftstep  |   | Y | Number of grouped bits to shift for one step of |\n");
    printf("|      |            |   |   | -s shift.                                       |\n");
	printf("| -m   | maskfile1  | 1 | Y | source/reference iris masks (?n/!n = n-th * in  |\n");
	printf("|      | maskfile2  |   |   | infile1/img, ?n = n-th * in infile2)            |\n");
	printf("| -a   | algorithm  | 1 | Y | HD-based algorithm (minhd)                      |\n");
	printf("|      |            |   |   | minhd: minimum HD for all shifts                |\n");
	printf("|      |            |   |   | maxhd: 1-maximum HD for all shifts              |\n");
	printf("|      |            |   |   | ssf: shift score fusion                         |\n");
	printf("| -n   | from to    | 1 | Y | starting (0) and ending bit (MAX)               |\n");
	printf("| -o   | outfile    | 1 | Y | target text                                     |\n");
	printf("| -owp |            | 1 | Y | write full paths in outfile instead of file only|\n");
	printf("| -q   |            | 1 | Y | quiet mode on (off)                             |\n");
	printf("| -t   |            | 1 | Y | time progress on (off)                          |\n");
	printf("| -#   |            | 1 | N | use memoization if memory is not a concern.     |\n");
	printf("| -b   |            | 1 | N | also record the bit shift at which the HD occurs|\n");
	printf("| -h   |            | 2 | N | prints usage                                    |\n");
	printf("+------+------------+---+---+-------------------------------------------------+\n");
	printf("|                                                                             |\n");
	printf("| EXAMPLE USAGE                                                               |\n");
    printf("|                                                                             |\n");
    printf("| -i s1.png s2.png -m s1_mask.png s2_mask.png -o compare.txt                  |\n");
    printf("| -i *.png *.png -s -7 7 -o compare.txt -q -t                                 |\n");
    printf("| -i *.png *.png -a ssf -s ?1_shifted_*.png -7 7 -o compare.txt -q -t         |\n");
    printf("|                                                                             |\n");
	printf("| AUTHOR                                                                      |\n");
	printf("|                                                                             |\n");
	printf("| Peter Wild (pwild@cosy.sbg.ac.at)                                           |\n");
	printf("|                                                                             |\n");
	printf("| COPYRIGHT                                                                   |\n");
	printf("|                                                                             |\n");
	printf("| (C) 2012 All rights reserved. Do not distribute without written permission. |\n");
	printf("+-----------------------------------------------------------------------------+\n");
}

/**
 * Fast hamming distance estimation
 * a: sample iris code
 * b: reference iris code
 * start8: starting 8-bit block
 * stop8: ending 8-bit block
 */
unsigned int hd(const Mat a, const Mat b, const unsigned int start8, const unsigned int stop8, const Mat mask = Mat()){
	unsigned int dist = 0;
	MatConstIterator_<uchar> pa = a.begin<uchar>();
	MatConstIterator_<uchar> pb = b.begin<uchar>();
	MatConstIterator_<uchar> enda = pa + stop8;
	pa += start8;
	pb += start8;
	if (!mask.empty()){
		MatConstIterator_<uchar> pm = mask.begin<uchar>();
		//unsigned int * pm = (unsigned int*) mask.data;
		pm += start8;
		for (;pa<enda; pa++, pb++, pm++){
			dist += htlut[(*pa ^*pb) & *pm];

			/*unsigned int val = (*pa ^*pb) & *pm;
			while(val)
			{
				++dist;
				val &= val - 1;
			}*/
		}
	}
	else {
		for (;pa<enda; pa++, pb++){
			//unsigned int val = *pa ^*pb;
			dist += htlut[*pa ^*pb];
			/*
			while(val)
			{
				++dist;
				val &= val - 1;
			}*/
		}
	}
	return dist;

}

/**
 * Shifts source by a given shift count
 * src: source iris code
 * dst: destination (shifted) iris code
 * shifts: shift count (positive values indicate left shifts)
 */
void shift(const Mat src, Mat dst, const int shifts){
	int size = src.cols*src.rows; // size in bytes
	MatConstIterator_<uchar> psrc = src.begin<uchar>();
	MatConstIterator_<uchar> endsrc = src.end<uchar>();
	MatIterator_<uchar> pdst = dst.begin<uchar>();
	MatIterator_<uchar> enddst = dst.end<uchar>();

	if (shifts >= 0){ // left shift
		unsigned int offset = shifts / 8;
		psrc += offset;
		unsigned int shiftCount = shifts % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		for (;pdst<enddst; pdst++){
			*pdst = (*psrc << shiftCount);
			psrc++;
			if (psrc == endsrc) psrc = src.begin<uchar>();
			*pdst |= (*psrc >> (ishiftCount));
		}
	}
	else { // right shift
		unsigned int offset = (-shifts) / 8;
		offset = (size-offset-1) % size;
		psrc += offset;
		unsigned int shiftCount = (-shifts) % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		for (;pdst<enddst; pdst++){
			*pdst = (*psrc << ishiftCount);
			psrc++;
			if (psrc == endsrc) psrc = src.begin<uchar>();
			*pdst |= (*psrc >> (shiftCount));
		}
	}
}

/**
 * Shifts matrix a by a given shift count and intersects result with b
 * a: sample source iris mask
 * b: reference source iris mask
 * dst: destination (shifted) iris mask
 * shifts: shift count (positive values indicate left shifts)
 */
void intersectShifted(const Mat a, const Mat b, Mat dst, const int shifts){
	uchar * pb = b.data;
	uchar * pa = a.data;
	uchar * pdst = dst.data;
	unsigned int size = dst.cols;
	unsigned int offset;
	if (shifts >= 0){ // left shift
		offset = shifts / 8;
		uchar * enddst = pdst + dst.cols;
		unsigned int shiftCount = shifts % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		offset = (size + offset) % size;
		for (;pdst<enddst; pdst++, pb++){
			*pdst = (pa[offset] << shiftCount);
			offset = ((offset+1)%size);
			*pdst |= (pa[offset] >> (ishiftCount));
			*pdst &= *pb;
		}
	}
	else { // right shift
		offset = (-shifts) / 8;
		uchar * enddst = pdst + dst.cols;
		unsigned int shiftCount = (-shifts) % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		offset = (size-offset-1) % size;
		for (;pdst<enddst; pdst++, pb++){
			*pdst = (pa[offset] << ishiftCount);
			offset = ((offset+1)%size);
			*pdst |= (pa[offset] >> (shiftCount));
			*pdst &= *pb;
		}
	}
}

/**
 * determines the best fractional Hamming Distance of two iris codes
 * a: first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * shifts: number of shifts
 * aMask: mask for first iris code
 * bMask: mask for second iris code
 */
std::pair<double,int> minHD(const Mat& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const int minShifts, const int maxShifts,const int shiftStep, const Mat aMask, const Mat bMask = Mat()){
    std::pair<double, int> result = {1,0};
	if (!aMask.empty() && !bMask.empty()){
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 1;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int ss=minShifts; ss<=maxShifts; ss++){
            int s = ss*shiftStep;
			shift(a,imgSmplShifted,s);
			intersectShifted(aMask,bMask,mask,s);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(imgSmplShifted,b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = ss;
            }
		}
		result.first = hamdist;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int ss=minShifts; ss<=maxShifts; ss++){
            int s = ss*shiftStep;
			shift(a,imgSmplShifted,s);
			unsigned int shiftedHamdist = hd(imgSmplShifted,b,start8,stop8);
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = ss;
            }
		}
		result.first = (((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines the best fractional Hamming Distance of an iris code with a list of shifted versions of this code
 * a: shifted versions of first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * aMask: masks for corresponding shifted versions of first iris code
 * bMask: mask for second iris code
 */
std::pair<double, int> minHD(const vector<Mat>& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const vector<Mat>& aMask, const Mat bMask = Mat()){
    std::pair<double, int> result = {1,0};
	if (!aMask.empty() && !bMask.empty()){
		CV_Assert(aMask.size() == a.size());
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 1;
		for (unsigned int i=0; i<a.size();i++){
			intersectShifted(aMask[i],bMask,mask,0);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(a[i],b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = i;
            }
		}
		result.first = hamdist;
	} else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		for (unsigned int i=0; i<a.size();i++){
			unsigned int shiftedHamdist = hd(a[i],b,start8,stop8);
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = i;
            }
		}
		result.first = (((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines the worst fractional Hamming Distance of two iris codes
 * a: first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * shifts: number of shifts
 * aMask: mask for first iris code
 * bMask: mask for second iris code
 */
std::pair<double, int> maxHD(const Mat& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const int minShifts, const int maxShifts, const int shiftStep, const Mat aMask, const Mat bMask = Mat()){
    std::pair<double, int> result = {0,0};
	if (!aMask.empty() && !bMask.empty()){
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int ss=minShifts; ss<=maxShifts; ss++){
            int s = ss*shiftStep;
			shift(a,imgSmplShifted,s);
			intersectShifted(aMask,bMask,mask,s);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(imgSmplShifted,b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist > hamdist){
                hamdist = shiftedHamdist;
                result.second = ss;
            }
		}
		result.first = 1 - hamdist;
	}
	else {

		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int ss=minShifts; ss<=maxShifts; ss++){
            int s = ss*shiftStep;
			shift(a,imgSmplShifted,s);
			unsigned int shiftedHamdist = hd(imgSmplShifted,b,start8,stop8);
			if (shiftedHamdist > hamdist){
                hamdist = shiftedHamdist;
                result.second = ss;
            }
		}
		result.first = 1-(((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines the worst fractional Hamming Distance of an iris code with a list of shifted versions of this code
 * a: shifted versions of first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * aMask: masks for corresponding shifted versions of first iris code
 * bMask: mask for second iris code
 */
std::pair<double, int> maxHD(const vector<Mat>& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const vector<Mat>& aMask, const Mat bMask = Mat()){
	std::pair<double,int>  result = {1,0};
	if (!aMask.empty() && !bMask.empty()){
		CV_Assert(aMask.size() == a.size());
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			intersectShifted(aMask[i],bMask,mask,0);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(a[i],b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist > hamdist){
                hamdist = shiftedHamdist;
                result.second = i;
            }
		}
		result.first = 1 - hamdist;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			unsigned int shiftedHamdist = hd(a[i],b,start8,stop8);
			if (shiftedHamdist > hamdist){
                hamdist = shiftedHamdist;
                result.second = i;
            }
		}
		result.first = 1- (((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines Shifting Score Fusion of two iris codes
 * a: first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * shifts: number of shifts
 * aMask: mask for first iris code
 * bMask: mask for second iris code
 */
std::pair<double,int>  ssf(const Mat& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const int minShifts, const int maxShifts, const int shiftStep, const Mat aMask, const Mat bMask = Mat()){
	std::pair<double,int>  result = {0,666};
	if (!aMask.empty() && !bMask.empty()){
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		intersectShifted(aMask,bMask,mask,0);
		double hamdist = 1;
		double maxhamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int ss=minShifts; ss<=maxShifts; ss++){
            int s = ss*shiftStep;
			shift(a,imgSmplShifted,s);
			intersectShifted(aMask,bMask,mask,s);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(imgSmplShifted,b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = ss;
            }
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result.first = ((1-maxhamdist) +  hamdist)/2;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		unsigned int maxhamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int ss=minShifts; ss<=maxShifts; ss++){
            int s = ss*shiftStep;
			shift(a,imgSmplShifted,s);
			unsigned int shiftedHamdist = hd(imgSmplShifted,b,start8,stop8);
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = ss;
            }
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result.first = ((1-(((double)maxhamdist) / (codeLengthBits))) + (((double)hamdist) / (codeLengthBits)))/2;
	}
	return result;
}

/**
 * determines Shifting Score Fusion of an iris code with a list of shifted versions of this code
 * a: shifted versions of first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * aMask: masks for corresponding shifted versions of first iris code
 * bMask: mask for second iris code
 */
std::pair<double,int>  ssf(const vector<Mat>& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const vector<Mat>& aMask, const Mat bMask = Mat()){
	std::pair<double,int>  result = {0,0};
	if (!aMask.empty() && !bMask.empty()){
		CV_Assert(aMask.size() == a.size());
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		int codeLengthBits = 0;
		double hamdist = 1;
		double maxhamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			intersectShifted(aMask[i],bMask,mask,0);
			codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(a[i],b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = i;
            }
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result.first = ((1-maxhamdist) +  hamdist)/2;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		unsigned int maxhamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			unsigned int shiftedHamdist = hd(a[i],b,start8,stop8);
			if (shiftedHamdist < hamdist){
                hamdist = shiftedHamdist;
                result.second = i;
            }
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result.first = ((1-(((double)maxhamdist) / (codeLengthBits))) + (((double)hamdist) / (codeLengthBits)))/2;
	}
	return result;
}

/** ------------------------------- commandline functions ------------------------------- **/

/**
 * Parses a command line
 * This routine should be called for parsing command lines for executables.
 * Note, that all options require '-' as prefix and may contain an arbitrary
 * number of optional arguments.
 *
 * cmd: commandline representation
 * argc: number of parameters
 * argv: string array of argument values
 */
void cmdRead(map<string ,vector<string> >& cmd, int argc, char *argv[]){
	for (int i=1; i< argc; i++){
		char * argument = argv[i];
		if (strlen(argument) > 1 && argument[0] == '-' && (argument[1] < '0' || argument[1] > '9')){
			cmd[argument]; // insert
			char * argument2;
			while (i + 1 < argc && (strlen(argument2 = argv[i+1]) <= 1 || argument2[0] != '-'  || (argument2[1] >= '0' && argument2[1] <= '9'))){
				cmd[argument].push_back(argument2);
				i++;
			}
		}
		else {
			CV_Error(CV_StsBadArg,"Invalid command line format");
		}
	}
}

/**
 * Checks, if each command line option is valid, i.e. exists in the options array
 *
 * cmd: commandline representation
 * validOptions: list of valid options separated by pipe (i.e. |) character
 */
void cmdCheckOpts(map<string ,vector<string> >& cmd, const string validOptions){
	vector<string> tokens;
	const string delimiters = "|";
	string::size_type lastPos = validOptions.find_first_not_of(delimiters,0); // skip delimiters at beginning
	string::size_type pos = validOptions.find_first_of(delimiters, lastPos); // find first non-delimiter
	while (string::npos != pos || string::npos != lastPos){
		tokens.push_back(validOptions.substr(lastPos,pos - lastPos)); // add found token to vector
		lastPos = validOptions.find_first_not_of(delimiters,pos); // skip delimiters
		pos = validOptions.find_first_of(delimiters,lastPos); // find next non-delimiter
	}
	sort(tokens.begin(), tokens.end());
	for (map<string, vector<string> >::iterator it = cmd.begin(); it != cmd.end(); it++){
		if (!binary_search(tokens.begin(),tokens.end(),it->first)){
			CV_Error(CV_StsBadArg,"Command line parameter '" + it->first + "' not allowed.");
			tokens.clear();
			return;
		}
	}
	tokens.clear();
}

/*
 * Checks, if a specific required option exists in the command line
 *
 * cmd: commandline representation
 * option: option name
 */
void cmdCheckOptExists(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it == cmd.end()) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is required, but does not exist.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * size: appropriate number of parameters for the option
 */
void cmdCheckOptSize(map<string ,vector<string> >& cmd, const string option, const unsigned int size = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it->second.size() != size) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' has unexpected size.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * min: minimum appropriate number of parameters for the option
 * max: maximum appropriate number of parameters for the option
 */
void cmdCheckOptRange(map<string ,vector<string> >& cmd, string option, unsigned int min = 0, unsigned int max = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	unsigned int size = it->second.size();
	if (size < min || size > max) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is out of range.");
}

/*
 * Returns the list of parameters for a given option
 *
 * cmd: commandline representation
 * option: name of the option
 */
vector<string> * cmdGetOpt(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? &(it->second) : 0;
}

/*
 * Returns number of parameters in an option
 *
 * cmd: commandline representation
 * option: name of the option
 */
unsigned int cmdSizePars(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? it->second.size() : 0;
}

/*
 * Returns a specific parameter type (int) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
int cmdGetParInt(map<string ,vector<string> >& cmd, string option, unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atoi(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (float) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
float cmdGetParFloat(map<string ,vector<string> >& cmd, const string option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atof(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (string) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
string cmdGetPar(map<string ,vector<string> >& cmd, const string option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return it->second[param];
		}
	}
	return 0;
}

/** ------------------------------- timing functions ------------------------------- **/

/**
 * Class for handling timing progress information
 */
class Timing{
public:
	/** integer indicating progress with respect tot total **/
	int progress;
	/** total count for progress **/
	int total;

	/*
	 * Default constructor for timing initializing time.
	 * Automatically calls init()
	 *
	 * seconds: update interval in seconds
	 * eraseMode: if true, outputs sends erase characters at each print command
	 */
	Timing(long seconds, bool eraseMode){
		updateInterval = seconds;
		progress = 1;
		total = 100;
		eraseCount=0;
		erase = eraseMode;
		init();
	}

	/*
	 * Destructor
	 */
	~Timing(){}

	/*
	 * Initializes timing variables
	 */
	void init(void){
		start = boost::posix_time::microsec_clock::universal_time();
		lastPrint = start - boost::posix_time::seconds(updateInterval);
	}

	/*
	 * Clears printing (for erase option only)
	 */
	void clear(void){
		string erase(eraseCount,'\r');
		erase.append(eraseCount,' ');
		erase.append(eraseCount,'\r');
		printf("%s",erase.c_str());
		eraseCount = 0;
	}

	/*
	 * Updates current time and returns true, if output should be printed
	 */
	bool update(void){
		current = boost::posix_time::microsec_clock::universal_time();
		return ((current - lastPrint > boost::posix_time::seconds(updateInterval)) || (progress == total));
	}

	/*
	 * Prints timing object to STDOUT
	 */
	void print(void){
		lastPrint = current;
		float percent = 100.f * progress / total;
		boost::posix_time::time_duration passed = (current - start);
		boost::posix_time::time_duration togo = passed * (total - progress) / max(1,progress);
		if (erase) {
			string erase(eraseCount,'\r');
			printf("%s",erase.c_str());
			int newEraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
			if (newEraseCount < eraseCount) {
				string erase(newEraseCount-eraseCount,' ');
				erase.append(newEraseCount-eraseCount,'\r');
				printf("%s",erase.c_str());
			}
			eraseCount = newEraseCount;
		}
		else {
			eraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
		}
	}
private:
	long updateInterval;
	boost::posix_time::ptime start;
	boost::posix_time::ptime current;
	boost::posix_time::ptime lastPrint;
	int eraseCount;
	bool erase;
};

/** ------------------------------- file pattern matching functions ------------------------------- **/

/*
 * Formats a given string, such that it can be used as a regular expression
 * I.e. escapes special characters and uses * and ? as wildcards
 *
 * pattern: regular expression path pattern
 * pos: substring starting index
 * n: substring size
 *
 * returning: escaped substring
 */
string patternSubstrRegex(string& pattern, size_t pos, size_t n){
	string result;
	for (size_t i=pos, e=pos+n; i < e; i++ ) {
		char c = pattern[i];
		if ( c == '\\' || c == '.' || c == '+' || c == '[' || c == '{' || c == '|' || c == '(' || c == ')' || c == '^' || c == '$' || c == '}' || c == ']') {
			result.append(1,'\\');
			result.append(1,c);
		}
		else if (c == '*'){
			result.append("([^/\\\\]*)");
		}
		else if (c == '?'){
			result.append("([^/\\\\])");
		}
		else {
			result.append(1,c);
		}
	}
	return result;
}

/*
 * Converts a regular expression path pattern into a list of files matching with this pattern by replacing wildcards
 * starting in position pos assuming that all prior wildcards have been resolved yielding intermediate directory path.
 * I.e. this function appends the files in the specified path according to yet unresolved pattern by recursive calling.
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 * pos: an index such that positions 0...pos-1 of pattern are already considered/matched yielding path
 * path: the current directory (or empty)
 */
void patternToFiles(string& pattern, vector<string>& files, const size_t& pos, const string& path){
	size_t first_unknown = pattern.find_first_of("*?",pos); // find unknown * in pattern
	if (first_unknown != string::npos){
		size_t last_dirpath = pattern.find_last_of("/\\",first_unknown);
		size_t next_dirpath = pattern.find_first_of("/\\",first_unknown);
		if (next_dirpath != string::npos){
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,next_dirpath-last_dirpath-1) : patternSubstrRegex(pattern,pos,next_dirpath-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr( ((path.length() > 0) ? path + pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					if (boost::filesystem::is_directory(itr->path())){
						boost::filesystem::path p = itr->path().filename();
						string s =  p.string();
						if (boost::regex_match(s.c_str(), expr)){
							patternToFiles(pattern,files,(int)(next_dirpath+1),((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
						}
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
		else {
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,pattern.length()-last_dirpath-1) : patternSubstrRegex(pattern,pos,pattern.length()-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr(((path.length() > 0) ? path +  pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					boost::filesystem::path p = itr->path().filename();
					string s =  p.string();
					if (boost::regex_match(s.c_str(), expr)){
						files.push_back(((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
	}
	else { // no unknown symbols
		boost::filesystem::path file(((path.length() > 0) ? path + "/" : "") + pattern.substr(pos,pattern.length()-pos));
		if (boost::filesystem::exists(file)){
			files.push_back(file.string());
		}
	}
}

/**
 * Converts a regular expression path pattern into a list of files matching with this pattern
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 */
void patternToFiles(string& pattern, vector<string>& files){
	patternToFiles(pattern,files,0,"");
}

/*
 * Renames a given filename corresponding to the actual file pattern using a renaming pattern.
 * Wildcards can be referred to as ?1, ?2, ... in the order they appeared in the file pattern.
 *
 * pattern: regular expression path pattern
 * renamePattern: renaming pattern using ?1, ?2, ... as placeholders for wildcards
 * infile: path of the file (matching with pattern) to be renamed
 * outfile: path of the renamed file
 * par: used parameter (default: '?')
 */
void patternFileRename(string& pattern, const string& renamePattern, const string& infile, string& outfile, const char par = '?'){
	size_t first_unknown = renamePattern.find_first_of(par,0); // find unknown ? in renamePattern
	if (first_unknown != string::npos){
		string formatOut = "";
		for (size_t i=0, e=renamePattern.length(); i < e; i++ ) {
			char c = renamePattern[i];
			if ( c == par && i+1 < e) {
				c = renamePattern[i+1];
				if (c > '0' && c <= '9'){
					formatOut.append(1,'$');
					formatOut.append(1,c);
				}
				else {
					formatOut.append(1,par);
					formatOut.append(1,c);
				}
				i++;
			}
			else {
				formatOut.append(1,c);
			}
		}
		boost::regex patternOut(patternSubstrRegex(pattern,0,pattern.length()));
		outfile = boost::regex_replace(infile,patternOut,formatOut,boost::match_default | boost::format_perl);
	} else {
		outfile = renamePattern;
	}
}

string skipPath(string from){
    string::size_type idx = from.find_last_of("/\\"); // / is for linux, \\ is for windows
    if( idx == string::npos ) return from; // not found, do nothing
    return from.substr(idx+1); // return after path
}

/** ------------------------------- Program ------------------------------- **/

bool use_mem = false;
Mat imread_mem( const string& filename, int flags=1){
    if( !use_mem) return imread(filename, flags);
    static map< std::pair<string, int>, Mat> memmap;
    auto key = std::make_pair(filename, flags);
    auto memitem = memmap.find( key );
    Mat ret;
    if( memitem != memmap.end()){
        ret =  memitem->second;
    } else{
        memmap[ key ]=imread(filename, flags);
        ret = memmap[ key ];
    }
    return ret;
}

/*
 * Main program
 */
int main(int argc, char *argv[])
{
	int mode = MODE_HELP;
	map<string,vector<string> > cmd;
	try {
		cmdRead(cmd,argc,argv);
		if (cmd.size() == 0 || cmdGetOpt(cmd,"-h") != 0) mode = MODE_HELP;
		else mode = MODE_MAIN;
		if (mode == MODE_MAIN){
			// validate command line
			cmdCheckOpts(cmd,"-i|-m|-s|-ss|-a|-n|-o|-owp|-q|-t|-#|-b");
			cmdCheckOptExists(cmd,"-i");
			cmdCheckOptSize(cmd,"-i",2);
			string infilesSmpl = cmdGetPar(cmd,"-i",0);
			string infilesRef = cmdGetPar(cmd,"-i",1);
			bool masks = (cmdGetOpt(cmd,"-m") != 0);
			if (masks) cmdCheckOptSize(cmd,"-m",2);
			string masksSmpl = ((masks) ? cmdGetPar(cmd,"-m",0) : "");
			string masksRef = ((masks) ? cmdGetPar(cmd,"-m",1) : "");
			bool shiftedfiles = (cmdGetOpt(cmd,"-s") != 0 && cmdSizePars(cmd,"-s") == 1);
			string shiftfiles = ((shiftedfiles) ? cmdGetPar(cmd,"-s") : "");
			int minShifts = 0;
			int maxShifts = 0;
			if (cmdGetOpt(cmd,"-s") != 0 && !shiftedfiles){
				cmdCheckOptSize(cmd,"-s",2);
				minShifts = cmdGetParInt(cmd,"-s",0);
				maxShifts = cmdGetParInt(cmd,"-s",1);
			}
			int shiftStep = 1;
			if (cmdGetOpt(cmd,"-ss") != 0 && !shiftedfiles){
				cmdCheckOptSize(cmd,"-ss",1);
				shiftStep = cmdGetParInt(cmd,"-ss",0);
			}
			int alg = ALG_MINHD;
			if (cmdGetOpt(cmd,"-a") != 0){
				cmdCheckOptSize(cmd,"-a",1);
				string algo = cmdGetPar(cmd,"-a");
				if (algo == "maxhd"){
					alg = ALG_MAXHD;
				}
				else if (algo == "ssf"){
					alg = ALG_SSF;
				}
			}
			unsigned int from = 0;
			unsigned int to = INT_MAX;
			if (cmdGetOpt(cmd,"-n") != 0){
				cmdCheckOptSize(cmd,"-n",2);
				from = cmdGetParInt(cmd,"-n",0);
				CV_Assert(from % 8 == 0);
				from /= 8;
				to = cmdGetParInt(cmd,"-n",1);
				CV_Assert(to % 8 == 0);
				to /= 8;
			}
			string outfile;
            bool outfile_with_path = false;
			if (cmdGetOpt(cmd,"-o") != 0){
				cmdCheckOptSize(cmd,"-o",1);
				outfile = cmdGetPar(cmd,"-o");
                if( cmdGetOpt(cmd, "-owp") != 0){
				    cmdCheckOptSize(cmd,"-owp",0);
                    outfile_with_path = true;
                }
			}
			bool quiet = false;
			if (cmdGetOpt(cmd,"-q") != 0){
				cmdCheckOptSize(cmd,"-q",0);
				quiet = true;
			}
            bool writebitshift = false;
			if (cmdGetOpt(cmd,"-b") != 0){
				cmdCheckOptSize(cmd,"-b",0);
                writebitshift = true;
			}
            //global use_mem
			if (cmdGetOpt(cmd,"-#") != 0){
				cmdCheckOptSize(cmd,"-#",0);
				use_mem = true;
			}
			bool time = false;
			if (cmdGetOpt(cmd,"-t") != 0){
				cmdCheckOptSize(cmd,"-t",0);
				time = true;
			}
			// starting routine
			Timing timing(1,quiet);
			vector<string> filesSmpl;
			patternToFiles(infilesSmpl,filesSmpl);
			vector<string> filesRef;
			patternToFiles(infilesRef,filesRef);
			CV_Assert(filesSmpl.size() > 0);
			CV_Assert(filesRef.size() > 0);
			timing.total = filesSmpl.size() * filesRef.size();
			ofstream cfile;
			if (!outfile.empty()){
				if (!quiet) printf("Opening result file '%s' ...\n", outfile.c_str());;
				cfile.open(outfile.c_str(),ios::out | ios::trunc);
				if (!(cfile.is_open())) {
					CV_Error(CV_StsError,"Could not open result file '" + outfile + "'");
				}
			}
			for (vector<string>::iterator infileSmpl = filesSmpl.begin(); infileSmpl != filesSmpl.end(); ++infileSmpl){
				vector<Mat> imgSmpl;
				vector<Mat> maskSmpl;
				if (shiftedfiles){
					string shiftfile;
					patternFileRename(infilesSmpl,shiftfiles,*infileSmpl,shiftfile);
					vector<string> shiftsSmpl;
					patternToFiles(shiftfile,shiftsSmpl);
					// now load virtual files
					for (vector<string>::iterator shiftSmpl = shiftsSmpl.begin(); shiftSmpl != shiftsSmpl.end(); ++shiftSmpl){
						Mat img = imread_mem(*shiftSmpl, CV_LOAD_IMAGE_UNCHANGED);
						CV_Assert(img.data != 0);
						CV_Assert(img.type() == CV_8UC1);
						if (imgSmpl.size() > 0) { CV_Assert(imgSmpl.back().size() == img.size());}
						imgSmpl.push_back(img);
						if (masks){
							string maskfile1, maskfile2;
							patternFileRename(infilesSmpl,masksSmpl,*infileSmpl,maskfile1);
							patternFileRename(infilesSmpl,maskfile1,*infileSmpl,maskfile2,'!');
							Mat msk = imread_mem(maskfile2, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(msk.data != 0);
							CV_Assert(msk.type() == CV_8UC1);
							CV_Assert(img.size() == msk.size());
							maskSmpl.push_back(msk);
						}
					}
				}
				else {
					Mat img = imread(*infileSmpl, CV_LOAD_IMAGE_UNCHANGED);
					CV_Assert(img.data != 0);
					CV_Assert(img.type() == CV_8UC1);
					imgSmpl.push_back(img);
					if (masks){
						string maskSmplFile;
						patternFileRename(infilesSmpl,masksSmpl,*infileSmpl,maskSmplFile);
						Mat msk = imread_mem(maskSmplFile, CV_LOAD_IMAGE_UNCHANGED);
						CV_Assert(msk.data != 0);
						CV_Assert(msk.type() == CV_8UC1);
						CV_Assert(msk.size() == img.size());
						maskSmpl.push_back(msk);
					}
				}
				Size codeSize = imgSmpl[0].size();
				unsigned int codeLength = codeSize.height * codeSize.width;
				unsigned int bitStop = min(to,codeLength);
				//CV_Assert(codeLength % sizeof(int) == 0);
				for (vector<string>::iterator infileRef = filesRef.begin(); infileRef != filesRef.end(); ++infileRef, timing.progress++){
					Mat imgRef = imread_mem(*infileRef, CV_LOAD_IMAGE_UNCHANGED);
					CV_Assert(imgRef.data != 0);
					CV_Assert(imgRef.type() == CV_8UC1);
					CV_Assert(imgRef.size() == codeSize);
					Mat maskRef;
					if (masks){
						string maskRefFile;
						patternFileRename(infilesRef,masksRef,*infileRef,maskRefFile);
						maskRef = imread_mem(maskRefFile, CV_LOAD_IMAGE_UNCHANGED);
						CV_Assert(maskRef.data != 0);
						CV_Assert(maskRef.type() == CV_8UC1);
						CV_Assert(maskRef.size() == codeSize);
					}
					else {
						maskRef = Mat();
					}
                    std::pair<double, int> score = (alg == ALG_MINHD) ? (shiftedfiles) ? minHD(imgSmpl,imgRef,from,bitStop, maskSmpl, maskRef) : minHD(imgSmpl[0],imgRef,from,bitStop,minShifts, maxShifts, shiftStep, (maskSmpl.size() > 0) ? maskSmpl[0] : Mat(), maskRef) :
								(alg == ALG_MAXHD) ? (shiftedfiles) ? maxHD(imgSmpl,imgRef,from,bitStop, maskSmpl, maskRef) : maxHD(imgSmpl[0],imgRef,from,bitStop,minShifts, maxShifts, shiftStep, (maskSmpl.size() > 0) ? maskSmpl[0] : Mat(), maskRef) :
								(shiftedfiles) ? ssf(imgSmpl,imgRef,from,bitStop,maskSmpl, maskRef) : ssf(imgSmpl[0],imgRef,from,bitStop,minShifts, maxShifts, shiftStep, (maskSmpl.size() > 0) ? maskSmpl[0] : Mat(), maskRef);
					if (!quiet){
                        if (writebitshift)
                            printf("hd(%s,%s) = %f at %d bits\n",(*infileSmpl).c_str(), (*infileRef).c_str(), score.first, score.second);
                        else
                            printf("hd(%s,%s) = %f\n",(*infileSmpl).c_str(), (*infileRef).c_str(), score.first);
                    }

					if (!outfile.empty() && cfile.is_open()){
						if( outfile_with_path){
                            cfile << *infileSmpl << " " << *infileRef;
                        } else {
    						cfile << skipPath(*infileSmpl) << " " << skipPath(*infileRef);
                        }
                        cfile  << " " << score.first;
                        if( writebitshift) cfile << " " << score.second;
                        cfile << endl;
					}
					if (time && timing.update()) timing.print();
				}
			}
			if (time && quiet) timing.clear();
			if (!outfile.empty() && cfile.is_open()){
				cfile.close();
			}
    	}
    	else if (mode == MODE_HELP){
			// validate command line
			cmdCheckOpts(cmd,"-h");
			if (cmdGetOpt(cmd,"-h") != 0) cmdCheckOptSize(cmd,"-h",0);
			// starting routine
			printUsage();
    	}
    }
	catch (...){
	   	printf("Exit with errors.\n");
	   	exit(EXIT_FAILURE);
	}
    return EXIT_SUCCESS;
}


#include <vector>
#include <algorithm>
#include <set>
#include <iostream>
#include <armadillo>
#include <iomanip>

#include "Util.h"
#include "PostCal.h"

using namespace arma;

void printGSLPrint(mat &A, int row, int col) {
	for(int i = 0; i < row; i++) {
		for(int j = 0; j < col; j++)
			printf("%g ", A(i, j));
		printf("\n");
	}
}

string PostCal::convertConfig2String(int * config, int size) {
	string result = "0";
	for(int i = 0; i < size; i++)
		if(config[i]==1)
			result+= "_" + convertInt(i);
	return result;
}

// We compute dmvnorm(Zcc, mean=rep(0,nrow(Rcc)), Rcc + Rcc %*% Rcc) / dmvnorm(Zcc, rep(0, nrow(Rcc)), Rcc))
// togheter to avoid numerical over flow
double PostCal::fracdmvnorm(mat Z, mat mean, mat R, mat diagC, double NCP) {
        mat newR = R + R * diagC  * R;
        mat ZcenterMean = Z - mean;
        //mat res1 = trans(ZcenterMean) * inv(R) * (ZcenterMean);
        //mat res2 = trans(ZcenterMean) * inv(newR) *  (ZcenterMean);
	mat res1 = trans(ZcenterMean) * solve(R, eye(size(R))) * (ZcenterMean);
	mat res2 = trans(ZcenterMean) * solve(newR, eye(size(newR))) *  (ZcenterMean);
        double v1 = res1(0,0)/2-res2(0,0)/2;
	//CHANGE: MOVE FORM NORMAL CALCULATION TO LOG SPACE
        //return(exp(v1)/sqrt(det(newR))* sqrt(det(R)));
        return(v1 - log( sqrt(det(newR)) ) + log( sqrt(det(R)) ) );
}

// We compute dmvnorm(Zcc, mean=rep(0,nrow(Rcc)), Rcc + Rcc %*% Rcc) / dmvnorm(Zcc, rep(0, nrow(Rcc)), Rcc))
// togheter to avoid numerical over flow, We deal with singular LD matrix
// eign decomposition matrx R
// R = Q M Q^T where M is the diagonal matrix of eign values
double PostCal::fracdmvnorm2(mat Z, mat mean, mat R, mat diagC, double NCP) {
	int rowCount = R.n_rows;
	double MDet=1;
	mat Q = zeros(rowCount, rowCount);
	mat eignVec;
	vec eignVal;
	mat MHalfInv = zeros(rowCount, rowCount);
	mat MHalf = zeros(rowCount, rowCount);
	eig_sym(eignVal, eignVec, R);
	mat ZcenterMean = Z - mean;
	uvec indices;
	indices = sort_index(abs(ZcenterMean));
	for(int i = 0 ; i < rowCount; i++){
		if(eignVal[indices[i]] > 0) {
			MHalfInv(i,i) = 1/sqrt(eignVal[indices[i]]);
			MDet = MDet * eignVal[indices[i]];
			MHalf(i,i) = sqrt(eignVal[indices[i]]);
			for (int j = 0; j < rowCount; j++){
				Q(i,j) = eignVec(indices[i],j);
			}
		}
	}
	mat ZcenterMeanTilda = MHalfInv * Q.t() * ZcenterMean;
	//mat res1 = ZcenterMeanTilda.t() * ZcenterMeanTilda;
	//double v1 = -res1(0,0)/2 - log (sqrt(MDet));
	mat MHalfQ = MHalf * Q.t();
	//mat res2 = ZcenterMeanTilda.t() * inv(eye(rowCount, rowCount) + MHalfQ * diagC * MHalfQ.t())  * ZcenterMeanTilda;
	mat res3 = ZcenterMeanTilda.t() * (inv(eye(rowCount, rowCount) + MHalfQ * diagC * MHalfQ.t())-eye(rowCount,rowCount)) * ZcenterMeanTilda;
	//double v2 = -res2(0,0)/2 - log (sqrt(MDet*det(eye(rowCount, rowCount)+diagC*R)));
	double v3 = -res3(0,0)/2 - log (sqrt(MDet*det(eye(rowCount, rowCount)+diagC*R))) + log (sqrt(MDet));
	return (v3);
}

double PostCal::dmvnorm(mat Z, mat mean, mat R) {
        mat ZcenterMean = Z - mean;
        mat res = trans(ZcenterMean) * inv(R) * (ZcenterMean);
        double v1 = res(0,0);
        double v2 = log(sqrt(det(R)));
        return (exp(-v1/2-v2));
}

// cc=causal SNPs
// Rcc = LD of causal SNPs
// Zcc = Z-score of causal SNPs
// dmvnorm(Zcc, mean=rep(0,nrow(Rcc)), Rcc + Rcc %*% Rcc) / dmvnorm(Zcc, rep(0, nrow(Rcc)), Rcc))
//
double PostCal::fastLikelihood(int * configure, double * stat, double NCP) {
	int causalCount = 0;
	vector <int> causalIndex;
	for(int i = 0; i < snpCount; i++) {
		causalCount += configure[i];
		if(configure[i] == 1)
			causalIndex.push_back(i);
	}

	if (causalCount == 0) {
		int maxVal = 0;
		for(int i = 0; i < snpCount; i++) {
			if (maxVal < abs(stat[i]))
				maxVal = stat[i];
		}
	}

	mat Rcc(causalCount, causalCount, fill::zeros);
	mat Zcc(causalCount, 1, fill::zeros);
	mat mean(causalCount, 1, fill::zeros);
	mat diagC(causalCount, causalCount, fill::zeros);

	for (int i = 0; i < causalCount; i++){
		for(int j = 0; j < causalCount; j++) {
			Rcc(i,j) = sigmaMatrix(causalIndex[i], causalIndex[j]);
		}
		Zcc(i,0) = stat[causalIndex[i]];
		diagC(i,i) = NCP;
	}

	return fracdmvnorm(Zcc, mean, Rcc, diagC, NCP);
}

/*
 * This function is not depricated and invSigmaMatrix is not used anymore
 */
double PostCal::likelihood(int * configure, double * stat, double NCP) {
	int causalCount = 0;
	int index_C = 0;
        double matDet = 0;
	double res    = 0;

	for(int i = 0; i < snpCount; i++)
		causalCount += configure[i];
	if(causalCount == 0){
		mat tmpResultMatrix1N = statMatrixtTran * invSigmaMatrix;
		mat tmpResultMatrix11 = tmpResultMatrix1N * statMatrix;
		res = tmpResultMatrix11(0,0);
		matDet = sigmaDet;
		return( exp(-res/2)/sqrt(abs(matDet)) );
	}
	mat U(snpCount, causalCount, fill::zeros);
	mat V(causalCount, snpCount, fill::zeros);
	mat VU(causalCount, causalCount, fill::zeros);

	for(int i = 0; i < snpCount; i++) {
                if (configure[i] == 0)	continue;
                else {
                        for(int j = 0; j < snpCount; j++)
                                U(j, index_C) = sigmaMatrix(j,i);
			V(index_C, i) = NCP;
                        index_C++;
                }
        }
	VU = V * U;
	mat I_AA   = mat(snpCount, snpCount, fill::eye);
	mat tmp_CC = mat(causalCount, causalCount, fill::eye)+ VU;
	matDet = det(tmp_CC) * sigmaDet;
	mat tmp_AA = invSigmaMatrix - (invSigmaMatrix * U) * pinv(tmp_CC) * V ;
	//tmp_AA     = invSigmaMatrix * tmp_AA;
	mat tmpResultMatrix1N = statMatrixtTran * tmp_AA;
        mat tmpResultMatrix11 = tmpResultMatrix1N * statMatrix;
        res = tmpResultMatrix11(0,0);

	if(matDet==0) {
		cout << "Error the matrix is singular and we fail to fix it." << endl;
		exit(0);
	}
	/*
		We compute the log of -res/2-log(det) to see if it is too big or not.
		In the case it is too big we just make it a MAX value.
	*/
	double tmplogDet = log(sqrt(abs(matDet)));
	double tmpFinalRes = -res/2 - tmplogDet;
	if(tmpFinalRes > 700)
		return(exp(700));
	return( exp(-res/2)/sqrt(abs(matDet)) );
}

int PostCal::nextBinary(int * data, int size) {
	int i = 0;
	int total_one = 0;
	int index = size-1;
        int one_countinus_in_end = 0;

        while(index >= 0 && data[index] == 1) {
                index = index - 1;
                one_countinus_in_end = one_countinus_in_end + 1;
	}
	if(index >= 0) {
        	while(index >= 0 && data[index] == 0) {
               	 index = index - 1;
		}
	}
        if(index == -1) {
                while(i <  one_countinus_in_end+1 && i < size) {
                        data[i] = 1;
                        i=i+1;
		}
                i = 0;
                while(i < size-one_countinus_in_end-1) {
                        data[i+one_countinus_in_end+1] = 0;
                        i=i+1;
		}
	}
        else if(one_countinus_in_end == 0) {
                data[index] = 0;
                data[index+1] = 1;
	} else {
                data[index] = 0;
                while(i < one_countinus_in_end + 1) {
                        data[i+index+1] = 1;
			if(i+index+1 >= size)
				printf("ERROR3 %d\n", i+index+1);
                        i=i+1;
		}
                i = 0;
                while(i < size - index - one_countinus_in_end - 2) {
                        data[i+index+one_countinus_in_end+2] = 0;
			if(i+index+one_countinus_in_end+2 >= size) {
				printf("ERROR4 %d\n", i+index+one_countinus_in_end+2);
			}
                        i=i+1;
		}
	}
	i = 0;
	total_one = 0;
	for(i = 0; i < size; i++)
		if(data[i] == 1)
			total_one = total_one + 1;

	return(total_one);
}

double PostCal::computeTotalLikelihood(double * stat, double NCP) {
	int num = 0;
	double sumLikelihood = 0;
	double tmp_likelihood = 0;
	long int total_iteration = 0 ;
	int * configure = (int *) malloc (snpCount * sizeof(int *)); // original data

	for(long int i = 0; i <= maxCausalSNP; i++)
		total_iteration = total_iteration + nCr(snpCount, i);
	cout << "Max Causal=" << maxCausalSNP << endl;
	for(long int i = 0; i < snpCount; i++)
		configure[i] = 0;
	for(long int i = 0; i < total_iteration; i++) {
                tmp_likelihood = fastLikelihood(configure, stat, NCP) + num * log(gamma) + (snpCount-num) * log(1-gamma);
                sumLikelihood = addlogSpace(sumLikelihood, tmp_likelihood);
		for(int j = 0; j < snpCount; j++) {
                        postValues[j] = addlogSpace(postValues[j], tmp_likelihood * configure[j]);
		}
		histValues[num] = addlogSpace(histValues[num], tmp_likelihood);
		/*for (int j = 0; j < snpCount; j++) {
			if (configure[j] != 0)
				cout << j << ",";
		}
		cout << " " << tmp_likelihood << endl;*/
		num = nextBinary(configure, snpCount);
		//cout << i << " "  << exp(tmp_likelihood) << endl;
		if(i % 1000 == 0)
			cerr << "\r                                                                 \r" << (double) (i) / (double) total_iteration * 100.0 << "%";
	}
	for(int i = 0; i <= maxCausalSNP; i++)
		histValues[i] = exp(histValues[i]-sumLikelihood);
        free(configure);
        return(sumLikelihood);
}

bool PostCal::validConfigutation(int * configure, char * pcausalSet) {
	for(int i = 0; i < snpCount; i++){
		if(configure[i] == 1 && pcausalSet[i] == '0')
			return false;
	}
	return true;
}

/*
 * This is a auxilary function used to generate all possible causal set that
 * are selected in the p-causal set
*/
void PostCal::computeALLCausalSetConfiguration(double * stat, double NCP, char * pcausalSet, string outputFileName) {
	int num = 0;
        double sumLikelihood = 0;
        double tmp_likelihood = 0;
        long int total_iteration = 0 ;
        int * configure = (int *) malloc (snpCount * sizeof(int *)); // original data

        for(long int i = 0; i <= maxCausalSNP; i++)
                total_iteration = total_iteration + nCr(snpCount, i);
        for(long int i = 0; i < snpCount; i++)
                configure[i] = 0;
        for(long int i = 0; i < total_iteration; i++) {
		if (validConfigutation(configure, pcausalSet)) {
			//log space
                	tmp_likelihood = fastLikelihood(configure, stat, NCP) +  num * log(gamma) + (snpCount-num) * log(1-gamma);
			exportVector2File(outputFileName, configure, snpCount);
			export2File(outputFileName, tmp_likelihood);
		}
		num = nextBinary(configure, snpCount);
	}
}

/*
	stat is the z-scpres
	sigma is the correaltion matrix
	G is the map between snp and the gene (snp, gene)
*/
double PostCal::findOptimalSetGreedy(double * stat, double NCP, char * pcausalSet, int *rank,  double inputRho, string outputFileName) {
	int index = 0;
        double rho = 0;
        double total_post = 0;

        totalLikeLihoodLOG = computeTotalLikelihood(stat, NCP);

	export2File(outputFileName+".log", exp(totalLikeLihoodLOG)); //Output the total likelihood to the log File
	for(int i = 0; i < snpCount; i++)
		total_post = addlogSpace(total_post, postValues[i]);
	printf("Total Likelihood= %e SNP=%d \n", total_post, snpCount);

        std::vector<data> items;
        std::set<int>::iterator it;
	//output the poster to files
        for(int i = 0; i < snpCount; i++) {
             //printf("%d==>%e ",i, postValues[i]/total_likelihood);
             items.push_back(data(exp(postValues[i]-total_post), i, 0));
        }
        printf("\n");
        std::sort(items.begin(), items.end(), by_number());
        for(int i = 0; i < snpCount; i++)
                rank[i] = items[i].index1;

        for(int i = 0; i < snpCount; i++)
                pcausalSet[i] = '0';
        do{
                rho += exp(postValues[rank[index]]-total_post);
                pcausalSet[rank[index]] = '1';
                printf("%d %e\n", rank[index], rho);
                index++;
        } while( rho < inputRho);

        printf("\n");
	return(0);
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

#ifndef SMOOTH__MANIFOLD_VECTOR_HPP_
#define SMOOTH__MANIFOLD_VECTOR_HPP_

#include <Eigen/Sparse>
#include <numeric>

#include "manifold.hpp"

namespace smooth {

/**
 * @brief \p std::vector based Manifold container.
 *
 * Convenient to treat a collection
 * \f[
 *   m = (m_1, m_2, ..., m_k) \in M \times M \times ... \times M
 * \f]
 * of Manifold elements as a single Manifold element \f$m\f$.
 */
template<Manifold M>
class ManifoldVector : public std::vector<M>
{
private:
  using Base = std::vector<M>;

public:
  //! Default constructor of empty ManifoldVector
  ManifoldVector() = default;
  //! Copy constructor
  ManifoldVector(const ManifoldVector & o) = default;
  //! Move constructor
  ManifoldVector(ManifoldVector && o) = default;
  //! Copy assignment operator
  ManifoldVector & operator=(const ManifoldVector & o) = default;
  //! Move assignment operator
  ManifoldVector & operator=(ManifoldVector && o) = default;
  ~ManifoldVector()                               = default;

  /**
   * Forwarding constructor to std::vector.
   */
  template<typename... Ts>
  ManifoldVector(Ts &&... ts) : Base(std::forward<Ts>(ts)...)
  {}

  /**
   * @brief Cast to different scalar type.
   */
  template<typename NewScalar>
  ManifoldVector<CastT<NewScalar, M>> cast() const
  {
    ManifoldVector<CastT<NewScalar, M>> ret;
    ret.reserve(size());
    std::transform(this->begin(), this->end(), std::back_insert_iterator(ret), [](const auto & x) {
      return ::smooth::cast<NewScalar>(x);
    });
    return ret;
  }

  /**
   * @brief Number of elements in ManifoldVector.
   */
  std::size_t size() const { return Base::size(); }

  /**
   * @brief Runtime degrees of freedom.
   *
   * Sum of the degrees of freedom of constituent elements.
   */
  Eigen::Index dof() const
  {
    if constexpr (Dof < M >> 0) {
      return size() * Dof<M>;
    } else {
      return std::accumulate(this->begin(), this->end(), 0u, [](auto s, const auto & item) {
        return s + ::smooth::dof<M>(item);
      });
    }
  }

  /**
   * @brief In-place addition.
   *
   * @note It must hold that dof() == a.dof()
   */
  template<typename Derived>
  ManifoldVector<M> & operator+=(const Eigen::MatrixBase<Derived> & a)
  {
    Eigen::Index dof_cntr = 0;
    for (auto i = 0u; i != this->size(); ++i) {
      const auto dof_i = ::smooth::dof<M>(this->operator[](i));
      this->operator[](i) =
        rplus<M>(this->operator[](i), a.template segment<Dof<M>>(dof_cntr, dof_i));
      dof_cntr += dof_i;
    }
    return *this;
  }

  /**
   * @brief Addition.
   *
   * @note It must hold that `dof() == a.dof()`
   */
  template<typename Derived>
  ManifoldVector<M> operator+(const Eigen::MatrixBase<Derived> & a) const
  {
    ManifoldVector<M> ret = *this;
    ret += a;
    return ret;
  }

  /**
   * @brief Subtraction.
   *
   * @note It must hold that `dof() == o.dof()`
   */
  Eigen::VectorX<Scalar<M>> operator-(const ManifoldVector<M> & o) const
  {
    std::size_t dof_cnts = 0;
    if (Dof < M >> 0) {
      dof_cnts = Dof<M> * size();
    } else {
      for (auto i = 0u; i != size(); ++i) { dof_cnts += ::smooth::dof<M>(this->operator[](i)); }
    }

    Eigen::VectorX<Scalar<M>> ret(dof_cnts);
    Eigen::Index idx = 0;
    for (auto i = 0u; i != size(); ++i) {
      const auto & size_i                       = ::smooth::dof<M>(this->operator[](i));
      ret.template segment<Dof<M>>(idx, size_i) = rminus<M>(this->operator[](i), o[i]);
      idx += size_i;
    }

    return ret;
  }
};

/**
 * @brief Manifold interface for ManifoldVector
 */
template<Manifold M>
struct traits::man<ManifoldVector<M>>
{
  // \cond
  using Scalar      = ::smooth::Scalar<M>;
  using PlainObject = ManifoldVector<M>;
  template<typename NewScalar>
  using CastT = ManifoldVector<typename man<M>::template CastT<NewScalar>>;

  static constexpr Eigen::Index Dof = -1;

  static inline Eigen::Index dof(const ManifoldVector<M> & m) { return m.dof(); }

  static inline PlainObject Default(Eigen::Index dof)
  {
    /// @note If underlying M has dynamic size, mdof not uniquely defined
    const Eigen::Index mdof = ::smooth::Dof<M> != -1 ? ::smooth::Dof<M> : 1;
    const Eigen::Index size = dof / mdof;

    return PlainObject(size, Default<M>(mdof));
  }

  template<typename NewScalar>
  static inline auto cast(const ManifoldVector<M> & m)
  {
    return m.template cast<NewScalar>();
  }

  template<typename Derived>
  static inline ManifoldVector<M>
  rplus(const ManifoldVector<M> & m, const Eigen::MatrixBase<Derived> & a)
  {
    return m + a;
  }

  static inline Eigen::Matrix<Scalar, Dof, 1>
  rminus(const ManifoldVector<M> & m1, const ManifoldVector<M> & m2)
  {
    return m1 - m2;
  }
  // \endcond
};

}  // namespace smooth

template<typename Stream, smooth::Manifold M>
Stream & operator<<(Stream & s, const smooth::ManifoldVector<M> & g)
{
  s << "ManifoldVector with " << g.size() << " elements:\n";
  for (auto i = 0u; i != g.size(); ++i) {
    s << i << ": " << g[i];
    if (i != g.size() - 1) { s << '\n'; }
  }
  return s;
}

#endif  // SMOOTH__MANIFOLD_VECTOR_HPP_


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/pow.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/exp.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( nthroot_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::integer_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
      using bA0 = bs::as_logical_t<A0>;
      A0 x =  bs::abs(a0);
      A0 aa1 = bs::tofloat(a1);
      A0 y = bs::fast_(bs::pow_abs)(x,rec(aa1));
      bA0 nul_a1 =  bs::is_eqz(bitwise_cast<A0>(a1));
      bA0 is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      A0 p = fast_(bs::pow_abs)(y, aa1);
      y = bs::if_plus( bs::logical_or(bs::is_nez(y), nul_a1)
                     , y
                     , -(p - x)/(aa1*p/y)
                     );
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      bA0 test =  logical_andnot(is_ltza0, is_odda1);
      bA0 done =  test;
      y = if_nan_else(test, y);  // a0 < O and a1 is not odd
      bA0 newtest =  is_equal(x, One<A0>());
      test  = logical_andnot(newtest, done);
      done  = logical_or(done, newtest);
      y = if_else(test, a0, y); // 1^a1 or (-1)^a1
      newtest =  nul_a1;
      test  = logical_andnot(newtest, done);
      done  = logical_or(done, newtest);
      y =  if_else(test,
                   if_zero_else(is_less(x, One<A0>()),
                                sign(a0)*Inf<A0>()
                               ),
                   y);
      newtest =  is_eqz(a0);
      test  = logical_andnot(newtest, done);
      done  = logical_or(done, newtest);
      y =  if_zero_else(test, y);
      #ifndef BOOST_SIMD_NO_INFINITIES
      newtest =  is_inf(a0);
      test  = logical_andnot(newtest, done);
      done  = logical_or(done, newtest);
      y =  if_else(test, if_else(is_nez(a1), a0, One<A0>()), y);
      #endif
      return bs::bitwise_or(y, bs::bitofsign(a0));
      }
   };

   BOOST_DISPATCH_OVERLOAD( nthroot_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::integer_<A1>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const fast_tag &, const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
     {
       auto aa1 =  abs(a1);
       A0 aa0 = abs(a0);
       A0 y = sign(a0)*bs::exp(bs::log(aa0)/tofloat(aa1));
       auto l =  is_ltz(aa1);
       y =  if_nan_else(logical_and(l, is_even(a1)), y);
       return if_else(is_ltz(a1), rec(y), y);
     }
   };

} } }

#endif


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

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/.

*/

#ifndef PIC_COMPUTER_VISION_CAMERA_MARTIX_HPP
#define PIC_COMPUTER_VISION_CAMERA_MARTIX_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../util/math.hpp"
#include "../util/eigen_util.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
    #include "../externals/Eigen/Geometry"
    #include "../externals/Eigen/QR"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
    #include <Eigen/QR>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief computeEpipole computes the epipole of a fundamental matrix F.
 * @param F is a fundamental matrix.
 * @return It returns the epipole of F.
 */
PIC_INLINE Eigen::Vector3d computeEpipole(Eigen::Matrix3d &F)
{
    Eigen::JacobiSVD< Eigen::Matrix3d > svdF(F, Eigen::ComputeFullV);
    Eigen::Matrix3d V = svdF.matrixV();

    Eigen::Vector3d e;

    e[0] = V(0, 2);
    e[1] = V(1, 2);
    e[2] = V(2, 2);

    return e;
}

/**
 * @brief getCameraMatrixFromHomography
 * @param H is 3x3 homography matrix.
 * @param K
 * @return
 */
PIC_INLINE Eigen::Matrix34d getCameraMatrixFromHomography(Eigen::Matrix3d &H, Eigen::Matrix3d &K)
{
    Eigen::Matrix34d m;
    m.setZero();

    Eigen::Matrix3d K_inv = K.inverse();

    Eigen::Matrix3d H_p = K_inv * H;

    Eigen::Vector3d r_0(H_p(0, 0), H_p(1, 0), H_p(2, 0));
    Eigen::Vector3d r_1(H_p(0, 1), H_p(1, 1), H_p(2, 1));

    r_0.normalize();
    r_1.normalize();
    Eigen::Vector3d r_2 = r_0.cross(r_1);

    Eigen::Vector3d t(H_p(0, 2), H_p(1, 2), H_p(2, 2));

    m(0, 0) = r_0[0];
    m(1, 0) = r_0[1];
    m(2, 0) = r_0[2];

    m(0, 1) = r_1[0];
    m(1, 1) = r_1[1];
    m(2, 1) = r_1[2];

    m(0, 2) = r_2[0];
    m(1, 2) = r_2[1];
    m(2, 2) = r_2[2];

    m(0 , 3) = t[0];
    m(1 , 3) = t[1];
    m(2 , 3) = t[2];

    return K * m;
}

/**
 * @brief getCameraMatrixIdentity
 * @param K
 * @return
 */
PIC_INLINE Eigen::Matrix34d getCameraMatrixIdentity(Eigen::Matrix3d &K)
{
    Eigen::Matrix34d m;
    m.setIdentity();
    return K * m;
}

/**
 * @brief getCameraMatrix
 * @param K
 * @param R
 * @param t
 * @return
 */
PIC_INLINE Eigen::Matrix34d getCameraMatrix(Eigen::Matrix3d &K, Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    Eigen::Matrix34d m;

    m(0, 0) = R(0, 0);
    m(1, 0) = R(1, 0);
    m(2, 0) = R(2, 0);

    m(0, 1) = R(0, 1);
    m(1, 1) = R(1, 1);
    m(2, 1) = R(2, 1);

    m(0, 2) = R(0, 2);
    m(1, 2) = R(1, 2);
    m(2, 2) = R(2, 2);

    m(0, 3) = t[0];
    m(1, 3) = t[1];
    m(2, 3) = t[2];

    return K * m;
}

/**
 * @brief decomposeCameraMatrix
 * @param P
 * @param K
 * @param R
 * @param t
 */
PIC_INLINE void decomposeCameraMatrix(Eigen::Matrix34d &P,
                                      Eigen::Matrix3d  &K,
                                      Eigen::Matrix3d  &R,
                                      Eigen::Vector3d  &t)
{
    Eigen::Matrix3d matrix = P.block<3, 3>(0, 0).inverse();

    //QR decomposition
    Eigen::HouseholderQR<Eigen::Matrix3d> qr(matrix.rows(), matrix.cols());
    qr.compute(matrix);

    Eigen::Matrix3d Q = qr.householderQ();
    Eigen::Matrix3d U = qr.matrixQR().triangularView<Eigen::Upper>();

    auto U_d = getDiagonalFromMatrix(U);
    Eigen::Vector3d d = U_d;
    for(int i = 0; i < 3; i++) {
        if(d[i] != 0.0) {
            d[i] = U_d[i] > 0.0 ? 1.0 : -1.0;
        }
    }
    auto D = DiagonalMatrix(d);

    Q = Q * D;
    U = D * U;

    //compute K, R, and t
    auto Q_t = Eigen::Transpose< Eigen::Matrix3d >(Q);
    auto s = Q.determinant();

    R = s * Q_t;
    t = s * U * P.col(3);

    if(U(2, 2) > 0.0) {
        U /= U(2, 2);
    }

    K = U.inverse();
}

/**
 * @brief cameraMatrixProject projects a point, p, using the camera
 * matrix, M.
 * @param M
 * @param p is a 3D point encoded in homogenous coordinate (4D vector)
 * @return
 */
PIC_INLINE Eigen::Vector2i cameraMatrixProject(Eigen::Matrix34d &M, Eigen::Vector4d &p)
{
    Eigen::Vector3d proj = M * p;
    proj[0] /= proj[2];
    proj[1] /= proj[2];

    return Eigen::Vector2i(int(proj[0]), int(proj[1]));
}

/**
 * @brief cameraMatrixProject projects a point, p, using the camera
 * matrix, M.
 * @param M
 * @param p is a 3D point (3D vector)
 * @return
 */
PIC_INLINE Eigen::Vector2i cameraMatrixProject(Eigen::Matrix34d &M, Eigen::Vector3d &p)
{
    Eigen::Vector4d p4d(p[0], p[1], p[2], 1.0);
    return cameraMatrixProject(M, p4d);
}

/**
 * @brief cameraMatrixProjection
 * @param M
 * @param p
 * @param cx
 * @param cy
 * @param fx
 * @param fy
 * @param lambda
 * @return
 */
PIC_INLINE Eigen::Vector2i cameraMatrixProjection(Eigen::Matrix34d &M, Eigen::Vector3d &p, double cx, double cy, double fx, double fy, double lambda)
{
    Eigen::Vector4d p_t = Eigen::Vector4d(p[0], p[1], p[2], 1.0);
    Eigen::Vector2i out;
    Eigen::Vector3d proj = M * p_t;
    proj[0] /= proj[2];
    proj[1] /= proj[2];

    double x_cx =  (proj[0] - cx);
    double y_cy =  (proj[1] - cy);

    double dx = x_cx / fx;
    double dy = y_cy / fy;
    double rho_sq = dx * dx + dy * dy;

    double factor = 1.0 / (1.0 + rho_sq * lambda);

    proj[0] = x_cx * factor + cx;
    proj[1] = y_cy * factor + cy;

    out[0] = int(proj[0]);
    out[1] = int(proj[1]);

    return out;
}

/**
 * @brief getOpticalCenter
 * @param P the camera matrix of a view
 * @return it returns the camera center of P
 */
PIC_INLINE Eigen::Vector3d getOpticalCenter(Eigen::Matrix34d &P)
{
    Eigen::Matrix3d Q = P.block<3, 3>(0, 0);
    auto Q_inv = Q.inverse();
    return - Q_inv * P.col(3);
}

/**
 * @brief cameraRectify
 * @param K0 intrisic matrix of view0
 * @param R0 rotation matrix of view0
 * @param t0 translation vector of view0
 * @param K1 intrisic matrix of view1
 * @param R1 rotation matrix of view1
 * @param t1 translation vector of view1
 * @param P0 new camera matrix of view0
 * @param P1 new camera matrix of view1
 * @param T0 transformation matrix for view0
 * @param T1 transformation matrix for view1
 */
PIC_INLINE void cameraRectify(Eigen::Matrix3d &K0, Eigen::Matrix3d &R0, Eigen::Vector3d &t0,
                              Eigen::Matrix3d &K1, Eigen::Matrix3d &R1, Eigen::Vector3d &t1,
                              Eigen::Matrix34d &P0_out, Eigen::Matrix34d &P1_out,
                              Eigen::Matrix3d &T0, Eigen::Matrix3d &T1)
{
    auto P0_in = getCameraMatrix(K0, R0, t0);
    auto P1_in = getCameraMatrix(K1, R1, t1);

    /*
    auto K0_i = K0.inverse();
    auto K1_i = K1.inverse();

    auto R0_t = Eigen::Transpose< Eigen::Matrix3d >(R0);
    auto R1_t = Eigen::Transpose< Eigen::Matrix3d >(R1);
    auto c0 = -R0_t * K0_i * P0t.col(3);
    auto c1 = -R1_t * K1_i * P1t.col(3);
    */

    //compute optical centers

    auto c0 = getOpticalCenter(P0_in);
    auto c1 = getOpticalCenter(P1_in);

    //compute new rotation matrix
    Eigen::Vector3d x_axis = c1 - c0;
    Eigen::Vector3d tmp = R1.row(2);
    Eigen::Vector3d y_axis = tmp.cross(x_axis);
    Eigen::Vector3d z_axis = x_axis.cross(y_axis);

    x_axis.normalize();
    y_axis.normalize();
    z_axis.normalize();

    Eigen::Matrix3d R;

    R(0, 0) = x_axis[0];
    R(0, 1) = x_axis[1];
    R(0, 2) = x_axis[2];

    R(1, 0) = y_axis[0];
    R(1, 1) = y_axis[1];
    R(1, 2) = y_axis[2];

    R(2, 0) = z_axis[0];
    R(2, 1) = z_axis[1];
    R(2, 2) = z_axis[2];

    //new camera matrices
    Eigen::Matrix3d K;
    K.setZero();
    K(0, 0) = K0(0, 0);
    K(1, 1) = K0(1, 1);
    K(0, 2) = (K0(0, 2) + K1(0, 2)) * 0.5;
    K(1, 2) = (K0(1, 2) + K1(1, 2)) * 0.5;
    K(2, 2) = 1.0;

    Eigen::Vector3d t0n = -R * c0;
    P0_out = getCameraMatrix(K, R, t0n);

    Eigen::Vector3d t1n = -R * c1;
    P1_out = getCameraMatrix(K, R, t1n);

    //transformations
    auto Q0o = P0_in.block<3, 3>(0, 0);
    auto Q0n = P0_out.block<3, 3>(0, 0);
    T0 = Q0n * Q0o.inverse();

    auto Q1o = P1_in.block<3, 3>(0, 0);
    auto Q1n = P1_out.block<3, 3>(0, 0);
    T1 = Q1n * Q1o.inverse();
}

/**
 * @brief cameraRectify
 * @param P0_in
 * @param P1_in
 * @param P0_out
 * @param P1_out
 * @param T0
 * @param T1
 */
PIC_INLINE void cameraRectify(Eigen::Matrix34d &P0_in, Eigen::Matrix34d &P1_in,
                              Eigen::Matrix34d &P0_out, Eigen::Matrix34d &P1_out,
                              Eigen::Matrix3d &T0, Eigen::Matrix3d &T1)
{
    Eigen::Matrix3d K0, K1, R0, R1;
    Eigen::Vector3d t0, t1;

    decomposeCameraMatrix(P0_in, K0, R0, t0);

    decomposeCameraMatrix(P1_in, K1, R1, t1);

    cameraRectify(K0, R0, t0,
                  K1, R1, t1,
                  P0_out, P1_out,
                  T0, T1);
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_CAMERA_MARTIX_HPP


#include <toynet/w2v.h>
#include <toynet/math.h>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace toynet {

std::vector<int> get_context(const std::vector<int>& words, int index, int historyN, int futureN)
{
    std::vector<int> ret;
    for (int j = std::max(0, index - historyN);  j < index;  ++j)
        ret.push_back(words[j]);
    for (int j = 1;  j <= futureN && index+j < words.size();  ++j)
        ret.push_back(words[index+j]);
    return ret;
}

void gradient_descent(ublas::matrix<double>& out, const ublas::matrix<double>& gradients, double lr)
{
    out -= gradients * lr;
}

CBOWModelGradients::CBOWModelGradients(int W, int D)
    : P(ublas::zero_matrix<double>(W, D))
    , O(ublas::zero_matrix<double>(W, D))
{
}

CBOWModel::CBOWModel(int W, int D, int historyN, int futureN)
    : W(W)
    , D(D)
    , historyN(historyN)
    , futureN(futureN)
    , P(W, D)
    , O(W, D)
{
}

void CBOWModel::save(std::ostream& os) const
{
    boost::archive::text_oarchive oa(os);
    oa << *this;
}

void CBOWModel::load(std::istream& is)
{
    boost::archive::text_iarchive ia(is);
    ia >> *this;
}

std::vector<std::pair<double, int>> CBOWModel::predict(const std::vector<int>& context) const
{
    ublas::vector<double> smax = predict_helper(context);
    std::vector<std::pair<double, int>> ret(W);
    for (int i = 0;  i < W;  ++i)
        ret[i] = std::make_pair(smax[i], i);
    std::sort(ret.begin(), ret.end(), std::greater<>());
    return ret;
}

double CBOWModel::predict(const std::vector<int>& context, int word) const
{
    ublas::vector<double> smax = predict_helper(context);
    return smax[word];
}

ublas::vector<double> CBOWModel::predict_helper(const std::vector<int>& context) const
{
    // average embedding of all context words
    ublas::vector<double> avg(D, 0.0);
    for (int wordidx : context)
        avg += ublas::row(P, wordidx);
    avg /= context.size();
    // output layer (before softmax)
    ublas::vector<double> out(W, 0.0);
    for (int i = 0;  i < W;  ++i)
        out[i] = dot_product(avg, row(O, i));
    // softmax
    ublas::vector<double> smax = softmax(out);
    return smax;
}

double CBOWModel::avg_log_prob(const std::vector<int>& words) const
{
    double sum = 0.0;
    for (int i = 0;  i < words.size();  ++i) {
        std::vector<int> context = get_context(words, i, historyN, futureN);
        double p = predict(context, words[i]);
        sum += std::log(p);
    }
    return sum / words.size();
}

CBOWModelGradients CBOWModel::gradients() const
{
    CBOWModelGradients ret(W, D);
    // TODO!
    return ret;
}

void CBOWModel::update(const CBOWModelGradients& gradients, double lr)
{
    gradient_descent(P, gradients.P, lr);
    gradient_descent(O, gradients.O, lr);
}

SimpleReporter::SimpleReporter(std::ostream& os)
    : os(os)
{
}

SimpleLearningRate::SimpleLearningRate(double lr, int cutoff, double base)
    : lr(lr)
    , cutoff(cutoff)
    , base(base)
{
}

void SimpleReporter::operator()(const ReportData& data) const
{
    os << "epoch " << data.epoch
       << " prob " << data.avg_log_prob
       << " lr " << data.lr
       << "\n";
}

double SimpleLearningRate::operator()(int epoch) const
{
    return (epoch <= cutoff)
         ? lr
         : lr * std::pow(base, epoch - cutoff);
}

Trainer::Trainer()
    : epochs(1)
    , D(50)
    , historyN(4)
    , futureN(4)
    , initReporter(nullptr)
    , epochReporter(nullptr)
    , exitReporter(nullptr)
    , learningRate(nullptr)
{
}

Trainer& Trainer::setEpochs(int epochs)
{
    this->epochs = epochs;
    return *this;
}

Trainer& Trainer::setEmbeddingSize(int D)
{
    this->D = D;
    return *this;
}

Trainer& Trainer::setHistoryN(int historyN)
{
    this->historyN = historyN;
    return *this;
}

Trainer& Trainer::setFutureN(int futureN)
{
    this->futureN = futureN;
    return *this;
}

Trainer& Trainer::setInitReporter(const Reporter *initReporter)
{
    this->initReporter = initReporter;
    return *this;
}

Trainer& Trainer::setEpochReporter(const Reporter *epochReporter)
{
    this->epochReporter = epochReporter;
    return *this;
}

Trainer& Trainer::setExitReporter(const Reporter *exitReporter)
{
    this->exitReporter = exitReporter;
    return *this;
}

Trainer& Trainer::setLearningRate(const LearningRate *learningRate)
{
    this->learningRate = learningRate;
    return *this;
}

CBOWModel Trainer::train(const std::vector<int>& corpus) const
{
    // Find W, the maximum number of words
    int W = *std::max_element(corpus.begin(), corpus.end()) + 1;
    CBOWModel model(W, D, historyN, futureN);
    int e = 0;
    double avg_log_prob = model.avg_log_prob(corpus);
    double lr = (*learningRate)(e);
    if (initReporter)
        (*initReporter)({e, avg_log_prob, lr});
    while (e <= epochs) {
        ++e;
        CBOWModelGradients gradients = model.gradients();
        lr = learningRate ? (*learningRate)(e) : 1.0;
        model.update(gradients, lr);
        avg_log_prob = model.avg_log_prob(corpus);
        if (epochReporter)
            (*epochReporter)({e, avg_log_prob, lr});
        // TODO: compute loss on validation data
    }
    if (exitReporter)
        (*exitReporter)({-1, avg_log_prob, lr});
    return model;
}

} // namespace toynet


#include <tuple>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>

#include "vice/functors.h"
#include "vice/integrate.h"
#include "vice/benchmark.h"

namespace py = pybind11;

//
// Trapezoid functions
//
template <typename Scalar, typename ParamType, typename Integrator>
std::tuple<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>, Eigen::Matrix<long int, Eigen::Dynamic, 1>, double>
benchmark_trapezoid (Scalar depth, Scalar duration, Scalar ingress,
                     Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, const Scalar texp,
                     ParamType param)
{
  vice::functors::TrapFunctor<Scalar> func(depth, duration, ingress);
  auto integrator = Integrator();
  return vice::benchmark::integrate(func, integrator, time, texp, param);
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
trapezoid_exact (Scalar depth, Scalar duration, Scalar ingress,
                 Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, Scalar texp)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fluence(time.rows());
  vice::functors::TrapFunctor<Scalar> func(depth, duration, ingress);
  for (int i = 0; i < time.rows(); ++i) {
    Scalar lower = time(i) - 0.5*texp;
    Scalar upper = time(i) + 0.5*texp;
    fluence(i) = func.integrate(lower, upper) / texp;
  }
  return fluence;
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
trapezoid_flux (Scalar depth, Scalar duration, Scalar ingress,
                Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> flux(time.rows());
  vice::functors::TrapFunctor<Scalar> func(depth, duration, ingress);
  for (int i = 0; i < time.rows(); ++i) {
    flux(i) = func(time(i));
  }
  return flux;
}

//
// Transit functions
//
template <typename Scalar, typename ParamType, typename Integrator>
std::tuple<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>, Eigen::Matrix<long int, Eigen::Dynamic, 1>, double>
benchmark_transit (Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> u, Scalar r, Scalar b, Scalar tau,
                   Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, const Scalar texp,
                   ParamType param)
{
  vice::functors::StarryFunctor<Scalar> func(u, r, b, tau);
  auto integrator = Integrator();
  return vice::benchmark::integrate(func, integrator, time, texp, param);
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
transit_exact (Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> u, Scalar r, Scalar b, Scalar tau,
               Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, Scalar texp,
               Scalar tol, unsigned max_depth)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fluence(time.rows());
  vice::functors::StarryFunctor<Scalar> func(u, r, b, tau);
  for (int i = 0; i < time.rows(); ++i) {
    Scalar lower = time(i) - 0.5*texp;
    Scalar upper = time(i) + 0.5*texp;
    fluence(i) = func.integrate(lower, upper, tol, max_depth) / texp;
  }
  return fluence;
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
transit_flux (Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> u, Scalar r, Scalar b, Scalar tau,
              Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> flux(time.rows());
  vice::functors::StarryFunctor<Scalar> func(u, r, b, tau);
  for (int i = 0; i < time.rows(); ++i) {
    flux(i) = func(time(i));
  }
  return flux;
}

//
// Python interface
//
PYBIND11_MODULE(benchmark, m) {

  m.def("trapezoid_flux",            &trapezoid_flux<double>);
  m.def("transit_flux",              &transit_flux<double>);

  m.def("trapezoid_exact",           &trapezoid_exact<double>);
  m.def("transit_exact",             &transit_exact<double>,
                                     py::arg("u"), py::arg("r"), py::arg("b"), py::arg("tau"), py::arg("t"), py::arg("texp"),
                                     py::arg("tol")=1e-12, py::arg("max_depth")=15);

  m.def("trapezoid_riemann",         &benchmark_trapezoid<double, unsigned, vice::integrate::riemann>);
  m.def("trapezoid_trapezoid_fixed", &benchmark_trapezoid<double, unsigned, vice::integrate::trapezoid_fixed>);
  m.def("trapezoid_simpson_fixed",   &benchmark_trapezoid<double, unsigned, vice::integrate::simpson_fixed>);
  m.def("trapezoid_trapezoid_adapt", &benchmark_trapezoid<double, double, vice::integrate::trapezoid_adapt>);
  m.def("trapezoid_simpson_adapt",   &benchmark_trapezoid<double, double, vice::integrate::simpson_adapt>);
  m.def("trapezoid_gauss",           &benchmark_trapezoid<double, double, vice::integrate::quadrature<15>>);

  m.def("transit_riemann",           &benchmark_transit<double, unsigned, vice::integrate::riemann>);
  m.def("transit_trapezoid_fixed",   &benchmark_transit<double, unsigned, vice::integrate::trapezoid_fixed>);
  m.def("transit_simpson_fixed",     &benchmark_transit<double, unsigned, vice::integrate::simpson_fixed>);
  m.def("transit_trapezoid_adapt",   &benchmark_transit<double, double, vice::integrate::trapezoid_adapt>);
  m.def("transit_simpson_adapt",     &benchmark_transit<double, double, vice::integrate::simpson_adapt>);
  m.def("transit_gauss",             &benchmark_transit<double, double, vice::integrate::quadrature<15>>);

}