#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sophus/so3.h>
#include <sophus/se3.h>

#include <stdio.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <opencv2/core/eigen.hpp>

using namespace std;

cv::Mat convertRt2T(const cv::Mat &R, const cv::Mat &t)
{
    cv::Mat T = (cv::Mat_<double>(4, 4) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
            R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
            R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0),
            0, 0, 0, 1);
    return T;
}

Sophus::SE3 transT_cv2sophus(const cv::Mat &T_cv)
{
    Eigen::Matrix3d R_eigen;
    cv::cv2eigen(T_cv(cv::Rect2d(0, 0, 3, 3)), R_eigen);
    Eigen::Vector3d t_eigen(T_cv.at<double>(0, 3), T_cv.at<double>(1, 3), T_cv.at<double>(2, 3));
    Sophus::SE3 SE3_Rt(R_eigen, t_eigen);
    return SE3_Rt;
}

cv::Mat transT_sophus2cv(const Sophus::SE3 &T_sophus)
{
    Eigen::Vector3d eigen_t(T_sophus.translation());
    Eigen::Matrix3d eigen_R(T_sophus.rotation_matrix());

    cv::Mat cv_t, cv_R;
    eigen2cv(eigen_t, cv_t);
    eigen2cv(eigen_R, cv_R);

    return convertRt2T(cv_R, cv_t);
}

int main(int argc, char **argv)
{
    // -------------------- Init value from OpenCV --------------------
    cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat rvec = (cv::Mat_<double>(3, 1) << 0.3, 0.1, -0.5);
    cv::Rodrigues(rvec, R);
    cv::Mat tvec = (cv::Mat_<double>(3, 1) << 1, 2, 3);

    cout << "----- Initial value in OpenCV form -----" << endl;
    cout << "R=\n"
         << R << endl;
    cout << "R_vec = " << rvec.t() << endl;
    cout << "t=" << tvec.t() << endl;

    // -------------------- Convert to Sophus --------------------
    Sophus::SE3 T = Sophus::SE3(
            Sophus::SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
            Eigen::Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0)));
    cout << "Change form to Sophus:" << endl;
    cout << "Sophus::SE3 T = \n"
         << T << endl;

    // -------------------- Then convert to Eigen --------------------
    cout << "\n\nChange back to Eigen:" << endl;
    Eigen::Vector3d eigen_t(T.translation());
    Eigen::Matrix3d eigen_R(T.rotation_matrix());
    cout << eigen_t << endl;
    cout << eigen_R << endl;

    // -------------------- Then convert to OpenCV --------------------
    cout << "\n\nChange back to OpenCV:" << endl;
    cv::Mat cv_t, cv_R;
    eigen2cv(eigen_t, cv_t);
    eigen2cv(eigen_R, cv_R);
    cout << cv_t << endl;
    cout << cv_R << endl;

    // -------------------- Direct trans from T_cv to T_Sophus --------------------
    cv::Mat T_cv = convertRt2T(R, tvec);
    Sophus::SE3 T_SE3 = transT_cv2sophus(T_cv);
    cout << "\nDirect trans from T_cv to T_Sophus: \n"
         << T_SE3 << endl;
    // -------------------- Direct trans from T_sophus to T_cv --------------------
    T_cv = transT_sophus2cv(T_SE3);
    cout << "\nDirect trans from T_sophus to T_cv:\n"
         << T_cv << endl;
    return 0;
}


/*
 * Introduction to the header template.
 * For each function FUNC, the file name shall be FUNC.hpp in src/DeemaAlomair folder.
 * Every FUNC shall be replaced with implemented function name.
 * The type of a CFS with FLOAT is CFST<FLOAT>
 * The code shall follow c++17 standard.
 * Only the c++ standard libraries are allowed.
 * C-style code is highly not recommended.
 */

#ifndef FSL_INTEGRAL_H
#define FSL_INTEGRAL_H

/*Other library inclusion is here*/
#include <boost/math/quadrature/trapezoidal.hpp>
/*
template<class F, class Real>
Real trapezoidal(F f, Real a, Real b,
                 Real tol = sqrt(std::numeric_limits<Real>::epsilon()),
                 size_t max_refinements = 10,
                 Real* error_estimate = nullptr,
                 Real* L1 = nullptr);
*/

namespace FSL{
template<class FLOAT, class FUNC>
FLOAT Integral(FUNC f, FLOAT a, FLOAT b)
{
	using boost::math::quadrature::trapezoidal;
	return trapezoidal<FUNC, FLOAT>(f, a, b);
}
}
#endif

/*
 * Type N in MIS shall be translated to size_t in C++.
 * This type is already introduced in CFSData.h
 */


//qbr clang++ -std=c++11 test.cpp
//qbr ./a.out --force-colour -b

#include <iostream>

#include "pathfinding.hpp"

#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include <vector>
#include <boost/utility.hpp>
using namespace pf;

TEST_CASE( "SimpleGraph", "[graph]" ) {

	//the only requirement for a node is that it must implement '=='.
	struct SimpleNode : boost::noncopyable{

		SimpleNode( char name ) : name(name) {}

		char name;
		std::vector<SimpleNode*>adjacentNodes;

		//you can define the == operator pretty much anyway you'd like
		//even comparing memory locations works
		bool operator==(const SimpleNode& rhs) const{
			return ( this == &rhs );
		}
	};

	class SimpleAdaptor : Adaptor<SimpleNode>
	{
	public:
		using Adaptor::node_t;

		std::vector<node_t*> getAdjacentNodes(const node_t& node){
			return node.adjacentNodes;
		}

		unsigned heuristicDistanceBetweenAdjacentNodes(const node_t& a,
													   const node_t& b){
			return 1;
		}
	};

	SECTION( "chain" ) {
		//
		//  a--b--c
		//
		SimpleNode a('a');
		SimpleNode b('b');
		SimpleNode c('c');

		a.adjacentNodes.push_back( &b );
		b.adjacentNodes.push_back( &c );

		Search<SimpleAdaptor> search( a, c );
		auto results = search.path();
		REQUIRE( results.size() == 3 );
		REQUIRE( results[0]->name == 'a' );
		REQUIRE( results[1]->name == 'b' );
		REQUIRE( results[2]->name == 'c' );
	}

	SECTION( "3-node path vs. 2-node path" ) {
		//	  __u__
		//	 /	   \
		//	s		e
		//	 \d1_d2/
		//
		SimpleNode s('s');
		SimpleNode u('u');
		SimpleNode d1('d');
		SimpleNode d2('D');
		SimpleNode e('e');

		s.adjacentNodes.push_back( &u );
		s.adjacentNodes.push_back( &d1 );

		d1.adjacentNodes.push_back( &d2 );
		d2.adjacentNodes.push_back( &e );

		u.adjacentNodes.push_back( &e );

		Search<SimpleAdaptor> search( s, e );
		auto results = search.path();

		REQUIRE( results.size() == 3 );
		REQUIRE( results[0]->name == 's' );
		REQUIRE( results[1]->name == 'u' );
		REQUIRE( results[2]->name == 'e' );
	}

}

TEST_CASE( "Generating node's on the fly", "[graph]" ) {

	using namespace std;

	//std::pair is a perfectly fine node
	//it defines '=='.
	typedef std::pair<int,int> MyNode;

	class MyAdaptor : Adaptor<MyNode>
	{
	public:
		using Adaptor::node_t;
	 	const bool * grid;

		vector<unique_ptr<node_t> >garbageCollecter;
		MyAdaptor( const bool * grid ) : grid(grid){
		}

		bool validNode( int i, int j ){
			if( ( i >= 5 )||( i < 0 ) )
				return false;
			if( ( j >= 5 )||( j < 0 ) )
				return false;
			return grid[i+j*5] == 0;
		}

		node_t * makeNode( int i, int j ){
			return new pair<int, int>( i, j);
			garbageCollecter.push_back(
				unique_ptr<std::pair<int, int>>(
					new pair<int, int>(i,j)
					)
				);
			return garbageCollecter[ garbageCollecter.size() - 1].get();
		}

		std::vector<node_t*> getAdjacentNodes(const node_t& node){
			//std::cout << "getAdjacentNodes("<<node.first<<','<<node.second<<")\n";
			std::vector<std::pair<int,int>*> returnVector;
			int i = node.first;
			int j = node.second;
			if( validNode( i-1, j ) )
				returnVector.push_back( makeNode( i-1, j ) );
			if( validNode( i+1, j ) )
				returnVector.push_back( makeNode( i+1, j ) );
			if( validNode( i, j-1 ) )
				returnVector.push_back( makeNode( i, j-1 ) );
			if( validNode( i, j+1 ) )
				returnVector.push_back( makeNode( i, j+1 ) );
			return returnVector;
		}

		unsigned heuristicDistanceBetweenAdjacentNodes(const node_t& a,
													   const node_t& b){
			return abs(a.first - b.first + a.second - b.second);
		}
	};

	SECTION( "open grid" ) {
		const bool grid[25] =
				{	0,	0,	0,	0,	0,
					0,	0,	0,	0,	0,
					0,	0,	0,	0,	0,
					0,	0,	0,	0,	0,
					0,	0,	0,	0,	0 };
		Search<MyAdaptor, const bool *> search(
			std::pair<int,int>(0,0), std::pair<int,int>(4,4), grid );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
	}

	SECTION( "one path grid" ) {
		const bool grid[25] =
				{	0,	1,	0,	0,	0,
					0,	1,	0,	0,	0,
					0,	1,	0,	0,	0,
					0,	1,	1,	1,	1,
					0,	0,	0,	0,	0 };
		Search<MyAdaptor, const bool *> search(
			std::pair<int,int>(0,0), std::pair<int,int>(4,4), grid );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
		REQUIRE( path[4]->first ==0 );
		REQUIRE( path[4]->second==4 );
	}

	SECTION( "better lower path grid" ) {
		const bool grid[25] =
				{	0,	0,	0,	0,	0,
					0,	1,	1,	1,	0,
					0,	1,	0,	0,	0,
					0,	1,	0,	1,	1,
					0,	0,	0,	0,	0 };
		Search<MyAdaptor, const bool *> search(
			std::pair<int,int>(0,0), std::pair<int,int>(4,4), grid );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
		REQUIRE( path[4]->first ==0 );
		REQUIRE( path[4]->second==4 );
	}

		SECTION( "better upper path grid" ) {
		const bool grid[25] =
				{	0,	0,	0,	0,	0,
					0,	1,	1,	1,	0,
					0,	1,	0,	0,	0,
					0,	1,	0,	1,	0,
					0,	0,	0,	1,	0 };
		Search<MyAdaptor, const bool *> search(
			std::pair<int,int>(0,0), std::pair<int,int>(4,4), grid );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
		REQUIRE( path[4]->first ==4 );
		REQUIRE( path[4]->second==0 );
	}

}

TEST_CASE( "spied on optimization check", "[graph]" ) {

	using namespace std;

	//std::pair is a perfectly fine node
	//it defines '=='.
	typedef std::pair<int,int> MyNode;

	class MyAdaptor : Adaptor<MyNode>
	{
	public:
		using Adaptor::node_t;
	 	const bool * grid;

		int &adjacentNodeAskCount;

		vector<unique_ptr<node_t> >garbageCollecter;
		MyAdaptor( const bool * grid, int &adjacentNodeAskCount ):
			grid(grid),
			adjacentNodeAskCount( adjacentNodeAskCount ){
		}

		bool validNode( int i, int j ){
			if( ( i >= 5 )||( i < 0 ) )
				return false;
			if( ( j >= 5 )||( j < 0 ) )
				return false;
			return grid[i+j*5] == 0;
		}

		node_t * makeNode( int i, int j ){
			return new pair<int, int>( i, j);
			garbageCollecter.push_back(
				unique_ptr<std::pair<int, int>>(
					new pair<int, int>(i,j)
					)
				);
			return garbageCollecter[ garbageCollecter.size() - 1].get();
		}

		std::vector<node_t*> getAdjacentNodes(const node_t& node){
			adjacentNodeAskCount++;
			std::vector<std::pair<int,int>*> returnVector;
			int i = node.first;
			int j = node.second;
			if( validNode( i-1, j ) )
				returnVector.push_back( makeNode( i-1, j ) );
			if( validNode( i+1, j ) )
				returnVector.push_back( makeNode( i+1, j ) );
			if( validNode( i, j-1 ) )
				returnVector.push_back( makeNode( i, j-1 ) );
			if( validNode( i, j+1 ) )
				returnVector.push_back( makeNode( i, j+1 ) );
			return returnVector;
		}

		unsigned heuristicDistanceBetweenAdjacentNodes(const node_t& a,
													   const node_t& b){
			return abs(a.first - b.first + a.second - b.second);
		}
	};

	SECTION( "open grid" ) {
		const bool grid[25] =
				{	0,	0,	0,	0,	0,
					0,	0,	0,	0,	0,
					0,	0,	0,	0,	0,
					0,	0,	0,	0,	0,
					0,	0,	0,	0,	0 };
		int adjacentNodeAskCount = 0;
		Search<MyAdaptor, const bool *, int& > search(
			std::pair<int,int>(0,0),
			std::pair<int,int>(4,4),
			grid,
			adjacentNodeAskCount );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
		REQUIRE( adjacentNodeAskCount == 8 );
	}

	SECTION( "zigzag grid" ) {
		const bool grid[25] =
				{	0,	1,	0,	0,	0,
					0,	1,	0,	1,	0,
					0,	1,	0,	1,	0,
					0,	1,	0,	1,	0,
					0,	0,	0,	1,	0 };
		int adjacentNodeAskCount = 0;
		Search<MyAdaptor, const bool *, int& > search(
			std::pair<int,int>(0,0),
			std::pair<int,int>(4,4),
			grid,
			adjacentNodeAskCount );
		auto path = search.path();
		REQUIRE( path.size() == 17 );
		REQUIRE( adjacentNodeAskCount == 16 );
	}

	SECTION( "lowerpath better grid" ) {
		const bool grid[25] =
				{	0,	0,	0,	0,	0,
					0,	1,	1,	1,	0,
					0,	1,	0,	0,	0,
					0,	1,	0,	1,	1,
					0,	0,	0,	0,	0 };
		int adjacentNodeAskCount = 0;
		Search<MyAdaptor, const bool *, int& > search(
			std::pair<int,int>(0,0),
			std::pair<int,int>(4,4),
			grid,
			adjacentNodeAskCount );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
		REQUIRE( adjacentNodeAskCount == 8 );
	}

	SECTION( "upper path better grid" ) {
		const bool grid[25] =
				{	0,	0,	0,	0,	0,
					0,	1,	1,	1,	0,
					0,	1,	0,	0,	0,
					0,	1,	0,	1,	0,
					0,	0,	0,	1,	0 };
		int adjacentNodeAskCount = 0;
		Search<MyAdaptor, const bool *, int& > search(
			std::pair<int,int>(0,0),
			std::pair<int,int>(4,4),
			grid,
			adjacentNodeAskCount );
		auto path = search.path();
		REQUIRE( path.size() == 9 );
		REQUIRE( adjacentNodeAskCount == 8 );
	}

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[make_3d_point
//` Using make to construct a three dimensional point

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>

int main()
{
    typedef boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian> point_type;
    point_type p = boost::geometry::make<point_type>(1, 2, 3);
    std::cout << boost::geometry::dsv(p) << std::endl;
    return 0;
}

//]

//[make_3d_point_output
/*`
Output:
[pre
(1, 2, 3)
]
*/
//]


//#####################################################################
//  Copyright (c) 2011-2019 Nathan Mitchell, Eftychios Sifakis, Yutian Tao.
//  This file is covered by the FreeBSD license. Please refer to the
//  license.txt file for more information.
//#####################################################################

//#include <PhysBAM_Tools/Matrices/MATRIX_3X3.h>
#include <iostream>
#include <iomanip>
#include <bitset> // for bitset
#include <type_traits> // for is_same
#include <stdexcept> // for logic error
#include <Eigen/Dense>

using namespace Eigen;

namespace{

    namespace {
        typedef union {
            int i;
            float f;
        } floatConverter;

        typedef union {
            long long int i;
            double f;
        } doubleConverter;
    }

    template<class T>
        void Print_Bdiff(const T C[9], const T C_reference[9], std::ostream& output);

    template<>
        void Print_Bdiff(const float C[9], const float C_reference[9], std::ostream& output)
    {
        floatConverter cvt;
        floatConverter cvt_ref;
        for (int i=0; i<9; i++) {
            cvt.f = C[i];
            cvt_ref.f = C_reference[i];
                output << std::bitset<32>(cvt.i^cvt_ref.i)<<std::endl;
        }
    }

    template<>
        void Print_Bdiff(const double C[9], const double C_reference[9], std::ostream& output)
    {
        doubleConverter cvt;
        doubleConverter cvt_ref;
        for (int i=0; i<9; i++) {
            cvt.f = C[i];
            cvt_ref.f = C_reference[i];
            output << std::bitset<64>(cvt.i^cvt_ref.i)<<std::endl;
        }
    }

template<class T_MATRIX>
    void Print_Formatted(const T_MATRIX& A,std::ostream& output)
{
    for(int i=0;i<A.rows();i++){
        for(int j=0;j<A.cols();j++){
            output<<std::setw(12)<<A(i,j);
            if(j<A.cols()-1) output<<" ";}
        output<<std::endl;}
}
}

template<class T>
void Matrix_Times_Matrix_Reference(const T A[9], const T B[9], T C[9])
{
    Map<const Matrix<T,3,3>> mA=Map<const Matrix<T,3,3>>(A);
    Map<const Matrix<T,3,3>> mB=Map<const Matrix<T,3,3>>(B);
    Map<Matrix<T,3,3>> mC=Map<Matrix<T,3,3>>(C);

    mC=mA*mB;
}

template<class T>
bool Matrix_Times_Matrix_Compare(const T C[9], const T C_reference[9])
{
    Map<const Matrix<T,3,3>> mC=Map<const Matrix<T,3,3>>(C);
    Map<const Matrix<T,3,3>> mC_reference=Map<const Matrix<T,3,3>>(C_reference);

    std::cout<<"Computed matrix C :"<<std::endl;Print_Formatted(mC,std::cout);
    std::cout<<"Reference matrix C :"<<std::endl;Print_Formatted(mC_reference,std::cout);
    std::cout<<"Difference = "<<(mC-mC_reference).norm()<<std::endl;
    Print_Bdiff(C, C_reference, std::cout);
    if( (mC-mC_reference).norm() < 0.00001 )
        return true;
    else
        return false;
}

template void Matrix_Times_Matrix_Reference(const float A[9], const float B[9], float C[9]);
template bool Matrix_Times_Matrix_Compare(const float C[9], const float C_reference[9]);

template void Matrix_Times_Matrix_Reference(const double A[9], const double B[9], double C[9]);
template bool Matrix_Times_Matrix_Compare(const double C[9], const double C_reference[9]);


/**
 * \file dcs/math/stats/distribution/detail/rgamma.cpp
 *
 * \brief Random variates from the Gamma distribution.
 *
 *  REFERENCES
 *
 *    [1] Shape parameter a >= 1.  Algorithm GD in:
 *
 *	  Ahrens, J.H. and Dieter, U. (1982).
 *	  Generating gamma variates by a modified
 *	  rejection technique.
 *	  Comm. ACM, 25, 47-54.
 *
 *
 *    [2] Shape parameter 0 < a < 1. Algorithm GS in:
 *
 *	  Ahrens, J.H. and Dieter, U. (1974).
 *	  Computer methods for sampling from gamma, beta,
 *	  poisson and binomial distributions.
 *	  Computing, 12, 223-246.
 *
 *    Input: a = parameter (mean) of the standard gamma distribution.
 *    Output: a variate from the gamma(a)-distribution
 *
 * \author Ross Ihaka (The R Development Core Team)
 *
 * <hr/>
 *
 *  Mathlib : A C Library of Special Functions
 *  Copyright (C) 1998 Ross Ihaka
 *  Copyright (C) 2000--2008 The R Development Core Team
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, a copy is available at
 *  http://www.r-project.org/Licenses/
 */

//#include "nmath.h"
#include <dcs/math/stats/distribution//exponential.hpp>
#include <dcs/math/stats/distribution//normal.hpp>
#include <boost/math/special_functions/expm1.hpp>

namespace dcs { namespace math { namespace stats { namespace detail {

/**
 * \brief Random variates from the Gamma distribution.
 *
 *  \param a Parameter (mean) of the standard Gamma distribution.
 *  \return A variate from the Gamma(a)-distribution.
 *
 * REFERENCES
 * \see
 *
 *  - [1] Shape parameter a >= 1.  Algorithm GD in:
 *
 *	  Ahrens, J.H. and Dieter, U. (1982).
 *	  Generating gamma variates by a modified
 *	  rejection technique.
 *	  Comm. ACM, 25, 47-54.
 *
 *
 *  - [2] Shape parameter 0 < a < 1. Algorithm GS in:
 *
 *	  Ahrens, J.H. and Dieter, U. (1974).
 *	  Computer methods for sampling from gamma, beta,
 *	  poisson and binomial distributions.
 *	  Computing, 12, 223-246.
 *  .
 *
 * \author Ross Ihaka (The R Development Core Team)
 */
template <typename RealT, typename UniformRandomGenerator>
RealT rgamma(RealT a, RealT scale, UniformRandomGenerator& eng)
{
	typedef RealT real_type;

/* Constants : */
    const static real_type sqrt32 = 5.656854;
    const static real_type exp_m1 = 0.36787944117144232159;/* exp(-1) = 1/e */

    /* Coefficients q[k] - for q0 = sum(q[k]*a^(-k))
     * Coefficients a[k] - for q = q0+(t*t/2)*sum(a[k]*v^k)
     * Coefficients e[k] - for exp(q)-1 = sum(e[k]*q^k)
     */
    const static real_type q1 = 0.04166669;
    const static real_type q2 = 0.02083148;
    const static real_type q3 = 0.00801191;
    const static real_type q4 = 0.00144121;
    const static real_type q5 = -7.388e-5;
    const static real_type q6 = 2.4511e-4;
    const static real_type q7 = 2.424e-4;

    const static real_type a1 = 0.3333333;
    const static real_type a2 = -0.250003;
    const static real_type a3 = 0.2000062;
    const static real_type a4 = -0.1662921;
    const static real_type a5 = 0.1423657;
    const static real_type a6 = -0.1367177;
    const static real_type a7 = 0.1233795;

    /* State variables [FIXME for threading!] :*/
    static real_type aa = 0.;
    static real_type aaa = 0.;
    static real_type s, s2, d;    /* no. 1 (step 1) */
    static real_type q0, b, si, c;/* no. 2 (step 4) */

    real_type e, p, q, r, t, u, v, w, x, ret_val;

    if (
		a < real_type(0) || scale <= real_type(0)
	) {
		if(scale == real_type(0))
		{
			return real_type(0);
		}
		return 0;//::std::numeric_limits<real_type>::quiet_NaN();
    }

    if (a < real_type(1))
	{
		/* GS algorithm for parameters a < 1 */
		if(a == real_type(0))
		{
			return real_type(0);
		}
		e = real_type(1) + exp_m1 * a;
		for (;;)
		{
			p = e * eng();
			if (p >= real_type(1))
			{
				x = -::std::log((e - p) / a);
				real_type exp_rand = ::boost::exponential_distribution<real_type>()(eng);
				if (exp_rand >= (real_type(1) - a) * ::std::log(x))
				{
					break;
				}
			}
			else
			{
				x = ::std::exp(::std::log(p) / a);
				real_type exp_rand = ::boost::exponential_distribution<real_type>()(eng);
				if (exp_rand >= x)
				{
					break;
				}
			}
		}
		return scale * x;
    }

    /* --- a >= 1 : GD algorithm --- */

    /* Step 1: Recalculations of s2, s, d if a has changed */
    if (a != aa)
	{
		aa = a;
		s2 = a - real_type(0.5);
		s = ::std::sqrt(s2);
		d = sqrt32 - s * real_type(12);
    }
    /* Step 2: t = standard normal deviate,
               x = (s,1/2) -normal deviate. */

    /* immediate acceptance (i) */
	real_type norm_rand = ::boost::normal_distribution<real_type>()(eng);
    t = norm_rand;
    x = s + 0.5 * t;
    ret_val = x * x;
    if (t >= 0.0)
	return scale * ret_val;

    /* Step 3: u = 0,1 - uniform sample. squeeze acceptance (s) */
    u = eng();
    if (d * u <= t * t * t)
	return scale * ret_val;

    /* Step 4: recalculations of q0, b, si, c if necessary */

    if (a != aaa) {
	aaa = a;
	r = 1.0 / a;
	q0 = ((((((q7 * r + q6) * r + q5) * r + q4) * r + q3) * r
	       + q2) * r + q1) * r;

	/* Approximation depending on size of parameter a */
	/* The constants in the expressions for b, si and c */
	/* were established by numerical experiments */

	if (a <= 3.686) {
	    b = 0.463 + s + 0.178 * s2;
	    si = 1.235;
	    c = 0.195 / s - 0.079 + 0.16 * s;
	} else if (a <= 13.022) {
	    b = 1.654 + 0.0076 * s2;
	    si = 1.68 / s + 0.275;
	    c = 0.062 / s + 0.024;
	} else {
	    b = 1.77;
	    si = 0.75;
	    c = 0.1515 / s;
	}
    }
    /* Step 5: no quotient test if x not positive */

    if (x > 0.0) {
	/* Step 6: calculation of v and quotient q */
	v = t / (s + s);
	if (::std::fabs(v) <= 0.25)
	    q = q0 + 0.5 * t * t * ((((((a7 * v + a6) * v + a5) * v + a4) * v
				      + a3) * v + a2) * v + a1) * v;
	else
	    q = q0 - s * t + 0.25 * t * t + (s2 + s2) * ::std::log(1.0 + v);

	/* Step 7: quotient acceptance (q) */
	if (log(1.0 - u) <= q)
	    return scale * ret_val;
    }

    for (;;)
	{
		/* Step 8: e = standard exponential deviate
		 *	u =  0,1 -uniform deviate
		 *	t = (b,si)-double exponential (laplace) sample */
		e = ::boost::exponential_distribution<real_type>()(eng);
		u = eng();
		u = u + u - 1.0;
		if (u < 0.0)
			t = b - si * e;
		else
			t = b + si * e;
		/* Step	 9:  rejection if t < tau(1) = -0.71874483771719 */
		if (t >= -0.71874483771719) {
			/* Step 10:	 calculation of v and quotient q */
			v = t / (s + s);
			if (::std::fabs(v) <= 0.25)
			q = q0 + 0.5 * t * t *
				((((((a7 * v + a6) * v + a5) * v + a4) * v + a3) * v
				  + a2) * v + a1) * v;
			else
			q = q0 - s * t + 0.25 * t * t + (s2 + s2) * ::std::log(1.0 + v);
			/* Step 11:	 hat acceptance (h) */
			/* (if q not positive go to step 8) */
			if (q > 0.0) {
			w = ::boost::math::expm1(q);
			/*  ^^^^^ original code had approximation with rel.err < 2e-7 */
			/* if t is rejected sample again at step 8 */
			if (c * ::std::fabs(u) <= w * ::std::exp(e - 0.5 * t * t))
				break;
			}
		}
    } /* repeat .. until  `t' is accepted */
    x = s + 0.5 * t;
    return scale * x * x;
}

}}}} // Namespace dcs::math::stats::detail


#ifndef _BOOSTSPATIALINDEX_SHAPE
#define _BOOSTSPATIALINDEX_SHAPE
#include <memory>
#include <string>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>

enum ShapeType {bbox, circle, annulus, poly};

typedef boost::geometry::model::point<double, 2, boost::geometry::cs::geographic<boost::geometry::degree>> point;
typedef boost::geometry::model::box<point> box;
typedef boost::geometry::model::polygon<point, false, false> polygon;

class Shape {
  protected:
    std::string id;
    ShapeType type;
    box envelope; // the minimum bounding rectangle embedding the provided shape
    box envelopeFromCircle(point const& p, double radius);
    double getDistance(point const& origin, point const& p);

    /*
     * shape-specific storage objects
     * (boxes do not need those as the envelope already describe these)
     */
    // - circle and annulus
    point center;
    double outerRadius;
    //std::pair<double, double>

    // annulus
    double innerRadius;

    // polygon
    polygon polygonShape;

  public:
    // Registers a box shape
    Shape(std::string id_, box const& b);

    // Registers a circle shape. Radius must be in meters
    Shape(std::string id_, point const& p, double radius);

    // Registers an annulus shape. Radiuses must be in meters
    Shape(std::string id_, point const& p, double outerRadius_, double innerRadius_);

    // Registers a polygon shape
    Shape(std::string id_, polygon const& p);

    // getters
    const char *getId();
    box getEnvelope();

    // others
    bool covered(point const& p);
};

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_ULPDIST_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_ULPDIST_HPP_INCLUDED
#include <nt2/sdk/constant/eps_related.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/ldexp.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/is_nan.hpp>

///////////////////////////////////////////////////////////////////////////////
// It is often difficult to  answer to the following question:
//  - are these two floating computations results similar enough ?
//
// The ulpdist is a way to answer tuned for relative errors estimations
// and peculiarity of limited bits accuracy of floating point representation
// The method is the following:
//    Properly normalize the two numbers by the same factor in a way that
//    the largest of the two numbers exponents will be brought to zero
//
//    Return this nt2::absolute difference of these normalized numbers
//    divided by the rounding error Eps
//
//    The roundind error is the ulp (unit in the last place) value, i.e. the
//    floating number, the exponent of which is 0 and the mantissa is all zeros
//    but a 1 in the last digit (it is not hard coded that way however).
//    Yhis means 2^-23 for floats and 2^-52 for double
//
//    For instance if two floating numbers (of same type) have an ulpdist of
//    zero that means that their floating representation are identical.
//
//    Generally equality up to 0.5ulp is the best that one can wish beyond
//    strict equality.
//
//    Typically if a double is compared to the double representation of
//    its floating conversion (they are exceptions as for fully representable
//    reals) the ulpdist will be around 2^26.5 (~10^8)
//
//    The ulpdist is also roughly equivalent to the number of representable
//    floating points values between two given floating points values.
//
//     ulpdist( 1.0, 1+nt2::Eps<double>())   == 0.5
//     ulpdist( 1.0, 1+nt2::Eps<double>()/2) == 0.0
//     ulpdist( 1.0, 1-nt2::Eps<double>()/2) == 0.25
//     ulpdist( 1.0, 1-nt2::Eps<double>())   == 0.5
//     ulpdist(double(nt2::Pi<float>()), nt2::Pi<double>()) == 9.84293e+07
///////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ulpdist_, tag::cpu_,
                          (A0)(X),
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                         );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::ulpdist_(tag::simd_<tag::arithmetic_, X> ,
                            tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0,A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      return (max(a0, a1)-min(a0,a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ulpdist_, tag::cpu_,
                          (A0)(X),
                          ((simd_<real_<A0>,X>))
                          ((simd_<real_<A0>,X>))
                         );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::ulpdist_(tag::simd_<tag::real_, X> ,
                            tag::simd_<tag::real_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0,A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename meta::as_integer<A0>::type itype;
      itype e1, e2;
      A0 m1, m2;
      boost::fusion::tie(m1, e1) = nt2::frexp(a0);
      boost::fusion::tie(m2, e2) = nt2::frexp(a1);
      itype expo = -nt2::max(e1, e2);
      A0 e = sel(is_equal(e1, e2), nt2::abs(m1-m2), nt2::abs(nt2::ldexp(a0, expo)-nt2::ldexp(a1, expo)));
      return sel((is_nan(a0)&is_nan(a1))|is_equal(a0, a1),  Zero<A0>(), e/Eps<A0>());
    }
  };
} }

#endif
// modified by jt the 04/01/2011


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file TransformTests.cpp
/// \brief Unit tests for the implementation of the transformation matrix class.
/// \details Unit tests for the various Lie Group functions will test both
/// special cases,
///          and randomly generated cases.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#include <gtest/gtest.h>

#include <math.h>
#include <iomanip>
#include <ios>
#include <iostream>

#include <Eigen/Dense>
#include <lgmath/CommonMath.hpp>

#include <lgmath/se3/Operations.hpp>
#include <lgmath/se3/Transformation.hpp>
#include <lgmath/so3/Operations.hpp>

/////////////////////////////////////////////////////////////////////////////////////////////
///
/// UNIT TESTS OF TRANSFORMATION MATRIX
///
/////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of transformation constructors
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TransformationConstructors) {
  // Generate random transform from most basic constructor
  Eigen::Matrix<double, 3, 3> C_ba =
      lgmath::so3::vec2rot(Eigen::Matrix<double, 3, 1>::Random());
  Eigen::Matrix<double, 3, 1> r_ba_ina = Eigen::Matrix<double, 3, 1>::Random();
  lgmath::se3::Transformation rand(C_ba, r_ba_ina);

  // Transformation();
  {
    lgmath::se3::Transformation tmatrix;
    Eigen::Matrix4d test = Eigen::Matrix4d::Identity();
    std::cout << "tmat: " << tmatrix.matrix() << std::endl;
    std::cout << "test: " << test << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(tmatrix.matrix(), test, 1e-6));
  }

  // Transformation(const Transformation& T);
  {
    lgmath::se3::Transformation test(rand);
    std::cout << "tmat: " << rand.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(rand.matrix(), test.matrix(), 1e-6));
  }

  // Transformation(const Eigen::Matrix4d& T);
  {
    lgmath::se3::Transformation test(rand.matrix());
    std::cout << "tmat: " << rand.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(rand.matrix(), test.matrix(), 1e-6));

    // Test forced reprojection (ones to identity)
    Eigen::Matrix4d proj_test = Eigen::Matrix4d::Identity();
    proj_test.topRightCorner<3, 1>() = -r_ba_ina;
    Eigen::Matrix3d notRotation = Eigen::Matrix3d::Ones();
    Eigen::Matrix4d notTransform = Eigen::Matrix4d::Identity();
    notTransform.topLeftCorner<3, 3>() = notRotation;
    notTransform.topRightCorner<3, 1>() = -r_ba_ina;
    lgmath::se3::Transformation test_bad(notTransform);  // force reproj
    std::cout << "cmat: " << proj_test.matrix() << std::endl;
    std::cout << "test: " << test_bad.matrix() << std::endl;
    EXPECT_TRUE(
        lgmath::common::nearEqual(proj_test.matrix(), test_bad.matrix(), 1e-6));
  }

  // Transformation& operator=(Transformation T);
  {
    lgmath::se3::Transformation test = rand;
    std::cout << "tmat: " << rand.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(rand.matrix(), test.matrix(), 1e-6));
  }

  // Transformation(const Eigen::Matrix<double,6,1>& vec, unsigned int numTerms
  // = 0);
  {
    Eigen::Matrix<double, 6, 1> vec = Eigen::Matrix<double, 6, 1>::Random();
    Eigen::Matrix4d tmat = lgmath::se3::vec2tran(vec);
    lgmath::se3::Transformation testAnalytical(vec);
    lgmath::se3::Transformation testNumerical(vec, 15);
    std::cout << "tmat: " << tmat << std::endl;
    std::cout << "testAnalytical: " << testAnalytical.matrix() << std::endl;
    std::cout << "testNumerical: " << testNumerical.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(tmat, testAnalytical.matrix(), 1e-6));
    EXPECT_TRUE(lgmath::common::nearEqual(tmat, testNumerical.matrix(), 1e-6));
  }

  // Transformation(const Eigen::VectorXd& vec);
  {
    Eigen::VectorXd vec = Eigen::Matrix<double, 6, 1>::Random();
    Eigen::Matrix4d tmat = lgmath::se3::vec2tran(vec);
    lgmath::se3::Transformation test(vec);
    std::cout << "tmat: " << tmat << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(tmat, test.matrix(), 1e-6));
  }

  // Transformation(const Eigen::VectorXd& vec);
  {
    Eigen::VectorXd vec = Eigen::Matrix<double, 6, 1>::Random();
    lgmath::se3::Transformation test(vec);

    // Wrong size vector
    Eigen::VectorXd badvec = Eigen::Matrix<double, 3, 1>::Random();
    lgmath::se3::Transformation testFailure;
    try {
      testFailure = lgmath::se3::Transformation(badvec);
    } catch (const std::invalid_argument& e) {
      testFailure = test;
    }
    std::cout << "tmat: " << testFailure.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(
        lgmath::common::nearEqual(testFailure.matrix(), test.matrix(), 1e-6));
  }

  // Transformation(const Eigen::Matrix3d& C_ba,
  //               const Eigen::Vector3d& r_ba_ina);
  {
    lgmath::se3::Transformation tmat(C_ba, r_ba_ina);
    Eigen::Matrix4d test = Eigen::Matrix4d::Identity();
    test.topLeftCorner<3, 3>() = C_ba;
    test.topRightCorner<3, 1>() = -C_ba * r_ba_ina;
    std::cout << "tmat: " << tmat.matrix() << std::endl;
    std::cout << "test: " << test << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(tmat.matrix(), test, 1e-6));

    // Test forced reprojection (ones to identity)
    Eigen::Matrix4d proj_test = Eigen::Matrix4d::Identity();
    proj_test.topRightCorner<3, 1>() = -Eigen::Matrix3d::Identity() * r_ba_ina;
    Eigen::Matrix3d notRotation = Eigen::Matrix3d::Ones();
    lgmath::se3::Transformation test_bad(notRotation,
                                         r_ba_ina);  // forces reprojection
    std::cout << "cmat: " << proj_test.matrix() << std::endl;
    std::cout << "test: " << test_bad.matrix() << std::endl;
    EXPECT_TRUE(
        lgmath::common::nearEqual(proj_test.matrix(), test_bad.matrix(), 1e-6));
  }

  // Transformation(Transformation&&);
  {
    auto rand2 = rand;
    lgmath::se3::Transformation test(std::move(rand));
    rand = rand2;

    std::cout << "tmat: " << test.matrix() << std::endl;
    std::cout << "test: " << rand.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(test.matrix(), rand.matrix(), 1e-6));
  }

  // Transformation = Transformation&&;
  {
    lgmath::se3::Transformation test;
    auto rand2 = rand;
    test = std::move(rand);
    rand = rand2;

    std::cout << "tmat: " << test.matrix() << std::endl;
    std::cout << "test: " << rand.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(test.matrix(), rand.matrix(), 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Test some get methods
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TransformationGetMethods) {
  // Generate random transform from most basic constructor
  Eigen::Matrix<double, 3, 3> C_ba =
      lgmath::so3::vec2rot(Eigen::Matrix<double, 3, 1>::Random());
  Eigen::Matrix<double, 3, 1> r_ba_ina = Eigen::Matrix<double, 3, 1>::Random();
  lgmath::se3::Transformation T_ba(C_ba, r_ba_ina);

  // Construct simple eigen matrix from random rotation and translation
  Eigen::Matrix4d test = Eigen::Matrix4d::Identity();
  Eigen::Matrix<double, 3, 1> r_ab_inb = -C_ba * r_ba_ina;
  test.topLeftCorner<3, 3>() = C_ba;
  test.topRightCorner<3, 1>() = r_ab_inb;

  // Test matrix()
  std::cout << "T_ba: " << T_ba.matrix() << std::endl;
  std::cout << "test: " << test << std::endl;
  EXPECT_TRUE(lgmath::common::nearEqual(T_ba.matrix(), test, 1e-6));

  // Test C_ba()
  std::cout << "T_ba: " << T_ba.C_ba() << std::endl;
  std::cout << "C_ba: " << C_ba << std::endl;
  EXPECT_TRUE(lgmath::common::nearEqual(T_ba.C_ba(), C_ba, 1e-6));

  // Test r_ba_ina()
  std::cout << "T_ba: " << T_ba.r_ba_ina() << std::endl;
  std::cout << "r_ba_ina: " << r_ba_ina << std::endl;
  EXPECT_TRUE(lgmath::common::nearEqual(T_ba.r_ba_ina(), r_ba_ina, 1e-6));

  // Test r_ab_inb()
  std::cout << "T_ba: " << T_ba.r_ab_inb() << std::endl;
  std::cout << "r_ab_inb: " << r_ab_inb << std::endl;
  EXPECT_TRUE(lgmath::common::nearEqual(T_ba.r_ab_inb(), r_ab_inb, 1e-6));
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Test exponential map construction and logarithmic vec() method
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TransformationToFromSE3Algebra) {
  // Add vectors to be tested
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  Eigen::Matrix<double, 6, 1> temp;
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Calc transformation matrices
  std::vector<Eigen::Matrix4d> transMatrices;
  for (unsigned i = 0; i < numTests; i++) {
    transMatrices.push_back(lgmath::se3::vec2tran(trueVecs.at(i)));
  }

  // Calc transformations
  std::vector<lgmath::se3::Transformation> transformations;
  for (unsigned i = 0; i < numTests; i++) {
    transformations.push_back(lgmath::se3::Transformation(trueVecs.at(i)));
  }

  // Compare matrices
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "matr: " << transMatrices.at(i) << std::endl;
      std::cout << "tran: " << transformations.at(i).matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(
          transMatrices.at(i), transformations.at(i).matrix(), 1e-6));
    }
  }

  // Test logarithmic map
  {
    for (unsigned i = 0; i < numTests; i++) {
      Eigen::Matrix<double, 6, 1> testVec = transformations.at(i).vec();
      std::cout << "true: " << trueVecs.at(i) << std::endl;
      std::cout << "func: " << testVec << std::endl;
      EXPECT_TRUE(
          lgmath::common::nearEqualLieAlg(trueVecs.at(i), testVec, 1e-6));
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Test inverse, adjoint and operatations
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, TransformationInverse) {
  // Add vectors to be tested
  std::vector<Eigen::Matrix<double, 6, 1> > trueVecs;
  Eigen::Matrix<double, 6, 1> temp;
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double, 6, 1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Add vectors to be tested - random
  std::vector<Eigen::Matrix<double, 4, 1> > landmarks;
  for (unsigned i = 0; i < numTests; i++) {
    landmarks.push_back(Eigen::Matrix<double, 4, 1>::Random());
  }

  // Calc transformation matrices
  std::vector<Eigen::Matrix4d> transMatrices;
  for (unsigned i = 0; i < numTests; i++) {
    transMatrices.push_back(lgmath::se3::vec2tran(trueVecs.at(i)));
  }

  // Calc transformations
  std::vector<lgmath::se3::Transformation> transformations;
  for (unsigned i = 0; i < numTests; i++) {
    transformations.push_back(lgmath::se3::Transformation(trueVecs.at(i)));
  }

  // Compare inverse to basic matrix inverse
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "matr: " << transMatrices.at(i).inverse() << std::endl;
      std::cout << "tran: " << transformations.at(i).inverse().matrix()
                << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(
          transMatrices.at(i).inverse(),
          transformations.at(i).inverse().matrix(), 1e-6));
    }
  }

  // Test that product of inverse and self make identity
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "T*Tinv: "
                << transformations.at(i).matrix() *
                       transformations.at(i).inverse().matrix();
      EXPECT_TRUE(lgmath::common::nearEqual(
          transformations.at(i).matrix() *
              transformations.at(i).inverse().matrix(),
          Eigen::Matrix4d::Identity(), 1e-6));
    }
  }

  // Test adjoint
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "matr: " << lgmath::se3::tranAd(transMatrices.at(i))
                << std::endl;
      std::cout << "tran: " << transformations.at(i).adjoint() << std::endl;
      EXPECT_TRUE(
          lgmath::common::nearEqual(lgmath::se3::tranAd(transMatrices.at(i)),
                                    transformations.at(i).adjoint(), 1e-6));
    }
  }

  // Test self-product
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::se3::Transformation test = transformations.at(i);
      test *= transformations.at(i + 1);
      Eigen::Matrix4d matrix = transMatrices.at(i) * transMatrices.at(i + 1);
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test product
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::se3::Transformation test =
          transformations.at(i) * transformations.at(i + 1);
      Eigen::Matrix4d matrix = transMatrices.at(i) * transMatrices.at(i + 1);
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test self product with inverse
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::se3::Transformation test = transformations.at(i);
      test /= transformations.at(i + 1);
      Eigen::Matrix4d matrix =
          transMatrices.at(i) * transMatrices.at(i + 1).inverse();
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test product with inverse
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::se3::Transformation test =
          transformations.at(i) / transformations.at(i + 1);
      Eigen::Matrix4d matrix =
          transMatrices.at(i) * transMatrices.at(i + 1).inverse();
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test product with landmark
  {
    for (unsigned i = 0; i < numTests; i++) {
      Eigen::Matrix<double, 4, 1> mat = transMatrices.at(i) * landmarks.at(i);
      Eigen::Matrix<double, 4, 1> test =
          transformations.at(i) * landmarks.at(i);

      std::cout << "matr: " << mat << std::endl;
      std::cout << "test: " << test << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(mat, test, 1e-6));
    }
  }
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include "spectral_shape.hpp"

#include <algorithm>
#include <numeric>

#include <deal.II/lac/petsc_full_matrix.h>

namespace bart::acceleration::two_grid::spectral_shape {

namespace  {
using DealiiMatrix = dealii::FullMatrix<double>;
auto matrix_size_error_text(const DealiiMatrix& sigma_t, const DealiiMatrix& sigma_s) {
  return std::string{"Error in SpectralShape::CalculateSpectralShape, matrix size mismatch: Sigma_T matrix has "
                     "dimensions (" + std::to_string(sigma_t.m()) + ", " + std::to_string(sigma_t.n()) +
      "), and Sigma_S matrix has dimensions (" + std::to_string(sigma_s.m()) + ", "
                         + std::to_string(sigma_s.n())};
}
} // namespace

SpectralShape::SpectralShape(std::unique_ptr<EigenvalueSolver> eigenvalue_solver_ptr)
    : eigenvalue_solver_ptr_(std::move(eigenvalue_solver_ptr)) {
  AssertPointerNotNull(eigenvalue_solver_ptr_.get(), "eigenvalue solver", "SpectralShape constructor");
}

auto SpectralShape::CalculateSpectralShape(const DealiiMatrix& sigma_t,
                                           const DealiiMatrix& sigma_s) -> std::vector<double> {
  AssertThrow(sigma_t.m() == sigma_s.m() && sigma_t.n() == sigma_s.n() && sigma_t.m() == sigma_t.n() &&
      sigma_s.m() == sigma_s.n(), dealii::ExcMessage(matrix_size_error_text(sigma_t, sigma_s)))
  const int n_groups = sigma_t.m();
  DealiiMatrix downscattering(n_groups, n_groups), upscattering(n_groups, n_groups);

  for (int i = 0; i < n_groups; ++i) {
    for (int j = 0; j < i + 1; ++j)
      downscattering(i, j) = sigma_s(i, j);
    for (int j = i + 1; j < n_groups; ++j)
      upscattering(i, j) = sigma_s(i, j);
  }
  DealiiMatrix a(n_groups, n_groups);
  DealiiMatrix lhs(sigma_t);
  lhs.add(-1, downscattering);
  lhs.gauss_jordan();
  lhs.mmult(a, upscattering);

  dealii::PETScWrappers::FullMatrix petsc_matrix(n_groups, n_groups);
  for (int i = 0; i < n_groups; ++i) {
    for (int j = 0; j < n_groups; ++j) {
      petsc_matrix.set(i, j, a(i, j));
    }
  }
  petsc_matrix.compress(dealii::VectorOperation::insert);
  auto [eigenvalue, eigenvector] = this->eigenvalue_solver_ptr_->SpectralRadius(petsc_matrix);

  // Normalize in the L1 norm
  const double sum = std::accumulate(eigenvector.begin(), eigenvector.end(), 0.0,
                                     [](double running_sum, double val){ return running_sum + std::abs(val); });
  std::transform(eigenvector.begin(), eigenvector.end(), eigenvector.begin(),
                 [sum](const double val) { return std::abs(val) / sum; });

  return eigenvector;
}

} // namespace bart::acceleration::two_grid::spectral_shape


///////////////////////////////////////////////////////////////////////////////
// acvf.hpp                                                                  //
//                                                                           //
//  Copyright 2008 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 BOOST_ACCUMULATORS_STATISTICS_ACVF_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_ACVF_HPP_ER_2008_04
#include <cmath>
#include <vector>
#include <algorithm>
#include <boost/mpl/int.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/delay.hpp> // in accumulators_filter

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // acvf_impl (Autocovariance function)
    //
    template<typename T,typename I>
    class acvf_impl
      : public accumulator_base
    {
	   typedef std::vector<T>                       acvs_type;
       typedef delay_impl<T,I>                      delay_type;
	   typedef typename delay_type::result_type     input_type;
    public:
        typedef I discriminator_type;

        typedef boost::iterator_range<
            typename acvs_type::const_iterator
        > result_type;

        template<typename Args>
        acvf_impl(Args const &args)
        :acvs(
            args[tag::delay<I>::cache_size|delay(args).size()],
            static_cast<T>(0)
        )
        {
        }

        acvf_impl(const acvf_impl& that)
        :acvs(that.acvs){}

        acvf_impl& operator=(const acvf_impl& that){
            if(&that!=this){
                acvs = that.acvs;
            }
            return *this;
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
          input_type in = delay(args); //0,1,2,...,K
          typedef typename input_type::const_iterator  in_iter_type;
          typedef typename acvs_type::iterator         out_iter_type;
          std::size_t in_sz = in.size();

          BOOST_ASSERT((in_sz<acvs.size())||(in_sz==acvs.size()));
          in_iter_type i = begin(in);
          in_iter_type e = end(in);

          T x0 = (*i);
          std::size_t n = count(args);
          std::size_t k = 0;
		  while((k<in_sz) && (n>1+k)){
		      BOOST_ASSERT(i<e);
              T xk = (*i);
              T div = (T) ((n-1)-k);
              T sum_prod = acvs[k] * div;
              T mean_val = mean(args);
              sum_prod += (xk - mean_val) * (x0 - mean_val);
              div = (T)(n-k);
              acvs[k] = sum_prod / div;
              ++i;
              ++k;
          }
		}

        result_type result(dont_care) const
        {
          return boost::make_iterator_range(acvs.begin(),acvs.end());
        }

    private:
	   acvs_type   acvs;

    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::acvf
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct acvf
      : depends_on<count, mean, delay<I> >
    {
        /// INTERNAL ONLY
      typedef accumulators::impl::acvf_impl<mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::acvf
//

namespace extract
{
//  extractor<tag::acvf<> > const acvf = {};
//  //a non-default discriminator requires
//  //struct my_other_delay {};
//  //extractor<tag::delay<my_other_delay> > other_delay={};

  template<typename I,typename AccumulatorSet>
  typename
    mpl::apply<AccumulatorSet,tag::acvf<I> >::type::result_type
  acvf(AccumulatorSet const& acc){
    typedef tag::acvf<I> the_tag;
    return extract_result<the_tag>(acc);
  }//typical call://acvf<default_delay_discriminator>(acc)

//  TODO
//  //overload
//  template<typename AccumulatorSet>
//  typename mpl::apply<AccumulatorSet,tag::acvf<> >::type::result_type
//  acvf(AccumulatorSet const& acc){
//    return acvf<default_delay_discriminator,AccumulatorSet>(acc);
//  }
// /../boost_1_35_0/boost/mpl/aux_/preprocessed/gcc/apply_wrap.hpp|39|error: no
// class template named ‘apply’
// in ‘struct boost::accumulators::default_delay_discriminator’|

}

using extract::acvf;

}} // namespace boost::accumulators

#endif


//============================================================================
// Name         : dnatemplategeodesyfuncs.hpp
// Author       : Roger Fraser
// Contributors :
// Version      : 1.00
// Copyright    : Copyright 2017 Geoscience Australia
//
//                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.
//
// Description  : Basic Geodetic Functions
//============================================================================

#ifndef DNATEMPLATEGEODESYFUNCS_H_
#define DNATEMPLATEGEODESYFUNCS_H_

#if defined(_MSC_VER)
	#if defined(LIST_INCLUDES_ON_BUILD)
		#pragma message("  " __FILE__)
	#endif
#endif

#include <algorithm>
#include <functional>
#include <sstream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <iostream>

#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>

#include <include/parameters/dnaellipsoid.hpp>
#include <include/parameters/dnaprojection.hpp>
#include <include/config/dnatypes.hpp>

using namespace std;
using namespace boost;

using namespace dynadjust::datum_parameters;

// nu helper
template <class T>
T primeVertical(const CDnaEllipsoid* ellipsoid, const T& latitude)
{
	return primeVertical_(ellipsoid->GetSemiMajor(), ellipsoid->GetE1sqd(), latitude);
}

// rho helper
template <class T>
double primeMeridian(const CDnaEllipsoid* ellipsoid, const T& latitude)
{
	return primeMeridian_(ellipsoid->GetSemiMajor(), ellipsoid->GetE1sqd(), latitude);
}

// nu and rho helper
template <class T>
void primeVerticalandMeridian(const CDnaEllipsoid* ellipsoid, const T& latitude, T& nu, T& rho)
{
	primeVerticalandMeridian_(ellipsoid->GetSemiMajor(), ellipsoid->GetE1sqd(), latitude, nu, rho);
}

// average radius of curvature helper
template <class T>
T averageRadiusofCurvature(const CDnaEllipsoid* ellipsoid, const T& latitude)
{
	return averageRadiusofCurvature_(ellipsoid->GetSemiMajor(), ellipsoid->GetE1sqd(), latitude);
}

template <class T>
void GeoToCart(const T& Latitude, const T& Longitude, const T& Height, T* X, T* Y, T* Z,
			   const CDnaEllipsoid* ellipsoid)
{
	// copy variables in case the caller overwrites original values
	T latitude(Latitude), longitude(Longitude), height(Height);

	// calculate prime vertical (once)
	T Nu(primeVertical(ellipsoid, latitude));

	*X = (Nu + height) * cos(latitude) * cos(longitude);
	*Y = (Nu + height) * cos(latitude) * sin(longitude);
	*Z = ((Nu * (1. - ellipsoid->GetE1sqd())) + height) * sin(latitude);
}

template <class T>
void CartToGeo_SimpleIteration(const T& X, const T& Y, const T& Z,
			   T* latitude, T* longitude, T* height,
			   const CDnaEllipsoid* ellipsoid) // ellipsoid parameters
{
	// copy variables in case the caller overwrites original values
	T x(X), y(Y), z(Z);

	T f(ellipsoid->GetFlattening());
	T e2(ellipsoid->GetE1sqd());
	T p(pow(((x * x) + (y * y)), 0.5));

	// "Cartesian to geographic" conversion problem resides in the
	// equation to compute latitude (phi)...latitude is required on both
	// sides of the equation. A simple iterative method is used to determine
	// the latitude, whereby the starting latitude is computed from
	// tan(phi) = z / p

	// Compute starting lat value from second eccentricity squared
	T lat1(atan(z / p));
	*latitude = atan2((z + (e2 * ellipsoid->GetSemiMajor() * sin(lat1))), p);
	T Nu(primeVertical(ellipsoid, *latitude));

	for (UINT16 i(0); i<16; i++)
	{
		lat1 = atan2((z + (e2 * Nu * sin(*latitude))), p);
		//Nu = ellipsoid->PrimeVertical(*latitude);
		Nu = primeVertical(ellipsoid, *latitude);
		*latitude = atan2((z + (e2 * Nu * sin(lat1))), p);

		if (fabs(lat1 - *latitude) < PRECISION_1E16)
			break;
	}

	// Compute longitude
	*longitude = atan(y / x);

	// determine correct quadrant and apply negative long accordingly
	if (x < 0.0 && y > 0.0)
		*longitude += PI;
	else if (x < 0.0 && y < 0.0)
		*longitude = -(PI - *longitude);

	// Compute height
	*height = (p / cos(*latitude)) - Nu;
}

template <class T>
// K Lin and J Wang's method based on Newton's iteration
// 	double dXAxis(-3563081.362), dYAxis(-2057145.984), dZAxis(-4870449.482), dHeight(0.);
//	CDnaEllipsoid e;
//	CartToGeo<double>(dXAxis, dYAxis, dZAxis, &dXAxis, &dYAxis, &dHeight, &e);
//	stringstream ss;
//	ss << setw(MSR) << right << FormatDmsString(RadtoDms(dXAxis), 5, true, false) << ", ";
//	ss << setw(MSR) << right << FormatDmsString(RadtoDms(dYAxis), 5, true, false) << ", ";
//	ss << setw(MSR) << setprecision(4) << fixed << right << dHeight;
//	string comp(ss.str());
//	comp should equal  "-50 00 00.0000, -150 00 00.0000, 10000.000"
//
void CartToGeo(const T& X, const T& Y, const T& Z,
			   T* latitude, T* longitude, T* height,
			   const CDnaEllipsoid* ellipsoid) // ellipsoid parameters
{
	// copy variables in case the caller overwrites original values
	T x(X), y(Y), z(Z);

	T p2((x * x) + (y * y));
	T p(sqrt(p2));
	T a2(ellipsoid->GetSemiMajor() * ellipsoid->GetSemiMajor());
	T b2(ellipsoid->GetSemiMinor() * ellipsoid->GetSemiMinor());
	T Z2(z * z);
	T a2Z2(a2 * Z2);
	T b2p2(b2 * p2);
	T A(a2Z2 + b2p2);

	// Compute initial approximation of m (Lin and Wang 1995, eq. 9, p. 301)
	T m0((ellipsoid->GetSemiMajor() * ellipsoid->GetSemiMinor() * sqrt(A) * A - a2 * b2 * A) / (2. *
		((a2 * a2Z2) + (b2 * b2p2))));

	T twom, a2twom, b2twom, f, df, m;

	// Generally converges after one iteration and
	// so shouldn't need more than five iterations.
	for (UINT16 i(0); i<5; ++i)
	{
		m = m0;
		twom = m * 2.;
		a2twom = a2 + twom;
		b2twom = b2 + twom;

		f = (a2 * p2 / (a2twom * a2twom)) + (b2 * Z2 / (b2twom * b2twom)) - 1.;

		// if f is sufficiently close to zero, break.
		if (fabs(f) < PRECISION_1E12)
			break;

		df = -4. * ((a2 * p2 / (a2twom * a2twom * a2twom)) + (b2 * Z2 / (b2twom * b2twom * b2twom)));

		// recompute new value for m
		m0 = m - (f / df);
		m = m0;
	}

	twom = m * 2.;

	T p_E(a2 * p / (a2 + twom));
	T Z_E(b2 * z / (b2 + twom));

	// Compute latitude
	*latitude = atan(a2 * Z_E / (b2 * p_E));

	// Compute longitude
	*longitude = atan(y / x);

	// determine correct quadrant and apply negative long accordingly
	if (x < 0.0 && y > 0.0)
		*longitude += PI;
	else if (x < 0.0 && y < 0.0)
		*longitude = -(PI - *longitude);

	// The following line causes an issue for west longitudes, which by nature are negative!
	// Not sure why this was introduced.  Removing the conditional absolute has no adverse impact on
	// positions which are located in the eastern hemisphere,
	//if (*longitude < 0.)
	//	*longitude += TWO_PI;

	// Compute height
	*height = sqrt(((p - p_E) * (p - p_E)) + ((z - Z_E) * (z - Z_E)));
	if ((p + fabs(z)) < (p_E + fabs(Z_E)))
		*height *= -1.;
}

template <class T>
// K Lin and J Wang's method based on Newton's iteration
T CartToLat(const T& X, const T& Y, const T& Z, const CDnaEllipsoid* ellipsoid)
{
	// copy variables in case the caller overwrites original values
	T x(X), y(Y), z(Z);

	T p2((x * x) + (y * y));
	T p(sqrt(p2));
	T a2(ellipsoid->GetSemiMajor() * ellipsoid->GetSemiMajor());
	T b2(ellipsoid->GetSemiMinor() * ellipsoid->GetSemiMinor());
	T Z2(z * z);
	T a2Z2(a2 * Z2);
	T b2p2(b2 * p2);
	T A(a2Z2 + b2p2);

	// Compute initial approximation of m (Lin and Wang 1995, eq. 9, p. 301)
	T m0((ellipsoid->GetSemiMajor() * ellipsoid->GetSemiMinor() * sqrt(A) * A - a2 * b2 * A) / (2. *
		((a2 * a2Z2) + (b2 * b2p2))));

	T twom, a2twom, b2twom, f, df, m;

	// Generally converges after one iteration and
	// so shouldn't need more than five iterations.
	for (UINT16 i(0); i<5; ++i)
	{
		m = m0;
		twom = m * 2.;
		a2twom = a2 + twom;
		b2twom = b2 + twom;

		f = (a2 * p2 / (a2twom * a2twom)) + (b2 * Z2 / (b2twom * b2twom)) - 1.;

		// if f is sufficiently close to zero, break.
		if (fabs(f) < PRECISION_1E12)
			break;

		df = -4. * ((a2 * p2 / (a2twom * a2twom * a2twom)) + (b2 * Z2 / (b2twom * b2twom * b2twom)));

		// recompute new value for m
		m0 = m - (f / df);
		m = m0;
	}

	twom = m * 2.;

	T p_E(a2 * p / (a2 + twom));
	T Z_E(b2 * z / (b2 + twom));

	// Compute latitude
	return atan(a2 * Z_E / (b2 * p_E));
}

template <class T>
T PartialD_Latitude(const T& X, const T& Y, const T& Z,
			   const _CART_ELEM_& element,  const T& latitude, const CDnaEllipsoid* ellipsoid)
{
	if (element > z_element)
		return 0.;

	const T small_inc = PRECISION_1E4;

	// Compute the partial derivative.
	// 1. add small increment to the required element
	T cart[3] = { X, Y, Z };
	cart[element] += small_inc;

	// 2. f(x + small_inc)
	T fx_small_inc(CartToLat(
		cart[x_element],				// X1
		cart[y_element],				// Y1
		cart[z_element],				// Z1
		ellipsoid));

	//			  f(x + small_inc) - f(x)
	// 3. f'(x) = -----------------------
	//					 small_inc
	return (fx_small_inc - latitude) / small_inc;
}

template <class T>
T PartialD_Latitude_F(const T& X, const T& Y, const T& Z,
			   const _CART_ELEM_& element,  T* latitude, const CDnaEllipsoid* ellipsoid)
{
	if (element > z_element)
		return 0.;

	// compute the new latitude
	*latitude = CartToLat(X, Y, Z, ellipsoid);

	return PartialD_Latitude(X, Y, Z, element, *latitude, ellipsoid);
}

template <class T>
T PartialD_HorizAngle(const T X1, const T Y1, const T Z1,
				 const T X2, const T Y2, const T Z2,
				 const T X3, const T Y3, const T Z3,
				 const T currentLatitude, const T currentLongitude,
				 const _STATION_ELEM_& station, const _CART_ELEM_& element, const T angle)
{
	if (element > z_element)
		return 0.;

	const T small_inc = PRECISION_1E4;

	// Compute the partial derivative.
	// 1. add small increment to the required element
	T cart[3][3] = { X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3 };
	cart[station][element] += small_inc;

	// Temporary variables
	T dir12, dir13, loc12e, loc12n, loc13e, loc13n;

	// 2. f(x + small_inc)
	T fx_small_inc(HorizontalAngle(
		cart[station_1][x_element],				// X1
		cart[station_1][y_element],				// Y1
		cart[station_1][z_element],				// Z1
		cart[station_2][x_element],				// X2
		cart[station_2][y_element],				// Y2
		cart[station_2][z_element],				// Z2
		cart[station_3][x_element],				// X3
		cart[station_3][y_element],				// Y3ZONE
		cart[station_3][z_element],				// Z3
		currentLatitude, currentLongitude,
		&dir12, &dir13, &loc12e, &loc12n, &loc13e, &loc13n));

	//			  f(x + small_inc) - f(x)
	// 3. f'(x) = -----------------------
	//					 small_inc
	return (fx_small_inc - angle) / small_inc;
}

template <class T>
// latitude and longitude in radians
void GeoToGrid(const T& Latitude, const T& Longitude, T* easting, T* northing, T* zone,
			   const CDnaEllipsoid* ellipsoid, const CDnaProjection* projection, bool COMPUTE_ZONE)
{
	T latitude(Latitude);
	T longitude(Longitude);

	// Compute Zone if not previously specified
	if (COMPUTE_ZONE)
		*zone = floor((Degrees(longitude) - projection->GetLongWesternEdgeZone0()) / projection->GetZoneWidth());

	// Compute Geodetic Longitude of the Central Meridian of pGeoValue->dNum3 (the UTM Zone)
	T CMeridian((*zone * projection->GetZoneWidth()) + projection->GetLongCentralMeridianZone0());

	// Compute diff in Longitude between CMeridian and Longitude
	T omega(longitude - Radians(CMeridian));

	T e2(ellipsoid->GetE1sqd());
	T e2_2(pow(e2, 2.0));
	T e2_3(pow(e2, 3.0));

	T Nu, Rho, Nu_div_Rho;
	primeVerticalandMeridian(ellipsoid, latitude, Nu, Rho);
	Nu_div_Rho = Nu / Rho;

	T A0(1.0 - (e2 / 4.0) - (3.0 * e2_2 / 64.0) - (5.0 * e2_3 / 256.0));
	T A2(3.0 / 8.0 * (e2 + (e2_2 / 4.0) + (15.0 * e2_3 / 128.0)));
	T A4(15.0 / 256.0 * (e2_2 + (3.0 * e2_3 / 4.0)));
	T A6(35.0 * e2_3 / 3072.0);

	T m(ellipsoid->GetSemiMajor() * ((A0 * latitude) -
		(A2 * sin(2.0 * latitude)) +
		(A4 * sin(4.0 * latitude)) -
		(A6 * sin(6.0 * latitude))));

	T cos_lat(cos(latitude));
	T sin_lat(sin(latitude));
	T Num1(K0 * Nu * omega * cos_lat);

	T tan_lat_2(pow(tan(latitude), 2.0));
	T tan_lat_4(pow(tan(latitude), 4.0));

	// Compute Easting
	T Term1((pow(omega, 2.0) / 6.0) * (pow(cos_lat, 2.0)) * (Nu_div_Rho - tan_lat_2));
	T Term2((pow(omega, 4.0) / 120) * (pow(cos_lat, 4.0)) *
		(((4.0 * pow(Nu_div_Rho, 3.0)) * (1.0 - (6.0 * tan_lat_2))) +
		((pow(Nu_div_Rho, 2.0)) * (1.0 + (8.0 * tan_lat_2))) -
		(Nu_div_Rho * 2.0 * tan_lat_2) + tan_lat_4));
	T Term3((pow(omega, 6.0) / 5040) * (pow(cos_lat, 6.0)) *
		(61.0 - (479.0 * tan_lat_2) + (179.0 * tan_lat_4) - (tan_lat_4)));

	*easting = (Num1 * (1.0 + Term1 + Term2 + Term3)) + FALSE_E;

	// Compute Northing
	Term1 = (pow(omega, 2.0) / 2.0 * Nu * sin_lat * cos_lat);
	Term2 = (pow(omega, 4.0) / 24.0 * Nu * sin_lat * pow(cos_lat, 3.0));
	Term2 *= ((4.0 * pow(Nu_div_Rho, 2.0)) + Nu_div_Rho - tan_lat_2);
	Term3 = (pow(omega, 6.0) / 720.0 * Nu * sin_lat * pow(cos_lat, 5.0));
	Term3 *= ((8.0 * pow(Nu_div_Rho, 4.0) * (11.0 - (24.0 * tan_lat_2))) -
		(28.0 * pow(Nu_div_Rho, 3.0) * (1.0 - (6.0 * tan_lat_2))) +
		(pow(Nu_div_Rho, 2.0) * (1.0 - (32.0 * tan_lat_2))) -
		(Nu_div_Rho * 2.0 * tan_lat_2) + tan_lat_4);

	T Term4(pow(omega, 8.0) / 40320 * Nu * sin_lat * pow(cos_lat, 7.0));
	Term4 *= (1385.0 - (3111.0 * tan_lat_2) + (543.0 * tan_lat_4) - (pow(tan(latitude), 6.0)));

	*northing = (K0 * (m + Term1 + Term2 + Term3 + Term4)) + FALSE_N;
}

// returns lat/long values in radians
template <class T, typename U>
void GridToGeo(const T& easting, const T& northing, const U& zone, T* latitude, T* longitude,
			   const T& a, const T& inv_f, // ellipsoid parameters
			   const T& FALSE_E, const T& FALSE_N, const T& kO, const T& lcmZ1, const T& zW)	// projecton parameters
{
	T f = 1 / inv_f;
	T b = a * (1 - f);
	T e2 = (2 * f) - (f * f);
	//T e = sqrt(e2);
	//T Seconde2 = e2 / (1 - e2);
	//T Seconde = sqrt(Seconde2);
	T n = (a - b) / (a + b);
	T n2 = pow(n, 2.0);
	T n3 = pow(n, 3.0);
	T n4 = pow(n, 4.0);
	T G = a * (1 - n) * (1 - n2);
	G *= (1 + (9 * n2 / 4) + (225 * n4 / 64));
	G *= (PI / 180.);

	T ePrime = easting - FALSE_E;
	T nPrime = northing - FALSE_N;
	T m = nPrime / kO;
	T sigma = (m * PI) / (180 * G);

	T latPrime = sigma + (( (3 * n / 2) - (27 * n3 / 32) ) * sin(2 * sigma));
	latPrime += ( (21 * n2 / 16) - (55 * n4 / 32) ) * sin(4 * sigma);
	latPrime += (151 * n3 / 96) * sin(6 * sigma);
	latPrime += (1097 * n4 / 512) * sin(8 * sigma);

	T rho = a * (1 - e2) / pow( (1 - (e2 * pow(sin(latPrime), 2.0))), 1.5);
	T nu = a / pow( (1 - (e2 * pow(sin(latPrime), 2.0))), 0.5);
	T num1 = tan(latPrime) / (kO * rho);
	T x = ePrime / (kO * nu);

	T term1 = num1 * x * ePrime / 2;
	T term2 = num1 * ePrime * pow(x, 3.0) / 24;
	term2 *= ( (-4 * pow((nu / rho), 2.0)) + (9 * nu / rho * (1 - pow((tan(latPrime)), 2.0))) + (12 * pow((tan(latPrime)), 2.0))  );
	T term3 = num1 * ePrime * pow(x, 5.0) / 720;
	term3 *= (
		(8 * pow((nu / rho), 4.0) * (11 - (24 * pow((tan(latPrime)), 2.0)))) -
		(12 * pow((nu / rho), 3.0) * (21 - (71 * pow((tan(latPrime)), 2.0)))) +
		(15 * pow((nu / rho), 2.0) * (15 - (98 * pow((tan(latPrime)), 2.0)) + (15 * pow((tan(latPrime)), 4.0)))) +
		(180 * (nu / rho) * ((5 * pow((tan(latPrime)), 2.0))-(3 * pow((tan(latPrime)), 4.0))))+
		(360 * pow((tan(latPrime)), 4.0))
		);
	T term4 = num1 * ePrime * pow(x, 7.0) / 40320;
	term4 *= (1385 +
		(3633 * pow((tan(latPrime)), 2.0)) +
		(4095 * pow((tan(latPrime)), 4.0)) +
		(1575 * pow((tan(latPrime)), 6.0))
		);

	// Store radians value of Geodetic Latitude
	*latitude = latPrime - term1 + term2 - term3 + term4;

	// Compute Geodetic Longitude of the Central Meridian of pGridValue->dNum3 (the UTM Zone) in radians
	T centralMeridian = ((zone * zW) + lcmZ1 - zW) * PI / 180;

	num1 = 1 / (cos(latPrime));

	term1 = x * num1;
	term2 = ( pow(x, 3.0) / 6 * num1 * ((nu / rho) + (2. * tan(latPrime) * tan(latPrime))) );
	term3 = ( pow(x, 5.0) / 120 * num1 * (
		(-4 * pow((nu / rho), 3.0) * (1 - (6 * tan(latPrime) * tan(latPrime)))) +
		(pow((nu / rho), 2.0) * (9 - (68 * tan(latPrime) * tan(latPrime)))) +
		(72 * (nu / rho) * tan(latPrime) * tan(latPrime)) +
		(24 * tan(latPrime) * tan(latPrime) * tan(latPrime) * tan(latPrime))
		));
	term4 = ( pow(x, 7.0) / 5040 * num1 * (
		61 + (662 * tan(latPrime) * tan(latPrime)) +
		(1320 * tan(latPrime) * tan(latPrime) * tan(latPrime) * tan(latPrime)) +
		(720 * tan(latPrime) * tan(latPrime) * tan(latPrime) * tan(latPrime) * tan(latPrime) * tan(latPrime))
		));

	// Store radians value of Geodetic Longitude
	*longitude = centralMeridian + term1 - term2 + term3 - term4;

}

// Great Circle Distance
template <class T>
T GreatCircleDistance(const T& dLatitudeAT, const T& dLongitudeAT, const T& dLatitudeTO, const T& dLongitudeTO)
{
	T deltaLatitude(dLatitudeTO - dLatitudeAT);
	T deltaLongitude(dLongitudeTO - dLongitudeAT);
	T a(sin(deltaLatitude / 2) * sin(deltaLatitude / 2) + cos(dLatitudeAT) * cos(dLatitudeTO) * sin(deltaLongitude / 2) * sin(deltaLongitude / 2));
	T c(2 * atan2(sqrt(a), sqrt(1 - a)));
	return c * T(6372797.);
}

// Rigorous Geodesic via Robbins' formula
// Robbins, A. R. (1962). �Long lines on the spheroid.� Surv. Rev., XVI(125), 301�309.
template <class T>
T RobbinsReverse(const T& dLatitudeAT, const T& dLongitudeAT, const T& dLatitudeTO, const T& dLongitudeTO, T* pAzimuth, const CDnaEllipsoid* ellipsoid)
{
	T s, z, c, g, h, h2, chi;
	T sinsigma, sigma, sigma2, sigma3, sigma4, sigma5;

	T dPVertA(primeVertical(ellipsoid, dLatitudeAT));
	T dPVertB(primeVertical(ellipsoid, dLatitudeTO));
	T tanzeta2 = (1 - ellipsoid->GetE1sqd()) * tan(dLatitudeTO) + ellipsoid->GetE1sqd() * dPVertA * sin(dLatitudeAT) / (dPVertB * cos(dLatitudeTO));
	T tau1 = cos(dLatitudeAT) * tanzeta2 - sin(dLatitudeAT) * cos(dLongitudeTO - dLongitudeAT);
	T tanazimuthAB;
	if (fabs (dLongitudeTO - dLongitudeAT) < PRECISION_1E15)
		tanazimuthAB = 0.0;
	else
		tanazimuthAB = sin (dLongitudeTO - dLongitudeAT) / tau1;

	// Compute the azimuth, check for the correct sign, quadrant etc. -
	*pAzimuth = atan(tanazimuthAB);
	if ((*pAzimuth) < 0.0)
		*pAzimuth += PI;
	if (dLongitudeTO < dLongitudeAT)
		*pAzimuth += PI;
	if ((fabs (*pAzimuth) < PRECISION_1E15) && (dLatitudeTO < dLatitudeAT))
		*pAzimuth += PI;

	// Check here for the sign of the computed azimuth - eg to add pi or 2pi etc.
	s = sin (*pAzimuth);
	z = atan (tanzeta2);
	c = cos (z);

	// If sin (alpha12) is close to zero then chi is calculated one way
	// otherwise it is calculated differently.  An arbitrary
	// "boundary value" of 0.2 has been used.
	if (fabs (s) < 0.2)
		chi = tau1 / cos(*pAzimuth);
	else
		chi = sin(dLongitudeTO - dLongitudeAT) / s;
	sinsigma = chi * c;
	sigma = asin (sinsigma);
	sigma2 = sigma * sigma;
	sigma3 = sigma2 * sigma;
	sigma4 = sigma2 * sigma2;
	sigma5 = sigma3 * sigma2;
	g = ellipsoid->GetE2() * sin (dLatitudeAT);
	h = ellipsoid->GetE2() * cos (dLatitudeAT) * cos (*pAzimuth);
	h2 = h * h;
	return (dPVertA * sigma * (1 - sigma2 * h2 * (1 - h2) / 6 + sigma3 * g * h * (1 - 2 * h2) / 8 + sigma4 * (h2 * (4 - 7 * h2) - 3 * g * g * (1 - 7 * h2)) / 120 - sigma5 * g * h / 48));
}

template <class T>
void VincentyDirect(const T& dLatitudeAT, const T& dLongitudeAT, const T& dAzimuth, const T& dDistance,
					T *dLatitudeTO, T *dLongitudeTO, const CDnaEllipsoid* ellipsoid)
{
	// calculate fundamentals
	T f = ellipsoid->GetFlattening();
	T b = ellipsoid->GetSemiMinor();

	// parametric latitude of P'
	T tanUI = (1.0 - f) * tan(dLatitudeAT);
	// angular distance
	T tanSigma1 = tanUI / cos(dAzimuth);
	// parametric latitude of the geodesic vertex, or
	// azimuth of the geodesic at the equator
	T sinAlpha = cos(atan(tanUI)) * sin(dAzimuth);
	T Alpha = asin(sinAlpha);
	T cosAlpha = cos(Alpha);
	// geodesic constant
	T u2 = pow(cosAlpha, 2.0) * (pow(ellipsoid->GetSemiMajor(), 2.0) - pow(b, 2.0)) / pow(b, 2.0);

	// Vincenty's constants A' and B'
	T A = 1.0 + (u2/16384.0) * (4096.0 + (u2 * (-768.0 + (u2 * (320.0 - (175.0 * u2))))));
	T B = (u2/1024.0) * (256.0 + (u2 * (-128.0 + (u2 * (74.0 - (47.0 * u2))))));

	T Sigma = dDistance / (b * A);
	T twoSigmam, deltaSigma, SigmaDiff(99.);

	// iterate until no signigicant change in sigma
	for (UINT16 i(0); i<10; i++)
	{
		twoSigmam = (2.0 * atan(tanSigma1)) + Sigma;
		deltaSigma = B * sin(Sigma) * (cos(twoSigmam) + (B / 4.0 * ((cos(Sigma) * (-1.0 + (2.0 * pow(cos(twoSigmam), 2.0)))) - (B / 6.0 * cos(twoSigmam) * (-3.0 + (4.0 * pow(sin(Sigma), 2.0))) * ((-3.0 + (4.0 * pow(cos(twoSigmam), 2.0))))))));
		SigmaDiff = Sigma;
		Sigma = (dDistance / (b * A)) + deltaSigma;
		SigmaDiff -= Sigma;

		if (fabs(SigmaDiff) < PRECISION_1E16)
			break;
	}

	// latitude of new position
	*dLatitudeTO = atan2(((sin(atan(tanUI)) * cos(Sigma)) + (cos(atan(tanUI)) * sin(Sigma) * cos(dAzimuth))), ((1.0 - f) * pow((pow(sinAlpha, 2.0) + pow(((sin(atan(tanUI)) * sin(Sigma)) - (cos(atan(tanUI)) * cos(Sigma) * cos(dAzimuth))), 2.0)), 0.5)));

	T Lambda = atan2((sin(Sigma) * sin(dAzimuth)), ((cos(atan(tanUI)) * cos(Sigma)) - (sin(atan(tanUI))*sin(Sigma)*cos(dAzimuth))));
	T C = (f / 16.0) * pow(cosAlpha, 2.0) * (4.0 + (f * (4.0 - (3.0 * pow(cosAlpha, 2.0)))));
	T Omega = Lambda - ((1.0 - C) * f * sinAlpha * (Sigma + (C * sin(Sigma) * (cos(twoSigmam) + (C * cos(Sigma) * (-1 + (2 * pow(cos(twoSigmam), 2.0))))))));

	// longitude of new position
	*dLongitudeTO = dLongitudeAT + Omega;
}

template <class T>
void ComputeLocalElements3D(const T X1, const T Y1, const T Z1,
			const T X2, const T Y2, const T Z2,
			const T currentLatitude, const T currentLongitude,
			T* local_12e, T* local_12n, T* local_12up)
{
	// 1->2
	T dX12(X2 - X1);
	T dY12(Y2 - Y1);
	T dZ12(Z2 - Z1);

	// helpers
	T sin_lat(sin(currentLatitude));
	T cos_lat(cos(currentLatitude));
	T sin_long(sin(currentLongitude));
	T cos_long(cos(currentLongitude));

	*local_12e = -sin_long * dX12 + cos_long * dY12;
	*local_12n = -sin_lat * cos_long * dX12 -
		sin_lat * sin_long * dY12 +
		cos_lat * dZ12;
	*local_12up = cos_lat * cos_long * dX12 +
		cos_lat * sin_long * dY12 +
		sin_lat * dZ12;
}

template <class T>
void ComputeLocalElements2D(const T X1, const T Y1, const T Z1,
	const T X2, const T Y2, const T Z2,
	const T currentLatitude, const T currentLongitude,
	T* local_12e, T* local_12n)
{
	// 1->2
	T dX12(X2 - X1);
	T dY12(Y2 - Y1);
	T dZ12(Z2 - Z1);

	// helpers
	T sin_lat(sin(currentLatitude));
	T cos_lat(cos(currentLatitude));
	T sin_long(sin(currentLongitude));
	T cos_long(cos(currentLongitude));

	*local_12e = -sin_long * dX12 + cos_long * dY12;
	*local_12n = -sin_lat * cos_long * dX12 -
		sin_lat * sin_long * dY12 +
		cos_lat * dZ12;
}

template <class T>
T Direction(const T local_12e, const T local_12n)
{
	// "computed" direction 1->2
	T direction12;

	if (fabs(local_12e) < fabs(local_12n))
		direction12 = atan_2(local_12e, local_12n);
	else
		direction12 = HALF_PI - atan_2(local_12n, local_12e);

	if (direction12 < 0)
		direction12 += TWO_PI;

	return direction12;
}

template <class T>
T Direction(const T X1, const T Y1, const T Z1,
			const T X2, const T Y2, const T Z2,
			const T currentLatitude, const T currentLongitude,
			T* local_12e, T* local_12n)
{
	ComputeLocalElements2D(X1, Y1, Z1, X2, Y2, Z2, currentLatitude, currentLongitude,
		local_12e, local_12n);

	return Direction(*local_12e, *local_12n);
}

template <class T>
// helper function
T Direction(const T X1, const T Y1, const T Z1,
			const T X2, const T Y2, const T Z2,
			const T currentLatitude, const T currentLongitude)
{
	T local_12e, local_12n;

	return Direction(X1, Y1, Z1,
		X2, Y2, Z2,
		currentLatitude, currentLongitude,
		&local_12e, &local_12n);
}

template <class T>
T HorizontalAngle(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T X3, const T Y3, const T Z3,
				  const T currentLatitude, const T currentLongitude,
				  T* direction12, T* direction13,
				  T* local_12e, T* local_12n, T* local_13e, T* local_13n)
{
	// compute vectors [1->2] & [1->3] in the local reference frame
	//
	// 1->2
	*direction12 = Direction(X1, Y1, Z1, X2, Y2, Z2, currentLatitude, currentLongitude, local_12e, local_12n);
	*direction13 = Direction(X1, Y1, Z1, X3, Y3, Z3, currentLatitude, currentLongitude, local_13e, local_13n);

	if (*direction12 > *direction13)
		*direction13 += TWO_PI;

	// angle 123
	T angle = *direction13 - *direction12;

	return angle;
}

template <class T>
// helper function
T HorizontalAngle(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T X3, const T Y3, const T Z3,
				  const T currentLatitude, const T currentLongitude,
				  T* direction12, T* direction13)
{
	T local_12e, local_12n, local_13e, local_13n;

	return HorizontalAngle(X1, Y1, Z1,
		X2, Y2, Z2,
		X3, Y3, Z3,
		currentLatitude, currentLongitude,
		direction12, direction13,
		&local_12e, &local_12n, &local_13e, &local_13n);
}

template <class T>
void CartesianElementsFromInstrumentHeight(const T height, T* dX, T* dY, T* dZ,
				  const T Latitude, const T Longitude)
{
	// Use rotation matrix for local vector -> cartesian vector, whereby
	// local elements for e and n are zero
	*dX = cos(Latitude) * cos(Longitude) * height;
	*dY = cos(Latitude) * sin(Longitude) * height;
	*dZ = sin(Latitude) * height;
}

template <class T>
// The return value is the true vertical between the (local) horizontal plane
// and the instrument-target vector.  The local_12e/n/up elements represent
// the geometric difference between the two stations
T VerticalAngle(const T& local_12e, const T& local_12n, const T& local_12up)
{
	return atan2(local_12up, sqrt((local_12e * local_12e) + (local_12n * local_12n)));
}

template <class T>
// The return value is the true vertical between the (local) horizontal plane
// and the instrument-target vector.  The local_12e/n/up elements represent
// the geometric difference between the two stations
T VerticalAngle(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T latitude1, const T longitude1,
				  const T latitude2, const T longitude2,
				  const T instrumentHeight, const T targetHeight,
				  T* local_12e, T* local_12n, T* local_12up)
{
	// helpers
	T sin_lat1(sin(latitude1));
	T cos_lat1(cos(latitude1));
	T sin_long1(sin(longitude1));
	T cos_long1(cos(longitude1));

	// Compute cartesian elements dX, dY, dZ for instrument to target
	// First, compute cartesian vector for both instrument and target
	T dXih, dYih, dZih, dXth, dYth, dZth;
	CartesianElementsFromInstrumentHeight(instrumentHeight,
		&dXih, &dYih, &dZih, latitude1, longitude1);
	CartesianElementsFromInstrumentHeight(targetHeight,
		&dXth, &dYth, &dZth, latitude2, longitude2);

	T dX12(X2 - X1 + dXth - dXih);
	T dY12(Y2 - Y1 + dYth - dYih);
	T dZ12(Z2 - Z1 + dZth - dZih);

	// compute local reference frame elements (station1 to station2)
	*local_12e = -sin_long1 * dX12 + cos_long1 * dY12;
	*local_12n = -sin_lat1 * cos_long1 * dX12 -
					sin_lat1 * sin_long1 * dY12 +
					cos_lat1 * dZ12;
	*local_12up = cos_lat1 * cos_long1 * dX12 +
					cos_lat1 * sin_long1 * dY12 +
					sin_lat1 * dZ12;

	// compute angle (instrument to target)
	return VerticalAngle(*local_12e, *local_12n, *local_12up);
	//return atan2((*local_12up), sqrt(((*local_12e) * (*local_12e)) + ((*local_12n) * (*local_12n))));
	//////////////////////////////////////////////////////
}

template <class T>
// helper function
T VerticalAngle(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T latitude1, const T longitude1,
				  const T latitude2, const T longitude2,
				  const T instrumentHeight, const T targetHeight)
{
	T local_12e, local_12n, local_12up;

	return VerticalAngle(
		X1, Y1, Z1,
		X2, Y2, Z2,
		latitude1, longitude1,
		latitude2, longitude2,
		instrumentHeight, targetHeight,
		&local_12e, &local_12n, &local_12up);
}

template <class T>
// The return value is the true zenith distance between the ellipsoid normal
// and the instrument-target vector.  The local_12e/n/up elements represent
// the geometric difference between the two stations
T ZenithDistance(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T latitude1, const T longitude1,
				  const T latitude2, const T longitude2,
				  const T instrumentHeight, const T targetHeight,
				  T* local_12e, T* local_12n, T* local_12up)
{
	// helpers
	T sin_lat1(sin(latitude1));
	T cos_lat1(cos(latitude1));
	T sin_long1(sin(longitude1));
	T cos_long1(cos(longitude1));

	// Compute cartesian elements dX, dY, dZ for instrument to target
	// First, compute cartesian vector for both instrument and target
	T dXih, dYih, dZih, dXth, dYth, dZth;
	CartesianElementsFromInstrumentHeight(instrumentHeight,
		&dXih, &dYih, &dZih, latitude1, longitude1);
	CartesianElementsFromInstrumentHeight(targetHeight,
		&dXth, &dYth, &dZth, latitude2, longitude2);

	T dX12(X2 - X1 + dXth - dXih);
	T dY12(Y2 - Y1 + dYth - dYih);
	T dZ12(Z2 - Z1 + dZth - dZih);

	// compute local reference frame elements (station1 to station2)
	*local_12e = -sin_long1 * dX12 + cos_long1 * dY12;
	*local_12n = -sin_lat1 * cos_long1 * dX12 -
					sin_lat1 * sin_long1 * dY12 +
					cos_lat1 * dZ12;
	*local_12up = cos_lat1 * cos_long1 * dX12 +
					cos_lat1 * sin_long1 * dY12 +
					sin_lat1 * dZ12;

	// compute angle (instrument to target)
	return atan2(sqrt((*local_12e) * (*local_12e) + (*local_12n) * (*local_12n)), (*local_12up));
	//////////////////////////////////////////////////////
}

template <class T>
// helper function
T ZenithDistance(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T latitude1, const T longitude1,
				  const T latitude2, const T longitude2,
				  const T instrumentHeight, const T targetHeight)
{
	T local_12e, local_12n, local_12up;

	return ZenithDistance(
		X1, Y1, Z1, X2, Y2, Z2,
		latitude1, longitude1,
		latitude2, longitude2,
		instrumentHeight, targetHeight,
		&local_12e, &local_12n, &local_12up);
}

template <class T>
T EllipsoidHeight(const T X, const T Y, const T Z,
				  const T latitude, T* nu, T* Zn,
				  const CDnaEllipsoid* ellipsoid)
{
	*nu = primeVertical(ellipsoid, latitude);
	// Zn is the z coordinate element of the point on the z-axis
	// which intersects with the the normal at the given Latitude
	*Zn = ellipsoid->GetE1sqd() * (*nu) * sin(latitude);

	return sqrt(X*X + Y*Y + pow(Z+(*Zn), (int)2)) - (*nu);
}

template <class T>
T EllipsoidHeightDifference(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T Latitude1, const T Latitude2,
				  T* h1, T* h2, T* nu1, T* nu2, T* Zn1, T* Zn2,
				  const CDnaEllipsoid* ellipsoid)
{
	return ((*h2 = EllipsoidHeight(X2, Y2, Z2, Latitude2, nu2, Zn2, ellipsoid)) -
		(*h1 = EllipsoidHeight(X1, Y1, Z1, Latitude1, nu1, Zn1, ellipsoid)));
}

template <class T>
T magnitude(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2)
{
	return sqrt(((X2 - X1) * (X2 - X1)) + ((Y2 - Y1) * (Y2 - Y1)) + ((Z2 - Z1) * (Z2 - Z1)));
}

template <class T>
T magnitude(const T dX, const T dY, const T dZ)
{
	return sqrt((dX * dX) + (dY * dY) + (dZ * dZ));
}

template <class T>
T magnitude(const T X1, const T N1, const T X2, const T N2)
{
	return sqrt(((X2 - X1) * (X2 - X1)) + ((N2 - N1) * (N2 - N1)));
}

template <class T>
T magnitude(const T dX, const T dN)
{
	return sqrt((dX * dX) + (dN * dN));
}

template <class T>
T EllipsoidChordDistance(const T X1, const T Y1, const T Z1,
				  const T X2, const T Y2, const T Z2,
				  const T latitude1, const T latitude2,
				  const T Height1, const T Height2,
				  T* dX, T* dY, T* dZ,
				  const CDnaEllipsoid* ellipsoid)
{
	T nu1(primeVertical(ellipsoid, latitude1));
	T nu2(primeVertical(ellipsoid, latitude2));

	T scale1(nu1 / (nu1 + Height1));
	T scale2(nu2 / (nu2 + Height2));

	// Zn1,2 is the z coordinate element of the point on the z-axis
	// which intersects with the the normal at the given Latitude
	T Zn1(ellipsoid->GetE1sqd() * nu1 * sin(latitude1));
	T Zn2(ellipsoid->GetE1sqd() * nu2 * sin(latitude2));

	// station 1
	T x1(X1 * scale1);
	T y1(Y1 * scale1);
	T z1((Z1 + Zn1) * scale1 - Zn1);

	// station 2
	T x2(X2 * scale2);
	T y2(Y2 * scale2);
	T z2((Z2 + Zn2) * scale2 - Zn2);

	*dX = x2 - x1;
	*dY = y2 - y1;
	*dZ = z2 - z1;

	return magnitude(*dX, *dY, *dZ);
}

template <class T>
T RadiusCurvatureInChordDirection(const T X1, const T Y1, const T Z1,
			 const T X2, const T Y2, const T Z2,
			 const T latitude1, const T longitude1, const T latitude2,
			 const CDnaEllipsoid* ellipsoid)
{
	T nu, rho;
	primeVerticalandMeridian(ellipsoid, average(latitude1, latitude2), nu, rho);

	T local_12e, local_12n;
	T direction12(Direction(X1, Y1, Z1, X2, Y2, Z2,
		latitude1, longitude1, &local_12e, &local_12n));
	T cos_dir(cos(direction12));
	T sin_dir(sin(direction12));
	return  rho * nu / ((nu * cos_dir * cos_dir) + (rho * sin_dir * sin_dir));
}

template <class T>
T EllipsoidArctoEllipsoidChord(const T arc,
			 const T X1, const T Y1, const T Z1,
			 const T X2, const T Y2, const T Z2,
			 const T Latitude1, const T Longitude1, const T Latitude2,
			 const CDnaEllipsoid* ellipsoid)
{
	T r(RadiusCurvatureInChordDirection(X1, Y1, Z1, X2, Y2, Z2, Latitude1, Longitude1, Latitude2, ellipsoid));
	return 2.0 * r * sin(arc / 2.0 / r);
}

template <class T>
T EllipsoidChordtoEllipsoidArc(const T chord,
			 const T X1, const T Y1, const T Z1,
			 const T X2, const T Y2, const T Z2,
			 const T Latitude1, const T Longitude1, const T Latitude2,
			 const CDnaEllipsoid* ellipsoid)
{
	T r(RadiusCurvatureInChordDirection(X1, Y1, Z1, X2, Y2, Z2, Latitude1, Longitude1, Latitude2, ellipsoid));
	return asin(chord / 2.0 / r) * 2.0 * r;
}

template <class T>
T EllipsoidArcDistance(
			 const T X1, const T Y1, const T Z1,
			 const T X2, const T Y2, const T Z2,
			 const T Latitude1, const T Longitude1, const T Latitude2,
			 const T Height1, const T Height2,
			 const CDnaEllipsoid* ellipsoid)
{
	T dx, dy, dz, ellipsoid_chord;
	ellipsoid_chord = EllipsoidChordDistance<T>(
		X1, Y1, Z1,
		X2, Y2, Z2,
		Latitude1, Latitude2,
		Height1, Height2, &dx, &dy, &dz, ellipsoid);

	return EllipsoidChordtoEllipsoidArc<T>(
		ellipsoid_chord,
		X1, Y1, Z1,
		X2, Y2, Z2,
		Latitude1, Longitude1, Latitude2,
		ellipsoid);
}

template <class T>
T MSLChordtoMSLArc(const T chord,
			 const T latitude1, const T latitude2,
			 const T N1, const T N2,
			 const CDnaEllipsoid* ellipsoid)
{
	T nu, rho;
	primeVerticalandMeridian(ellipsoid, average(latitude1, latitude2), nu, rho);

	T r(sqrt(nu*rho) + average(N1, N2));
	return asin(chord / 2.0 / r) * 2.0 * r;
}

template <class T>
T MSLArctoMSLChord(const T arc,
			 const T latitude1, const T latitude2,
			 const T N1, const T N2,
			 const CDnaEllipsoid* ellipsoid)
{
	T nu, rho;
	primeVerticalandMeridian(ellipsoid, average(latitude1, latitude2), nu, rho);

	T r(sqrt(nu*rho) + average(N1, N2));
	return 2.0 * r * sin(arc / 2.0 / r);
}

template <class T>
T MSLChordtoEllipsoidChord(const T msl_chord,
						   const T Latitude1, const T Latitude2,
						   const T N1, const T N2,
						   const CDnaEllipsoid* ellipsoid)
{
	T ellipsoid_chord(msl_chord * msl_chord);
	ellipsoid_chord -= pow(N2 - N1, (int)2);

	T meanLat(average(Latitude1, Latitude2));
	ellipsoid_chord /= 1. + N1 / averageRadiusofCurvature(ellipsoid, meanLat);
	ellipsoid_chord /= 1. + N2 / averageRadiusofCurvature(ellipsoid, meanLat);
	return sqrt(ellipsoid_chord);
}

template <class T>
T MSLArctoEllipsoidChord(const T msl_arc,
						   const T Latitude1, const T Latitude2,
						   const T N1, const T N2,
						   const CDnaEllipsoid* ellipsoid)
{
	// 1. Convert MSL Arc -> MSL Chord
	T msl_chord = MSLArctoMSLChord<T>(msl_arc,
		Latitude1, Latitude2,
		N1, N2,
		ellipsoid);

	// 2. Convert MSL Chord -> Ellipsoid Chord
	return MSLChordtoEllipsoidChord<T>(msl_chord,
		Latitude1, Latitude2,
		N1, N2,
		ellipsoid);
}

template <class T>
T EllipsoidChordtoMSLChord(const T ellipsoid_chord,
						   const T Latitude1, const T Latitude2,
						   const T N1, const T N2,
						   const CDnaEllipsoid* ellipsoid)
{
	T msl_chord(ellipsoid_chord * ellipsoid_chord);
	T meanLat(average(Latitude1, Latitude2));

	msl_chord *= 1. + N1 / averageRadiusofCurvature(ellipsoid, meanLat);
	msl_chord *= 1. + N2 / averageRadiusofCurvature(ellipsoid, meanLat);
	msl_chord += pow(N2 - N1, (int)2);

	return sqrt(msl_chord);
}

template <class T>
T EllipsoidChordtoMSLArc(const T ellipsoid_chord,
						   const T Latitude1, const T Latitude2,
						   const T N1, const T N2,
						   const CDnaEllipsoid* ellipsoid)
{
	// 1. Convert Ellipsoid Chord -> MSL Chord
	T msl_chord = EllipsoidChordtoMSLChord<T>(
		ellipsoid_chord, Latitude1, Latitude2,
		N1, N2, ellipsoid);

	// 2. Convert MSL Chord to MSL Arc
	return MSLChordtoMSLArc<T>(
		msl_chord, Latitude1, Latitude2,
		N1, N2, ellipsoid);
}

template <class T>
T MSLArcDistance(
			 const T X1, const T Y1, const T Z1,
			 const T X2, const T Y2, const T Z2,
			 const T Latitude1, const T Longitude1, const T Latitude2,
			 const T Height1, const T Height2,
			 const T N1, const T N2,
			 const CDnaEllipsoid* ellipsoid)
{
	// 1. Calculate Ellipsoid Chord
	T dx, dy, dz, chord;
	chord = EllipsoidChordDistance<T>(
		X1, Y1, Z1,
		X2, Y2, Z2,
		Latitude1, Latitude2,
		Height1, Height2, &dx, &dy, &dz, ellipsoid);

	// 2. Convert Ellipsoid Chord -> MSL Chord
	chord = EllipsoidChordtoMSLChord<T>(
		chord, Latitude1, Latitude2,
		N1, N2, ellipsoid);

	// 3. Convert MSL Chord -> MSL Arc
	return MSLChordtoMSLArc<T>(
		chord, Latitude1, Latitude2,
		N1, N2, ellipsoid);
}

template <class T>
T LaplaceCorrection(const T azimuth, const T zenith,
					const T deflPrimeV, const T deflPrimeM,
					const T Latitude)
{
	return deflPrimeV * tan(Latitude) + ((deflPrimeM * sin(azimuth) - deflPrimeV * cos(azimuth)) / tan(zenith));	// cot(z) = 1/tan(z)
}

template <class T>
T ZenithDeflectionCorrection(const T azimuth, const T deflPrimeV, const T deflPrimeM)
{
	return deflPrimeM * cos(azimuth) + deflPrimeV * sin(azimuth);
}

template <class T>
T DirectionDeflectionCorrection(const T azimuth, const T zenith,
									  const T deflPrimeV, const T deflPrimeM)
{
	return (deflPrimeM * sin(azimuth) - deflPrimeV * cos(azimuth)) / tan(zenith);	// cot(z) = 1/tan(z)
}

template <class T>
T HzAngleDeflectionCorrection(const T azimuth12, const T zenith12,
									  const T azimuth13, const T zenith13,
									  const T deflPrimeV, const T deflPrimeM)
{
	return DirectionDeflectionCorrection(azimuth13, zenith13, deflPrimeV, deflPrimeM) -
		DirectionDeflectionCorrection(azimuth12, zenith12, deflPrimeV, deflPrimeM);
}

template <class T>
T HzAngleDeflectionCorrections(const T azimuth12, const T zenith12,
	const T azimuth13, const T zenith13,
	const T deflPrimeV, const T deflPrimeM, T& correction12, T& correction13)
{
	return (correction13 = DirectionDeflectionCorrection(azimuth13, zenith13, deflPrimeV, deflPrimeM)) -
		(correction12 = DirectionDeflectionCorrection(azimuth12, zenith12, deflPrimeV, deflPrimeM));
}
#endif /* DNATEMPLATEGEODESYFUNCS_H_ */


/* boost random/exponential_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: exponential_distribution.hpp,v 1.1 2007/02/12 18:25:54 irving Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>

namespace boost {

// exponential distribution: p(x) = lambda * exp(-lambda * x)
template<class RealType = double>
class exponential_distribution
{
public:
  typedef RealType input_type;
  typedef RealType result_type;

#if !defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS) && !(defined(BOOST_MSVC) && BOOST_MSVC <= 1300)
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit exponential_distribution(result_type lambda = result_type(1))
    : _lambda(lambda) { assert(lambda > result_type(0)); }

  // compiler-generated copy ctor and assignment operator are fine

  result_type lambda() const { return _lambda; }

  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    return -result_type(1) / _lambda * log(result_type(1)-eng());
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const exponential_distribution& ed)
  {
    os << ed._lambda;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, exponential_distribution& ed)
  {
    is >> std::ws >> ed._lambda;
    return is;
  }
#endif

private:
  result_type _lambda;
};

} // namespace boost

#endif // BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP