#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>
#include <chrono>

using namespace std;
using namespace boost::filesystem;
using namespace cv;

typedef struct IrisCode
{
  string name;
  unsigned char **data;
  int stepsize;
} IrisCode;

static const int CODE_WIDTH = 512;
static const int CODE_HEIGHT = 20;

static const int MIN_SHIFT = -16;
static const int MAX_SHIFT = 16;

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

static const int ALG_MINHD = 0, ALG_3A = 1, ALG_3AL = 2, ALG_3AS = 3;
static const int MODE_MAIN = 1, MODE_HELP = 2;
enum MAIN_MODE { STATIC, DYNAMIC};

void printUsage() {
	printf("+-----------------------------------------------------------------------------+\n");
	printf("| TripleA - 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("| -f   | folder     | 1 | N | folder of iris-codes generated using LG or QSW  |\n");
	printf("| -o   | file       | 1 | N | output file for the results                     |\n");
	printf("| -s   | step-size  | 1 | Y | step-size used in static mode (5)               |\n");
	printf("| -c   | constant   | 1 | Y | constant used in dynamic mode (1)               |\n");
	printf("| -a   | algorithm  | 1 | Y | TripleA algorithm (3a) (default)                |\n");
	printf("|      |            |   |   | TripleA-limited algorithm (3al)                 |\n");
	printf("|      |            |   |   | TripleA-single-sided algorithm (3as)            |\n");
	printf("| -h   |            | 2 | N | prints usage                                    |\n");
	printf("+------+------------+---+---+-------------------------------------------------+\n");
	printf("|                                                                             |\n");
	printf("| The default is to run the program in dynamic mode. When a step size (-s) is |\n");
	printf("| set the program automatically switches to static-mode.                      |\n");
	printf("|                                                                             |\n");
	printf("| EXAMPLE USAGE                                                               |\n");
    printf("|                                                                             |\n");
    printf("| -f codes -s 4 -c 0.33 -a 3a -o compare.txt                                  |\n");
    printf("|                                                                             |\n");
	printf("| AUTHOR                                                                      |\n");
	printf("|                                                                             |\n");
	printf("| Christian Rathgeb                                                           |\n");
	printf("|                                                                             |\n");
	printf("| COPYRIGHT                                                                   |\n");
	printf("|                                                                             |\n");
	printf("| (C) 2016 All rights reserved. Do not distribute without written permission. |\n");
	printf("+-----------------------------------------------------------------------------+\n");
}

// estimate the Hamming distance between two given codes using the TripleA algorithm
void TripleA(IrisCode code1, IrisCode code2, int start, int stop, ofstream& log, int stepsize){

	float mindist = CODE_HEIGHT*CODE_WIDTH;
	unsigned int dist;
	int pos_min = 0;

	int w = CODE_WIDTH;
	int h;
	int border;

	if (CODE_HEIGHT%8 == 0) h = CODE_HEIGHT/8;
	else h = CODE_HEIGHT/8 + 1;

	auto start_time = chrono::high_resolution_clock::now();

	// alignmnet with step-size
	border = stop%stepsize;
	if (border != 0){
		int k=start;
		dist = 0;
		for (int i=0; i < h; i++){
			for (int j=0; j < w; j++){
				dist+= htlut[code1.data[j][i] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][i]];
			}
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
		start+=border;
	}

	for (int k = start; k <= stop; k+=stepsize){
		dist = 0;
		for (int i=0; i < h; i++){
			for (int j=0; j < w; j++){
				dist+= htlut[code1.data[j][i] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][i]];
			}
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}

	if (border != 0){
		int k=stop;
		dist = 0;
		for (int i=0; i < h; i++){
			for (int j=0; j < w; j++){
				dist+= htlut[code1.data[j][i] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][i]];
			}
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}

	start = max(pos_min - stepsize + 1, start);
	stop = min(pos_min + stepsize - 1, stop);

	for (int k = start; k <= stop; k++){
		dist = 0;
		for (int i=0; i < h; i++){
			for (int j=0; j < w; j++){
				dist+= htlut[code1.data[j][i] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][i]];
			}
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}

	auto end_time = chrono::high_resolution_clock::now();

	log << "hd(" << code1.name << "," << code2.name << "):" <<  mindist/(CODE_HEIGHT*CODE_WIDTH) << "\t time: " << chrono::duration_cast<chrono::microseconds>(end_time - start_time).count() << " micro seconds \t step-size: " << stepsize << endl;
	log.flush();
}

// estimate the Hamming distance between two given codes using the TripleA-limited algorithm
void TripleA_limited(IrisCode code1, IrisCode code2, int start, int stop, ofstream& log, int stepsize){

	float mindist = CODE_HEIGHT*CODE_WIDTH;
	unsigned int dist;
	int pos_min = 0;

	int w = CODE_WIDTH;
	int h;
	int border;

	if (CODE_HEIGHT%8 == 0) h = CODE_HEIGHT/8;
	else h = CODE_HEIGHT/8 + 1;

	auto start_time = chrono::high_resolution_clock::now();

	// limited alignment with step-size
	border = stop%stepsize;
	if (border != 0){
		int k=start;
		dist = 0;
		for (int j=0; j < w; j++){
			dist+= htlut[code1.data[j][0] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][0]];
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
		start+=border;
	}

	for (int k = start; k <= stop; k+=stepsize){
		dist = 0;
		for (int j=0; j < w; j++){
			dist+= htlut[code1.data[j][0] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][0]];
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}

	if (border != 0){
		int k=stop;
		dist = 0;
		for (int j=0; j < w; j++){
			dist+= htlut[code1.data[j][0] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][0]];
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}
	mindist = CODE_HEIGHT*CODE_WIDTH;

	start = max(pos_min - stepsize + 1, start); // note that we already tested the boundaries
	stop = min(pos_min + stepsize - 1, stop);

	for (int k = start; k <= stop; k++){
		dist = 0;
		for (int i=0; i < h; i++){
			for (int j=0; j < w; j++){
				dist+= htlut[code1.data[j][i] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][i]];
			}
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}

	auto end_time = chrono::high_resolution_clock::now();

	log << "hd(" << code1.name << "," << code2.name << "):" <<  mindist/(CODE_HEIGHT*CODE_WIDTH) << "\t time: " << chrono::duration_cast<chrono::microseconds>(end_time - start_time).count() << " micro seconds \t step-size: " << stepsize << endl;
	log.flush();
}

// estimate the Hamming distance between two given codes using the TripleA-single-sided algorithm
void TripleA_singlesided(IrisCode code1, IrisCode code2, int start, int stop, ofstream& log, int stepsize){

	float mindist = CODE_HEIGHT*CODE_WIDTH;
	unsigned int dist;
	int pos_min = 0;
	int idx_min = 0;

	int w = CODE_WIDTH;
	int h;
	int border;
	int off = 0;

	if (CODE_HEIGHT%8 == 0) h = CODE_HEIGHT/8;
	else h = CODE_HEIGHT/8 + 1;

	border = stop%stepsize;
	if (border > 0) off=2;

	int cnt=0;
	int scores[(abs(start)+stop)/stepsize + 1 + off];

	auto start_time = chrono::high_resolution_clock::now();

	if (border != 0){
		int k=start;
		dist = 0;
		for (int j=0; j < w; j++){
			dist+= htlut[code1.data[j][0] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][0]];
		}
		scores[cnt] = dist;
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
			idx_min = cnt;
		}
		cnt++;
		start+=border;
	}

	for (int k = start; k <= stop; k+=stepsize){
		dist = 0;

		for (int j=0; j < w; j++){
			dist+= htlut[code1.data[j][0] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][0]];
		}
		scores[cnt] = dist;
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
			idx_min = cnt;
		}
		cnt++;
	}

	if (border != 0){
		int k=stop;
		dist = 0;
		for (int j=0; j < w; j++){
			dist+= htlut[code1.data[j][0] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][0]];
		}
		scores[cnt] = dist;
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
			idx_min = cnt;
		}
	}

	mindist = CODE_HEIGHT*CODE_WIDTH;
	if (idx_min == 0){
		start = pos_min;
		stop = pos_min + stepsize - 1;
	}
	else if (idx_min == (abs(start)+stop)/stepsize + off){
		start = pos_min - stepsize + 1;
		stop = pos_min;
	}
	else if (scores[idx_min +1] > scores[idx_min -1]){
		start = pos_min - stepsize + 1;
		stop = pos_min;
	}
	else{
		start = pos_min;
		stop = pos_min + stepsize - 1;
	}

	for (int k = start; k <= stop; k++){
		dist = 0;
		for (int i=0; i < h; i++){
			for (int j=0; j < w; j++){
				dist+= htlut[code1.data[j][i] ^ code2.data[(j+k+CODE_WIDTH)%CODE_WIDTH][i]];
			}
		}
		if (dist < mindist){
			mindist = dist;
			pos_min = k;
		}
	}
	auto end_time = chrono::high_resolution_clock::now();
	log << "hd(" << code1.name << "," << code2.name << "):" <<  mindist/(CODE_HEIGHT*CODE_WIDTH) << "\t time: " << chrono::duration_cast<chrono::microseconds>(end_time - start_time).count() << " micro seconds \t step-size: " << stepsize << endl;
	log.flush();
}

// read an iris-code form a given file name, extract the (dynamic) step size
// and map it to a more efficient representation
IrisCode getCode(string filename, float k)
{
	IrisCode iriscode;
	Mat img = imread(filename, CV_LOAD_IMAGE_UNCHANGED);
	CV_Assert(img.data != 0);
	CV_Assert(img.type() == CV_8UC1);

	int w = CODE_WIDTH;
	int h;

	iriscode.name=filename;

	if (CODE_HEIGHT%8 == 0) h = CODE_HEIGHT/8;
	else h = CODE_HEIGHT/8 + 1;

	unsigned char c;
	unsigned char bincode[CODE_WIDTH][CODE_HEIGHT];

	// initialize with 0s
	for (int i=0; i < CODE_HEIGHT; i++){
		for (int j=0; j < CODE_WIDTH; j++){
			bincode[j][i] = 0;
		}
	}

	// now map the given codes to 0/1 representation
	for (int i=0; i < CODE_HEIGHT; i++){
		for (int j=0; j < CODE_WIDTH; j++){
			c=pow(2,7-(j%8));
			bincode[j][i]=htlut[img.data[j/8+i*CODE_WIDTH/8]&c];
		}
	}

	float mu = 0;
	int length;
	int count=0;
	for (int i=0; i < CODE_HEIGHT; i++){
		length = 1;
		for (int j=1; j < CODE_WIDTH; j++){
			if (bincode[j][i] == bincode[j-1][i]) length++;
			else {
				mu+=length;
				length=1;
				count++;
			}
		}
		mu+=length;
		length=1;
		count++;
	}
	mu/=count;

	// assign the average dynamic step-size according to the given constant k
	iriscode.stepsize = max((int)(mu*k),1);

	unsigned char **code;
	code = (unsigned char **)malloc(w * sizeof(unsigned char *));
	for (int i = 0; i < w; i++) code[i] = (unsigned char *)malloc(h * sizeof(unsigned char));

	// initialize with 0s
	for (int i=0; i < h; i++){
		for (int j=0; j < w; j++){
			code[j][i] = 0;
		}
	}

	// now map the given codes to a more efficient representation
	// create bytes out of 8 adjacent row-bits
	for (int i=0; i < h; i++){
		for (int j=0; j < w; j++){
			c=pow(2,7-(j%8));
			for (int k = 0; k < 8; k++){
				if (i*8+k >=CODE_HEIGHT) break;
				code[j][i]+=htlut[img.data[j/8+i*w+k*w/8]&c] * pow(2,7-k);
			}
		}
	}

	iriscode.data=code;
	return iriscode;
}

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

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

/*
 * Main program
 */
int main(int argc, char *argv[])
{
	std::vector<IrisCode> iriscodes;

	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,"-f|-s|-a|-c|-o|-h");
			string folder;
			if (cmdGetOpt(cmd,"-f") != 0){
				cmdCheckOptSize(cmd,"-f",1);
				folder = cmdGetPar(cmd,"-f");
			}
			int alg = ALG_3A;
			if (cmdGetOpt(cmd,"-a") != 0){
				cmdCheckOptSize(cmd,"-a",1);
				string algo = cmdGetPar(cmd,"-a");
				if (algo == "3al"){
					alg = ALG_3AL;
				}
				if (algo == "3as"){
					alg = ALG_3AS;
				}
			}
            MAIN_MODE main_mode = DYNAMIC;
            string outfilename = "dynamic.txt";
			// parameter for static mode
			int s = 5;
			if (cmdGetOpt(cmd,"-s") != 0){
				cmdCheckOptSize(cmd,"-s",1);
				s = cmdGetParInt(cmd,"-s");
                main_mode =  STATIC;
                outfilename="static.txt";
            }
			float c = 0.33;
			if (cmdGetOpt(cmd,"-c") != 0){
                cmdCheckOptSize(cmd,"-c",1);
                c = cmdGetParFloat(cmd,"-c");
                main_mode = DYNAMIC;
                outfilename="dynamic.txt";
            }

            if (cmdGetOpt(cmd,"-o") != 0){

                cmdCheckOptSize(cmd,"-o",1);
                outfilename = cmdGetPar(cmd,"-o");
            }

			// starting routine
			path p(folder);
			cout << "loading iriscodes..." << endl;
			for (auto i = directory_iterator(p); i != directory_iterator(); i++){
				if (!is_directory(i->path())){
					iriscodes.push_back(getCode(i->path().string(), c));
				}
				else continue;
			}
			cout << "loaded " << (int)iriscodes.size() << " iriscodes" << endl;

			ofstream log;
            log.open(outfilename, std::ios_base::app);
            cout << "output written to " << outfilename << endl;

            if (  main_mode == STATIC){
                // evaluation with static step size
                cout << "evaluation with static step size of " << s << " bit" << endl;
                for (int i=0; i<(int)iriscodes.size()-1; i++){
                    for (int j=i+1; j<(int)iriscodes.size(); j++){
                        if (i!=j) {
                            if (alg == ALG_3A){
                                TripleA(iriscodes.at(i), iriscodes.at(j), MIN_SHIFT, MAX_SHIFT, log, s);
                            }
                            else if (alg == ALG_3AL){
                                TripleA_limited(iriscodes.at(i), iriscodes.at(j), MIN_SHIFT, MAX_SHIFT, log, s);
                            }
                            else {
                                TripleA_singlesided(iriscodes.at(i), iriscodes.at(j), MIN_SHIFT, MAX_SHIFT, log, s);
                            }
                        }
                    }
                }
            } else if ( main_mode == DYNAMIC){
                // evaluation with dynamic step size
                cout << "evaluation with dynamic step size and constant of " << c << " bit" << endl;
                for (int i=0; i<(int)iriscodes.size()-1; i++){
                    for (int j=i+1; j<(int)iriscodes.size(); j++){
                        if (i!=j) {
                            if (alg == ALG_3A){
                                TripleA(iriscodes.at(i), iriscodes.at(j), MIN_SHIFT, MAX_SHIFT, log, iriscodes.at(i).stepsize);
                            }
                            else if (alg == ALG_3AL){
                                TripleA_limited(iriscodes.at(i), iriscodes.at(j), MIN_SHIFT, MAX_SHIFT, log, iriscodes.at(i).stepsize);
                            }
                            else {
                                TripleA_singlesided(iriscodes.at(i), iriscodes.at(j), MIN_SHIFT, MAX_SHIFT, log, iriscodes.at(i).stepsize);
                            }
                        }
                    }
                }
            }
			log.close();
    	}
    	else if (mode == MODE_HELP){
			// validate command line
			cmdCheckOpts(cmd,"-h");
			if (cmdGetOpt(cmd,"-h") != 0) cmdCheckOptSize(cmd,"-h",0);
			// starting routine
			printUsage();
    	}
    }
	catch (...){
	   	printf("Exit with errors.\n");
	   	exit(EXIT_FAILURE);
	}
    return EXIT_SUCCESS;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string>

#include "hh_post_det_model.hpp"
//#include "vgcc_det_model.hpp"
//#include "pmca_det_model.hpp"
#include "ca_sensor_gillespie_class.hpp"
#include "vgcc_gillespie_class.hpp"
#include "pmca_gillespie_class.hpp"

#include "gillespie_class.hpp"
#include "physical_constants.hpp"
#include "stl_vector_operation_functions.hpp"

#include <boost/numeric/odeint.hpp> /*  Specifying the file within < > lets c++ search for it at -I path  */
#include <boost/range/iterator_range.hpp>
#include <boost/range/iterator.hpp>

namespace bno=boost::numeric::odeint;
namespace gill=astron::gillespie;
namespace g_vgcc=gill::vgcc;
namespace g_pmca=gill::pmca;
namespace consts=astron::phy_const;

typedef std::vector< double > state_type;
typedef state_type::iterator state_type_it;
typedef bno::runge_kutta4 < state_type > rk4_stepper_type;

//-----------------
int main(int argc, char *argv[])
{
  //-------------------------------------
  std::string filename;         // Output filename variable
  // decode arguments passed
  if (argc < 2){
    filename = argv[0];
    std::cout << "No filename prefix provided" << std::endl;
    filename = filename.substr(2,filename.length()-2) + "_run";
  }
  else{
    filename = std::string(argv[1]);
  }
  std::ofstream fout(filename);
  if (!fout){
    std::cout << "Error opening file " << filename << "\n";
    return (0);
  }
  std::cout << "File = " << filename << std::endl;
  // ----------------------------------
  const double vol_pre = 1E-18;	// Volume of whole presynaptic terminal
  const double vol_pmca = 1E-18;
  const double vol_vgcc = 1E-18;

  double conCaIn = 100E-09;	// Baseline Calcium concentration
  int numCaIn = conCaIn * consts::Av * vol_pre * 1000; // Molarity in Liters 1L = 1000 * 10^-6 meter^3

  // Initialize deterministic objects
  rk4_stepper_type stepper;

  HH_POST hh_post(500E-12);                             // constructs an hh_pre object with 25 micron radius
  state_type X_hh_post = {0,0,0,-80E-03};                    //{m,h,n,V}

  //PMCA pmca(100E-09);                                   // constructs an PMCA object with 100 nM calcium concentration (intracellular)
  //state_type X_pmca = {100E-09, 17E-03, 0, 0};          //{conCaIn, PMCA0, PMCA1, PMCA2}

  //VGCC vgcc;                                            // constructs VGCCs
  //state_type X_vgcc = {50, 0, 0, 0, 0};                //{C0, C1, C2, C3, O}
  //-------------------------------------

  // Initialize MCMC: PMCA
  const int n_pmca = 1;
  const int n_states_pmca = 5; 	// [NCa, Leak, PMCA0, PMCA1, PMCA2]
  std::vector<int> X_pmca = {62,1530,1530,0,0}; // [NCa, NLeak, NPMCA0, PMCA1, PMCA2]
  std::vector<g_pmca::pmca_gillespie_class> gpmca;
  for(int i=0; i <n_pmca; i++){
    gpmca.push_back(g_pmca::pmca_gillespie_class(1000,X_pmca));
  }
  // Initialize MCMC: VGCC
  const int n_vgcc = 1;
  const int n_states_vgcc = 5;	// [C0 C1 C2 C3 O]
  std::vector<int> X_vgcc = {50,0,0,0,0};
  std::vector<g_vgcc::vgcc_gillespie_class> gvgcc;
  for(int i=0; i <n_vgcc; i++){
    gvgcc.push_back(g_vgcc::vgcc_gillespie_class(vol_vgcc,X_vgcc));
  }
  // Initialize MCMC: ca_sensors
  //const int n_ca_sensors = 1; // number of individual Ca-sensors/vesicles
  //const int n_states_ca_sensor=21; // number of Ca-sensor states
  //std::vector<int> X_cas(n_states_ca_sensor); // State vector for ca_sensor initialization in state X0Y0;
 //for(int i=0;i<n_states_ca_sensor; i++){
 //  X_cas[i] = 0;
 //};
 //X_cas[0] = 7;
 //std::vector<gill::ca_sensor_gillespie_class> ca_sensors;    // ca_sensor object construction
 //for(int i=0; i<n_ca_sensors; i++){
 //  ca_sensors.push_back(gill::ca_sensor_gillespie_class(X_cas)); // Loads state to each ca_sensor
 //}
 ////-------------------------------------
 ////                      time(1) IExt(1) X_hh_post(4) vgcc(5) pmca(4) conCaIn(1) ca_sensor(21) spon_rel(1) syn_rel(1) asyn_rel(1) total_rel(1) =
 //const int n_param_out = (1) + 2+ 21;
 //std::vector< double > Y(11);                           // Ouput vector
  //--------------------------------------
  double t = 0.0;
  const double dt = 10E-06;
  const double tmax = 100E-03;
  // -------------------------------------

  while (t <= tmax){
    //std::cout << "conCaIn = " << conCaIn << " numCaIn = " << numCaIn << std::endl;

    if( (t > 22E-03) && ( t < (24E-03 )) ){
      hh_post.IExt = 20E-02;	// External current to trigger presynaptic spike
    }
    if( (t >= 24E-03) && ( t <= (tmax )) ){
      hh_post.IExt = 0;
    }
    stepper.do_step( hh_post, X_hh_post, t, dt); // update voltage

    //gpmca[0].X[0] = numCaIn;
    for(int i=0; i<n_pmca; i++){
      gillespie(gpmca[i],t,dt);
    }
    numCaIn = gpmca[0].X[0]; //abs(g_pmca::pmca_gillespie_class::numCaIn);
    conCaIn = ((numCaIn) / (consts::Av * vol_pre * 1000));

    g_vgcc::vgcc_gillespie_class::V = X_hh_post[3];
    g_vgcc::vgcc_gillespie_class::conCaIn = conCaIn;
    for(int i=0; i<n_vgcc; i++){
      gillespie(gvgcc[i],t,dt);
    }
    numCaIn = numCaIn + abs(g_vgcc::vgcc_gillespie_class::numCaFlux);
    conCaIn = numCaIn / (consts::Av * vol_pre * 1000);

    fout << t << " " << X_hh_post << " " << gpmca[0].X << " " << gvgcc[0].X << " " << numCaIn << " " << conCaIn << std::endl;
    //std::cout << t << " " << gpmca[0].X << std::endl;
    //std::cin.get();
    t = t + dt;
  }
  fout.close();
  return 0;
}

// Extra codes
  // g_pmca::pmca_gillespie_class::numCaIn = numCaIn;
//g_vgcc::vgcc_gillespie_class::conCaIn = conCaIn;		   // Set vgcc.conCaIn
    //g_vgcc::vgcc_gillespie_class::V = X_hh_post[3];		   // Set vgcc.V
   // for(int i=0; i<n_vgcc; i++){
   //   gillespie(gvgcc[i],t,dt);
   // }
    //conCaIn = conCaIn + (g_pmca::pmca_gillespie_class::numCaIn / (consts::Av * vol_pmca) ); // update conCaIn

    //vgcc.V = X_hh_post[3];	// Feed presynaptic voltage to VGCC
    //stepper.do_step( vgcc, X_vgcc, t,  dt); // update vgcc
    //vgcc.update(dt, X_vgcc[4]);
    //X_pmca[0]  = conCaIn; // update conCaIn of PMCA buffers
    //stepper.do_step( pmca, X_pmca, t, dt);
    //conCaIn = X_pmca[0];	// update conCaIn
    //numCaIn_ca_sensor = round(vgcc.numCaIn/10) + 1;//und((vgcc.numCaIn - 59) / 60 );//round((vgcc.conCaIn - 59) / 6 );
    //gill::ca_sensor_gillespie_class::conCaIn = numCaIn_ca_sensor;
    //for(int i=0; i<n_ca_sensors; i++){
    //   gillespie(ca_sensors[i],t,dt);
    //}

   //Y[7] = gill::ca_sensor_gillespie_class::sponRelcount;
   //Y[8] = gill::ca_sensor_gillespie_class::synRelcount;
   //Y[9] = gill::ca_sensor_gillespie_class::asynRelcount;
   //Y[10] = gill::ca_sensor_gillespie_class::totalRelcount;


// Copyright 2018 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <stdint.h>  // NOLINT
#include <algorithm>
#include <iomanip>

#include <Eigen/Dense>
#include "cpp/sensor_fusion/geometry_toolbox_mahony.h"
#include "cpp/sensor_fusion/low_pass_filter.h"
#include "cpp/sensor_fusion/orientation_filter.h"
#include "cpp/sensor_fusion/stationary_detector.h"

namespace {

// Minimum time step between sensor updates. This corresponds to 1000 Hz.
static const double kMinTimestepS = 0.001f;

// Maximum time step between sensor updates. This corresponds to 1 Hz.
static const double kMaxTimestepS = 1.0f;

// Gravity constant in m.s^(-2).
static const double kMagnitudeOfGravity = 9.81;

// Small threshold to check if close to zero.
static const double kEpsilon = 1e-9;

// Number of runs for bias correction to remain static.
// This is less than 1 sec.
static const int kMagBiasCorrectionStaticCount = 40;

// Small threshold used for mag initialization.
static const double kMagInitTolerance = 1e-6;

// Number of mag samples used for mag measurement initialization. 25 samples
// correspond to ~.4 seconds for mag at 60 Hz.
static const int kNumMagForInitialization = 25;

// Number of continuous invalid mag measurement that results in temporarily
// disabling mag fusion, unless new mag calibration is performed.
static const int kNumMagForFilterOutlierRejection = 10;

// Threshold of rejecting using one mag measurement. The value for this
// threshold is in rad, which corresponds to 5 degree.
static const double kMaxAllowedMagDeviationRadians = 0.0872665;

// Threshold of using mag measurements on its timestamp. If the timestamp
// between mag and gyro is larger than the given value, we simply do not use
// the current mag measurement. This behavior happens in this multi-threaded
// sensor fusion system, but not often.
static const double kMaxTimeDifferentInMagAndGyroInSeconds = 0.003f;

// Helper method to validate the time steps of sensor timestamps.
bool IsTimestampDeltaValid(double timestamp_delta_s) {
  if (timestamp_delta_s <= kMinTimestepS) {
    return false;
  }
  if (timestamp_delta_s > kMaxTimestepS) {
    return false;
  }
  return true;
}

mahony_filter::StationaryDetector::StationaryDetectorConfiguration
GetStationaryDetectorConfig(double stationary_bias_correction_gain) {
  mahony_filter::StationaryDetector::StationaryDetectorConfiguration
      stationary_config;
  stationary_config.stationary_bias_correction_gain =
      stationary_bias_correction_gain;
  return stationary_config;
}

}  // namespace

OrientationFilter::OrientationFilter(
    const OrientationFilterConfiguration& config)
    : config_(config),
      state_(Eigen::Matrix<double, 7, 1>::Zero()),
      next_state_(Eigen::Matrix<double, 7, 1>::Zero()),
      is_orientation_initialized_(false),
      first_accel_timestamp_s_(0.0),
      has_received_gyro_sample_(false),
      mag_is_available_(false),
      mag_low_pass_filter_(config.magnetometer_low_pass_cutoff_frequency),
      accel_aligned_R_yaw_mag_aligned_(Eigen::Matrix3d::Identity()),
      num_mag_measurements_for_yaw_initialization_(kNumMagForInitialization),
      mag_meas_for_init_index_(0),
      mag_bias_(Eigen::Vector3d::Zero()),
      new_mag_calibration_available_(false),
      mag_status_(MagStatus::kInitial),
      accumulated_num_of_outlier_mag_measurement_(0),
      current_mag_sample_fits_calibration_(false),
      stationary_detector_(
          GetStationaryDetectorConfig(config.stationary_bias_correction_gain)) {
  // Initialize quaternions to start with the identity rotation.
  state_(3) = 1.0;
  next_state_(3) = 1.0;

  // Initialize the data buffer for mag initialization.
  projected_mag_measurement_vector_.resize(
      Eigen::NoChange, num_mag_measurements_for_yaw_initialization_);

  // Set bias of magnetometer calibrator when it's provided in configuration.
  if (config_.init_mag_bias != Eigen::Vector3d::Zero()) {
    SetMagBias(config_.init_mag_bias);
  }
}

void OrientationFilter::Run() {
  // Initialization for the filter orientation.
  if (!is_orientation_initialized_ &&
      current_accel_measurement_.timestamp_s > 0.0 &&
      (config_.init_config ==
           OrientationFilterConfiguration::InitialOrientationConfiguration::
               kDontUseMagToInitOrientation ||
       current_mag_measurement_.timestamp_s > 0.0)) {
    is_orientation_initialized_ = OrientationFromAccelAndMag();
  }

  // Only start to propagate once the orientation has already been initialized
  // by the accel / mag.
  if (is_orientation_initialized_) {
    FilterPropagate();
  }
}

void OrientationFilter::RegisterOnBadMagnetometerCalibrationDetectedCallback(
    std::function<void()>* on_bad_mag_calibration_detected_callback) {
  on_bad_mag_calibration_detected_callbacks_.insert(
      on_bad_mag_calibration_detected_callback);
}

void OrientationFilter::UnRegisterOnBadMagnetometerCalibrationDetectedCallback(
    std::function<void()>* on_bad_mag_calibration_detected_callback) {
  on_bad_mag_calibration_detected_callbacks_.erase(
      on_bad_mag_calibration_detected_callback);
}

void OrientationFilter::BadMagnetometerCalibrationDetectedBroadcast() const {
  for (auto callback : on_bad_mag_calibration_detected_callbacks_) {
    if (callback) {
      (*callback)();
    }
  }
}

void OrientationFilter::AddAccelMeasurement(const Eigen::Vector3d& sample,
                                            double timestamp_s) {
  if (first_accel_timestamp_s_ == 0.0) {
    first_accel_timestamp_s_ = timestamp_s;
    if (is_orientation_initialized_) {
      // Check if the state is aligned with gravity and fix it if it is not.
      const Eigen::Vector3d g_from_acc = sample.normalized();
      const Eigen::Vector3d g_est = ComputeGravityEstimate();

      double dot_product = g_from_acc.dot(g_est);
      // Clamp dot_prodcut.
      if (std::abs(dot_product) > 1.0) {
        dot_product = (dot_product > 0.0 ? 1.0 : -1.0);
      }
      const double angular_error_deg = 180. / M_PI * acos(dot_product);
      // TODO: Add a better gravity / acceleration check here.
      if (std::abs(angular_error_deg) > 44) {
        // The error after SetPose is too large. Correct directly the aligment
        // with gravity.
        const Eigen::Vector4d correction_quat =
            geometry_toolbox::RotateInto(g_est, g_from_acc);

        state_.head<4>() = geometry_toolbox::QuaternionMultiplication(
            correction_quat, state_.head<4>());
      }
    }
  }

  current_accel_measurement_ = SensorSample(sample, timestamp_s);

  if (IsStationaryBiasCorrectionEnabled()) {
    stationary_detector_.AddAccelMeasurement(current_accel_measurement_.sample,
                                             timestamp_s);
  }

  if (!has_received_gyro_sample_) {
    // If we have not received a gyro sample supply fake gyro data to make use
    // of every accel sample.
    current_gyro_measurement_ =
        SensorSample(Eigen::Vector3d::Zero(), timestamp_s);
    Run();
  }
}

void OrientationFilter::AddGyroMeasurement(const Eigen::Vector3d& sample,
                                           double timestamp_s) {
  current_gyro_measurement_ = SensorSample(sample, timestamp_s);

  if (IsStationaryBiasCorrectionEnabled()) {
    stationary_detector_.AddGyroMeasurement(current_gyro_measurement_.sample,
                                            timestamp_s);
  }

  const double delta_t_s =
      (timestamp_s - previous_gyro_measurement_.timestamp_s);
  if (IsTimestampDeltaValid(delta_t_s)) {
    if (!has_received_gyro_sample_) {
      has_received_gyro_sample_ = true;
    }
    Run();
  }

  previous_gyro_measurement_ = current_gyro_measurement_;
}

void OrientationFilter::AddMagMeasurement(const Eigen::Vector3d& sample,
                                          double timestamp_s,
                                          bool fits_calibration) {
  // First sample is only used for setting up the timeline.
  if (current_mag_measurement_.timestamp_s == 0.0) {
    current_mag_measurement_ = SensorSample(sample, timestamp_s);
    current_mag_sample_fits_calibration_ = fits_calibration;
    return;
  }

  const double mag_delta_time_s =
      (timestamp_s - previous_mag_measurement_.timestamp_s);

  // Use uncalibrated mag correction only if stationary correction is not
  // enabled.
  if (!IsStationaryBiasCorrectionEnabled()) {
    // Now that the timestep is valid we can initialize the low_pass filter.
    mag_low_pass_filter_.AddSampleData(sample, mag_delta_time_s);
    if (mag_low_pass_filter_.IsInitialized()) {
      previous_mag_measurement_ =
          SensorSample(mag_low_pass_filter_.GetFilteredData(),
                       current_mag_measurement_.timestamp_s);
    }
  } else {
    previous_mag_measurement_ = current_mag_measurement_;
  }

  current_mag_measurement_ = SensorSample(sample, timestamp_s);
  current_mag_sample_fits_calibration_ = fits_calibration;

  // Only process valid mag sample.
  mag_is_available_ = IsTimestampDeltaValid(mag_delta_time_s);
}

bool OrientationFilter::ComputeIterativeSolution(
    const Eigen::VectorXd& mag_vector, const double initial_solution,
    Eigen::Matrix3d* accel_aligned_R_yaw_mag_aligned) {
  CHECK(accel_aligned_R_yaw_mag_aligned != nullptr);
  double current_solution = initial_solution;

  // Parameters for Gauss-Newton algorithm.
  constexpr int kMaxIteration = 25;
  constexpr double kNormCorrectionForConvergence = 1e-5;

  int current_iteration = 0;
  for (; current_iteration < kMaxIteration; ++current_iteration) {
    // Estimated measurement and Jacobian for Gauss-Newton.
    Eigen::Vector2d z_est(-std::sin(current_solution),
                          std::cos(current_solution));
    Eigen::Vector2d Jacobian(-std::cos(current_solution),
                             -std::sin(current_solution));

    double residual = 0;
    double hessian = 0;

    for (int i = 0; i < mag_vector.rows() / 2; ++i) {
      const Eigen::Vector2d residual_i = z_est - mag_vector.segment<2>(i * 2);
      residual += Jacobian.transpose() * residual_i;
      hessian += Jacobian.transpose() * Jacobian;
    }

    double correction = -residual / hessian;
    if (hessian < kMagInitTolerance) {
      // Invalid Hessian, mag initialization fails.
      return false;
    }

    current_solution += correction;

    if (std::abs(correction) < kNormCorrectionForConvergence) {
      break;
    }
  }

  if (current_iteration >= kMaxIteration) {
    // Maximum iteration reached, mag initialization fails.
    return false;
  }

  *accel_aligned_R_yaw_mag_aligned << std::cos(current_solution),
      -std::sin(current_solution), 0, std::sin(current_solution),
      std::cos(current_solution), 0, 0, 0, 1;

  return true;
}

bool OrientationFilter::ComputeYawAlignmentMatrix(
    const Eigen::Matrix3Xd& mag_proj,
    Eigen::Matrix3d* accel_aligned_R_yaw_mag_aligned) {
  CHECK(accel_aligned_R_yaw_mag_aligned != nullptr);
  const int num_rows = 2 * mag_proj.cols();
  Eigen::VectorXd mag_vector(num_rows, 1);
  for (int i = 0; i < mag_proj.cols(); ++i) {
    if (mag_proj.col(i).head<2>().squaredNorm() < kMagInitTolerance) {
      return false;
    }

    mag_vector.segment<2>(i * 2) = mag_proj.col(i).head<2>().normalized();
  }

  const double init_solution = -std::atan2(mag_vector(0), mag_vector(1));

  return ComputeIterativeSolution(mag_vector, init_solution,
                                  accel_aligned_R_yaw_mag_aligned);
}

bool OrientationFilter::OrientationFromAccelAndMag() {
  Eigen::Vector3d L_x;
  Eigen::Vector3d L_y;
  Eigen::Vector3d L_z;
  Eigen::Matrix<double, 3, 3> L_R_G;

  // East-North-Up frame of reference:
  //   - Gravity vector lies along +z axis
  //   - Horizontal component of Mag vector lies along +y axis
  //   - x-axis points east
  L_z = current_accel_measurement_.sample.normalized();

  switch (config_.init_config) {
    case OrientationFilterConfiguration::InitialOrientationConfiguration::
        kDontUseMagToInitOrientation: {
      // Depending on whether we are portrait or landscape, this will
      // be the direction we expect gravity to be pointing if the device
      // is right-side-up.
      Eigen::Vector3d canonical_down;
      // Is it more in landscape or in portrait mode?
      if (std::abs(L_z.dot(Eigen::Vector3d::UnitY())) <
          std::abs(L_z.dot(Eigen::Vector3d::UnitX()))) {
        // Landscape
        L_y = Eigen::Vector3d::UnitY();
        canonical_down = Eigen::Vector3d::UnitX();
      } else {
        // Portrait
        L_y = -Eigen::Vector3d::UnitX();
        canonical_down = Eigen::Vector3d::UnitY();
      }
      // Is it right-side-up or upside-down?
      if (L_z.dot(canonical_down) < 0) {
        L_y = -L_y;
      }
      break;
    }
    case OrientationFilterConfiguration::InitialOrientationConfiguration::
        kUseMagToInitOrientation:
      L_y = current_mag_measurement_.sample.normalized();
      L_y -= L_z * L_z.transpose() * L_y;
  }

  L_x = -L_z.cross(L_y);
  if (L_x.norm() == 0.0) {
    return false;
  }

  L_x = L_x / L_x.norm();
  L_y = L_z.cross(L_x);
  if (L_y.norm() == 0.0) {
    return false;
  }

  L_R_G.block<3, 1>(0, 0) = L_x;
  L_R_G.block<3, 1>(0, 1) = L_y;
  L_R_G.block<3, 1>(0, 2) = L_z;

  // Compute and assign the quaternion of orientation from the resulting
  // rotation matrix.
  state_.block<4, 1>(0, 0) =
      geometry_toolbox::RotationMatrixToQuaternion(L_R_G);

  state_from_previous_mag_ = state_;
  mag_is_available_ = false;
  return true;
}

void OrientationFilter::FilterPropagate() {
  // TODO: Orientation filter should be robust to different update
  // frequency.
  const double delta_t = current_gyro_measurement_.timestamp_s -
                         previous_gyro_measurement_.timestamp_s;
  if (!IsTimestampDeltaValid(delta_t)) {
    return;
  }

  // Ignore the current mag measurement if it is too old.
  if (mag_is_available_) {
    const double gyro_time_ahead = current_gyro_measurement_.timestamp_s -
                                   current_mag_measurement_.timestamp_s;

    if (gyro_time_ahead > kMaxTimeDifferentInMagAndGyroInSeconds) {
      // If this behavior happens very often, we should project previous mag
      // measurement onto current timestamp, to do not waste too much data.
      // In practice, in current experiments, this happens at a very low
      // frequency.
      mag_is_available_ = false;
    }
  }

  Eigen::Matrix<double, 6, 1> gyro_measurements;
  gyro_measurements.head<3>() = previous_gyro_measurement_.sample;
  gyro_measurements.tail<3>() = current_gyro_measurement_.sample;
  // Bias compensate the gyro data. Since imu_measurement format is
  // [prev_x, prev_y, prev_z, curr_x, curr_y, curr_z] this can be accomplished
  // by subtracting the bias_x, bias_y, bias_z from the appropriate elements.
  const Eigen::Vector3d rate_correction = ComputeAccelAndMagRateCorrection();

  gyro_measurements.head<3>() +=
      (-state_.tail<3>() +
       (IsInitializing()
            ? config_.attitude_correction_gain_during_initialization
            : config_.attitude_correction_gain) *
           rate_correction);
  gyro_measurements.tail<3>() +=
      (-state_.tail<3>() +
       (IsInitializing()
            ? config_.attitude_correction_gain_during_initialization
            : config_.attitude_correction_gain) *
           rate_correction);

  Eigen::Matrix<double, 4, 1> current_q, next_q;
  current_q = state_.head<4>();
  next_q = state_.head<4>();
  quaternion_integrator_.Integrate(current_q, gyro_measurements, delta_t,
                                   &next_q);
  next_state_.head<4>() = next_q;
  next_state_.tail<3>() =
      state_.tail<3>() -
      ((IsInitializing() ? 0.0 : config_.gyroscope_bias_correction_gain) *
       delta_t * rate_correction);

  // Bias correction if stationary.
  if (IsStationaryBiasCorrectionEnabled()) {
    next_state_.tail<3>() -=
        delta_t *
        stationary_detector_.GetGyroBiasCorrection(
            next_state_.tail<3>(), current_gyro_measurement_.timestamp_s);
  }
  state_ = next_state_;

  if (mag_is_available_) {
    const double mag_delta_t = (current_mag_measurement_.timestamp_s -
                                previous_mag_measurement_.timestamp_s);
    if (!IsStationaryBiasCorrectionEnabled()) {
      // Only apply this correction if the stationary bias correction is not
      // enabled.
      const Eigen::Vector3d mag_correction = EstimateBiasUpdateUsingMag();
      state_.tail<3>() -=
          mag_delta_t *
          config_.magnetometer_gain_for_gyroscope_bias_estimation *
          mag_correction;
    }

    state_from_previous_mag_ = state_;
    mag_is_available_ = false;
  }
}

Eigen::Vector3d OrientationFilter::EstimateBiasUpdateUsingMag() {
  if (!mag_low_pass_filter_.IsInitialized()) {
    return Eigen::Vector3d::Zero();
  }
  // Check that gyro norm is below threshold. This is to ensure we only estimate
  // drift when device is still and gyro signal is most likely due to drift.
  const bool is_gyro_static =
      current_gyro_measurement_.sample.norm() <
      config_.maximum_allowed_gyro_norm_changed_for_mag_bias_correction;

  // Check that mag norm is below threshold. This is to ensure we only estimate
  // drift when device is still and mag signal is similar to noise.
  const bool is_mag_static =
      (previous_mag_measurement_.sample -
       mag_low_pass_filter_.GetFilteredData())
          .norm() < config_.maximum_allowed_magnitude_magnetometer_change_mt;
  mag_low_pass_filter_.SetIsStatic(is_gyro_static && is_mag_static);

  if (!mag_low_pass_filter_.GetIsStaticForN(kMagBiasCorrectionStaticCount)) {
    return Eigen::Vector3d::Zero();
  }

  // Get down direction from both previous mag and current state.
  const Eigen::Vector3d previous_accel_est =
      geometry_toolbox::QuaternionToRotationMatrix(
          state_from_previous_mag_.head<4>())
          .col(2);
  const Eigen::Vector3d previous_mag_est =
      geometry_toolbox::QuaternionToRotationMatrix(
          state_from_previous_mag_.head<4>())
          .col(1);
  const Eigen::Vector3d current_accel_est =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>()).col(2);
  const Eigen::Vector3d current_mag_est =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>()).col(1);

  // Grab the previous mag, remove gravity and normalize.
  Eigen::Vector3d previous_mag_meas = previous_mag_measurement_.sample;
  previous_mag_meas -=
      previous_accel_est * previous_accel_est.transpose() * previous_mag_meas;
  previous_mag_meas.normalize();

  // Grab the current mag, remove gravity and normalize.
  Eigen::Vector3d mag_meas = mag_low_pass_filter_.GetFilteredData();
  mag_meas -= current_accel_est * current_accel_est.transpose() * mag_meas;
  mag_meas.normalize();

  return mag_meas.cross(previous_mag_meas) -
         current_mag_est.cross(previous_mag_est);
}

Eigen::Vector3d OrientationFilter::ComputeAccelAndMagRateCorrection() {
  Eigen::Vector3d accel_meas = current_accel_measurement_.sample;
  const double accel_magnitude = accel_meas.norm();
  if (accel_magnitude < 1e-6) {
    return Eigen::Vector3d::Zero();
  }

  accel_meas.normalize();
  Eigen::Matrix<double, 3, 3> L_R_G_accel_aligned =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>());
  Eigen::Vector3d accel_est = L_R_G_accel_aligned.col(2);

  // Dampen the effect of body acceleration. This is only applied when not
  // initializing because it would otherwise slow down convergence.
  double gain = 1.0;
  const double gyro_norm = current_gyro_measurement_.sample.norm();
  if (!IsInitializing()) {
    // Don't update filter while moving too fast.
    // TODO: Consider using something different than this, since most
    // accelerometer sensors don't report 1G at rest.
    gain /= (1.0 + std::abs(accel_magnitude - kMagnitudeOfGravity));

    // Don't update filter while moving to fast.
    // Use full gain between 0 and 0.1, dampen toward 0 between 0.1 to
    // 0.3 rad.s^(-1).
    // 1 __                  //
    //     \                 //
    //      \                //
    //       \______ 0.0     //
    //  ^  ^  ^              //
    //  0 .1  .3             //
    gain *= std::max(0.0, std::min(1.0, 1.5 - 5.0 * gyro_norm));
  } else {
    // Use full gain between 0 and 0.04, dampen toward 0 between 0.04 to
    // 0.1 rad.s^(-1).
    // 1 __                  //
    //     \                 //
    //      \                //
    //       \______ 0.0     //
    //  ^  ^  ^              //
    //  0 .04  .1            //
    gain *= std::max(0.0, std::min(1.0, 1.5 - 15.0 * gyro_norm));
  }

  Eigen::Vector3d rate_correction_vector = gain / 2 *
                                           config_.accel_yaw_correction_gain *
                                           accel_meas.cross(accel_est);

  // Check if there is a valid mag sample ready to be consumed.
  if (mag_is_available_ && current_mag_sample_fits_calibration_ &&
      config_.mag_yaw_correction_gain > 0 &&
      current_mag_measurement_.timestamp_s > 0) {
    // First check if received a new mag calibration.
    if (new_mag_calibration_available_) {
      // Invalidate the existing mag alignement and force recompute.
      mag_status_ = MagStatus::kAligning;
      mag_meas_for_init_index_ = 0;
      LOG(INFO) << "SensorFusion: Received new bias, estimating alignment.";
      new_mag_calibration_available_ = false;
    }

    const Eigen::Vector3d current_calibrated_mag_measurement =
        current_mag_measurement_.sample - mag_bias_;

    // Compute estimated mag value, projected on yaw and represented locally.
    Eigen::Vector3d mag_est_projection = current_calibrated_mag_measurement;
    mag_est_projection -=
        accel_est * accel_est.transpose() * mag_est_projection;
    mag_est_projection.normalize();

    // Estimate alignment between the filter orientation and magnetic north.
    if (mag_status_ == MagStatus::kAligning) {
      // Store the projected mag in a buffer that will be used by the iterative
      // solver.
      projected_mag_measurement_vector_.col(mag_meas_for_init_index_) =
          L_R_G_accel_aligned.transpose() * mag_est_projection;

      ++mag_meas_for_init_index_;

      // Check if we have enough sample to run the iterative solver.
      if (mag_meas_for_init_index_ ==
          num_mag_measurements_for_yaw_initialization_) {
        if (ComputeYawAlignmentMatrix(projected_mag_measurement_vector_,
                                      &accel_aligned_R_yaw_mag_aligned_)) {
          // In this case, mag is successfully initialized.
          mag_status_ = MagStatus::kAligned;
        } else {
          // Mag initialization fails. Clear mag measurements, and re-start data
          // collection.
          mag_meas_for_init_index_ = 0;
          LOG(INFO) << "SensorFusion: Mag alignment failed in orientation "
                       "tracker. Will retry.";
        }
      }
    }

    Eigen::Vector3d mag_meas = current_calibrated_mag_measurement;
    mag_meas -= accel_meas * accel_meas.transpose() * mag_meas;
    if (mag_meas.norm() < 1e-6) {
      return Eigen::Vector3d();
    }
    mag_meas.normalize();

    if (mag_status_ == MagStatus::kAligned) {
      // Magnetometer is calibrated and aligned with the filter now so we can
      // use it for yaw correction.
      mag_is_available_ = false;

      const Eigen::Vector3d mag_est =
          (L_R_G_accel_aligned * accel_aligned_R_yaw_mag_aligned_).col(1);

      // Test whether the current mag estimate and mag measurement diverge too
      // much.
      const double angle_between_est_and_meas =
          std::acos(mag_est_projection.dot(mag_est));

      if (angle_between_est_and_meas > kMaxAllowedMagDeviationRadians) {
        ++accumulated_num_of_outlier_mag_measurement_;
        if (accumulated_num_of_outlier_mag_measurement_ >
            kNumMagForFilterOutlierRejection) {
          // Invalidate the existing mag alignement and force recompute.
          mag_status_ = MagStatus::kAligning;
          state_from_previous_mag_ = state_;
          mag_meas_for_init_index_ = 0;
          LOG(INFO) << "Consistently received outlier measurements, resetting.";
          BadMagnetometerCalibrationDetectedBroadcast();
          accumulated_num_of_outlier_mag_measurement_ = 0;
        }
      } else {
        rate_correction_vector +=
            config_.mag_yaw_correction_gain * mag_meas.cross(mag_est);

        accumulated_num_of_outlier_mag_measurement_ = 0;
      }
    }
  }

  return rate_correction_vector;
}

void OrientationFilter::Recenter() {
  // This function enforces two constraints on the updated L_R_G_recenter
  // - The gravity direction is kept
  //    L_R_G_recenter * z = L_R_G * z
  // - yaw is equal to zero
  //    L_R_G_recenter^{-1} * -z  =[sin(pitch) 0 cos(pitch)]
  // Notation:
  // L_R_G^{-1} * -z =  [cos(yaw) * sin(pitch) sin(yaw) cos(yaw) * cos(pitch)]

  //
  // We can deduce that
  // L_R_G_recenter(:,3) == L_R_G(:,3) (1)
  // L_R_G_recenter(3,2) = 0.0 (2)
  //
  // As L_R_G is an orthonormal matrix
  // |L_R_G(:,2)| = 1 (3)
  //
  // Combining (2) and (3)
  // |L_R_G(1:2,2)| = 1 (4)
  // L_R_G(1:2,2) . L_R_G(1:2,3) = 0  (5) Orthogonality
  //
  // if L_R_G(1,3) >  Eps
  // L_R_G(2,2) =  +/- sqrt( 1 / ( 1 + (L_R_G(2,3) / L_R_G(1,3))^2)) (6)
  // L_R_G(1,2) = - L_R_G(2,2) * L_R_G(2,3) / L_R_G(1,3)
  //
  // Finally we use the orthogonality constraint for the first column
  // L_R_G(:,1) = L_R_G(:,2) x L_R_G(:,3)

  const Eigen::Matrix3d L_R_G =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>());

  Eigen::Matrix3d L_R_G_recentered;
  // Keep down direction the same (1).
  L_R_G_recentered.block<3, 1>(0, 2) = L_R_G.block<3, 1>(0, 2);

  if (std::abs(L_R_G(0, 2)) < kEpsilon) {
    // Arbitrary deciding to use X axis.
    L_R_G_recentered(0, 1) = 1.0;
    L_R_G_recentered(1, 1) = 0.0;
  } else {
    const double x_y_ratio = L_R_G(1, 2) / L_R_G(0, 2);  // Eq. (6)
    // The sign  is kept positive to enforce "forwardness".
    L_R_G_recentered(1, 1) = sqrt(1.0 / (1 + x_y_ratio * x_y_ratio));
    L_R_G_recentered(0, 1) = -L_R_G_recentered(1, 1) * x_y_ratio;
  }
  L_R_G_recentered(2, 1) = 0.0;  // Eq. (2)

  // Enforce orthogonality.
  L_R_G_recentered.block<3, 1>(0, 0) = L_R_G_recentered.block<3, 1>(0, 1).cross(
      L_R_G_recentered.block<3, 1>(0, 2));

  state_.head<4>() =
      geometry_toolbox::RotationMatrixToQuaternion(L_R_G_recentered);

  // Reset the mag based bias update by setting the previous state to current
  // state. This avoids using the change in pose due to recentering for bias
  // update.
  state_from_previous_mag_ = state_;
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_phi2.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_tsfn.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace gstmerc
    {

            struct par_gstmerc
            {
                double lamc;
                double phic;
                double c;
                double n1;
                double n2;
                double XS;
                double YS;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_gstmerc_spheroid : public base_t_fi<base_gstmerc_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_gstmerc m_proj_parm;

                inline base_gstmerc_spheroid(const Parameters& par)
                    : base_t_fi<base_gstmerc_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double L, Ls, sinLs1, Ls1;
                    L= this->m_proj_parm.n1*lp_lon;
                    Ls= this->m_proj_parm.c+this->m_proj_parm.n1*log(pj_tsfn(-1.0*lp_lat,-1.0*sin(lp_lat),this->m_par.e));
                    sinLs1= sin(L)/cosh(Ls);
                    Ls1= log(pj_tsfn(-1.0*asin(sinLs1),0.0,0.0));
                    xy_x= (this->m_proj_parm.XS + this->m_proj_parm.n2*Ls1)*this->m_par.ra;
                    xy_y= (this->m_proj_parm.YS + this->m_proj_parm.n2*atan(sinh(Ls)/cos(L)))*this->m_par.ra;
                    /*fprintf(stderr,"fwd:\nL      =%16.13f\nLs     =%16.13f\nLs1    =%16.13f\nLP(%16.13f,%16.13f)=XY(%16.4f,%16.4f)\n",L,Ls,Ls1,lp_lon+this->m_par.lam0,lp_lat,(xy_x*this->m_par.a + this->m_par.x0)*this->m_par.to_meter,(xy_y*this->m_par.a + this->m_par.y0)*this->m_par.to_meter);*/
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double L, LC, sinC;
                    L= atan(sinh((xy_x*this->m_par.a - this->m_proj_parm.XS)/this->m_proj_parm.n2)/cos((xy_y*this->m_par.a - this->m_proj_parm.YS)/this->m_proj_parm.n2));
                    sinC= sin((xy_y*this->m_par.a - this->m_proj_parm.YS)/this->m_proj_parm.n2)/cosh((xy_x*this->m_par.a - this->m_proj_parm.XS)/this->m_proj_parm.n2);
                    LC= log(pj_tsfn(-1.0*asin(sinC),0.0,0.0));
                    lp_lon= L/this->m_proj_parm.n1;
                    lp_lat= -1.0*pj_phi2(exp((LC-this->m_proj_parm.c)/this->m_proj_parm.n1),this->m_par.e);
                    /*fprintf(stderr,"inv:\nL      =%16.13f\nsinC   =%16.13f\nLC     =%16.13f\nXY(%16.4f,%16.4f)=LP(%16.13f,%16.13f)\n",L,sinC,LC,((xy_x/this->m_par.ra)+this->m_par.x0)/this->m_par.to_meter,((xy_y/this->m_par.ra)+this->m_par.y0)/this->m_par.to_meter,lp_lon+this->m_par.lam0,lp_lat);*/
                }

                static inline std::string get_name()
                {
                    return "gstmerc_spheroid";
                }

            };

            // Gauss-Schreiber Transverse Mercator (aka Gauss-Laborde Reunion)
            template <typename Parameters>
            void setup_gstmerc(Parameters& par, par_gstmerc& proj_parm)
            {
                proj_parm.lamc= par.lam0;
                proj_parm.n1= sqrt(1.0+par.es*pow(cos(par.phi0),4.0)/(1.0-par.es));
                proj_parm.phic= asin(sin(par.phi0)/proj_parm.n1);
                proj_parm.c=       log(pj_tsfn(-1.0*proj_parm.phic,0.0,0.0))
                     -proj_parm.n1*log(pj_tsfn(-1.0*par.phi0,-1.0*sin(par.phi0),par.e));
                proj_parm.n2= par.k0*par.a*sqrt(1.0-par.es)/(1.0-par.es*sin(par.phi0)*sin(par.phi0));
                proj_parm.XS= 0;/* -par.x0 */
                proj_parm.YS= -1.0*proj_parm.n2*proj_parm.phic;/* -par.y0 */
                /*fprintf(stderr,"a  (m) =%16.4f\ne      =%16.13f\nl0(rad)=%16.13f\np0(rad)=%16.13f\nk0     =%16.4f\nX0  (m)=%16.4f\nY0  (m)=%16.4f\n\nlC(rad)=%16.13f\npC(rad)=%16.13f\nc      =%16.13f\nn1     =%16.13f\nn2 (m) =%16.4f\nXS (m) =%16.4f\nYS (m) =%16.4f\n", par.a, par.e, par.lam0, par.phi0, par.k0, par.x0, par.y0, proj_parm.lamc, proj_parm.phic, proj_parm.c, proj_parm.n1, proj_parm.n2, proj_parm.XS +par.x0, proj_parm.YS + par.y0);*/
            }

        }} // namespace detail::gstmerc
    #endif // doxygen

    /*!
        \brief Gauss-Schreiber Transverse Mercator (aka Gauss-Laborde Reunion) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin
         - lon_0: Central meridian
         - k_0: Scale factor
        \par Example
        \image html ex_gstmerc.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct gstmerc_spheroid : public detail::gstmerc::base_gstmerc_spheroid<Geographic, Cartesian, Parameters>
    {
        inline gstmerc_spheroid(const Parameters& par) : detail::gstmerc::base_gstmerc_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::gstmerc::setup_gstmerc(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class gstmerc_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gstmerc_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void gstmerc_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("gstmerc", new gstmerc_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP