//Released under the MIT License - https://opensource.org/licenses/MIT
//
//Copyright (c) 2019 AIT Austrian Institute of Technology GmbH
//
//Permission is hereby granted, free of charge, to any person obtaining
//a copy of this software and associated documentation files (the "Software"),
//to deal in the Software without restriction, including without limitation
//the rights to use, copy, modify, merge, publish, distribute, sublicense,
//and/or sell copies of the Software, and to permit persons to whom the
//Software is furnished to do so, subject to the following conditions:
//
//The above copyright notice and this permission notice shall be included
//in all copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
//EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
//MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
//IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
//DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
//OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
//USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//Author: Josef Maier (josefjohann-dot-maier-at-gmail-dot-at)
/**********************************************************************************************************
 FILE: RectStructMot.cpp

 PLATFORM: Windows 7, MS Visual Studio 2010, OpenCV 2.4.2

 CODE: C++

 AUTOR: Josef Maier, AIT Austrian Institute of Technology

 DATE: July 2015

 LOCATION: TechGate Vienna, Donau-City-Strasse 1, 1220 Vienna

 VERSION: 1.0

 DISCRIPTION: This file provides functions for calculating the extrinsic camera parameters R & t based
			  on multiple homography alignment.
**********************************************************************************************************/

#include "HomographyAlignment.h"
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include "poselib/pose_helper.h"

using namespace std;
using namespace cv;

/* --------------------- Functions --------------------- */

/* Estimation of R & t based on multi homography alignment.
 *
 * vector<pair<Mat,Mat>> inl_points		Input  -> Vector containing the correspondences of the different planes in the camera
 *												  coordinate system. Each vector element contains the correspondeces of one
 *												  plane where the first points are coordinates of the left camera and the second
 *												  points are coordinates of the right camera. The first vector element must contain
 *												  the largest correspondence set for the dominatest plane in the images.
 * vector<Mat> Hs						Input  -> Homographies of the planes in the camera coordinate system. The vector-ordering
 *												  of the homographys must be in the same order than their correspondences in
 *												  inl_points.
 * vector<unsigned int> num_inl			Input  -> Number of inliers (correspondences) for each plane. The vector-ordering
 *												  must be in the same order than Hs.
 * Mat R1_2								Input & Output -> If a rotation matrix is provided, the homography alignment is initialized
 *														  with this rotation and with t1_2 (in this case both, R1_2 & t1_2 have to
 *														  be provided. Be careful to use the right rotation matrix and not its
 *														  inverse R1_2.t(). If no initialization should be performed R1_2 must be empty.
 *														  The resulting rotation matrix after homography alignment is stored in R1_2.
 * Mat t1_2								Input & Output -> If a translation vector is provided, the homography alignment is initialized
 *														  with this translation and with R1_2 (in this case both, R1_2 & t1_2 have to
 *														  be provided. Be careful to use the right rotation matrix and not its
 *														  inverse -t1_2. If no initialization should be performed t1_2 must be empty.
 *														  The resulting translation vector after homography alignment is stored in t1_2.
 * double tol							Input  -> Inlier threshold in the camera coordinate system.
 * Mat N								Output -> The resulting plane normal vectors after refinement in the same order than Hs.
 * vector<Mat> Hs_out					Output -> The refined homography matrices in the same order than Hs.
 *
 * Return value:						1 :		Everything ok
 *										0 :		Homography alignment not possible/failed
 */
int ComputeHomographyMotion(const std::vector<std::pair<cv::Mat,cv::Mat>>& inl_points,
							std::vector<cv::Mat> Hs,
							const std::vector<unsigned int>& num_inl,
							cv::Mat & R1_2,
							cv::Mat & t1_2,
							double tol,
							cv::Mat & N,
							std::vector<cv::Mat> Hs_out)
{
	Mat norms, homo, rot2_1;
	vector<Mat> rot_b2, dt_b2, norm2, inv_rot_b2;
	int num_planes;

	int i;
	double error[2];

	num_planes = (int)num_inl.size();

	norms = Mat(3,num_planes,CV_64F);

	if(num_planes > 1)
	{
		//rot is rotation from frame 1 to frame 2
		if(!t1_2.empty() && cv::norm(t1_2) > 0.0)
		{
			if(!R1_2.empty())
				rot2_1 = R1_2.t();
            Homographys_Alignment_initial_rotation(inl_points, num_inl, Hs[0], homo, rot2_1, t1_2, norms, tol);
		}
		else
		{
            Homographys_Alignment(inl_points, num_inl, Hs[0], homo, rot2_1, t1_2, norms, tol);
		}
		R1_2 = rot2_1.t();
		norms.col(0).copyTo(N);
		for(i = 0; i < num_planes; ++i)
		{
            construct_analytic_homography(rot2_1, t1_2, norms.col(i), homo);
			Hs_out.push_back(homo.clone());
		}
	}
	else if(num_planes == 1)
	{
		if(t1_2.empty() || cv::norm(t1_2) < 1e-6)
			return 0;

        Longuet_Higgins_Solution(Hs[0], rot_b2, dt_b2, norm2);
		inv_rot_b2.push_back(rot_b2[0].t());
		inv_rot_b2.push_back(rot_b2[1].t());
		error[0] = 0.0;
		error[1] = 0.0;

		error[0] = t1_2.dot(dt_b2[0]);
		error[1] = t1_2.dot(dt_b2[1]);
		if(error[0] > error[1])
		{
			R1_2 = rot_b2[0].t();
			dt_b2[0].copyTo(t1_2);
			norm2[0].copyTo(N);
		}
		else
		{
			R1_2 = rot_b2[1].t();
			dt_b2[1].copyTo(t1_2);
			norm2[1].copyTo(N);
		}
	}
	else
	{
		//PRT_MSG("there is no plane in this data set")
		return 0;
	}

//#define  CHECK_REPROJECTION_ERROR
#ifdef CHECK_REPROJECTION_ERROR
  double *h_a = (double*)Hs_out[0].data;
  Mat test_pts2, p_diff;
  for(j = 0; j < (int)num_inl[0]; ++j)
  {
    double op[2];
	test_pts2 = Mat(2,1,CV_64F,op);
	double *test_pts = (double*)inl_points[0].first.row(j).data;
	homography_transfer_33D(h_a, test_pts, op);
	p_diff = test_pts2 - inl_points[0].first.row(j).t();
	cout << "reprojection error " << p_diff.at<double>(0) << ", " << p_diff.at<double>(1) << endl;
  }
#endif

  return 1;//*numm;
}

/* Estimation of R & t based on multi homography alignment without initialization.
 *
 * vector<pair<Mat,Mat>> inl_points		Input  -> Vector containing the correspondences of the different planes in the camera
 *												  coordinate system. Each vector element contains the correspondeces of one
 *												  plane where the first points are coordinates of the left camera and the second
 *												  points are coordinates of the right camera. The first vector element must contain
 *												  the largest correspondence set for the dominatest plane in the images.
 * vector<unsigned int> num_inl			Input  -> Number of inliers (correspondences) for each plane. The vector-ordering
 *												  must be in the same order than inl_points.
 * InputArray H							Input  -> Homography corresponding to the first entry (correspondences) of inl_points. This
 *												  homography should have the largest support set of correspondences.
 * Mat homo								Output -> The refined input homography H after homography alignment.
 * Mat R2_1								Output -> The resulting rotation matrix from camera 1 to camera 2.
 * Mat t1_2								Output -> The resulting translation vector from camera 2 to camera 1.
 * Mat & norms							Output -> The resulting plane normal vectors after refinement for all planes.
 * double tol							Input  -> Inlier threshold in the camera coordinate system.
 *
 * Return value:						1 :		Everything ok
 *										0 :		Homography alignment not possible/failed
 */
#ifndef MAX_ITERATION
#define MAX_ITERATION 40
//estimated rotation is from camera 1 to camera 2
//estimate translation if the camera 2 in camera 1 frame
int  Homographys_Alignment(std::vector<std::pair<cv::Mat,cv::Mat>> inl_points,
                           std::vector<unsigned int> num_inl,
                           cv::InputArray H,
                           cv::Mat & homo,
                           cv::Mat & R2_1,
                           cv::Mat & t1_2,
                           cv::Mat & norms,
                           double tol)
{

	Mat Hm = H.getMat();
	std::vector<Mat> rot_b2, dt_b2, norm2, base_rt2, rot2, homo2, t2/*, rt2*/;
	Mat dn, dn2, h0, rt0, rot_b, dt_b, norm_b, _hh;
	rot2.emplace_back(3,3,CV_64F);
	rot2.emplace_back(Mat(3,3,CV_64F));
	homo2.emplace_back(Mat(3,3,CV_64F));
	homo2.emplace_back(Mat(3,3,CV_64F));
	t2.emplace_back(Mat(3,1,CV_64F));
	t2.emplace_back(Mat(3,1,CV_64F));
	base_rt2.emplace_back(Mat(3,1,CV_64F));
	base_rt2.emplace_back(Mat(3,1,CV_64F));
	int i ;
	int iter;
	double hh[3][3];
	double e1, e2;
	int nump;
	int q;
	int iter1;

	double errors[2];
	double s1;

	int num_patches;
	num_patches = (int)num_inl.size();
	_hh = Mat(3,3,CV_64F,hh);

	nump = 0;
	for(i = 0; i < num_patches; ++i)
	{
		nump += (int)num_inl[i];
	}

	dn = Mat(num_patches,3,CV_64F);
	dn2 = Mat(2*num_patches,3,CV_64F);

	Hm.copyTo(h0);

    Longuet_Higgins_Solution(Hm, rot_b2, dt_b2, norm2);
	errors[0] = Check_motion_error(inl_points, num_inl, rot_b2[0], dt_b2[0]);
	errors[1] = Check_motion_error(inl_points, num_inl, rot_b2[1], dt_b2[1]);

	dn.row(0) = Mat::zeros(1,3,CV_64F);

	base_rt2[0] = rot_b2[0] * dt_b2[0];
	base_rt2[1] = rot_b2[1] * dt_b2[1];

	//q is the index of which solution is used for HA
	for(q = 0; q < 2; ++q)
	{
		Hm.copyTo(h0);
		base_rt2[q].copyTo(rt0);
		rot_b2[q].copyTo(rot_b);
		dt_b2[q].copyTo(dt_b);
		e2 = 100000.0;
		for(iter1 = 0; iter1 < 4; ++iter1)
		{
			//this is to update the h0 and dn
			for(iter = 0 ; iter < MAX_ITERATION; ++iter)
			{
				dn.row(0) = Mat::zeros(1,3,CV_64F);

				for(i = 1; i < num_patches; ++i)
				{
					cv::Mat tmp = dn.row(i);
					update_dn(inl_points[i].first, inl_points[i].second, (int)num_inl[i], h0, rt0, tmp);
				}

				update_h0_rt(inl_points,num_inl,h0,dn,rt0);
				e1 = check_error(inl_points, num_inl, h0, dn, rt0);

				if((fabs(e1-e2) < 0.00001 || e1 < tol)&& iter > 2)
				{
					break;
				}
				else
				{
					e2 = e1;
				}
				//update k
				//compute the total error
			}

            Longuet_Higgins_Solution_with_initial(h0, rot_b, dt_b, norm_b);
			rt0 = rot_b * dt_b;

			e1 = check_error(inl_points, num_inl, h0, dn, rt0);

			if((fabs(e1-e2) < 0.000001 || e1 < tol)&& iter > 2)
			{
				break;
			}
			else
			{
				e2 = e1;
			}
			//update k
			//compute the total error
		}

		rot_b.copyTo(rot2[q]);
		norm_b.copyTo(norm2[q]);
		h0.copyTo(homo2[q]);
		dt_b.copyTo(t2[q]);

		errors[q] = e1;
		for(i = 0; i < num_patches; ++i)
		{
			dn.row(i).copyTo(dn2.row(q*num_patches + i));//keep the old dn for the future uses
		}
	}

	//Longuet_Higgins_Solution(h0, rot_b2, dt_b2, norm2);
	errors[0] = Check_motion_error(inl_points, num_inl, rot2[0], t2[0]);
	errors[1] = Check_motion_error(inl_points, num_inl, rot2[1], t2[1]);

	if(errors[0] < errors[1])
	{
		norm2[0].copyTo(norms.col(0));
		rot2[0].copyTo(R2_1);
		homo2[0].copyTo(homo);
		t2[0].copyTo(t1_2);
	}
	else
	{
		norm2[1].copyTo(norms.col(0));
		rot2[1].copyTo(R2_1);
		homo2[1].copyTo(homo);
		t2[1].copyTo(t1_2);
	}
	dn2.rowRange(0, num_patches).copyTo(dn);

	//compute the scale factor of the first homography and mulple it to rest dns
	for(int j = 0; j < 3; j++)
	{
		for(int j1 = 0; j1 < 3; j1++)
		{
			hh[j][j1] = t1_2.at<double>(j) * norms.at<double>(j1,0);
		}
	}
	h0 = Mat::eye(3,3,CV_64F);
	_hh = h0 - _hh;
	h0 = R2_1 * _hh;
	s1 = h0.at<double>(2,2);

	// update the dn and surface normal vectors

	for(i = 1; i < num_patches; ++i)
	{
		dn.row(i) = s1 * dn.row(i);
		norms.col(i) = norms.col(0) - dn.row(i).t();
	}

	return 1;
}
#undef MAX_ITERATION
#endif

/* Estimation of R & t based on multi homography alignment with initialization.
 *
 * vector<pair<Mat,Mat>> inl_points		Input  -> Vector containing the correspondences of the different planes in the camera
 *												  coordinate system. Each vector element contains the correspondeces of one
 *												  plane where the first points are coordinates of the left camera and the second
 *												  points are coordinates of the right camera. The first vector element must contain
 *												  the largest correspondence set for the dominatest plane in the images.
 * vector<unsigned int> num_inl			Input  -> Number of inliers (correspondences) for each plane. The vector-ordering
 *												  must be in the same order than inl_points.
 * InputArray H							Input  -> Homography corresponding to the first entry (correspondences) of inl_points. This
 *												  homography should have the largest support set of correspondences.
 * Mat homo								Output -> The refined input homography H after homography alignment.
 * Mat R2_1								Input & Output -> The homography alignment is initialized with this rotation. The resulting
 *														  rotation matrix from camera 1 to camera 2 after homography alignment is
 *														  copied back to R2_1.
 * Mat t1_2								Output -> The resulting translation vector from camera 2 to camera 1.
 * Mat & norms							Output -> The resulting plane normal vectors after refinement for all planes.
 * double tol							Input  -> Inlier threshold in the camera coordinate system.
 *
 * Return value:						1 :		Everything ok
 *										0 :		Homography alignment not possible/failed
 */
#ifndef MAX_ITERATION
#define MAX_ITERATION 30

int Homographys_Alignment_initial_rotation(std::vector<std::pair<cv::Mat,cv::Mat>> inl_points,
                                           std::vector<unsigned int> num_inl,
                                           cv::InputArray H,
                                           cv::Mat & homo,
                                           cv::Mat & R2_1,
                                           cv::Mat & t1_2,
                                           cv::Mat & norms,
                                           double tol)
{
	Mat h0, Hm, R, dR, rot_b, dn, dn2, rt0, rt00, dt_b, norm_b, _hh;
	std::vector<Mat> rot_b2, dt_b2, norm2, base_rt2, rot2, homo2, t2;
    Hm = H.getMat();
	base_rt2.emplace_back(3,1,CV_64F);
	base_rt2.emplace_back(Mat(3,1,CV_64F));
	rot2.emplace_back(Mat(3,3,CV_64F));
	rot2.emplace_back(Mat(3,3,CV_64F));
	homo2.emplace_back(Mat(3,3,CV_64F));
	homo2.emplace_back(Mat(3,3,CV_64F));
	t2.emplace_back(Mat(3,1,CV_64F));
	t2.emplace_back(Mat(3,1,CV_64F));
	int i ;
	int iter;
	double hh[3][3];
	double e1, e2;

	int nump;
	int q;
	int iter1;
	double errors[2];
	double s1;
	int num_patches;
	num_patches = (int)num_inl.size();
	_hh = Mat(3,3,CV_64F,hh);
	nump = 0;
	for(i = 0; i < num_patches; ++i)
	{
		nump += (int)num_inl[i];
	}

	dn = Mat(num_patches,3,CV_64F);
	dn2 = Mat(2*num_patches,3,CV_64F);

	Hm.copyTo(h0);

    Longuet_Higgins_Solution(h0, rot_b2, dt_b2, norm2);
	R = R2_1.t();
	dR = R * rot_b2[0];
	Eigen::Matrix3d rot_e;
	Eigen::Vector4d quat_e;
	Eigen::Vector3d axis_e;
	cv2eigen(dR, rot_e);
	poselib::MatToQuat(rot_e, quat_e);
	poselib::QuatToAxisAngle(quat_e, axis_e, errors[0]);
	dR = R * rot_b2[1];
	cv2eigen(dR, rot_e);
	poselib::MatToQuat(rot_e, quat_e);
	poselib::QuatToAxisAngle(quat_e, axis_e, errors[1]);

	//	there are two solutions here
	//  pick up this solution, in which, the estimated surface faces more or less to the first camera
	//  because the camera 1 optical point is 0 0 1, therefore the larger abs(n[2]) one is the real solution

	if(errors[0] < errors[1])
	{
		base_rt2[0] = rot_b2[0] * dt_b2[0];
		base_rt2[1] = rot_b2[1] * dt_b2[1];
		rot_b2[0].copyTo(rot_b);
		dt_b2[0].copyTo(dt_b);
		norm2[0].copyTo(norm_b);
	}
	else
	{
		base_rt2[0] = rot_b2[1] * dt_b2[1];
		base_rt2[1] = rot_b2[0] * dt_b2[0];
		rot_b2[1].copyTo(rot_b);
		dt_b2[1].copyTo(dt_b);
		norm2[1].copyTo(norm_b);
	}
	dn.row(0) = Mat::zeros(1,3,CV_64F);

	//q is the index of which solution is used for HA
	for(q = 0; q < 1; ++q)
	{
		Hm.copyTo(h0);
		base_rt2[q].copyTo(rt0);
		rt0.copyTo(rt00);
		e2 = 100000.0;
		for(iter1 = 0; iter1 < 4; ++iter1)
		{
			for(iter = 0 ; iter < MAX_ITERATION; ++iter)
			{
				dn.row(0) = Mat::zeros(1,3,CV_64F);

				for(i = 1; i < num_patches; ++i)
				{
					cv::Mat tmp = dn.row(i);
					update_dn(inl_points[i].first, inl_points[i].second, (int)num_inl[i], h0, rt0, tmp);
				}

				update_h0_rt(inl_points,num_inl,h0,dn,rt0);
				e1 = check_error(inl_points, num_inl, h0, dn, rt0);

				if((fabs(e1-e2) < 0.00001 || e1 < tol)&& iter > 2)
				{
					break;
				}
				else
				{
					e2 = e1;
				}
				//update k
				//compute the total error
			}
			//update rt0
            Longuet_Higgins_Solution_with_initial(h0, rot_b, dt_b, norm_b);

			//this may no tbe the right solution
			rt0 = rot_b * dt_b;
			rot_b.copyTo(rot2[q]);
			norm_b.copyTo(norm2[q]);
			h0.copyTo(homo2[q]);
			dt_b.copyTo(t2[q]);

			e1 = check_error(inl_points, num_inl, h0, dn, rt0);
			if((fabs(e1-e2) < 0.000001 || e1 < tol)&& iter > 2)
			{
				break;
			}
			else
			{
				e2 = e1;
			}
			//update k
			//compute the total error
		}
		errors[q] = e1;
		for(i = 0; i < num_patches; ++i)
		{
			dn.row(i).copyTo(dn2.row(q*num_patches + i));//keep the old dn for the future uses
		}
	}

	norm2[0].copyTo(norms.col(0));
	rot2[0].copyTo(R2_1);
	homo2[0].copyTo(homo);
	t2[0].copyTo(t1_2);

	dn2.rowRange(0, num_patches).copyTo(dn);

	//compute the scale factor of the first homography and mulple it to rest dns
	for(size_t j = 0; j < 3; j++)
	{
		for(size_t j1 = 0; j1 < 3; j1++)
		{
			hh[j][j1] = t1_2.at<double>(j) * norms.at<double>(j1,0);
		}
	}
	h0 = Mat::eye(3,3,CV_64F);
	_hh = h0 - _hh;
	h0 = R2_1 * _hh;
	s1 = h0.at<double>(2,2);

	// update the dn and surface normal vectors

	for(i = 1; i < num_patches; ++i)
	{
		dn.row(i) = s1 * dn.row(i);
		norms.col(i) = norms.col(0) - dn.row(i).t();
	}

	return 1;
}
#undef MAX_ITERATION
#endif

/* Estimation of R, t & the plane normal vector (two possible solutions) based on the algorithm from Longuet-Higgine "A computer
 * algorithm for reconstructing a scene from two projections", 1981. These parameters are estimted from a homography H and its
 * inliers (in the camera coordinate system).
 *
 * InputArray H							Input  -> Homography between images and a world plane in the camera coordinate system.
 * vector<Mat> R1_2						Output -> Two possible solutions for the rotation matrix.
 * vector<Mat> dt2in1					Output -> Two possible solutions for the translation vector.
 * vector<Mat> norm						Output -> Two possible solutions for the plane normal vector.
 *
 * Return value:						1 :		Everything ok
 *										0 :		Estimation not possible/failed
 */
int Longuet_Higgins_Solution(cv::InputArray H, std::vector<cv::Mat> & R1_2, std::vector<cv::Mat> & dt2in1, std::vector<cv::Mat> & norm)
{
    Mat Hm = H.getMat();
	if(!R1_2.empty())
		R1_2.clear();

	{
		Mat tmp = Mat(3,3,CV_64F);
		R1_2.push_back(tmp.clone());
		R1_2.push_back(tmp.clone());
	}
	if(!dt2in1.empty())
		dt2in1.clear();

	{
		Mat tmp = Mat(3,1,CV_64F);
		dt2in1.push_back(tmp.clone());
		dt2in1.push_back(tmp.clone());
	}
	if(!norm.empty())
		norm.clear();

	{
		Mat tmp = Mat(3,1,CV_64F);
		norm.push_back(tmp.clone());
		norm.push_back(tmp.clone());
	}

	double hh[3][3], d[3], u[3][3], n[3], check, d2;
    double p[3], t[3], tn[3][3];
    double ts[3];
    double ps[3] ;
    double check1;
    int s1, s2, iter, k, i;

	Mat ht, ut, h_norm, x, r, _ts, _n, trans, plane, invtn, rot;
	Mat _tn = Mat(3,3,CV_64F,tn);
	Mat _hh = Mat(3,3,CV_64F,hh);
	Mat _u = Mat(3,3,CV_64F,u);
	ht = Hm.t();
	_hh = ht * Hm;
    //u here is the Ut on the right side
    jacobi_mat_33(hh, d, u, &iter);
	ut = _u.t();

	_hh = d[1] * _hh;
    d2 = 1.0/sqrt(d[1]);
	h_norm = d2 * Hm;//the homo_norm is the homo transform with
    d[0] = d[0]/d[1];
    d[2] = d[2]/d[1];
    d[1] = 1.0;

    if(d[0] - d[1] < 0.000001 && d[1]- d[2] < 0.000001)
    {
        // the two images are too close each other
		dt2in1[0] = Mat::zeros(3,1,CV_64F);
		dt2in1[1] = Mat::zeros(3,1,CV_64F);
        //the homography degerate to a rotation
		h_norm.copyTo(R1_2[0]);
		h_norm.copyTo(R1_2[1]);

        //the surface normal cannot be estimated here
		norm[0] = Mat::zeros(3,1,CV_64F);
		norm[1] = Mat::zeros(3,1,CV_64F);
        return 1;
    }

    if(d[0] > 1.0 && d[2] <= 1.0)
    {
        t[0] = sqrt((d[0] - 1.0)*d[2]/(d[0] - d[2]));
        t[1] = 0.0;
        t[2] = sqrt((1.0 - d[2])*d[0]/(d[0] - d[2]));
        p[0] = sqrt((d[0] - 1.0)*d[0]/(d[0] - d[2]));
        p[1] = 0.0;
        p[2] = sqrt((1.0 - d[2])*d[2]/(d[0] - d[2]));
    }
    else
    {
        return 0;
    }
	x = Mat::zeros(3,1,CV_64F);
	x.at<double>(2) = 1.0;
    k = 0;
	r = _u * x;
	_ts = Mat(3,1,CV_64F,ts);
	_ts = Mat::zeros(3,1,CV_64F);
	_n = Mat(3,1,CV_64F,n);

    for(s1 =-1; s1 <=1; s1 +=2)
    {
        for(s2 =-1; s2 <=1; s2 +=2)
        {
		    ps[0] = -s1*p[0];
			ps[1] = 0.0;
			ps[2] = -s2*p[2];
		    n[0] = s1*t[0] + ps[0];
		    n[1] = 0.0;
		    n[2] = s2*t[2] + ps[2];
			ts[0] = s1*t[0];
			ts[1] = 0.0;
			ts[2] = s2*t[2];

			trans = _u * _ts;
			plane = _u * _n;
			check1 = _n.dot(r);
			check = trans.dot(plane) - 1.0;

			if(plane.at<double>(2) > 0.0 )
			{
				trans.copyTo(dt2in1[k]);
				plane.copyTo(norm[k]);
				k++;
			}
         }
    }

    for(i = 0; i < k; ++i)
    {
		for(size_t j = 0; j < 3; j++)
		{
			for(size_t j1 = 0; j1 < 3; j1++)
			{
				tn[j][j1] = dt2in1[i].at<double>(j) * norm[i].at<double>(j1);
			}
		}

		tn[0][0] -=1.0;
		tn[1][1] -=1.0;
		tn[2][2] -=1.0;
		_tn = -1.0 * _tn;
		invtn = _tn.inv();
        rot = h_norm * invtn;
		if(determinant(rot) < 0.0)
		{
			rot = -1.0 * rot;
		}
		rot.copyTo(R1_2[i]);
    }

    return 1;
}

/* Estimation of R, t & the plane normal vector (two possible solutions) based on the algorithm from Longuet-Higgine "A computer
 * algorithm for reconstructing a scene from two projections", 1981. These parameters are estimted from a homography H and its
 * inliers (in the camera coordinate system).
 *
 * InputArray H							Input  -> Homography between images and a world plane in the camera coordinate system.
 * vector<Mat> R1_2						Output -> The resulting rotation matrix.
 * vector<Mat> dt1						Input & Output -> As input an initial translation vector has to be specified. The resulting
 *														  translation vector is copied back to dt1.
 * vector<Mat> norm						Output -> The resulting plane normal vector.
 *
 * Return value:						1 :		Everything ok
 *										0 :		Estimation not possible/failed
 */
int Longuet_Higgins_Solution_with_initial(cv::InputArray H, cv::Mat & R2_1, cv::Mat & dt1, cv::Mat & norm1)
{
    Mat Hm = H.getMat();
	double hh[3][3], d[3], u[3][3], n[3], check, d2;
    double p[3], t[3], tn[3][3];

    double ts[3];
    double ps[3] ;

    double check1;

    int s1, s2, iter, i;

	Mat ht, h_norm, rot, x, r, _ts, trans, plane, _ps, _n, invtn;
	vector<Mat> dt, norm;
	Mat _tn = Mat(3,3,CV_64F,tn);
	Mat _hh = Mat(3,3,CV_64F,hh);
	Mat _u = Mat(3,3,CV_64F,u);

	ht = Hm.t();
	_hh = ht * Hm;
    //u here is the Ut on the right side
    jacobi_mat_33(hh, d, u, &iter);

    //if d[1] != 1.0 make a correction
	_hh = -1.0 * _hh;
    d2 = 1.0/sqrt(d[1]);
	h_norm = d2 * Hm;//the homo_norm is the homo transform with
    d[0] = d[0]/d[1];
    d[2] = d[2]/d[1];
    d[1] = 1.0;
    if(d[0] - d[1] < 0.000001 && d[1]- d[2] < 0.000001)
    {
        // the two images are too close each other
		dt1 = Mat::zeros(3,1,CV_64F);
        //the homography degerate to a rotation
		h_norm.copyTo(rot);
		h_norm.copyTo(R2_1);

        //the surface normal cannot be estimated here
		norm1 = Mat::zeros(3,1,CV_64F);
        return 1;
    }

    if(d[0] > 1.0 && d[2] <= 1.0)
    {
        t[0] = sqrt((d[0] - 1.0)*d[2]/(d[0] - d[2]));
        t[1] = 0.0;
        t[2] = sqrt((1.0 - d[2])*d[0]/(d[0] - d[2]));
        p[0] = sqrt((d[0] - 1.0)*d[0]/(d[0] - d[2]));
        p[1] = 0.0;
        p[2] = sqrt((1.0 - d[2])*d[2]/(d[0] - d[2]));
    }
    else
    {
        return 0;
    }

	x = Mat::zeros(3,1,CV_64F);
	x.at<double>(2) = 1.0;
	r = _u * x;
	_ts = Mat(3,1,CV_64F,ts);
	_ts = Mat::zeros(3,1,CV_64F);
	_n = Mat(3,1,CV_64F,n);

    for(s1 =-1; s1 <=1; s1 +=2)
    {
        for(s2 =-1; s2 <=1; s2 +=2)
        {
		    ps[0] = -s1*p[0];
			ps[1] = 0.0;
			ps[2] = -s2*p[2];
		    n[0] = s1*t[0] + ps[0];
		    n[1] = 0.0;
		    n[2] = s2*t[2] + ps[2];
			ts[0] = s1*t[0];
			ts[1] = 0.0;
			ts[2] = s2*t[2];

			trans = _u * _ts;
			plane = _u * _n;
			check1 = _n.dot(r);
			check = trans.dot(plane) - 1.0;

			if(plane.at<double>(2) > 0.0 )
			{
				dt.push_back(trans.clone());
				norm.push_back(plane.clone());
			}
        }
    }
	if(dt[0].dot(dt1) > dt[1].dot(dt1)){
		i = 0;
		dt[0].copyTo(dt1);
		norm[0].copyTo(norm1);
    }else{
	    i = 1;
		dt[1].copyTo(dt1);
		norm[1].copyTo(norm1);
    }

	for(size_t j = 0; j < 3; j++)
	{
		for(size_t j1 = 0; j1 < 3; j1++)
		{
			tn[j][j1] = dt[i].at<double>(j) * norm[i].at<double>(j1);
		}
	}
    tn[0][0] -=1.0;
    tn[1][1] -=1.0;
    tn[2][2] -=1.0;
	_tn = -1.0 * _tn;
	invtn = _tn.inv();
	rot = h_norm * invtn;

    if(determinant(rot) < 0.0)
    {
		rot = -1.0 * rot;
    }
    rot.copyTo(R2_1);

    return 1;
}

/* Diagonalization of the matrix W=(H^T)H with the unknown orthogonal matrix U (where H is a homography) to solve UWU^T=Diag(d1, d2, d3)
 * as described in the paper "Real-time Surface Estimation by Homography Alignment for Spacecraft Safe Landing" from Yang Cheng
 * in 2010. The input to this function is a=W=(H^T)H. The output are the 3 diagonal elemnts d and the matrix v=U^T.
 *
 * double a[3][3]						Input  -> Matrix W=(H^T)H, where H is a homography matrix
 * double d[3]							Output -> The resulting diagonal entries of the diagonal matrix Diag(d1, d2, d3)
 * double v[3][3]						Output -> The resulting orthogonal matrix U^T
 * int *nrot							Output -> Number of iterations needed for estimating d and v
 *
 * Return value:						1 :		Everything ok
 *										0 :		Estimation failed (max. number of iterations exceeded)
 */
#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
	a[k][l]=h+s*(g-h*tau);

int jacobi_mat_33(double a[3][3], double *d, double v[3][3], int *nrot)
{
	int j,iq,ip,i;
	double tresh,theta,tau,t,sm,s,h,g,c, b[3],z[3];
    double dt[3][3], abc[3][3], tmp[3][3], tmp1[3][3];
	memcpy(abc,a,9*sizeof(double));

	for (ip=0;ip<3;ip++) {
		for (iq=0;iq<3;iq++) v[ip][iq]=0.0;
		v[ip][ip]=1.0;
	}
	for (ip=0;ip< 3;ip++) {
		b[ip]=d[ip]=a[ip][ip];
		z[ip]=0.0;
	}
	*nrot=0;
	for (i=1;i<=50;i++) {
		sm=0.0;
		for (ip=0;ip<2;ip++) {
			for (iq=ip+1;iq< 3;iq++)
				sm += fabs(a[ip][iq]);
		}
		if (sm < 0.000001) {
			if(d[1] > d[0])
			{
				c = d[0];
				d[0] = d[1];
				d[1] = c;
				memset(dt,0,9*sizeof(double));
				dt[0][1] = 1.0;
				dt[1][0] = 1.0;
				dt[2][2] = 1.0;

				Mat _v = Mat(3,3,CV_64F,v);
				Mat _dt = Mat(3,3,CV_64F,dt);
				Mat _tmp = Mat(3,3,CV_64F,tmp);
				_tmp = _v * _dt;

				memcpy(v,tmp,9*sizeof(double));
			}
			if(d[2] > d[1])
			{
				c = d[1];
				d[1] = d[2];
				d[2] = c;
				memset(dt,0,9*sizeof(double));
				dt[0][0] = 1.0;
				dt[1][2] = 1.0;
				dt[2][1] = 1.0;

				Mat _v = Mat(3,3,CV_64F,v);
				Mat _dt = Mat(3,3,CV_64F,dt);
				Mat _tmp = Mat(3,3,CV_64F,tmp);
				_tmp = _v * _dt;

				memcpy(v,tmp,9*sizeof(double));
			}
			if(d[1] > d[0])
			{
				c = d[0];
				d[0] = d[1];
				d[1] = c;
				memset(dt,0,9*sizeof(double));
				dt[0][1] = 1.0;
				dt[1][0] = 1.0;
				dt[2][2] = 1.0;

				Mat _v = Mat(3,3,CV_64F,v);
				Mat _dt = Mat(3,3,CV_64F,dt);
				Mat _tmp = Mat(3,3,CV_64F,tmp);
				_tmp = _v * _dt;

				memcpy(v,tmp,9*sizeof(double));
			}
			Mat _v = Mat(3,3,CV_64F,v);
			Mat _dt = Mat(3,3,CV_64F,dt);
			_dt = _v.t();
			Mat _abc = Mat(3,3,CV_64F,abc);
			Mat _tmp = Mat(3,3,CV_64F,tmp);
			_tmp = _dt * _abc;
			Mat _tmp1 = Mat(3,3,CV_64F,tmp1);
			_tmp1 = _tmp * _v;
			memcpy(a,abc,9*sizeof(double));//copy back the a
			return 1;
		}
		if (i < 4)
			tresh=0.2*sm/9;
		else
			tresh=0.0;
		for (ip=0;ip<3-1;ip++) {
			for (iq=ip+1;iq<3;iq++) {
				g=100.0*fabs(a[ip][iq]);
				if (i > 4 && (fabs(d[ip])+g) == fabs(d[ip])
					&& (fabs(d[iq])+g) == fabs(d[iq]))
					a[ip][iq]=0.0;
				else if (fabs(a[ip][iq]) > tresh) {
					h=d[iq]-d[ip];
					if ((fabs(h)+g) == fabs(h))
						t=(a[ip][iq])/h;
					else {
						theta=0.5*h/(a[ip][iq]);
						t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0) t = -t;
					}
					c=1.0/sqrt(1+t*t);
					s=t*c;
					tau=s/(1.0+c);
					h=t*a[ip][iq];
					z[ip] -= h;
					z[iq] += h;
					d[ip] -= h;
					d[iq] += h;
					a[ip][iq]=0.0;
					for (j=0;j<=ip-1;j++) {
						ROTATE(a,j,ip,j,iq)
					}
					for (j=ip+1;j<=iq-1;j++) {
						ROTATE(a,ip,j,j,iq)
					}
					for (j=iq+1;j<3;j++) {
						ROTATE(a,ip,j,iq,j)
					}
					for (j=0;j<3;j++) {
						ROTATE(v,j,ip,j,iq)
					}
					++(*nrot);
				}
			}
		}
		for (ip=0;ip<3;ip++) {
			b[ip] += z[ip];
			d[ip]=b[ip];
			z[ip]=0.0;
		}
	}
	return 0;
}
#undef ROTATE

double Check_motion_error(std::vector<std::pair<cv::Mat,cv::Mat>> inl_points,
						  std::vector<unsigned int> num_inl,
						  const cv::Mat& R1_2,
						  const cv::Mat& t)
{
	Mat c0, R2_1, p, r1, r2, p1, p2, dp;
	size_t num_homo = num_inl.size();
    size_t i, j;
	double error, error1;
	int total_pts;

	c0 = Mat::zeros(3,1,CV_64F);
	R2_1 = R1_2.t();
	total_pts = 0;
	error = 0.0;
	for(i = 0; i < num_homo; ++i)
	{
		error1 = 0;
		for(j = 0; j < num_inl[i]; ++j)
		{
			p = (Mat_<double>(3, 1) << inl_points[i].first.at<double>(j,0), inl_points[i].first.at<double>(j,1), 1.0);

			r1 = p / cv::norm(p);

			p = (Mat_<double>(3, 1) << inl_points[i].second.at<double>(j,0), inl_points[i].second.at<double>(j,1), 1.0);

			p1 = R2_1 * p;
			r2 = p1 / cv::norm(p1);

			if(Rays_Closest_Points(c0, r1, t, r2, p1, p2) != 0)
			{
				dp = p1 - p2;
				error1 += cv::norm(dp);

				total_pts++;
			}
		}
		error +=error1;
	}
	return error/(double)total_pts;
}

int Rays_Closest_Points(const cv::Mat& pt1, const cv::Mat& ray1, const cv::Mat& pt2, const cv::Mat& ray2, cv::Mat & p1, cv::Mat & p2)
{
    double m1, m2, dotp, dotbv1, dotbv2;
	Mat b;

	if(fabs(ray1.dot(ray2)) >0.9999999)
	{
		//two rays are close to parallel
		return 0;
	}
	b = pt2 - pt1;
	dotp = ray1.dot(ray2);
	dotbv1 = b.dot(ray1);
	dotbv2 = b.dot(ray2);

	if(dotp == 0.0)
	{
		m1 = dotbv1;
		m2 = dotbv2;
	}
	else
	{
		m1 = (dotbv1 - dotbv2*dotp)/(1.0-dotp*dotp);
		m2 = dotp*m1 - dotbv2;
	}
	p1 = m1 * ray1;
	p2 = m2 * ray2;
	p1 = pt1 + p1;
	p2 = pt2 + p2;
	return 1;
}

int update_dn(cv::Mat points1, cv::Mat points2, int num_pts, const cv::Mat& h0, const cv::Mat& k0, cv::Mat & dn)
{
	Mat A, B, invA, p, p1, t, t1, _hp, _tt, ty;
	int i;

	double hp[3];
	double tt[3][3];
	double y;
	double d;
	double *_k0;
	_k0 = (double*)k0.data;
	_tt = Mat(3,3,CV_64F,tt);
	_hp = Mat(3,1,CV_64F,hp);
	B = Mat::zeros(3,1,CV_64F);
	A = Mat::zeros(3,3,CV_64F);
	for(i = 0; i < num_pts; ++i)
	{
		p = (Mat_<double>(3, 1) << points1.at<double>(i,0), points1.at<double>(i,1), 1.0);
		p1 = (Mat_<double>(3, 1) << points2.at<double>(i,0), points2.at<double>(i,1), 1.0);
		t = _k0[0] * p;
		d = _k0[2] * points2.at<double>(i,0);
		t1 = d * p;
		t = t - t1;
		_hp = h0 * p;
		y = points2.at<double>(i,0) * hp[2] - hp[0];
		for(int j = 0; j < 3; j++)
		{
			for(int j1 = 0; j1 < 3; j1++)
			{
				tt[j][j1] = t.at<double>(j) * t.at<double>(j1);
			}
		}
		ty = y * t;
		A = _tt + A;
		B = ty + B;

		t = _k0[1] * p;
		d = _k0[2] * points2.at<double>(i,1);
		t1 = d * p;
		t = t - t1;
		_hp = h0 * p;
		y = points2.at<double>(i,1) * hp[2] - hp[1];
		for(int j = 0; j < 3; j++)
		{
			for(int j1 = 0; j1 < 3; j1++)
			{
				tt[j][j1] = t.at<double>(j) * t.at<double>(j1);
			}
		}
		ty = y * t;
		A = _tt + A;
		B = ty + B;
	}
	invA = A.inv();
	dn = (invA * B).t();

	return 1;
}

 int update_h0_rt(std::vector<std::pair<cv::Mat,cv::Mat>> inl_points,
				  std::vector<unsigned int> num_inl,
				  cv::Mat & homo,
				  const cv::Mat& dn,
				  cv::Mat & rt)
{
	Mat rt_back, _p1;
	size_t num_homo = num_inl.size();
	double *_homo;
	double *_rt;
	_homo = (double*)homo.data;
	_rt = (double*)rt.data;
    size_t i, j;
	double r[11];
	double a[121], inva[121], b[11], m[11];
	double c[121];
	double p1[3];
	double p2[3];
	double d;
	double dnp;
	_p1 = Mat(3,1,CV_64F,p1);

	rt.copyTo(rt_back);
	for(i = 0; i<11; ++i)
	{
		b[i] = 0.0;
		m[i] = 0.0;
	}
	for(i = 0; i < 121; ++i)
	{
		a[i] = 0.0;
	}
	for(i = 0; i < num_homo; ++i)
	{
		for(j = 0; j < num_inl[i]; ++j)
		{
			p1[0] = inl_points[i].first.at<double>(j,0);
			p1[1] = inl_points[i].first.at<double>(j,1);
			p1[2] = 1.0;

			dnp = _p1.dot(dn.row(i).t());
			p2[0] = inl_points[i].second.at<double>(j,0);
			p2[1] = inl_points[i].second.at<double>(j,1);
			r[0] = p1[0];
			r[1] = p1[1];
			r[2] = 1.0;
			r[3] = 0.0;
			r[4] = 0.0;
			r[5] = 0.0;

			r[6] = (double)(-p1[0]*p2[0]);
			r[7] = (double)(-p1[1]*p2[0]);

			r[8] = dnp;
			r[9] = 0.0;
			r[10] = -p2[0]*dnp;
            Linear_Transform_D(r, r, c, 11, 1, 11);
            add_matrix_D(c, a, a, 11, 11);
			d = (double)p2[0];
            scale_matrix_D(d, r, r, 11, 1);
            add_matrix_D(r, b, b, 11, 1);

			r[0] = 0.0;
			r[1] = 0.0;
			r[2] = 0.0;
			r[3] = p1[0];
			r[4] = p1[1];
			r[5] = 1.0;

			r[6] = (double)(-p1[0]*p2[1]);
			r[7] = (double)(-p1[1]*p2[1]);

			r[8] = 0.0;
			r[9] = dnp;
			r[10] = -p2[1]*dnp;
            Linear_Transform_D(r, r, c, 11, 1, 11);
            add_matrix_D(c, a, a, 11, 11);
			d = (double)p2[1];
            scale_matrix_D(d, r, r, 11, 1);
            add_matrix_D(r, b, b, 11, 1);
		}
	}
	if(invert_matrix_D(a, inva, 11, 11) == 0)
	{
		return (0);
	}

    Linear_Transform_D(inva, b, m, 11, 11, 1);
	_homo[0] = m[0];
	_homo[1] = m[1];
	_homo[2] = m[2];

	_homo[3] = m[3];
	_homo[4] = m[4];
	_homo[5] = m[5];

	_homo[6] = m[6];
	_homo[7] = m[7];
	_homo[8] = 1.0;
	_rt[0] = m[8];
	_rt[1] = m[9];
	_rt[2] = m[10];
	return 1;
}

/* Linear transformations, for transforming vectors and matrices.
 * This works for row vectors and column vectors alike.
 *	L[nRows][lCol]	- input (left) matrix
 *	rg[lCol][rCol]	- transformation (right) matrix
 *	P[nRows][rCol]	- output (product) matrix
 *
 * Examples:
 * v[3] * M[3][3] -> w[3] :			MLLinearTransform(&v[0], &M[0][0], &w[0], 1, 3, 3);
 * M[3][3] * v[3] -> w[3] :			MLLinearTransform(&M[0][0], &v[0], &w[0], 3, 3, 1);
 * M[4][4] * N[4][4] -> P[4][4]:	MLLinearTransform(&M[0][0], &N[0][0], &P[0][0], 4, 4, 4);
 * v[4] * M[4][3] -> w[3]:			MLLinearTransform(&v[0], &M[0][0], &w[0], 1, 4, 3);
 * v[3] tensor w[3] -> T[3][3]:		MLLinearTransform(&v[0], &w[0], T[3][3], 3, 1, 3);
 * This can be used In Place, i.e.,
 * to transform the left matrix
 * by the right matrix, placing the result back in the left.  By its nature,
 * then, this can only be used for transforming row vectors or concatenating
 * matrices from the right.
 */
#define MAXDIM	32			/* The maximum dimension of a matrix */

void Linear_Transform_D(
        const double *L, //The left mat
        const double *R, //The right mat
        double	*P,	//The result mat
        int nRows, //Number of rows of the left and result matrices
        int lCol, //Number of columns in the left matrix
        int rCol) //The number of columns in the result matrix
{
	const double *lp;		/* Left matrix pointer for dot product */
	const char *rp;		/* Right matrix pointer for dot product */
	int k;				/* Loop counter */
	double sum;			/* Extended precision for intermediate results */
    size_t rowBytes = lCol * sizeof(double);
    size_t rRowBytes = rCol * sizeof(double);
	int j, i;				/* Loop counters */
	size_t lRowBytes = lCol * sizeof(double);
	const char *lb = (const char*)L;
	double temp[MAXDIM*MAXDIM]; // Temporary storage for in-place transformations
	double *tp;

	if (P == L) {  // IN PLACE
		double *op = P;				/* Output geometry */
		for (i = nRows; i--; lb += rowBytes) {	/* Each row in L */
			{
				for (k = lCol, lp = (double*)lb, tp = &temp[0]; k--; )
					*tp++ = *lp++;			/* Copy one input vector to temp storage */
			}
			for (j = 0; j < lCol; j++) {		/* Each column in R */
				lp = &temp[0];				/* Left of ith row of L */
				rp = (const char *)(R + j);	/* Top of jth column of R */
				sum = 0;
				for (k = lCol; k--; rp += rowBytes)
					sum += *lp++ * (*((const double*)rp));	/* *P += L[i'][k'] * R[k'][j] */
				*op++ = sum;
			}
		}
	} else if (P != R) {
		for (i = nRows; i--; lb += lRowBytes) {	/* Each row in L */
			for (j = 0; j < rCol; j++) {	/* Each column in R */
				lp = (const double *)lb;		/* Left of ith row of L */
				rp = (const char *)(R + j);	/* Top of jth column of R */
				sum = 0;
				for (k = lCol; k--; rp += rRowBytes)
					sum += *lp++ * (*((const double*)rp));	/* *P += L[i'][k'] * R[k'][j] */
				*P++ = sum;
			}
		}
	} else { // P == R
		for (tp = temp, i = lCol * rCol; i--; ) *tp++ = *R++;  // copy R
		for (i = nRows; i--; lb += lRowBytes) {	/* Each row in L */
			for (j = 0; j < rCol; j++) {	/* Each column in R */
				lp = (const double *)lb;		/* Left of ith row of L */
				rp = (const char *)(temp + j);	/* Top of jth column of R (now in temp) */
				sum = 0;
				for (k = lCol; k--; rp += rRowBytes)
					sum += *lp++ * (*((const double*)rp));	/* *P += L[i'][k'] * R[k'][j] */
				*P++ = sum;
			}
		}
	}
}

void add_matrix_D(double *A, double *B, double *result, int m, int n)
{
	for (int i = m * n; i --; A++, B++, result ++)
		*result = (*A) + (*B);
}

void scale_matrix_D (double scale, double *from, double *to, int m, int n)
{
	for (int i = m * n; i--; from ++, to++)
		*to = scale * (*from);
}

/*  Inverts square matrices
 *	With tall matrices, invert upper part and transform the bottom
 *	rows as would be expected if embedded into a larger matrix.
 *	Undefined for wide matrices.
 * M^(-1) --> Minv
 *
 * 1 is returned if the matrix was non-singular and the inversion was successful
 * 0 is returned if the matrix was singular and the inversion failed
 */

size_t invert_matrix_D (const double *M, double *Minv, int nRows, int n)
{
	double *m;
	int tallerBy = nRows - n;		/* Excess of rows over columns */
	int j, i;
	double b[MAXDIM];
	double lu[MAXDIM*MAXDIM+MAXDIM];

	/* Decompose matrix into L and U triangular matrices */
	if ((tallerBy < 0) || (LU_decompose_D(M, lu, n) == 0)) {
		return(0);		/* Singular */
	}

	/* Invert matrix by solving n simultaneous equations n times */
	for (i = 0, m = Minv; i < n; i++, m += n) {
		for(j = 0; j < n; j++)
			b[j] = 0;
		b[i] = 1;

        LU_solve_D(lu, b, m, n);	/* Into a row of m */
	}

	/* Special post-processing for affine transformations (e.g. 4x3) */
	if (tallerBy) {	/* Affine transformation */
		double *t = Minv+n*n;/* Translation vector */
		m = Minv;/* Reset m */
        Linear_Transform_D(t, m, t, tallerBy, n, n);/* Invert translation */
		for (j = tallerBy * n; n--; t++)
			*t = -*t;/* Negate translation vector */
	}

	return(1);
}

/* Decomposes the coefficient matrix A into upper and lower
 * triangular matrices, the composite being the LU matrix.
 * This is then followed by multiple applications of FELUSolve(),
 * to solve several problems with the same system matrix.
 *
 * 1 is returned if the matrix is non-singular and the decomposition was successful;
 * 0 is returned if the matrix is singular and the decomposition failed.
 */
#define luel(i, j)  lu[(i)*n+(j)]
#define ael(i, j)	a[(i)*n+(j)]

size_t LU_decompose_D(
        const double *a, //n x n coefficient matrix
        double *lu, //n x n LU matrix augmented by an n x 1 pivot sequence
        int n) //Order of the matrix
{
	int i, j, k;
    int pivotindex;
	double pivot, biggest, mult, tempf;
	int *ps;
	double scales[MAXDIM];

	ps = (int *)(&lu[n*n]); /* Memory for ps[] comes after LU[][] */

	for (i = 0; i < n; i++) {	/* For each row */
		/* Find the largest element in each row for row equilibration */
		biggest = 0.0;
		for (j = 0; j < n; j++)
			if (biggest < (tempf = fabs(luel(i,j) = ael(j,i)))) /* A transposed for row vectors */
				biggest = tempf;
		if (biggest != 0.0)
			scales[i] = 1.0 / biggest;
		else {
			scales[i] = 0.0;
			return(0);	/* Zero row: singular matrix */
		}

		ps[i] = i;		/* Initialize pivot sequence */
	}

	for (k = 0; k < n-1; k++) { /* For each column */
		/* Find the largest element in each column to pivot around */
		biggest = 0.0;
		for (i = k; i < n; i++) {
			if (biggest < (tempf = fabs(luel(ps[i],k)) * scales[ps[i]])) {
				biggest = tempf;
				pivotindex = i;
			}
		}
		if (biggest == 0.0)
			return(0);	/* Zero column: singular matrix */
		if (pivotindex != k) {	/* Update pivot sequence */
			j = ps[k];
			ps[k] = ps[pivotindex];
			ps[pivotindex] = j;
		}

		/* Pivot, eliminating an extra variable each time */
		pivot = luel(ps[k],k);
		for (i = k+1; i < n; i++) {
			luel(ps[i],k) = mult = luel(ps[i],k) / pivot;
			if (mult != 0.0) {
				for (j = k+1; j < n; j++)
					luel(ps[i],j) -= mult * luel(ps[k],j);
			}
		}
	}
	return(luel(ps[n-1],n-1) != 0.0);	/* 0 if singular, 1 if not */
}

/*
 * Solves the linear equation xA = b after the matrix A has
 * been decomposed with LUDecompose into the lower and upper triangular
 * matrices L and U, giving the equivalent equation xUL = b.
 */
void LU_solve_D(
        const double *lu, //decomposed LU matrix
        const double *b, //constant vector
        double *x, //solution vector
        int n) //order of the equation
{
	int i, j;
	double dot;
	const int *ps;

	ps = (const int *)(&lu[n*n]); /* Memory for ps[] comes after LU[][] */

	/* Vector reduction using U triangular matrix */
	for (i = 0; i < n; i++) {
		dot = 0.0;
		for (j = 0; j < i; j++)
			dot += luel(ps[i],j) * x[j];
		x[i] = b[ps[i]] - dot;
	}

	/* Back substitution, in L triangular matrix */
	for (i = n-1; i >= 0; i--) {
		dot = 0.0;
		for (j = i+1; j < n; j++)
			dot += luel(ps[i],j) * x[j];
		x[i] = (x[i] - dot) / luel(ps[i],i);
	}
}

//compute mean of reprojection error
double check_error(std::vector<std::pair<cv::Mat,cv::Mat>> inl_points,
                   std::vector<unsigned int> num_inl,
                   const cv::Mat& base_homo,
                   cv::Mat dn,
                   cv::Mat rt)
{
	size_t num_homo = num_inl.size();
	double *points1, *points2;
	Mat _h;
    size_t i, j;
	double error, error1;
	double dx, dy;
	double op[2];
	double h[3][3];
	double s;
	int total_pts;
	_h = Mat(3,3,CV_64F,h);

	total_pts = 0;
	error = 0.0;
	for(i = 0; i < num_homo; ++i)
	{
		for(size_t j1 = 0; j1 < 3; j1++)
		{
			for(size_t j2 = 0; j2 < 3; j2++)
			{
				h[j1][j2] = rt.at<double>(j1) * dn.at<double>(i,j2);
			}
		}
		_h = base_homo + _h;
		s = 1.0/h[2][2];
		_h = s * _h;
		error1 = 0;
		for(j = 0; j < num_inl[i]; ++j)
		{
			points1 = (double*)inl_points[i].first.row(j).data;
			points2 = (double*)inl_points[i].second.row(j).data;
            homography_transfer_33D(h, points1, op);
			dx = op[0] - points2[0];
			dy = op[1] - points2[1];
			error1 +=sqrt(dx*dx + dy*dy);
			total_pts++;
		}
		error +=error1;
	}
	return error/(float)total_pts;
}

int homography_transfer_33D(double h[3][3], double *ip, double *op)
{
	double  p[3];
	double hp[3];
	Mat _h, _p, _hp;
	_h = Mat(3,3,CV_64F,h);
	_hp = Mat(3,1,CV_64F,hp);
	_p = Mat(3,1,CV_64F,p);
    convertToHomography(ip, p);
	_hp = _h * _p;
	convertToImage(hp, op);
	return 1;
}

int convertToHomography(const double *ip, double *op)
{
	op[0] = ip[0];
	op[1] = ip[1];
	op[2] = 1.0;
	return 1;
}

int convertToImage(const double ip[3], double op[2])
{
	op[0] = ip[0]/ip[2];
	op[1] = ip[1]/ip[2];
	return 1;
}

//rot is from camera1 to camera 2
//t is the translation in camera 1 frame, plane is in the camera 1 frame
//in this formula, the 3d point translation is
//P2 = RP1 + dt
//the argument input is
//P2 = R(P1-t)

int construct_analytic_homography(const cv::Mat& rot, cv::Mat t, cv::Mat plane, cv::Mat & h)
{
	Mat _tp, im;
	double tp[3][3], s;
	_tp = Mat(3,3,CV_64F,tp);

	for(size_t j1 = 0; j1 < 3; j1++)
	{
		for(size_t j2 = 0; j2 < 3; j2++)
		{
			tp[j1][j2] = t.at<double>(j1) * plane.at<double>(j2);
		}
	}
	im = Mat::eye(3,3,CV_64F);
	_tp = im - _tp;
	h = rot * _tp;
	if(fabs(h.at<double>(2,2)) > 0.0)
	{
		s = 1.0/h.at<double>(2,2);
		h = s * h;
		return 1;
	}
	else
	{
		return 0;
	}
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "lapack_common.h"
#include <Eigen/Eigenvalues>

// computes eigen values and vectors of a general N-by-N matrix A
EIGEN_LAPACK_FUNC(syev,(char *jobz, char *uplo, int* n, Scalar* a, int *lda, Scalar* w, Scalar* /*work*/, int* lwork, int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;

  *info = 0;
        if(*jobz!='N' && *jobz!='V')                    *info = -1;
  else  if(UPLO(*uplo)==INVALID)                        *info = -2;
  else  if(*n<0)                                        *info = -3;
  else  if(*lda<std::max(1,*n))                         *info = -5;
  else  if((!query_size) && *lwork<std::max(1,3**n-1))  *info = -8;

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"SYEV ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0)
    return 0;

  PlainMatrixType mat(*n,*n);
  if(UPLO(*uplo)==UP) mat = matrix(a,*n,*n,*lda).adjoint();
  else                mat = matrix(a,*n,*n,*lda);

  bool computeVectors = *jobz=='V' || *jobz=='v';
  SelfAdjointEigenSolver<PlainMatrixType> eig(mat,computeVectors?ComputeEigenvectors:EigenvaluesOnly);

  if(eig.info()==NoConvergence)
  {
    make_vector(w,*n).setZero();
    if(computeVectors)
      matrix(a,*n,*n,*lda).setIdentity();
    //*info = 1;
    return 0;
  }

  make_vector(w,*n) = eig.eigenvalues();
  if(computeVectors)
    matrix(a,*n,*n,*lda) = eig.eigenvectors();

  return 0;
}


#ifndef VIENNACL_LINALG_AMG_HPP_
#define VIENNACL_LINALG_AMG_HPP_

/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** @file viennacl/linalg/amg.hpp
    @brief Main include file for algebraic multigrid (AMG) preconditioners.  Experimental.

    Implementation contributed by Markus Wagner
*/

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <vector>
#include <cmath>
#include "viennacl/forwards.h"
#include "viennacl/tools/tools.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/direct_solve.hpp"

#include "viennacl/linalg/detail/amg/amg_base.hpp"
#include "viennacl/linalg/detail/amg/amg_coarse.hpp"
#include "viennacl/linalg/detail/amg/amg_interpol.hpp"

#include <map>

#ifdef VIENNACL_WITH_OPENMP
 #include <omp.h>
#endif

#include "viennacl/linalg/detail/amg/amg_debug.hpp"

#define VIENNACL_AMG_COARSE_LIMIT 50
#define VIENNACL_AMG_MAX_LEVELS 100

namespace viennacl
{
namespace linalg
{

typedef detail::amg::amg_tag          amg_tag;

/** @brief Setup AMG preconditioner
*
* @param A            Operator matrices on all levels
* @param P            Prolongation/Interpolation operators on all levels
* @param pointvector  Vector of points on all levels
* @param tag          AMG preconditioner tag
*/
template<typename InternalT1, typename InternalT2>
void amg_setup(InternalT1 & A, InternalT1 & P, InternalT2 & pointvector, amg_tag & tag)
{
  typedef typename InternalT2::value_type      PointVectorType;

  unsigned int i, iterations, c_points, f_points;
  detail::amg::amg_slicing<InternalT1,InternalT2> slicing;

  // Set number of iterations. If automatic coarse grid construction is chosen (0), then set a maximum size and stop during the process.
  iterations = tag.get_coarselevels();
  if (iterations == 0)
    iterations = VIENNACL_AMG_MAX_LEVELS;

  // For parallel coarsenings build data structures (number of threads set automatically).
  if (tag.get_coarse() == VIENNACL_AMG_COARSE_RS0 || tag.get_coarse() == VIENNACL_AMG_COARSE_RS3)
    slicing.init(iterations);

  for (i=0; i<iterations; ++i)
  {
    // Initialize Pointvector on level i and construct points.
    pointvector[i] = PointVectorType(static_cast<unsigned int>(A[i].size1()));
    pointvector[i].init_points();

    // Construct C and F points on coarse level (i is fine level, i+1 coarse level).
    detail::amg::amg_coarse (i, A, pointvector, slicing, tag);

    // Calculate number of C and F points on level i.
    c_points = pointvector[i].get_cpoints();
    f_points = pointvector[i].get_fpoints();

    #if defined (VIENNACL_AMG_DEBUG) //or defined(VIENNACL_AMG_DEBUGBENCH)
    std::cout << "Level " << i << ": ";
    std::cout << "No of C points = " << c_points << ", ";
    std::cout << "No of F points = " << f_points << std::endl;
    #endif

    // Stop routine when the maximal coarse level is found (no C or F point). Coarsest level is level i.
    if (c_points == 0 || f_points == 0)
      break;

    // Construct interpolation matrix for level i.
    detail::amg::amg_interpol (i, A, P, pointvector, tag);

    // Compute coarse grid operator (A[i+1] = R * A[i] * P) with R = trans(P).
    detail::amg::amg_galerkin_prod(A[i], P[i], A[i+1]);

    // Test triple matrix product. Very slow for large matrix sizes (ublas).
    // test_triplematprod(A[i],P[i],A[i+1]);

    pointvector[i].delete_points();

    #ifdef VIENNACL_AMG_DEBUG
    std::cout << "Coarse Grid Operator Matrix:" << std::endl;
    printmatrix (A[i+1]);
    #endif

    // If Limit of coarse points is reached then stop. Coarsest level is level i+1.
    if (tag.get_coarselevels() == 0 && c_points <= VIENNACL_AMG_COARSE_LIMIT)
    {
      tag.set_coarselevels(i+1);
      return;
    }
  }
  tag.set_coarselevels(i);
}

/** @brief Initialize AMG preconditioner
*
* @param mat          System matrix
* @param A            Operator matrices on all levels
* @param P            Prolongation/Interpolation operators on all levels
* @param pointvector  Vector of points on all levels
* @param tag          AMG preconditioner tag
*/
template<typename MatrixT, typename InternalT1, typename InternalT2>
void amg_init(MatrixT const & mat, InternalT1 & A, InternalT1 & P, InternalT2 & pointvector, amg_tag & tag)
{
  //typedef typename MatrixType::value_type ScalarType;
  typedef typename InternalT1::value_type SparseMatrixType;

  if (tag.get_coarselevels() > 0)
  {
    A.resize(tag.get_coarselevels()+1);
    P.resize(tag.get_coarselevels());
    pointvector.resize(tag.get_coarselevels());
  }
  else
  {
    A.resize(VIENNACL_AMG_MAX_LEVELS+1);
    P.resize(VIENNACL_AMG_MAX_LEVELS);
    pointvector.resize(VIENNACL_AMG_MAX_LEVELS);
  }

  // Insert operator matrix as operator for finest level.
  SparseMatrixType A0(mat);
  A.insert_element(0, A0);
}

/** @brief Save operators after setup phase for CPU computation.
*
* @param A      Operator matrices on all levels on the CPU
* @param P      Prolongation/Interpolation operators on all levels on the CPU
* @param R      Restriction operators on all levels on the CPU
* @param A_setup    Operators matrices on all levels from setup phase
* @param P_setup    Prolongation/Interpolation operators on all levels from setup phase
* @param tag    AMG preconditioner tag
*/
template<typename InternalT1, typename InternalT2>
void amg_transform_cpu(InternalT1 & A, InternalT1 & P, InternalT1 & R, InternalT2 & A_setup, InternalT2 & P_setup, amg_tag & tag)
{
  //typedef typename InternalType1::value_type MatrixType;

  // Resize internal data structures to actual size.
  A.resize(tag.get_coarselevels()+1);
  P.resize(tag.get_coarselevels());
  R.resize(tag.get_coarselevels());

  // Transform into matrix type.
  for (unsigned int i=0; i<tag.get_coarselevels()+1; ++i)
  {
    A[i].resize(A_setup[i].size1(),A_setup[i].size2(),false);
    A[i] = A_setup[i];
  }
  for (unsigned int i=0; i<tag.get_coarselevels(); ++i)
  {
    P[i].resize(P_setup[i].size1(),P_setup[i].size2(),false);
    P[i] = P_setup[i];
  }
  for (unsigned int i=0; i<tag.get_coarselevels(); ++i)
  {
    R[i].resize(P_setup[i].size2(),P_setup[i].size1(),false);
    P_setup[i].set_trans(true);
    R[i] = P_setup[i];
    P_setup[i].set_trans(false);
  }
}

/** @brief Save operators after setup phase for GPU computation.
*
* @param A          Operator matrices on all levels on the GPU
* @param P          Prolongation/Interpolation operators on all levels on the GPU
* @param R          Restriction operators on all levels on the GPU
* @param A_setup    Operators matrices on all levels from setup phase
* @param P_setup    Prolongation/Interpolation operators on all levels from setup phase
* @param tag        AMG preconditioner tag
* @param ctx        Optional context in which the auxiliary objects are created (one out of multiple OpenCL contexts, CUDA, host)
*/
template<typename InternalT1, typename InternalT2>
void amg_transform_gpu(InternalT1 & A, InternalT1 & P, InternalT1 & R, InternalT2 & A_setup, InternalT2 & P_setup, amg_tag & tag, viennacl::context ctx)
{
  typedef typename InternalT2::value_type::value_type    NumericType;

  // Resize internal data structures to actual size.
  A.resize(tag.get_coarselevels()+1);
  P.resize(tag.get_coarselevels());
  R.resize(tag.get_coarselevels());

  // Copy to GPU using the internal sparse matrix structure: std::vector<std::map>.
  for (unsigned int i=0; i<tag.get_coarselevels()+1; ++i)
  {
    viennacl::switch_memory_context(A[i], ctx);
    //A[i].resize(A_setup[i].size1(),A_setup[i].size2(),false);
    viennacl::copy(*(A_setup[i].get_internal_pointer()),A[i]);
  }
  for (unsigned int i=0; i<tag.get_coarselevels(); ++i)
  {
    // find number of nonzeros in P:
    vcl_size_t nonzeros = 0;
    for (vcl_size_t j=0; j<P_setup[i].get_internal_pointer()->size(); ++j)
      nonzeros += (*P_setup[i].get_internal_pointer())[j].size();

    viennacl::switch_memory_context(P[i], ctx);
    //P[i].resize(P_setup[i].size1(),P_setup[i].size2(),false);
    viennacl::detail::copy_impl(tools::const_sparse_matrix_adapter<NumericType>(*(P_setup[i].get_internal_pointer()), P_setup[i].size1(), P_setup[i].size2()), P[i], nonzeros);
    //viennacl::copy((boost::numeric::ublas::compressed_matrix<ScalarType>)P_setup[i],P[i]);

    viennacl::switch_memory_context(R[i], ctx);
    //R[i].resize(P_setup[i].size2(),P_setup[i].size1(),false);
    P_setup[i].set_trans(true);
    viennacl::detail::copy_impl(tools::const_sparse_matrix_adapter<NumericType>(*(P_setup[i].get_internal_pointer()), P_setup[i].size1(), P_setup[i].size2()), R[i], nonzeros);
    P_setup[i].set_trans(false);
  }
}

/** @brief Setup data structures for precondition phase.
*
* @param result      Result vector on all levels
* @param rhs         RHS vector on all levels
* @param residual    Residual vector on all levels
* @param A           Operators matrices on all levels from setup phase
* @param tag         AMG preconditioner tag
*/
template<typename InternalVectorT, typename SparseMatrixT>
void amg_setup_apply(InternalVectorT & result, InternalVectorT & rhs, InternalVectorT & residual, SparseMatrixT const & A, amg_tag const & tag)
{
  typedef typename InternalVectorT::value_type VectorType;

  result.resize(tag.get_coarselevels()+1);
  rhs.resize(tag.get_coarselevels()+1);
  residual.resize(tag.get_coarselevels());

  for (unsigned int level=0; level < tag.get_coarselevels()+1; ++level)
  {
    result[level] = VectorType(A[level].size1());
    result[level].clear();
    rhs[level] = VectorType(A[level].size1());
    rhs[level].clear();
  }
  for (unsigned int level=0; level < tag.get_coarselevels(); ++level)
  {
    residual[level] = VectorType(A[level].size1());
    residual[level].clear();
  }
}

/** @brief Setup data structures for precondition phase for later use on the GPU
*
* @param result      Result vector on all levels
* @param rhs         RHS vector on all levels
* @param residual    Residual vector on all levels
* @param A           Operators matrices on all levels from setup phase
* @param tag         AMG preconditioner tag
* @param ctx         Optional context in which the auxiliary objects are created (one out of multiple OpenCL contexts, CUDA, host)
*/
template<typename InternalVectorT, typename SparseMatrixT>
void amg_setup_apply(InternalVectorT & result, InternalVectorT & rhs, InternalVectorT & residual, SparseMatrixT const & A, amg_tag const & tag, viennacl::context ctx)
{
  typedef typename InternalVectorT::value_type VectorType;

  result.resize(tag.get_coarselevels()+1);
  rhs.resize(tag.get_coarselevels()+1);
  residual.resize(tag.get_coarselevels());

  for (unsigned int level=0; level < tag.get_coarselevels()+1; ++level)
  {
    result[level] = VectorType(A[level].size1(), ctx);
      rhs[level] = VectorType(A[level].size1(), ctx);
  }
  for (unsigned int level=0; level < tag.get_coarselevels(); ++level)
  {
    residual[level] = VectorType(A[level].size1(), ctx);
  }
}

/** @brief Pre-compute LU factorization for direct solve (ublas library).
 *  @brief Speeds up precondition phase as this is computed only once overall instead of once per iteration.
*
* @param op           Operator matrix for direct solve
* @param permutation  Permutation matrix which saves the factorization result
* @param A            Operator matrix on coarsest level
*/
template<typename NumericT, typename SparseMatrixT>
void amg_lu(boost::numeric::ublas::compressed_matrix<NumericT> & op, boost::numeric::ublas::permutation_matrix<> & permutation, SparseMatrixT const & A)
{
  typedef typename SparseMatrixT::const_iterator1 ConstRowIterator;
  typedef typename SparseMatrixT::const_iterator2 ConstColIterator;

  // Copy to operator matrix. Needed
  op.resize(A.size1(),A.size2(),false);
  for (ConstRowIterator row_iter = A.begin1(); row_iter != A.end1(); ++row_iter)
    for (ConstColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      op (col_iter.index1(), col_iter.index2()) = *col_iter;

  // Permutation matrix has to be reinitialized with actual size. Do not clear() or resize()!
  permutation = boost::numeric::ublas::permutation_matrix<> (op.size1());
  boost::numeric::ublas::lu_factorize(op, permutation);
}

/** @brief AMG preconditioner class, can be supplied to solve()-routines
*/
template<typename MatrixT>
class amg_precond
{
  typedef typename MatrixT::value_type                NumericType;
  typedef boost::numeric::ublas::vector<NumericType>  VectorType;
  typedef detail::amg::amg_sparsematrix<NumericType>  SparseMatrixType;
  typedef detail::amg::amg_pointvector                PointVectorType;

  typedef typename SparseMatrixType::const_iterator1  InternalConstRowIterator;
  typedef typename SparseMatrixType::const_iterator2  InternalConstColIterator;
  typedef typename SparseMatrixType::iterator1        InternalRowIterator;
  typedef typename SparseMatrixType::iterator2        InternalColIterator;

  boost::numeric::ublas::vector<SparseMatrixType> A_setup_;
  boost::numeric::ublas::vector<SparseMatrixType> P_setup_;
  boost::numeric::ublas::vector<MatrixT>          A_;
  boost::numeric::ublas::vector<MatrixT>          P_;
  boost::numeric::ublas::vector<MatrixT>          R_;
  boost::numeric::ublas::vector<PointVectorType>  pointvector_;

  mutable boost::numeric::ublas::compressed_matrix<NumericType> op_;
  mutable boost::numeric::ublas::permutation_matrix<>           permutation_;

  mutable boost::numeric::ublas::vector<VectorType> result_;
  mutable boost::numeric::ublas::vector<VectorType> rhs_;
  mutable boost::numeric::ublas::vector<VectorType> residual_;

  mutable bool done_init_apply_;

  amg_tag tag_;
public:

  amg_precond(): permutation_(0) {}
  /** @brief The constructor. Saves system matrix, tag and builds data structures for setup.
  *
  * @param mat  System matrix
  * @param tag  The AMG tag
  */
  amg_precond(MatrixT const & mat, amg_tag const & tag): permutation_(0)
  {
    tag_ = tag;
    // Initialize data structures.
    amg_init (mat, A_setup_, P_setup_, pointvector_, tag_);

    done_init_apply_ = false;
  }

  /** @brief Start setup phase for this class and copy data structures.
  */
  void setup()
  {
    // Start setup phase.
    amg_setup(A_setup_, P_setup_, pointvector_, tag_);
    // Transform to CPU-Matrixtype for precondition phase.
    amg_transform_cpu(A_, P_, R_, A_setup_, P_setup_, tag_);

    done_init_apply_ = false;
  }

  /** @brief Prepare data structures for preconditioning:
   *  Build data structures for precondition phase.
   *  Do LU factorization on coarsest level.
  */
  void init_apply() const
  {
    // Setup precondition phase (Data structures).
    amg_setup_apply(result_, rhs_, residual_, A_setup_, tag_);
    // Do LU factorization for direct solve.
    amg_lu(op_, permutation_, A_setup_[tag_.get_coarselevels()]);

    done_init_apply_ = true;
  }

  /** @brief Returns complexity measures.
  *
  * @param avgstencil  Average stencil sizes on all levels
  * @return            Operator complexity of AMG method
  */
  template<typename VectorT>
  NumericType calc_complexity(VectorT & avgstencil)
  {
    avgstencil = VectorT(tag_.get_coarselevels()+1);
    unsigned int nonzero=0, systemmat_nonzero=0, level_coefficients=0;

    for (unsigned int level=0; level < tag_.get_coarselevels()+1; ++level)
    {
      level_coefficients = 0;
      for (InternalRowIterator row_iter = A_setup_[level].begin1(); row_iter != A_setup_[level].end1(); ++row_iter)
      {
        for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          if (level == 0)
            systemmat_nonzero++;
          nonzero++;
          level_coefficients++;
        }
      }
      avgstencil[level] = static_cast<NumericType>(level_coefficients)/static_cast<NumericType>(A_setup_[level].size1());
    }
    return static_cast<NumericType>(nonzero) / static_cast<NumericType>(systemmat_nonzero);
  }

  /** @brief Precondition Operation
  *
  * @param vec The vector to which preconditioning is applied to (ublas version)
  */
  template<typename VectorT>
  void apply(VectorT & vec) const
  {
    // Build data structures and do lu factorization before first iteration step.
    if (!done_init_apply_)
      init_apply();

    int level;

    // Precondition operation (Yang, p.3)
    rhs_[0] = vec;
    for (level=0; level<static_cast<int>(tag_.get_coarselevels()); level++)
    {
      result_[level].clear();

      // Apply Smoother presmooth_ times.
      smooth_jacobi (level, tag_.get_presmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After presmooth:" << std::endl;
      printvector(result_[level]);
      #endif

      // Compute residual.
      residual_[level] = rhs_[level] - boost::numeric::ublas::prod(A_[level], result_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Residual:" << std::endl;
      printvector(residual_[level]);
      #endif

      // Restrict to coarse level. Restricted residual is RHS of coarse level.
      rhs_[level+1] = boost::numeric::ublas::prod(R_[level], residual_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Restricted Residual: " << std::endl;
      printvector(rhs_[level+1]);
      #endif
    }

    // On highest level use direct solve to solve equation.
    result_[level] = rhs_[level];
    boost::numeric::ublas::lu_substitute(op_, permutation_, result_[level]);

    #ifdef VIENNACL_AMG_DEBUG
    std::cout << "After direct solve: " << std::endl;
    printvector(result_[level]);
    #endif

    for (level=tag_.get_coarselevels()-1; level >= 0; level--)
    {
      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Coarse Error: " << std::endl;
      printvector(result_[level+1]);
      #endif

      // Interpolate error to fine level. Correct solution by adding error.
      result_[level] += boost::numeric::ublas::prod(P_[level], result_[level+1]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Corrected Result: " << std::endl;
      printvector(result_[level]);
      #endif

      // Apply Smoother postsmooth_ times.
      smooth_jacobi(level, tag_.get_postsmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After postsmooth: " << std::endl;
      printvector(result_[level]);
      #endif
    }
    vec = result_[0];
  }

  /** @brief (Weighted) Jacobi Smoother (CPU version)
  * @param level       Coarse level to which smoother is applied to
  * @param iterations  Number of smoother iterations
  * @param x           The vector smoothing is applied to
  * @param rhs_smooth  The right hand side of the equation for the smoother
  */
  template<typename VectorT>
  void smooth_jacobi(int level, int const iterations, VectorT & x, VectorT const & rhs_smooth) const
  {
    VectorT old_result(x.size());
    long index;

    for (int i=0; i<iterations; ++i)
    {
      old_result = x;
      x.clear();
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for
#endif
      for (index=0; index < static_cast<long>(A_setup_[level].size1()); ++index)
      {
        InternalConstRowIterator row_iter = A_setup_[level].begin1();
        row_iter += index;
        NumericType sum  = 0;
        NumericType diag = 1;
        for (InternalConstColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          if (col_iter.index1() == col_iter.index2())
            diag = *col_iter;
          else
            sum += *col_iter * old_result[col_iter.index2()];
        }
        x[index]= static_cast<NumericType>(tag_.get_jacobiweight()) * (rhs_smooth[index] - sum) / diag + (1-static_cast<NumericType>(tag_.get_jacobiweight())) * old_result[index];
      }
    }
  }

  amg_tag & tag() { return tag_; }
};

/** @brief AMG preconditioner class, can be supplied to solve()-routines.
*
*  Specialization for compressed_matrix
*/
template<typename NumericT, unsigned int AlignmentV>
class amg_precond< compressed_matrix<NumericT, AlignmentV> >
{
  typedef viennacl::compressed_matrix<NumericT, AlignmentV> MatrixType;
  typedef viennacl::vector<NumericT>                        VectorType;
  typedef detail::amg::amg_sparsematrix<NumericT>           SparseMatrixType;
  typedef detail::amg::amg_pointvector                      PointVectorType;

  typedef typename SparseMatrixType::const_iterator1   InternalConstRowIterator;
  typedef typename SparseMatrixType::const_iterator2   InternalConstColIterator;
  typedef typename SparseMatrixType::iterator1         InternalRowIterator;
  typedef typename SparseMatrixType::iterator2         InternalColIterator;

  boost::numeric::ublas::vector<SparseMatrixType> A_setup_;
  boost::numeric::ublas::vector<SparseMatrixType> P_setup_;
  boost::numeric::ublas::vector<MatrixType>       A_;
  boost::numeric::ublas::vector<MatrixType>       P_;
  boost::numeric::ublas::vector<MatrixType>       R_;
  boost::numeric::ublas::vector<PointVectorType>  pointvector_;

  mutable boost::numeric::ublas::compressed_matrix<NumericT>  op_;
  mutable boost::numeric::ublas::permutation_matrix<>         permutation_;

  mutable boost::numeric::ublas::vector<VectorType> result_;
  mutable boost::numeric::ublas::vector<VectorType> rhs_;
  mutable boost::numeric::ublas::vector<VectorType> residual_;

  viennacl::context ctx_;

  mutable bool done_init_apply_;

  amg_tag tag_;

public:

  amg_precond(): permutation_(0) {}

  /** @brief The constructor. Builds data structures.
  *
  * @param mat  System matrix
  * @param tag  The AMG tag
  */
  amg_precond(compressed_matrix<NumericT, AlignmentV> const & mat, amg_tag const & tag): permutation_(0), ctx_(viennacl::traits::context(mat))
  {
    tag_ = tag;

    // Copy to CPU. Internal structure of sparse matrix is used for copy operation.
    std::vector<std::map<unsigned int, NumericT> > mat2 = std::vector<std::map<unsigned int, NumericT> >(mat.size1());
    viennacl::copy(mat, mat2);

    // Initialize data structures.
    amg_init (mat2, A_setup_, P_setup_, pointvector_, tag_);

    done_init_apply_ = false;
  }

  /** @brief Start setup phase for this class and copy data structures.
  */
  void setup()
  {
    // Start setup phase.
    amg_setup(A_setup_, P_setup_, pointvector_, tag_);
    // Transform to GPU-Matrixtype for precondition phase.
    amg_transform_gpu(A_, P_, R_, A_setup_, P_setup_, tag_, ctx_);

    done_init_apply_ = false;
  }

  /** @brief Prepare data structures for preconditioning:
   *  Build data structures for precondition phase.
   *  Do LU factorization on coarsest level.
  */
  void init_apply() const
  {
    // Setup precondition phase (Data structures).
    amg_setup_apply(result_, rhs_, residual_, A_setup_, tag_, ctx_);
    // Do LU factorization for direct solve.
    amg_lu(op_, permutation_, A_setup_[tag_.get_coarselevels()]);

    done_init_apply_ = true;
  }

  /** @brief Returns complexity measures
  *
  * @param avgstencil  Average stencil sizes on all levels
  * @return     Operator complexity of AMG method
  */
  template<typename VectorT>
  NumericT calc_complexity(VectorT & avgstencil)
  {
    avgstencil = VectorT(tag_.get_coarselevels()+1);
    unsigned int nonzero=0, systemmat_nonzero=0, level_coefficients=0;

    for (unsigned int level=0; level < tag_.get_coarselevels()+1; ++level)
    {
      level_coefficients = 0;
      for (InternalRowIterator row_iter = A_setup_[level].begin1(); row_iter != A_setup_[level].end1(); ++row_iter)
      {
        for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          if (level == 0)
            systemmat_nonzero++;
          nonzero++;
          level_coefficients++;
        }
      }
      avgstencil[level] = level_coefficients/static_cast<double>(A_[level].size1());
    }
    return nonzero/static_cast<double>(systemmat_nonzero);
  }

  /** @brief Precondition Operation
  *
  * @param vec The vector to which preconditioning is applied to
  */
  template<typename VectorT>
  void apply(VectorT & vec) const
  {
    if (!done_init_apply_)
      init_apply();

    vcl_size_t level;

    // Precondition operation (Yang, p.3).
    rhs_[0] = vec;
    for (level=0; level < tag_.get_coarselevels(); level++)
    {
      result_[level].clear();

      // Apply Smoother presmooth_ times.
      smooth_jacobi(level, tag_.get_presmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After presmooth: " << std::endl;
      printvector(result_[level]);
      #endif

      // Compute residual.
      //residual[level] = rhs_[level] - viennacl::linalg::prod(A_[level], result_[level]);
      residual_[level] = viennacl::linalg::prod(A_[level], result_[level]);
      residual_[level] = rhs_[level] - residual_[level];

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Residual: " << std::endl;
      printvector(residual_[level]);
      #endif

      // Restrict to coarse level. Result is RHS of coarse level equation.
      //residual_coarse[level] = viennacl::linalg::prod(R[level],residual[level]);
      rhs_[level+1] = viennacl::linalg::prod(R_[level], residual_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Restricted Residual: " << std::endl;
      printvector(rhs_[level+1]);
      #endif
    }

    // On highest level use direct solve to solve equation (on the CPU)
    //TODO: Use GPU direct solve!
    result_[level] = rhs_[level];
    boost::numeric::ublas::vector<NumericT> result_cpu(result_[level].size());

    viennacl::copy(result_[level], result_cpu);
    boost::numeric::ublas::lu_substitute(op_, permutation_, result_cpu);
    viennacl::copy(result_cpu, result_[level]);

    #ifdef VIENNACL_AMG_DEBUG
    std::cout << "After direct solve: " << std::endl;
    printvector (result[level]);
    #endif

    for (int level2 = static_cast<int>(tag_.get_coarselevels()-1); level2 >= 0; level2--)
    {
      level = static_cast<vcl_size_t>(level2);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Coarse Error: " << std::endl;
      printvector(result[level+1]);
      #endif

      // Interpolate error to fine level and correct solution.
      result_[level] += viennacl::linalg::prod(P_[level], result_[level+1]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Corrected Result: " << std::endl;
      printvector(result_[level]);
      #endif

      // Apply Smoother postsmooth_ times.
      smooth_jacobi(level, tag_.get_postsmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After postsmooth: " << std::endl;
      printvector(result_[level]);
      #endif
    }
    vec = result_[0];
  }

  /** @brief Jacobi Smoother (GPU version)
  * @param level       Coarse level to which smoother is applied to
  * @param iterations  Number of smoother iterations
  * @param x           The vector smoothing is applied to
  * @param rhs_smooth  The right hand side of the equation for the smoother
  */
  template<typename VectorT>
  void smooth_jacobi(vcl_size_t level, unsigned int iterations, VectorT & x, VectorT const & rhs_smooth) const
  {
    VectorType old_result = x;

    viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(x).context());
    viennacl::linalg::opencl::kernels::compressed_matrix<NumericT>::init(ctx);
    viennacl::ocl::kernel & k = ctx.get_kernel(viennacl::linalg::opencl::kernels::compressed_matrix<NumericT>::program_name(), "jacobi");

    for (unsigned int i=0; i<iterations; ++i)
    {
      if (i > 0)
        old_result = x;
      x.clear();
      viennacl::ocl::enqueue(k(A_[level].handle1().opencl_handle(), A_[level].handle2().opencl_handle(), A_[level].handle().opencl_handle(),
                              static_cast<NumericT>(tag_.get_jacobiweight()),
                              viennacl::traits::opencl_handle(old_result),
                              viennacl::traits::opencl_handle(x),
                              viennacl::traits::opencl_handle(rhs_smooth),
                              static_cast<cl_uint>(rhs_smooth.size())));

    }
  }

  amg_tag & tag() { return tag_; }
};

}
}

#endif


#include <cmath>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <random>
#include <sstream>
#include <fftw3.h>
#include "parameter.hpp"
#include "utilities.hpp"
#include <boost/filesystem.hpp>

namespace fs = boost::filesystem;

void dir_manage(const std::string exist_dir, const std::string new_dir )
{

    //Path of the existing data directory
    const fs::path existpath("../" + exist_dir );

    //Tries to remove all files in there. If it fails, it throws an error.
    try {
        fs::remove_all(existpath);
    }
    catch (fs::filesystem_error& ex) {
        std::cout << ex.what() << std::endl;
        throw;
    }

    //Path of the newly set directory
    const fs::path newpath("../" + new_dir );

    //Tries to create a directory. If it fails, it throws an error.
    boost::system::error_code error;
    const bool result = fs::create_directory(newpath, error);
    if (!result || error) {
        std::cout << "failed to create directory" << std::endl;
    }

}

void file_manage(const std::string exist_file)
{
    //Path of the existing status file
    const fs::path statuspath("../" + exist_file);

    //Tries to remove the file. If it fails, it throws an error.
    try {
        fs::remove(statuspath);
    }
    catch (fs::filesystem_error& ex) {
        std::cout << ex.what() << std::endl;
        throw;
    }

}

double rand_uniform(void)
{
    std::random_device rnd;
    std::mt19937 mt( rnd() );
    std::uniform_real_distribution<> rand( 0, 1 );
 //   std::cout << rand(mt) << "\n";
    return (rand(mt));
}

void set_mode(double p2, double m2, double *field, double *deriv, int real)
{
    double phase,phase2, amplitude, rms_amplitude, omega;
    double re_f_left, im_f_left, re_f_right, im_f_right;
#if  dim==1
    static double norm = m*pow(L/(dx*dx),.5)/sqrt(4*M_PI);
#elif  dim==2
    static double norm =  m*pow(L/(dx*dx),1)/(sqrt(2*M_PI));
#elif  dim==3
    static double norm =  m*pow(L/(dx*dx),1.5)/sqrt(2);
#endif
    static int tachyonic = 0; //Avoid printing the same error repeatedly

    if(p2+m2>0)
        omega=sqrt(p2+m2);
    else
    {
        if(tachyonic==0)
            std::cout <<"Warning: Tachyonic mode(s) may be initialized inaccurately"<< std::endl;
        omega=sqrt(p2);
        tachyonic=1;
    }

    if(omega>0.)
        rms_amplitude=norm/sqrt(omega)*pow(p2,.75-(double)dim/4.);
    else
        rms_amplitude=0.;

        //Amplitude = RMS amplitude x Rayleigh distributed random number
        // The same amplitude is used for left and right moving waves to generate standing waves. The extra 1/sqrt(2) normalizes the initial occupation number correctly.

    amplitude = rms_amplitude/sqrt(2.)*sqrt(log(1./rand_uniform()));
    phase = 2*M_PI*rand_uniform();
  // std::cout << "phase1 " << phase/(2*M_PI) << std::endl;
        //Left moving component
        re_f_left = amplitude * cos( phase );
        im_f_left = amplitude * sin( phase );
        //Right moving component
    phase2 = 2*M_PI*rand_uniform();
  //  std::cout << "phase2 " << phase/(2*M_PI) << std::endl;
        re_f_right = amplitude * cos( phase2 );
        im_f_right = amplitude * sin( phase2 );

    field[0] = re_f_left + re_f_right;
    field[1] = im_f_left + im_f_right;
    deriv[0] = omega*(im_f_left - im_f_right);
    deriv[1] = -omega*(re_f_left - re_f_right);
    if(real==1)
    {
        field[1]=0;
        deriv[1]=0;
    }
    return;
   }

void DFT_c2rD1( double* f)
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N]();
    p = fftw_plan_dft_c2r_1d( N, in, out, FFTW_ESTIMATE );

        for( int j = 0; j < N/2+1; ++j ){
            int idx = j;
            if(idx==0)
            {
                in[idx][0] = f[idx]  ;
                in[idx][1] = 0  ;

            }else if(idx==N/2){
                in[idx][0] = f[1]  ;
                in[idx][1] = 0  ;
            }else{
                in[idx][0] = f[2*idx]  ;
                in[idx][1] = f[2*idx+1]  ;
            }

        }

    fftw_execute(p);

    // Set output data
    for( int j = 0; j < N; ++j ){
        int idx = j;
        f[idx] = out[idx]/N;
    }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}
/*
void DFT_c2rD2d( double* df,double* fdnyquist )
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * N * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N*N]();
    p = fftw_plan_dft_c2r_2d( N, N, in, out, FFTW_ESTIMATE );

    for( int j = 0; j < N; ++j ){
        //#pragma omp parallel for schedule( static ) num_threads( num_threads )
        for( int k = 0; k < N/2+1; ++k ){
            int idx = (N/2+1)*j + k;
            // int idx = j*N + k;
            if(k == N/2){
                in[idx][0] = (double)fdnyquist[2*j] ;
                in[idx][1] = (double)fdnyquist[2*j+1] ;
               // std::cout << "in[" << idx << "][0] = " << in[idx][0] << std::endl;
               // std::cout << "in[" << idx << "][1] = " << in[idx][1] << std::endl;
            }else {
                in[idx][0] =  (double)df[j*N + 2*k] ;
                in[idx][1] = (double)df[j*N + 2*k+1] ;
               // std::cout << "in[" << idx << "][0] = " << in[idx][0] << std::endl;
               // std::cout << "in[" << idx << "][1] = " << in[idx][1] << std::endl;
            }
        }
    }
    fftw_execute(p);

    // Set output data

    for( int j = 0; j < N; ++j ){
        //#pragma omp parallel for schedule( static ) num_threads( num_threads )
        for( int k = 0; k < N; ++k ){
            int idx = j*N + k;
            std::cout << "out[" << idx << "] = " << out[idx] << std::endl;
            df[idx] = out[idx]/(N*N);
        }
    }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}
*/
void DFT_c2rD2( double* f,double* fnyquist )
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * N * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N*N]();
    p = fftw_plan_dft_c2r_2d( N, N, in, out, FFTW_ESTIMATE );

        for( int j = 0; j < N; ++j ){
            for( int k = 0; k < N/2+1; ++k ){
                int idx = (N/2+1)*j + k;
              //  int idx = j*N + k;
                if(k == N/2){
                    in[idx][0] =  fnyquist[2*j] ;
                    in[idx][1] =  fnyquist[2*j+1] ;
                }else {
                    in[idx][0] =  f[j*N + 2*k];
                    in[idx][1] =  f[j*N + 2*k+1] ;
                }
            }
        }
        fftw_execute(p);

        // Set output data

     for( int j = 0; j < N; ++j ){
        for( int k = 0; k < N; ++k ){
            int idx = j*N + k;
            f[idx] = out[idx]/(N*N);
        }
     }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}

void DFT_c2rD3( double* f,double** fnyquist )
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * N * N * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N*N*N]();
    p = fftw_plan_dft_c2r_3d( N, N, N, in, out, FFTW_ESTIMATE );

//#pragma omp parallel for simd collapse(3) schedule( static ) num_threads( num_threads )
        for( int j = 0; j < N; ++j ){
            for( int k = 0; k < N; ++k ){
                for( int l = 0; l < N/2+1; ++l ){
                    int idx = (j*N + k)*(N/2+1) + l;
                    if(l == N/2){
                        in[idx][0] = fnyquist[j][2*k] ;
                        in[idx][1] = fnyquist[j][2*k+1] ;
                    }
                    else{
                        in[idx][0] =  f[(j*N + k)*N + 2*l] ;
                        in[idx][1] =  f[(j*N + k)*N + 2*l+1] ;
                    }
                }
            }
        }
        fftw_execute(p);

        // Set output data

         for( int j = 0; j < N; ++j ){
        for( int k = 0; k < N; ++k ){
            for( int l = 0; l < N; ++l ){
                int idx = (j*N + k)*N + l;
                f[idx] = out[idx]/(N*N*N);
            }
        }
    }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}

/*
void DFT_c2r( double** f,double* fnyquist )
{
	  fftw_plan p;
		double* out;
    fftw_complex *in;
		size_t in_size;

		switch( dim )
		{
			case 1:
				in_size = sizeof(fftw_complex) * (N/2+1);
				in  = (fftw_complex*)fftw_malloc( in_size );
				out = new double [N]();
				p = fftw_plan_dft_c2r_1d( N, in, out, FFTW_ESTIMATE );
				break;
			case 2:
				in_size = sizeof(fftw_complex) * N * (N/2+1);
				in  = (fftw_complex*)fftw_malloc( in_size );
				out = new double [N*N]();
				p = fftw_plan_dft_c2r_2d( N, N, in, out, FFTW_ESTIMATE );
				break;
			case 3:
				in_size = sizeof(fftw_complex) * N * N * (N/2+1);
				in  = (fftw_complex*)fftw_malloc( in_size );
				out = new double [N*N*N]();
				p = fftw_plan_dft_c2r_3d( N, N, N, in, out, FFTW_ESTIMATE );
				break;
		}

		for( int i = 0; i < num_fields; ++i )
		{

				// Create input data
				switch( dim ){
					case 1:
						#pragma omp parallel for schedule( static ) num_threads( num_threads )
						for( int j = 0; j < N/2+1; ++j ){
							int idx = j;
							if(idx==0)
                            {
                                in[idx][0] = f[i][idx]  ;
                                in[idx][1] = 0  ;

                            }else if(idx==N/2){
                                in[idx][0] = f[i][1]  ;
                                in[idx][1] = 0  ;
                            }else{
                                in[idx][0] = f[i][2*idx]  ;
                                in[idx][1] = f[i][2*idx+1]  ;
                            }

						}
						break;
					case 2:
						#pragma omp parallel for schedule( static ) num_threads( num_threads )
						for( int j = 0; j < N; ++j ){
							for( int k = 0; k < N/2+1; k=k+2 ){
									int idx = j*N + k;
                                if(k == N/2){
                                    in[idx][0] = fnyquist[2*j] ;
                                    in[idx][1] = fnyquist[2*j+1] ;
                                }else {
                                    in[idx][0] =  f[i][idx] ;
                                    in[idx][1] =  f[i][idx+1] ;
                                }
							}
						}
						break;
					case 3:
						#pragma omp parallel for schedule( static ) num_threads( num_threads )
						for( int j = 0; j < N; ++j ){
							for( int k = 0; k < N; ++k ){
									for( int l = 0; l < N/2+1; l=l+2 ){
											int idx = (j*N + k)*N + l;
                                        if(l == N/2){
                                        in[idx][0] = fnyquist[j][2*k] ;
                                        in[idx][1] = fnyquist[j][2*k+1] ;
                                        }
                                        else{
                                            in[idx][0] =  f[i][idx] ;
                                            in[idx][1] =  f[i][idx+1] ;
                                        }
                                        }
							}
						}
						break;
				}

				fftw_execute(p);

				// Set output data
        #pragma omp parallel for schedule( static ) num_threads( num_threads )
        for( int j = 0; j < N; ++j ){
            switch( dim ){
							case 1:
                int idx = j;
                f[i][idx] = out[idx]/N;
								break;
            	case 2:
                for( int k = 0; k < N; ++k ){
                    int idx = j*N + k;
                    f[i][idx] = out[idx]/(N*N);
                }
								break;
            	case 3:
                for( int k = 0; k < N; ++k ){
                    for( int l = 0; l < N; ++l ){
                        int idx = (j*N + k)*N + l;
                        f[i][idx] = out[idx]/(N*N*N);
                    }
                }
								break;
						}
        }
    }

		if( p ) fftw_destroy_plan(p);
		if( in ) fftw_free(in);
		delete[] out;
}
*/

void write_VTK_f( const std::string dir_f, double* f, std::string str, int loop )
{
   // double a = leapfrog->a();
	unsigned int size;
	std::stringstream ss;
	std::ofstream fout;

	if( dim == 1 ){
		ss << "../" << dir_f << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".txt";
    	fout.open( ss.str().c_str() );

    	for( int j = 0; j < N; j++ ){
			int idx = j;
			fout << idx*dx << " " << f[idx] << std::endl;
		}
    }else{
    	ss << "../" << dir_f << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".vti";
    	fout.open( ss.str().c_str() );

  	  	fout << "<?xml version=\"1.0\"?>" << std::endl;
  		fout << "<VTKFile type=\"ImageData\" version=\"1.0\" byte_order=\"LittleEndian\" header_type=\"UInt32\">" << std::endl <<std::endl;
    	switch( dim ){
    		case 2:
				size = sizeof(double) * pow(N, 2);//8byte*pow(N,2)
				fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
				fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\">" << std::endl;
    			break;
			case 3:
				size = sizeof(double) * pow(N, 3);
				fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 <<" 0 " << N-1 << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
				fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << N-1 << "\">" << std::endl;
				break;
    	}
    	fout << "<PointData Scalars=\"field\">" << std::endl;
    	fout << "<DataArray type=\"Float64\" Name=\"field\" format=\"appended\" offset=\"0\" />" << std::endl;
    	fout << "</PointData>" << std::endl;
	    fout << "<CellData>" << std::endl;
	    fout << "</CellData>" << std::endl;
	    fout << "</Piece>" << std::endl << std::endl;

	    fout << "</ImageData>" << std::endl << std::endl;
	    fout << "<AppendedData encoding=\"raw\">" << std::endl;
	    fout << "_" ;
	    fout.close();

	    fout.open( ss.str().c_str(), std::ios::binary | std::ios::app);
	    fout.write( (char*) &size, sizeof(unsigned int) );//4byte

           fout.write( (char*) f, size );
	    fout.close();
	    fout.open( ss.str().c_str(), std::ios::app);
	    fout << std::endl << "</AppendedData>" << std::endl;
		fout << "</VTKFile>" ;
	}
	fout.close();
}

void write_VTK_ed( const std::string dir_ed, double* f, std::string str, int loop )
{
  //  double a = leapfrog->a();
   // std::cout << "aaa = " <<  a << std::endl;
    unsigned int size;
    std::stringstream ss;
    std::ofstream fout;

    if( dim == 1 ){
        ss << "../" << dir_ed << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".txt";
        fout.open( ss.str().c_str() );

        for( int j = 0; j < N; j++ ){
            int idx = j;
            fout << idx*dx << " " << f[idx] << std::endl;
        }
    }else{
        ss << "../" << dir_ed << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".vti";
        fout.open( ss.str().c_str() );

        fout << "<?xml version=\"1.0\"?>" << std::endl;
        fout << "<VTKFile type=\"ImageData\" version=\"1.0\" byte_order=\"LittleEndian\" header_type=\"UInt32\">" << std::endl <<std::endl;
        switch( dim ){
            case 2:
                size = sizeof(double) * pow(N, 2);//8byte*pow(N,2)
                fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
                fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\">" << std::endl;
                break;
            case 3:
                size = sizeof(double) * pow(N, 3);
                fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 <<" 0 " << N-1 << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
                fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << N-1 << "\">" << std::endl;
                break;
        }
        fout << "<PointData Scalars=\"energy\">" << std::endl;
        fout << "<DataArray type=\"Float64\" Name=\"energy density\" format=\"appended\" offset=\"0\" />" << std::endl;
        fout << "</PointData>" << std::endl;
        fout << "<CellData>" << std::endl;
        fout << "</CellData>" << std::endl;
        fout << "</Piece>" << std::endl << std::endl;

        fout << "</ImageData>" << std::endl << std::endl;
        fout << "<AppendedData encoding=\"raw\">" << std::endl;
        fout << "_" ;
        fout.close();

        fout.open( ss.str().c_str(), std::ios::binary | std::ios::app);
        fout.write( (char*) &size, sizeof(unsigned int) );//4byte

            fout.write( (char*) f, size );

        fout.close();
        fout.open( ss.str().c_str(), std::ios::app);
        fout << std::endl << "</AppendedData>" << std::endl;
        fout << "</VTKFile>" ;
    }
    fout.close();
}

void write_status( const std::string status_file, Field* field, LeapFrog* leapfrog, Energy* energy, double** f, double t )
{
	double a = leapfrog->a();
	std::ofstream ofs;

	if( t == t0 )
	{
		ofs.open( "../" + status_file, std::ios::trunc );

		ofs << std::setw(3) << std::right << "  t ";
		if( expansion ) ofs << "  a ";
		for( int i = 0; i < num_fields; ++i ) ofs << "field_ave["  << i << "] ";
		for( int i = 0; i < num_fields; ++i ) ofs << "field_var["  << i << "] ";
        for( int i = 0; i < num_fields; ++i ) ofs << "field_deriv_ave["  << i << "] ";
        for( int i = 0; i < num_fields; ++i ) ofs << "field_deriv_var["  << i << "] ";
		for( int i = 0; i < num_fields; ++i ) ofs << "energy_ave[" << i << "] ";
        for( int i = 0; i < num_fields; ++i ) ofs << "energy_var[" << i << "] ";
        ofs << "total_energy_ave ";
         ofs << "time_deriv_ave ";
         ofs << "gradient_ave ";
        ofs << "potential_ave ";
        ofs << "hubble ";
        ofs << "adotdot ";
        ofs << "energy_max" << std::endl;
	}
	else ofs.open( "../" + status_file, std::ios::app );

	ofs << std::setw(3) << std::right << t << " ";
	if( expansion )
	{
		ofs << std::setw(3) << std::right << a << " ";
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_average(f[i], i)/a << " "; //Reduced Plank units
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_variance(f[i], i)/a << " ";//Reduced Plank units
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_average(f[i], i) << " ";//Programming variable
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_variance(f[i], i) << " ";//Programming variable
	}
	else
	{
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_average(f[i], i) << " ";//Reduced Plank units
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_variance(f[i], i) << " ";//Reduced Plank units
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_average(f[i], i) << " ";//Programming variable
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_variance(f[i], i) << " ";//Programming variable
	}
	for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << energy->average(i) << " ";
    for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << energy->variance(i) << " ";
	ofs << std::showpos << std::scientific << std::setprecision(4) << energy->total_average() << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << energy->timederiv_average () << " ";
     ofs << std::showpos << std::scientific << std::setprecision(4) << energy->grad_average ()  << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << energy->potential_average () << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << leapfrog->hubble() << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << leapfrog->adotdot() << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << energy->energy_max() << std::endl;

}