#include "stdafx.h"

#include <vector>

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Cholesky>

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include <iostream>

#include <math/func.h>
#include <math/matrix.h>
#include <math/SparseBlockSquareMatrix.h>

#include <import.h>
#include <api/cl_wrapper.h>

#include <time.h>

Eigen::VectorXf pcg( const SparseBlockSquareMatrix &A, const Eigen::VectorXf &b, int maxIters, float threshold = 1e-4 ) {
	Eigen::VectorXf M = A.diagonal();
	for ( int i = 0; i < M.rows(); i++ ) {
		if ( fabs( M[i] ) > threshold ) {
			M[i] = 1.0f / M[i];
		} else {
			M[i] = 1.0f;
		}
	}

	Eigen::VectorXf x = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf p = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf r = b; // b - A * x

	int m_numIters = 0;
	clock_t begin = clock();
	for ( int i = 0; i < maxIters; i++ ) {
		m_numIters++;

		Eigen::VectorXf Mr = M.array() * r.array();
		float rMr = r.dot( Mr );
		if ( threshold <= 0 && rMr < 1e-6 ) {
			rMr = 0.0f;
		} else {
			rMr = 1.0f / rMr;
		}
		p += Mr * rMr;

		Eigen::VectorXf Ap = A * p;
		float pAp = p.dot( Ap );
		if ( threshold <= 0 && pAp < 1e-6 ) {
			pAp = 0.0f;
		} else {
			pAp = 1.0f / pAp;
		}
		x +=  p * pAp;
		r -= Ap * pAp;

		float rme = sqrt( r.dot( r ) / A.size() );
		if ( rme < 1e-6 ) {
			break;
		}
	}
	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
	printf( "pcg cpu (ms) : %.3f : %.3f : %d\n", time_spent * 1000, time_spent * 1000 / m_numIters, m_numIters );

	return x;
}

void cl_reduction(
	cl::CommandQueue &cq,
	cl::Kernel &kn_reduction,
	cl::Mem &mem,
	int elem_num,
	int global_work_size, int local_work_size
) {
	int group_num = ( global_work_size + local_work_size - 1 ) / local_work_size;
	global_work_size = local_work_size * group_num;
	while ( elem_num > 1 ) { //
		int activeGroupNum = std::min( group_num, ( elem_num + local_work_size - 1 ) / local_work_size );
		cq.Kernel1D( ( kn_reduction << (int)elem_num, (int)activeGroupNum, cl::Arg( sizeof( int ) * local_work_size ), mem ), local_work_size * activeGroupNum, local_work_size );
		elem_num = activeGroupNum;
	}
}

float cl_dot_product(
	cl::CommandQueue &cq,
	cl::Kernel &kn_mul_v_v, cl::Kernel &kn_reduction,
	cl::Mem &mem_src0, cl::Mem &mem_src1, cl::Mem &mem_dst,
	int elem_num,
	int global_work_size, int local_work_size
) {
	// rMr = r * Mr;
	cq.Kernel1D( (kn_mul_v_v << elem_num, mem_src0, mem_src1, mem_dst ), global_work_size, local_work_size );

	// rMr' = r dot rMr;
	cl_reduction( cq, kn_reduction, mem_dst, elem_num, global_work_size, local_work_size );

	float dot = 0.0f;
	cq.ReadBuffer( mem_dst, CL_TRUE, 0, sizeof( float ), &dot );
	return dot;
}

class cl_block_pcg {
public :
	int m_size, m_blockSize, m_gridSize;

	int m_numIters;

	// mem
	// static matrix
	cl::Mem mem_M;
	cl::Mem mem_A_blocks;
	cl::Mem mem_A_blockInfos;
	cl::Mem mem_A_rowScan;

	// updating vector
	cl::Mem mem_x;
	cl::Mem mem_p;
	cl::Mem mem_r;

	// temp vector
	cl::Mem mem_Mv;
	cl::Mem mem_dot;

	// kernel
	cl::Kernel kn_reduction;
	cl::Kernel kn_zero_v;
	cl::Kernel kn_mul_v_v;
	cl::Kernel kn_mad_v_s;
	cl::Kernel kn_mul_m_v;

	void genTopology( cl::Context &context, SparseBlockSquareMatrix &A ) {
		m_size = A.size();
		m_blockSize = A.blockSize();
		m_gridSize = A.gridSize();

		size_t bufferSize = sizeof( float ) * m_size;

		// static topology
		mem_A_blockInfos.CreateBuffer( context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, A.m_blockInfos );
		mem_A_rowScan.CreateBuffer( context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, A.m_rowScan );

		// matrix
		mem_A_blocks.CreateBuffer( context, CL_MEM_READ_WRITE, A.m_blocks.size() * sizeof( float ) );
		mem_M.CreateBuffer( context, CL_MEM_READ_WRITE, bufferSize ); // A diagonal

		// updating vector
		mem_x.CreateBuffer( context, CL_MEM_READ_WRITE, bufferSize );
		mem_p.CreateBuffer( context, CL_MEM_READ_WRITE, bufferSize );
		mem_r.CreateBuffer( context, CL_MEM_READ_WRITE, bufferSize );

		// temp vector
		mem_Mv.CreateBuffer( context, CL_MEM_READ_WRITE, bufferSize ); // Mr, Ap
		mem_dot.CreateBuffer( context, CL_MEM_READ_WRITE, bufferSize ); // rMr, pAp
	}

	void initMatrix( cl::CommandQueue &cq, SparseBlockSquareMatrix &A ) {
		size_t bufferSize = sizeof( float ) * m_size;
		int local_work_size = 256;
		int global_work_size = ceil( m_size, local_work_size );

		Eigen::VectorXf M = A.diagonal();
		for ( int i = 0; i < M.rows(); i++ ) {
			if ( fabs( M[i] ) > 1e-6f ) {
				M[i] = 1.0f / M[i];
			} else {
				M[i] = 1.0f;
			}
		}

		cq.WriteBuffer( mem_A_blocks, CL_TRUE, A.m_blocks );
		cq.WriteBuffer( mem_M, CL_TRUE, 0, bufferSize, M.data() );
	}

	void initVector( cl::CommandQueue &cq, Eigen::VectorXf &b ) {
		size_t bufferSize = sizeof( float ) * m_size;
		int local_work_size = 256;
		int global_work_size = ceil( m_size, local_work_size );

		// x = 0;
		cq.Kernel1D( (kn_zero_v << m_size, mem_x), global_work_size, local_work_size );
		//cq.WriteBuffer( mem_x, CL_TRUE, 0, bufferSize, x.data() );

		// p = 0;
		cq.Kernel1D( (kn_zero_v << m_size, mem_p), global_work_size, local_work_size );

		// r = b = b - A * x
		cq.WriteBuffer( mem_r, CL_TRUE, 0, bufferSize, b.data() );
	}

	void getResult( cl::CommandQueue &cq, Eigen::VectorXf &x ) {
		x = Eigen::VectorXf::Zero( m_size );
		size_t bufferSize = sizeof( float ) * m_size;
		cq.ReadBuffer( mem_x, CL_TRUE, 0, bufferSize, x.data() );
	}

	void compute(
		cl::CommandQueue &cq,
		int maxIters,
		float threshold,
		int local_work_size,
		int local_work_size_A
	) {
		int global_work_size = ceil( m_size, local_work_size );

		m_numIters = 0;
		clock_t begin = clock();
		for ( int i = 0; i < maxIters; i++ ) {
			m_numIters++;

			// Mr = M * r;
			cq.Kernel1D( (kn_mul_v_v << m_size, mem_M, mem_r, mem_Mv ), global_work_size, local_work_size );

			// rMr = r dot Mr;
			float rMr = cl_dot_product( cq, kn_mul_v_v, kn_reduction, mem_r, mem_Mv, mem_dot, m_size, global_work_size, local_work_size );
			if ( rMr < threshold * m_size ) {
				break;
			}
			if ( threshold <= 0 && rMr < 1e-6 ) {
				rMr = 0.0f;
			} else {
				rMr = 1.0f / rMr;
			}

			// p += Mr / rMr'
			cq.Kernel1D( (kn_mad_v_s << m_size, mem_Mv, rMr, mem_p ), global_work_size, local_work_size );

			// Ap = A * p
			cq.Kernel1D( (kn_zero_v << m_size, mem_Mv), global_work_size, local_work_size );
			//cq.Kernel1D( (kn_mul_m_v << m_gridSize, mem_A_rowScan, mem_A_blockInfos, mem_A_blocks, mem_p, mem_Mv ), ceil( m_gridSize, 256 ), 256 );
			cq.Kernel1D( (kn_mul_m_v << m_gridSize, m_blockSize, mem_A_rowScan, mem_A_blockInfos, mem_A_blocks, mem_p, mem_Mv, cl::Arg( m_blockSize * local_work_size_A * sizeof( float ) ) ), m_gridSize *local_work_size_A, local_work_size_A );

			// pAp = p dot Ap
			float pAp = cl_dot_product( cq, kn_mul_v_v, kn_reduction, mem_p, mem_Mv, mem_dot, m_size, global_work_size, local_work_size );
			if ( threshold <= 0 && pAp < 1e-6 ) {
				pAp = 0.0f;
			} else {
				pAp = 1.0f / pAp;
			}

			// r -= Ap / pAp;
			cq.Kernel1D( (kn_mad_v_s << m_size, mem_Mv, -pAp, mem_r), global_work_size, local_work_size );

			// x +=  p / pAp;
			cq.Kernel1D( (kn_mad_v_s << m_size, mem_p , pAp, mem_x), global_work_size, local_work_size );

#if 0
			float rme =  cl_dot_product( cq, kn_mul_v_v, kn_reduction, mem_r, mem_r, mem_dot, m_size, global_work_size, local_work_size );
			rme = sqrt( rme / m_size );
			printf( "PCG : %d : %.3f\n", i, rme );
			if ( rme < 1e-6 ) {
				break;
			}
#endif
		}
		cq.Finish();

		clock_t end = clock();
		double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

		printf( "pcg gpu (ms) : %.3f : %.3f : %d\n", time_spent * 1000, time_spent * 1000 / m_numIters, m_numIters );
	}

	void compileProgram( cl::Context &context ) {
		// read source
		{
			std::vector<char> source;
			read_text_file( "../../src/cl-shader/reduction.txt", source );
			const char *sources[] = {
				"#define T float\n",
				source.data(),
				0
			};

			// program
			const char *flags = "-cl-denorms-are-zero -cl-mad-enable -cl-no-signed-zeros";
			cl::Program program;
			program.Create( context, 2, sources, 0, flags );

			// kernal
			kn_reduction.Create( program, "reduction" );
		}

		{
			std::vector<char> source;
			read_text_file( "../../src/cl-shader/vector.txt", source );
			const char *sources[] = {
				source.data(),
				0
			};

			// program
			const char *flags = "-cl-denorms-are-zero -cl-mad-enable -cl-no-signed-zeros";
			cl::Program program;
			program.Create( context, 1, sources, 0, flags );

			// kernal
			kn_zero_v.Create( program, "zero_v" );
			kn_mul_v_v.Create( program, "mul_v_v" );
			kn_mad_v_s.Create( program, "mad_v_s" );

			kn_mul_m_v.Create( program, "mul_m_v" );
		}
	}
};

void TestPCG( void ) {
	int dim = 12 * 100;

	// A : positive definite matrix
	Eigen::MatrixXf A = Eigen::MatrixXf::Random( dim, dim );
	A = A * A.transpose();
	A += Eigen::MatrixXf::Identity( dim, dim ) * dim;
	printf( "Random A\n" );
	//std::cout << A << std::endl;

	// x
	Eigen::VectorXf x = Eigen::VectorXf::Random( dim );

	// b
	Eigen::VectorXf b = A * x;

#if 0
	{
		clock_t begin = clock();
		Eigen::VectorXf xx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
		clock_t end = clock();
		double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
		printf( "cpu eigen time %fms\n", time_spent * 1000 );

		std::cout << ( xx - x ).dot( xx - x ) / xx.rows() << std::endl << std::endl;
	}
#endif

	std::cout << std::endl << "=============== Eigen ===============" << std::endl;
	{
		// fill A and b
		Eigen::ConjugateGradient<Eigen::MatrixXf, Eigen::Lower|Eigen::Upper> cg;
		clock_t begin = clock();

		cg.compute(A);
		x = cg.solve(b);

		clock_t end = clock();
		double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
		printf( "cpu eigen (ms) : %.3f : %.3f : %d\n", time_spent * 1000, time_spent * 1000 / cg.iterations(), cg.iterations() );
		std::cout << cg.error() << std::endl;
	}

	SparseBlockSquareMatrix B;
	B.createFromDenseMatrix( A, 12 );

	float threshold = 0.0f;

	std::cout << std::endl << "=============== CPU ===============" << std::endl;
	{
		Eigen::VectorXf xx = pcg( B, b, 100, threshold );
		std::cout << ( xx - x ).dot( xx - x ) / xx.rows() << std::endl << std::endl;
	}

	std::cout << std::endl << "=============== GPU ===============" << std::endl;
	{
		// context
		cl::Context context;
		cl::System::CreateContext( context );
		cl::CommandQueue cq( context );

		cl_block_pcg pcg;
		pcg.compileProgram( context );

		pcg.genTopology( context, B );

		pcg.initMatrix( cq, B );
		pcg.initVector( cq, b );

		pcg.compute( cq, 100, threshold, 256, 256 );

		Eigen::VectorXf xx;
		pcg.getResult( cq, xx );
		std::cout << ( xx - x ).dot( xx - x ) / xx.rows() << std::endl << std::endl;
	}
}

int _tmain(int argc, _TCHAR* argv[])
{
	TestPCG();

	return 0;
}


#include "Clausen.h"
#include <boost/math/special_functions/zeta.hpp>
namespace ucd {

double Clausen( double theta )
{
    if ( theta == 0 )
        return 0;
    while ( theta < 0 )
        theta += 2*M_PI;
    while ( theta > 2*M_PI )
        theta -= 2*M_PI;
    if ( theta > M_PI )
    {
        theta -= M_PI;
        double diff = theta - M_PI/2;
        theta -= 2*diff;
        return -Clausen( theta );
    }

    // precompute coefficients for the partial sum part of the Clausen
    // function
    static std::vector< double > coeff;
    if ( coeff.size( ) == 0 )
    {
        std::vector< double > num, den, frac;
        for ( int i = 1; i < 20; ++i )
        {
            num.push_back( boost::math::zeta( 2*i ) );
            den.push_back( i*(2*i + 1) );
            frac.push_back( num.back( ) / den.back( ) );
        }

        coeff.push_back( 0 );
        for ( int i = 0; i < frac.size( ); ++i )
        {
            coeff.push_back( 0 );
            coeff.push_back( frac[i] );
        }
    }

    double res = 1 - log( theta );

    double hornerSum = 0.0;
    double term = theta / (2 * M_PI);
    for ( int i = coeff.size( ) - 1; i >= 0; --i )
    {
        if ( i == coeff.size( ) - 1 )
            hornerSum = coeff[i];
        else
            hornerSum = coeff[i] + hornerSum*term;
    }
    res += hornerSum;

    return theta*res;
}

} // namespace ucd


/**
 * @file
 * @brief Investigates when parametrization from reference element breaks down
 * @author Anian Ruoss
 * @date   2019-02-04 13:36:17
 * @copyright MIT License
 */

#include <lf/geometry/geometry.h>
#include <lf/quad/quad.h>
#include <lf/refinement/refinement.h>

#include <Eigen/Eigen>
#include <filesystem>
#include <fstream>
#include <string>

/**
 * @brief Stores an Eigen::MatrixXd to .csv file
 * @param file_path path to .csv file
 * @param matrix matrix to be saved
 */
void writeMatrixToCSV(const std::string& file_path,
                      const Eigen::MatrixXd& matrix) {
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream file(file_path.c_str());
  file << matrix.format(CSVFormat);
  file.close();
}

/**
 * @brief Saves the global vertex/midpoint coordinates
 * @param file_path path to .csv file
 * @param geom second-order geometry object
 */
void storeSecondOrderCoords(const std::string& file_path,
                            const lf::geometry::Geometry& geom) {
  const Eigen::MatrixXd& local_vertex_coords = geom.RefEl().NodeCoords();
  const long num_vertices = local_vertex_coords.cols();

  Eigen::MatrixXd all_local_coords(local_vertex_coords.rows(),
                                   2 * num_vertices);

  // compute local vertex and midpoint coordinates from reference element
  for (auto col_idx = 0; col_idx < num_vertices; ++col_idx) {
    all_local_coords.col(col_idx) = local_vertex_coords.col(col_idx);
    all_local_coords.col(col_idx + num_vertices) =
        (local_vertex_coords.col(col_idx) +
         local_vertex_coords.col((col_idx + 1) % num_vertices)) /
        2.;
  }

  writeMatrixToCSV(file_path, geom.Global(all_local_coords));
}

/**
 * @brief Evaluates parametrization and corresponding jacobian determinant
 * @param base_file_path base path for .csv files
 * @param geom geometry object
 * @param qr_order order of quadrature rule to sample points from
 */
void storeParametrizationEvals(const std::string& base_file_path,
                               const lf::geometry::Geometry& geom,
                               const size_t& qr_order) {
  // use quadrature to sample random points on the reference element
  auto qr = lf::quad::make_QuadRule(geom.RefEl(), qr_order);

  const auto& points = qr.Points();
  const Eigen::MatrixXd& jacobians = geom.Jacobian(points);
  Eigen::VectorXd determinants(points.cols());

  for (Eigen::Index point_idx = 0; point_idx < points.cols(); ++point_idx) {
    determinants(point_idx) = jacobians
                                  .block(0, point_idx * geom.DimLocal(),
                                         geom.DimGlobal(), geom.DimLocal())
                                  .determinant();
  }

  writeMatrixToCSV(base_file_path + "_refpoints.csv", points);
  writeMatrixToCSV(base_file_path + "_points.csv", geom.Global(points));
  writeMatrixToCSV(base_file_path + "_jacdets.csv", determinants);
}

/**
 * @brief Computes volume of geometry object by means of overkill quadrature
 * @param geom geometry object
 * @return geometry volume
 */
double computeGeometryVolume(const lf::geometry::Geometry& geom) {
  const auto qr = lf::quad::make_QuadRule(geom.RefEl(), 23);

  const auto& points = qr.Points();
  const auto& weights = qr.Weights();
  const auto& integrationElements = geom.IntegrationElement(points);

  double vol = 0.;

  for (size_t j = 0; j < points.cols(); ++j) {
    vol += weights(j) * integrationElements(j);
  }

  return vol;
}

int main() {
  // create a directory to store results
  const std::string results_dir = "results/";
  std::filesystem::create_directory(results_dir);

  // define second-order geometry elements
  lf::geometry::TriaO2 tria(
      (Eigen::MatrixXd(2, 6) << 1, 6, 3, 3.7, 4.2, 2.3, 1, 3, 8, 1.2, 5.2, 4.5)
          .finished());
  lf::geometry::TriaO2 tria_degenerate(
      (Eigen::MatrixXd(2, 6) << 1, 6, 3, 5, 4.5, 1.75, 1, 3, 8, 4, 9, 6.5)
          .finished());
  lf::geometry::QuadO2 quad((Eigen::MatrixXd(2, 8) << 3, 7, 4, 1, 5, 5.4, 2.5,
                             1.8, 1, 3, 7, 8, 2.5, 5.9, 6.9, 4.1)
                                .finished());
  lf::geometry::QuadO2 quad_degenerate(
      (Eigen::MatrixXd(2, 8) << 3, 7, 4, 1, 6, 5, 2, 2, 1, 3, 7, 8, 0, 6, 5, 2)
          .finished());

  // store vertex/midpoint coordinates, random point evaluations and
  // corresponding jacobian determinants for every geometry object
  for (const auto& geom_element :
       {std::pair<std::string, lf::geometry::Geometry*>{"tria", &tria},
        std::pair<std::string, lf::geometry::Geometry*>{"tria_degenerate",
                                                        &tria_degenerate},
        std::pair<std::string, lf::geometry::Geometry*>{"quad", &quad},
        std::pair<std::string, lf::geometry::Geometry*>{"quad_degenerate",
                                                        &quad_degenerate}}) {
    storeSecondOrderCoords(results_dir + geom_element.first + "_coords.csv",
                           *geom_element.second);
    storeParametrizationEvals(results_dir + geom_element.first,
                              *geom_element.second, 50);

    const double volume = computeGeometryVolume(*geom_element.second);
    double refined_volume = 0;

    // compute child geometries by means of regular refinement
    auto children = geom_element.second->ChildGeometry(
        lf::refinement::Hybrid2DRefinementPattern(geom_element.second->RefEl(),
                                                  lf::refinement::rp_regular),
        0);

    // store vertex/midpoint coordinates, random point evaluations and
    // corresponding jacobian determinants for every child geometry object
    for (std::size_t child_idx = 0; child_idx < children.size(); ++child_idx) {
      storeSecondOrderCoords(results_dir + geom_element.first + "_child_" +
                                 std::to_string(child_idx) + "_coords.csv",
                             *children[child_idx]);
      storeParametrizationEvals(results_dir + geom_element.first + "_child_" +
                                    std::to_string(child_idx),
                                *children[child_idx], 20);

      refined_volume += computeGeometryVolume(*children[child_idx]);
    }

    // save volumes
    writeMatrixToCSV(results_dir + geom_element.first + "_volumes.csv",
                     (Eigen::VectorXd(2) << volume, refined_volume).finished());
  }

  return 0;
}


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


/**
 * \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


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


#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


//============================================================================
// 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_ */


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <string>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include "DatasetAR.h"
#include "DatasetPoly.h"
#include "GaussSeq.h"
#include "ARSeq.h"
#include "ARPoly.h"
#include "utils.h"
using utils::my_float;
using boost::multiprecision::cpp_bin_float_50;
using boost::random::uniform_real_distribution;

int main() {
    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u(0.001, 1);

    // prepare
    std::map<int, my_float> coeff_3 {
        {1, 0.4},
        {3, -20},
    };
    std::map<int, my_float> pow_3 {
        {1, 3},
        {3, 2},
    };
    std::map<int, my_float> coeff_4 {
        {1, 0.4},
        {3, 0.1},
    };
    std::map<int, my_float> pow_4 {
        {1, 7},
        {3, 0.5},
    };
    std::vector<my_float> v_seed_three(3);

    // Non-stationary, non-linear AR process 1
    for (auto type = 0; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            // Seed differently
            for (auto i = 0; i < 3; i++) {
                v_seed_three[i] = u(gen);
            }
            ARPoly targ_seq(coeff_3, pow_3, u(gen));
            targ_seq.seed_prev_vals(v_seed_three);

            DatasetPoly dat("./ARPoly-1/ARPoly-1-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), targ_seq, type);
            dat.write_csv();
        }
    }

    // Non-stationary, non-linear AR process 2
    for (auto type = 0; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            // Seed differently
            for (auto i = 0; i < 3; i++) {
                v_seed_three[i] = u(gen);
            }
            ARPoly targ_seq(coeff_4, pow_4, u(gen));
            targ_seq.seed_prev_vals(v_seed_three);

            DatasetPoly dat("./ARPoly-2/ARPoly-2-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), targ_seq, type);
            dat.write_csv();
        }
    }

    return 0;
}


//! [mathematical-all]
#include <cmath>
#include <iostream>
#include <string>
#include <vector>

#include <boost/align/aligned_allocator.hpp>
#include <boost/align/aligned_delete.hpp>

#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/function/cos.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/sin.hpp>
#include <boost/simd/function/sincos.hpp>
#include <boost/simd/function/store.hpp>
#include <boost/simd/function/ulpdist.hpp>
#include <boost/simd/pack.hpp>

namespace bs = boost::simd;

template <typename T>
void test_results(const std::string& mes, const T& scalr, const T& simdr)
{
  std::cout << mes;
  for (int i = 0; i < scalr.size(); ++i) {
    if (bs::ulpdist(scalr[i], simdr[i]) > 0.5) {
      std::cout << " failed" << std::endl;
      return;
    }
  }
  std::cout << " succeeded" << std::endl;
}

int main(int argc, char** argv)
{
  namespace ba = boost::alignment;

  using pack_t = bs::pack<float>;

  std::size_t num_elements = 1024;
  std::size_t alignment    = pack_t::alignment;
  //! [mathematical-declare]
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> X(num_elements);
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> sinX(num_elements),
    sc_sinX(num_elements);
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> cosX(num_elements),
    sc_cosX(num_elements);
  //! [mathematical-declare]

  //! [fill-input]
  for (int i = 0; i < num_elements; ++i) {
    X[i] = (float(i) / num_elements) * bs::Pio_4<float>();
  }

  //! [fill-input]
  //! [mathematical-scalar]
  for (int i = 0; i < num_elements; ++i) {
    sc_sinX[i] = std::sin(X[i]);
    sc_cosX[i] = std::cos(X[i]);
  }
  //! [mathematical-scalar]

  //! [mathematical-calc-individ]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    bs::store(bs::sin(v0), &sinX[i]);
    bs::store(bs::cos(v0), &cosX[i]);
  }
  //! [mathematical-calc-individ]
  test_results("sin test               ", sc_sinX, sinX);
  test_results("cos test               ", sc_cosX, cosX);

  //! [mathematical-calc-combine]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    auto res  = bs::sincos(v0);
    bs::store(res.first, &sinX[i]);
    bs::store(res.second, &cosX[i]);
  }
  //! [mathematical-calc-combine]
  test_results("sincos test for sin    ", sc_sinX, sinX);
  test_results("sincos test for cos    ", sc_cosX, cosX);

  //! [mathematical-calc-restricted]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    bs::store(bs::restricted_(bs::sin)(v0), &sinX[i]);
    bs::store(bs::restricted_(bs::cos)(v0), &cosX[i]);
  }
  //! [mathematical-calc-restricted]
  test_results("restricted_(sin) test  ", sc_sinX, sinX);
  test_results("restricted_(cos) test  ", sc_cosX, cosX);

  return 0;
}
// This code can be compiled using (for instance for gcc)
// g++ mathematical.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o mathematical
// -I/path_to/boost_simd/ -I/path_to/boost/

//! [mathematical-all]


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/sign.hpp
 *
 * \brief Compute the sign function for each element of a vector or matrix
 *  expression.
 *
 * The sign function for real numbers as defined as follows:
 * \f[
 * \operatorname{sign}(x):=\begin{cases}
 *                          -1 & \text{if } x<0,\\
 *                           0 & \text{if } x=0,\\
 *                           1 & \text{if } x>0.
 *                         \end{cases}
 * \f]
 * In case of complex numbers, the sign function is defined as follows:
 * \f[
 * \operatorname{sign}(z)=\begin{cases}
 *                          \frac{z}{|z|} & \text{if } z \ne 0,\\
 *                          0 & \text{if } z = 0+0i.
 *                        \end{cases}
 * \f]
 *
 * \sa The sign function at Wikipedia: https://en.wikipedia.org/wiki/Sign_function
 *
 * \author comcon1 (original version)
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * 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_NUMERIC_UBLASX_OPERATION_SIGN_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SIGN_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/utility/enable_if.hpp>
#include <cmath>
#include <complex>
#include <limits>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_sign_functor_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
    typedef typename type_traits<signature_argument_type>::value_type signature_result_type;
    typedef vector_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_sign_functor_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
    typedef typename type_traits<signature_argument_type>::value_type signature_result_type;
    typedef matrix_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

/// Auxiliary function for real types: sign(x) = 1 if x > 0, 0 if x == 0, -1 otherwise.
template <typename T>
BOOST_UBLAS_INLINE
typename ::boost::disable_if<
            ::boost::is_complex<T>,
            T
>::type sign_impl(T x)
{
    if (::std::isnan(x))
    {
        return ::std::numeric_limits<T>::quiet_NaN();
    }
    return (x > 0) ? 1 : ((x < 0) ? -1 : 0);
}

/// Auxiliary function for complex types: sign(x) = x ./ abs(x)
template <typename T>
BOOST_UBLAS_INLINE
typename ::boost::enable_if<
            ::boost::is_complex<T>,
            T
>::type sign_impl(T x)
{
    typename T::value_type a = ::std::abs(x);
    return (a == 0) ? T(0,0) : (x / a);
}

//template <typename RealType>
//BOOST_UBLAS_INLINE
//RealType sign(RealType v)
//{
//    return ( -(RealType)( ::std::signbit(v) ) + 0.5 ) * 2.0;
//}

} // Namespace detail

/**
 * \brief Applies the \c std::sign function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::sign to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_sign_functor_traits<VectorExprT>::result_type sign(vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_sign_functor_traits<VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_sign_functor_traits<VectorExprT>::signature_result_type signature_result_type;

    return expression_type(ve(), detail::sign_impl<signature_result_type>);
}

/**
 * \brief Applies the \c std::sign function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::sign to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_sign_functor_traits<MatrixExprT>::result_type sign(matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_sign_functor_traits<MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_sign_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

    return expression_type(me(), detail::sign_impl<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SIGN_HPP


/**
 * @file  maximumprinciple.cc
 * @brief NPDE homework "MaximumPrinciple" code
 * @author Oliver Rietmann
 * @date 25.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "maximumprinciple.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <vector>

namespace MaximumPrinciple {

/**
 * @brief Assembly on a tensor product mesh
 *
 * Compute the global Galerkin matrix from the local
 * element matrix.
 *
 * @param M Number of interior vertices in x and y direction.
 * @param B_K Local element matrix.
 * @return Global Galerkin matrix of size M^2 times M^2.
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<double> assemble(int M, const Eigen::Matrix3d &B_K) {
  int M2 = M * M;
  Eigen::SparseMatrix<double> A(M2, M2);
  double near_neighbour_contribution = 2.0 * B_K(0, 1);
  double far_neighbour_contribution = 2.0 * B_K(1, 2);
  double self_contribution = 2.0 * (B_K(0, 0) + B_K(1, 1) + B_K(2, 2));

  std::vector<double> contribution = {
      far_neighbour_contribution,  near_neighbour_contribution,
      near_neighbour_contribution, self_contribution,
      near_neighbour_contribution, near_neighbour_contribution,
      far_neighbour_contribution};

  std::vector<Eigen::Vector2i> shift = {{-1, -1}, {0, -1}, {-1, 0}, {0, 0},
                                        {1, 0},   {0, 1},  {1, 1}};

  std::vector<Eigen::Triplet<double>> tripletList;
  for (int i = 0; i < M; ++i) {
    for (int j = 0; j < M; ++j) {
      Eigen::Vector2i self = Eigen::Vector2i(i, j);
      for (int k = 0; k < 7; ++k) {
        Eigen::Vector2i other = self + shift[k];
        if (0 <= other(0) && other(0) < M && 0 <= other(1) && other(1) < M) {
          tripletList.push_back(Eigen::Triplet<double>(
              self(0) + M * self(1), other(0) + M * other(1), contribution[k]));
        }
      }
    }
  }
  A.setFromTriplets(tripletList.begin(), tripletList.end());
  return A;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<double> computeGalerkinMatrix(int M, double c) {
  Eigen::Matrix3d B_K;
  double h = 1.0 / (M + 1);
  Eigen::Matrix3d A_K;
  A_K << 1.0, -0.5, -0.5, -0.5, 0.5, 0.0, -0.5, 0.0, 0.5;
  Eigen::Matrix3d M_K;
  M_K << 2.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 2.0;
  M_K *= h * h / 24.0;
  B_K = (1.0 - c) * A_K + c * M_K;
  return assemble(M, B_K);
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_4 */
Eigen::SparseMatrix<double> computeGalerkinMatrixTR(int M, double c) {
  Eigen::Matrix3d B_K;
  double h = 1.0 / (M + 1);
  Eigen::Matrix3d A_K;
  A_K << 1.0, -0.5, -0.5, -0.5, 0.5, 0.0, -0.5, 0.0, 0.5;
  Eigen::Matrix3d M_K = h * h / 6.0 * Eigen::Matrix3d::Identity();
  B_K = (1.0 - c) * A_K + c * M_K;
  return assemble(M, B_K);
}
/* SAM_LISTING_END_4 */

}  // namespace MaximumPrinciple


// ideal_random_walk.cpp

#include <algorithm>
#include <fstream>

#include <boost/math/special_functions/factorials.hpp>

#include "ideal_random_walk.h"

namespace idealRandomWalk {

using boost::math::factorial;

long double IdealRandomWalks::num_walks(
        VectorThree start_pos,
        VectorThree end_pos,
        int steps) {

    // Check stored values
    VectorThree DR {end_pos - start_pos};

    // Only work with one permutation of DR
    DR = DR.absolute().sort();
    pair<VectorThree, int> walk_key {DR, steps};
    if (m_num_walks.count(walk_key)) {
        return m_num_walks.at(walk_key);
    }
    int DX {DR[0]};
    int DY {DR[1]};
    int DZ {DR[2]};
    int Nminus {(steps - DX - DY - DZ) / 2};
    int Nplus {(steps - DX - DY + DZ) / 2};
    long double walks {0};

    int DR_sum {DX + DY + DZ};
    if (DR_sum > steps or (steps - DR_sum) % 2 != 0) {

        // Add entry
        m_num_walks[walk_key] = walks;
        return walks;
    }

    // Need some negative steps to reach a negative location
    for (int ybar {0}; ybar != Nminus + 1; ybar++) {
        for (int xbar {0}; xbar != Nminus + 1 - ybar; xbar++) {
            auto f1 {factorial<long double>(steps)};
            auto f2 {factorial<long double>(xbar)};
            if (xbar + DX < 0) {
                continue;
            }

            auto f3 {factorial<long double>(xbar + DX)};
            auto f4 {factorial<long double>(ybar)};
            if (ybar + DY < 0) {
                continue;
            }

            auto f5 {factorial<long double>(ybar + DY)};
            auto f6 {factorial<long double>(Nminus - xbar - ybar)};
            if (Nplus - xbar - ybar < 0) {
                continue;
            }

            auto f7 {factorial<long double>(Nplus - xbar - ybar)};
            walks += f1 / (f2 * f3 * f4 * f5 * f6 * f7);
        }
    }

    // Add entries
    m_num_walks[walk_key] = walks;

    return walks;
}

void IdealRandomWalks::delete_entry(
        VectorThree start_pos,
        VectorThree end_pos,
        int steps) {

    VectorThree DR {end_pos - start_pos};
    DR = DR.absolute().sort();
    pair<VectorThree, int> walk_key {DR, steps};
    m_num_walks.erase(walk_key);
}
} // namespace idealRandomWalk


#ifndef PERLINNOISESOURCE_HPP
#define PERLINNOISESOURCE_HPP

#include <cmath>

#include <boost/multi_array.hpp>

#include "Math.hpp"
#include "NoiseSource.hpp"
#include "Random.hpp"
#include "Vector3.hpp"

/**
 * A noise source that uses the perlin noise algorithm.
 */
template <typename T>
class PerlinNoiseSource : public NoiseSource<T>
{
public:
	/**
	 * Create a perlin noise source.
	 *
	 * @param size the range in each dimension that will be unique.
	 * @param rng the random number generator to use.
	 */
	PerlinNoiseSource(unsigned int size, Random& rng) :
		gradientArray(boost::extents[size][size][size]),
		size(size)
	{
		//Generate random gradients
		for( unsigned int x = 0; x < size; x++ )
		{
			for( unsigned int y = 0; y < size; y++ )
			{
				for( unsigned int z = 0; z < size; z++ )
				{
					Vector3<T> gradient( normalizeReal(rng.nextReal()), normalizeReal(rng.nextReal()), normalizeReal(rng.nextReal()) );
					gradient.normalize();
					this->gradientArray[x][y][z] = gradient;
				}
			}
		}
	}

	T noise(T x, T y = 0, T z = 0) const
	{
		clampDomain<T>(x, size);
		clampDomain<T>(y, size);
		clampDomain<T>(z, size);

		//Determine the cell that we are in
		T xi, yi, zi;
		T xf = std::modf(x, &xi);
		T yf = std::modf(y, &yi);
		T zf = std::modf(z, &zi);

		int a[] = {static_cast<int>(xi) % static_cast<int>(size),
			static_cast<int>(xi + 1) % static_cast<int>(size)};
		int b[] = {static_cast<int>(yi) % static_cast<int>(size),
			static_cast<int>(yi + 1) % static_cast<int>(size)};
		int c[] = {static_cast<int>(zi) % static_cast<int>(size),
			static_cast<int>(zi + 1) % static_cast<int>(size)};

		//Compute dot products
		T dots[2][2][2];
		for( int i = 0; i < 2; i++ )
		{
			for( int j = 0; j < 2; j++ )
			{
				for( int k = 0; k < 2; k++ )
				{
					dots[i][j][k] = dot(this->gradientArray[a[i]][b[j]][c[k]], Vector3<T>(xf - (T)i, yf - (T)j, zf - (T)k));
				}
			}
		}

		//Compute fade in each dimension
		T fadeX = fade<T>(xf);
		T fadeY = fade<T>(yf);
		T fadeZ = fade<T>(zf);

		//Perform trilinear interpolation
		T ix1 = lerp<T>(dots[0][0][0], dots[1][0][0], fadeX);
		T ix2 = lerp<T>(dots[0][1][0], dots[1][1][0], fadeX);
		T ix3 = lerp<T>(dots[0][0][1], dots[1][0][1], fadeX);
		T ix4 = lerp<T>(dots[0][1][1], dots[1][1][1], fadeX);

		T iy1 = lerp<T>(ix1, ix2, fadeY);
		T iy2 = lerp<T>(ix3, ix4, fadeY);

		return lerp<T>(iy1, iy2, fadeZ);
	}

private:
	boost::multi_array<Vector3<T>, 3> gradientArray;
	unsigned int size;
};

#endif // PERLINNOISESOURCE_HPP


#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>

template<int innodes, int outnodes>
struct NNLayer {
  Eigen::Matrix<float, innodes, outnodes> w;
  Eigen::Matrix<float, 1, outnodes> bias;
  Eigen::Matrix<float, 1, outnodes> output;
  bool needActivation;

  NNLayer() : needActivation(true)
    {
      w      = Eigen::Matrix<float, innodes, outnodes>::Random(innodes, outnodes);
      bias   = Eigen::Matrix<float, 1, outnodes>::Random(1, outnodes);
      output = Eigen::Matrix<float, 1, outnodes>::Random(1, outnodes);
    }

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m)
    {
      output = (m * w);
      output = output - bias;
      if (needActivation)
        return activate();
      else
        return output;
    }

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> activate()
    {
      Eigen::Matrix<float, 1, outnodes> o = output;
      for(int i=0; i<output.cols(); i++)
        {
          o(0, i) = 1.0/(1.0 + exp(o(0, i)));
        }
      return o;
    }

  void setActivation(bool isneed)
    {
      needActivation = isneed;
    }
};

class NeuralNetwork {
  NNLayer<2, 2> l1;
  NNLayer<2, 2> l2;
  NNLayer<2, 1> l3;

  public:
    NeuralNetwork()
      {
        l3.setActivation(false);
      };

    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> input)
      {
        return l3.forward( l2.forward( l1.forward( input ) ) );
      };

    void back_propagation()
      {
      };
};

int main(void)
{
  NeuralNetwork nn;

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> out;
  Eigen::Matrix<float, 1, 2> input;
  input << 3, 3;

  out = nn.forward( input );

  std::cout << "out = " << out << std::endl;

  return 0;
}