/*
Copyright (c) 2013, Regents of the University of Alaska

All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    * 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.
    * Neither the name of the Geographic Information Network of Alaska 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 OWNER 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.

This code was developed by Dan Stahlke for the Geographic Information Network of Alaska.
*/

#include "common.h"

#include <boost/lexical_cast.hpp>

#include <vector>

using namespace dangdal;

struct ScaledBand {
	ScaledBand() :
		oversample(0), lo_w(0), lo_h(0), hi_w(0), hi_h(0),
		delta_x(0), delta_y(0), band(NULL), line_buf_idx(0)
	{ }

	int oversample;
	size_t lo_w, lo_h;
	size_t hi_w, hi_h;
	int delta_x, delta_y;
	std::vector<std::vector<double> > kernel_x;
	std::vector<std::vector<double> > kernel_y;
	GDALRasterBandH band;
	std::vector<std::vector<double> > lines_buf;
	int line_buf_idx;
};

void copyGeoCode(GDALDatasetH dst_ds, GDALDatasetH src_ds);
ScaledBand getScaledBand(GDALDatasetH lores_ds, int band_id, GDALDatasetH hires_ds);
void readLineScaled(ScaledBand &sb, int row, double *hires_buf);
double avoidNDV(double in, double ndv, GDALDataType out_dt);

void usage(const std::string &cmdname) {
	printf("Usage:\n %s\n", cmdname.c_str());
	printf(
"      -rgb <src_rgb.tif> [ -rgb <src.tif> ... ]\n"
"      [ -lum <lum.tif> <weight> ... ] -pan <pan.tif>\n"
"      [ -ndv <nodataval> ] -o <out-rgb.tif>\n"
"\nWhere:\n"
"    rgb.tif    Source bands that are to be enhanced\n"
"    lum.tif    Bands used to simulate lo-res pan band\n"
"    pan.tif    Hi-res panchromatic band\n"
"\nExamples, basic usage:\n"
"    gdal_landsat_pansharp -rgb quickbird_rgb.tif -pan quickbird_pan.tif -o out.tif\n"
"\nExamples, using simulated pan band (gives better results):\n"
"(coefficients from http://www.dpi.inpe.br/~leila/publications/boggione2003spb.pdf)\n"
"    gdal_landsat_pansharp -rgb lansat321.tif -lum landsat234.tif 0.25 0.23 0.52 \\\n"
"      -pan landsat8.tif -ndv 0 -o out.tif\n\n"
"    gdal_landsat_pansharp -rgb landsat3.tif -rgb landsat2.tif -rgb landsat1.tif \\\n"
"      -lum landsat2.tif 0.25 -lum landsat3.tif 0.23 -lum landsat4.tif 0.52 \\\n"
"      -pan landsat8.tif -ndv 0 -o out.tif\n\n"
);
	exit(1);
}

int main(int argc, char *argv[]) {
	const std::string cmdname = argv[0];
	if(argc == 1) usage(cmdname);
	std::vector<std::string> arg_list = argv_to_list(argc, argv);

	std::vector<GDALDatasetH> rgb_ds;
	std::vector<GDALDatasetH> lum_ds;
	std::vector<double> lum_weights;

	std::string pan_fn;
	std::string dst_fn;
	std::string output_format;
	double ndv = 0;
	bool use_ndv = 0;

	GDALAllRegister();

	size_t argp = 1;
	while(argp < arg_list.size()) {
		const std::string &arg = arg_list[argp++];
		// FIXME - check duplicate values
		if(arg[0] == '-') {
			try {
				if(arg == "-ndv") {
					if(argp == arg_list.size()) usage(cmdname);
					ndv = boost::lexical_cast<double>(arg_list[argp++].c_str());
					use_ndv = true;
				}
				else if(arg == "-of" ) { if(argp == arg_list.size()) usage(cmdname); output_format = arg_list[argp++]; }
				else if(arg == "-o"  ) { if(argp == arg_list.size()) usage(cmdname); dst_fn = arg_list[argp++]; }
				else if(arg == "-pan") { if(argp == arg_list.size()) usage(cmdname); pan_fn = arg_list[argp++]; }
				else if(arg == "-rgb") {
					if(argp == arg_list.size()) usage(cmdname);
					std::string fn = arg_list[argp++];
					GDALDatasetH ds = GDALOpen(fn.c_str(), GA_ReadOnly);
					if(!ds) fatal_error("open failed");
					rgb_ds.push_back(ds);
				}
				else if(arg == "-lum") {
					if(argp == arg_list.size()) usage(cmdname);
					std::string fn = arg_list[argp++];
					GDALDatasetH ds = GDALOpen(fn.c_str(), GA_ReadOnly);
					if(!ds) fatal_error("open failed");
					lum_ds.push_back(ds);
					int nb = GDALGetRasterCount(ds);
					while(nb) {
						if(argp == arg_list.size()) usage(cmdname);
						double w = boost::lexical_cast<double>(arg_list[argp++]);
						lum_weights.push_back(w);
						nb--;
					}
				} else {
					usage(cmdname);
				}
			} catch(boost::bad_lexical_cast &e) {
				fatal_error("cannot parse number given on command line");
			}
		} else {
			usage(cmdname);
		}
	}

	if(pan_fn.empty() || dst_fn.empty()) usage(cmdname);

	if(output_format.empty()) output_format = "GTiff";

	//////// open source ////////

	GDALDatasetH pan_ds = GDALOpen(pan_fn.c_str(), GA_ReadOnly);
	if(!pan_ds) fatal_error("open failed");

	size_t w = GDALGetRasterXSize(pan_ds);
	size_t h = GDALGetRasterYSize(pan_ds);
	if(!w || !h) fatal_error("missing width/height");

	if(GDALGetRasterCount(pan_ds) != 1) fatal_error("Pan input must be only one band");

	GDALRasterBandH pan_band = GDALGetRasterBand(pan_ds, 1);

	// this will be updated via GDALDataTypeUnion as RGB bands are opened
	GDALDataType out_dt = GDALGetRasterDataType(pan_band);

	/////

	std::vector<ScaledBand> rgb_bands;
	for(size_t ds_idx=0; ds_idx<rgb_ds.size(); ds_idx++) {
		int nb = GDALGetRasterCount(rgb_ds[ds_idx]);
		for(int i=0; i<nb; i++) {
			rgb_bands.push_back(getScaledBand(rgb_ds[ds_idx], i+1, pan_ds));
			out_dt = GDALDataTypeUnion(out_dt, GDALGetRasterDataType(
				GDALGetRasterBand(rgb_ds[ds_idx], i+1)));
		}
	}
	size_t rgb_band_count = rgb_bands.size();
	if(!rgb_band_count) usage(cmdname);

	std::vector<ScaledBand> lum_bands;
	if(!lum_ds.empty()) {
		for(size_t ds_idx=0; ds_idx<lum_ds.size(); ds_idx++) {
			int nb = GDALGetRasterCount(lum_ds[ds_idx]);
			for(int i=0; i<nb; i++) {
				lum_bands.push_back(getScaledBand(lum_ds[ds_idx], i+1, pan_ds));
			}
		}
	} else {
		lum_bands = rgb_bands;
		lum_weights.assign(rgb_band_count, 1);
	}
	size_t lum_band_count = lum_bands.size();

	double lum_weight_total = 0;
	for(size_t i=0; i<lum_band_count; i++) {
		lum_weight_total += lum_weights[i];
	}
	//printf("lum weights:");
	for(size_t i=0; i<lum_band_count; i++) {
		lum_weights[i] /= lum_weight_total;
		//printf(" %lf", lum_weights[i]);
	}
	//printf("\n");

	//////// open output ////////

	printf("Output size is %zd x %zd x %zd\n", w, h, rgb_band_count);
	printf("Output datatype is %s\n", GDALGetDataTypeName(out_dt));

	GDALDriverH dst_driver = GDALGetDriverByName(output_format.c_str());
	if(!dst_driver) fatal_error("unrecognized output format (%s)", output_format.c_str());
	GDALDatasetH dst_ds = GDALCreate(dst_driver, dst_fn.c_str(), w, h, rgb_band_count, out_dt, NULL);
	if(!dst_ds) fatal_error("could not create output");
	copyGeoCode(dst_ds, pan_ds);

	std::vector<GDALRasterBandH> dst_bands;
	for(size_t i=0; i<rgb_band_count; i++) {
		dst_bands.push_back(GDALGetRasterBand(dst_ds, i+1));
		if(use_ndv) {
			GDALSetRasterNoDataValue(dst_bands[i], ndv);
		}
	}

	//////// process data ////////

	std::vector<std::vector<double> > lum_buf(lum_band_count);
	for(size_t band_idx=0; band_idx<lum_band_count; band_idx++) {
		lum_buf[band_idx].resize(w);
	}
	std::vector<double> pan_buf(w);
	std::vector<double> rgb_buf(w);
	std::vector<double> out_buf(w);
	std::vector<double> scale_buf(w);

	for(size_t row=0; row<h; row++) {
		GDALTermProgress((double)row/h, NULL, NULL);

		GDALRasterIO(pan_band, GF_Read, 0, row, w, 1, &pan_buf[0], w, 1, GDT_Float64, 0, 0);
		for(size_t band_idx=0; band_idx<lum_band_count; band_idx++) {
			readLineScaled(lum_bands[band_idx], row, &lum_buf[band_idx][0]);
		}

		for(size_t col=0; col<w; col++) {
			bool skip = 0;

			if(use_ndv) {
				if(pan_buf[col] == ndv) skip = 1;
				for(size_t band_idx=0; band_idx<lum_band_count; band_idx++) {
					if(lum_buf[band_idx][col] == ndv) {
						skip = 1;
					}
				}
			}

			if(skip) {
				scale_buf[col] = 1;
			} else {
				double lum_out = (double)pan_buf[col];
				double lum_in = 0;
				for(size_t i=0; i<lum_band_count; i++) {
					lum_in += lum_buf[i][col] * lum_weights[i];
				}

				scale_buf[col] = lum_in>0 ? lum_out/lum_in : 0;
			} // skip
		} // col

		for(size_t band_idx=0; band_idx<rgb_band_count; band_idx++) {
			readLineScaled(rgb_bands[band_idx], row, &rgb_buf[0]);

			for(size_t col=0; col<w; col++) {
				if(use_ndv && rgb_buf[col] == ndv) {
					out_buf[col] = ndv;
				} else {
					double dbl_val = rgb_buf[col] * scale_buf[col];

					if(use_ndv) {
						dbl_val = avoidNDV(dbl_val, ndv, out_dt);
					}

					out_buf[col] = dbl_val;
				}
			}

			GDALRasterIO(dst_bands[band_idx], GF_Write, 0, row, w, 1,
				&out_buf[0], w, 1, GDT_Float64, 0, 0);
		}
	} // row

	for(size_t i=0; i<rgb_ds.size(); i++) {
		GDALClose(rgb_ds[i]);
	}
	for(size_t i=0; i<lum_ds.size(); i++) {
		GDALClose(lum_ds[i]);
	}
	GDALClose(pan_ds);
	GDALClose(dst_ds);

	GDALTermProgress(1, NULL, NULL);

	return 0;
}

void copyGeoCode(GDALDatasetH dst_ds, GDALDatasetH src_ds) {
	double affine[6];
	if(GDALGetGeoTransform(src_ds, affine) == CE_None) {
		GDALSetGeoTransform(dst_ds, affine);
	}
	GDALSetProjection(dst_ds, GDALGetProjectionRef(src_ds));
}

ScaledBand getScaledBand(GDALDatasetH lores_ds, int band_id, GDALDatasetH hires_ds) {
	double lores_affine[6];
	double hires_affine[6];
	if(GDALGetGeoTransform(lores_ds, lores_affine) != CE_None) {
		fatal_error("cannot determine affine");
	}
	if(GDALGetGeoTransform(hires_ds, hires_affine) != CE_None) {
		fatal_error("cannot determine affine");
	}
	// FIXME - we could probably work it out, as long as all the inputs
	// are rotated the same way
	if(
		lores_affine[2] || lores_affine[4] ||
		hires_affine[2] || hires_affine[4]
	) fatal_error("input must not be rotated");
	// again, it could probably be made to work...
	if(
		lores_affine[1] != -lores_affine[5] ||
		hires_affine[1] != -hires_affine[5]
	) fatal_error("input must have square pixels");

	ScaledBand sb;

	double band_res = lores_affine[1];
	double base_res = hires_affine[1];
	double offset_x =  (hires_affine[0] - lores_affine[0]) / band_res;
	double offset_y = -(hires_affine[3] - lores_affine[3]) / band_res;
	double scale = band_res / base_res;
	sb.oversample = (int)round(scale);
	if(fabs(scale - (double)sb.oversample) > 1e+6)
		fatal_error("scales of input images must differ by an integer factor");

	sb.lo_w = GDALGetRasterXSize(lores_ds);
	sb.lo_h = GDALGetRasterYSize(lores_ds);
	if(!sb.lo_w || !sb.lo_h) fatal_error("missing width/height");

	sb.hi_w = GDALGetRasterXSize(hires_ds);
	sb.hi_h = GDALGetRasterYSize(hires_ds);
	if(!sb.hi_w || !sb.hi_h) fatal_error("missing width/height");

	//printf("%d: %f %f %d %dx%d\n", band_id,
	//	offset_x, offset_y, sb.oversample, sb.lo_w, sb.lo_h);

	sb.band = GDALGetRasterBand(lores_ds, band_id);

	sb.delta_x = (int)floor(offset_x);
	offset_x -= (double)sb.delta_x;
	sb.delta_y = (int)floor(offset_y);
	offset_y -= (double)sb.delta_y;

	// Cubic convolution resampling.  For more info see:
	// http://www.imgfsr.com/ResamplingCVPR.pdf
	sb.kernel_x.resize(sb.oversample);
	sb.kernel_y.resize(sb.oversample);
	for(int mod=0; mod<sb.oversample; mod++) {
		sb.kernel_x[mod].resize(4);
		sb.kernel_y[mod].resize(4);
		double t = offset_x + (double)mod / (double)sb.oversample;
		sb.kernel_x[mod][0] = -0.5*t*t*t + 1.0*t*t - 0.5*t;
		sb.kernel_x[mod][1] =  1.5*t*t*t - 2.5*t*t + 1;
		sb.kernel_x[mod][2] = -1.5*t*t*t + 2.0*t*t + 0.5*t;
		sb.kernel_x[mod][3] =  0.5*t*t*t - 0.5*t*t;
		t = offset_y + (double)mod / (double)sb.oversample;
		sb.kernel_y[mod][0] = -0.5*t*t*t + 1.0*t*t - 0.5*t;
		sb.kernel_y[mod][1] =  1.5*t*t*t - 2.5*t*t + 1;
		sb.kernel_y[mod][2] = -1.5*t*t*t + 2.0*t*t + 0.5*t;
		sb.kernel_y[mod][3] =  0.5*t*t*t - 0.5*t*t;
	}

	sb.lines_buf.resize(4);
	for(int j=0; j<4; j++) {
		sb.lines_buf[j].resize(sb.hi_w);
	}
	sb.line_buf_idx = -1000000;

	return sb;
}

void readLineScaled1D(ScaledBand &sb, int row, double *hires_buf) {
	std::vector<double> lores_buf(sb.lo_w);

	if(row < 0 || size_t(row) >= sb.lo_h) {
		for(size_t col=0; col<sb.hi_w; col++) {
			hires_buf[col] = 0;
		}
	} else {
		GDALRasterIO(sb.band, GF_Read, 0, row, sb.lo_w, 1, &lores_buf[0], sb.lo_w, 1, GDT_Float64, 0, 0);
		for(int mx=0; mx<sb.oversample; mx++) {
			double *kernel = &sb.kernel_x[mx][0];
			for(int x0=0; ; x0++) {
				size_t col = x0 * sb.oversample + mx;
				if(col >= sb.hi_w) break;
				double accum = 0;
				for(int i=0; i<4; i++) {
					int x = x0 - 1 + i + sb.delta_x;
					double v = (x<0 || size_t(x) >= sb.lo_w) ? 0 : lores_buf[x];
					accum += v * kernel[i];
				}
				hires_buf[col] = accum;
			}
		}
	}
}

void readLineScaled(ScaledBand &sb, int row, double *hires_buf) {
	int y0 = row / sb.oversample;
	int my = row % sb.oversample;
	double *kernel = &sb.kernel_y[my][0];

	int top_y = y0 - 1 + sb.delta_y;
	if(top_y == sb.line_buf_idx) {
		// no action
	} else if(top_y == sb.line_buf_idx+1) {
		std::vector<double> tmp;
		std::swap(tmp, sb.lines_buf[0]);
		for(int j=0; j<3; j++) {
			std::swap(sb.lines_buf[j], sb.lines_buf[j+1]);
		}
		std::swap(sb.lines_buf[3], tmp);
		readLineScaled1D(sb, top_y+3, &sb.lines_buf[3][0]);
		sb.line_buf_idx = top_y;
	} else {
		for(int j=0; j<4; j++) {
			readLineScaled1D(sb, top_y+j, &sb.lines_buf[j][0]);
		}
		sb.line_buf_idx = top_y;
	}

	for(size_t col=0; col<sb.hi_w; col++) {
		double accum = 0;
		for(int j=0; j<4; j++) {
			accum += sb.lines_buf[j][col] * kernel[j];
		}
		hires_buf[col] = accum;
	}
}

// Helper for avoidNDV, used when datatype is an integer type.
template <typename T>
double avoidNDV_int(double in, double ndv) {
	int64_t in_int = int64_t(round(in));
	int64_t ndv_int = int64_t(round(ndv));
	if(in_int != ndv_int) return in_int;

	int64_t valid_low  = std::numeric_limits<T>::min();
	int64_t valid_high = std::numeric_limits<T>::max();

	// Compute center of valid range.
	// NOTE: this addition doesn't overflow.
	int64_t mid = (valid_low+valid_high)/2;
	// Avoid NDV, by moving toward center of valid range.
	// This way, we don't fall off the edge.
	return (ndv < mid) ? in_int+1 : in_int-1;
}

// If in==ndv, then perturb the value to avoid NDV.
double avoidNDV(double in, double ndv, GDALDataType out_dt) {
	// First, clip to valid range.  This is the only way to know whether the pixel will end up
	// being NDV after being written.  This is probably not the fastest way to do it though...
	{
		// size of largest possible datatype (GDT_CFloat64), doubled (just in case).
		char tmp[32];
		GDALCopyWords(&in, GDT_Float64, 0, tmp, out_dt, 0, 1);
		GDALCopyWords(tmp, out_dt, 0, &in, GDT_Float64, 0, 1);
	}

	switch(out_dt) {
		case GDT_Byte:   return avoidNDV_int< uint8_t>(in, ndv);
		case GDT_UInt16: return avoidNDV_int<uint16_t>(in, ndv);
		case GDT_Int16:  return avoidNDV_int< int16_t>(in, ndv);
		case GDT_UInt32: return avoidNDV_int<uint32_t>(in, ndv);
		case GDT_Int32:  return avoidNDV_int< int32_t>(in, ndv);
		default:         return (in==ndv) ? in+1 : in;
	}
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "structures.h"
#include "transformations.h"
#include "sheaf_of_planes_observation_equation_jacobian.h"

struct Plane{
	double a;
	double b;
	double c;
	double d;
};

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -20.0;
float translate_x, translate_y = 0.0;

std::vector<Eigen::Affine3d> sheaf_of_planes;
std::pair<Eigen::Vector3d, Eigen::Vector3d> line;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){
	for(size_t i = 0; i < 10; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 90;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;

		sheaf_of_planes.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}

	line.first = Eigen::Vector3d(0.1,0.1,0.1);
	line.second = Eigen::Vector3d(0.1,0.1,1.1);

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("sheaf_of_planes");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	for(size_t i = 0 ; i < sheaf_of_planes.size(); i++){
		Eigen::Affine3d &m = sheaf_of_planes[i];

		glBegin(GL_LINES);
			glColor3f(m(0,2),m(1,2),m(2,2));
			for(float step = -1.0f; step <= 1.1f; step += 0.1f)
			{
				Eigen::Vector3d v1(-1, step, 0);
				Eigen::Vector3d v2( 1, step, 0);

				Eigen::Vector3d v1t = m * v1;
				Eigen::Vector3d v2t = m * v2;

				glVertex3f(v1t.x(), v1t.y(), v1t.z());
				glVertex3f(v2t.x(), v2t.y(), v2t.z());

				v1 = Eigen::Vector3d(step,-1, 0);
				v2 = Eigen::Vector3d(step, 1, 0);

				v1t = m * v1;
				v2t = m * v2;

				glVertex3f(v1t.x(), v1t.y(), v1t.z());
				glVertex3f(v2t.x(), v2t.y(), v2t.z());
			}
		glEnd();
	}

	glLineWidth(5);
	Eigen::Vector3d line_direction = line.second - line.first;
	glColor3f(0,1,0);
	glBegin(GL_LINES);
		glVertex3f(line.first.x() + line_direction.x() * 100.0, line.first.y() + line_direction.y() * 100.0, line.first.z() + line_direction.z() * 100.0);
		glVertex3f(line.first.x() - line_direction.x() * 100.0, line.first.y() - line_direction.y() * 100.0, line.first.z() - line_direction.z() * 100.0);
	glEnd();
	glLineWidth(1);

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < sheaf_of_planes.size(); i++){
				Plane plane;
				plane.a = sheaf_of_planes[i](0,2);
				plane.b = sheaf_of_planes[i](1,2);
				plane.c = sheaf_of_planes[i](2,2);
				plane.d = -plane.a * sheaf_of_planes[i](0,3) - plane.b * sheaf_of_planes[i](1,3) - plane.c * sheaf_of_planes[i](2,3);

				Eigen::Matrix<double, 4, 1> residual;
				sheaf_of_planes_observation_equation(residual, line.first.x(), line.first.y(), line.first.z(), line.second.x(), line.second.y(), line.second.z(), plane.a, plane.b, plane.c, plane.d);

				Eigen::Matrix<double, 4, 6> jacobian;
				sheaf_of_planes_observation_equation_jacobian(jacobian, line.first.x(), line.first.y(), line.first.z(), line.second.x(), line.second.y(), line.second.z(), plane.a, plane.b, plane.c, plane.d);

				int ir = tripletListB.size();

				for(size_t j = 0 ; j < 4; j ++){
					for(size_t k = 0 ; k < 6; k ++){
						tripletListA.emplace_back(ir + j, k, -jacobian(j,k));
					}
				}

				tripletListP.emplace_back(ir    , ir    ,  1);
				tripletListP.emplace_back(ir + 1, ir + 1,  1);
				tripletListP.emplace_back(ir + 2, ir + 2,  1000000);
				tripletListP.emplace_back(ir + 3, ir + 3,  1);

				tripletListB.emplace_back(ir    , 0,  residual(0,0));
				tripletListB.emplace_back(ir + 1, 0,  residual(1,0));
				tripletListB.emplace_back(ir + 2, 0,  residual(2,0));
				tripletListB.emplace_back(ir + 3, 0,  residual(3,0));
			}

			Eigen::Matrix<double, 1, 1> residual;
			Eigen::Matrix<double, 1, 6> jacobian;
			line_direction_norm_observation_equation(residual, line.first.x(), line.first.y(), line.first.z(), line.second.x(), line.second.y(), line.second.z());
			line_direction_observation_equation_jacobian(jacobian, line.first.x(), line.first.y(), line.first.z(), line.second.x(), line.second.y(), line.second.z());

			int ir = tripletListB.size();

			for(size_t k = 0 ; k < 6; k ++){
				tripletListA.emplace_back(ir, k, -jacobian(0,k));
			}
			tripletListP.emplace_back(ir    , ir    ,  1000000);
			tripletListB.emplace_back(ir    , 0,  residual(0,0));

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(6, 6);
			Eigen::SparseMatrix<double> AtPB(6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					//std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;
			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6){
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				line.first.x() += h_x[0];
				line.first.y() += h_x[1];
				line.first.z() += h_x[2];
				line.second.x() += h_x[3];
				line.second.y() += h_x[4];
				line.second.z() += h_x[5];

				std::cout << "line: " << line.first.x() <<  " " << line.first.y() << " " <<  line.first.z() << " " << line.second.x() << " " <<  line.second.y() << " " << line.second.z() << std::endl;

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}

			break;
		}
		case 'n':{
			TaitBryanPose pose;

			pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0;
			pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0;
			pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0;

			pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

			for(size_t i = 0; i < sheaf_of_planes.size(); i++){
				TaitBryanPose pose_omfika;

				pose_omfika.px = 0;
				pose_omfika.py = 0;
				pose_omfika.pz = 0;

				pose_omfika.om = ((float(rand()%1000000))/1000000.0f - 0.5) * .012;
				pose_omfika.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * .012;
				pose_omfika.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * .012;

				sheaf_of_planes[i] =  affine_matrix_from_pose_tait_bryan(pose_omfika) * affine_matrix_from_pose_tait_bryan(pose) * sheaf_of_planes[i] * affine_matrix_from_pose_tait_bryan(pose_omfika).inverse();
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: modify planes" << std::endl;
	std::cout << "t: optimize" << std::endl;
}


/*
 * GenericGraph2D.cpp
 *
 *   Created on: Nov 23, 2010
 *       Author: nikai
 *  Description: generic graph types used in partitioning
 */
#include <iostream>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>

#include <gtsam/base/DSFVector.h>

#include "GenericGraph.h"

using namespace std;

namespace gtsam { namespace partition {

  /**
   * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
   */
  list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
  {
    typedef pair<int, int> IntPair;
    typedef list<sharedGenericFactor2D> FactorList;
    typedef map<IntPair, FactorList::iterator> Connections;

    // create disjoin set forest
    DSFVector dsf(workspace.dsf, keys);

    FactorList factors(graph.begin(), graph.end());
    size_t nrFactors = factors.size();
    FactorList::iterator itEnd;
    workspace.prepareDictionary(keys);
    while (nrFactors) {
      Connections connections;
      bool succeed = false;
      itEnd = factors.end();
      list<FactorList::iterator> toErase;
      for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {

        // remove invalid factors
        GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
        if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
          toErase.push_back(itFactor);  nrFactors--; continue;
        }

        size_t label1 = dsf.find(key1.index);
        size_t label2 = dsf.find(key2.index);
        if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }

        // merge two trees if the connection is strong enough, otherwise cache it
        // an odometry factor always merges two islands
        if (key1.type == NODE_POSE_2D && key2.type  == NODE_POSE_2D) {
          toErase.push_back(itFactor); nrFactors--;
          dsf.merge(label1, label2);
          succeed = true;
          break;
        }

        // single landmark island only need one measurement
        if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
            (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
          toErase.push_back(itFactor); nrFactors--;
          dsf.merge(label1, label2);
          succeed = true;
          break;
        }

        // stack the current factor with the cached constraint
        IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
        Connections::iterator itCached = connections.find(labels);
        if (itCached == connections.end()) {
          connections.insert(make_pair(labels, itFactor));
          continue;
        } else {
          GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
          // if observe the same landmark, we can not merge, abandon the current factor
          if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
              (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
              (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
              (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
            toErase.push_back(itFactor); nrFactors--;
            continue;
          } else {
            toErase.push_back(itFactor); nrFactors--;
            toErase.push_back(itCached->second); nrFactors--;
            dsf.merge(label1, label2);
            connections.erase(itCached);
            succeed = true;
            break;
          }
        }
      }

      // erase unused factors
      for(const FactorList::iterator& it: toErase)
        factors.erase(it);

      if (!succeed) break;
    }

    list<vector<size_t> > islands;
    map<size_t, vector<size_t> > arrays = dsf.arrays();
    for(const auto& kv : arrays)
      islands.push_back(kv.second);
    return islands;
  }

  /* ************************************************************************* */
  void print(const GenericGraph2D& graph, const std::string name) {
    cout << name << endl;
    for(const sharedGenericFactor2D& factor_: graph)
      cout << factor_->key1.index << " " << factor_->key2.index << endl;
  }

  /* ************************************************************************* */
  void print(const GenericGraph3D& graph, const std::string name) {
    cout << name << endl;
    for(const sharedGenericFactor3D& factor_: graph)
      cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
      factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
  }

  /* ************************************************************************* */
  // create disjoin set forest
  DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
    DSFVector dsf(workspace.dsf, keys);
    typedef list<sharedGenericFactor3D> FactorList;

    FactorList factors(graph.begin(), graph.end());
    size_t nrFactors = factors.size();
    FactorList::iterator itEnd;
    while (nrFactors) {

      bool succeed = false;
      itEnd = factors.end();
      list<FactorList::iterator> toErase;
      for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {

        // remove invalid factors
        if (graph.size() == 178765) cout << "kai21" <<  endl;
        GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
        if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
        if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
          toErase.push_back(itFactor);  nrFactors--; continue;
        }

        if (graph.size() == 178765) cout << "kai22" << endl;
        size_t label1 = dsf.find(key1.index);
        size_t label2 = dsf.find(key2.index);
        if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }

        if (graph.size() == 178765) cout << "kai23" << endl;
        // merge two trees if the connection is strong enough, otherwise cache it
        // an odometry factor always merges two islands
        if ((key1.type == NODE_POSE_3D && key2.type  == NODE_LANDMARK_3D) ||
            (key1.type == NODE_POSE_3D && key2.type  == NODE_POSE_3D)) {
          toErase.push_back(itFactor); nrFactors--;
          dsf.merge(label1, label2);
          succeed = true;
          break;
        }

        if (graph.size() == 178765) cout << "kai24" << endl;

      }

      // erase unused factors
      for(const FactorList::iterator& it: toErase)
      factors.erase(it);

      if (!succeed) break;
    }
    return dsf;
  }

  /* ************************************************************************* */
  // first check the type of the key (pose or landmark), and then check whether it is singular
  inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
    switch(node.type) {
    case NODE_POSE_3D:
      return singularCameras.find(node.index) != singularCameras.end(); break;
    case NODE_LANDMARK_3D:
      return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
    default:
      throw runtime_error("unrecognized key type!");
    }
  }

  /* ************************************************************************* */
  void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
      const vector<bool>& isCamera, const vector<bool>& isLandmark,
      set<size_t>& singularCameras, set<size_t>& singularLandmarks,  vector<int>& nrConstraints,
      bool& foundSingularCamera, bool& foundSingularLandmark,
      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {

    // compute the constraint number per camera
    std::fill(nrConstraints.begin(),  nrConstraints.end(),    0);
    for(const sharedGenericFactor3D& factor_: graph) {
      const int& key1 = factor_->key1.index;
      const int& key2 = factor_->key2.index;
      if (workspace.dictionary[key1] != -1 &&  workspace.dictionary[key2] != -1 &&
          !isSingular(singularCameras, singularLandmarks, factor_->key1) &&
          !isSingular(singularCameras, singularLandmarks, factor_->key2)) {
        nrConstraints[key1]++;
        nrConstraints[key2]++;

        // a single pose constraint is sufficient for stereo, so we add 2 to the counter
        // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
        if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
          nrConstraints[key1]+=2;
          nrConstraints[key2]+=2;
        }
      }
    }

    // find singular cameras and landmarks
    foundSingularCamera = false;
    foundSingularLandmark = false;
    for (size_t i=0; i<nrConstraints.size(); i++) {
      if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
          singularCameras.find(i) == singularCameras.end()) {
        singularCameras.insert(i);
        foundSingularCamera = true;
      }
      if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
          singularLandmarks.find(i) == singularLandmarks.end()) {
        singularLandmarks.insert(i);
        foundSingularLandmark = true;
      }
    }
  }

  /* ************************************************************************* */
  list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
      const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {

    // create disjoint set forest
    workspace.prepareDictionary(keys);
    DSFVector dsf = createDSF(graph, keys, workspace);

    const bool verbose = false;
    bool foundSingularCamera = true;
    bool foundSingularLandmark = true;

    list<vector<size_t> > islands;
    set<size_t> singularCameras, singularLandmarks;
    vector<bool> isCamera(workspace.dictionary.size(), false);
    vector<bool> isLandmark(workspace.dictionary.size(), false);

    // check the constraint number of every variable
    // find the camera and landmark keys
    for(const sharedGenericFactor3D& factor_: graph) {
      //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
      if (workspace.dictionary[factor_->key1.index] != -1) {
        if (factor_->key1.type == NODE_POSE_3D)
          isCamera[factor_->key1.index] = true;
        else
          isLandmark[factor_->key1.index] = true;
      }
            if (workspace.dictionary[factor_->key2.index] != -1) {
        if (factor_->key2.type == NODE_POSE_3D)
          isCamera[factor_->key2.index] = true;
        else
          isLandmark[factor_->key2.index] = true;
            }
    }

    vector<int> nrConstraints(workspace.dictionary.size(), 0);
    // iterate until all singular variables have been removed. Removing a singular variable
    // can cause another to become singular, so this will probably run several times
    while (foundSingularCamera || foundSingularLandmark) {
      findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark,      // input
          singularCameras, singularLandmarks, nrConstraints,                    // output
          foundSingularCamera, foundSingularLandmark,                           // output
          minNrConstraintsPerCamera,  minNrConstraintsPerLandmark);             // input
    }

    // add singular variables directly as islands
    if (!singularCameras.empty()) {
      if (verbose) cout << "singular cameras:";
      for(const size_t i: singularCameras) {
        islands.push_back(vector<size_t>(1, i)); // <---------------------------
        if (verbose) cout << i << " ";
      }
      if (verbose) cout << endl;
    }
    if (!singularLandmarks.empty()) {
      if (verbose) cout << "singular landmarks:";
      for(const size_t i: singularLandmarks) {
        islands.push_back(vector<size_t>(1, i)); // <---------------------------
        if (verbose) cout << i << " ";
      }
      if (verbose) cout << endl;
    }

    // regenerating islands
    map<size_t, vector<size_t> > labelIslands = dsf.arrays();
    size_t label; vector<size_t> island;
    for(const auto& li: labelIslands) {
      tie(label, island) = li;
      vector<size_t> filteredIsland; // remove singular cameras from array
      filteredIsland.reserve(island.size());
      for(const size_t key: island) {
        if ((isCamera[key]   && singularCameras.find(key) == singularCameras.end()) ||        // not singular
            (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) ||    // not singular
            (!isCamera[key] && !isLandmark[key])) {   // the key is not involved in any factor, so the type is undertermined
          filteredIsland.push_back(key);
        }
      }
      islands.push_back(filteredIsland);
    }

    // sanity check
    size_t nrKeys = 0;
    for(const vector<size_t>& island: islands)
      nrKeys += island.size();
    if (nrKeys != keys.size())  {
      cout << nrKeys << " vs " << keys.size() << endl;
      throw runtime_error("findIslands: the number of keys is inconsistent!");
    }

    if (verbose) cout << "found " << islands.size() << " islands!" << endl;
    return islands;
  }

  /* ************************************************************************* */
  // return the number of intersection between two **sorted** landmark vectors
  inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
    size_t i1 = 0, i2 = 0;
    int nrCommonLandmarks = 0;
    while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
      if (landmarks1[i1] < landmarks2[i2])
        i1 ++;
      else if (landmarks1[i1] > landmarks2[i2])
        i2 ++;
      else {
        i1++; i2++;
        nrCommonLandmarks ++;
      }
    }
    return nrCommonLandmarks;
  }

  /* ************************************************************************* */
  void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
      const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph) {

    typedef size_t LandmarkKey;
    // get a mapping from each landmark to its connected cameras
    vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
    // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
    vector<int> cameraToCamera(dictionary.size(), -1);
    size_t key_i, key_j;
    for(const sharedGenericFactor3D& factor_: graph) {
      if (factor_->key1.type == NODE_POSE_3D) {
        if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
          cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
        }
        else { // odometry factor
          if (factor_->key1.index < factor_->key2.index) {
            key_i = factor_->key1.index;
            key_j = factor_->key2.index;
          } else {
            key_i = factor_->key2.index;
            key_j = factor_->key1.index;
          }
          cameraToCamera[key_i] = key_j;
        }
      }
    }

    // sort the landmark keys for the late getNrCommonLandmarks call
    for(vector<LandmarkKey> &landmarks: cameraToLandmarks){
      if (!landmarks.empty())
        std::sort(landmarks.begin(), landmarks.end());
    }

    // generate the reduced graph
    reducedGraph.clear();
    int factorIndex = 0;
    int camera1, camera2, nrTotalConstraints;
    bool hasOdometry;
    for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
      for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
        camera1 = cameraKeys[i1];
        camera2 = cameraKeys[i2];
        int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
        hasOdometry =  cameraToCamera[camera1] == camera2;
        if (nrCommonLandmarks > 0 || hasOdometry) {
          nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
          reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
              factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
        }
      }
    }
  }

  /* ************************************************************************* */
  void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
      WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
    workspace.prepareDictionary(frontals);
    vector<size_t> nrConstraints(workspace.dictionary.size(), 0);

    // summarize the constraint number
    const vector<int>& dictionary = workspace.dictionary;
    vector<bool> isValidCamera(workspace.dictionary.size(), false);
    vector<bool> isValidLandmark(workspace.dictionary.size(), false);
    for(const sharedGenericFactor3D& factor_: graph) {
      assert(factor_->key1.type == NODE_POSE_3D);
      //assert(factor_->key2.type == NODE_LANDMARK_3D);
      const size_t& key1 = factor_->key1.index;
      const size_t& key2 = factor_->key2.index;
      if (dictionary[key1] == -1 || dictionary[key2] == -1)
        continue;

      isValidCamera[key1] = true;
      if(factor_->key2.type == NODE_LANDMARK_3D)
        isValidLandmark[key2] = true;
      else
        isValidCamera[key2] = true;

      nrConstraints[key1]++;
      nrConstraints[key2]++;

      // a single pose constraint is sufficient for stereo, so we add 2 to the counter
      // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
      if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
        nrConstraints[key1]+=2;
        nrConstraints[key2]+=2;
      }
    }

    // find the minimum constraint for cameras and landmarks
    size_t minFoundConstraintsPerCamera = 10000;
    size_t minFoundConstraintsPerLandmark = 10000;

    for (size_t i=0; i<isValidCamera.size(); i++) {
      if (isValidCamera[i]) {
        minFoundConstraintsPerCamera   = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
        if (nrConstraints[i] < minNrConstraintsPerCamera)
              cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
      }

    }
    for (size_t j=0; j<isValidLandmark.size(); j++) {
      if (isValidLandmark[j]) {
        minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
        if (nrConstraints[j] < minNrConstraintsPerLandmark)
          cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
      }
    }

    // debug info
    for(const size_t key: frontals) {
      if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
        cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
    }

     if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
      throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
    if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
      throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
  }

}} // namespace


//=============================================================================================================
/**
 * @file     hpifit.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>;
 *           Ruben Dörfel <ruben.doerfel@tu-ilmenau.de>;
 *           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
 * @since    0.1.0
 * @date     March, 2017
 *
 * @section  LICENSE
 *
 * Copyright (C) 2017, Lorenz Esch, Matti Hamalainen. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * 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.
 *     * Neither the name of MNE-CPP authors 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 OWNER 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.
 *
 *
 * @brief    HPIFit class defintion.
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "hpifit.h"
#include "hpifitdata.h"

#include <utils/ioutils.h>
#include <utils/mnemath.h>

#include <iostream>
#include <fiff/fiff_cov.h>
#include <fiff/fiff_dig_point_set.h>
#include <fstream>

#include <fwd/fwd_coil_set.h>

//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================

#include <Eigen/Dense>

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QFuture>
#include <QtConcurrent/QtConcurrent>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace INVERSELIB;
using namespace FIFFLIB;
using namespace FWDLIB;

//=============================================================================================================
// DEFINE GLOBAL METHODS
//=============================================================================================================

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

HPIFit::HPIFit(FiffInfo::SPtr pFiffInfo,
               bool bDoFastFit)
    : m_bDoFastFit(bDoFastFit)
{
    // init member variables
    m_lChannels = QList<FIFFLIB::FiffChInfo>();
    m_vecInnerind = QVector<int>();
    m_sensors = SensorSet();
    m_lBads = pFiffInfo->bads;
    m_matModel = MatrixXd(0,0);
    m_vecFreqs = QVector<int>();

    updateChannels(pFiffInfo);
    updateSensor();
}

//=============================================================================================================

void HPIFit::fitHPI(const MatrixXd& t_mat,
                    const MatrixXd& t_matProjectors,
                    FiffCoordTrans& transDevHead,
                    const QVector<int>& vecFreqs,
                    QVector<double>& vecError,
                    VectorXd& vecGoF,
                    FiffDigPointSet& fittedPointSet,
                    FiffInfo::SPtr pFiffInfo,
                    bool bDoDebug,
                    const QString& sHPIResourceDir,
                    int iMaxIterations,
                    float fAbortError)
{
    //Check if data was passed
    if(t_mat.rows() == 0 || t_mat.cols() == 0 ) {
        std::cout<<std::endl<< "HPIFit::fitHPI - No data passed. Returning.";
        return;
    }
    //Check if projector was passed
    if(t_matProjectors.rows() == 0 || t_matProjectors.cols() == 0 ) {
        std::cout<<std::endl<< "HPIFit::fitHPI - No projector passed. Returning.";
        return;
    }

    bool bUpdateModel = false;

    // check if bads have changed and update coils/channellist if so
    if(!(m_lBads == pFiffInfo->bads) || m_lChannels.isEmpty()) {
        m_lBads = pFiffInfo->bads;
        updateChannels(pFiffInfo);
        updateSensor();
        bUpdateModel = true;
    }

    if(m_lChannels.isEmpty()) {
        qWarning() << "HPIFit::fitHPI - Channel list is empty. Returning.";
        return;
    }

    // check if we have to update the model
    if(bUpdateModel || (m_matModel.rows() == 0) || (m_vecFreqs != vecFreqs) || (t_mat.cols() != m_matModel.cols())) {
        updateModel(pFiffInfo->sfreq, t_mat.cols(), pFiffInfo->linefreq, vecFreqs);
        m_vecFreqs = vecFreqs;
        bUpdateModel = false;
    }

    // Make sure the fitted digitzers are empty
    fittedPointSet.clear();

    // init coil parameters
    struct CoilParam coil;

    //Get HPI coils from digitizers and set number of coils
    int iNumCoils = 0;
    QList<FiffDigPoint> lHPIPoints;

    for(int i = 0; i < pFiffInfo->dig.size(); ++i) {
        if(pFiffInfo->dig[i].kind == FIFFV_POINT_HPI) {
            iNumCoils++;
            lHPIPoints.append(pFiffInfo->dig[i]);
        }
    }

    //Set coil frequencies
    VectorXd vecCoilfreq(iNumCoils);

    if(vecFreqs.size() >= iNumCoils) {
        for(int i = 0; i < iNumCoils; ++i) {
            vecCoilfreq[i] = vecFreqs.at(i);
            //std::cout<<std::endl << vecCoilfreq[i] << "Hz";
        }
    } else {
        std::cout<<std::endl<< "HPIFit::fitHPI - Not enough coil frequencies specified. Returning.";
        return;
    }

    // Initialize HPI coils location and moment
    coil.pos = MatrixXd::Zero(iNumCoils,3);
    coil.mom = MatrixXd::Zero(iNumCoils,3);
    coil.dpfiterror = VectorXd::Zero(iNumCoils);
    coil.dpfitnumitr = VectorXd::Zero(iNumCoils);

    // Create digitized HPI coil position matrix
    MatrixXd matHeadHPI(iNumCoils,3);

    // check the pFiffInfo->dig information. If dig is empty, set the matHeadHPI is 0;
    if (lHPIPoints.size() > 0) {
        for (int i = 0; i < lHPIPoints.size(); ++i) {
            matHeadHPI(i,0) = lHPIPoints.at(i).r[0];
            matHeadHPI(i,1) = lHPIPoints.at(i).r[1];
            matHeadHPI(i,2) = lHPIPoints.at(i).r[2];
        }
    } else {
        matHeadHPI.fill(0);
    }

    //Create new projector based on the excluded channels, first exclude the rows then the columns
    MatrixXd matProjectorsRows(m_vecInnerind.size(),t_matProjectors.cols());
    MatrixXd matProjectorsInnerind(m_vecInnerind.size(),m_vecInnerind.size());

    for (int i = 0; i < matProjectorsRows.rows(); ++i) {
        matProjectorsRows.row(i) = t_matProjectors.row(m_vecInnerind.at(i));
    }

    for (int i = 0; i < matProjectorsInnerind.cols(); ++i) {
        matProjectorsInnerind.col(i) = matProjectorsRows.col(m_vecInnerind.at(i));
    }

    // Get the data from inner layer channels
    MatrixXd matInnerdata(m_vecInnerind.size(), t_mat.cols());

    for(int j = 0; j < m_vecInnerind.size(); ++j) {
        matInnerdata.row(j) << t_mat.row(m_vecInnerind[j]);
    }

    // Calculate topo
    MatrixXd matTopo;
    MatrixXd matAmp(m_vecInnerind.size(), iNumCoils);
    MatrixXd matAmpC(m_vecInnerind.size(), iNumCoils);

    matTopo = m_matModel * matInnerdata.transpose(); // topo: # of good inner channel x 8

    if(m_bDoFastFit) {
        // Select sine or cosine component depending on the relative size
        matTopo.transposeInPlace();
        matAmp = matTopo.leftCols(iNumCoils);
        matAmpC = matTopo.rightCols(iNumCoils);
        for(int j = 0; j < iNumCoils; ++j) {
           float fNS = 0.0;
           float fNC = 0.0;
           fNS = matAmp.col(j).array().square().sum();
           fNC = matAmpC.col(j).array().square().sum();
           if(fNC > fNS) {
               matAmp.col(j) = matAmpC.col(j);
           }
        }
    } else {
        // estimate the sinusoid phase
        for(int i = 0; i < iNumCoils; ++i) {
            int from = 2*i;
            MatrixXd m = matTopo.block(from,0,2,matTopo.cols());
            JacobiSVD<MatrixXd> svd(m, ComputeThinU | ComputeThinV);
            matAmp.col(i) = svd.singularValues()(0) * svd.matrixV().col(0);
        }
    }

    //Find good seed point/starting point for the coil position in 3D space
    //Find biggest amplitude per pickup coil (sensor) and store corresponding sensor channel index
    VectorXi vecChIdcs(iNumCoils);

    for (int j = 0; j < iNumCoils; j++) {
        int iChIdx = 0;
        VectorXd::Index indMax;
        matAmp.col(j).maxCoeff(&indMax);
        if(indMax < m_vecInnerind.size()) {
            iChIdx = m_vecInnerind.at(indMax);
        }
        vecChIdcs(j) = iChIdx;
    }

    vecError.resize(iNumCoils);
    double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();
    MatrixXd matCoilPos = MatrixXd::Zero(iNumCoils,3);

    // Generate seed point by projection the found channel position 3cm inwards if previous transDevHead is identity or bad fit
    if(transDevHead.trans == MatrixXd::Identity(4,4).cast<float>() || dError > 0.010) {
        for (int j = 0; j < vecChIdcs.rows(); ++j) {
            if(vecChIdcs(j) < pFiffInfo->chs.size()) {
                Vector3f r0 = pFiffInfo->chs.at(vecChIdcs(j)).chpos.r0;
                matCoilPos.row(j) = (-1 * pFiffInfo->chs.at(vecChIdcs(j)).chpos.ez * 0.03 + r0).cast<double>();
            }
        }
    } else {
        matCoilPos = transDevHead.apply_inverse_trans(matHeadHPI.cast<float>()).cast<double>();
    }

    coil.pos = matCoilPos;

    // Perform actual localization
    coil = dipfit(coil,
                  m_sensors,
                  matAmp,
                  iNumCoils,
                  matProjectorsInnerind,
                  iMaxIterations,
                  fAbortError);

    Matrix4d matTrans = computeTransformation(matHeadHPI, coil.pos);
    transDevHead = FiffCoordTrans::make(1,4,matTrans.cast<float>(),true);

    //Calculate Error
    MatrixXd matTemp = coil.pos;
    MatrixXd matTestPos = transDevHead.apply_trans(matTemp.cast<float>()).cast<double>();
    MatrixXd matDiffPos = matTestPos - matHeadHPI;

    for(int i = 0; i < matDiffPos.rows(); ++i) {
        vecError[i] = matDiffPos.row(i).norm();
    }

    // store Goodness of Fit
    vecGoF = coil.dpfiterror;
    for(int i = 0; i < vecGoF.size(); ++i) {
        vecGoF(i) = 1 - vecGoF(i);
    }

    //Generate final fitted points and store in digitizer set
    for(int i = 0; i < coil.pos.rows(); ++i) {
        FiffDigPoint digPoint;
        digPoint.kind = FIFFV_POINT_EEG; //Store as EEG so they have a different color
        digPoint.ident = i;
        digPoint.r[0] = coil.pos(i,0);
        digPoint.r[1] = coil.pos(i,1);
        digPoint.r[2] = coil.pos(i,2);

        fittedPointSet << digPoint;
    }

    if(bDoDebug) {
        // DEBUG HPI fitting and write debug results
        std::cout << std::endl << std::endl << "HPIFit::fitHPI - dpfiterror" << coil.dpfiterror << std::endl << std::endl;
        std::cout << std::endl << std::endl << "HPIFit::fitHPI - Initial seed point for HPI coils" << std::endl << matCoilPos << std::endl;
        std::cout << std::endl << std::endl << "HPIFit::fitHPI - temp" << std::endl << matTemp << std::endl;
        std::cout << std::endl << std::endl << "HPIFit::fitHPI - testPos" << std::endl << matTestPos << std::endl;
        std::cout << std::endl << std::endl << "HPIFit::fitHPI - Diff fitted - original" << std::endl << matDiffPos << std::endl;
        std::cout << std::endl << std::endl << "HPIFit::fitHPI - dev/head trans" << std::endl << matTrans << std::endl;

        QString sTimeStamp = QDateTime::currentDateTime().toString("yyMMdd_hhmmss");

        if(!QDir(sHPIResourceDir).exists()) {
            QDir().mkdir(sHPIResourceDir);
        }

        UTILSLIB::IOUtils::write_eigen_matrix(matCoilPos, QString("%1/%2_coilPosSeed_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        UTILSLIB::IOUtils::write_eigen_matrix(vecGoF, QString("%1/%2_gof_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        UTILSLIB::IOUtils::write_eigen_matrix(coil.pos, QString("%1/%2_coilPos_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        UTILSLIB::IOUtils::write_eigen_matrix(matHeadHPI, QString("%1/%2_headHPI_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        MatrixXd testPosCut = matTestPos.transpose();//block(0,0,3,4);
        UTILSLIB::IOUtils::write_eigen_matrix(testPosCut, QString("%1/%2_testPos_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        MatrixXi matIdx(vecChIdcs.rows(),1);
        matIdx.col(0) = vecChIdcs;
        UTILSLIB::IOUtils::write_eigen_matrix(matIdx, QString("%1/%2_idx_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        MatrixXd matCoilFreq(vecCoilfreq.rows(),1);
        matCoilFreq.col(0) = vecCoilfreq;
        UTILSLIB::IOUtils::write_eigen_matrix(matCoilFreq, QString("%1/%2_coilFreq_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        UTILSLIB::IOUtils::write_eigen_matrix(matDiffPos, QString("%1/%2_diffPos_mat").arg(sHPIResourceDir).arg(sTimeStamp));

        UTILSLIB::IOUtils::write_eigen_matrix(matAmp, QString("%1/%2_amp_mat").arg(sHPIResourceDir).arg(sTimeStamp));
    }
}

//=============================================================================================================

void HPIFit::findOrder(const MatrixXd& t_mat,
                       const MatrixXd& t_matProjectors,
                       FiffCoordTrans& transDevHead,
                       QVector<int>& vecFreqs,
                       QVector<double>& vecError,
                       VectorXd& vecGoF,
                       FiffDigPointSet& fittedPointSet,
                       FiffInfo::SPtr pFiffInfo)
{
    // clear points
    fittedPointSet.clear();
    transDevHead.clear();
    vecError.fill(0);

    // casual fit to get fitted coils
    fitHPI(t_mat, t_matProjectors, transDevHead, vecFreqs, vecError, vecGoF, fittedPointSet, pFiffInfo);

    // extract digitized and fitted coils
    int iNumCoils = vecFreqs.length();
    QVector<int> vecToOrder = vecFreqs;
    VectorXd vecOrder(iNumCoils);
    Vector4d vecInit(0,1,2,3);
    MatrixXd matCoil = MatrixXd::Zero(iNumCoils,3);
    MatrixXd matDig = MatrixXd::Zero(iNumCoils,3);
    VectorXd vecDist = VectorXd::Zero(iNumCoils);
    QList<FiffDigPoint> lHPIPoints;

    for(int i = 0; i < iNumCoils; ++i) {
        matCoil(i,0) = fittedPointSet[i].r[0];
        matCoil(i,1) = fittedPointSet[i].r[1];
        matCoil(i,2) = fittedPointSet[i].r[2];
    }

    for(int i = 0; i < pFiffInfo->dig.size(); ++i) {
        if(pFiffInfo->dig[i].kind == FIFFV_POINT_HPI) {
            lHPIPoints.append(pFiffInfo->dig[i]);
        }
    }

    for (int i = 0; i < lHPIPoints.size(); ++i) {
        matDig(i,0) = lHPIPoints.at(i).r[0];
        matDig(i,1) = lHPIPoints.at(i).r[1];
        matDig(i,2) = lHPIPoints.at(i).r[2];
    }

    // Find closest point to each dig and find with that the order

    for(int i = 0; i < iNumCoils; i++) {
        // distances from of fitted point to all digitized
        vecDist = (matCoil.rowwise() - matDig.row(i)).rowwise().norm();
        Eigen::MatrixXf::Index min_index;
        vecDist.minCoeff(&min_index);
        vecOrder(i) = vecInit(min_index);
        vecToOrder[i] = vecFreqs[min_index];
    }

    // check if still all frequencies are represented and update model
    if(std::accumulate(vecFreqs.begin(), vecFreqs.end(), .0) ==  std::accumulate(vecToOrder.begin(), vecToOrder.end(), .0)) {
        vecFreqs = vecToOrder;
    } else {
        qWarning() << "HPIFit::findOrder: frequency ordering was not succesfull.";
    }
}

//=============================================================================================================

CoilParam HPIFit::dipfit(struct CoilParam coil,
                         const SensorSet& sensors,
                         const MatrixXd& matData,
                         int iNumCoils,
                         const MatrixXd& t_matProjectors,
                         int iMaxIterations,
                         float fAbortError)
{
    //Do this in conncurrent mode
    //Generate QList structure which can be handled by the QConcurrent framework
    QList<HPIFitData> lCoilData;

    for(qint32 i = 0; i < iNumCoils; ++i) {
        HPIFitData coilData;
        coilData.m_coilPos = coil.pos.row(i);
        coilData.m_sensorData = matData.col(i);
        coilData.m_sensors = sensors;
        coilData.m_matProjector = t_matProjectors;
        coilData.m_iMaxIterations = iMaxIterations;
        coilData.m_fAbortError = fAbortError;

        lCoilData.append(coilData);
    }
    //Do the concurrent filtering
    if(!lCoilData.isEmpty()) {
//        //Do sequential
//        for(int l = 0; l < lCoilData.size(); ++l) {
//            doDipfitConcurrent(lCoilData[l]);
//        }

        //Do concurrent
        QFuture<void> future = QtConcurrent::map(lCoilData,
                                                 &HPIFitData::doDipfitConcurrent);
        future.waitForFinished();

        //Transform results to final coil information
        for(qint32 i = 0; i < lCoilData.size(); ++i) {
            coil.pos.row(i) = lCoilData.at(i).m_coilPos;
            coil.mom = lCoilData.at(i).m_errorInfo.moment.transpose();
            coil.dpfiterror(i) = lCoilData.at(i).m_errorInfo.error;
            coil.dpfitnumitr(i) = lCoilData.at(i).m_errorInfo.numIterations;

            //std::cout<<std::endl<< "HPIFit::dipfit - Itr steps for coil " << i << " =" <<coil.dpfitnumitr(i);
        }
    }

    return coil;
}

//=============================================================================================================

Eigen::Matrix4d HPIFit::computeTransformation(Eigen::MatrixXd matNH, MatrixXd matBT)
{
    MatrixXd matXdiff, matYdiff, matZdiff, matC, matQ;
    Matrix4d matTransFinal = Matrix4d::Identity(4,4);
    Matrix4d matRot = Matrix4d::Zero(4,4);
    Matrix4d matTrans = Matrix4d::Identity(4,4);
    double dMeanX,dMeanY,dMeanZ,dNormf;

    for(int i = 0; i < 15; ++i) {
        // Calculate mean translation for all points -> centroid of both data sets
        matXdiff = matNH.col(0) - matBT.col(0);
        matYdiff = matNH.col(1) - matBT.col(1);
        matZdiff = matNH.col(2) - matBT.col(2);

        dMeanX = matXdiff.mean();
        dMeanY = matYdiff.mean();
        dMeanZ = matZdiff.mean();

        // Apply translation -> bring both data sets to the same center location
        for (int j = 0; j < matBT.rows(); ++j) {
            matBT(j,0) = matBT(j,0) + dMeanX;
            matBT(j,1) = matBT(j,1) + dMeanY;
            matBT(j,2) = matBT(j,2) + dMeanZ;
        }

        // Estimate rotation component
        matC = matBT.transpose() * matNH;

        JacobiSVD< MatrixXd > svd(matC ,Eigen::ComputeThinU | ComputeThinV);

        matQ = svd.matrixU() * svd.matrixV().transpose();

        //Handle special reflection case
        if(matQ.determinant() < 0) {
            matQ(0,2) = matQ(0,2) * -1;
            matQ(1,2) = matQ(1,2) * -1;
            matQ(2,2) = matQ(2,2) * -1;
        }

        // Apply rotation on translated points
        matBT = matBT * matQ;

        // Calculate GOF
        dNormf = (matNH.transpose()-matBT.transpose()).norm();

        // Store rotation part to transformation matrix
        matRot(3,3) = 1;
        for(int j = 0; j < 3; ++j) {
            for(int k = 0; k < 3; ++k) {
                matRot(j,k) = matQ(k,j);
            }
        }

        // Store translation part to transformation matrix
        matTrans(0,3) = dMeanX;
        matTrans(1,3) = dMeanY;
        matTrans(2,3) = dMeanZ;

        // Safe rotation and translation to final matrix for next iteration step
        // This step is safe to do since we change one of the input point sets (matBT)
        // ToDo: Replace this for loop with a least square solution process
        matTransFinal = matRot * matTrans * matTransFinal;
    }
    return matTransFinal;
}

//=============================================================================================================

void HPIFit::createSensorSet(SensorSet& sensors,
                             QSharedPointer<FWDLIB::FwdCoilSet> coils)
{
    int iNchan = coils->ncoil;

    // init sensor struct
    int iNp = coils->coils[0]->np;
    sensors.w = RowVectorXd(iNchan*iNp);
    sensors.r0 = MatrixXd(iNchan,3);
    sensors.cosmag = MatrixXd(iNchan*iNp,3);
    sensors.rmag = MatrixXd(iNchan*iNp,3);
    sensors.ncoils = iNchan;
    sensors.tra = MatrixXd::Identity(iNchan,iNchan);
    sensors.np = iNp;

    for(int i = 0; i < iNchan; i++){
        FwdCoil* coil = (coils->coils[i]);
        MatrixXd matRmag = MatrixXd::Zero(iNp,3);
        MatrixXd matCosmag = MatrixXd::Zero(iNp,3);
        RowVectorXd vecW(iNp);

        sensors.r0(i,0) = coil->r0[0];
        sensors.r0(i,1) = coil->r0[1];
        sensors.r0(i,2) = coil->r0[2];

        for (int p = 0; p < iNp; p++){
            sensors.w(i*iNp+p) = coil->w[p];
            for (int c = 0; c < 3; c++) {
                matRmag(p,c)   = coil->rmag[p][c];
                matCosmag(p,c) = coil->cosmag[p][c];
            }
        }

        sensors.cosmag.block(i*iNp,0,iNp,3) = matCosmag;
        sensors.rmag.block(i*iNp,0,iNp,3) = matRmag;
    }
}

//=============================================================================================================

void HPIFit::storeHeadPosition(float fTime,
                               const Eigen::MatrixXf& transDevHead,
                               Eigen::MatrixXd& matPosition,
                               const Eigen::VectorXd& vecGoF,
                               const QVector<double>& vecError)

{
    // Write quaternions and vecTime in position matrix. Format is the same like MaxFilter's .pos files.
    Matrix3f matRot = transDevHead.block(0,0,3,3);

    double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();     // HPI estimation Error
    Eigen::Quaternionf quatHPI(matRot);

//    qDebug() << "quatHPI.x() " << "quatHPI.y() " << "quatHPI.y() " << "trans x " << "trans y " << "trans z ";
//    qDebug() << quatHPI.x() << quatHPI.y() << quatHPI.z() << transDevHead(0,3) << transDevHead(1,3) << transDevHead(2,3);

    matPosition.conservativeResize(matPosition.rows()+1, 10);
    matPosition(matPosition.rows()-1,0) = fTime;
    matPosition(matPosition.rows()-1,1) = quatHPI.x();
    matPosition(matPosition.rows()-1,2) = quatHPI.y();
    matPosition(matPosition.rows()-1,3) = quatHPI.z();
    matPosition(matPosition.rows()-1,4) = transDevHead(0,3);
    matPosition(matPosition.rows()-1,5) = transDevHead(1,3);
    matPosition(matPosition.rows()-1,6) = transDevHead(2,3);
    matPosition(matPosition.rows()-1,7) = vecGoF.mean();
    matPosition(matPosition.rows()-1,8) = dError;
    matPosition(matPosition.rows()-1,9) = 0;
}

//=============================================================================================================

void HPIFit::updateSensor()
{
    // Create MEG-Coils and read data
    int iAcc = 2;
    int iNch = m_lChannels.size();

    if(iNch == 0) {
        return;
    }

    FiffCoordTransOld* t = NULL;

    if(m_pCoilTemplate.isNull()) {
        // read coil_def.dat
        QString qPath = QString(QCoreApplication::applicationDirPath() + "/resources/general/coilDefinitions/coil_def.dat");
        m_pCoilTemplate = QSharedPointer<FWDLIB::FwdCoilSet>(FwdCoilSet::read_coil_defs(qPath));
    }

    // create sensor set
    m_pCoilMeg = QSharedPointer<FWDLIB::FwdCoilSet>(m_pCoilTemplate->create_meg_coils(m_lChannels, iNch, iAcc, t));
    createSensorSet(m_sensors, m_pCoilMeg);
}

//=============================================================================================================

void HPIFit::updateChannels(QSharedPointer<FIFFLIB::FiffInfo> pFiffInfo)
{
    // Get the indices of inner layer channels and exclude bad channels and create channellist
    int iNumCh = pFiffInfo->nchan;

    for (int i = 0; i < iNumCh; ++i) {
        if(pFiffInfo->chs[i].chpos.coil_type == FIFFV_COIL_BABY_MAG ||
           pFiffInfo->chs[i].chpos.coil_type == FIFFV_COIL_VV_PLANAR_T1 ||
           pFiffInfo->chs[i].chpos.coil_type == FIFFV_COIL_VV_PLANAR_T2 ||
           pFiffInfo->chs[i].chpos.coil_type == FIFFV_COIL_VV_PLANAR_T3) {
            // Check if the sensor is bad, if not append to innerind
            if(!(pFiffInfo->bads.contains(pFiffInfo->ch_names.at(i)))) {
                m_vecInnerind.append(i);
                m_lChannels.append(pFiffInfo->chs[i]);
            }
        }
    }

    m_lBads = pFiffInfo->bads;
}

//=============================================================================================================

void HPIFit::updateModel(const int iSamF,
                         const int iSamLoc,
                         int iLineF,
                         const QVector<int>& vecFreqs)
{
    int iNumCoils = vecFreqs.size();
    MatrixXd matSimsig;
    VectorXd vecTime = VectorXd::LinSpaced(iSamLoc, 0, iSamLoc-1) *1.0/iSamF;

    if(m_bDoFastFit){
        // Generate simulated data Matrix
        matSimsig.conservativeResize(iSamLoc,iNumCoils*2);

        for(int i = 0; i < iNumCoils; ++i) {
            matSimsig.col(i) = sin(2*M_PI*vecFreqs[i]*vecTime.array());
            matSimsig.col(i+iNumCoils) = cos(2*M_PI*vecFreqs[i]*vecTime.array());
        }
        m_matModel = UTILSLIB::MNEMath::pinv(matSimsig);
        return;

    } else {
        // add linefreq + harmonics + DC part to model
        matSimsig.conservativeResize(iSamLoc,iNumCoils*4);
        for(int i = 0; i < iNumCoils; ++i) {
            matSimsig.col(i) = sin(2*M_PI*vecFreqs[i]*vecTime.array());
            matSimsig.col(i+iNumCoils) = cos(2*M_PI*vecFreqs[i]*vecTime.array());
            matSimsig.col(i+2*iNumCoils) = sin(2*M_PI*iLineF*i*vecTime.array());
            matSimsig.col(i+3*iNumCoils) = cos(2*M_PI*iLineF*i*vecTime.array());
        }
        matSimsig.col(14) = RowVectorXd::LinSpaced(iSamLoc, -0.5, 0.5);
        matSimsig.col(15).fill(1);
    }
    m_matModel = UTILSLIB::MNEMath::pinv(matSimsig);

    // reorder for faster computation
    MatrixXd matTemp = m_matModel;
    RowVectorXi vecIndex(2*iNumCoils);
    vecIndex << 0,4,1,5,2,6,3,7;
    for(int i = 0; i < vecIndex.size(); ++i) {
        matTemp.row(i) = m_matModel.row(vecIndex(i));
    }
    m_matModel = matTemp;
}


/***
第三章	 模板进阶知识
第一节 万能引用（universal reference / forwarding reference：转发引用）
（1）类型区别基本含义

（2）universal reference / 万能引用 / 未定义引用基本认识
结论：万能引用是一种类型。就跟int是一种类型是一个道理。再次强调，万能引用是一种类型。
右值引用（全称：右值引用类型）是用&&符号表示。右值引用是绑定到右值上。
万能引用离不开两种语境：
a)必须是函数模板
b)必须是发生了模板类型推断并且函数模板形参长这样：T&&
如果实参传递了一个整型左值给形参，tmprv的类型最终会被推断为int &类型。
如果果实参传递了一个整型右值给形参，tmprv的类型最终会被推断为int &&类型。
结论：T&&是一个万能引用类型。

题目:
void func(int&& param){...}   //右值引用，因为func不是函数模板而是一个普通函数。

template <typename T>
void func(T && tmpvalue){...} //是万能引用

template <typename T>
void func(std::vector<T>&& param) { ... } //右值引用

什么情形才是万能引用？
(a)一个是函数模板中用作函数参数的类型推断（参数中要涉及到类型推断），T&&
(b）auto &&tmpvalue = ..... 也是万能引用，这个后续再谈。
其他情况的&&，都是右值引用。

（3）万能引用资格的剥夺与辨认
（3.1）剥夺:const会剥夺 一个引用成为万能引用的资格，被打回原型成右值引用
（3.2）辨认
***/

#include <iostream>
#include <vector>

//#include <boost/type_index.hpp>
using namespace std;
//#pragma warning(disable : 4996)

namespace _nmsp1
{
	void  func(const int& abc) {}

	template <typename T>
	void  func(const T& abc) {} //T = int ,abc = const int&

	void myfunc(int&& tmprv)    //参数tmprv是个右值引用类型
	{
		cout << tmprv << endl;
		return;
	}

	/*
	template <typename T>
	void myfunc(T&& tmprv) //注意，&&是属于tmprv类型的一部分，不是T类型的一部分（&&和T类型没有关系）
	{
		tmprv = 12;        //不管tmprv的类型是左值引用还是右值引用，都可以给tmprv赋值。因为tmprv本身是个左值。
		cout << tmprv << endl;
		return;
	}
	*/

	template <typename T>
	void func(std::vector<T>&& param)
	{

	}
}

namespace _nmsp2
{

	template <typename T>
	void myfunc(T&& tmprv) //注意，&&是属于tmprv类型的一部分，不是T类型的一部分（&&和T类型没有关系）
	{
		tmprv = 12;        //不管tmprv的类型是左值引用还是右值引用，都可以给tmprv赋值。因为tmprv本身是个左值。
		cout << tmprv << endl;
		return;
	}

}

namespace _nmsp3
{
	template <typename T>
	void myfunc(const T&& tmprv)  //有const修饰，万能引用资格被剥夺，因为&&，所以只能是个右值引用
	{
		cout << tmprv << endl;
		return;
	}

	template <typename T>
	class mytestc
	{
	public:
		void testfunc(T&& x) {};   //这个不是万能引用，而是右值引用,因为 testfunc成员函数本身没有涉及到类型推断。

	public:
		template <typename T2>
		void testfunc2(T2&& x) {}; //x类型是万能引用类型

	};
}

int main()
{
	/*测试_nmsp1*/
	_nmsp1::func(10);
	int&& rv = 1000;

	_nmsp1::myfunc(10); //正确，右值做实参。
	int i1 = 100;        //i左值
	//_nmsp1::myfunc(i1);  //错，右值 引用不能接（绑）左值。

	std::vector<int> aa = { 1 };
	_nmsp1::func(std::move(aa));

	int i2 = 100;
	//_nmsp1::myfunc(i2);           //不可以，只能传递右值进去，必须是std::move(i);
	_nmsp2::myfunc(std::move(i2));

	/*测试_nmsp2*/
	int k1 = 100;
	_nmsp2::myfunc(k1);             //左值被传递，因此tmprv是个左值引用，也就是int&，最终i值变成12；
	printf("i2 = %d\n", k1);

	int k2 = 200;
	_nmsp2::myfunc(std::move(k2));  //右值被传递，因此tmprv是个右值引用，也就是int&&，最终i值变成12；
	printf("k2 = %d\n", k2);

    //测试_nmsp3
	_nmsp3::mytestc<int> mc;
	int j1 = 100;
	mc.testfunc(std::move(j1));  //左值不能绑定到右值引用上，必须修改为std::move(i);

	_nmsp3::mytestc<int> myoc;
	int j2 = 10;
	myoc.testfunc2(j2); //int &
	myoc.testfunc2(3);  //int &&

	return 0;
}