/*
 * 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;
}


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

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

#define BOOST_TEST_MODULE equatorial_ha_coord

#include <iostream>
#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/astronomy/coordinate/coord_sys/equatorial_ha_coord.hpp>

#include <boost/test/unit_test.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::astronomy::coordinate;

namespace bud = boost::units::degree;

BOOST_AUTO_TEST_SUITE(equatorial_ha_coord_constructors)

BOOST_AUTO_TEST_CASE(equatorial_ha_coord_default_constructor) {
  equatorial_ha_coord<
      double,
      quantity<bud::plane_angle>,
      quantity<bud::plane_angle>>
      eha;

  //Check set_ha_dec
  eha.set_ha_dec(45.0 * bud::degrees, 18.0 * bud::degrees);

  //Check values
  BOOST_CHECK_CLOSE(eha.get_ha().value(), 45.0, 0.001);
  BOOST_CHECK_CLOSE(eha.get_dec().value(), 18.0, 0.001);

  //Quantities stored as expected?
  BOOST_TEST((std::is_same<decltype(eha.get_ha()), quantity<bud::plane_angle>>::value));
  BOOST_TEST((std::is_same<decltype(eha.get_dec()), quantity<bud::plane_angle>>::value));
}

BOOST_AUTO_TEST_CASE(equatorial_ha_coord_quantities_constructor) {
  //Make Equatorial Coordinate Check
  auto eha1 = make_equatorial_ha_coord
      (15.0 * bud::degrees, 39.0 * bud::degrees);
  BOOST_CHECK_CLOSE(eha1.get_ha().value(), 15.0, 0.001);
  BOOST_CHECK_CLOSE(eha1.get_dec().value(), 39.0, 0.001);

  //Quantities stored as expected?
  BOOST_TEST((std::is_same<decltype(eha1.get_ha()), quantity<bud::plane_angle>>::value));
  BOOST_TEST((std::is_same<decltype(eha1.get_dec()), quantity<bud::plane_angle>>::value));

  //Equatorial Coordinate constructor
  equatorial_ha_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      eha2(1.5 * bud::degrees, 9.0 * bud::degrees);
  BOOST_CHECK_CLOSE(eha2.get_ha().value(), 1.5, 0.001);
  BOOST_CHECK_CLOSE(eha2.get_dec().value(), 9.0, 0.001);

  //Quantities stored as expected?
  BOOST_TEST((std::is_same<decltype(eha2.get_ha()), quantity<bud::plane_angle>>::value));
  BOOST_TEST((std::is_same<decltype(eha2.get_dec()), quantity<bud::plane_angle>>::value));
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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_FUNCTION_ENUMERATE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ENUMERATE_HPP_INCLUDED

namespace boost { namespace simd
{
#if defined(DOXYGEN_ONLY)
  /*!
    @ingroup group-swar

    Linear enumeration of value

    Return a SIMD register containing a linear enumeration of values defined
    by a @c seed value and a @c step value.

    @par Semantic:

    For any given SIMD type @c T, the following code:

    @code
    T r = enumerate<T>();
    @endcode

    is equivalent to

    @code
    T r = make<T>(0, 1, ... , N-1);
    @endcode

    where @c N is the equal to <tt>cardinal_of<T>::value</tt>.

    For any given SIMD type @c T, and any value @c seed of a scalar type @c S, the following code:

    @code
    T r = enumerate<T>(seed);
    @endcode

    is equivalent to

    @code
    T r = make<T>(seed, seed + 1, ... ,  seed + (N-1));
    @endcode

    where @c N is the equal to <tt>cardinal_of<T>::value</tt>.

    For any given SIMD type @c T, and any value @c seed and @c step of a SIMD type @c S, the
    following code:

    @code
    T r = enumerate<T>(seed, step);
    @endcode

    is equivalent to

    @code
    T r = seed + step*enumerate<T>();
    @endcode

    @param seed Initial value of the enumeration. By default, equals to 0.
    @param step Initial value of the enumeration. By default, equals to 1.

    @return A pack containing the enumeration based on @c seed and @c step
  **/
  template<class T, class B, class S>
  T enumerate(const B& seed = 0, const S& step = 1) noexcept;

#endif
} }

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

#endif


// NonCentralChiSquaredDistribution.hpp
//
// (C) Datasim Education BV  2009

#pragma once

#include <boost/math/distributions.hpp>

using namespace System;

namespace Wrapper
{

	// Wrapper for the boost::math::non_central_chi_squared_ditribution class
	// We use the .NET naming conventions instead of the original C++ name
	public ref class NonCentralChiSquaredDistribution
	{
	private:
		// The wrapped native class (only pointers to native classes can be a C++/CLI class datamember)
		boost::math::non_central_chi_squared_distribution<>* m_distribution;

	public:
		// Default constructor
		NonCentralChiSquaredDistribution();

		// Constructor with lower and upper value
		NonCentralChiSquaredDistribution(double df, double lambda);

		// Finaliser (called by garbage collector or destructor)
		!NonCentralChiSquaredDistribution();

		// Destructor (Dispose)
		~NonCentralChiSquaredDistribution();

		// Get the native object
		boost::math::non_central_chi_squared_distribution<>* GetNative();
	};
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/Wrapper/NonCentralChiSquaredDistribution.hpp


/* Author: R. Falque
 *
 * main for transfering normals
 * by R. Falque
 * 12/11/2019
 */

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

#include "IO/readPLY.h"
#include "IO/writePLY.h"
#include "EigenTools/nanoflannWrapper.hpp"

int main(int argc, char* argv[])
{
    std::string source_path, target_path, output_path;

    // check input
    if (argc == 1) {
        std::cout << "Files defined as default setting and should be:\n";
        source_path = "../data/source.ply";
        target_path = "../data/target.ply";
        output_path = "../data/output.ply";
    } else if (argc == 4) {
        std::cout << "Files provided as command line argument and are defend as:\n";
        source_path = argv[1];
        target_path = argv[2];
        output_path = argv[3];
    } else {
        std::cout << "ERROR, wrong number of arguments: the program arguments should be source_path, target_path, output_path.\n";
        exit (EXIT_FAILURE);
    }

    std::cout << "Source file: " << source_path << "\n";
    std::cout << "Target file: " << target_path << "\n";
    std::cout << "Output file: " << output_path << "\n";

    Eigen::MatrixXd source_V, target_V;     // V: vertex of the surface
    Eigen::MatrixXi source_RGB, target_RGB; // not used but required for IO
    Eigen::MatrixXd source_N, target_N;     // N: normal of the surface
    Eigen::MatrixXi source_F, target_F;     // F: faces of the surface (used for plots)

    readPLY(source_path, source_V, source_F, source_N, source_RGB);
    readPLY(target_path, target_V, target_F, target_N, target_RGB);

    std::cout << "\nBefore normals transfer:\n";
    std::cout << "size of V: " << source_V.rows() << std::endl;
    std::cout << "size of F: " << source_F.rows() << std::endl;
    std::cout << "size of N: " << source_N.rows() << std::endl;
    std::cout << "size of RGB: " << source_RGB.rows() << std::endl;

    source_N = source_V; // lazy initialization
    nanoflann_wrapper knn_search(target_V);
    for (int i=0; i<source_V.rows(); i++) {
        std::vector < int > closest_vertex;
        closest_vertex = knn_search.return_k_closest_points(source_V.row(i), 1);
        source_N.row(i) = target_N.row(closest_vertex[0]);
        if (target_N.row(i)(0) == 0 && target_N.row(i)(2) == 0 && target_N.row(i)(2) == 0)
        {

            std::cout << "Warning, zero value normal found\n";
        }
        if (target_N.row(i).norm() == 0)
        {
            std::cout << "Warning, zero value normal found\n";
        }
        if (target_N.row(i).norm() != 1)
        {
            std::cout << "Warning, non unit normal found: normal length:" << target_N.row(i).norm() << "\n";
            source_N.row(i) = source_N.row(i) / source_N.row(i).norm();
        }

    }

    std::cout << "\nAfter normals transfer:\n";
    std::cout << "size of V: " << source_V.rows() << std::endl;
    std::cout << "size of F: " << source_F.rows() << std::endl;
    std::cout << "size of N: " << source_N.rows() << std::endl;
    std::cout << "size of RGB: " << source_RGB.rows() << std::endl;

    std::cout << "\nNormals transfered\n";
    writePLY(output_path, source_V, source_F, source_N, source_RGB, false);
    std::cout << "File succesfully saved\n";

    return 0;
}


/**
 * @file test_activation.cpp
 * @author Maximilien Naveau (maximilien.naveau@gmail.com)
 * @license License BSD-2-Clause
 * @copyright Copyright (c) 2019, New York University, Max Planck Gesellshaft and LAAS.
 * @date 2019-06-18
 */

#include <boost/test/unit_test.hpp>
// #include "crocoddyl/core/"

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_activation) {}

BOOST_AUTO_TEST_SUITE_END()

// import numpy as np
// from crocoddyl import ActivationModelQuad, ActivationModelSmoothAbs, ActivationModelWeightedQuad
// from crocoddyl.utils import EPS
// from testutils import assertNumDiff

// # Comment:
// '''
// c = sum( a(ri) )
// c' = sum( [a(ri)]' ) = sum( ri' a'(ri) ) = R' [ a'(ri) ]_i
// c'' = R' [a'(ri) ]_i' = R' [a''(ri) ] R

// ex
// a(x) =  x**2/x
// a'(x) = x
// a''(x) = 1

// sum(a(ri)) = sum(ri**2/2) = .5*r'r
// sum(ri' a'(ri)) = sum(ri' ri) = R' r
// sum(ri' a''(ri) ri') = R' r
// c'' = R'R
// '''

// # - ------------------------------
// # --- Dim 1 ----------------------
// h = np.sqrt(2 * EPS)

// def df(am, ad, x):
//     return (am.calc(ad, x + h) - am.calc(ad, x)) / h

// def ddf(am, ad, x):
//     return (am.calcDiff(ad, x + h)[0] - am.calcDiff(ad, x)[0]) / h

// am = ActivationModelQuad()
// ad = am.createData()
// x = np.random.rand(1)

// am.calc(ad, x)
// assertNumDiff(df(am, ad, x),
//               am.calcDiff(ad, x)[0], 1e-6)  # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
// assertNumDiff(ddf(am, ad, x),
//               am.calcDiff(ad, x)[1], 1e-6)  # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)

// am = ActivationModelWeightedQuad(np.random.rand(1))
// ad = am.createData()
// assertNumDiff(df(am, ad, x),
//               am.calcDiff(ad, x)[0], 1e-6)  # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
// assertNumDiff(ddf(am, ad, x),
//               am.calcDiff(ad, x)[1], 1e-6)  # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)

// am = ActivationModelSmoothAbs()
// ad = am.createData()
// assertNumDiff(df(am, ad, x),
//               am.calcDiff(ad, x)[0], 1e-6)  # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
// assertNumDiff(ddf(am, ad, x),
//               am.calcDiff(ad, x)[1], 1e-6)  # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)

// # - ------------------------------
// # --- Dim N ----------------------

// def df(am, ad, x):
//     dx = x * 0
//     J = np.zeros([len(x), len(x)])
//     for i, _ in enumerate(x):
//         dx[i] = h
//         J[:, i] = (am.calc(ad, x + dx) - am.calc(ad, x)) / h
//         dx[i] = 0
//     return J

// def ddf(am, ad, x):
//     dx = x * 0
//     J = np.zeros([len(x), len(x)])
//     for i, _ in enumerate(x):
//         dx[i] = h
//         J[:, i] = (am.calcDiff(ad, x + dx)[0] - am.calcDiff(ad, x)[0]) / h
//         dx[i] = 0
//     return J
//     return

// x = np.random.rand(3)

// am = ActivationModelQuad()
// ad = am.createData()
// J = df(am, ad, x)
// H = ddf(am, ad, x)
// assertNumDiff(np.diag(J.diagonal()), J, 5e-8)  # threshold was 1e-9, is now 5e-8 (see assertNumDiff.__doc__)
// assertNumDiff(np.diag(H.diagonal()), H, 5e-8)  # threshold was 1e-9, is now 5e-8 (see assertNumDiff.__doc__)
// assertNumDiff(df(am, ad, x).diagonal(),
//               am.calcDiff(ad, x)[0],
//               np.sqrt(2 * EPS))  # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
// assertNumDiff(ddf(am, ad, x).diagonal(),
//               am.calcDiff(ad, x)[1][:, 0],
//               np.sqrt(2 * EPS))  # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)

// am = ActivationModelWeightedQuad(np.random.rand(len(x)))
// ad = am.createData()
// assertNumDiff(df(am, ad, x).diagonal(),
//               am.calcDiff(ad, x)[0],
//               np.sqrt(2 * EPS))  # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
// assertNumDiff(ddf(am, ad, x).diagonal(),
//               am.calcDiff(ad, x)[1][:, 0],
//               np.sqrt(2 * EPS))  # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)

// am = ActivationModelSmoothAbs()
// ad = am.createData()
// assertNumDiff(df(am, ad, x).diagonal(),
//               am.calcDiff(ad, x)[0],
//               np.sqrt(2 * EPS))  # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
// assertNumDiff(ddf(am, ad, x).diagonal(),
//               am.calcDiff(ad, x)[1][:, 0],
//               np.sqrt(2 * EPS))  # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/QR>

template<typename MatrixType> void householder(const MatrixType& m)
{
  typedef typename MatrixType::Index Index;
  static bool even = true;
  even = !even;
  /* this test covers the following files:
     Householder.h
  */
  Index rows = m.rows();
  Index cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
  typedef Matrix<Scalar, internal::decrement_size<MatrixType::RowsAtCompileTime>::ret, 1> EssentialVectorType;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;
  typedef Matrix<Scalar, Dynamic, MatrixType::ColsAtCompileTime> HBlockMatrixType;
  typedef Matrix<Scalar, Dynamic, 1> HCoeffsVectorType;

  typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::RowsAtCompileTime> TMatrixType;

  Matrix<Scalar, EIGEN_SIZE_MAX(MatrixType::RowsAtCompileTime,MatrixType::ColsAtCompileTime), 1> _tmp((std::max)(rows,cols));
  Scalar* tmp = &_tmp.coeffRef(0,0);

  Scalar beta;
  RealScalar alpha;
  EssentialVectorType essential;

  VectorType v1 = VectorType::Random(rows), v2;
  v2 = v1;
  v1.makeHouseholder(essential, beta, alpha);
  v1.applyHouseholderOnTheLeft(essential,beta,tmp);
  VERIFY_IS_APPROX(v1.norm(), v2.norm());
  if(rows>=2) VERIFY_IS_MUCH_SMALLER_THAN(v1.tail(rows-1).norm(), v1.norm());
  v1 = VectorType::Random(rows);
  v2 = v1;
  v1.applyHouseholderOnTheLeft(essential,beta,tmp);
  VERIFY_IS_APPROX(v1.norm(), v2.norm());

  MatrixType m1(rows, cols),
             m2(rows, cols);

  v1 = VectorType::Random(rows);
  if(even) v1.tail(rows-1).setZero();
  m1.colwise() = v1;
  m2 = m1;
  m1.col(0).makeHouseholder(essential, beta, alpha);
  m1.applyHouseholderOnTheLeft(essential,beta,tmp);
  VERIFY_IS_APPROX(m1.norm(), m2.norm());
  if(rows>=2) VERIFY_IS_MUCH_SMALLER_THAN(m1.block(1,0,rows-1,cols).norm(), m1.norm());
  VERIFY_IS_MUCH_SMALLER_THAN(numext::imag(m1(0,0)), numext::real(m1(0,0)));
  VERIFY_IS_APPROX(numext::real(m1(0,0)), alpha);

  v1 = VectorType::Random(rows);
  if(even) v1.tail(rows-1).setZero();
  SquareMatrixType m3(rows,rows), m4(rows,rows);
  m3.rowwise() = v1.transpose();
  m4 = m3;
  m3.row(0).makeHouseholder(essential, beta, alpha);
  m3.applyHouseholderOnTheRight(essential,beta,tmp);
  VERIFY_IS_APPROX(m3.norm(), m4.norm());
  if(rows>=2) VERIFY_IS_MUCH_SMALLER_THAN(m3.block(0,1,rows,rows-1).norm(), m3.norm());
  VERIFY_IS_MUCH_SMALLER_THAN(numext::imag(m3(0,0)), numext::real(m3(0,0)));
  VERIFY_IS_APPROX(numext::real(m3(0,0)), alpha);

  // test householder sequence on the left with a shift

  Index shift = internal::random<Index>(0, std::max<Index>(rows-2,0));
  Index brows = rows - shift;
  m1.setRandom(rows, cols);
  HBlockMatrixType hbm = m1.block(shift,0,brows,cols);
  HouseholderQR<HBlockMatrixType> qr(hbm);
  m2 = m1;
  m2.block(shift,0,brows,cols) = qr.matrixQR();
  HCoeffsVectorType hc = qr.hCoeffs().conjugate();
  HouseholderSequence<MatrixType, HCoeffsVectorType> hseq(m2, hc);
  hseq.setLength(hc.size()).setShift(shift);
  VERIFY(hseq.length() == hc.size());
  VERIFY(hseq.shift() == shift);

  MatrixType m5 = m2;
  m5.block(shift,0,brows,cols).template triangularView<StrictlyLower>().setZero();
  VERIFY_IS_APPROX(hseq * m5, m1); // test applying hseq directly
  m3 = hseq;
  VERIFY_IS_APPROX(m3 * m5, m1); // test evaluating hseq to a dense matrix, then applying

  SquareMatrixType hseq_mat = hseq;
  SquareMatrixType hseq_mat_conj = hseq.conjugate();
  SquareMatrixType hseq_mat_adj = hseq.adjoint();
  SquareMatrixType hseq_mat_trans = hseq.transpose();
  SquareMatrixType m6 = SquareMatrixType::Random(rows, rows);
  VERIFY_IS_APPROX(hseq_mat.adjoint(),    hseq_mat_adj);
  VERIFY_IS_APPROX(hseq_mat.conjugate(),  hseq_mat_conj);
  VERIFY_IS_APPROX(hseq_mat.transpose(),  hseq_mat_trans);
  VERIFY_IS_APPROX(hseq_mat * m6,             hseq_mat * m6);
  VERIFY_IS_APPROX(hseq_mat.adjoint() * m6,   hseq_mat_adj * m6);
  VERIFY_IS_APPROX(hseq_mat.conjugate() * m6, hseq_mat_conj * m6);
  VERIFY_IS_APPROX(hseq_mat.transpose() * m6, hseq_mat_trans * m6);
  VERIFY_IS_APPROX(m6 * hseq_mat,             m6 * hseq_mat);
  VERIFY_IS_APPROX(m6 * hseq_mat.adjoint(),   m6 * hseq_mat_adj);
  VERIFY_IS_APPROX(m6 * hseq_mat.conjugate(), m6 * hseq_mat_conj);
  VERIFY_IS_APPROX(m6 * hseq_mat.transpose(), m6 * hseq_mat_trans);

  // test householder sequence on the right with a shift

  TMatrixType tm2 = m2.transpose();
  HouseholderSequence<TMatrixType, HCoeffsVectorType, OnTheRight> rhseq(tm2, hc);
  rhseq.setLength(hc.size()).setShift(shift);
  VERIFY_IS_APPROX(rhseq * m5, m1); // test applying rhseq directly
  m3 = rhseq;
  VERIFY_IS_APPROX(m3 * m5, m1); // test evaluating rhseq to a dense matrix, then applying
}

void test_householder()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( householder(Matrix<double,2,2>()) );
    CALL_SUBTEST_2( householder(Matrix<float,2,3>()) );
    CALL_SUBTEST_3( householder(Matrix<double,3,5>()) );
    CALL_SUBTEST_4( householder(Matrix<float,4,4>()) );
    CALL_SUBTEST_5( householder(MatrixXd(internal::random<int>(1,EIGEN_TEST_MAX_SIZE),internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
    CALL_SUBTEST_6( householder(MatrixXcf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE),internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
    CALL_SUBTEST_7( householder(MatrixXf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE),internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
    CALL_SUBTEST_8( householder(Matrix<double,1,1>()) );
  }
}


#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);
}


#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include "../field.h"
#include "utils.h"

#include <NTL/GF2EX.h>

TEST_CASE("Basic Arithmetic in all fields", "[field]") {
  banquet_params_t params[] = {Banquet_L1_Param1, Banquet_L1_Param3,
                               Banquet_L1_Param4};
  for (auto param : params) {
    field::GF2E::init_extension_field(banquet_instance_get(param));
    field::GF2E zero;
    field::GF2E one(1);
    field::GF2E x(2);
    field::GF2E x_2(4);

    REQUIRE(one + zero == one);
    REQUIRE(zero + one == one);
    REQUIRE(zero + zero == zero);
    REQUIRE(one + one == zero);

    REQUIRE(one * one == one);
    REQUIRE(zero * one == zero);
    REQUIRE(one * zero == zero);
    REQUIRE(zero * zero == zero);

    REQUIRE(x * one == x);
    REQUIRE(x * x == x_2);
  }
}
TEST_CASE("Modular Arithmetic GF(2^32)", "[field]") {
  field::GF2E::init_extension_field(banquet_instance_get(Banquet_L1_Param1));
  field::GF2E a, b;
  a.set_coeff(31);
  a.set_coeff(29);
  a.set_coeff(28);
  a.set_coeff(24);
  a.set_coeff(23);
  a.set_coeff(21);
  a.set_coeff(19);
  a.set_coeff(15);
  a.set_coeff(14);
  a.set_coeff(9);
  a.set_coeff(8);
  a.set_coeff(0);

  b.set_coeff(29);
  b.set_coeff(27);
  b.set_coeff(26);
  b.set_coeff(25);
  b.set_coeff(20);
  b.set_coeff(17);
  b.set_coeff(14);
  b.set_coeff(11);
  b.set_coeff(10);
  b.set_coeff(5);
  b.set_coeff(3);
  b.set_coeff(2);
  field::GF2E a_int(2980627201), b_int(772951084);
  REQUIRE(a == a_int);
  REQUIRE(b == b_int);

  field::GF2E ab(3895706975);
  field::GF2E a_2(2846443116);
  field::GF2E b_2(234130046);
  REQUIRE(a * b == ab);
  REQUIRE(a * a == a_2);
  REQUIRE(b * b == b_2);
}

TEST_CASE("Modular Arithmetic GF(2^40)", "[field]") {
  field::GF2E::init_extension_field(banquet_instance_get(Banquet_L1_Param3));
  field::GF2E a(683080732428ULL), b(437065243549ULL);

  field::GF2E ab(916833885315ULL);
  field::GF2E a_2(224904587486ULL);
  field::GF2E b_2(153370336291ULL);
  REQUIRE(a * b == ab);
  REQUIRE(a * a == a_2);
  REQUIRE(b * b == b_2);
}

TEST_CASE("Modular Arithmetic GF(2^48)", "[field]") {
  field::GF2E::init_extension_field(banquet_instance_get(Banquet_L1_Param4));
  field::GF2E a(94834555057164ULL), b(119504161027501ULL);

  field::GF2E ab(130190305526807ULL);
  field::GF2E a_2(187414204277907ULL);
  field::GF2E b_2(119412372920018ULL);
  REQUIRE(a * b == ab);
  REQUIRE(a * a == a_2);
  REQUIRE(b * b == b_2);
}

TEST_CASE("NTL to_bytes = custom to_bytes", "[field]") {
  field::GF2E::init_extension_field(banquet_instance_get(Banquet_L1_Param1));
  utils::init_extension_field(banquet_instance_get(Banquet_L1_Param1));
  field::GF2E a, b;
  a.set_coeff(31);
  a.set_coeff(29);
  a.set_coeff(28);
  a.set_coeff(24);
  a.set_coeff(23);
  a.set_coeff(21);
  a.set_coeff(19);
  a.set_coeff(15);
  a.set_coeff(14);
  a.set_coeff(9);
  a.set_coeff(8);
  a.set_coeff(0);

  b.set_coeff(29);
  b.set_coeff(27);
  b.set_coeff(26);
  b.set_coeff(25);
  b.set_coeff(20);
  b.set_coeff(17);
  b.set_coeff(14);
  b.set_coeff(11);
  b.set_coeff(10);
  b.set_coeff(5);
  b.set_coeff(3);
  b.set_coeff(2);
  GF2X c, d;
  SetCoeff(c, 31);
  SetCoeff(c, 29);
  SetCoeff(c, 28);
  SetCoeff(c, 24);
  SetCoeff(c, 23);
  SetCoeff(c, 21);
  SetCoeff(c, 19);
  SetCoeff(c, 15);
  SetCoeff(c, 14);
  SetCoeff(c, 9);
  SetCoeff(c, 8);
  SetCoeff(c, 0);
  GF2E c_e = conv<GF2E>(c);

  SetCoeff(d, 29);
  SetCoeff(d, 27);
  SetCoeff(d, 26);
  SetCoeff(d, 25);
  SetCoeff(d, 20);
  SetCoeff(d, 17);
  SetCoeff(d, 14);
  SetCoeff(d, 11);
  SetCoeff(d, 10);
  SetCoeff(d, 5);
  SetCoeff(d, 3);
  SetCoeff(d, 2);
  GF2E d_e = conv<GF2E>(d);

  const GF2X &poly_rep_c = rep(c_e);
  std::vector<uint8_t> buffer_c(4);
  BytesFromGF2X(buffer_c.data(), poly_rep_c, buffer_c.size());
  const GF2X &poly_rep_d = rep(d_e);
  std::vector<uint8_t> buffer_d(4);
  BytesFromGF2X(buffer_d.data(), poly_rep_d, buffer_d.size());

  std::vector<uint8_t> buffer_a(4);
  a.to_bytes(buffer_a.data());
  std::vector<uint8_t> buffer_b(4);
  b.to_bytes(buffer_b.data());
  REQUIRE(buffer_a == buffer_c);
  REQUIRE(buffer_b == buffer_d);
}
TEST_CASE("NTL to custom conversion", "[field]") {
  banquet_params_t params[] = {Banquet_L1_Param1, Banquet_L1_Param3,
                               Banquet_L1_Param4};
  for (auto param : params) {
    utils::init_extension_field(banquet_instance_get(param));
    field::GF2E::init_extension_field(banquet_instance_get(param));
    field::GF2E a, b;
    a.set_coeff(31);
    a.set_coeff(29);
    a.set_coeff(28);
    a.set_coeff(24);
    a.set_coeff(23);
    a.set_coeff(21);
    a.set_coeff(19);
    a.set_coeff(15);
    a.set_coeff(14);
    a.set_coeff(9);
    a.set_coeff(8);
    a.set_coeff(0);

    b.set_coeff(29);
    b.set_coeff(27);
    b.set_coeff(26);
    b.set_coeff(25);
    b.set_coeff(20);
    b.set_coeff(17);
    b.set_coeff(14);
    b.set_coeff(11);
    b.set_coeff(10);
    b.set_coeff(5);
    b.set_coeff(3);
    b.set_coeff(2);

    field::GF2E ab = a * b;
    GF2E a_ntl = utils::custom_to_ntl(a);
    GF2E ab_ntl = utils::custom_to_ntl(ab);
    GF2E b_ntl = ab_ntl / a_ntl;
    field::GF2E b2 = utils::ntl_to_custom(b_ntl);
    REQUIRE(b == b2);
  }
}
TEST_CASE("NTL inverse == custom", "[field]") {
  banquet_params_t params[] = {Banquet_L1_Param1, Banquet_L1_Param3,
                               Banquet_L1_Param4};
  for (auto param : params) {
    utils::init_extension_field(banquet_instance_get(param));
    field::GF2E::init_extension_field(banquet_instance_get(param));
    field::GF2E a;
    a.set_coeff(31);
    a.set_coeff(29);
    a.set_coeff(28);
    a.set_coeff(24);
    a.set_coeff(23);
    a.set_coeff(21);
    a.set_coeff(19);
    a.set_coeff(15);
    a.set_coeff(14);
    a.set_coeff(9);
    a.set_coeff(8);
    a.set_coeff(0);

    field::GF2E b = a.inverse();
    field::GF2E c = utils::ntl_to_custom(inv(utils::custom_to_ntl(a)));
    std::cout << utils::custom_to_ntl(a) << ", " << utils::custom_to_ntl(b)
              << ", " << utils::custom_to_ntl(c) << "\n";
    std::cout << utils::custom_to_ntl(a * b) << ", "
              << utils::custom_to_ntl(a * c) << ", "
              << utils::custom_to_ntl(a) * utils::custom_to_ntl(c) << "\n";
    REQUIRE(b == c);
    REQUIRE(a * b == field::GF2E(1));
  }
}
TEST_CASE("NTL interpolation == custom", "[field]") {
  field::GF2E::init_extension_field(banquet_instance_get(Banquet_L1_Param1));
  utils::init_extension_field(banquet_instance_get(Banquet_L1_Param1));

  std::vector<field::GF2E> a = field::get_first_n_field_elements(100);
  vec_GF2E b = utils::get_first_n_field_elements(100);
  for (size_t i = 0; i < 100; i++) {
    REQUIRE(a[i] == utils::ntl_to_custom(b[i]));
  }
  std::vector<field::GF2E> a_from_roots = field::build_from_roots(a);
  GF2EX b_from_roots = BuildFromRoots(b);
  REQUIRE(a_from_roots.size() == (size_t)b_from_roots.rep.length());
  for (size_t j = 0; j < a_from_roots.size(); j++) {
    REQUIRE(a_from_roots[j] == utils::ntl_to_custom(b_from_roots[j]));
  }

  std::vector<std::vector<field::GF2E>> a_lag =
      field::precompute_lagrange_polynomials(a);
  std::vector<GF2EX> b_lag = utils::precompute_lagrange_polynomials(b);

  REQUIRE(a_lag.size() == b_lag.size());
  for (size_t i = 0; i < a_lag.size(); i++) {
    REQUIRE(a_lag[i].size() == (size_t)b_lag[i].rep.length());
    for (size_t j = 0; j < a_lag[i].size(); j++) {
      REQUIRE(a_lag[i][j] == utils::ntl_to_custom(b_lag[i][j]));
    }
  }
}


#include <torch/Transform.h>
#include <Eigen/Eigen>
#include <torch/BoundingBox.h>
#include <torch/Normal.h>
#include <torch/Point.h>
#include <torch/Vector.h>

namespace torch
{

Transform::Transform() :
  m_matrix(optix::Matrix4x4::identity())
{
}

Transform::Transform(const optix::Matrix4x4& matrix) :
  m_matrix(matrix)
{
}

Transform::Transform(optix::Transform transform)
{
  transform->getMatrix(false, m_matrix.getData(), nullptr);
}

void torch::Transform::SetTransform(const Sophus::SE3f& transform)
{
  Eigen::Vector3f t = transform.translation();
  Eigen::Vector3f r = transform.so3().matrix().eulerAngles(0, 1, 2);
  SetTranslation(t[0], t[1], t[2]);
  SetRotation(r[0], r[1], r[2]);
}

optix::Matrix4x4 Transform::GetRotationMatrix() const
{
  unsigned int count = 0;
  optix::Matrix4x4 R = m_matrix;
  R.setCol(3, make_float4(0, 0, 0, 1));
  optix::Matrix4x4 Rnext;
  float norm = 0.0f;

  do
  {
    Rnext = 0.5f * (R + R.transpose().inverse());
    norm = GetNorm(R, Rnext);
    R = Rnext;
  }
  while (++count < 100 && norm > 1E-4);

  return R;
}

Vector Transform::GetEulerAngles() const
{
  Vector result;
  const optix::Matrix4x4 R = GetRotationMatrix();
  const float c2 = sqrt(R[0] * R[0] + R[4] * R[4]);
  result.x = atan2(R[9], R[10]);

  if (result.x > 0)
  {
    result.x = result.x - M_PIf;
    result.y = atan2(-R[8], -c2);
  }
  else
  {
    result.y = atan2(-R[8], c2);
  }

  const float s1 = sin(result.x);
  const float c1 = cos(result.x);
  result.z = atan2(s1 * R[2] - c1 * R[1], c1 * R[5] - s1 * R[6]);
  return -result;
}

void Transform::SetRotation(const optix::Matrix3x3& rotation)
{
  const optix::Matrix4x4 S = GetScaleMatrix();
  optix::Matrix4x4 M = GetTranslationMatrix();

  float* dst = M.getData();
  const float* src = rotation.getData();
  std::copy(&src[0], &src[3], &dst[0]);
  std::copy(&src[3], &src[6], &dst[4]);
  std::copy(&src[6], &src[9], &dst[8]);

  m_matrix = M * S;
}

void Transform::SetRotation(float x, float y, float z, float w)
{
  Eigen::Quaternionf quaternion(w, x, y, z);
  Eigen::Matrix3f matrix(quaternion);
  Eigen::Vector3f angles = matrix.eulerAngles(0, 1, 2);
  SetRotation(angles[0], angles[1], angles[2]);
}

void Transform::SetRotation(float x, float y, float z)
{
  const optix::Matrix4x4 R =
      optix::Matrix4x4::rotate(x, make_float3(1, 0, 0)) *
      optix::Matrix4x4::rotate(y, make_float3(0, 1, 0)) *
      optix::Matrix4x4::rotate(z, make_float3(0, 0, 1));

  const optix::Matrix4x4 T = GetTranslationMatrix();
  const optix::Matrix4x4 S = GetScaleMatrix();
  m_matrix = T * R * S;
}

void Transform::SetRotation(const Vector& rotation)
{
  SetRotation(rotation.x, rotation.y, rotation.z);
}

Vector Transform::GetTranslation() const
{
  const float4 col = m_matrix.getCol(3);
  return Vector(col.x, col.y, col.z);
}

optix::Matrix4x4 Transform::GetTranslationMatrix() const
{
  optix::Matrix4x4 T = optix::Matrix4x4::identity();
  T.setCol(3, m_matrix.getCol(3));
  return T;
}

void Transform::SetTranslation(float x, float y, float z)
{
  SetTranslation(Vector(x, y, z));
}

void Transform::SetTranslation(const Vector& translation)
{
  float4 col;
  col.x = translation.x;
  col.y = translation.y;
  col.z = translation.z;
  col.w = 1;

  m_matrix.setCol(3, col);
}

optix::Matrix4x4 Transform::GetScaleMatrix() const
{
  const optix::Matrix4x4 R = GetRotationMatrix();
  return R.transpose() * m_matrix;
}

Vector Transform::GetScale() const
{
  const optix::Matrix4x4 S = GetScaleMatrix();
  return Vector(S[0], S[5], S[10]);
}

void Transform::SetScale(float x, float y, float z)
{
  SetScale(Vector(x, y, z));
}

void Transform::SetScale(const Vector& scale)
{
  const float3 s = make_float3(scale.x, scale.y, scale.z);
  const optix::Matrix4x4 T = GetTranslationMatrix();
  const optix::Matrix4x4 R = GetRotationMatrix();
  const optix::Matrix4x4 S = optix::Matrix4x4::scale(s);
  m_matrix = T * R * S;
}

void Transform::SetScale(float scale)
{
  SetScale(scale, scale, scale);
}

optix::Matrix4x4 Transform::GetMatrix() const
{
  return m_matrix;
}

optix::Matrix3x4 Transform::GetMatrix3x4() const
{
  return optix::Matrix3x4(m_matrix.getData());
}

Transform Transform::Inverse() const
{
  Transform result;
  result.m_matrix = m_matrix.inverse();
  return result;
}

Transform Transform::operator*(const Transform& transform) const
{
  Transform result;
  result.m_matrix = m_matrix * transform.m_matrix;
  return result;
}

Normal Transform::operator*(const Normal& normal) const
{
  float4 n = make_float4(normal.x, normal.y, normal.z, 0);
  n = m_matrix.inverse() * n;
  return Normal(n.x, n.y, n.z);
}

Point Transform::operator*(const Point& point) const
{
  const float4 p = m_matrix * make_float4(point.x, point.y, point.z, 1);
  return Point(p.x, p.y, p.z);
}

Vector Transform::operator*(const Vector& vector) const
{
  const float4 v = m_matrix * make_float4(vector.x, vector.y, vector.z, 0);
  return Vector(v.x, v.y, v.z);
}

BoundingBox Transform::operator*(const BoundingBox& b) const
{
  BoundingBox result;
  result.Union((*this) * Point(b.bmin.x, b.bmin.y, b.bmin.z));
  result.Union((*this) * Point(b.bmin.x, b.bmin.y, b.bmax.z));
  result.Union((*this) * Point(b.bmin.x, b.bmax.y, b.bmin.z));
  result.Union((*this) * Point(b.bmin.x, b.bmax.y, b.bmax.z));
  result.Union((*this) * Point(b.bmax.x, b.bmin.y, b.bmin.z));
  result.Union((*this) * Point(b.bmax.x, b.bmin.y, b.bmax.z));
  result.Union((*this) * Point(b.bmax.x, b.bmax.y, b.bmin.z));
  result.Union((*this) * Point(b.bmax.x, b.bmax.y, b.bmax.z));
  return result;
}

void Transform::Write(optix::Transform transform) const
{
  transform->setMatrix(false, m_matrix.getData(), nullptr);
}

void Transform::Write(optix::Variable variable) const
{
  variable->setMatrix4x4fv(false, m_matrix.getData());
}

float Transform::GetNorm(const optix::Matrix4x4& a, const optix::Matrix4x4& b)
{
  float norm = 0;

  for (unsigned int i = 0; i < 3; ++i)
  {
    float n = fabsf(a[4 * 0 + i] - b[4 * 0 + i]) +
              fabsf(a[4 * 1 + i] - b[4 * 1 + i]) +
              fabsf(a[4 * 2 + i] - b[4 * 2 + i]);

    norm = fmaxf(norm, n);
  }

  return norm;
}

} // namespace torch


#include <SHARMLib.hpp>
#include <armadillo>

int main(){

	std::cout << "Found SHARMLib \n";

	arma::mat Cnm2f(1,1);
	arma::mat Snm2f(1,1);

	int n_degree =0;
	double ref_radius = 1;

	double r0[3] = {1,1,1};
	double r1[3] = {1,1,1};
	double r2[3] = {1,1,1};

	bool normalized = true;

	SHARMLib::ComputePolyhedralCS(Cnm2f,
		Snm2f,
		n_degree,
		ref_radius,
		r0,
		r1,
		r2,
		normalized);
	std::cout << "Ran ComputePolyhedralCS \n";

	return 0;
}


#pragma once

#include <pose_estimation/UnscentedKalmanFilter.hpp>
#include <pose_estimation/Measurement.hpp>

#include <uwv_dynamic_model/DataTypes.hpp>
#include <boost/shared_ptr.hpp>
#include <mtk/types/SOn.hpp>
#include <mtk/types/vect.hpp>
#include <mtk/startIdx.hpp>
#include <mtk/build_manifold.hpp>

namespace uwv_dynamic_model
{
    class ModelSimulation;
}

namespace uwv_kalman_filters
{

typedef ukfom::mtkwrap< MTK::vect<3, double> > VelocityType;
typedef ukfom::mtkwrap< MTK::vect<1, double> > ZPosType;

MTK_BUILD_MANIFOLD(VelocityState,
   ((VelocityType, velocity))
   ((ZPosType, z_position))
)

/**
 * The focus of this filter is to provide a model-aided linear and angular velocity estimate.
 * Due to the model-aiding the velocities can be provided in a high frequency.
 */
class VelocityUKF : public pose_estimation::UnscentedKalmanFilter<VelocityState>
{
public:
    MEASUREMENT(DVLMeasurement, 3)
    MEASUREMENT(GyroMeasurement, 3)
    MEASUREMENT(BodyEffortsMeasurement, 6)
    MEASUREMENT(PressureMeasurement, 1)

public:
    VelocityUKF(const State& initial_state, const Covariance& state_cov);
    virtual ~VelocityUKF() {}

    /** Set AUV motion model parameters */
    bool setupMotionModel(const uwv_dynamic_model::UWVParameters& parameters);

    /** Velocity of the AUV */
    void integrateMeasurement(const DVLMeasurement& measurement);

    /** Rotation rates of the AUV */
    void integrateMeasurement(const GyroMeasurement& measurement);

    /** Forces and torques in the body frame */
    void integrateMeasurement(const BodyEffortsMeasurement& measurement);

    /** Altitude to the AUV */
    void integrateMeasurement(const PressureMeasurement& measurement);

protected:
    void predictionStepImpl(double delta_t);

protected:
    boost::shared_ptr<uwv_dynamic_model::ModelSimulation> motion_model;
    boost::shared_ptr<uwv_dynamic_model::ModelSimulation> prediction_model;
    GyroMeasurement angular_velocity;
    BodyEffortsMeasurement body_efforts;
};

}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#include <armnn/TypesUtils.hpp>

#include <boost/assert.hpp>
#include <boost/numeric/conversion/cast.hpp>

template<typename QuantizedType>
QuantizedType armnn::Quantize(float value, float scale, int32_t offset)
{
    static_assert(IsQuantizedType<QuantizedType>(), "Not an integer type.");
    constexpr QuantizedType max = std::numeric_limits<QuantizedType>::max();
    constexpr QuantizedType min = std::numeric_limits<QuantizedType>::lowest();
    BOOST_ASSERT(scale != 0.f);
    BOOST_ASSERT(!std::isnan(value));

    float clampedValue = std::min(std::max(static_cast<float>(round(value/scale) + offset), static_cast<float>(min)),
                                  static_cast<float>(max));
    auto quantizedBits = static_cast<QuantizedType>(clampedValue);

    return quantizedBits;
}

template <typename QuantizedType>
float armnn::Dequantize(QuantizedType value, float scale, int32_t offset)
{
    static_assert(IsQuantizedType<QuantizedType>(), "Not an integer type.");
    BOOST_ASSERT(scale != 0.f);
    BOOST_ASSERT(!std::isnan(value));
    float dequantized = boost::numeric_cast<float>(value - offset) * scale;
    return dequantized;
}

/// Explicit specialization of Quantize for uint8_t
template
uint8_t armnn::Quantize<uint8_t>(float value, float scale, int32_t offset);

/// Explicit specialization of Quantize for int16_t
template
int16_t armnn::Quantize<int16_t>(float value, float scale, int32_t offset);

/// Explicit specialization of Quantize for int32_t
template
int32_t armnn::Quantize<int32_t>(float value, float scale, int32_t offset);

/// Explicit specialization of Dequantize for uint8_t
template
float armnn::Dequantize<uint8_t>(uint8_t value, float scale, int32_t offset);

/// Explicit specialization of Dequantize for int16_t
template
float armnn::Dequantize<int16_t>(int16_t value, float scale, int32_t offset);

/// Explicit specialization of Dequantize for int32_t
template
float armnn::Dequantize<int32_t>(int32_t value, float scale, int32_t offset);


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

#ifndef EKF_SLAM_CORE_HPP
#define EKF_SLAM_CORE_HPP

#include <ros/ros.h>

#include <eigen3/Eigen/Eigen>
#include <eigen3/Eigen/Dense>
#include <Eigen/SparseCore>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/tf.h>

#include "correspondence_class/correspondence_class.hpp"
#include "correspondence_class/correspondence_mbes.hpp"
#include "correspondence_class/correspondence_fls.hpp"

#include <queue>
#include <math.h>

#include <boost/thread/mutex.hpp>
#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>

#include "utils/ekf_utils.hpp"
#include "munkres/munkres.h"

class EKFCore{

public:

    EKFCore(Eigen::VectorXd& mu, Eigen::MatrixXd& Sigma, Eigen::MatrixXd& R, Eigen::MatrixXd& Q_fls, Eigen::MatrixXd &Q_mbes,
            double& lambda_fls, double& lambda_mbes, tf::StampedTransform& tf_base_sensor, const double mh_dist_fls, const double mh_dist_mbes);
    ~EKFCore();
    std::tuple<Eigen::VectorXd, Eigen::MatrixXd> ekfUpdate();
    void predictMotion(nav_msgs::Odometry odom_reading);
    void batchDataAssociation(std::vector<Eigen::Vector3d> z_t, const utils::MeasSensor& sens_type);

private:

    // System state variables
    Eigen::VectorXd mu_;
    Eigen::MatrixXd Sigma_;
    Eigen::VectorXd mu_hat_;
    Eigen::MatrixXd Sigma_hat_;
    Eigen::Vector3d mu_auv_odom_;

    // Noise models
    Eigen::MatrixXd R_;
    Eigen::MatrixXd Q_fls_;
    Eigen::MatrixXd Q_mbes_;

    // Mapping variables
    tf::StampedTransform tf_base_sensor_;
    tf::Transform tf_sensor_base_;
    double lambda_mbes_;
    double lambda_fls_;
    double mh_dist_fls_;
    double mh_dist_mbes_;
    int lm_num_;

    void predictBatchMeasurement(const Eigen::Vector3d &landmark_j,
                                const Eigen::Vector3d &z_i,
                                unsigned int i,
                                unsigned int j,
                                const tf::Transform &transf_base_odom,
                                const Eigen::MatrixXd &temp_sigma,
                                h_comp h_comps,
                                const utils::MeasSensor &sens_type,
                                std::vector<CorrespondenceClass> &corresp_i_list,
                                Eigen::MatrixXd &corresp_table);

    void sequentialUpdate(const CorrespondenceClass &c_i_j, Eigen::MatrixXd temp_sigma);

};

#endif // EKF_SLAM_CORE_HPP