// MovementPatternFunctions.cpp : Defines the entry point for the console application.
//

/*
#include <iostream>
#include <vector>
#include <math.h>
#include <complex>
#include <algorithm>
#include <numeric>
#include <tuple>

#include <Eigen/Dense>
*/
#include "UDG_Hokuyo.h"
using namespace std;
using namespace Eigen;

const float PI = 3.14159265;

void scanningPattern_rotateToAngle(vector<float>& x_pos, vector<float>& y_pos, float xC, float yC, float rotationAngle){

	rotationAngle = rotationAngle*(PI / 180);

	for (int i = 0; i < static_cast<int>(x_pos.size()); i++)
	{
		x_pos[i] = cos(rotationAngle) * (x_pos[i] - xC) - sin(rotationAngle) * (y_pos[i] - yC) + xC;
		y_pos[i] = sin(rotationAngle) * (x_pos[i] - xC) + cos(rotationAngle) * (y_pos[i] - yC) + yC;
	}

}

void scanningPattern_horiz(vector<float>& x_vec, vector<float>& y_vec, vector<float>& z_vec, float xC, float yC, float radius, float height, float rotAngle, float startAngle, float endAngle, float deltaPoints){

	if (startAngle > endAngle){
		deltaPoints = -deltaPoints;
	}

	endAngle += deltaPoints;

	for (float i = startAngle; i < endAngle; i = i+ deltaPoints)
	{
		x_vec.push_back(xC + radius*cos(i*(PI / 180)));
		y_vec.push_back(yC + radius*sin(i*(PI / 180)));
		z_vec.push_back(height);
	}

	scanningPattern_rotateToAngle(x_vec, y_vec, xC, yC, rotAngle);

}

//float calculatePCA_rotAngle(float positions[][2], int size){
float calculatePCA_rotAngle(vector<double>& x_pos, vector<double>& y_pos){

	int size = x_pos.size();

	MatrixXf posMat(x_pos.size(), 2);

	for (int i = 0; i < size; i++)
	{
		posMat(i, 0) = x_pos[i];
		posMat(i, 1) = y_pos[i];
	}

	/*
	for (int i = 0; i < size; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			posMat(i, j) = positions[i][j];
		}
	}
	*/

	float avg_x = 0.0;
	float sum_x = 0.0;
	float avg_y = 0.0;
	float sum_y = 0.0;
	for (int i = 0; i < size; i++)
	{
		sum_x += posMat(i, 0);
		sum_y += posMat(i, 1);
	}
	avg_x = ((float)sum_x) / size;
	avg_y = ((float)sum_y) / size;

	for (int i = 0; i < size; i++)
	{
		posMat(i, 0) -= avg_x;
		posMat(i, 1) -= avg_y;
	}

	int fact = size - 1;

	MatrixXf covMat(2, 2);
	covMat = (posMat.transpose()*posMat.conjugate()) / fact;

	/*
	std::cout << covMat(0, 0) << ' ';
	std::cout << covMat(0, 1) << std::endl;
	std::cout << covMat(1, 0) << ' ';
	std::cout << covMat(1, 1) << std::endl;
*/

	EigenSolver<MatrixXf> es(covMat);
	/*
	cout << "Eigenvalue:"
		<< endl << es.eigenvalues() << endl;

	cout << "Eigenvectors:"
		<< endl << es.eigenvectors() << endl;

	*/
	float eigenvalueCalc[2] = { es.eigenvalues()[0].real(), es.eigenvalues()[1].real() };

	float eigenvectorCalc[2][2] = { { es.eigenvectors()(0, 0).real(), es.eigenvectors()(0, 1).real() }, { es.eigenvectors()(1, 0).real(), es.eigenvectors()(1, 1).real() } };

	int index = -1;

	if (eigenvalueCalc[0] > eigenvalueCalc[1])
	{
		index = 0;
	}
	else
	{
		index = 1;
	}

	float maxEigenVec[2] = { eigenvectorCalc[0][index], eigenvectorCalc[1][index] };
	/*
	std::cout << maxEigenVec[0] << ' ';
	std::cout << maxEigenVec[1] << std::endl;
	*/
	float bladeRotAngle = atan2(maxEigenVec[0], maxEigenVec[1])*(180 / PI);

	if (bladeRotAngle < 0)
	{
		bladeRotAngle = bladeRotAngle + 180;
	}

	return bladeRotAngle;

}

void lidarData_thresholdByDistance(vector<int>& angles, vector<int>& distances, float minDist, float maxDist){

	std::vector<size_t> results;

	auto it = std::find_if(std::begin(distances), std::end(distances), [maxDist, minDist](int i){return i > maxDist || i < minDist; });
	while (it != std::end(distances)) {
		results.emplace_back(std::distance(std::begin(distances), it));
		it = std::find_if(std::next(it), std::end(distances), [maxDist, minDist](int i){return i > maxDist || i < minDist; });

	}

	for (size_t i = 0; i < results.size(); i++)
	{

		distances[results[i]] = -1;
		angles[results[i]] = -1;
	}

	distances.erase(remove(distances.begin(), distances.end(), -1), distances.end());
	angles.erase(remove(angles.begin(), angles.end(), -1), angles.end());

}

tuple<double, double> calculateMeans(vector<int>& angles, vector<int>& distances){

	double meanDist = -1;
	double meanAngle = -1;

	meanDist = accumulate(distances.begin(), distances.end(), 0.0) / distances.size();

	vector<double> sinAngles;
	vector<double> cosAngles;

	for (size_t i = 0; i < angles.size(); i++)
	{
		sinAngles.push_back(sin(angles[i] * (PI / 180)));
		cosAngles.push_back(cos(angles[i] * (PI / 180)));

	}

	double avgSin = accumulate(sinAngles.begin(), sinAngles.end(), 0.0) / sinAngles.size();

	double avgCos = accumulate(cosAngles.begin(), cosAngles.end(), 0.0) / cosAngles.size();

	if (avgSin>0 && avgCos>0)
	{
		meanAngle = atan(avgSin / avgCos);
	}
	else if (avgCos <0)
	{
		meanAngle = atan(avgSin / avgCos) + 180 * (PI / 180);
	}
	else if (avgSin <0 && avgCos >0)
	{
		meanAngle = atan(avgSin / avgCos) + 360 * (PI / 180);
	}

	meanAngle = meanAngle * (180 / PI);

	return make_tuple(meanAngle, meanDist);

}

void lidar_PolarToCart(double centerX, double centerY, vector<int>& angles, vector<int>& distances, vector<double>& posX, vector<double>& posY){

	for (size_t i = 0; i < distances.size(); i++)
	{
		posX.push_back(centerX + distances[i] * sin(angles[i] * (PI / 180)));
		posY.push_back(centerY + distances[i] * cos(angles[i] * (PI / 180)));
	}

}

tuple<double, double, double, int, int> getPathCenterFromMaxDist(vector<double>& x_pos, vector<double>& y_pos){

	double maxDistance = 0;
	double maxDistance_ind[2] = {-1,-1};

	double tempDist = 0;
	for (size_t i = 0; i < x_pos.size(); i++)
	{
		for (size_t j = 0; j < x_pos.size(); j++)
		{
			tempDist = sqrt(pow(x_pos[i] - x_pos[j], 2) + pow(y_pos[i] - y_pos[j], 2));
			if (tempDist > maxDistance)
			{
				maxDistance = tempDist;
				maxDistance_ind[0] = i;
				maxDistance_ind[1] = j;
			}

		}
	}

	int path_centerIndex = max(maxDistance_ind[0], maxDistance_ind[1]);
	double path_centerX = x_pos[path_centerIndex];
	double path_centerY = y_pos[path_centerIndex];

	return  make_tuple(maxDistance, path_centerX, path_centerY, maxDistance_ind[0], maxDistance_ind[1]);

}


#include <Eigen/Dense>
#include "numerical_gradient.h"

namespace MyDL{

    using namespace Eigen;

    MatrixXd numerical_gradient(double (*f)(MatrixXd), MatrixXd x){
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for(int i = 0; i < size_x; i++){
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2*h);

            x(i) = tmp_val;
        }

        return grad;
    }

    // TwoLayerNetなどで用いるもの(ラムダ式使用・DNNのインスタンスに含まれるパラメータを参照して使用する場合)
    MatrixXd numerical_gradient(const std::function<double(MatrixXd)>& f, MatrixXd& x)
    {
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for (int i = 0; i < size_x; i++)
        {
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            x(i) = tmp_val;
        }

        return grad;
    }
}


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXf;
using Eigen::MatrixXf;
using namespace LBFGSpp;

class Rosenbrock
{
private:
    int n;
    uint64_t ncalls = 0;
    float delta = 1e-7;
public:
    Rosenbrock(int n_) : n(n_) {}
    float operator()(const VectorXf& x, VectorXf& grad)
    {
        ncalls++;
        if (ncalls != 1) return getForward(x);

        float fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
            fx += t1 * t1 + t2 * t2;
        }
        return fx;
    }
    float getForward(const VectorXf& x)
    {
        float fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            fx += t1 * t1 + t2 * t2;
        }
        return fx;
    }
    float getDg(const VectorXf& drt, const VectorXf& x1, const float& fx)
    {
        float res = 0;
        VectorXf x2 = x1 + delta * drt;
        float fx2 = getForward(x2);
        res = ( fx2 - fx ) / delta;
        return res;
    }
    void getGrad(const VectorXf& x, VectorXf& grad)
    {
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
        }
    }
};

int main()
{
    const int n = 10;
    LBFGSParam<float> param;
    LBFGSSolver<float> solver(param);
    Rosenbrock fun(n);

    VectorXf x = VectorXf::Zero(n);
    float fx;
    int niter = solver.minimize(fun, x, fx);

    std::cout << niter << " iterations" << std::endl;
    std::cout << "x = \n" << x.transpose() << std::endl;
    std::cout << "f(x) = " << fx << std::endl;

    return 0;
}


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <boost/python.hpp>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/list.hpp>
using namespace std;

class matrix
{
	int rows,cols;
	float a[10][10];
public:
	void dimensions(int r,int c)
	{
		rows=r;
		cols=c;
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				a[i][j]=0;
	}
	void read( float x,int i, int j)
	{
		a[i][j]=x;

	}
	void display()
	{
		for(int i=0;i<rows;i++)
		{
			for(int j=0;j<cols;j++)
				cout<<a[i][j]<<"  ";
			cout<<endl;
		}
	}
	matrix operator + (matrix obj)
	{
		matrix sum;
		sum.dimensions(rows,cols);
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				sum.a[i][j]=a[i][j]+obj.a[i][j];
		return sum;
	}
	matrix operator - (matrix obj)
	{
		matrix dif;
		dif.dimensions(rows,cols);
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				dif.a[i][j]=a[i][j]-obj.a[i][j];
		return dif;
	}
	matrix operator * (matrix obj)	//only for square matrices
	{
		matrix product;
		product.dimensions(rows,cols);
		for(int i=0;i<rows;i++)
			for(int j=0;j<obj.cols;j++)
			{
				product.a[i][j]=0;
				for(int m=0;m<cols;m++)
					product.a[i][j]=product.a[i][j]+a[i][m]*obj.a[m][j];
			}
		return product;
	}
 	//int operator ++()
	//{
	//	return determinant(a,R);
	//}
	matrix operator & (matrix obj)
	{
		matrix inv;
		inv.dimensions(rows,cols);
		inverse(a,inv.a);
		return inv;

	}
	void cofactor(float mat[10][10], float temp[10][10], int p, int q, int n)
	{
    	int i = 0, j = 0;
    	for (int row = 0; row < n; row++)
        	for (int col = 0; col < n; col++)
            	if (row != p && col != q)
            	{
                	temp[i][j++] = mat[row][col];
                	if (j == n - 1)
                	{
                    	j = 0;
                    	i++;
                	}
            	}
	}
	float determinant(float mat[10][10], float n)
	{
    	float sum = 0;
    	if (n == 1)
     	   return mat[0][0];
 		float temp[10][10];
	 	float sign = 1;
    	for (int f = 0; f < n; f++)
    	{
   	    	cofactor(mat, temp, 0, f, n);
       		sum =sum+ sign * mat[0][f] * determinant(temp, n - 1);
        	sign = -sign;
    	}
 		return sum;
	}
	void adjoint(float A[10][10],float adj[10][10])
	{
    	if (rows == 1)
    	{
      		adj[0][0] = 1;
       		return;
    	}
    	float sign = 1;
    	float temp[10][10];
 		for (int i=0; i<rows; i++)
    	{
        	for (int j=0; j<rows; j++)
        	{
        	    cofactor(A, temp, i, j, rows);
        	    sign = ((i+j)%2==0)? 1: -1;
            	adj[j][i] = (sign)*(determinant(temp, rows-1));
        	}
    	}
	}
	bool inverse(float A[10][10], float inverse[10][10])
	{
    	float det = determinant(A, rows);
    	if (det == 0)
    	{
       		cout << "Inverse can't be found as matrix is singular"<<endl;
        	return false;
    	}
    	float adj[10][10];
    	adjoint(A, adj);
    	for (int i=0; i<rows; i++)
       		for (int j=0; j<rows; j++)
            	inverse[i][j] = adj[i][j]/det;
 		return true;
	}
};

using namespace boost::python;
BOOST_PYTHON_MODULE(matrix)
{
	class_<matrix>("matrix")
		.def("dimensions",&matrix::dimensions)
		.def("read",&matrix::read)
		.def("display",&matrix::display)
		.def("cofactor",&matrix::cofactor)
		.def("determinant",&matrix::determinant)
		.def("adjoint",&matrix::adjoint)
		.def("inverse",&matrix::inverse)
		.def(self+self)
		.def(self-self)
		.def(self*self)
//		.def(++self)
		.def(self&self)
	;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3f A(3,3);
A << 1,2,3,  4,5,6,  7,8,10;
Matrix<float,3,2> B;
B << 3,1, 3,1, 4,1;
Matrix<float,3,2> X;
X = A.fullPivLu().solve(B);
cout << "The solution with right-hand side (3,3,4) is:" << endl;
cout << X.col(0) << endl;
cout << "The solution with right-hand side (1,1,1) is:" << endl;
cout << X.col(1) << endl;

  return 0;
}


#include <boost/program_options.hpp>
#include <cmath>
#include <iostream>
#include <string>

static unsigned long long gcd(unsigned long long a, unsigned long long b) {
    unsigned long long c;
    while (a != 0) {
        c = a;
        a = b % a;
        b = c;
    }
    return b;
}

static bool is_prime(unsigned int number) {
    const unsigned int upper_limit = std::floor(std::sqrt(number));

    for (unsigned int a = 2; a <= upper_limit; a++) {
        if (number % a == 0) {
            return false;
        }
    }
    return true;
}

void output_coprimes(const unsigned int composite_number, const unsigned int length) {
    unsigned output_length  = 0;
    unsigned current_number = 2;

    while (output_length < length) {
        if (gcd(current_number, composite_number) == 1) {
            std::cout << current_number << "\n";
            output_length++;
        }
        current_number++;
    }
}

void output_primes(const unsigned int composite_number, const unsigned int length) {
    unsigned output_length  = 0;
    unsigned current_number = 2;

    while (output_length < length) {
        if (is_prime(current_number) && gcd(current_number, composite_number) == 1) {
            std::cout << current_number << "\n";
            output_length++;
        }
        current_number++;
    }
}

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    po::options_description description("JKQ DDSIM by https://iic.jku.at/eda/ -- Allowed options");
    description.add_options()("help,h", "produce help message")("composite_number,N", po::value<unsigned int>()->required(),
                                                                "number of measurements on the final quantum state")("strategy,S", po::value<std::string>()->required(),
                                                                                "strategy for prime base generation (primes, coprimes)")("length,L", po::value<unsigned int>()->required(), "how many bases to generate");
    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, description), vm);
        if (vm.count("help")) {
            std::cout << description;
            return 0;
        }
        po::notify(vm);
    } catch (const po::error& e) {
        std::cerr << "[ERROR] " << e.what() << "! Try option '--help' for available commandline options.\n";
        std::exit(1);
    }

    if (vm["strategy"].as<std::string>() == "coprimes") {
        output_coprimes(vm["composite_number"].as<unsigned int>(), vm["length"].as<unsigned int>());
    } else if (vm["strategy"].as<std::string>() == "primes") {
        output_primes(vm["composite_number"].as<unsigned int>(), vm["length"].as<unsigned int>());
    } else {
        std::cerr << "Invalid stragety.\n";
    }
}


#include <Eigen/Core>
#include "problem.h"
#include "random.h"
#include "so3.h"

#include <math.h>

void compute_error( const Eigen::Matrix3d &R, const Eigen::Vector3d &t,
                   const Eigen::Matrix3d &Rsoln, const Eigen::Vector3d &tsoln,
                  double &rot_angle_err, double &trans_angle_err, double &trans_scale_err )
{
    Eigen::Matrix3d Rdiff = R*Rsoln.transpose();
    Eigen::Vector3d rdiff = so3ln(Rdiff);
    rot_angle_err = rdiff.norm();

    trans_angle_err = acos(t.dot(tsoln)/t.norm()/tsoln.norm());

    if ( isnan(trans_angle_err) ) trans_angle_err = 0;
    trans_scale_err = tsoln.norm()/t.norm();
}

void generateProblem( double trans_mag, double angle, double noise, Problem &prob, int nrays )
{
    Eigen::Vector3d X;    // 3D points
    Eigen::Vector3d cu;   // camera centers for first multi-camera rig
    Eigen::Vector3d cv;   // camera centers for second multi-camera rig
    Eigen::Vector3d PX;   // 3D points after R,t transformation
    Ray u;                // observations in first multi-camera rig
    Ray v;                // observations in second multi-camera rig

    Eigen::Vector3d w = rand3f();
    w = w/w.norm();
    double theta = angle*M_PI/180;
    prob.R = so3exp(w*theta);                // random rotation
    prob.t = rand3f();                       // random translation
    while ( prob.t.norm() < 1e-10 ) prob.t = rand3f();
    prob.t = prob.t/prob.t.norm()*trans_mag;

    prob.ray_pairs.clear();

    for ( size_t i = 0; i < nrays; i++ ) {
        X = rand3f();    // sample random point
        while ( X.norm() == 0 ) X = rand3f();
        X = X/X.norm();
        double scale_factor = ((double)rand()/RAND_MAX) * 4 + 4;
        X *= scale_factor;

        cu = rand3f();        // sample random camera locations
        cv = rand3f(); // inter-camera
        //cv = cu;    // intra-camera

        PX = prob.R*X+prob.t;    // transform point
        u.x = X-cu;    // make observation rays
        u.c = cu;
        v.x = PX-cv;
        v.c = cv;

        if ( noise > 0. )    // add noise with requested standard deviation
        {
            double unorm = u.x.norm();
            Eigen::Vector3d uproj = u.x/u.x(2);
            uproj.head(2) += grand2f()*noise;
            uproj /= uproj.norm();
            uproj *= unorm;
            if ( (X-cu).dot(uproj) < 0 ) uproj = -uproj;
            u.x = uproj;

            double vnorm = v.x.norm();
            Eigen::Vector3d vproj = v.x/v.x(2);
            vproj.head(2) += grand2f()*noise;
            vproj /= vproj.norm();
            vproj *= vnorm;
            if ( (PX-cv).dot(vproj) < 0 ) vproj = -vproj;
            v.x = vproj;
        }

        prob.ray_pairs.push_back( RayPair( u, v ) );
    }
}


#include "sophus/se3.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <iostream>

using namespace std;
using namespace Eigen;

/// Basic Sophus implementation

int main(int argc, char **argv) {

  // Rotation of 90 degrees about thez axis
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // Quaternion object
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);  // Sophus::SO3d Lie algebra object from rotation matrix
  Sophus::SO3d SO3_q(q);  // Same lie algebra object from quaternion
  // Print the SO3 matrix from both the lie algebra object
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // Log is the 3D vector Lie Algebra representation of the rotation matrix
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat representation which is the skew symmetric matrix
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // vee gets the vector from the skew symmetric matrix
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // Apply a left perturbation
  Vector3d update_so3(1e-4, 0, 0);  // A small perturbation
  // Get the lie group (Rotation matrix) from the rotation vector and apply the rotation
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // SE3 sophus
  Vector3d t(1, 0, 0);        // Translation by 1m
  Sophus::SE3d SE3_Rt(R, t);  // Lie Algebra from R,t SE(3)
  Sophus::SE3d SE3_qt(q, t);  // Lie Algebra from q,t SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // Lie Algebra for SE3 is a 6 Dof Vector
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // Get the hat and recover the vector format of Lie Algebra from the hat
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // Left perturb SE3
  Vector6d update_se3;  // Small perturbation in roh
  update_se3.setZero();
  update_se3(0, 0) = 1e-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}


#include <cmath>
#include <random>
#include <Eigen/Dense>
#include "aggregate_gen_SA.hpp"
using namespace Eigen;
using namespace std;

//return pos_sph matrix
MatrixXd aggregate_gen_SA(const double& a, const int& num_sph, const double& kf, const double& Df, const double& tol){
    MatrixXd pos_sph= MatrixXd::Zero(3,num_sph);
    pos_sph.col(0) << -a,0,0;
    pos_sph.col(1) << a,0,0;

    Vector3d pos_cand(3);
    double rN;
    double phi;
    double u;
    ArrayXd dist_sph;

    for(int N=3; N <= num_sph; ++N){
        rN= N*N*a*a/(N-1)*pow(N/kf,2/Df)-N*a*a/(N-1)-N*a*a*pow((N-1)/kf,2/Df);
        rN= sqrt(rN);
        dist_sph.resize(N-1);

        while(1){
            phi= 2*M_PI*urf(re);
            u= 2*urf(re)-1;
            pos_cand << sqrt(1.0-u*u)*cos(phi), sqrt(1.0-u*u)*sin(phi), u;
            pos_cand *= rN;

            for(int i= 0; i < N-1; ++i){
                dist_sph(i)=(pos_sph.col(i)-pos_cand).norm();
            }

            if(dist_sph.minCoeff() < 2*a+tol && dist_sph.minCoeff() > 2*a){
                pos_sph.col(N-1)= pos_cand;
                pos_sph.colwise() -= pos_sph.rowwise().mean(); // translate the centroid to origin
                break;
            }
        }
    }

    // final check of the attached condition
    ArrayXd mindist_c(num_sph);
    for(int i= 0; i< num_sph; ++i){
        mindist_c(i)=((pos_sph.colwise()-pos_sph.col(i)).colwise().norm().array()-2*a).array().abs().minCoeff();
    }
    assert(mindist_c.maxCoeff() < tol);

    return pos_sph;
}


// https://www.boost.org/doc/libs/1_67_0/libs/math/example/numerical_derivative_example.cpp

// Copyright Christopher Kormanyos 2013.
// 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).

#ifdef _MSC_VER
#  pragma warning (disable : 4996) // assignment operator could not be generated.
#endif

# include <iostream>
# include <iomanip>
# include <limits>
# include <cmath>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/math/special_functions/next.hpp> // for float_distance

//[numeric_derivative_example
/*`The following example shows how multiprecision calculations can be used to
obtain full precision in a numerical derivative calculation that suffers from precision loss.

Consider some well-known central difference rules for numerically
computing the 1st derivative of a function [f'(x)] with [/x] real.

Need a reference here?  Introduction to Partial Differential Equations, Peter J. Olver
 December 16, 2012

Here, the implementation uses a C++ template that can be instantiated with various
floating-point types such as `float`, `double`, `long double`, or even
a user-defined floating-point type like __multiprecision.

We will now use the derivative template with the built-in type `double` in
order to numerically compute the derivative of a function, and then repeat
with a 5 decimal digit higher precision user-defined floating-point type.

Consider the function  shown below.
!!
(3)
We will now take the derivative of this function with respect to x evaluated
at x = 3= 2. In other words,

(4)

The expected result is

 0:74535 59924 99929 89880 . (5)
The program below uses the derivative template in order to perform
the numerical calculation of this derivative. The program also compares the
numerically-obtained result with the expected result and reports the absolute
relative error scaled to a deviation that can easily be related to the number of
bits of lost precision.

*/

/*` [note Rquires the C++11 feature of
[@http://en.wikipedia.org/wiki/Anonymous_function#C.2B.2B anonymous functions]
for the derivative function calls like `[]( const double & x_) -> double`.
*/

template <typename value_type,  typename function_type>
value_type derivative (const value_type x, const value_type dx, function_type function)
{
  /*! \brief Compute the derivative of function using a 3-point central difference rule of O(dx^6).
    \tparam value_type, floating-point type, for example: `double` or `cpp_dec_float_50`
    \tparam function_type

    \param x Value at which to evaluate derivative.
    \param dx Incremental step-size.
    \param function Function whose derivative is to computed.

    \return derivative at x.
  */

  BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<value_type>::is_integer, "value_type must be a floating-point type!");

  const value_type dx2(dx * 2U);
  const value_type dx3(dx * 3U);
  // Difference terms.
  const value_type m1 ((function (x + dx) - function(x - dx)) / 2U);
  const value_type m2 ((function (x + dx2) - function(x - dx2)) / 4U);
  const value_type m3 ((function (x + dx3) - function(x - dx3)) / 6U);
  const value_type fifteen_m1 (m1 * 15U);
  const value_type six_m2 (m2 * 6U);
  const value_type ten_dx (dx * 10U);
  return ((fifteen_m1 - six_m2) + m3) / ten_dx;  // Derivative.
} //

#include <boost/multiprecision/cpp_dec_float.hpp>
  using boost::multiprecision::number;
  using boost::multiprecision::cpp_dec_float;

// Re-compute using 5 extra decimal digits precision (22) than double (17).
#define MP_DIGITS10 unsigned (std::numeric_limits<double>::max_digits10 + 5)

typedef cpp_dec_float<MP_DIGITS10> mp_backend;
typedef number<mp_backend> mp_type;

int main()
{
  {
    const double d =
      derivative
      ( 1.5, // x = 3.2
        std::ldexp (1., -9), // step size 2^-9 = see below for choice.
        [](const double & x)->double // Function f(x).
        {
          return std::sqrt((x * x) - 1.) - std::acos(1. / x);
        }
      );

    // The 'exactly right' result is [sqrt]5 / 3 = 0.74535599249992989880.
    const double rel_error = (d - 0.74535599249992989880) / 0.74535599249992989880;
    const double bit_error = std::abs(rel_error) / std::numeric_limits<double>::epsilon();
    std::cout.precision (std::numeric_limits<double>::digits10); // Show all guaranteed decimal digits.
    std::cout << std::showpoint ; // Ensure that any trailing zeros are shown too.

    std::cout << " derivative : " << d << std::endl;
    std::cout << " expected   : " << 0.74535599249992989880 << std::endl;
    // Can compute an 'exact' value using multiprecision type.
    std::cout << " expected   : " << sqrt(static_cast<mp_type>(5))/3U << std::endl;
    std::cout << " bit_error : " << static_cast<unsigned long>(bit_error)  << std::endl;

    std::cout.precision(6);
    std::cout << "float_distance = " << boost::math::float_distance(0.74535599249992989880, d) << std::endl;

  }

  { // Compute using multiprecision type with an extra 5 decimal digits of precision.
    const mp_type mp =
      derivative(mp_type(mp_type(3) / 2U), // x = 3/2
        mp_type(mp_type(1) / 10000000U), // Step size 10^7.
        [](const mp_type & x)->mp_type
        {
          return sqrt((x * x) - 1.) - acos (1. / x); // Function
        }
    );

    const double d = mp.convert_to<double>(); // Convert to closest double.
    const double rel_error = (d - 0.74535599249992989880) / 0.74535599249992989880;
    const double bit_error = std::abs (rel_error) / std::numeric_limits<double>::epsilon();
    std::cout.precision (std::numeric_limits <double>::digits10); // All guaranteed decimal digits.
    std::cout << std::showpoint ; // Ensure that any trailing zeros are shown too.
    std::cout << " derivative : " << d << std::endl;
    // Can compute an 'exact' value using multiprecision type.
    std::cout << " expected   : " << sqrt(static_cast<mp_type>(5))/3U << std::endl;
    std::cout << " expected   : " << 0.74535599249992989880
    << std::endl;
    std::cout << " bit_error : "  << static_cast<unsigned long>(bit_error)  << std::endl;

    std::cout.precision(6);
    std::cout << "float_distance = " << boost::math::float_distance(0.74535599249992989880, d) << std::endl;

  }

} // int main()

/*`
The result of this program on a system with an eight-byte, 64-bit IEEE-754
conforming floating-point representation for `double` is:

 derivative : 0.745355992499951

 derivative : 0.745355992499943
 expected   : 0.74535599249993
 bit_error : 78

    derivative : 0.745355992499930
   expected   : 0.745355992499930
   bit_error : 0

The resulting bit error is 0. This means that the result of the derivative
calculation is bit-identical with the double representation of the expected result,
and this is the best result possible for the built-in type.

The derivative in this example has a known closed form. There are, however,
countless situations in numerical analysis (and not only for numerical deriva-
tives) for which the calculation at hand does not have a known closed-form
solution or for which the closed-form solution is highly inconvenient to use. In
such cases, this technique may be useful.

This example has shown how multiprecision can be used to add extra digits
to an ill-conditioned calculation that suffers from precision loss. When the result
of the multiprecision calculation is converted to a built-in type such as double,
the entire precision of the result in double is preserved.

 */

/*

  Description: Autorun "J:\Cpp\big_number\Debug\numerical_derivative_example.exe"
   derivative : 0.745355992499943
   expected   : 0.745355992499930
   expected   : 0.745355992499930
   bit_error : 78
  float_distance = 117.000
   derivative : 0.745355992499930
   expected   : 0.745355992499930
   expected   : 0.745355992499930
   bit_error : 0
  float_distance = 0.000000

 */


#include "interpolator.h"
#include <cmath>
#include <Eigen/Core>
#include <Eigen/LU>

using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::FullPivLU;
using Eigen::Map;

namespace rbf
{

extern VectorXd solveLinearSystem(const MatrixXd& A, const VectorXd& y);

Interpolator::Interpolator(FunctionType functionType, const double epsilon) :
    functionType(functionType),
    epsilon(epsilon)
{
}

void Interpolator::reset()
{
    ys.clear();
    xs.clear();
    w.clear();
}

void Interpolator::addCenterPoint(const double y, const vector<double>& x)
{
    ys.push_back(y);
    xs.push_back(x);
}

void Interpolator::computeWeights(const bool useRegularization, const double lambda)
{
    assert(ys.size() == xs.size());

    const int dim = ys.size();

    MatrixXd O = MatrixXd::Zero(dim, dim);
    VectorXd y = Map<VectorXd>(&ys[0], dim);

    for (int i = 0; i < dim; ++ i)
    {
        for (int j = 0; j < dim; ++ j)
        {
            O(i, j) = getRbfValue(xs[i], xs[j]);
        }
    }

    MatrixXd A;
    VectorXd b;
    if (useRegularization)
    {
        MatrixXd O2 = MatrixXd::Zero(dim * 2, dim);
        for (int i = 0; i < dim; ++ i)
        {
            for (int j = 0; j < dim; ++ j)
            {
                O2(i, j) = O(i, j);
            }
        }
        const double coef = 0.5 * lambda;
        for (int i = 0; i < dim; ++ i)
        {
            O2(i + dim, i) = coef;
        }

        VectorXd y2 = VectorXd::Zero(dim * 2);
        for (int i = 0; i < dim; ++ i)
        {
            y2(i) = y(i);
        }

        A = O2.transpose() * O2;
        b = O2.transpose() * y2;
    }
    else
    {
        A = O;
        b = y;
    }

    const VectorXd x = solveLinearSystem(A, b);
    assert(x.rows() == dim);

    w.resize(dim);
    for (int i = 0; i < dim; ++ i)
    {
        w[i] = x(i);
    }
}

double Interpolator::getInterpolatedValue(const vector<double>& x) const
{
    assert(w.size() == xs.size());

    const int dim = w.size();

    double result = 0.0;
    for (int i = 0; i < dim; ++ i)
    {
        result += w[i] * getRbfValue(x, xs[i]);
    }

    return result;
}

double Interpolator::getRbfValue(const double r) const
{
    double result;
    switch (functionType)
    {
        case FunctionType::Gaussian:
            result = exp(- pow((epsilon * r), 2.0));
            break;
        case FunctionType::ThinPlateSpline:
            result = r * r * log(r);
            if (isnan(result))
            {
                result = 0.0;
            }
            break;
        case FunctionType::InverseQuadratic:
            result = 1.0 / (1.0 + pow((epsilon * r), 2.0));
            break;
        case FunctionType::BiharmonicSpline:
            result = r;
            break;
        default:
            break;
    }
    return result;
}

double Interpolator::getRbfValue(const vector<double>& xi, const vector<double>& xj) const
{
    assert (xi.size() == xj.size());

    const VectorXd xiVec = Map<const VectorXd>(&xi[0], xi.size());
    const VectorXd xjVec = Map<const VectorXd>(&xj[0], xj.size());

    return getRbfValue((xjVec - xiVec).norm());
}

VectorXd solveLinearSystem(const MatrixXd& A, const VectorXd& y)
{
    FullPivLU<MatrixXd> lu(A);
    return lu.solve(y);
}

}


#include <vector>
#include <boost/format.hpp>
#include <algorithm>
#include <fstream>
#include <string>
#include <random>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>

using boost::multiprecision::cpp_int;
template <typename Rng, size_t BitCount>
using bit_generator = boost::random::independent_bits_engine<Rng, BitCount, cpp_int>;

template<typename E>
E gcd(E a, E b) {
    E zero(0);
    while (b != zero) {
        a = a % b;
        std::swap(a, b);
    }
    return a;
}

template<typename T>
std::pair<T, T>
quotient_remainder(T a, T b) {
  return { a / b, a % b };
}

template <typename E>
std::pair<E, E> extended_gcd(E a, E b) {
    E x0(1);
    E x1(0);
    while (b != E(0)) {
        // compute new r and x
        std::pair<E, E> qr = quotient_remainder(a, b);
        E x2 = x0 - qr.first * x1;
        // shift r and x
        x0 = x1;
        x1 = x2;
        a = b;
        b = qr.second;
    }
    return {x0, a};
}

template<typename Integer>
constexpr
inline
bool odd(Integer x) {
  return (x & 1) == 1;
}

template<typename Integer>
constexpr
inline
bool even(Integer x) {
  return !odd(x);
}

template<typename Integer>
constexpr
inline
Integer half(Integer x) {
  // precondition(x >= 0)
  return x >> 1;
}

template <typename A, typename N, typename Op>
// requires (Domain<Op, A>)
A power_accumulate_semigroup(A r, A a, N n, Op op) {
    // precondition(n >= 0);
    if (n == 0) return r;
    while (true) {
        if (odd(n)) {
            r = op(r, a);
            if (n == 1) return r;
        }
        n = half(n);
        a = op(a, a);
    }
}

template <typename MultiplicativeSemigroup, typename Integer>
MultiplicativeSemigroup power_accumulate_semigroup(MultiplicativeSemigroup r, MultiplicativeSemigroup a, Integer n) {
    // precondition(n >= 0);
    if (n == 0) return r;
    while (true) {
        if (odd(n)) {
            r = r * a;
            if (n == 1) return r;
        }
        n = half(n);
        a = a * a;
    }
}

template <typename A, typename N, typename Op>
// requires (Domain<Op, A>)
A power_semigroup(A a, N n, Op op) {
    // precondition(n > 0);
    while (!odd(n)) {
        a = op(a, a);
        n = half(n);
    }
    if (n == 1) return a;
    // Auch
    return power_accumulate_semigroup(a, op(a, a), half(N{n - 1}), op);
}

template <typename MultiplicativeSemigroup, typename Integer>
MultiplicativeSemigroup power_semigroup(MultiplicativeSemigroup a, Integer n) {
    // precondition(n > 0);
    while (!odd(n)) {
        a = a * a;
        n = half(n);
    }
    if (n == 1) return a;
    return power_accumulate_semigroup(a, a * a, half(n - 1));
}

template<typename T>
struct modulo_multiply {
  T value;
  modulo_multiply() = default;
  modulo_multiply(const modulo_multiply &) = default;
  modulo_multiply(const T x) : value(x) {}
  T operator()(T x, T y) {
    return (x * y) % value;
  }
};

template<typename T>
std::pair<T, T> factorize(T x, T val) {
  // Bad code, can be better
  T k;
  for (k = 0; x % val == 0; x /= val, ++k);
  return std::make_pair(x, k);
}

template <typename I>
bool miller_rabin_test(I n, I q, I k, I w) {
    // precondition n > 1 && n - 1 = 2^kq && q is odd

    modulo_multiply<I> mmult(n);
    I x = power_semigroup(w, q, mmult);
    if (x == I(1) || x == n - I(1)) return true;
    for (I i(1); i < k; ++i) {

        // invariant x = w^{2^{i-1}q}

        x = mmult(x, x);
        if (x == n - I(1)) return true;
        if (x == I(1))     return false;
    }
    return false;
}

template <typename N>
N stein_gcd(N m, N n) {
    if (m < N(0)) m = -m;
    if (n < N(0)) n = -n;
    if (m == N(0)) return n;
    if (n == N(0)) return m;

    // m > 0 && n > 0

    int d_m = 0;
    while (even(m)) { m >>= 1; ++d_m;}

    int d_n = 0;
    while (even(n)) { n >>= 1; ++d_n;}

    // odd(m) && odd(n)

    while (m != n) {
      if (n > m) std::swap(n, m);
      m -= n;
      do m >>= 1; while (even(m));
    }

    // m == n

    return m << std::min(d_m, d_n);
}

template<typename Integer, typename RandomGenerator>
inline
Integer random_coprime(Integer x, RandomGenerator& rand) {
  Integer number = rand();
  while (number == 0) number = rand();

  Integer gcd_value = gcd(x, number);
  if (gcd_value == Integer{1}) {
    return number;
  }
  ++number;
  return number;
}

template<typename Integer, typename ProbeGenerator>
bool check_primarity(Integer n, ProbeGenerator& rand) {
  if (even(n)) { return false; }

  std::pair<Integer, Integer> qk = factorize(Integer{n - 1}, Integer{2});
  const constexpr size_t witnesses = 100;

  for (size_t i = 0; i < witnesses; ++i) {
    Integer w = random_coprime(n, rand);
    if (!miller_rabin_test(n, qk.first, qk.second, w)) {
      return false;
    }
  }
  return true;
}

template<typename PrimarityTest, typename ProbeGenerator, typename WitnessGenerator>
auto probable_prime(PrimarityTest primarity_test, ProbeGenerator& rand, WitnessGenerator& rand_wintness) {
  auto x = rand();
  while (!primarity_test(x, rand_wintness))  {
    x = rand();
  }
  return x;
}

template<typename ProbeGenerator, typename WitnessGenerator>
auto probable_prime(ProbeGenerator& rng, WitnessGenerator& wng) {
  return probable_prime([](const auto &x, auto& rnd) { return check_primarity(x, rnd); }, rng, wng);
}

template <typename I>
I multiplicative_inverse(I a, I n) {
    std::pair<I, I> p = extended_gcd(a, n);
    if (p.second != I(1)) return I(0);
    if (p.first < I(0)) return p.first + n;
    return p.first;
}

template<typename T0, typename T1, typename T2>
struct triple {
  T0 m0;
  T1 m1;
  T2 m2;
  triple(const T0 &m0, const T1 &m1, const T2 &m2) : m0(m0), m1(m1), m2(m2) {}
};

struct rsa_keychain {
  cpp_int n = 0;
  cpp_int public_key = 0;
  cpp_int private_key = 0;
  rsa_keychain(const cpp_int &n, const cpp_int &public_key, const cpp_int &private_key) : n(n),
                                                                                public_key(public_key),
                                                                                private_key(private_key) {}
  size_t block_power_of_2() const
  {
    cpp_int value;
    if (public_key == 0) { value = private_key; }
    else if (private_key == 0) { value = public_key; }
    else { value = std::min(public_key, private_key); }
    const cpp_int m = cpp_int{std::min(value, n)};
    return static_cast<size_t>(factorize(m, cpp_int{2}).first);
  }
};

template <typename ProbeGenerator, typename WitnessGenerator>
rsa_keychain rsa_key_generation(ProbeGenerator& p_generator, WitnessGenerator& w_generator) {
  typedef cpp_int Integer;
  while(true) {
  Integer p1 = probable_prime(p_generator, w_generator);
  Integer p2 = probable_prime(p_generator, w_generator);
  Integer n = p1 * p2;
  Integer phi = (p1 - 1) * (p2 - 1);
  Integer public_key = random_coprime(phi, p_generator);
  while (public_key >= phi) {
    public_key = random_coprime(phi, p_generator);
  }
  Integer private_key = multiplicative_inverse(public_key, phi);
  if (private_key != 0)
  return rsa_keychain(n, public_key, private_key);
  }
}

// implement using operator<<
std::pair<std::string, std::string> save(const rsa_keychain &rsa_data, const std::string &file_name) {
  auto first = file_name + ".public";
  std::ofstream public_file(first);
  public_file << std::hex << rsa_data.n << std::endl << rsa_data.public_key;
  auto second = file_name + ".public";
  std::ofstream private_file(second);
  private_file << std::hex << rsa_data.n << std::endl << rsa_data.private_key;
  return {first, second};
}

/*
template<typename OutputIterator>
OutputIterator encrypt(uint8_t *first, uint8_t *last, const cpp_int &n, const cpp_int &public_key, OutputIterator out) {
  constexpr size_t block_size = factorize(n, cpp_int{2});
  auto qr = quotient_remainder(size_t{last - first}, block_size);
  for (size_t i = 0; i != qr.first; ++i) {
    cpp_int val = *(uint64_t*)first;
  }
  return out;
}
*/

cpp_int crypt(cpp_int val, cpp_int n, cpp_int key) {
  return power_semigroup(val, key, modulo_multiply<cpp_int>(n));
}

int main() {
  //boost::mt19937 rng(42);
  bit_generator<std::random_device, 512> probe_generator, witness_generator;
  auto rsa_triple = rsa_key_generation(probe_generator, witness_generator);
  cpp_int value = 65;
  auto encrypted = crypt(value, rsa_triple.n, rsa_triple.public_key);
  auto decrypted = crypt(encrypted, rsa_triple.n, rsa_triple.private_key);
  std::cout << decrypted << std::endl;

}


#include "Arduino.h"
#include "math.h"
#include "navduino.h"
#include "wgs84.h"
#include "eigen.h"
#include <Eigen/LU>
#include <Eigen/Geometry>

using namespace Eigen;

/*
  Description:
  ------------
  This function converts a vector of euler angles into a Direction Cosine
  Matrix (DCM) given a rotation and frame sequence. In the case of this
  function, the returned DCM maps vectors from one coordinate frame (either
  North-East-Down (NED) or body frame) into the other. This is done by
  multiplying the DCM with the vector to be rotated: v_body = DCM * v_NED.

  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& angles       - Vector of euler angles to describe the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Matrix3f dcm - Direction cosine matrix (rotation matix)
*/
Matrix3f angle2dcm(const Vector3f& angles, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    float roll;
    float pitch;
    float yaw;

    if (angle_unit == DEGREES)
    {
        roll  = deg2rad(angles(0));
        pitch = deg2rad(angles(1));
        yaw   = deg2rad(angles(2));
    }
    else
    {
        roll  = angles(0);
        pitch = angles(1);
        yaw   = angles(2);
    }

    Matrix3f R1(3, 3);
    Matrix3f R2(3, 3);
    Matrix3f R3(3, 3);

    R1 << 1,          0,         0,
          0,  cos(roll), sin(roll),
          0, -sin(roll), cos(roll);

    R2 << cos(pitch), 0, -sin(pitch),
                   0, 1,           0,
          sin(pitch), 0,  cos(pitch);

    R3 << cos(yaw), sin(yaw), 0,
         -sin(yaw), cos(yaw), 0,
                 0,        0, 1;

    Matrix3f dcm(3, 3);

    // Note that multiplication of the matrices are opposite to the
    // rotation sequence due to how matrix multiplication works
    if (rotation_sequence == 321)
        dcm = R1 * R2 * R3;
    else if (rotation_sequence == 312)
        dcm = R2 * R1 * R3;
    else if (rotation_sequence == 231)
        dcm = R1 * R3 * R2;
    else if (rotation_sequence == 213)
        dcm = R3 * R1 * R2;
    else if (rotation_sequence == 132)
        dcm = R2 * R3 * R1;
    else if (rotation_sequence == 123)
        dcm = R3 * R2 * R1;
    else
        dcm = R1 * R2 * R3;

    if (!NED_to_body)
        return dcm.transpose();

    return dcm;
}

/*
  Description:
  ------------
  This function converts a Direction Cosine Matrix (DCM) into the
  corresponding euler angles.

  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Matrix3f& dcm          - Direction cosine matrix (rotation matix)
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f angles - Vector of euler angles to describe the rotation
*/
Vector3f dcm2angle(const Matrix3f& dcm, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    Vector3f angles;

    if (rotation_sequence == 321)
    {
        if (NED_to_body)
        {
            angles << atan2(dcm(1, 2), dcm(2, 2)), // Roll
                     -asin(dcm(0, 2)),             // Pitch
                      atan2(dcm(0, 1), dcm(0, 0)); // Yaw
        }
        else
        {
            angles << atan2(dcm(2, 1), dcm(2, 2)), // Roll
                     -asin(dcm(2, 0)),             // Pitch
                      atan2(dcm(1, 0), dcm(0, 0)); // Yaw
        }
    }

    if (angle_unit == DEGREES)
    {
        angles(0) = rad2deg(angles(0));
        angles(1) = rad2deg(angles(1));
        angles(2) = rad2deg(angles(2));
    }

    return angles;
}

/*
  Description:
  ------------
  Convert a sequence of euler angles to an equivalent unit quaternion.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& angles       - Vector of euler angles to describe the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Quaternionf quat - Quaternion that describes the rotation
*/
Quaternionf angle2quat(const Vector3f& angles, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    Quaternionf quat(angle2dcm(angles, angle_unit, NED_to_body, rotation_sequence));
    return quat;
}

/*
  Description:
  ------------
  Convert a unit quaternion to the equivalent sequence of euler angles
  about the rotation_sequence axes.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Quaternionf quat       - Quaternion that describes the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f& angles - Vector of euler angles to describe the rotation
*/
Vector3f quat2angle(const Quaternionf& quat, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    return dcm2angle(quat.toRotationMatrix(), angle_unit, NED_to_body, rotation_sequence);
}

/*
  Description:
  ------------
  Convert a single unit quaternion to one DCM.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions

  Arguments:
  ----------
  * const Quaternionf quat       - Quaternion that describes the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Matrix3f& dcm - Rotates vectors from one coordinate frame to the other
*/
Matrix3f quat2dcm(const Quaternionf& quat)
{
    return quat.toRotationMatrix();
}

/*
  Description:
  ------------
  Convert a DCM to a unit quaternion.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions

  Arguments:
  ----------
  * const Matrix3f& dcm          - Direction cosine matrix (rotation matix)
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Quaternionf quat - Quaternion that describes the rotation
*/
Quaternionf dcm2quat(const Matrix3f& dcm)
{
    Quaternionf quat(dcm);
    return quat;
}

/*
  Description:
  ------------
  Convert a Rodrigues rotation vector to a DCM.

  **NOTE: The Rodrigues vector's rotation angle must be in RADIANS.**

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Rotation_matrix

  Arguments:
  ----------
  * const Vector3f& vec - Rodrigues rotation vector

  Returns:
  --------
  * Matrix3f dcm - Direction cosine matrix (rotation matix)
*/
Matrix3f vec2dcm(const Vector3f& vec)
{
    float theta   = vec.norm();
    Vector3f axis = vec / theta;

    AngleAxisf angAxis(theta, axis);
    return angAxis.toRotationMatrix();
}

/*
  Description:
  ------------
  Convert a DCM to a Rodrigues rotation vector.

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Rotation_matrix

  Arguments:
  ----------
  * const Matrix3f& dcm - Direction cosine matrix (rotation matix)

  Returns:
  --------
  * Vector3f vec - Rodrigues rotation vector (rotation angle is in RADIANS)
*/
Vector3f dcm2vec(const Matrix3f& dcm)
{
    AngleAxisf angAxis(dcm);
    return angAxis.angle() * angAxis.axis();
}

/*
  Description:
  ------------
  Convert a Rodrigues rotation vector to a quaternion.

  **NOTE: The Rodrigues vector's rotation angle must be in RADIANS.**

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://eater.net/quaternions

  Arguments:
  ----------
  * const Vector3f& vec - Rodrigues rotation vector

  Returns:
  --------
  * Quaternionf quat - Quaternion that describes the rotation
*/
Quaternionf vec2quat(const Vector3f& vec)
{
    return dcm2quat(vec2dcm(vec));
}

/*
  Description:
  ------------
  Convert a quaternion to a Rodrigues rotation vector.

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://eater.net/quaternions

  Arguments:
  ----------
  * const Quaternionf& quat - Quaternion that describes the rotation

  Returns:
  --------
  * Vector3f vec - Rodrigues rotation vector (rotation angle is in RADIANS)
*/
Vector3f quat2vec(const Quaternionf& quat)
{
    AngleAxisf angAxis(quat);
    return angAxis.angle() * angAxis.axis();
}

/*
  Description:
  ------------
  Convert a Rodrigues rotation vector to a vector of euler angles.

  **NOTE: The Rodrigues vector's rotation angle must be in RADIANS.**

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& vec          - Rodrigues rotation vector
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f angles - Vector of euler angles
*/
Vector3f vec2angle(const Vector3f& vec, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    return dcm2angle(vec2dcm(vec), angle_unit, NED_to_body, rotation_sequence);
}

/*
  Description:
  ------------
  Convert a vector of euler angles to a Rodrigues rotation vector.

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& angles       - Vector of euler angles
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f vec - Rodrigues rotation vector (rotation angle is in RADIANS)
*/
Vector3f angle2vec(const Vector3f& angles, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    return dcm2vec(angle2dcm(angles, angle_unit, NED_to_body, rotation_sequence));
}

/*
  Description:
  ------------
  Calculate Earth's geocentric radius at a given geodetic latitude in
  meters. "The geocentric radius is the distance from the Earth's
  center to a point on the spheroid surface at geodetic latitude".

  https://en.wikipedia.org/wiki/Earth_radius
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * float radius - Earth's geocentric radius in meters at a given latitude in meters
*/
float earthGeoRad(const float& _lat, const bool& angle_unit)
{
    float lat = _lat;

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    float num = pow(a_sqrd * cos(lat), 2) + pow(b_sqrd * sin(lat), 2);
    float den = pow(a * cos(lat), 2) + pow(b * sin(lat), 2);

    return sqrt(num / den);
}

/*
  Description:
  ------------
  Calculate Earth's radius of curvature in the prime vertical (East -
  West) - denoted as N - and meridian (North - South) - denoted as
  M - at a given latitude.

  https://en.wikipedia.org/wiki/Radius_of_curvature
  https://en.wikipedia.org/wiki/Earth_radius
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * Vector2f radii - Earth's radii of curvature { N, M } in meters at a given latitude
*/
Vector2f earthRad(const float& _lat, const bool& angle_unit)
{
    float lat = _lat;

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    float R_N = a / sqrt(1 - (ecc_sqrd * pow(sin(lat), 2)));
    float R_M = (a * (1 - ecc_sqrd)) / pow(1 - (ecc_sqrd * pow(sin(lat), 2)), 1.5);

    Vector2f radii;
    radii << R_N, // Earth's prime-vertical radius of curvature
             R_M; // Earth's meridional radius of curvature

    return radii;
}

/*
  Description:
  ------------
  Calculate azimuthal radius at a given latitude. This is the
  Earth's radius of cuvature at a given latitude in the
  direction of a given asimuth.

  https://en.wikipedia.org/wiki/Radius_of_curvature
  https://en.wikipedia.org/wiki/Earth_radius
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * float radius - Earth's azimuthal radius in meters at a given latitude and azimuth
*/
float earthAzimRad(const float& lat, const float& _azimuth, const bool& angle_unit)
{
    float azimuth = _azimuth;

    if (angle_unit == DEGREES)
        azimuth = deg2rad(azimuth);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float N = eradvec(0);
    float M = eradvec(1);

    return 1 / ((pow(cos(azimuth), 2) / M) + (pow(sin(azimuth), 2) / N));
}

/*
  Description:
  ------------
  Calculate Earth's angular velocity in m/s resolved in the NED frame at a given
  latitude.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * Vector3f e - Earth's angular velocity resolved in the NED frame at a given latitude
*/
Vector3f earthRate(const float& _lat, const bool& angle_unit)
{
    float lat = _lat;

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    Vector3f e;
    e << omega_E * cos(lat),
         0,
        -omega_E * sin(lat);

    return e;
}

/*
  Description:
  ------------
  Calculate the latitude, longitude, and altitude (LLA) angular rates
  given the locally tangent velocity in the NED frame and latitude.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& vned   - Velocity vector in m/s in the NED frame
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees) (this also affects the output units
                             (rad/s or degrees/s)

  Returns:
  --------
  * Vector3f lla_dot - LLA angular rates given the locally tangent velocity
                       in the NED frame and latitude
*/
Vector3f llaRate(const Vector3f& vned, const Vector3f& lla, const bool& angle_unit)
{
    float VN = vned(0);
    float VE = vned(1);
    float VD = vned(2);

    float lat = lla(0);
    float alt = lla(2);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float Rew = eradvec(0);
    float Rns = eradvec(1);

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    Vector3f lla_dot;

    if (angle_unit == RADIANS)
    {
        lla_dot << VN / (Rns + alt),
                   VE / (Rew + alt) / cos(lat),
                  -VD;
    }
    else
    {
        lla_dot << rad2deg(VN / (Rns + alt)),
                   rad2deg(VE / (Rew + alt) / cos(lat)),
                  -VD;
    }

    return lla_dot;
}

/*
  Description:
  ------------
  Calculate the navigation/transport angular rates given the locally tangent
  velocity in the NED frame and latitude. The navigation/transport rate is
  the angular velocity of the NED frame relative to the ECEF frame.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& vned   - Velocity vector in m/s in the NED frame
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees) (this also affects the output units
                             (rad/s or degrees/s)

  Returns:
  --------
  * Vector3f rho - Navigation/transport angular rates in the ECEF frame given
                   the locally tangent velocity in the NED frame and latitude
*/
Vector3f navRate(const Vector3f& vned, const Vector3f& lla, const bool& angle_unit)
{
    float VN = vned(0);
    float VE = vned(1);

    float lat = lla(0);
    float alt = lla(2);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float Rew = eradvec(0);
    float Rns = eradvec(1);

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    Vector3f rho;

    if (angle_unit == RADIANS)
    {
        rho << VE / (Rew + alt),
              -VN / (Rns + alt),
              -VE * tan(lat) / (Rew + alt);
    }
    else
    {
        rho << rad2deg(VE / (Rew + alt)),
               rad2deg(-VN / (Rns + alt)),
               rad2deg(-VE * tan(lat) / (Rew + alt));
    }

    return rho;
}

/*
  Description:
  ------------
  Convert a LLA coordinate to an ECEF coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * Vector3f ecef - ECEF coordinate in meters
*/
Vector3f lla2ecef(const Vector3f& lla, const bool& angle_unit)
{
    float lat = lla(0);
    float lon = lla(1);
    float alt = lla(2);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float Rew = eradvec(0);

    if (angle_unit == DEGREES)
    {
        lat = deg2rad(lat);
        lon = deg2rad(lon);
    }

    Vector3f ecef;

    ecef << (Rew + alt) * cos(lat) * cos(lon),
            (Rew + alt)* cos(lat)* sin(lon),
            ((1 - ecc_sqrd) * Rew + alt)* sin(lat);

    return ecef;
}

/*
  Description:
  ------------
  Convert an ECEF coordinate to a LLA coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ecef   - ECEF coordinate in meters
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * Vector3f lla - LLA coordinate (altitude in meters)
*/
Vector3f ecef2lla(const Vector3f& ecef, const bool& angle_unit)
{
    float x = ecef(0);
    float y = ecef(1);
    float z = ecef(2);

    float x_sqrd = pow(x, 2);
    float y_sqrd = pow(y, 2);
    float z_sqrd = pow(z, 2);

    float lon = atan2(y, x);

    float p      = sqrt(x_sqrd + y_sqrd);
    float p_sqrd = pow(p, 2);
    float F      = 54 * b_sqrd * z_sqrd;
    float G      = p_sqrd + ((1 - ecc_sqrd) * z_sqrd) - (ecc_sqrd * (a_sqrd - b_sqrd));
    float G_sqrd = pow(G, 2);
    float c      = (pow(ecc, 4) * F * p_sqrd) / pow(G, 3);
    float c_sqrd = pow(c, 2);
    float s      = pow(1 + c + sqrt(c_sqrd + (2 * c)), 1.5);
    float k      = s + 1 + (1 / s);
    float k_sqrd = pow(k, 2);
    float P      = F / (3 * k_sqrd * G_sqrd);
    float Q      = sqrt(1 + (2 * pow(ecc, 4) * P));
    float r0     = ((-P * ecc_sqrd * p) / (1 + Q)) + sqrt((0.5 * a_sqrd * (1 + (1 / Q))) - ((P * (1 - ecc_sqrd) * z_sqrd) / (Q * (1 + Q))) - (0.5 * P * p_sqrd));
    float U      = sqrt(pow(p - (ecc_sqrd * r0), 2) + z_sqrd);
    float V      = sqrt(pow(p - (ecc_sqrd * r0), 2) + ((1 - ecc_sqrd) * z_sqrd));
    float z0     = (b_sqrd * z) / (a * V);

    float h   = U * (1 - (b_sqrd / (a * V)));
    float lat = atan2(z + (ecc_prime_sqrd * z0), p);

    if (angle_unit == DEGREES)
    {
        lat = rad2deg(lat);
        lon = rad2deg(lon);
    }

    Vector3f lla;
    lla << lat,
           lon,
           h;

    return lla;
}

/*
  Description:
  ------------
  Find the direction cosine matrix that describes the rotation from the ECEF
  coordinate frame to the NED frame given a lat/lon/alt location.

  https://www.mathworks.com/help/aeroblks/directioncosinematrixeceftoned.html
  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * Matrix3f C -
*/
Matrix3f ecef2ned_dcm(const Vector3f& lla, const bool& angle_unit)
{
    float lat = lla(0);
    float lon = lla(1);

    if (angle_unit == DEGREES)
    {
        lat = deg2rad(lat);
        lon = deg2rad(lon);
    }

    Matrix3f C;

    C(0, 0) = -sin(lat) * cos(lon);
    C(0, 1) = -sin(lat) * sin(lon);
    C(0, 2) = cos(lat);

    C(1, 0) = -sin(lon);
    C(1, 1) = cos(lon);
    C(1, 2) = 0;

    C(2, 0) = -cos(lat) * cos(lon);
    C(2, 1) = -cos(lat) * sin(lon);
    C(2, 2) = -sin(lat);

    return C;
}

/*
  Description:
  ------------
  Convert an ECEF coordinate to a NED coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ecef    - ECEF coordinate in meters
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f ned - NED coordinate in meters
*/
Vector3f ecef2ned(const Vector3f& ecef, const Vector3f& lla_ref, const bool& angle_unit)
{
    Vector3f ecef_ref = lla2ecef(lla_ref, angle_unit);
    Matrix3f C        = ecef2ned_dcm(lla_ref, angle_unit);

    return C * (ecef - ecef_ref);
}

/*
  Description:
  ------------
  Convert a LLA coordinate to a NED coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla     - LLA coordinate (altitude in meters)
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f ned - NED coordinate in meters
*/
Vector3f lla2ned(const Vector3f& lla, const Vector3f& lla_ref, const bool& angle_unit)
{
    Vector3f ecef = lla2ecef(lla, angle_unit);
    Vector3f ned  = ecef2ned(ecef, lla_ref, angle_unit);

    return ned;
}

/*
  Description:
  ------------
  Convert a NED coordinate to an ECEF coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned      - NED coordinate in meters
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f ecef - ECEF coordinate in meters
*/
Vector3f ned2ecef(const Vector3f& ned, const Vector3f& lla_ref, const bool& angle_unit)
{
    float lat_ref = lla_ref(0);
    float lon_ref = lla_ref(1);

    if (angle_unit == DEGREES)
    {
        lat_ref = deg2rad(lat_ref);
        lon_ref = deg2rad(lon_ref);
    }

    Matrix3f C(3, 3);

    C(0, 0) = -sin(lat_ref) * cos(lon_ref);
    C(0, 1) = -sin(lat_ref) * sin(lon_ref);
    C(0, 2) =  cos(lat_ref);

    C(1, 0) = -sin(lon_ref);
    C(1, 1) =  cos(lon_ref);
    C(1, 2) =  0;

    C(2, 0) = -cos(lat_ref) * cos(lon_ref);
    C(2, 1) = -cos(lat_ref) * sin(lon_ref);
    C(2, 2) = -sin(lat_ref);

    Vector3f ecef;
    ecef = C.transpose() * ned;

    return ecef;
}

/*
  Description:
  ------------
  Convert a NED coordinate to a LLA coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned     - NED coordinate in meters
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f lla - LLA coordinate (altitude in meters)
*/
Vector3f ned2lla(const Vector3f& ned, const Vector3f& lla_ref, const bool& angle_unit)
{
    Vector3f ecef     = ned2ecef(ned, lla_ref, angle_unit);
    Vector3f ecef_ref = lla2ecef(lla_ref, angle_unit);
    ecef += ecef_ref;

    Vector3f lla = ecef2lla(ecef, angle_unit);

    return lla;
}

/*
  Description:
  ------------
  Create a 4x4 pose matrix that can be used to apply an affine transform of a
  3 dimensional vector/point from one coordinate frame to another. The two
  coordinate frames do not need to be colocated.

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix3f& dcm - Direction cosine matrix that describes the rotation
                          between the two coordinate frames
  * const Vector3f& t   - Translation vector between the origins of the two
                          coordinate frames (unit of distance is arbitrary -
                          up to the user to decide)

  Returns:
  --------
  * Matrix4f poseMatrix - 4x4 pose matrix for affine coordinate frame transforms
*/
Matrix4f poseMat(const Matrix3f& dcm, const Vector3f& t)
{
    Matrix4f poseMatrix;
    poseMatrix << 0, 0, 0, 0,
                  0, 0, 0, 0,
                  0, 0, 0, 0,
                  0, 0, 0, 0;
    poseMatrix(seq(0, 2), seq(0, 2)) << dcm;
    poseMatrix(seq(0, 2), 3) << t;
    poseMatrix(3, 3) = 1;

    return poseMatrix;
}

/*
  Description:
  ------------
  Extract the DCM from a given pose matrix.

  https://en.wikipedia.org/wiki/Affine_transformation
  https://en.wikipedia.org/wiki/Rotation_matrix

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms

  Returns:
  --------
  * Matrix3f dcm - DCM extracted from pose matrix
*/
Matrix3f pose2dcm(const Matrix4f& poseMatrix)
{
    Matrix3f dcm;
    dcm << poseMatrix(seq(0, 2), seq(0, 2));

    return dcm;
}

/*
  Description:
  ------------
  Extract translation vector from a given pose matrix.

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms

  Returns:
  --------
  * Vector3f t - Translation vector extracted from pose matrix
*/
Vector3f pose2t(const Matrix4f& poseMatrix)
{
    Vector3f t;
    t << poseMatrix(seq(0, 2), 3);

    return t;
}

/*
  Description:
  ------------
  Create a reversed 4x4 pose matrix.

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms

  Returns:
  --------
  * Matrix4f poseMatrix - Reversed 4x4 pose matrix for affine coordinate frame
                          transforms
*/
Matrix4f reversePoseMat(const Matrix4f& poseMatrix)
{
    Matrix3f dcm = pose2dcm(poseMatrix);
    Vector3f t   = pose2t(poseMatrix);

    return poseMat(dcm.transpose(), dcm.transpose() * -t);
}

/*
  Description:
  ------------
  Compute the matrix derivative of the given pose matrix w.r.t the given variable.
  The possible variables include:
  * dRx - w.r.t. rotation around the x axis
  * dRy - w.r.t. rotation around the y axis
  * dRz - w.r.t. rotation around the z axis
  * dtx - w.r.t. translation around the x axis
  * dty - w.r.t. translation around the y axis
  * dtz - w.r.t. translation around the z axis

  This operation is often used when constructing Jacobian matrices for optimization
  problems (i.e. least squares/GN optimization)

  https://en.wikipedia.org/wiki/Affine_transformation
  https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant
  https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms
  * const int& derivType       - Variable to take the derivative w.r.t.

  Returns:
  --------
  * Matrix4f poseMatrix - Pose matrix derivative
*/
Matrix4f poseMatDeriv(const Matrix4f& poseMatrix, const int& derivType)
{
    Matrix4f derivPose;
    Matrix3f dcm = pose2dcm(poseMatrix);
    Vector3f vec;

    switch(derivType)
    {
        case dRx:
        {
            vec << 1, 0, 0;

            derivPose(seq(0, 2), seq(0, 2)) << skew(vec) * dcm;
            break;
        }

        case dRy:
        {
            vec << 0, 1, 0;

            derivPose(seq(0, 2), seq(0, 2)) << skew(vec) * dcm;
            break;
        }

        case dRz:
        {
            vec << 0, 0, 1;

            derivPose(seq(0, 2), seq(0, 2)) << skew(vec) * dcm;
            break;
        }

        case dtx:
        {
            vec << 1, 0, 0;

            Matrix4f tempPose;
            tempPose(3, seq(0, 2)) << vec;

            derivPose << tempPose * poseMatrix;
            break;
        }

        case dty:
        {
            vec << 0, 1, 0;

            Matrix4f tempPose;
            tempPose(3, seq(0, 2)) << vec;

            derivPose << tempPose * poseMatrix;
            break;
        }

        case dtz:
        {
            vec << 0, 0, 1;

            Matrix4f tempPose;
            tempPose(3, seq(0, 2)) << vec;

            derivPose << tempPose * poseMatrix;
            break;
        }
    }

    return derivPose;
}

/*
  Description:
  ------------
  Applies an affine transformation to vector/point x described by the given DCM and
  translation vector. This affine transformation converts the vector/point x from
  it's initial coordinate frame to another. A common use of this function would be
  to convert a 3D point in an airplane's sensor's body frame into the airplane's
  body frame (and vice versa).

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix3f& dcm - Direction cosine matrix that describes the rotation
                          between the two coordinate frames
  * const Vector3f& t   - Translation vector between the origins of the two
                          coordinate frames (unit of distance is arbitrary -
                          up to the user to decide)
  * const Vector3f& x   - The vector/point to be transformed

  Returns:
  --------
  * Vector3f new_x - The vector/point transformed to the new coordinate frame
*/
Vector3f transformPt(const Matrix3f& dcm, const Vector3f& t, const Vector3f& x)
{
    Vector4f x_1;
    x_1 << x;
    x_1(3) = 1;

    Vector4f new_x_1 = poseMat(dcm, t) * x_1;
    Vector3f new_x   = new_x_1(seq(0, 2));

    return new_x;
}

/*
  Description:
  ------------
  Applies an affine transformation to vector/point x described by the given pose
  matrix. This affine transformation converts the vector/point x from it's
  initial coordinate frame to another. A common use of this function would be
  to convert a 3D point in an airplane's sensor's body frame into the airplane's
  body frame (and vice versa).

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms
  * const Vector3f& x          - The vector/point to be transformed

  Returns:
  --------
  * Vector3f new_x - The vector/point transformed to the new coordinate frame
*/
Vector3f transformPt(const Matrix4f& poseMatrix, const Vector3f& x)
{
    Vector4f x_1;
    x_1 << x;
    x_1(3) = 1;

    Vector4f new_x_1 = poseMatrix * x_1;
    Vector3f new_x = new_x_1(seq(0, 2));

    return new_x;
}

/*
  Description:
  ------------
  Make a skew symmetric matrix from a 3 element vector. Skew
  symmetric matrices are often used to easily take cross products.

  https://en.wikipedia.org/wiki/Skew-symmetric_matrix

  Arguments:
  ----------
  * const Vector3f& w - 3 element vector

  Returns:
  --------
  * Matrix3f C - Skew symmetric matrix of vector w
*/
Matrix3f skew(const Vector3f& w)
{
    Matrix3f C;

    C << 0.0, -w(2),  w(1),
        w(2),   0.0, -w(0),
       -w(1),  w(0),   0.0;

    return C;
}

/*
  Description:
  ------------
  Calculate the bearing between two LLA coordinates.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla_1  - First LLA coordinate (altitude in meters)
  * const Vector3f& lla_2  - Second LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude, longitude, and bearing
                             angles (rad or degrees)

  Returns:
  --------
  * float bearing - The bearing between the two given LLA coordinates
*/
float bearingLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit)
{
    float lat_1 = lla_1(0);
    float lon_1 = lla_1(1);

    float lat_2 = lla_2(0);
    float lon_2 = lla_2(1);

    if (angle_unit == DEGREES)
    {
        lat_1 = deg2rad(lat_1);
        lon_1 = deg2rad(lon_1);

        lat_2 = deg2rad(lat_2);
        lon_2 = deg2rad(lon_2);
    }

    float deltaLon = lon_2 - lon_1;

    float x = cos(lat_2) * sin(deltaLon);
    float y = cos(lat_1) * sin(lat_2) - sin(lat_1) * cos(lat_2) * cos(deltaLon);

    return fmod(rad2deg(atan2(x, y)) + 360, 360);
}

/*
  Description:
  ------------
  Calculate the bearing between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1  - First NED coordinate in meters
  * const Vector3f& ned_2  - Second NED coordinate in meters
  * const bool& angle_unit - Unit of the latitude, longitude, and bearing
                             angles (rad or degrees)

  Returns:
  --------
  * float bearing - The bearing between the two given LLA coordinates
*/
float bearingNed(const Vector3f& ned_1, const Vector3f& ned_2)
{
    float n_1 = ned_1(0);
    float e_1 = ned_1(1);

    float n_2 = ned_2(0);
    float e_2 = ned_2(1);

    float x = e_2 - e_1;
    float y = n_2 - n_1;

    return fmod(rad2deg(atan2(x, y)) + 360, 360);
}

/*
  Description:
  ------------
  Calculate the distance between two LLA coordinates.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla_1  - First LLA coordinate (altitude in meters)
  * const Vector3f& lla_2  - Second LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * float dist - The distance between the two given LLA coordinates in meters
*/
double distanceLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit)
{
    float lat_1 = lla_1(0);
    float lon_1 = lla_1(1);

    float lat_2 = lla_2(0);
    float lon_2 = lla_2(1);

    if (angle_unit == DEGREES)
    {
        lat_1 = deg2rad(lat_1);
        lon_1 = deg2rad(lon_1);

        lat_2 = deg2rad(lat_2);
        lon_2 = deg2rad(lon_2);
    }

    double deltaLat = lat_2 - lat_1;
    double deltaLon = lon_2 - lon_1;

    double _a = (sin(deltaLat / 2) * sin(deltaLat / 2)) + cos(lat_1) * cos(lat_2) * (sin(deltaLon / 2)) * (sin(deltaLon / 2));

    float azimuth = bearingLla(lla_1, lla_2, angle_unit);
    float radius  = earthAzimRad(lla_1(0), azimuth, angle_unit);

    return 2 * radius * atan2(sqrt(_a), sqrt(1 - _a));
}

/*
  Description:
  ------------
  Calculate the total distance between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1 - First NED coordinate in meters
  * const Vector3f& ned_2 - Second NED coordinate in meters

  Returns:
  --------
  * float dist - The total distance between the two given NED coordinates
*/
float distanceNed(const Vector3f& ned_1, const Vector3f& ned_2)
{
    Vector3f out = ned_2 - ned_1;
    return out.norm();
}

/*
  Description:
  ------------
  Calculate the horizontal distance between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1 - First NED coordinate in meters
  * const Vector3f& ned_2 - Second NED coordinate in meters

  Returns:
  --------
  * float dist - The horizontal distance between the two given NED coordinates
*/
float distanceNedHoriz(const Vector3f& ned_1, const Vector3f& ned_2)
{
    Vector2f out;

    out << ned_2(0) - ned_1(0),
           ned_2(1) - ned_1(1);

    return out.norm();
}

/*
  Description:
  ------------
  Calculate the vertical distance between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1 - First NED coordinate in meters
  * const Vector3f& ned_2 - Second NED coordinate in meters

  Returns:
  --------
  * float dist - The vertical distance between the two given NED coordinates
*/
float distanceNedVert(const Vector3f& ned_1, const Vector3f& ned_2)
{
    return ned_2(2) - ned_1(2);
}

/*
  Description:
  ------------
  Calculate the total distance between two ECEF coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ecef_1 - First ECEF coordinate in meters
  * const Vector3f& ecef_2 - Second ECEF coordinate in meters

  Returns:
  --------
  * float dist - The total distance between the two given ECEF coordinates
*/
float distanceEcef(const Vector3f& ecef_1, const Vector3f& ecef_2)
{
    Vector3f out = ecef_2 - ecef_1;
    return out.norm();
}

/*
  Description:
  ------------
  Calculate the elevation angle between two LLA coordinates.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla_1  - First LLA coordinate (altitude in meters)
  * const Vector3f& lla_2  - Second LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude, longitude, and elevation
                             angles (rad or degrees)

  Returns:
  --------
  * float elevation - The elevation angle between the two given LLA coordinates
*/
float elevationLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit)
{
    float lat_1 = lla_1(0);
    float lon_1 = lla_1(1);
    float alt_1 = lla_1(2);

    float lat_2 = lla_2(0);
    float lon_2 = lla_2(1);
    float alt_2 = lla_2(2);

    if (angle_unit == DEGREES)
    {
        lat_1 = deg2rad(lat_1);
        lon_1 = deg2rad(lon_1);

        lat_2 = deg2rad(lat_2);
        lon_2 = deg2rad(lon_2);
    }

    float dist   = distanceLla(lla_1, lla_2, angle_unit);
    float height = alt_2 - alt_1;

    return rad2deg(atan2(height, dist));
}

/*
  Description:
  ------------
  Calculate the elevation angle between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1  - First NED coordinate (altitude in meters)
  * const Vector3f& ned_2  - Second NED coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude, longitude, and elevation
                             angles (rad or degrees)

  Returns:
  --------
  * float elevation - The elevation angle between the two given NED coordinates
*/
float elevationNed(const Vector3f& ned_1, const Vector3f& ned_2)
{
    float d_1 = -ned_1(2);
    float d_2 = -ned_2(2);

    float dist   = distanceNed(ned_1, ned_2);
    float height = d_2 - d_1;

    return rad2deg(atan2(height, dist));
}

/*
  Description:
  ------------
  Calculate a LLA coordinate based on a given LLA coordinate, distance,
  azimuth, and elevation angle.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla     - LLA coordinate (altitude in meters)
  * const float& dist       - "As the crow flies" distance between the two
                              LLA coordinates in meters
  * const float& _azimuth   - Azimuth angle between the two LLA coordinates
  * const float& _elevation - Elevation angle between the two LLA coordinates
  * const bool& angle_unit  - Unit of the latitude, longitude, azimuth, and
                              elevation angles (rad or degrees)

  Returns:
  --------
  * Vector3f& out - New LLA coordinate (altitude in meters)
*/
Vector3f LDAE2lla(const Vector3f& lla, const float& dist, const float& _azimuth, const float& _elevation, const bool& angle_unit)
{
    float lat = lla(0);
    float lon = lla(1);
    float alt = lla(2);

    float azimuth   = _azimuth;
    float elevation = _elevation;

    if (angle_unit == DEGREES)
    {
        lat = deg2rad(lat);
        lon = deg2rad(lon);

        azimuth   = deg2rad(azimuth);
        elevation = deg2rad(elevation);
    }

    float radius   = earthAzimRad(lla(0), _azimuth, angle_unit);
    float adj_dist = dist / radius;

    float lat_2 = asin(sin(lat) * cos(adj_dist) + cos(lat) * sin(adj_dist) * cos(azimuth));
    float lon_2 = lon + atan2(sin(azimuth) * sin(adj_dist) * cos(lat), cos(adj_dist) - sin(lat) * sin(lat_2));

    Vector3f out;

    out << rad2deg(lat_2),
           rad2deg(lon_2),
           alt + (dist * tan(elevation));

    return out;
}

/*
  Description:
  ------------
  Calculate a NED coordinate based on a given NED coordinate, distance,
  azimuth, and elevation angle.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned     - NED coordinate in meters
  * const float& dist       - Horizontal distance between the two
                              NED coordinates in meters
  * const float& _azimuth   - Azimuth angle between the two NED coordinates
  * const float& _elevation - Elevation angle between the two NED coordinates
  * const bool& angle_unit  - Unit of the azimuth and elevation angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f& out - New NED coordinate in meters
*/
Vector3f NDAE2ned(const Vector3f& ned, const float& dist, const float& _azimuth, const float& _elevation, const bool& angle_unit)
{
    float n = ned(0);
    float e = ned(1);
    float d = ned(2);

    float azimuth   = _azimuth;
    float elevation = _elevation;

    if (angle_unit == DEGREES)
    {
        azimuth   = deg2rad(azimuth);
        elevation = deg2rad(elevation);
    }

    Vector3f out;

    out << n + dist * cos(azimuth),
           e + dist * sin(azimuth),
           d + (dist * tan(elevation));

    return out;
}


#include <iostream>

#include <Eigen/Dense>
#include <Eigen/QR>

/* @brief QR decomposition from Cholesky decomposition
 * @param[in] A An $m \times n$ matrix
 * @param[out] R The upper triangular matrix from the QR decomposition of $A$
 * @param[out] Q The orthogonal matrix from the QR decomposition of $A$
 */
void CholeskyQR(const Eigen::MatrixXd &A, Eigen::MatrixXd &R, Eigen::MatrixXd &Q) {
	Eigen::MatrixXd L = (A.transpose() * A).llt().matrixL();
	Q = L.triangularView<Eigen::Lower>().solve(A.transpose()).transpose();
	R = L.transpose();
}

/* @brief Direct QR decomposition
 * @param[in] A An $m \times n$ matrix
 * @param[out] R The upper triangular matrix from the QR decomposition of $A$
 * @param[out] Q The orthogonal matrix from the QR decomposition of $A$
 */
void DirectQR(const Eigen::MatrixXd &A, Eigen::MatrixXd &R, Eigen::MatrixXd &Q) {

	int m = A.rows();
	int n = A.cols();

	Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
	Q = qr.householderQ() * Eigen::MatrixXd::Identity(m, n);
	R = (Q.transpose() * A);
}

int main() {
	int m = 3;
	int n = 2;

	Eigen::MatrixXd A(m,n);
	double epsilon = 1.e-8;
	//A << 3, 5, 1, 9, 7, 1;
	A << 1, 1, epsilon, 0, 0, epsilon;
	std::cout << "A =" << std::endl << A << std::endl;

	Eigen::MatrixXd R, Q;

	CholeskyQR(A, R, Q);
	std::cout << "CholeskyQR: ===========" << std::endl;
	std::cout << "R =" << std::endl << R << std::endl;
	std::cout << "Q =" << std::endl << Q << std::endl;

	DirectQR(A, R, Q);
	std::cout << "DirectQR: =============" << std::endl;
	std::cout << "R =" << std::endl << R << std::endl;
	std::cout << "Q =" << std::endl << Q << std::endl;

	return 0;
}