/**
 * Author	: Michael Fonder
 * Year		: 2016
 **/

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <unordered_map>
#include <unordered_set>
#include <set>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/opencv.hpp>
#include "opencv2/video/tracking.hpp"
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/eigen.hpp>

#include "dynamicVectorContainer.hpp"
#include "dynamicVectorReference.hpp"
#include "utils.hpp"
#include "imuState.hpp"
#include "MSCKF.hpp"

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

using namespace cv;
using namespace std;
using namespace Eigen;

#define Matrixfd Matrix<float, Eigen::Dynamic, Eigen::Dynamic>

MSCKF::MSCKF()
{
	imustate = new IMUstate();
	featureId_cnter = 150;
	maxNbrePoses = 24;
	maxNbreFeatures = 50;
	last_IMU_meas_time=0.0;

	camera.q_CI = Mat::zeros(4,1,CV_32FC1);

	// normal case orientation
	camera.q_CI.row(0) = 0.5;
	camera.q_CI.row(1) = -0.5;
	camera.q_CI.row(2) = 0.5;
	camera.q_CI.row(3) = -0.5;

	camProjMat = quatToProjMat(camera.q_CI);

	camera.p_CI = Mat::zeros(3,1,CV_32FC1);
	camera.p_CI = camProjMat*camera.p_CI;

	im_noise 			= 0.01;
	tracking_quality 	= 0.05;
	tracking_tresh 		= 0.55;
	posEst_tresh 		= 100;

}

void MSCKF::setCameraParams(const Mat &p_CI, const Mat &q_CI)
{
	q_CI.copyTo(camera.q_CI);
	camProjMat = quatToProjMat(camera.q_CI);
	camera.p_CI = camProjMat*p_CI;
}

void MSCKF::setFilterParams(const Mat &params)
{
	im_noise 			= params.at<float>(0,0);
	maxNbrePoses 		= params.at<float>(1,0);
	maxNbreFeatures 	= params.at<float>(2,0);
}

// Input == last measurement
void MSCKF::propagateIMUStateAndCovar(const Measurement &measurements, float timestamp)
{
	last_IMU_meas_time = timestamp;
	imustate->propagateState(measurements);
	Mat Phi = imustate->propagateCovar(measurements);
	if(cameraPoses.size() > 0)
		imuCamCovar = Phi*imuCamCovar;
}

/**
* Returns current position estimation
**/
Mat MSCKF::getPosition()
{
	return imustate->p_G;
}

/**
* Returns current attitude estimation
**/
Mat MSCKF::getOrientation()
{
	return imustate->q_IG;
}

/**
* Returns current filter covariance
**/
Mat MSCKF::getCovar()
{
	return imustate->covar;
}

void MSCKF::updateStateAndCovar()
{
	if(features_to_residualize.size() == 0)
		return;
}

// Augment state when new frame available
void MSCKF::augmentState(const Mat &frame, float timestamp)//const Feature &feature)
{
	Mat Jt;
	CameraPose cameraPose;
	imustate->getDelayedPose(cameraPose.pose, Jt, timestamp-last_IMU_meas_time);

	// store new IMU pose
	size_t N = cameraPoses.size();
	cameraPose.pose.p.copyTo(cameraPose.p_CG);
	cameraPose.pose.q.copyTo(cameraPose.q_CG);
	cameraPoses.push_back(cameraPose);

	addFrame(frame);

	if(points.size()==0) // If no features detected for current frame, remove current camera pose
		posesToPrune.insert(cameraPoses.last());

	// Build MSCKF covariance matrix for state augmentation
	size_t L = imustate->stateLength; // just for increased lisibility of the code
	Mat P = Mat::zeros(L+6*N, L+6*N, CV_32FC1);
	(imustate->covar).copyTo(P.rowRange(0,L).colRange(0,L));
	if(N>0)
	{
		camCovar.copyTo(P.rowRange(L, L+6*N).colRange(L,L+6*N));
		transpose(imuCamCovar, P.rowRange(L,L+6*N).colRange(0,L));
		imuCamCovar.copyTo(P.rowRange(0,L).colRange(L,L+6*N));
	}

	// Augment state
	Mat J = calcJ(N, Jt, cameraPose, timestamp-last_IMU_meas_time+imustate->delay);
	Mat p_aug = Mat::eye(6+L+6*N,L+6*N, CV_32FC1);
	J.copyTo(p_aug.rowRange(L+6*N,6+L+6*N).colRange(0,L+6*N));

	p_aug = p_aug*P*(p_aug.t());

	// assign result
	p_aug.rowRange(0,L)			.colRange(0,L)			.copyTo(imustate->covar);
	p_aug.rowRange(L,L+6+6*N)	.colRange(L,L+6+6*N)	.copyTo(camCovar);
	p_aug.rowRange(0,L)			.colRange(L, L+6+6*N)	.copyTo(imuCamCovar);

}

/**
* Extract and track features for a new incomming frame
* Params:
*	frame	: the new frame
* Returns the number of features tracked
**/
int MSCKF::addFrame(const Mat &frame)
{
	vector<uchar> status;
	vector<float> err;
	vector<float> err1;

	TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, tracking_quality);
	Size subPixWinSize(10,10), winSize(7,7);

	const int MAX_COUNT = 10;
	int discontinued = 0;

	if(cameraPoses.size()>1 && points.size()!= 0)
	{
		vector<Point2f> p1(points);
		vector<Point2f> p0r(points);

		// doc : http://docs.opencv.org/2.4/modules/video/doc/motion_analysis_and_object_tracking.html#calcopticalflowpyrlk
		calcOpticalFlowPyrLK(previous_frame, frame,points, p1, status, err, Size(39, 39), 4, termcrit, OPTFLOW_USE_INITIAL_FLOW);
		calcOpticalFlowPyrLK(frame, previous_frame,p1, p0r, status, err1, Size(39, 39), 4, termcrit, OPTFLOW_USE_INITIAL_FLOW);

		for(size_t i=0; i<points.size(); ++i)
		{
			if(norm(points[i]-p0r[i])>tracking_tresh)
			{
				status[i]= 0;
				discontinued++;
			}
			else
				points[i] = p1[i];
		}
	}

	int sx = frame.cols, sy= frame.rows;

	Mat mask = Mat::zeros(frame.rows, frame.cols,  CV_8UC1)*255;
	rectangle( mask, Point( sx*0.15, sy*0.15 ), Point( sx*0.85, sy*0.85), 255, -1, 8 );
	int size_circle = int(30*float(features.size()-discontinued)/float(maxNbreFeatures));
	for(size_t i=0; i<points.size(); ++i)
		circle(mask, points[i], size_circle, 0, -1);

	// Detect new features only if required
	if(features.size()-discontinued < maxNbreFeatures || float(countNonZero(mask))/float(sx*sy)>0.25)// && (cameraPoses.size()-1)%1 == 0)
	{

		imshow("mask", mask);

		Mat descriptors1;
		vector<KeyPoint> keypoints;
		BRISK extractor(48,3,1.5);
		extractor(frame, mask, keypoints, descriptors1);

		for(size_t i=0; i<keypoints.size(); ++i)
		{
			points.push_back(keypoints[i].pt);
			ids.push_back(featureId_cnter);
			status.push_back(1);
			Feature tmp;
			features.insert({featureId_cnter, tmp});
			++featureId_cnter;
		}
	}

	// Highlight tracked features in the image
	mask = frame;
	for(size_t i=0; i<points.size(); ++i)
		circle(mask, points[i], 3, 255, -1);

	imshow("frame", mask);
	waitKey(1);

	for(size_t i=0; i<points.size(); i++)
	{
		int x, y;
		x = points[i].x;
		y = points[i].y;

		// Discard features too close from the borders of the image
		if(status[i] == 1 && (x<0.15*sx || x>0.85*sx || y<0.15*sy || y>0.85*sy))
		{
			status[i] = 0;
		}
	}

	// Add point updates to data structure
	int longest = -1, max_size = 0;
	for(size_t i=0; i<points.size(); )
	{
		if(max_size<features[ids[i]].positions.size())
		{
			longest  = i;
			max_size = features[ids[i]].positions.size();
		}

		if(status[i] == 1)
		{
			Mat matPoint(1,1,CV_32FC2);
			float x = 160, y=120;
			matPoint = {points[i].x, points[i].y};
			undistortPoints(matPoint, matPoint,camera.intrisic, camera.distCoeffs);
			int test = ids[i];
			features[ids[i]].positions.push_back(matPoint.reshape(1,2));
			features[ids[i]].cameraPoses.push_back(cameraPoses.last());
			(cameraPoses.last())->featuresId.insert(ids[i]);
			++i;
		}
		else
		{
			features_to_residualize.insert(ids[i]);
			points.erase(points.begin()+i);
			ids.erase(ids.begin()+i);
			status.erase(status.begin()+i);
		}
	}

	if(features_to_residualize.size() < 1 && longest >= 0)
	{
		features_to_residualize.insert(ids[longest]);
		points.erase(points.begin()+longest);
		ids.erase(ids.begin()+longest);
		status.erase(status.begin()+longest);
	}

	frame.copyTo(previous_frame);
}

/**
* Performs the EKF update step
**/
void MSCKF::update()
{
	extractVariablesToPrune();

	// Exit function if nothing has to be done
	if(features_to_residualize.size() == 0 && posesToPrune.size()==0)
		return;

	// Assemble covariance matrix
	size_t N = imustate->stateLength + imuCamCovar.cols;
	Mat P = Mat::zeros(N,N,CV_32FC1);
	imustate->covar.copyTo(P.rowRange(0,imustate->stateLength).colRange(0, imustate->stateLength));
	imuCamCovar.copyTo(P.rowRange(0,imustate->stateLength).colRange(imustate->stateLength, N));
	transpose(imuCamCovar, P.rowRange(imustate->stateLength, N).colRange(0, imustate->stateLength));
	camCovar.copyTo(P.rowRange(imustate->stateLength, N).colRange(imustate->stateLength, N));

	Mat Ho, ro, Ro;

	int accepted_features = 0;

	for(std::unordered_set<int32_t>::iterator it = features_to_residualize.begin(); it != features_to_residualize.end(); it++)
	{
		Mat p_f_G, Hoj, Aj;
		int test = *it;
		Feature feature = features.find(*it)->second;

		if(feature.cameraPoses.size() < 3)
		{
			continue;
		}

		float d = calcGNPosEst(features[*it], p_f_G);
		if(d/float(features[*it].positions.size())>posEst_tresh)
		{
			continue;
		}

		calcHoj(p_f_G, feature, Hoj, Aj);

		//Compute residuals
		Mat rj(2*feature.positions.size(), 1, CV_32FC1); // NOTE : Previously : Mat rj = Mat::zeros(2*feature.positions.size(), 1, CV_32FC1);
		for(size_t i=0; i<feature.positions.size(); ++i)
		{
			Mat C_CG = camProjMat*quatToProjMat((feature.cameraPoses[i]).q_CG);
			Mat p_f_C = C_CG*(p_f_G-(feature.cameraPoses[i]).p_CG)-camera.p_CI;

			rj.at<float>(2*i,  0) = p_f_C.at<float>(0,0)/p_f_C.at<float>(2,0);
			rj.at<float>(2*i+1,0) = p_f_C.at<float>(1,0)/p_f_C.at<float>(2,0);
			rj.rowRange(2*i, 2*i+2) = feature.positions[i] - rj.rowRange(2*i, 2*i+2);
		}

		rj = Aj.t()*rj;

		// Mahanalobis gating test
		Mat Rj = Mat::eye(rj.rows, rj.rows, CV_32FC1)*im_noise;
		Mat S = Hoj*P*Hoj.t()+Rj;
		Mat MD;
		Mat tmp3;
		mySolve(S,rj,MD);
		MD = rj.t()*MD;
cout << "MD : " << MD <<chiSquared(rj.rows) << endl;
		if(isnan(abs(MD.at<float>(0,0))) || MD.at<float>(0,0) > chiSquared(rj.rows))
			continue;

		// Project matrices on the left nullspace
		Ho.push_back(Hoj);

		ro.push_back(rj);

		// Push Rj on the diag of Ro
		Mat tmp = Mat::zeros(Ro.rows+Rj.rows, Ro.cols+Rj.cols, CV_32FC1);
		Ro.copyTo(tmp.rowRange(0, Ro.rows).colRange(0, Ro.cols));// += Ro;
		Rj.copyTo(tmp.rowRange(Ro.rows, Ro.rows+Rj.rows).colRange(Ro.cols,Ro.cols+Rj.cols));// += Rj;
		tmp.copyTo(Ro);

		++accepted_features;
	}

	if(accepted_features > 0) //Do filter update only if there are some residuals to process
	{
		Mat Q1, Th;
		{	// Compute range space of Ho to speed up following computations
			MatrixXf eigenHo;
			cv2eigen(Ho, eigenHo);
			FullPivHouseholderQR<MatrixXf> qr(eigenHo);
			MatrixXf Q = qr.matrixQ();
			MatrixXf R = qr.matrixQR().triangularView<Eigen::Upper>();
			R = R*qr.colsPermutation().inverse();
			cout << "range space computed"<<endl;

			eigen2cv(Q, Q1);
			eigen2cv(R, Th);
		}

		// Speed boost by eliminating noise from the residuals
		for(size_t i=0; i<Th.rows; ++i)
		{
			size_t j=0;
			for(; j<Th.cols;++j)
				if(Th.at<float>(i,j)!=0)
					break;
			if(j==Th.cols)
			{
				Th.rowRange(0,i).copyTo(Th);
				Q1.colRange(0,i).copyTo(Q1);
				break;
			}
		}

		Ro = Q1.t()*Ro*Q1;
		Mat rn = Q1.t()*ro;
		rn.copyTo(ro);

		Mat tmp2 = Th*P*Th.t()+Ro;
		Mat K;
		{
			MatrixXf A, b;
			cv2eigen(tmp2.t(), A);
			cv2eigen((P*Th.t()).t(), b);

			// Choose your own solving method
// 				MatrixXf K2 = A.colPivHouseholderQr().solve(b);
			MatrixXf K2 = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
//				MatrixXf K2 = A.householderQr().solve(b);

			eigen2cv(K2, K);
		}
		transpose(K,K);

		// State correction term
		Mat deltaX = K*ro;
		cout << "DeltaX : " << endl << deltaX << endl;

		if(isnan(abs(deltaX.at<float>(0,0))) || isinf(abs(deltaX.at<float>(0,0))))
		{
			cout << "Warning ill conditionned matrices : State correction failed" << endl;
		}
		else
		{
			updateState(deltaX);

			// Covariance correction
			Mat tmp = Mat::eye(N,N, CV_32FC1) - K*Th;
			P = tmp*P*tmp.t() + K*Ro*K.t(); // P corrected

			P.rowRange(0,imustate->stateLength).colRange(0,imustate->stateLength)  .copyTo(imustate->covar);
			P.rowRange(0, imustate->stateLength).colRange(imustate->stateLength,N) .copyTo(imuCamCovar);
			P.rowRange(imustate->stateLength, N).colRange(imustate->stateLength, N).copyTo(camCovar);
		}
	}

	// State pruning
	pruneState();
}

/**
* Computes Jacobian used to augment the state
* Params:
*	N 			: Number of camera poses stored in the state;
*	Jt			: Jacobian of the time delay;
* 	cameraPose 	: camera pose;
* 	timeStep	: timestep between last received IMU measurements and current frame;
* Returns the Jacobian martix
**/
MatExpr MSCKF::calcJ(size_t N, const Mat &Jt, CameraPose &cameraPose, float timeStep)
{
	Mat rotMat = quatToProjMat(cameraPose.pose.q);
	Mat J = Mat::zeros(6, (imustate->stateLength)+6*N, CV_32FC1);
	J.rowRange(0,3).colRange(0,3) = Mat::eye(3,3,CV_32FC1);
	J.rowRange(3,6).colRange(12,15) = Mat::eye(3,3,CV_32FC1);
	J.rowRange(3,6).colRange(6,9) = Mat::eye(3,3,CV_32FC1)*timeStep;
	J.rowRange(0,6).colRange(15,16) = Jt;

	return J+0.0;
}

/**
* Computes Jacobian of a feature track for the state update
* Params:
*	p_f_G 	: feature position in the global frame;
*	feature	: feature track;
* 	Hoj 	: resulting matrix;
* 	Aj		: nullspace of Hfi
**/
void MSCKF::calcHoj(const Mat &p_f_G, const Feature &feature, Mat &Hoj, Mat &Aj) //const &feature?
{
	size_t IMU = imustate->stateLength; // length of IMU state
	size_t N = cameraPoses.size();
	size_t M = feature.positions.size();
	Mat Hfj = Mat::zeros(2*M, 3, CV_32FC1);
	Mat Hxj = Mat::zeros(2*M, IMU+6*N, CV_32FC1);

	for(size_t i=0; i<feature.positions.size(); i++)
	{
		int camPoseIndex = cameraPoses.getIndex(feature.cameraPoses.at(i));
		Mat p_CG = (feature.cameraPoses[i]).pose.p;
		Mat q_CG = (feature.cameraPoses[i]).pose.q;

		// Express feature position in the camera frame
		Mat p_f_C = camProjMat*quatToProjMat((feature.cameraPoses[i]).q_CG)*(p_f_G-(feature.cameraPoses[i]).p_CG)-camera.p_CI;
		Mat C_CG = camProjMat*quatToProjMat(q_CG);

		Mat Ji = Mat::zeros(2,3, CV_32FC1);
		Ji.at<float>(0,0) = 1;
		Ji.at<float>(1,1) = 1;
		Ji.at<float>(0,2) = -p_f_C.at<float>(0,0)/p_f_C.at<float>(2,0);
		Ji.at<float>(1,2) = -p_f_C.at<float>(1,0)/p_f_C.at<float>(2,0);
		Ji /= p_f_C.at<float>(2,0);

		Mat pos_diff = p_f_G-p_CG;
		Mat tmp = Mat::zeros(2,6, CV_32FC1);

		// Jacobian of the residual with respect to the feature position
		Hfj.rowRange(2*i, 2*i+2) = Ji*camProjMat*quatToProjMat((feature.cameraPoses[i]).q_CG);

		// Jacobian of the residual with respect to the IMU pose
		tmp.colRange(0, 3) = Hfj.rowRange(2*i, 2*i+2)*crossMat(pos_diff);//Ji*C_CG*crossMat(pos_diff);
		tmp.colRange(3, 6) = -Hfj.rowRange(2*i, 2*i+2);//Ji*C_CG;
		tmp.copyTo(Hxj.rowRange(2*i, 2*i+2).colRange(IMU+6*camPoseIndex, IMU+6*camPoseIndex+6));

		// Jacobian of the residual with respect to the time delay
		Hxj.rowRange(2*i, 2*i+2).colRange(15, 16) = -Ji*camProjMat*(crossMat((feature.cameraPoses[i]).pose.ang_vel).t())*quatToProjMat((feature.cameraPoses[i]).q_CG)*pos_diff-Ji*C_CG*(feature.cameraPoses[i]).pose.v;

		// Jacobian of the residual with respect to the camera pose
		Hxj.rowRange(2*i, 2*i+2).colRange(16, 19) = Ji*camProjMat*crossMat(quatToProjMat(q_CG)*pos_diff);
		Ji.copyTo(Hxj.rowRange(2*i, 2*i+2).colRange(19, 22));
	}

	// QR decomposition to have nullspace
	MatrixXf eigenHfj;
	cv2eigen(Hfj, eigenHfj);
	ColPivHouseholderQR<MatrixXf> qr(eigenHfj);
	MatrixXf Q = qr.matrixQ();
	Q = Q.rightCols(eigenHfj.rows()-eigenHfj.cols());
	eigen2cv(Q, Aj);

	// projection on the nullspace of the Jacobian of the residual with respect to the feature position
	Hoj = Aj.t()*Hxj;
}

/**
* Builds camera matrix from camera orientation and location
* Params:
*	C 	: rotation matrix;
*	t 	: translation matrix;
* Returns camera matrix
**/
Mat MSCKF::buildCameraMatrix(const Mat &C, const Mat &t)
{
	Mat matrix = Mat::zeros(3,4, CV_32FC1);
	C.copyTo(matrix.colRange(0,3));
	t.copyTo(matrix.col(3));
	return matrix+0.0;
}

/**
* Solves the AX=b linear system
* Params:
*	A 	: A matrix;
*	b 	: b matrix;
* 	res : matrix to store the result;
**/
void MSCKF::mySolve(const Mat &A, const Mat &b, Mat &res)
{
	Matrixfd eigA, eigb;
	cv2eigen(A, eigA);
	cv2eigen(b, eigb);
	Matrixfd eigRes = eigA.colPivHouseholderQr().solve(eigb);
	eigen2cv(eigRes, res);
}

/**
* Performs Gauss-Newton regression on a set of points corresponding to a same feature to estimate its 3d location
* Params:
*	feature : the feature whose position needs to be estimated;
*	pos 	: a matrix to store the result;
**/
float MSCKF::calcGNPosEst(const Feature &feature, Mat &pos)
{
	pos = Mat::zeros(3,1, CV_32FC1);
	Mat posCop;
	Mat p_IC = camProjMat.t()*camera.p_CI;

	{	// get estimate of feature position by triangulation for state initialisation
		Mat point_4d;
		Mat C1 = camProjMat*quatToProjMat(feature.cameraPoses.front().q_CG);
		Mat C2 = camProjMat*quatToProjMat(feature.cameraPoses.back().q_CG);
		Mat t1 = C1*feature.cameraPoses.front().p_CG+camera.p_CI;
		Mat t2 = C2*feature.cameraPoses.back().p_CG+camera.p_CI;

		triangulatePoints( buildCameraMatrix(C1, -t1), buildCameraMatrix(C2, -t2),
						feature.positions.front(), feature.positions.back(), point_4d);

		for(size_t i=0; i<3; ++i)
			pos.at<float>(i,0) = point_4d.at<float>(i,0)/point_4d.at<float>(3,0);

		pos = C1*(pos)-t1;
	}

	//Inverse depth parametrisation
	Mat xEst = Mat::ones(3,1, CV_32FC1);
	xEst.at<float>(0,0) = pos.at<float>(0,0)/pos.at<float>(2,0); //alphaBar
	xEst.at<float>(1,0) = pos.at<float>(1,0)/pos.at<float>(2,0); //betaBar
	xEst.at<float>(2,0) /= pos.at<float>(2,0);			 //rhoBar

	size_t n = feature.cameraPoses.size();
	int maxIter = 10;

	Mat prov;
	CameraPose cp_1;
	prov = quatLeftComp(camera.q_CI)*feature.cameraPoses.front().q_CG;
	prov = prov/norm(prov);
	prov.copyTo(cp_1.q_CG);
	prov = feature.cameraPoses.front().p_CG + quatToProjMat(cp_1.q_CG).t()*camera.p_CI;
	prov.copyTo(cp_1.p_CG);

	Mat sqrErr_prev = Mat::zeros(1,1,CV_32FC1);
	Mat sqrErr;
	for(size_t iter=0; iter<maxIter; iter++)
	{
		Mat E = Mat::zeros(2*n, 3, CV_32FC1);
		Mat errorVec = Mat::zeros(2*n,1, CV_32FC1);

		for(size_t i=0; i<n; i++)
		{
			CameraPose cp_i;
			prov = quatLeftComp(camera.q_CI)*feature.cameraPoses[i].q_CG;
			prov = prov/norm(prov);
			prov.copyTo(cp_i.q_CG);
			prov = feature.cameraPoses[i].p_CG + quatToProjMat(cp_i.q_CG).t()*camera.p_CI;
			prov.copyTo(cp_i.p_CG);

			// Compute evolution between first and current camera pose
			Mat C_i1 = quatToProjMat(cp_i.q_CG)*(quatToProjMat(cp_1.q_CG).t());
			Mat t_i1 = quatToProjMat(cp_i.q_CG)*(cp_1.p_CG - cp_i.p_CG);

			// Compute residuals
			Mat tmp = Mat::ones(3,1, CV_32FC1);
			xEst.rowRange(0,2).copyTo(tmp.rowRange(0,2));
			Mat h = C_i1*tmp + xEst.at<float>(2,0)*t_i1;
			tmp = Mat::ones(2,1,CV_32FC1);
			tmp.at<float>(0,0) = h.at<float>(0,0)/h.at<float>(2,0);
			tmp.at<float>(1,0) = h.at<float>(1,0)/h.at<float>(2,0);

			// Reproject estimated feature location in the 2d plane to compute the residuals
			errorVec.rowRange(2*i,2*i+2) = feature.positions[i]-tmp;

			// Form the Jacobian from eq. (39) in TR_MSCKF

			//dEdalpha
			E.at<float>(2*i, 0)   = -C_i1.at<float>(0,0)/h.at<float>(2,0) + (h.at<float>(0,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,0);
			E.at<float>(2*i+1, 0) = -C_i1.at<float>(1,0)/h.at<float>(2,0) + (h.at<float>(1,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,0);
			//dEdbeta
			E.at<float>(2*i, 1)   = -C_i1.at<float>(0,1)/h.at<float>(2,0) + (h.at<float>(0,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,1);
			E.at<float>(2*i+1, 1) = -C_i1.at<float>(1,1)/h.at<float>(2,0) + (h.at<float>(1,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,1);
			//dEdrho
			E.at<float>(2*i, 2)   = -t_i1.at<float>(0,0)/h.at<float>(2,0) + (h.at<float>(0,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*t_i1.at<float>(2,0);
			E.at<float>(2*i+1, 2) = -t_i1.at<float>(1,0)/h.at<float>(2,0) + (h.at<float>(1,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*t_i1.at<float>(2,0);
		}

		Mat delta;

		mySolve(E.t()*E, E.t()*errorVec, delta);
		xEst -= delta;

		sqrErr = 0.5*errorVec.t()*errorVec;
		Mat err = (sqrErr-sqrErr_prev)/n;//sqrErr;
		sqrErr.copyTo(sqrErr_prev);

		// Stop if gain in precision is too small
		if(err.at<float>(0,0) < 0.0000001)
			break;
	}

	// Reproject the feature location into the global referential
	Mat tmp = Mat::ones(3,1, CV_32FC1);
	xEst.rowRange(0,2).copyTo(tmp.rowRange(0,2));
	pos = (1/xEst.at<float>(2,0))*(quatToProjMat(cp_1.q_CG).t())*tmp +cp_1.p_CG;
	return sqrErr.at<float>(0,0);
}

/**
* Extracts the data to use for an EKF filter update
**/
void MSCKF::extractVariablesToPrune()
{
	// Prepare features pruning
	for(std::unordered_set<int32_t>::iterator it = features_to_residualize.begin(); it != features_to_residualize.end(); it++)
	{
		Feature feature = features.find(*it)->second;
		for(size_t i=0; i<feature.cameraPoses.size(); ++i)
		{
			feature.cameraPoses[i].featuresId.erase(*it);
			if(feature.cameraPoses[i].featuresId.size() == 0)
				posesToPrune.insert(feature.cameraPoses.at(i));
		}
	}

	// Prepare camera states pruning if too much poses in memory
	if(cameraPoses.size()-posesToPrune.size() < maxNbrePoses)
		return;

	std::unordered_map<int32_t, int32_t> features_correspondance;

	// Going through poses to prune
	for(size_t i=1; i<cameraPoses.size(); i+=3)
	{
		// Going through each feature detected for current pose

		for(std::unordered_set<int32_t>::iterator it = cameraPoses[i].featuresId.begin(); it != cameraPoses[i].featuresId.end(); ++it)
		{
			if(features_to_residualize.find(*it) != features_to_residualize.end())
				continue;

			if(features_correspondance.find(*it) == features_correspondance.end())
			{
				// Create temporary features to store information of features present in camera poses to prune
				features_correspondance.insert({*it, featureId_cnter});
				int index = features[*it].cameraPoses.getIndex(cameraPoses.at(i));
				Feature feature;
				features.insert({featureId_cnter, feature});
				features[*it].truePos.copyTo(features[featureId_cnter].truePos);
				features_to_residualize.insert(featureId_cnter);
				++featureId_cnter;

				features_to_prune.push_back(*it);
			}

			// If feature present in previous camstates, transfer information to correct temporary feature
			int id = features_correspondance[*it];
			int index = features[*it].cameraPoses.getIndex(cameraPoses.at(i));
			features[id].cameraPoses.push_back(cameraPoses.at(i));
			features[id].positions.push_back(features[*it].positions[index]);
			features[*it].positionsToPrune.insert(features[*it].positions.at(index));
		}
		posesToPrune.insert(cameraPoses.at(i));
	}
}

/**
* Deletes the data to use for an EKF filter update
**/
void MSCKF::pruneState()
{
	size_t nbre_remaining_states = cameraPoses.size() - posesToPrune.size();
	size_t L = imustate->stateLength;
	size_t N = 6*cameraPoses.size();
	Mat new_imuCamCovar = Mat:: zeros(L,6*nbre_remaining_states, CV_32FC1);

	size_t j=0;
	for(size_t i=0; i<cameraPoses.size(); ++i)
	{
		if(posesToPrune.find(cameraPoses.at(i)) != posesToPrune.end())
		{
			N = camCovar.rows-6; //6*(i-j);
			size_t size = camCovar.rows;
			Mat new_camCovar(N, N, CV_32FC1);

			if(j!=0)
				camCovar.rowRange(0, 6*j).colRange(0,6*j)			.copyTo(new_camCovar.rowRange(0, 6*j).colRange(0,6*j));
			if(j!=nbre_remaining_states)
			{
				if(j!=0)
				{
				camCovar.rowRange(6*j+6, size).colRange(0,6*j)		.copyTo(new_camCovar.rowRange(6*j, N).colRange(0,6*j));
				camCovar.rowRange(0,6*j).colRange(6*j+6, size)		.copyTo(new_camCovar.rowRange(0,6*j).colRange(6*j, N));
				}
				camCovar.rowRange(6*j+6, size).colRange(6*j+6, size).copyTo(new_camCovar.rowRange(6*j, N).colRange(6*j, N));
			}
			new_camCovar.copyTo(camCovar);
		}
		else
		{
			imuCamCovar.colRange(6*i, 6*i+6).copyTo(new_imuCamCovar.colRange(6*j, 6*j+6));
			++j;
		}

	}
	new_imuCamCovar.copyTo(imuCamCovar);
	for(std::unordered_set<int32_t>::iterator it = features_to_residualize.begin(); it != features_to_residualize.end(); ++it)
		features.erase(*it);
	for(size_t i=0; i<features_to_prune.size(); ++i)
	{
		int32_t id = features_to_prune[i];
		features[id].positions.remove(features[id].positionsToPrune);
		features[id].cameraPoses.remove(posesToPrune);
		features[id].positionsToPrune.clear();
	}

	cameraPoses.remove(posesToPrune);
	posesToPrune.clear();
	features_to_residualize.clear();
	features_to_prune.clear();
}

/**
* Applies the correction step
**/
void MSCKF::updateState(const Mat &deltaX)
{

	// Update IMU state
	imustate->q_IG 	= quatLeftComp( imustate->q_IG) *buildUpdateQuat(deltaX.rowRange(0,3));
	imustate->q_IG 	= imustate->q_IG/norm(imustate->q_IG);
	imustate->bg 	+= deltaX.rowRange(3,6);

	imustate->v_G 	+= deltaX.rowRange(6,9);
	imustate->ba 	+= deltaX.rowRange(9,12);
	imustate->p_G 	+= deltaX.rowRange(12,15);

	imustate->delay += deltaX.at<float>(16,1);
	camera.q_CI 	= quatLeftComp(camera.q_CI) *buildUpdateQuat(deltaX.rowRange(16,19));
	camera.q_CI 	/= float(norm(camera.q_CI));
	camProjMat 		= quatToProjMat(camera.q_CI);
	camera.p_CI 	+= camProjMat*deltaX.rowRange(19,22);

	cout << "Estimated time delay : " << imustate->delay << endl;

	// Update camera states
	size_t L = imustate->stateLength;
	for(size_t i=0; i<cameraPoses.size(); ++i)
	{
		cameraPoses[i].p_CG += deltaX.rowRange(L+6*i+3, L+6*i+6);
		cameraPoses[i].q_CG = quatLeftComp(cameraPoses[i].q_CG)*buildUpdateQuat(deltaX.rowRange(L+6*i, L+6*i+3));
		cameraPoses[i].q_CG /= float(norm(cameraPoses[i].q_CG));
	}
}


// Copyright 2018 Autoware Foundation. All rights reserved.
//
// 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 "shape_estimation/model/bounding_box.hpp"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <autoware_perception_msgs/msg/shape.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <algorithm>
#include <cmath>
#include <utility>
#include <vector>

#define EIGEN_MPL2_ONLY

#include <Eigen/Core>

constexpr float epsilon = 0.001;

BoundingBoxShapeModel::BoundingBoxShapeModel() : reference_yaw_(boost::none) {}

BoundingBoxShapeModel::BoundingBoxShapeModel(const boost::optional<float> & reference_yaw)
: reference_yaw_(reference_yaw)
{
}

bool BoundingBoxShapeModel::estimate(
  const pcl::PointCloud<pcl::PointXYZ> & cluster,
  autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
{
  float min_angle, max_angle;
  if (reference_yaw_) {
    min_angle = reference_yaw_.get() - autoware_utils::deg2rad(3);
    max_angle = reference_yaw_.get() + autoware_utils::deg2rad(3);
  } else {
    min_angle = 0.0;
    max_angle = M_PI / 2.0;
  }
  return fitLShape(cluster, min_angle, max_angle, shape_output, pose_output);
}

bool BoundingBoxShapeModel::fitLShape(
  const pcl::PointCloud<pcl::PointXYZ> & cluster, const float min_angle, const float max_angle,
  autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
{
  // calc min and max z for height
  float min_z = cluster.empty() ? 0.0 : cluster.at(0).z;
  float max_z = cluster.empty() ? 0.0 : cluster.at(0).z;
  for (const auto & point : cluster) {
    min_z = std::min(point.z, min_z);
    max_z = std::max(point.z, max_z);
  }

  /*
   * Paper : IV2017, Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners
   * Authors : Xio Zhang, Wenda Xu, Chiyu Dong and John M. Dolan
   */

  // Paper : Algo.2 Search-Based Rectangle Fitting
  std::vector<std::pair<float /*theta*/, float /*q*/>> Q;
  constexpr float angle_resolution = M_PI / 180.0;
  for (float theta = min_angle; theta <= max_angle + epsilon; theta += angle_resolution) {
    Eigen::Vector2f e_1;
    e_1 << std::cos(theta), std::sin(theta);  // col.3, Algo.2
    Eigen::Vector2f e_2;
    e_2 << -std::sin(theta), std::cos(theta);  // col.4, Algo.2
    std::vector<float> C_1;                    // col.5, Algo.2
    std::vector<float> C_2;                    // col.6, Algo.2
    for (const auto & point : cluster) {
      C_1.push_back(point.x * e_1.x() + point.y * e_1.y());
      C_2.push_back(point.x * e_2.x() + point.y * e_2.y());
    }
    float q = calcClosenessCriterion(C_1, C_2);  // col.7, Algo.2
    Q.push_back(std::make_pair(theta, q));       // col.8, Algo.2
  }

  float theta_star{0.0};  // col.10, Algo.2
  float max_q = 0.0;
  for (size_t i = 0; i < Q.size(); ++i) {
    if (max_q < Q.at(i).second || i == 0) {
      max_q = Q.at(i).second;
      theta_star = Q.at(i).first;
    }
  }
  const float sin_theta_star = std::sin(theta_star);
  const float cos_theta_star = std::cos(theta_star);

  Eigen::Vector2f e_1_star;  // col.11, Algo.2
  Eigen::Vector2f e_2_star;
  e_1_star << cos_theta_star, sin_theta_star;
  e_2_star << -sin_theta_star, cos_theta_star;
  std::vector<float> C_1_star;  // col.11, Algo.2
  std::vector<float> C_2_star;  // col.11, Algo.2
  for (const auto & point : cluster) {
    C_1_star.push_back(point.x * e_1_star.x() + point.y * e_1_star.y());
    C_2_star.push_back(point.x * e_2_star.x() + point.y * e_2_star.y());
  }

  // col.12, Algo.2
  const float min_C_1_star = *std::min_element(C_1_star.begin(), C_1_star.end());
  const float max_C_1_star = *std::max_element(C_1_star.begin(), C_1_star.end());
  const float min_C_2_star = *std::min_element(C_2_star.begin(), C_2_star.end());
  const float max_C_2_star = *std::max_element(C_2_star.begin(), C_2_star.end());

  const float a_1 = cos_theta_star;
  const float b_1 = sin_theta_star;
  const float c_1 = min_C_1_star;
  const float a_2 = -1.0 * sin_theta_star;
  const float b_2 = cos_theta_star;
  const float c_2 = min_C_2_star;
  const float a_3 = cos_theta_star;
  const float b_3 = sin_theta_star;
  const float c_3 = max_C_1_star;
  const float a_4 = -1.0 * sin_theta_star;
  const float b_4 = cos_theta_star;
  const float c_4 = max_C_2_star;

  // calc center of bounding box
  float intersection_x_1 = (b_1 * c_2 - b_2 * c_1) / (a_2 * b_1 - a_1 * b_2);
  float intersection_y_1 = (a_1 * c_2 - a_2 * c_1) / (a_1 * b_2 - a_2 * b_1);
  float intersection_x_2 = (b_3 * c_4 - b_4 * c_3) / (a_4 * b_3 - a_3 * b_4);
  float intersection_y_2 = (a_3 * c_4 - a_4 * c_3) / (a_3 * b_4 - a_4 * b_3);

  // calc dimension of bounding box
  Eigen::Vector2f e_x;
  Eigen::Vector2f e_y;
  e_x << a_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1)), b_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1));
  e_y << a_2 / (std::sqrt(a_2 * a_2 + b_2 * b_2)), b_2 / (std::sqrt(a_2 * a_2 + b_2 * b_2));
  Eigen::Vector2f diagonal_vec;
  diagonal_vec << intersection_x_1 - intersection_x_2, intersection_y_1 - intersection_y_2;

  // calc yaw
  tf2::Quaternion quat;
  quat.setEuler(/* roll */ 0, /* pitch */ 0, /* yaw */ std::atan2(e_1_star.y(), e_1_star.x()));

  // output
  shape_output.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
  shape_output.dimensions.x = std::fabs(e_x.dot(diagonal_vec));
  shape_output.dimensions.y = std::fabs(e_y.dot(diagonal_vec));
  shape_output.dimensions.z = std::max((max_z - min_z), epsilon);
  pose_output.position.x = (intersection_x_1 + intersection_x_2) * 0.5;
  pose_output.position.y = (intersection_y_1 + intersection_y_2) * 0.5;
  pose_output.position.z = min_z + shape_output.dimensions.z * 0.5;
  pose_output.orientation = tf2::toMsg(quat);
  // check wrong output
  shape_output.dimensions.x = std::max(static_cast<float>(shape_output.dimensions.x), epsilon);
  shape_output.dimensions.y = std::max(static_cast<float>(shape_output.dimensions.y), epsilon);

  return true;
}

float BoundingBoxShapeModel::calcClosenessCriterion(
  const std::vector<float> & C_1, const std::vector<float> & C_2)
{
  // Paper : Algo.4 Closeness Criterion
  const float min_c_1 = *std::min_element(C_1.begin(), C_1.end());  // col.2, Algo.4
  const float max_c_1 = *std::max_element(C_1.begin(), C_1.end());  // col.2, Algo.4
  const float min_c_2 = *std::min_element(C_2.begin(), C_2.end());  // col.3, Algo.4
  const float max_c_2 = *std::max_element(C_2.begin(), C_2.end());  // col.3, Algo.4

  std::vector<float> D_1;  // col.4, Algo.4
  for (const auto & c_1_element : C_1) {
    const float v = std::min(max_c_1 - c_1_element, c_1_element - min_c_1);
    D_1.push_back(v * v);
  }

  std::vector<float> D_2;  // col.5, Algo.4
  for (const auto & c_2_element : C_2) {
    const float v = std::min(max_c_2 - c_2_element, c_2_element - min_c_2);
    D_2.push_back(v * v);
  }
  constexpr float d_min = 0.1 * 0.1;
  constexpr float d_max = 0.4 * 0.4;
  float beta = 0;  // col.6, Algo.4
  for (size_t i = 0; i < D_1.size(); ++i) {
    if (d_max < std::min(D_1.at(i), D_2.at(i))) {
      continue;
    }
    const float d = std::max(std::min(D_1.at(i), D_2.at(i)), d_min);
    beta += 1.0 / d;
  }
  return beta;
}


#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>
#include <Eigen/StdVector>
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <mpi.h>
#include <mkl.h>
#include <mkl_spblas.h>
#include <fstream>
#include <iterator>
#include <vector>
#include <algorithm>
#include <complex>
#include <sstream>
#include <math.h>
#include <omp.h>
#include <numeric>
#include <chrono>
#include <ctime>
#include <limits>
#include <lyra/lyra.hpp>
#include <sys/stat.h>
#include <sys/types.h>
#include <cusparse.h>
#include <cusolverDn.h>
#include <cuComplex.h>
#include "cuda.h"
#include "cuda_runtime_api.h"
#include "CUDAaccelerated.h"
#include "bsplines.h"
#include "InputParameter.h"

#pragma GCC diagnostic ignored "-Wdeprecated-declarations"

#define watch(x) cout << (#x) << " is " << (x) << endl
#define pwatch(x) cout <<"rank " << rank <<" "<< (#x) << " is " << (x) << endl
#define bp(x) cout << "breakpoint" << (#x) << endl

using namespace Eigen;
using namespace std::complex_literals;
using namespace std::chrono;
using std::cout;
using std::endl;

int dim;
int rank,p,p_omp;
int64_t nnz_rank;
int states_chan_rank;
int states_chan_rank_begin;
int states_chan_rank_end;

//This is a COO matrix that holds a tupel of values for every matrix cell
class SpMatrix{
    //idea write a matrix class that has the functionality of init memory, write element,
    //multiply
    public:
        std::vector<int> nnz;
        int dim;
        int nmbrLEGO=3;

        //The COO matrix
        std::vector<std::vector<int> > Row;
        std::vector<std::vector<int> > Col;
        std::vector<std::vector<double> > Value;

        //The CSR matrix
        std::vector<std::vector<int> > RowCSR;
        std::vector<std::vector<int> > ColCSR;
        std::vector<std::vector<double> > ValueCSR;

        //GPU accelerated matrix vector
        //pointers to the device memory
        std::vector<int*> LEGO_d_cooCols;
        std::vector<int*> LEGO_d_rowPtr;
        std::vector<double* > LEGO_d_cooVals_sorted;
        std::vector<int*> LEGO_d_cscRows;
        std::vector<int*>  LEGO_d_cscColPtr;
        std::vector<double*> LEGO_d_cscVals;

    void initMatrix(int nnz_input){
        Row.resize(nmbrLEGO);
        Col.resize(nmbrLEGO);
        Value.resize(nmbrLEGO);

        RowCSR.resize(nmbrLEGO);
        ColCSR.resize(nmbrLEGO);
        ValueCSR.resize(nmbrLEGO);

        LEGO_d_cooVals_sorted.resize(nmbrLEGO);
        LEGO_d_cooCols.resize(nmbrLEGO);
        LEGO_d_rowPtr.resize(nmbrLEGO);
        LEGO_d_cscVals.resize(nmbrLEGO);
        LEGO_d_cscColPtr.resize(nmbrLEGO);
        LEGO_d_cscRows.resize(nmbrLEGO);

        for(int i = 0; i < nmbrLEGO; i++){
            Row[i].resize(nnz_input);
            Col[i].resize(nnz_input);
            Value[i].resize(nnz_input);
        }
        nnz.resize(nmbrLEGO);

    }

    void writeElement(int ind, int row, int col, int LEGOind,double value){
        //writes an element to the matrix
        Row[LEGOind][ind] = row;
        Col[LEGOind][ind] = col;
        Value[LEGOind][ind] = value;
    }

    void CSRMKL(){
        //allocate the memeory for the CSR matrices
        watch(nmbrLEGO);
        for(int i = 0; i < nmbrLEGO; i++){
            watch(i);
            watch(nnz[i]);
            RowCSR[i].resize(dim+2);
            ColCSR[i].resize(nnz[i]);
            ValueCSR[i].resize(nnz[i]);
        }
        bp("Allocate memory");

        //build the CSR matrix
        for (int i = 0; i< nmbrLEGO;i++){
            int job[5] = {2,0,0,nnz[i],0};
            int info = 0;
            mkl_dcsrcoo(job,
                    &dim,
                    ValueCSR[i].data(),
                    ColCSR[i].data(),
                    RowCSR[i].data(),
                    &nnz[i],
                    Value[i].data(),
                    Row[i].data(),
                    Col[i].data(),
                    &info
                );
        }

    }

    void CSRonDevice(){
        //converts the matrices stored in COO on the host to CSR on the device by
        //calling the accoarding CUDA function
        //iterate over the LEGO bricks
        if(rank==0){
             int i=0;
             makeCSR(nnz[i],dim,Row[i].data(),Col[i].data(),Value[i].data(),LEGO_d_cooCols[i],
                    LEGO_d_rowPtr[i], LEGO_d_cooVals_sorted[i],
                    LEGO_d_cscRows[i],
                    LEGO_d_cscColPtr[i],
                    LEGO_d_cscVals[i]);
        }
        for(int i=1; i < nmbrLEGO; i++){
            makeCSR(nnz[i],dim,Row[i].data(),Col[i].data(),Value[i].data(),LEGO_d_cooCols[i],
                    LEGO_d_rowPtr[i], LEGO_d_cooVals_sorted[i],
                    LEGO_d_cscRows[i],
                    LEGO_d_cscColPtr[i],
                    LEGO_d_cscVals[i]);

       }
    }

    void matrixVectorUpper(int LEGO,VectorXcd &x,VectorXcd &b,VectorXcd &ft){
       //This is a COO matrix vector product M * x = b
#pragma omp parallel
        {
        VectorXcd b_private= VectorXcd::Zero(dim);
#pragma omp for
        for(int  i = 0; i < nnz[LEGO]; i++)
        {
                b_private(Row[LEGO][i]) += Value[LEGO][i] * x(Col[LEGO][i]) * ft(LEGO);
            if (Row[LEGO][i] != Col[LEGO][i]){
                b_private(Col[LEGO][i]) += Value[LEGO][i] * x(Row[LEGO][i]) * std::conj(ft[LEGO]);
            }
        }
#pragma omp critical
        for(int i=0; i < dim; i++){
            b(i) += b_private(i);
        }
        }
    }

    //wrapper for MKL CSR SpMV
    void matrixVectorMKL(VectorXcd &x,VectorXcd &b,VectorXcd &ft){
        //as the matrix is real we seperate the vectors in real and iamg parts
        //this allows a real double multiplications
        VectorXd dydx_rank_real = b.real();
        VectorXd dydx_rank_imag = b.imag();

       char transa = 'N';
        char matdescra[] = {
            'G', // type of matrix
            ' ', // triangular indicator (ignored in multiplication)
            ' ', // diagonal indicator (ignored in multiplication)
            'C'  // type of indexing
        };

        double beta = 1.0;
        for(int j = 0; j < nmbrLEGO; j++){
            VectorXcd y_local = x * ft(j);
            VectorXd y_local_real = y_local.real();
            VectorXd y_local_imag = y_local.imag();

            //SpMVCuda
            double alpha=1.0;
            //non transpose real x, real y, alpha = 1
            mkl_dcsrmv(&transa,
                    &dim,
                    &dim,
                    &alpha,
                    matdescra,
                    ValueCSR[j].data(),
                    ColCSR[j].data(),
                    RowCSR[j].data(),
                    RowCSR[j].data()+1,
                    y_local_real.data(),
                    &beta,
                    dydx_rank_real.data());

            //non transpose imag x, imag y, alpha = 1
             mkl_dcsrmv(&transa,
                    &dim,
                    &dim,
                    &alpha,
                    matdescra,
                    ValueCSR[j].data(),
                    ColCSR[j].data(),
                    RowCSR[j].data(),
                    RowCSR[j].data()+1,
                    y_local_imag.data(),
                    &beta,
                    dydx_rank_imag.data());

            if (j != 0){
                alpha=-1.0;
                char transa = 'T';
                //transpose real x, real y , alpha = -1
                mkl_dcsrmv(&transa,
                    &dim,
                    &dim,
                    &alpha,
                    matdescra,
                    ValueCSR[j].data(),
                    ColCSR[j].data(),
                    RowCSR[j].data(),
                    RowCSR[j].data()+1,
                    y_local_real.data(),
                    &beta,
                    dydx_rank_real.data());

                //transpose imag x, imag y , alpha = -1
                mkl_dcsrmv(&transa,
                    &dim,
                    &dim,
                    &alpha,
                    matdescra,
                    ValueCSR[j].data(),
                    ColCSR[j].data(),
                    RowCSR[j].data(),
                    RowCSR[j].data()+1,
                    y_local_imag.data(),
                    &beta,
                    dydx_rank_imag.data());

            }
        }
        //join into b
        b.real()=dydx_rank_real;
        b.imag()=dydx_rank_imag;

    }
};

void Propergator(InputParameter &Inp,SpMatrix &H_rank,std::string PATH);

void BuildMatrix(InputParameter &Inp,SpMatrix &H_rank);

void getEigenStates(InputParameter Inp, int l,int states_chan,double dX,double &tmp_d,ArrayXd &weights,
        ArrayXd &x,ArrayXXd &B, ArrayXXd &d_B,ArrayXXd &diff_B,ArrayXXd &psi,ArrayXXd &d_psi
        ,ArrayXXd &diff_psi , MatrixXd &H_base, MatrixXd &S, ArrayXd &Energy);

void helperEigenStates(InputParameter Inp,int states_chan,double dX,double &tmp_d,
        ArrayXd &weights, ArrayXd &x,ArrayXXd &B,ArrayXXd &diff_B,MatrixXd &H_base,
        MatrixXd &S);

void polarization(SpMatrix &M,int states_chan, int &ind, int index,int l,int lm_index_L,int
        lm_index_R, double LM_faktor,double dX,ArrayXd &weights, ArrayXd &x, ArrayXXd &psi_1,
        ArrayXXd  &psi_2,ArrayXXd &d_psi_2);
void lanczos(InputParameter Inp,SpMatrix &A, int m,VectorXcd &y , MatrixXcd &Q, MatrixXcd &h,
        VectorXcd &FT);

auto stoportho = high_resolution_clock::now();
auto startortho = high_resolution_clock::now();
auto durationortho = duration_cast<milliseconds>(startortho-startortho);

int main(int argc, const char** argv){
    //Read in the sparse matrix representing the Hamiltonian
    //The matrix is in COO format(also called matrix market format)
    //The data is stored in 4 different files each of them
    //obtains a vector. 2 files hold the indices for our sparse matrix and
    //2 hold the values that belong to the to indices. One is the value
    //belonging to the Hamiltion the other one is an integer telling us which
    //ft to use
    MPI_Init(NULL,NULL);

    MPI_Comm_size(MPI_COMM_WORLD, &p);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);

    cout << "rank: " << rank << " of " << p << " is here" << endl;
    InputParameter Inp;

    //map the ranks to the devices(grpahic cards)
    //local rank within a node for multi node version
    //this has to be chagned
    int local_rank = rank;

    p_omp = omp_get_num_threads();
    watch(p_omp);

    //int id = omp_get_thread_num();
    std::setprecision (15);

    //Inp.ReadInput(argc,argv);
    auto cli
        =   lyra::opt(Inp.nnz, "nnz")
            ["-nnz"]["--nnz"]
            ("Number of max nnz")
        | lyra::opt(Inp.om, "om")
            ["-om"]["--om"]
            ("Photon Energy")
        | lyra::opt(Inp.E0, "E0")
            ["-E0"]["--E0"]
            ("Electric field strength")
        |lyra::opt(Inp.T_cycle, "T_cycle")
            ["-T"]["--T_cycle"]
            ("length of pulse in cycles")
        |lyra::opt(Inp.timesteps, "timesteps")
            ["-t"]["--timesteps"]
            ("Number of timesteps")
        |lyra::opt(Inp.b,"b")
            ["-b"]["--b"]
            ("Boxsize")
        |lyra::opt(Inp.N_b,"N_b")
            ["-N_b"]["--N_b"]
            ("Number of gridpoints")
        |lyra::opt(Inp.Lmax,"Lmax")
            ["-Lmax"]["--Lmax"]
            ("Maximum angular momentum")
        |lyra::opt(Inp.Mmax,"Mmax")
            ["-Mmax"]["--Mmax"]
            ("Maximum magnetic quantum number")
        |lyra::opt(Inp.Emax,"Emax")
            ["-Emax"]["--Emax"]
            ("Maximum Energy in simulation")
        |lyra::opt(Inp.I0,"I0")
            ["-I0"]["--I0"]
            ("Laser Intensity in 1e12")
        |lyra::opt(Inp.init_state,"Initial state")
            ["-init"]["--init"]
            ("Initial state of system")
        |lyra::opt(Inp.Accelerator, "Accelerator")
            ["-acc"]["--acc"]
            ("Use CPU or GPU as accelerator")
        |lyra::opt(Inp.zComp, "z-Comp")
            ["-zComp"]["--zComp"]
            ("z comp")
        |lyra::opt(Inp.xComp, "x-comp")
            ["-xComp"]["--xComp"]
            ("x Comp")
        |lyra::opt(Inp.xft, "x-ft")
            ["-xft"]["--xft"]
            ("x ft")
        |lyra::opt(Inp.zft, "z-ft")
            ["-zft"]["--zft"]
            ("z ft")
        |lyra::opt(Inp.l_quantum,"l Quantum number")
            ["-lq"]["--l_q"]
            ("l Quantum number of initial state")
        |lyra::opt(Inp.m_quantum,"n Quantum number")
            ["-mq"]["--m_q"]
            ("m Quantum number of initial state")
        |lyra::opt(Inp.n_quantum,"n Quantum number")
            ["-nq"]["--n_q"]
            ("n Quantum number of initial state");

        auto result = cli.parse({ argc, argv });
        if ( !result )
        {
                std::cerr << "Error in command line: " << result.errorMessage() << std::endl;
                    exit(1);
        }

    //number of accelerators in the system
    int num_devices = 0;
    if (Inp.Accelerator == "GPU" ||  Inp.Accelerator == "GPUFull"){
        setupGPUs(local_rank,num_devices);
        watch(num_devices);
    }

    SpMatrix H_rank;
    H_rank.nmbrLEGO = 1 + Inp.zComp +  Inp.xComp;
    BuildMatrix(Inp,H_rank);
    std::string PATH;
    Propergator(Inp,H_rank,PATH);

    if (Inp.Accelerator == "GPU" ||  Inp.Accelerator == "GPUFull"){
        CUDAFinalize(H_rank.nmbrLEGO,
                H_rank.LEGO_d_rowPtr,H_rank.LEGO_d_cooCols,H_rank.LEGO_d_cooVals_sorted,
                H_rank.LEGO_d_cscRows,H_rank.LEGO_d_cscColPtr,H_rank.LEGO_d_cscVals);
    }
    watch(rank);
    MPI_Finalize();
    cout << "MPI has been finalized" << endl;
    return 0;

}

void BuildMatrix(InputParameter &Inp,SpMatrix &H_rank){
    //recalculate variables that depend on command line input
        //spacing between breakpoints
    VectorXd x_break(Inp.N_b);
    double dX = (Inp.b - Inp.a)/ (Inp.N_b-1);
    x_break = ArrayXd::LinSpaced(Inp.N_b,Inp.a,Inp.b);

    int states_chan = (int) round(Inp.b/M_PI*sqrt(2*Inp.Emax));

    if (rank==0){
        //print the Input parameters
        watch(Inp.nnz);
        watch(Inp.a);
        watch(Inp.b);
        watch(Inp.Lmax);
        watch(Inp.Mmax);
        watch(Inp.Emax);
        watch(Inp.N_b);
        watch(states_chan);
        watch(Inp.zComp);
        watch(Inp.xComp);
        watch(Inp.zft);
        watch(Inp.xft);

        watch(Inp.Accelerator);
        watch(Inp.l_quantum);
        watch(Inp.m_quantum);
        watch(Inp.n_quantum);
        watch(Inp.init_state);
        watch(Inp.om);
        watch(Inp.I0);
        watch(Inp.E0);
        watch(Inp.T_puls);
        watch(Inp.T_cycle);
        watch(Inp.Tint);
        watch(Inp.timesteps);

    }

    auto start = system_clock::now();
      nnz_rank = Inp.nnz/p;
    states_chan_rank=states_chan/p;
    //distribute the remaining channels if states_cahn%p != 0
    if(rank < states_chan%p){
        states_chan_rank++;
    }
    //offset describes how many ranks have an extra channel
    int offset = std::min(states_chan%p,rank);
    states_chan_rank_begin=states_chan/p*rank+offset;
    states_chan_rank_end=states_chan/p*rank+states_chan_rank+offset;

    pwatch(states_chan_rank);
    pwatch(offset);
    pwatch(states_chan_rank_begin);
    pwatch(states_chan_rank_end);

    pwatch("matrix obj is created");
    bp("before memory is allocated");
    H_rank.initMatrix(Inp.nnz/p);
    H_rank.nnz[0] = nnz_rank;
    bp("after memory is allocated");

    //this is the ind on every rank his is a vector that holds the ind for all
    //different matrices linked to the different LEGO bricks
    std::vector< int>ind(H_rank.nmbrLEGO,0);

    //the matrix will be generated on different MPI ranks
    //generating the B-spliens does not take much time therefore all ranks generate
    //their own set. They also for now all hold their own set of psi

    //Gaus Legendre integration set up
    ArrayXd x((Inp.N_b-1)*Inp.n);
    ArrayXd weights((Inp.N_b-1)*Inp.n);
    bp("before gaus set up");
    gausLegendreSetup(Inp.n,Inp.N_b,x_break,x,weights);
    //gauss legendre returns same weights and x as python script
    //allocating a vector of Eigenvector matrices

    bp("after gaus set up");

    ArrayXXd B = ArrayXXd::Zero(x.size(),Inp.N_b-1+2*Inp.n);
    ArrayXXd d_B = ArrayXXd::Zero(x.size(),Inp.N_b-1+2*Inp.n);
    ArrayXXd diff_B = ArrayXXd::Zero(x.size(),Inp.N_b-1+2*Inp.n);

    int dim_H = Inp.N_b+Inp.n-3;
    generateBsplines(Inp,dX,x,B,d_B,diff_B);
    bp("bsplines generated");
    //Memory declaration
    ArrayXXd psi_1 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXXd d_psi_1 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXXd diff_psi_1 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXd E1 = ArrayXd::Zero(dim_H);

    ArrayXXd psi_2 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXXd d_psi_2 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXXd diff_psi_2 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXd E2 = ArrayXd::Zero(dim_H);

    ArrayXXd psi_3 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXXd d_psi_3 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXXd diff_psi_3 = ArrayXXd::Zero(x.size(),states_chan);
    ArrayXd E3 = ArrayXd::Zero(dim_H);

    MatrixXd H_base = MatrixXd::Zero(dim_H,dim_H);
    MatrixXd S = MatrixXd::Zero(dim_H,dim_H);
    double tmp_d;
    helperEigenStates(Inp,states_chan,dX,tmp_d,weights, x, B,diff_B,H_base,S);
     getEigenStates(Inp,Inp.Lmin,states_chan,dX,tmp_d,weights,x,B,d_B,diff_B,psi_1,d_psi_1,diff_psi_1,H_base,S,E1);
     bp("First set of psi");

    getEigenStates(Inp,Inp.Lmin+1,states_chan,dX,tmp_d,weights,x,B,d_B,diff_B,psi_2,d_psi_2,diff_psi_2,H_base,S,E2);

     bp("second set of psi");

     ArrayXd Input_Energies = ArrayXd::Zero(5000000);
    int ind2 =0;
   int lm_index_L =-1;
    for (int l=Inp.Lmin; l < Inp.Lmax+1; l++){
        pwatch(l);
       getEigenStates(Inp,l+2,states_chan,dX,tmp_d,weights,x,B,d_B,diff_B,psi_3,d_psi_3,diff_psi_3,H_base,S,E3);

        for(int m=-std::min(Inp.Mmax,l); m < std::min(Inp.Mmax,l)+1; m++){
            lm_index_L++;
            int lm_index_R=-1;
            for(int l2 = Inp.Lmin; l2 < Inp.Lmax+1; l2++){
                for(int m2 = -std::min(Inp.Mmax,l2);m2 < std::min(Inp.Mmax,l2)+1;m2++){
                    lm_index_R++;
                    if (l2==l+1 && m2 ==m){
                        //z-polarization
                        //< l m | x | l+1 m>:
                        if(Inp.zComp == 1){
                            double LM_faktor=sqrt(((l+1)*(l+1)-m*m)/(double) (4*((l+1)*(l+1))-1));
                            polarization(H_rank,states_chan, ind[Inp.zft],Inp.zft,l,lm_index_L,lm_index_R,
                                LM_faktor,dX,weights,x,psi_1,psi_2,d_psi_2);
                        }
                    }
                    if(l2== l+1 && m2 == m+1){
                        //x-polarization
                        //%< l m | p_x | l+1 m+1>:
                        if(Inp.xComp == 1){
                            double LM_faktor=-sqrt((l+m+1)*(l+m+2)/(double) (4*(2*l+1)*(2*l+3)));
                            polarization(H_rank,states_chan, ind[Inp.xft],Inp.xft,l,lm_index_L,lm_index_R,
                                LM_faktor,dX,weights,x,psi_1,psi_2,d_psi_2);
                        }
                    }
                    if(l2== l+1 && m2 == m-1){
                        //x-polarization
                        //< l m | p_x | l+1 m-1>:
                        if(Inp.xComp == 1){
                            double LM_faktor=sqrt((l-m+1)*(l-m+2)/(double) (4*(2*l+1)*(2*l+3)));
                            polarization(H_rank,states_chan, ind[Inp.xft],Inp.xft,l,lm_index_L,lm_index_R,
                                    LM_faktor,dX,weights,x,psi_1,psi_2,d_psi_2);
                        }
                    }
                }
            }

            //Lego brick 4 will unperturbated states will be only calculated on rank 0 as they are
            //not many. We also need the Input_Energies later as a txt file
            if(rank==0){
               //Lego brick 4
                for(int j= 0; j < states_chan;j++){
                    H_rank.writeElement(ind[0],lm_index_L*states_chan+j,lm_index_L*states_chan+j,0,E1(j));
                    Input_Energies(ind2)=std::real(E1(j));
                    ind[0]++;
                    ind2++;

                }
            }

        }
        //so we do not have to calculate the psi1,psi2 again
        //psi3 -> psi2, psi2 -> psi1
        E1=E2;
        psi_1=psi_2;
        d_psi_1=d_psi_2;
        diff_psi_1=diff_psi_2;

        E2=E3;
        psi_2=psi_3;
        d_psi_2=d_psi_3;
        diff_psi_2=diff_psi_3;
        if(rank==0){
            int nnzRank=0;
            for(int k=0; k < H_rank.nmbrLEGO; k++){
                watch(ind[k]);
                nnzRank+=ind[k];
            }
            watch(nnzRank);
            cout << "Total number of elements in 1e+6: " << (long long) p * nnzRank / 1.0e6 << endl;
        }
    }
    //the number of elements every matrix on every rank holds
    nnz_rank=ind[0];
    for(int i = 0; i < H_rank.nmbrLEGO; i++){
        H_rank.nnz[i]=ind[i];
    }
    //write to file
    if(rank ==0){
        dim=ind2;
        watch(ind2);
        std::ofstream f6 ("Energies_Input_shago.txt");
        cout << "Begin to write file" << endl;

              if (f6.is_open()){
               for (int i = 0; i < ind2; i++){
                    f6 << Input_Energies(i) << "\n";
               }
           }

         else cout << "Unable to open file";

        //End of timing
        auto stop = system_clock::now();
        auto duration = duration_cast<milliseconds>(stop - start);
        cout << "Time taken for generating the matrix: " << (double) duration.count()/1000 << "seconds" << endl;
    }

    //Broadcast the dimension of the vector;
    MPI_Bcast(&dim,1,MPI_INT,0,MPI_COMM_WORLD);

    cout << "rank" << " " << rank << " holds elemets: " << nnz_rank << endl;
    H_rank.dim=dim;
    pwatch(dim);
    pwatch(nnz_rank);

    //transform the matrix to CSR after the read in is done
    //we use less memory and obtain a better performance
    auto startSort = system_clock::now();
    if(Inp.Accelerator == "GPU" || Inp.Accelerator == "GPUFull"){
        bp("Before create CSR-GPU");
        H_rank.CSRonDevice();
        bp("After create CSR-GPU");
    }
    else if(Inp.Accelerator == "MKL"){
        bp("Before create CSR-MKL");
        H_rank.CSRMKL();
        bp("After create CSR-MKL");
    }
    else if(Inp.Accelerator != "CPU"){
        cout << "Inp.Accelerator is not a vaild accelerator [CPU,GPU,MKL]" << endl;
        exit(EXIT_FAILURE);

    }
    auto stopSort = system_clock::now();
    auto durationSort = duration_cast<milliseconds>(stopSort - startSort);
    cout << "Time taken for sorting the matrix: " << (double) durationSort.count()/1000 << "seconds" << endl;

}//End of matrix build up

void Propergator(InputParameter &Inp,SpMatrix &H_rank,std::string PATH){
    Inp.E0 = 5.338*sqrt(Inp.I0*1e12)*1e-9;
    Inp.T_puls = Inp.T_cycle * 2 * M_PI / Inp.om;

    int states_chan = (int) round(Inp.b/M_PI*sqrt(2*Inp.Emax));
    //the initial state mind that C++ index starts at 0
    //this depends on Lmax,Mmax & qunatum number n,l,m
    int l_states= std::min(Inp.l_quantum,Inp.Mmax);
    int m_states= std::min(Inp.m_quantum,Inp.Mmax);
    Inp.init_state = states_chan * (Inp.l_quantum * (l_states+1) + m_states)
         +  Inp.n_quantum-Inp.l_quantum-1;

   if(rank ==0){
        //Input param propagator
        watch(Inp.Accelerator);
        watch(Inp.l_quantum);
        watch(Inp.m_quantum);
        watch(Inp.n_quantum);
        watch(Inp.init_state);
        watch(Inp.om);
        watch(Inp.I0);
        watch(Inp.E0);
        watch(Inp.T_puls);
        watch(Inp.T_cycle);
        watch(Inp.Tint);
        watch(Inp.timesteps);
    }

    //initalize the state vector y
    VectorXcd y = VectorXcd::Zero(dim);
    //For the TDSE we start with the electron in the lowest possible state s1
    y(Inp.init_state) = 1.0;
    VectorXcd y_init(dim);
    VectorXcd p_init(Inp.timesteps);
    VectorXcd p_excited_1(Inp.timesteps);
    VectorXcd p_excited_2(Inp.timesteps);
    y_init << y;
    //Time propagator
    double dt = (Inp.T_puls-Inp.Tint)/Inp.timesteps;

    //Only needed on rank 0 but they are small
    MatrixXcd Q = MatrixXcd::Zero(dim,Inp.dim_kryl);  //Eigenvectors
    MatrixXcd h = MatrixXcd::Zero(Inp.dim_kryl,Inp.dim_kryl); //Eigenvalues
    VectorXcd d = VectorXcd::Zero(Inp.dim_kryl);
    VectorXd od = VectorXd::Zero(Inp.dim_kryl-1);

    if(rank==0)
        cout << "decleration of variables finished" << endl;

    //Start timing the propagation
    auto start2 = system_clock::now();

    if(rank==0)
        cout << "start timepropagation" << endl;

    MPI_Barrier(MPI_COMM_WORLD);
    if (Inp.Accelerator == "GPUFull"){
        cuDoubleComplex *dQ;
        cuDoubleComplex *dd;
        cuDoubleComplex *dy;
        double *od;
         cusparseHandle_t handle_sparse;
         cublasHandle_t handle_blas;
        cusolverDnHandle_t handle_solver;
        allocateMemoryPropagator(Inp,H_rank.dim,dQ,dd,od,dy,y.data());
        for(int i = 1; i < Inp.timesteps+1;i++){
            //normalize y

            double normStart;

            double t = dt *i;
            //get ft
            VectorXcd ft =  VectorXcd(116);
            ft_calc(Inp,t, ft.data());

            lanczosCUDA(
                        H_rank.nnz,H_rank.dim,Inp.dim_kryl, H_rank.nmbrLEGO,
                        normStart,
                            H_rank.LEGO_d_cooCols,
                            H_rank.LEGO_d_rowPtr,
                            H_rank.LEGO_d_cooVals_sorted,
                            H_rank.LEGO_d_cscRows,
                            H_rank.LEGO_d_cscColPtr,
                            H_rank.LEGO_d_cscVals,
                            dy,ft.data(),
                            od,dQ,dd);

            //physical propergator
            if (rank ==0){
                physPropagator(Inp,Inp.dim_kryl,dim,normStart,dt,dQ,dd,od,dy,y.data());

                if(i%20 == 0){
                    cout << "Timesteps: " << i << endl;
                    double norm = y.norm();
                watch(y(Inp.init_state));
                watch(y(Inp.init_state+1));
                watch(y(Inp.init_state+2));
                cout << "norm: " << norm << endl;
                cout << "Pinit: " <<  abs(y(Inp.init_state))*abs(y(Inp.init_state)) << endl;

                }

            }
        }

        freeMemoryPropagator(Inp,H_rank.dim,dQ,dd,od,dy,y.data());
                watch(y(Inp.init_state));
                watch(y(Inp.init_state+1));
                watch(y(Inp.init_state+2));
                cout << "Pinit: " <<  abs(y(Inp.init_state))*abs(y(Inp.init_state)) << endl;

    }
    else{
        for(int i = 1; i < Inp.timesteps+1; i++){
            double norm;
            if (rank==0){
                norm = y.norm();
                y = y/norm;
            }

            //get the ft values
            VectorXcd ft =  VectorXcd(116);
            double t = dt * i;
            ft_calc(Inp,t, ft.data());

            //Lanzcos with GPU for SpMV
            if(Inp.Accelerator == "CPU" || Inp.Accelerator == "MKL"){
                lanczos(Inp, H_rank,  dim, y , Q, h, ft);
            }
            cout << "h" << endl;
            cout << h << endl;
            //To get the matrix exponential first diaganolize the matrix the exponate its
            //elements.
            if(rank == 0){
                 startortho = high_resolution_clock::now();

                SelfAdjointEigenSolver<MatrixXcd> ces;
                ces.compute(h);
                MatrixXcd D = (-1i * dt * ces.eigenvalues()).array().exp().matrix().asDiagonal();
                MatrixXcd P = ces.eigenvectors();
                y = norm * Q * (P * D * P.inverse()).col(0);

                stoportho = high_resolution_clock::now();
                durationortho += duration_cast<milliseconds>(stoportho - startortho);

                //write data
                p_init(i-1)=abs(y(Inp.init_state))*abs(y(Inp.init_state));
                p_excited_1(i-1)=abs(y(Inp.init_state+1))*abs(y(Inp.init_state+1));
                p_excited_2(i-1)=abs(y(Inp.init_state+2))*abs(y(Inp.init_state+2));
               if(i%20 == 0){
                    cout << "Timesteps: " << i << endl;
                    norm = y.norm();
                    watch(y(Inp.init_state));
                    watch(y(Inp.init_state+1));
                    watch(y(Inp.init_state+2));
                    cout << "norm: " << norm << endl;
                    cout << "Pinit: " <<  abs(y(Inp.init_state))*abs(y(Inp.init_state)) << endl;

                }
            }
        }
    }
    watch(rank);
    //End of timing
    auto stop2 = system_clock::now();
    auto duration2 = duration_cast<milliseconds>(stop2 - start2);
    if (rank==0){
        cout << "Time taken by propagating the matrix: " << (double) duration2.count()/1000 << "seconds" << endl;

        cout << "Time taken by the actual propergator " << (double) durationortho.count()/1000
            <<"seconds" << endl;

       //write the output
        std::ofstream f1 (PATH + "fort.88");
        f1.precision(15);
        if (f1.is_open()){
            for (int i = 0; i < dim; i++){
                f1 << abs(y(i)) * abs(y(i)) << "\n";
            }
        }
        else cout << "Unable to open file";
        cout << "fort.88 is written" << endl;
        std::ofstream f2 (PATH +"fort.89");
        if (f2.is_open()){
            for (int i = 0; i < dim; i++){
                f2 << i << " " <<  y(i).real() << " " << y(i).imag() << "\n";
            }
        }
        else cout << "Unable to open file";
        cout << "fort.89 is written" << endl;
        //write the output
        std::ofstream f3 (PATH + "pinit.dat");
        f3.precision(15);
        if (f3.is_open()){
            for (int i = 0; i < Inp.timesteps; i++){
                double time = (i+1) * Inp.T_puls/Inp.timesteps;
                f3 << time <<" " << p_init(i).real() << "\n";
            }
        }
        else cout << "Unable to open file";

        std::ofstream f4 (PATH +"pexcited_1.dat");
        f4.precision(15);
        if (f4.is_open()){
            for (int i = 0; i < Inp.timesteps; i++){
                double time = (i+1) * Inp.T_puls/Inp.timesteps;
                f4 << time <<" " << p_excited_1(i).real() << "\n";
            }
        }
        else cout << "Unable to open file";

        std::ofstream f5 (PATH +"pexcited_2.dat");
        f5.precision(15);
        if (f5.is_open()){
            for (int i = 0; i < Inp.timesteps; i++){
                double time = (i+1) * Inp.T_puls/Inp.timesteps;
                f5 << time <<" " << p_excited_2(i).real() << "\n";
            }
        }
        else cout << "Unable to open file";
        cout << "pinit,pexcited are written" << endl;

    }
}

void lanczos(InputParameter Inp,SpMatrix &H_rank, int m,VectorXcd &y ,MatrixXcd &Q, MatrixXcd &h,
        VectorXcd &FT){
    /* Input
    H_rank: mxm matrix
    b: initial vector
    Inp.drim_kryl: dimension of Krylov subspace L in morten code
    m: dimesion of matrix H_rank
    Output:
    Q: orthogonal Krylov space
    h: tridiagonal matrix
    */
    VectorXcd d = VectorXcd::Zero(Inp.dim_kryl);
    VectorXcd od = VectorXcd::Zero(Inp.dim_kryl-1); //L-1 from morten
    VectorXcd dydx_rank = VectorXcd::Zero(dim);
    //will only be used on rank0
    VectorXcd dydx = VectorXcd::Zero(dim);

    Q.setZero();
    Q.col(0) = y;
    h.setZero();

    for (int i = 0; i < Inp.dim_kryl; i++){
        dydx_rank.setZero(dim);
        //broadcast changeing vector y to all ranks
        MPI_Bcast(y.data(),dim*2,MPI_DOUBLE,0,MPI_COMM_WORLD);

        //Matrix vector product this is the only loop that get executed in parallel
        //iterate over all LEGO bricks

        //the cpu COO version
        if(Inp.Accelerator == "CPU"){
            for(int j = 0; j < H_rank.nmbrLEGO; j++){
                H_rank.matrixVectorUpper(j,y,dydx_rank,FT);
            }
        }

        if(Inp.Accelerator == "MKL"){
        // the cpu MKL CSR version
            bp("Before Matrix Vector");
            H_rank.matrixVectorMKL(y,dydx_rank,FT);
            bp("After matrix Vector");
        }
      //sum the local dydx_rank into dydx on rank 0
        MPI_Reduce(dydx_rank.data(),dydx.data(),dim*2,MPI_DOUBLE,MPI_SUM, 0,MPI_COMM_WORLD);

        if(rank==0){
            d(i) = y.adjoint() * dydx;
               //Full reorthogonalization (x2):
            dydx = dydx - Q.block(0,0,dim,i+1) * ( Q.block(0,0,dim,i+1).adjoint() * dydx);
            dydx = dydx - Q.block(0,0,dim,i+1) * ( Q.block(0,0,dim,i+1).adjoint() * dydx);
            if (i < Inp.dim_kryl-1){
                od(i) = dydx.norm();
                if (abs(od(i)) == 0){
                    cout << "devision by 0 during renormalizing" <<endl;
                    break;
                }
                y = dydx/od(i);
                Q.col(i+1) = y;
                //adaptivly control the dimension of the Krylov space. At most we do dim_kryl
                //iterations if we converge earlier we terminate
                if((Q.col(i+1)-Q.col(i)).norm()  < Inp.eps){
                    for(int j = i+1; j < Inp.dim_kryl-1 ; j++){
                        //fill the remaining vectors
                        Q.col(j+1) = y;
                        d(j) = d(i);
                        od(j) = od(i);

                    }
                    d(Inp.dim_kryl-1)=d(i);
                    cout << "Krylov space converged after: " << i << " iterations\n";
                    break;

                }
            }
        }

    }
    //set the h Matrix mortens TT matrix
    for (int i = 1; i < Inp.dim_kryl; i++){
        h(i,i-1) = od(i-1);
        h(i-1,i) = od(i-1);
    }
    for (int i = 0; i < Inp.dim_kryl; i++){
        h(i,i) = d(i);
    }
   return;
}

void polarization(SpMatrix &M,int states_chan, int &ind, int index,int l,int lm_index_L,int lm_index_R,
        double LM_faktor,double dX,ArrayXd &weights, ArrayXd &x, ArrayXXd &psi_1,ArrayXXd
        &psi_2,ArrayXXd &d_psi_2){
    //Lego brick 1 + 2
#pragma omp parallel for collapse(2)
    for(int j =  states_chan_rank_begin; j < states_chan_rank_end; j++){
        for(int i = 0; i < states_chan; i++){
                M.writeElement(ind+j*states_chan+i,
                        lm_index_L*states_chan+j,
                        lm_index_R*states_chan+i,
                        index,
                        dX/2*(weights*psi_1.col(j)*psi_2.col(i)/x).sum()*LM_faktor*(l+1)
                        +dX/2*(weights*psi_1.col(j)*d_psi_2.col(i)).sum()*LM_faktor);
          }

    }

    //number of elements written on one rank over all threads
    ind = ind + (states_chan_rank_end-states_chan_rank_begin) * states_chan;
}

void helperEigenStates(InputParameter Inp, int states_chan,double dX,double &tmp_d,
        ArrayXd &weights, ArrayXd &x,ArrayXXd &B,ArrayXXd &diff_B,MatrixXd &H_base,
        MatrixXd &S){
    //This function should speed up the calculation
    //S and large aprts of H are always the same, by storing them and passing them to
    //getEigenStates redundant computation can be avoided
    //S and the kinetic Energy are the same for all l,

    int dim_H = Inp.N_b+Inp.n-3;
    for (int i = 0; i < dim_H ; i++){
        for (int j = 0; j < dim_H; j++){
            H_base(i,j) = - 0.5 * dX * 0.5 * (weights * B.col(i+1)*diff_B.col(j+1)).sum();
            ArrayXd tmp = dX * 0.5 * (weights * B.col(i+1)*B.col(j+1));
            H_base(i,j) += (tmp * -1/x).sum();
            S(i,j) = (tmp).sum();
        }
    }

}

void getEigenStates(InputParameter Inp, int l,int states_chan,double dX,double &tmp_d,ArrayXd &weights,
        ArrayXd &x,ArrayXXd &B, ArrayXXd &d_B,ArrayXXd &diff_B,ArrayXXd &psi,ArrayXXd &d_psi
        ,ArrayXXd &diff_psi , MatrixXd &H_base, MatrixXd &S, ArrayXd &Energy ){
    //The H and S matrix
    int dim_H = Inp.N_b+Inp.n-3;  //this equals to s_H
    MatrixXd H = MatrixXd::Zero(dim_H,dim_H);
    ArrayXd tmp_l = (l*(1+l)/(2*x*x));

//only works on rect loops
#pragma omp parallel for collapse(2)
    for (int i = 0; i < dim_H ; i++){
        for (int j = 0; j < dim_H; j++){
             //Kinetic Engergy
            H(i,j) = H_base(i,j);
            //potential Energy V
            ArrayXd tmp =  dX * 0.5 * weights * B.col(i+1)*B.col(j+1);
            H(i,j) += (tmp * tmp_l).sum();
       }
    }

    //Start timing the solver
    auto start2 = system_clock::now();
    //The Gen Eigenvalue solver on CPU
    MatrixXd Eigenvectors =  MatrixXd::Zero(dim_H,dim_H);
    if(Inp.Accelerator == "CPU" || Inp.Accelerator== "MKL")
    {
        GeneralizedSelfAdjointEigenSolver<MatrixXd> ges(H,S);
        Eigenvectors = ges.eigenvectors();
        Energy = ges.eigenvalues();
    }
   //Gen Eigenvalue solver on GPU
    //H,S are in coloum major, 0 index format
    //Cusolver assumes col-major no conversion needed
    //lda,ldb,m are all the same as dim W,V = Eigenvalues,Eigenvectors
    if(Inp.Accelerator == "GPU" || Inp.Accelerator =="GPUFull")
    {
        //VectorXd Eigenvalues =  VectorXd::Zero(dim_H);
        genEigenSolverCUDA(dim_H,H.data(),S.data(),Energy.data(),Eigenvectors.data());
    }

    auto stop2 = system_clock::now();
    auto duration2 = duration_cast<milliseconds>(stop2 - start2);
    if (rank==0){
        cout << "Time taken by the solver in l: "<< l << " : "  << (double) duration2.count()/1000
            << "seconds" << endl;}

    //set the psi to 0
    psi.setZero();
    d_psi.setZero();
    diff_psi.setZero();

    //watch(Energy);
    //watch(Eigenvectors);
#pragma omp parallel for collapse(2)
    for(int i = 0; i < states_chan; i++){
        for(int j=0; j < dim_H; j++){
            psi.col(i) = psi.col(i) + Eigenvectors(j,i) * B.col(j+1);
            d_psi.col(i) = d_psi.col(i) + Eigenvectors(j,i) * d_B.col(j+1);
            diff_psi.col(i) = diff_psi.col(i) + Eigenvectors(j,i) * diff_B.col(j+1);
       }
 }
    }


// -*- mode: c++; fill-column: 80; indent-tabs-mode: nil; -*-

#ifndef ARMA_LAPACK_EXTRA_HPP
#define ARMA_LAPACK_EXTRA_HPP

#include <armadillo>

#if !defined(ARMA_BLAS_CAPITALS)

#define arma_sbdsqr sbdsqr
#define arma_dbdsqr dbdsqr

/* Least-square problem */
#define arma_sgglse sgglse
#define arma_dgglse dgglse
#define arma_cgglse cgglse
#define arma_zgglse zgglse

/* QR factorization with column pivoting */
#define arma_sgeqp3 sgeqp3
#define arma_dgeqp3 dgeqp3
#define arma_cgeqp3 cgeqp3
#define arma_zgeqp3 zgeqp3

/* Eigenvalue decomposition of upper Hessenberg matrix */
#define arma_shseqr shseqr
#define arma_dhseqr dhseqr
#define arma_chseqr chseqr
#define arma_zhseqr zhseqr

/* Jacobi SVD */
#define arma_sgesvj sgesvj
#define arma_dgesvj dgesvj
#define arma_cgesvj cgesvj
#define arma_zgesvj zgesvj

/* Another Jacobi SVD */
#define arma_sgejsv sgejsv
#define arma_dgejsv dgejsv
#define arma_cgejsv cgejsv
#define arma_zgejsv zgejsv

#else

#define arma_sbdsqr SBDSQR
#define arma_dbdsqr DBDSQR

/* Least-square problem */
#define arma_sgglse SGGLSE
#define arma_dgglse DGGLSE
#define arma_cgglse CGGLSE
#define arma_zgglse ZGGLSE

/* QR factorization with column pivoting */
#define arma_sgeqp3 SGEQP3
#define arma_dgeqp3 DGEQP3
#define arma_cgeqp3 CGEQP3
#define arma_zgeqp3 ZGEQP3

/* Eigenvalue decomposition of upper Hessenberg matrix */
#define arma_shseqr SHSEQR
#define arma_dhseqr DHSEQR
#define arma_chseqr CHSEQR
#define arma_zhseqr ZHSEQR

/* Jacobi SVD */
#define arma_sgesvj SGESVJ
#define arma_dgesvj DGESVJ
#define arma_cgesvj CGESVJ
#define arma_zgesvj ZGESVJ

/* Another Jacobi SVD */
#define arma_sgejsv SGEJSV
#define arma_dgejsv DGEJSV
#define arma_cgejsv CGEJSV
#define arma_zgejsv ZGEJSV

#endif /* ARMA_BLAS_CAPITALS */

namespace arma
{

extern "C" {
// SVD of real bidiagonal matrix
void arma_fortran_noprefix(arma_sbdsqr)(char* uplo, blas_int* n, blas_int* ncvt,
                                        blas_int* nru, blas_int* ncc, float* d,
                                        float* e, float* vt, blas_int* ldvt,
                                        float* u, blas_int* ldu, float* c,
                                        blas_int* ldc, float* work,
                                        blas_int* info);

void arma_fortran_noprefix(arma_dbdsqr)(char* uplo, blas_int* n, blas_int* ncvt,
                                        blas_int* nru, blas_int* ncc, double* d,
                                        double* e, double* vt, blas_int* ldvt,
                                        double* u, blas_int* ldu, double* c,
                                        blas_int* ldc, double* work,
                                        blas_int* info);

// Solve generalized eigenvalue problem
void arma_fortran_noprefix(arma_sggev)(char* jobl, char* jobr, blas_int* n,
                                       float* a, blas_int* lda, float* b,
                                       blas_int* ldb, float* alphar,
                                       float* alphai, float* beta, float* vl,
                                       blas_int* ldvl, float* vr,
                                       blas_int* ldvr, float* work,
                                       blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_dggev)(char* jobl, char* jobr, blas_int* n,
                                       double* a, blas_int* lda, double* b,
                                       blas_int* ldb, double* alphar,
                                       double* alphai, double* beta, double* vl,
                                       blas_int* ldvl, double* vr,
                                       blas_int* ldvr, double* work,
                                       blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_cggev)(
    char* jobl, char* jobr, blas_int* n, void* a, blas_int* lda, void* b,
    blas_int* ldb, void* alpha, void* beta, void* vl, blas_int* ldvl, void* vr,
    blas_int* ldvr, void* work, blas_int* lwork, float* rwork, blas_int* info);

void arma_fortran_noprefix(arma_zggev)(
    char* jobl, char* jobr, blas_int* n, void* a, blas_int* lda, void* b,
    blas_int* ldb, void* alpha, void* beta, void* vl, blas_int* ldvl, void* vr,
    blas_int* ldvr, void* work, blas_int* lwork, double* rwork, blas_int* info);

// linear equality-constrained least squares problem (LSE)
void arma_fortran_noprefix(arma_sgglse)(blas_int* m, blas_int* n, blas_int* p,
                                        float* a, blas_int* lda, float* b,
                                        blas_int* ldb, float* c, float* d,
                                        float* x, float* work, blas_int* lwork,
                                        blas_int* info);

void arma_fortran_noprefix(arma_dgglse)(blas_int* m, blas_int* n, blas_int* p,
                                        double* a, blas_int* lda, double* b,
                                        blas_int* ldb, double* c, double* d,
                                        double* x, double* work,
                                        blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_cgglse)(blas_int* m, blas_int* n, blas_int* p,
                                        void* a, blas_int* lda, void* b,
                                        blas_int* ldb, void* c, void* d,
                                        void* x, void* work, blas_int* lwork,
                                        blas_int* info);

void arma_fortran_noprefix(arma_zgglse)(blas_int* m, blas_int* n, blas_int* p,
                                        void* a, blas_int* lda, void* b,
                                        blas_int* ldb, void* c, void* d,
                                        void* x, void* work, blas_int* lwork,
                                        blas_int* info);
// QR factorization with column pivoting
void arma_fortran_noprefix(arma_sgeqp3)(blas_int* m, blas_int* n, float* a,
                                        blas_int* lda, blas_int* jpiv,
                                        float* tau, float* work,
                                        blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_dgeqp3)(blas_int* m, blas_int* n, double* a,
                                        blas_int* lda, blas_int* jpiv,
                                        double* tau, double* work,
                                        blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_cgeqp3)(blas_int* m, blas_int* n, void* a,
                                        blas_int* lda, blas_int* jpiv,
                                        void* tau, void* work, blas_int* lwork,
                                        float* rwork, blas_int* info);
void arma_fortran_noprefix(arma_zgeqp3)(blas_int* m, blas_int* n, void* a,
                                        blas_int* lda, blas_int* jpiv,
                                        void* tau, void* work, blas_int* lwork,
                                        double* rwork, blas_int* info);

// xHSEQR --- Eigendecomposition of upper Hessenberg matrix by mutishift QR
void arma_fortran_noprefix(arma_shseqr)(char* job, char* compz, blas_int* n,
                                        blas_int* ilo, blas_int* ihi, float* h,
                                        blas_int* ldh, float* wr, float* wi,
                                        float* z, blas_int* ldz, float* work,
                                        blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_dhseqr)(char* job, char* compz, blas_int* n,
                                        blas_int* ilo, blas_int* ihi, double* h,
                                        blas_int* ldh, double* wr, double* wi,
                                        double* z, blas_int* ldz, double* work,
                                        blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_chseqr)(char* job, char* compz, blas_int* n,
                                        blas_int* ilo, blas_int* ihi, void* h,
                                        blas_int* ldh, void* w, void* z,
                                        blas_int* ldz, void* work,
                                        blas_int* lwork, blas_int* info);

void arma_fortran_noprefix(arma_zhseqr)(char* job, char* compz, blas_int* n,
                                        blas_int* ilo, blas_int* ihi, void* h,
                                        blas_int* ldh, void* w, void* z,
                                        blas_int* ldz, void* work,
                                        blas_int* lwork, blas_int* info);

// xGESVJ --- Jacobi SVD
void arma_fortran_noprefix(arma_sgesvj)(char* joba, char* jobu, char* jobv,
                                        blas_int* m, blas_int* n, float* a,
                                        blas_int* lda, float* sva, blas_int* mv,
                                        float* v, blas_int* ldv, float* work,
                                        blas_int* lwork, blas_int* info);
void arma_fortran_noprefix(arma_dgesvj)(char* joba, char* jobu, char* jobv,
                                        blas_int* m, blas_int* n, double* a,
                                        blas_int* lda, double* sva,
                                        blas_int* mv, double* v, blas_int* ldv,
                                        double* work, blas_int* lwork,
                                        blas_int* info);
void arma_fortran_noprefix(arma_cgesvj)(char* joba, char* jobu, char* jobv,
                                        blas_int* m, blas_int* n, void* a,
                                        blas_int* lda, float* sva, blas_int* mv,
                                        void* v, blas_int* ldv, void* cwork,
                                        blas_int* lwork, float* rwork,
                                        blas_int* lrwork, blas_int* info);
void arma_fortran_noprefix(arma_zgesvj)(char* joba, char* jobu, char* jobv,
                                        blas_int* m, blas_int* n, void* a,
                                        blas_int* lda, double* sva,
                                        blas_int* mv, void* v, blas_int* ldv,
                                        void* cwork, blas_int* lwork,
                                        double* rwork, blas_int* lrwork,
                                        blas_int* info);

// xGEJSV --- Jacobi SVD
void arma_fortran_noprefix(arma_sgejsv)(char* joba, char* jobu, char* jobv,
                                        char* jobr, char* jobt, char* jobp,
                                        blas_int* m, blas_int* n, float* a,
                                        blas_int* lda, float* sva, float* u,
                                        blas_int* ldu, float* v, blas_int* ldv,
                                        float* work, blas_int* lwork,
                                        blas_int* iwork, blas_int* info);

void arma_fortran_noprefix(arma_dgejsv)(char* joba, char* jobu, char* jobv,
                                        char* jobr, char* jobt, char* jobp,
                                        blas_int* m, blas_int* n, double* a,
                                        blas_int* lda, double* sva, double* u,
                                        blas_int* ldu, double* v, blas_int* ldv,
                                        double* work, blas_int* lwork,
                                        blas_int* iwork, blas_int* info);

void arma_fortran_noprefix(arma_cgejsv)(
    char* joba, char* jobu, char* jobv, char* jobr, char* jobt, char* jobp,
    blas_int* m, blas_int* n, void* a, blas_int* lda, float* sva, void* u,
    blas_int* ldu, void* v, blas_int* ldv, void* cwork, blas_int* lwork,
    float* rwork, blas_int* lrwork, blas_int* iwork, blas_int* info);

void arma_fortran_noprefix(arma_zgejsv)(
    char* joba, char* jobu, char* jobv, char* jobr, char* jobt, char* jobp,
    blas_int* m, blas_int* n, void* a, blas_int* lda, double* sva, void* u,
    blas_int* ldu, void* v, blas_int* ldv, void* cwork, blas_int* lwork,
    double* rwork, blas_int* lrwork, blas_int* iwork, blas_int* info);
}

namespace lapack
{

// xBDSQR
inline void bdsqr(char* uplo, blas_int* n, blas_int* ncvt, blas_int* nru,
                  blas_int* ncc, float* d, float* e, float* vt, blas_int* ldvt,
                  float* u, blas_int* ldu, float* c, blas_int* ldc, float* work,
                  blas_int* info)
{
    arma_fortran_noprefix(arma_sbdsqr)(uplo, n, ncvt, nru, ncc, d, e, vt, ldvt,
                                       u, ldu, c, ldc, work, info);
}

inline void bdsqr(char* uplo, blas_int* n, blas_int* ncvt, blas_int* nru,
                  blas_int* ncc, double* d, double* e, double* vt,
                  blas_int* ldvt, double* u, blas_int* ldu, double* c,
                  blas_int* ldc, double* work, blas_int* info)
{
    arma_fortran_noprefix(arma_dbdsqr)(uplo, n, ncvt, nru, ncc, d, e, vt, ldvt,
                                       u, ldu, c, ldc, work, info);
}

// xGGLSE
inline void gglse(blas_int* m, blas_int* n, blas_int* p, float* a,
                  blas_int* lda, float* b, blas_int* ldb, float* c, float* d,
                  float* x, float* work, blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_sgglse)(m, n, p, a, lda, b, ldb, c, d, x, work,
                                       lwork, info);
}

inline void gglse(blas_int* m, blas_int* n, blas_int* p, double* a,
                  blas_int* lda, double* b, blas_int* ldb, double* c, double* d,
                  double* x, double* work, blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_dgglse)(m, n, p, a, lda, b, ldb, c, d, x, work,
                                       lwork, info);
}

inline void gglse(blas_int* m, blas_int* n, blas_int* p, std::complex<float>* a,
                  blas_int* lda, std::complex<float>* b, blas_int* ldb,
                  std::complex<float>* c, std::complex<float>* d,
                  std::complex<float>* x, std::complex<float>* work,
                  blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_cgglse)(m, n, p, a, lda, b, ldb, c, d, x, work,
                                       lwork, info);
}

inline void gglse(blas_int* m, blas_int* n, blas_int* p,
                  std::complex<double>* a, blas_int* lda,
                  std::complex<double>* b, blas_int* ldb,
                  std::complex<double>* c, std::complex<double>* d,
                  std::complex<double>* x, std::complex<double>* work,
                  blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_zgglse)(m, n, p, a, lda, b, ldb, c, d, x, work,
                                       lwork, info);
}

// xGEQP3
inline void geqp3(blas_int* m, blas_int* n, float* a, blas_int* lda,
                  blas_int* jpiv, float* tau, float* work, blas_int* lwork,
                  blas_int* info)
{
    arma_fortran_noprefix(sgeqp3)(m, n, a, lda, jpiv, tau, work, lwork, info);
}

inline void geqp3(blas_int* m, blas_int* n, double* a, blas_int* lda,
                  blas_int* jpiv, double* tau, double* work, blas_int* lwork,
                  blas_int* info)
{
    arma_fortran_noprefix(dgeqp3)(m, n, a, lda, jpiv, tau, work, lwork, info);
}

inline void geqp3(blas_int* m, blas_int* n, std::complex<float>* a,
                  blas_int* lda, blas_int* jpiv, std::complex<float>* tau,
                  std::complex<float>* work, blas_int* lwork, float* rwork,
                  blas_int* info)
{
    arma_fortran_noprefix(cgeqp3)(m, n, a, lda, jpiv, tau, work, lwork, rwork,
                                  info);
}

inline void geqp3(blas_int* m, blas_int* n, std::complex<double>* a,
                  blas_int* lda, blas_int* jpiv, std::complex<double>* tau,
                  std::complex<double>* work, blas_int* lwork, double* rwork,
                  blas_int* info)
{
    arma_fortran_noprefix(zgeqp3)(m, n, a, lda, jpiv, tau, work, lwork, rwork,
                                  info);
}

// xHSEQR
inline void hseqr(char* job, char* compz, blas_int* n, blas_int* ilo,
                  blas_int* ihi, float* h, blas_int* ldh, float* wr, float* wi,
                  float* z, blas_int* ldz, float* work, blas_int* lwork,
                  blas_int* info)
{
    arma_fortran_noprefix(arma_shseqr)(job, compz, n, ilo, ihi, h, ldh, wr, wi,
                                       z, ldz, work, lwork, info);
}

inline void hseqr(char* job, char* compz, blas_int* n, blas_int* ilo,
                  blas_int* ihi, double* h, blas_int* ldh, double* wr,
                  double* wi, double* z, blas_int* ldz, double* work,
                  blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_dhseqr)(job, compz, n, ilo, ihi, h, ldh, wr, wi,
                                       z, ldz, work, lwork, info);
}

inline void hseqr(char* job, char* compz, blas_int* n, blas_int* ilo,
                  blas_int* ihi, std::complex<float>* h, blas_int* ldh,
                  std::complex<float>* w, std::complex<float>* z, blas_int* ldz,
                  std::complex<float>* work, blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_chseqr)(job, compz, n, ilo, ihi, h, ldh, w, z,
                                       ldz, work, lwork, info);
}

inline void hseqr(char* job, char* compz, blas_int* n, blas_int* ilo,
                  blas_int* ihi, std::complex<double>* h, blas_int* ldh,
                  std::complex<double>* w, std::complex<double>* z,
                  blas_int* ldz, std::complex<double>* work, blas_int* lwork,
                  blas_int* info)
{
    arma_fortran_noprefix(arma_zhseqr)(job, compz, n, ilo, ihi, h, ldh, w, z,
                                       ldz, work, lwork, info);
}

// Jacobi SVD
inline void gesvj(char* joba, char* jobu, char* jobv, blas_int* m, blas_int* n,
                  float* a, blas_int* lda, float* sva, blas_int* mv, float* v,
                  blas_int* ldv, float* work, blas_int* lwork, blas_int* info)
{
    arma_fortran_noprefix(arma_sgesvj)(joba, jobu, jobv, m, n, a, lda, sva, mv,
                                       v, ldv, work, lwork, info);
}

inline void gesvj(char* joba, char* jobu, char* jobv, blas_int* m, blas_int* n,
                  double* a, blas_int* lda, double* sva, blas_int* mv,
                  double* v, blas_int* ldv, double* work, blas_int* lwork,
                  blas_int* info)
{
    arma_fortran_noprefix(arma_dgesvj)(joba, jobu, jobv, m, n, a, lda, sva, mv,
                                       v, ldv, work, lwork, info);
}

inline void gesvj(char* joba, char* jobu, char* jobv, blas_int* m, blas_int* n,
                  std::complex<float>* a, blas_int* lda, float* sva,
                  blas_int* mv, std::complex<float>* v, blas_int* ldv,
                  std::complex<float>* cwork, blas_int* lwork, float* rwork,
                  blas_int* lrwork, blas_int* info)
{
    using complex_t = std::complex<float>;
    arma_fortran_noprefix(arma_cgesvj)(
        joba, jobu, jobv, m, n, (complex_t*)a, lda, sva, mv, (complex_t*)v, ldv,
        (complex_t*)cwork, lwork, rwork, lrwork, info);
}

inline void gesvj(char* joba, char* jobu, char* jobv, blas_int* m, blas_int* n,
                  std::complex<double>* a, blas_int* lda, double* sva,
                  blas_int* mv, std::complex<double>* v, blas_int* ldv,
                  std::complex<double>* cwork, blas_int* lwork, double* rwork,
                  blas_int* lrwork, blas_int* info)
{
    using complex_t = std::complex<float>;
    arma_fortran_noprefix(arma_zgesvj)(
        joba, jobu, jobv, m, n, (complex_t*)a, lda, sva, mv, (complex_t*)v, ldv,
        (complex_t*)cwork, lwork, rwork, lrwork, info);
}

// Another Jacobi SVD
inline void gejsv(char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
                  char* jobp, blas_int* m, blas_int* n, float* a, blas_int* lda,
                  float* sva, float* u, blas_int* ldu, float* v, blas_int* ldv,
                  float* work, blas_int* lwork, blas_int* iwork, blas_int* info)
{
    arma_fortran_noprefix(arma_sgejsv)(joba, jobu, jobv, jobr, jobt, jobp, m, n,
                                       a, lda, sva, u, ldu, v, ldv, work, lwork,
                                       iwork, info);
}

inline void gejsv(char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
                  char* jobp, blas_int* m, blas_int* n, double* a,
                  blas_int* lda, double* sva, double* u, blas_int* ldu,
                  double* v, blas_int* ldv, double* work, blas_int* lwork,
                  blas_int* iwork, blas_int* info)
{
    arma_fortran_noprefix(arma_dgejsv)(joba, jobu, jobv, jobr, jobt, jobp, m, n,
                                       a, lda, sva, u, ldu, v, ldv, work, lwork,
                                       iwork, info);
}

inline void gejsv(char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
                  char* jobp, blas_int* m, blas_int* n, std::complex<float>* a,
                  blas_int* lda, float* sva, std::complex<float>* u,
                  blas_int* ldu, std::complex<float>* v, blas_int* ldv,
                  std::complex<float>* cwork, blas_int* lwork, float* rwork,
                  blas_int* lrwork, blas_int* iwork, blas_int* info)
{
    arma_fortran_noprefix(arma_cgejsv)(joba, jobu, jobv, jobr, jobt, jobp, m, n,
                                       a, lda, sva, u, ldu, v, ldv, cwork,
                                       lwork, rwork, lrwork, iwork, info);
}

inline void gejsv(char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
                  char* jobp, blas_int* m, blas_int* n, std::complex<double>* a,
                  blas_int* lda, double* sva, std::complex<double>* u,
                  blas_int* ldu, std::complex<double>* v, blas_int* ldv,
                  std::complex<double>* cwork, blas_int* lwork, double* rwork,
                  blas_int* lrwork, blas_int* iwork, blas_int* info)
{
    arma_fortran_noprefix(arma_zgejsv)(joba, jobu, jobv, jobr, jobt, jobp, m, n,
                                       a, lda, sva, u, ldu, v, ldv, cwork,
                                       lwork, rwork, lrwork, iwork, info);
}

} // namespace: lapack
} // namespace: arma

#endif /* ARMA_LAPACK_EXTRA_HPP */