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


#include "subsystems/SwerveModuleSubsystem.h"

#include <iostream>
#include <memory>

#include <Eigen/Core>
#include <fmt/format.h>
#include <frc/MathUtil.h>
#include <frc/RobotBase.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <spdlog/spdlog.h>
#include <wpi/numbers>

#include "CustomUnits.h"

SwerveModuleSubsystem::SwerveModuleSubsystem(int throttlePort, int steeringPort, units::radian_t steeringOffset)
  : m_throttleMotor(std::make_unique<WPI_TalonFX>(throttlePort, "canivore")),
    m_steeringMotor(std::make_unique<WPI_TalonFX>(steeringPort, "canivore")), m_steeringoffset(steeringOffset)
{
    SetName(fmt::format("SwerveModuleSubsystem({}, {})", throttlePort, steeringPort));

    m_throttleMotor->ConfigFactoryDefault();
    m_throttleMotor->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor);
    m_throttleMotor->SetNeutralMode(NeutralMode::Brake);

    if (frc::RobotBase::IsReal())
    {
        m_throttleMotor->SetInverted(false);
    }

    m_steeringMotor->ConfigFactoryDefault();
    m_steeringMotor->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor);
    m_steeringMotor->SetNeutralMode(NeutralMode::Brake);
    m_steeringMotor->ConfigIntegratedSensorInitializationStrategy(SensorInitializationStrategy::BootToAbsolutePosition);
    m_steeringMotor->ConfigClosedloopRamp(0.05);
    m_steeringMotor->Config_kP(0, 0.2);
    m_steeringMotor->Config_kD(0, 0.005);
    m_steeringMotor->ConfigClosedLoopPeakOutput(0, .66);
    m_steeringMotor->ConfigVoltageMeasurementFilter(true);
    m_steeringMotor->ConfigVoltageCompSaturation(6);
    if (frc::RobotBase::IsReal())
    {
        m_steeringMotor->SetInverted(true);
    }

    m_throttleMotor->SetStatusFramePeriod(StatusFrame::Status_2_Feedback0_, Constants::LoopPeriod / 1_ms);
    m_steeringMotor->SetStatusFramePeriod(StatusFrame::Status_2_Feedback0_, Constants::LoopPeriod / 1_ms);
    // spdlog::info("{}, {}", throttlePort, m_steeringMotor->GetSelectedSensorPosition());
}

frc::SwerveModuleState SwerveModuleSubsystem::OptimizeStateContinuous(frc::SwerveModuleState state)
{
    // Optimizes module state for a continuously rotating module.
    //
    // For example, if the desired angle is 10 degrees, then 10, 190, 370, etc. are all valid angles as well. The same
    // goes for the negative direction. Given our current angle, we want to find the nearest member of that sequence
    // and use it as our optimized angle. We do this by factoring the desired angle out of both the sequence and our
    // current angle, then finding x s.t. x * 180 is closest to the adjusted current angle.
    auto measuredDegrees = GetMeasuredRotation().Degrees();
    auto stateDegrees = state.angle.Degrees();
    int nearest180 = units::math::round((measuredDegrees - stateDegrees) / 180_deg);
    bool shouldInvertThrottle = units::math::abs(frc::AngleModulus(stateDegrees - measuredDegrees)) > 90_deg;
    return {
        state.speed * (shouldInvertThrottle ? -1.0 : 1.0),
        nearest180 * 180_deg + stateDegrees,
    };
}

void SwerveModuleSubsystem::SetDesiredState(frc::SwerveModuleState desiredState)
{
    m_desiredState = {desiredState.speed, desiredState.speed == 0_mps ? m_desiredState.angle : desiredState.angle};
}

frc::Rotation2d SwerveModuleSubsystem::GetMeasuredRotation()
{
    return {
        (m_steeringMotor->GetSelectedSensorPosition() / Constants::TicksPerRevolution::TalonFX - m_steeringoffset) /
        Constants::SwerveModule::SteeringGearing};
}
frc::SwerveModuleState SwerveModuleSubsystem::GetMeasuredState()
{
    return {GetMeasuredVelocity(), GetMeasuredRotation()};
}

decltype(0_mps) SwerveModuleSubsystem::GetMeasuredVelocity()
{
    return m_throttleMotor->GetSelectedSensorVelocity() / Constants::TicksPerRevolution::TalonFX /
        Constants::VelocityFactor::TalonFX / Constants::SwerveModule::ThrottleGearing *
        Constants::SwerveModule::RolloutRatio;
}

void SwerveModuleSubsystem::TestingVoltage()
{
    m_throttleMotor->Set(ControlMode::PercentOutput, .1);
}

void SwerveModuleSubsystem::Periodic()
{
    auto optimizedState = OptimizeStateContinuous(m_desiredState);
    auto offsetAngle = optimizedState.angle.Radians() + m_steeringoffset / Constants::SwerveModule::SteeringGearing;

    auto throttlefeedforward =
        units::volt_t{m_throttleFeedforward.Calculate(Eigen::Vector<double, 1>(optimizedState.speed.value()))[0]};
    if (units::math::abs(throttlefeedforward) < Constants::MinimumFFVoltage)
    {
        throttlefeedforward = 0_V;
    }
    auto steeringfeedforward =
        units::volt_t{m_steeringFeedforward.Calculate(Eigen::Vector2d(offsetAngle.value(), 0.0))[0]};
    if (units::math::abs(steeringfeedforward) < Constants::MinimumFFVoltage)
    {
        steeringfeedforward = 0_V;
    }

    m_throttleMotor->Set(
        ControlMode::Velocity,
        optimizedState.speed / Constants::SwerveModule::RolloutRatio * Constants::SwerveModule::ThrottleGearing *
            Constants::VelocityFactor::TalonFX * Constants::TicksPerRevolution::TalonFX,
        DemandType::DemandType_ArbitraryFeedForward,
        (throttlefeedforward + Constants::SwerveModule::ThrottleKs * wpi::sgn(throttlefeedforward)) / 12.0_V);
    m_steeringMotor->Set(
        ControlMode::Position,
        offsetAngle * Constants::SwerveModule::SteeringGearing * Constants::TicksPerRevolution::TalonFX,
        DemandType::DemandType_ArbitraryFeedForward,
        (steeringfeedforward + Constants::SwerveModule::SteeringKs * wpi::sgn(steeringfeedforward)) / 12.0_V);

    // std::cout << "Raw Steering Motor Position: " << m_steeringMotor->GetDeviceID() << ": "
    //         << m_steeringMotor->GetSelectedSensorPosition() << std::endl;
    // frc::SmartDashboard::PutNumber(
    //    fmt::format("{}.RawPosition", GetName()), m_steeringMotor->GetSelectedSensorPosition());
    // frc::SmartDashboard::PutNumber(fmt::format("{}.DesiredThrottleVelocity", GetName()),
    // optimizedState.speed.value()); frc::SmartDashboard::PutNumber(
    //    fmt::format("{}.DesiredSteeringPosition", GetName()), optimizedState.angle.Radians().value());
    // frc::SmartDashboard::PutNumber(
    //    fmt::format("{}.MeasuredThrottleVelocity", GetName()), GetMeasuredVelocity().value());
    // frc::SmartDashboard::PutNumber(
    //    fmt::format("{}.MeasuredSteeringPosition", GetName()), GetMeasuredRotation().Radians().value());
}

void SwerveModuleSubsystem::SimulationPeriodic()
{
    m_throttleSim.SetInput(0, m_throttleMotor->GetMotorOutputVoltage());
    m_steeringSim.SetInput(0, m_steeringMotor->GetMotorOutputVoltage());

    m_throttleSim.Update(Constants::LoopPeriod);
    m_steeringSim.Update(Constants::LoopPeriod);

    auto throttleMotorSim = m_throttleMotor->GetSimCollection();
    auto throttleVelocityTicks = m_throttleSim.GetOutput(0) * 1_mps / Constants::SwerveModule::RolloutRatio *
        Constants::SwerveModule::ThrottleGearing * Constants::TicksPerRevolution::TalonFX;
    throttleMotorSim.SetIntegratedSensorVelocity(throttleVelocityTicks * Constants::VelocityFactor::TalonFX);
    throttleMotorSim.AddIntegratedSensorPosition(throttleVelocityTicks * Constants::LoopPeriod);

    auto steeringMotorSim = m_steeringMotor->GetSimCollection();
    auto steeringPositionTicks = m_steeringSim.GetOutput(0) * 1_rad * Constants::SwerveModule::SteeringGearing *
        Constants::TicksPerRevolution::TalonFX;
    steeringMotorSim.SetIntegratedSensorRawPosition(steeringPositionTicks);
}

void SwerveModuleSubsystem::Reset()
{
    m_desiredState = {};
    // TODO: use mag-encoders to reset integrated relative encoders
    m_throttleFeedforward.Reset(Eigen::Vector<double, 1>(GetMeasuredVelocity().value()));
    m_steeringFeedforward.Reset(Eigen::Vector<double, 2>(GetMeasuredRotation().Radians().value(), 0.0));
}


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

#include "fw/foc.h"

#include <boost/test/auto_unit_test.hpp>

using namespace moteus;

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE(FocTest1) {
  Cordic cordic;
  SinCos sin_cos = cordic.radians(0.0);

  DqTransform dq(sin_cos, 2.0, 0., 0.);
  BOOST_TEST(dq.d == 1.3333333333f);
  BOOST_TEST(dq.q == 0.0);

  ClarkTransform ct(2.0, 0., 0.);
  BOOST_TEST(ct.x == 1.3333333333f);
  BOOST_TEST(ct.y == 0.0);

  InverseClarkTransform ict(ct.x, ct.y);
  BOOST_TEST(ict.a == 1.3333333333f);
  BOOST_TEST(ict.b == -0.666666666666f);
  BOOST_TEST(ict.c == -0.666666666666f);

  ParkTransform pt(sin_cos, ct.x, ct.y);
  BOOST_TEST(pt.d == dq.d);
  BOOST_TEST(pt.q == dq.q);

  InverseParkTransform ipt(sin_cos, pt.d, pt.q);
  BOOST_TEST(ipt.x == ct.x);
  BOOST_TEST(ipt.y == ct.y);

  InverseDqTransform idq(sin_cos, pt.d, pt.q);
  BOOST_TEST(idq.a == ict.a);
  BOOST_TEST(idq.b == ict.b);
  BOOST_TEST(idq.c == ict.c);
}

BOOST_AUTO_TEST_CASE(FocTest2) {
  Cordic cordic;
  SinCos sin_cos = cordic.radians(kPi / 2.0f);

  DqTransform dq(sin_cos, 2.0, 0., 0.);
  BOOST_TEST(std::abs(dq.d) <= 1e-5);
  BOOST_TEST(dq.q == -1.33333333333f);

  ClarkTransform ct(2.0, 0., 0.);
  BOOST_TEST(ct.x == 1.333333333333f);
  BOOST_TEST(ct.y == 0.0f);

  InverseClarkTransform ict(ct.x, ct.y);
  BOOST_TEST(ict.a == 1.333333333333f);
  BOOST_TEST(ict.b == -0.66666666666f);
  BOOST_TEST(ict.c == -0.66666666666f);

  ParkTransform pt(sin_cos, ct.x, ct.y);
  BOOST_TEST(pt.d == dq.d);
  BOOST_TEST(pt.q == dq.q);

  InverseParkTransform ipt(sin_cos, pt.d, pt.q);
  BOOST_TEST(ipt.x == ct.x);
  BOOST_TEST(ipt.y == ct.y);

  InverseDqTransform idq(sin_cos, pt.d, pt.q);
  BOOST_TEST(idq.a == ict.a);
  BOOST_TEST(idq.b == ict.b);
  BOOST_TEST(idq.c == ict.c);
}


#include "mesh.h"

using namespace cslibs_vectormaps::dxf;

#include <gazebo/physics/Model.hh>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <boost/filesystem.hpp>
#include <ostream>
#include <istream>

namespace impl {

typedef Eigen::Vector3d Vec3d;

/// OPERATIONS
inline Vec3d cross(const Vec3d &p1,
                   const Vec3d &p2,
                   const Vec3d &p3)
{
    Vec3d  v_21 = p1 - p2;
    Vec3d  v_23 = p3 - p2;
    return v_21.cross(v_23);
}

/// DATA STRUCTURES
struct Vec3di {
    double operator [](const unsigned int i)
    {
        return data[i];
    }

    Vec3d        data;
    unsigned int index;
};

struct Triangle {
    unsigned int index[3];
    Vec3d        normal;

};

typedef std::vector<Triangle> Mesh;
typedef std::vector<Vec3di>   MeshPoints;

namespace Collada {
inline void copy(std::ifstream &in, std::ofstream &out)
{
    out << in.rdbuf();
}

inline void writeMesh(const Mesh &mesh,
                      const MeshPoints &pts,
                      std::ofstream &out)
{
    out << "<geometry id=\"Wall-mesh\" name=\"Wall\" >"
        << "\n";
    out << "<mesh>"
        << "\n";

    /// write mesh positions
    out << "<source id=\"Wall-mesh-positions\">"
        << "\n";
    out << "<float_array id=\"Wall-mesh-positions-array\" count=\"" << pts.size() * 3 << "\">"
        << "\n";

    for(MeshPoints::const_iterator it = pts.begin() ; it != pts.end() ; ++it) {
        out << it->data[0] << " " << it->data[1] << " " << it->data[2] << " ";
    }
    out << "</float_array>"
        << "\n";

    out << "<technique_common>"
        << "\n";
    out << "<accessor source=\"#Wall-mesh-positions-array\" count=\"" << pts.size() << "\" stride=\"3\">"
        << "\n";
    out << "<param name=\"X\" type=\"float\"/>"
        << "\n";
    out << "<param name=\"Y\" type=\"float\"/>"
        << "\n";
    out << "<param name=\"Z\" type=\"float\"/>"
        << "\n";
    out << "</accessor>"
        << "\n";
    out << "</technique_common>"
        << "\n";
    out << "</source>"
        << "\n";

    /// write normals
    out << "<source id=\"Wall-mesh-normals\">"
        << "\n";
    out << "<float_array id=\"Wall-mesh-normals-array\" count=\"" << mesh.size() * 3 * 2<< "\">"
        << "\n";

    for(Mesh::const_iterator it = mesh.begin() ; it != mesh.end() ; ++it) {
        out << it->normal[0] << " " << it->normal[1] << " " << it->normal[2] << " ";
        out << it->normal[0] * (-1) << " " << it->normal[1] * (-1) << " " << it->normal[2] * (-1) << " ";
    }
    out << "</float_array>"
        << "\n";

    out << "<technique_common>"
        << "\n";
    out << "<accessor source=\"#Wall-mesh-normals-array\" count=\"" << mesh.size() * 2 << "\" stride=\"3\">"
        << "\n";
    out << "<param name=\"X\" type=\"float\"/>"
        << "\n";
    out << "<param name=\"Y\" type=\"float\"/>"
        << "\n";
    out << "<param name=\"Z\" type=\"float\"/>"
        << "\n";
    out << "</accessor>"
        << "\n";
    out << "</technique_common>"
        << "\n";
    out << "</source>"
        << "\n";

    /// write vertices
    out << "<vertices id=\"Wall-mesh-vertices\">"
        << "\n";
    out << "<input semantic=\"POSITION\" source=\"#Wall-mesh-positions\"/>"
        << "\n";
    out << "</vertices>"
        << "\n";
    out << "<polylist count=\"" << mesh.size() * 2 << "\" material=\"Material_001-material\">"
        << "\n";
    out << "<input semantic=\"VERTEX\" source=\"#Wall-mesh-vertices\" offset=\"0\"/>"
        << "\n";
    out << "<input semantic=\"NORMAL\" source=\"#Wall-mesh-normals\" offset=\"1\"/>"
        << "\n";

    out << "<vcount>";
    for(unsigned int i = 0 ; i < mesh.size() * 2; i++) {
        out << 3 << " ";
    }
    out <<"</vcount>"
       << "\n";
    out << "<p>";
    for(unsigned int i = 0 ; i < mesh.size() ; i++) {
        out << mesh[i].index[0] << " " << 2 * i << " " ;
        out << mesh[i].index[1] << " " << 2 * i << " " ;
        out << mesh[i].index[2] << " " << 2 * i << " " ;
        out << mesh[i].index[2] << " " << 2 * i + 1 << " " ;
        out << mesh[i].index[1] << " " << 2 * i + 1 << " " ;
        out << mesh[i].index[0] << " " << 2 * i + 1 << " " ;
    }
    out << "</p>"
        << "\n";
    out << "</polylist>"
        << "\n";
    out << "</mesh>"
        << "\n";
    out << "<extra><technique profile=\"MAYA\"><double_sided>1</double_sided></technique></extra>"
        << "\n";
    out << "</geometry>"
        << "\n";
}

inline void insertToScene(std::ofstream &out)
{
    out << "<node id=\"Wall\" name=\"Wall\" type=\"NODE\">" << "\n";
    out << "<instance_geometry url=\"#Wall-mesh\">" << "\n";
    out << "<bind_material>" << "\n";
    out << "<technique_common>" << "\n";
    out << "<instance_material symbol=\"Material_001-material\" target=\"#Material_001-material\"/>" << "\n";
    out << "</technique_common>" << "\n";
    out << "</bind_material>" << "\n";
    out << "</instance_geometry>" << "\n";
    out << "</node>" << "\n";
}

inline void writeCollada(const Mesh       &mesh,
                         const MeshPoints &pts,
                         std::ifstream    &res,
                         std::ofstream    &out)
{
    out << "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
        << "\n";
    out << "<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">"
        << "\n";
    copy(res, out);
    out << "<library_geometries>"
        << "\n";
    writeMesh(mesh, pts, out);
    out << "</library_geometries>"
        << "\n";
    out << "<library_visual_scenes>"
        << "\n";
    out << "<visual_scene id=\"Scene\" name=\"Scene\">"
        << "\n";
    insertToScene(out);
    out << "</visual_scene>"
        << "\n";
    out << "</library_visual_scenes>"
        << "\n";
    out << "<scene><instance_visual_scene url=\"#Scene\"/></scene>"
        << "\n";
    out << "</COLLADA>"
        << "\n";

}
}

inline void addTriangles(const Vec3di &first_lower,  const Vec3di &first_upper,
                         const Vec3di &second_lower, const Vec3di &second_upper,
                         Mesh &m)
{
    Triangle lower;
    lower.index[2] = second_lower.index;
    lower.index[1] = first_lower.index;
    lower.index[0] = first_upper.index;
    lower.normal = cross(second_lower.data,
                         first_lower.data,
                         first_upper.data);

    Triangle upper;
    upper.index[0] = second_lower.index;
    upper.index[1] = second_upper.index;
    upper.index[2] = first_upper.index;
    upper.normal = cross(first_upper.data,
                         second_upper.data,
                         second_lower.data);

    m.push_back(lower);
    m.push_back(upper);
}

inline void generateWall(const DXFMap::Vector &vector,
                         const double          height,
                         Mesh       &m,
                         MeshPoints &ps)
{
    Vec3di first_lower;
    Vec3di first_upper;
    first_lower.data[0] = vector.first.x();
    first_lower.data[1] = vector.first.y();
    first_lower.data[2] = 0.0;
    first_upper.data = first_lower.data;
    first_upper.data[2] = height;

    first_lower.index = ps.size();
    ps.push_back(first_lower);
    first_upper.index = ps.size();
    ps.push_back(first_upper);

    Vec3di second_lower;
    Vec3di second_upper;
    second_lower.data[0] = vector.second.x();
    second_lower.data[1] = vector.second.y();
    second_lower.data[2] = 0.0;
    second_upper.data = second_lower.data;
    second_upper.data[2] = height;

    second_lower.index = ps.size();
    ps.push_back(second_lower);
    second_upper.index = ps.size();
    ps.push_back(second_upper);

    addTriangles(first_lower, first_upper,
                 second_lower, second_upper,
                 m);
}

inline void generateMesh(const DXFMap::Vectors &vectors,
                         const std::string     &mesh_path,
                         const std::string     &common_path,
                         const double           height)
{
    Mesh       m;
    MeshPoints ps;

    for(DXFMap::Vectors::const_iterator
        it = vectors.begin() ;
        it != vectors.end() ;
        ++it) {
        const DXFMap::Vector &vector = *it;
        generateWall(vector,
                     height,
                     m,
                     ps);
    }

    std::ifstream in(common_path.c_str());
    std::ofstream out(mesh_path.c_str());
    Collada::writeCollada(m, ps, in, out);
    in.close();
    out.close();
}
}

Mesh::Mesh(const std::string _material,
           const double _height) :
    material(_material),
    height(_height)
{

}

bool Mesh::generate(const DXFMap::Vectors &vectors,
                    const std::string &common_content_path,
                    const std::string &save_path)
{
    boost::filesystem::path fs_common_content_path = common_content_path;
    if(!boost::filesystem::exists(fs_common_content_path))
        return false;

    std::string   mesh_path = save_path + "_mesh.dae";
    impl::generateMesh(vectors,
                       mesh_path,
                       fs_common_content_path.string(),
                       height);

    std::ofstream out(save_path.c_str());

    out << "<sdf version='1.4'>"
        << "\n";
    out << "<model name ='map'>"
        << "\n";
    out << "<static>true</static>"
        << "\n";
    out << "<pose>0 0 0 0 0 0</pose>"
        << "\n";
    out << "<link name ='world'>"
        << "\n";
    out << "<pose>0 0 0 0 0 0</pose>"
        << "\n";
    out << "<collision name ='collision'>"
        << "\n";
    out << "<geometry>"
        << "\n";
    out << "<mesh><uri>file://" << mesh_path << "</uri></mesh>"
        << "\n";
    out << "</geometry>"
        << "\n";
    out << "</collision>"
        << "\n";
    out << "<visual name ='visual'>"
        << "\n";
    out << "<geometry>"
        << "\n";
    out << "<mesh><uri>file://" << mesh_path << "</uri></mesh>"
        << "\n";
    out << "</geometry>"
        << "\n";
    out << "<material><script><name>Gazebo/" + material + "</name></script></material>"
        << "\n";
    out << "</visual>"
        << "\n";
    out << "</link>"
        << "\n";
    out << "</model>"
        << "\n";
    out << "</sdf>"
        << "\n";
    out.close();

    return true;
}

bool Mesh::generate(const DXFMap::Vectors &vectors,
                    const std::string     &common_content_path)
{
    model.reset(new sdf::SDF);

    boost::filesystem::path fs_common_content_path = common_content_path;
    boost::filesystem::path fs_mesh_path;
    boost::filesystem::path tmp_dir   = boost::filesystem::temp_directory_path();
    fs_mesh_path = tmp_dir.string() + "/" + boost::filesystem::unique_path().string() + ".dae";
    while(boost::filesystem::exists(fs_mesh_path)) {
        fs_mesh_path = tmp_dir.string() + "/" + boost::filesystem::unique_path().string() + ".dae";
    }

    if(!boost::filesystem::exists(fs_common_content_path))
        return false;

    std::stringstream obj_string;

    impl::generateMesh(vectors,
                       fs_mesh_path.string(),
                       fs_common_content_path.string(),
                       height);

    obj_string << "<sdf version='1.4'>"
               << "\n";
    obj_string << "<model name ='map'>"
               << "\n";
    obj_string << "<static>true</static>"
               << "\n";
    obj_string << "<pose>0 0 0 0 0 0</pose>"
               << "\n";
    obj_string << "<link name ='world'>"
               << "\n";
    obj_string << "<pose>0 0 0 0 0 0</pose>"
               << "\n";
    obj_string << "<collision name ='collision'>"
               << "\n";
    obj_string << "<geometry>"
               << "\n";
    obj_string << "<mesh><uri>file://" << fs_mesh_path.string() << "</uri></mesh>"
               << "\n";
    obj_string << "</geometry>"
               << "\n";
    obj_string << "</collision>"
               << "\n";
    obj_string << "<visual name ='visual'>"
               << "\n";
    obj_string << "<geometry>"
               << "\n";
    obj_string << "<mesh><uri>file://" << fs_mesh_path.string() << "</uri></mesh>"
               << "\n";
    obj_string << "</geometry>"
               << "\n";
    obj_string << "<material><script><name>Gazebo/" + material + "</name></script></material>"
               << "\n";
    obj_string << "</visual>"
               << "\n";
    obj_string << "</link>"
               << "\n";
    obj_string << "</model>"
               << "\n";
    obj_string << "</sdf>"
               << "\n";

    model->SetFromString(obj_string.str());

    return true;
}

void Mesh::setName(const std::string &name)
{
    if(model) {
        sdf::ElementPtr root = model->root->GetElement("model");
        root->GetAttribute("name")->SetFromString(name);
    }
}


///////////////////////////////////////////////////////////////////////////////
// distribution::model::example::example1.cpp                                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef LIBS_STATISTICS_DETAIL_DISTRIBUTION_MODEL_EXAMPLE_EXAMPLE1_CPP_ER_2009
#define LIBS_STATISTICS_DETAIL_DISTRIBUTION_MODEL_EXAMPLE_EXAMPLE1_CPP_ER_2009
#include <boost/statistics/detail/distribution/model/models/likelihood/iid/iid.hpp>
#include <boost/statistics/detail/distribution/model/models/posterior/posterior.hpp>

void example1(std::ostream&)
{

}

#endif