#include "triangle_area_normal.h"
#include <Eigen/Geometry>

Eigen::RowVector3d triangle_area_normal(
  const Eigen::RowVector3d & a,
  const Eigen::RowVector3d & b,
  const Eigen::RowVector3d & c)
{
  ////////////////////////////////////////////////////////////////////////////
  Eigen::RowVector3d ab = b - a;
  Eigen::RowVector3d ac = c - a;

  // Compute the normal of the triangle
  Eigen::RowVector3d n = ab.cross(ac);

  // Compute the area of the triangle
  double area = (1.0 / 2.0) * n.norm();

  return area * n.normalized();
  ////////////////////////////////////////////////////////////////////////////
}


//
//  main.cpp
//  Forward Kinematics
//
//  Created by Aziel Hutajulu on 01/03/22.
//

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

using namespace std;
using namespace Eigen;

int main() {
    //class for coordinate frame from 1 to i, coordinate frame 0 known as base coordinate
    class CoordFrame {
        public:
        //declaration of DH Convention
        double theta; //align x(i-1) with x(i), in degree
        double d; //align z(i-1) with z(i), in inch
        double a; //align O(i-1) with O(i), in inch
        double alpha; //rotation on x-axis to make z(i) paralel with z(i-1), in degree
        Matrix4d Ttheta, Td, Ta, Talpha, TM; // TM for Transformation Matrix

        //input for each coordinate frame
        CoordFrame (double x, double y, double z, double v){
            theta = x;
            d = y;
            a = z;
            alpha = v;
        }

        //function below to get transformation matriks for CF(i-1) to CF(i)
        Matrix4d getTransMatrix(){
            Ttheta <<   cos(theta), -sin(theta), 0, 0,
                        sin(theta), cos(theta), 0, 0,
                        0, 0, 1, 0,
                        0, 0, 0, 1;
            Td <<   1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, d,
                    0, 0, 0, 1;
            Ta <<   1, 0, 0, a,
                    0, 1, 0, 0,
                    0, 0, 1, 0,
                    0, 0, 0, 1;
            Talpha <<   1, 0, 0, 0,
                        0, cos(alpha), -sin(alpha), 0,
                        0, sin(alpha), cos(alpha), 0,
                        0, 0, 0, 1;
            TM = Ttheta * Td * Ta * Talpha;
            return TM;
        }
    };

    //example declaratino of coordinate frame from Puma 260 Arm
    CoordFrame
        CF1(0, 13, 0, -90),
        CF2(0, -3, 8, 0),
        CF3(90, 0, 0, 90),
        CF4(0, 8, 0, -90),
        CF5(-90, 0, 0, 90),
        CF6(0, 5, 0, 0); //End Effector

    //example declaration for P0
    //P6 is the end effector
    Vector4d P0, P6;
    P0 <<   1, 1, 1, 1;
    Matrix4d T06; //P0 = T06 * P6

    double Theta1, Theta2, Theta3, Theta4, Theta5, Theta6; // declaration for input theta

    cout << "Angle of Rotation (degree) on:" << endl << "Joint 1 = ";
    cin >> Theta1;
    CF1.theta += Theta1;

    cout << "Joint 2 = ";
    cin >> Theta2;
    CF2.theta += Theta2;

    cout << "Joint 3 = ";
    cin >> Theta3;
    CF3.theta += Theta3;

    cout << "Joint 4 = ";
    cin >> Theta4;
    CF4.theta += Theta4;

    cout << "Joint 5 = ";
    cin >> Theta5;
    CF5.theta += Theta5;

    cout << "Joint 6 = ";
    cin >> Theta6;
    CF6.theta += Theta6;
    cout << endl;

    //count the transformation matrix of CF(i-1) to CF(i)
    CF1.getTransMatrix();
    CF2.getTransMatrix();
    CF3.getTransMatrix();
    CF4.getTransMatrix();
    CF5.getTransMatrix();
    CF6.getTransMatrix();
    T06 = ((((CF1.TM * CF2.TM) * CF3.TM) * CF4.TM) * CF5.TM) * CF6.TM;

    //print the transformation matrix from base origin to end effector
    cout << "Transformation Matrix from Base to End Effector:" << endl << T06 << endl;

    //print end effector frame rotation matrix w.r.t. Base origin
    Matrix3d RotationMatrix;
    RotationMatrix <<   T06(0,0), T06(0,1), T06(0,2),
                        T06(1,0), T06(1,1), T06(1,2),
                        T06(2,0), T06(2,1), T06(2,2);
    cout << endl << "End Effector frame Rotation Matrix w.r.t Base frame:" << endl << RotationMatrix << endl;

    //print the position of end effector
    P6 = T06 * P0;
    cout << endl << "Position of End Effector from Base Origin in inches:" << endl << P6 << endl;
}


/**
 * @file Solution1DHO.cpp
 *
 * Calcul de l'équation schrödinger d'oscillateur harmonique à une dimension
 */
#include <iostream>
#include <armadillo>
#include <cmath>
#include <fstream>
#include <stdlib.h>
#include "HermitePolynomials.h"
#include "Solution1DHO.h"
#include "Utils.h"

/** Constructeur
 *
 * valeur m,w vaut 1 par défaut
 */

Solution1DHO::Solution1DHO()
{
    m=1;
    w=1;
    h=1;
}

/** Constructeur avec des valeurs données
 *
 * @param masse
 * @param pulsation
 */

Solution1DHO::Solution1DHO(double masse, double pulsation)
{
    m=masse;
    w=pulsation;
}

/** Initialisation des constantes
 *
 * Cette fonction permet d'initialiser les constantes
 *
 * @param m masse
 * @param w pulsation de l'onde
 *
 * @return rien
 */
void Solution1DHO::setConstData(double m, double w)
{
    this->w = w;
    this->m = m;
}

/** Calcul de la fonction d'onde Psi
*
* Cette fonction a pour but de calculer la fonction Psi solution de l'équation de Schrödinger 1D-HO.
*
* @param n
* @param Z
*
* @return le résultat de la solution de 1D-HO Pn(z)
*/
arma::mat Solution1DHO::solution(int n, arma::mat Z)
{
    double f1;
    double f2;
    arma::mat f3;
    arma::mat res;

    f1 = (1/std::sqrt(std::exp(n * std::log(2)) * Utils::fac(n)));
    f2 = std::sqrt(std::sqrt(m * w / M_PI / h));
    f3 =  arma::exp(-(m * w * (Z % Z) / 2 / h));

    res = f1 * f2 * (f3 % HermitePolynomials::calculateHermite(n, std::sqrt(m * w / h) * Z));

    return res;
}

/** Stockage des valeurs de la fonction d'onde
*
* Cette fonction a pour but de stocker les valeurs de la fonction d'onde dans un fichier plot.txt
*
* @param n
* @param Z
*
* @return rien
*/
void Solution1DHO::solutionToFile(int n, arma::mat Z)
{
    arma::mat res = Solution1DHO::solution(n,Z);

    std::ofstream out("plot.txt");
    if (out.is_open())
    {
        out << Z;
        out << res << std::endl;
        out.close();
    }
    std::cout << "La solution de l'équation est stockée dans plot.txt" << std::endl;

}

/** Tracer le graphe de la fonction d'onde Psi
*
* Cette fonction a pour but de tracer le graph de la fonction d'onde Psi
* avec python par appel système.
*
* @return rien
*/
void Solution1DHO::drawSolution()
{
    std::cout << "Tracer du graphe avec les données de plot.txt! " << std::endl;
    system("python plot.py");
}

/**
 * Calcul de l'énergie
 *
 * Cette fonction a pout but de calculer l'énergie en connaissant la fonction d'onde pour vérifier qu'elle est constante.
 * # Equation de Schrödinger
 * \f[\left(\frac{\hat{p}_{(z)}^2}{2m} + \frac{1}{2}m\omega^2\hat{z}^2\right)\psi_n = E_n\psi_n.\f]
 *
 * @param n mode de l'énergie
 * @param a limite supérieur de Z
 * @param b limite inférieur de Z
 * @param N nombre de points z à caculer
 *
 *
 *
 * @retval E un vecteur contenant les valeurs des énergies pour N points.
 */
arma::mat Solution1DHO::energy(int n, double a, double b, int N)
{
    double pas = (b-a)/N;
    arma::mat Z = arma::linspace(a,b+2*pas,N+2);
    arma::mat Psi = Solution1DHO::solution(n,Z);
    arma::mat dPsi = Utils::derivative(Psi.rows(0,N), Psi.rows(1,N+1),Z.rows(0,N), Z.rows(1,N+1));
    arma::mat d2Psi = Utils::derivative(dPsi.rows(0,N-1), dPsi.rows(1,N), Z.rows(0,N-1), Z.rows(1,N));

    arma::mat ZN = Z.rows(0,N-1);
    arma::mat PsiN = Psi.rows(0,N-1);
    arma::mat E;
    E = -1 * h * h * d2Psi / (2 * m * PsiN) + m * w * w * ZN % ZN / 2;
    return E;
}


/*
######################################################
Introduction to Criptography - 2021 - Homework5
Last modification (12-May-2021)

Author: Gaina Robert-Adrian
Contact: gainarobertadrian@gmail.com

RSA system:
1. RSA encryption+decryption
2. RSA CRT decryption
3. Wiener attack
######################################################

Resources:
1. https://profs.info.uaic.ro/~tr/tr03-02.pdf
2. https://profs.info.uaic.ro/~siftene/Wiener.pdf
3. https://libntl.org/ (Big number library)
4. http://cacr.uwaterloo.ca/hac/about/chap8.pdf
*/
#include <NTL/ZZ.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <utility>
#include <windows.h>
#include <algorithm>
#include <chrono>
using namespace std;
using namespace std::chrono;
using namespace NTL;

ZZ encrypted_number;

class RSA {
private:
	ZZ p, q, phi, d;

	string numberToText(ZZ number) {
		ZZ copy = number;
		string ans = "";
		while (copy > 0) {
			char chr = 0;
			int aux = 1;
			for (int i = 0; i < 8; ++i) {
				chr += (copy % 2) * aux;
				copy /= 2;
				aux *= 2;
			}
			ans.push_back(chr);
		}
		reverse(ans.begin(), ans.end());
		return ans;
	}

	ZZ textToNumber(string text) {
		ZZ number(0);
		/*Transform the string into a number*/
		for (int i = 0; i < text.length(); ++i) {
			for (int j = 7; j >= 0; --j) {
				if ((text[i] >> j) & 1) {
					number *= 2;
					++number;
				}
				else {
					number *= 2;
				}
			}
		}
		return number;
	}

	vector<ZZ> continuedFractionExpansion(ZZ n, ZZ e) {
		vector<ZZ> ans;
		ZZ r;
		while (n != 0) {
			ans.push_back(e / n);
			r = e % n;
			e = n;
			n = r;
		}
		return ans;
	}

	vector<pair<ZZ, ZZ>> getConvergents(vector <ZZ> fractionExpansion) {
		vector<pair<ZZ, ZZ>> ans;
		ZZ alpha = fractionExpansion[0];
		ZZ beta(1);

		ans.push_back(make_pair(alpha, beta));

		alpha = fractionExpansion[0] * fractionExpansion[1] + 1;
		beta = fractionExpansion[1];
		ans.push_back(make_pair(alpha, beta));

		for (int i = 2; i < fractionExpansion.size(); ++i) {
			alpha = fractionExpansion[i] * alpha + ans[i - 2].first;
			beta = fractionExpansion[i] * beta + ans[i - 2].second;
			ans.push_back(make_pair(alpha, beta));
		}
		return ans;
	}

public:
	ZZ e, n;

	ZZ getP() {
		return p;
	}

	ZZ getQ() {
		return q;
	}

	ZZ getD() {
		return d;
	}

	pair<ZZ,ZZ> generateKeys(int length) {
		GenPrime(p, length);
		GenPrime(q, length);

		n = p * q;
		phi = (p - 1) * (q - 1);

		e = 2;
		while (GCD(e, phi) != 1) {
			//GenPrime(e, length - length / 2);
			GenPrime(e, 32);
		}

		InvMod(d, e, phi);

		cout << "p:" << p << "\n\n";
		cout << "q:" << q << "\n\n";
		cout << "n:" << n << "\n\n";
		cout << "phi:" << phi << "\n\n";
		cout << "e:" << e << "\n\n";
		cout << "d:" << d << "\n\n";

		return make_pair(e, n);
	}

	string encrypt(string text,pair<ZZ,ZZ> public_key) {
		ZZ number = textToNumber(text);

		cout <<"[encrypt]Converted number:"<< number << "\n";
		cout << "[encrypt]PublicKey<e,n>: <" << public_key.first << "," << public_key.second << ">\n";

		ZZ result = PowerMod(number, public_key.first, public_key.second);
		encrypted_number = result;

		cout <<"[encrypt]Encrypted number: "<< result<< "\n";
		string cypertext = numberToText(result);
		cout << "[encrypt]Encrypted text: " << cypertext << "\n\n";

		return cypertext;
	}

	string decrypt(string encrypted) {
		auto start = high_resolution_clock::now();
		ZZ decrypted = PowerMod(textToNumber(encrypted), d, n);
		auto stop = high_resolution_clock::now();
		string plaintext = numberToText(decrypted);
		cout << "[decrypt]Decrypted number: " << decrypted << "\n";
		cout << "[decrypt]Decrypted text: " << plaintext << "\n";
		auto duration = duration_cast<microseconds>(stop - start);

		cout << "[decrypt]Time taken by standard decryption: "
			<< duration.count() << " microseconds" << "\n\n";
		return plaintext;
	}

	string decryptCRT(ZZ n, ZZ p, ZZ q, ZZ d, ZZ number) {
		if (p > q) {
			swap(p, q);
		}
		cout << "[decryptCRT]Number to be decrypted:" << number << '\n';
		auto start = high_resolution_clock::now();
		ZZ n1 = d % (p - 1);
		ZZ n2 = d % (q - 1);
		ZZ inv = InvMod(p, q);

		ZZ x1 = PowerMod(number % p, n1, p);
		ZZ x2 = PowerMod(number % q, n2, q);
		ZZ mul = (x2 - x1) * (inv % q) % q;
		ZZ x = x1 + p * mul;
		auto stop = high_resolution_clock::now();
		string ans = numberToText(x);
		auto duration = duration_cast<microseconds>(stop - start);

		cout << "[decryptCRT]Decrypted number:" << x << '\n';
		cout << "[decryptCRT]Decrypted text:" << ans << "\n";
		cout << "[decryptCRT]Time taken by CRT decryption: "
			<< duration.count() << " microseconds" << "\n\n";
		return ans;
	}

	pair<ZZ, ZZ> generateWienerKeys() {
		ZZ p, q;
		while (true) {
			GenPrime(p, 512);
			GenPrime(q, 512);
			if (p < q) {
				swap(p, q);
			}
			if (q < p && p < 2 * q) {
				break;
			}
			cout << "I" << '\n';
		}
		ZZ n = p * q;
		ZZ phi = (p - 1) * (q - 1);

		ZZ d;
		while (true) {
			GenPrime(d, 32);
			if (d < SqrRoot(SqrRoot(n)) / 3)
				break;
		}
		ZZ e;
		InvMod(e, d, phi);

		cout << "P:" << p << "\n\n";
		cout << "Q:" << q << "\n\n";
		cout << "N:" << n << "\n\n";
		cout << "Phi:" << phi << "\n\n";
		cout << "E:" << e << "\n\n";
		cout << "D:" << d << "\n\n";

		return make_pair(e, n);

	}

	void wienerAttack(ZZ n, ZZ e) {
		vector<ZZ> fractionExpansion = continuedFractionExpansion(n, e);
		vector<pair<ZZ, ZZ>> convergents = getConvergents(fractionExpansion);

		ZZ d;

		for (auto fraction : convergents) {
			if (fraction.first == 0) { // If alpha is 0 alpha*beta can't be n
				continue;
			}

			if ((e * fraction.second - 1) % fraction.first != 0) { //e*d-1=l*phi(n) === (e*d)-1 mod l = 0
				continue;
			}
			ZZ phi = (e * fraction.second - 1) / fraction.first; //(e*d-1)/l=phi(n)
			/* Equation of rank 2 is: y^2 - (n+1-phi(n)*y - n = 0 */
			ZZ a(1);
			ZZ b = (n + 1 - phi) * -1;
			ZZ c = n;

			ZZ delta = b * b - 4 * a * c;
			if (delta <= 0) {
				continue;
			}

			ZZ sqrtDelta;
			SqrRoot(sqrtDelta, delta);
			if (sqrtDelta * sqrtDelta != delta) {
				continue;
			}
			b *= -1;
			ZZ x1 = (b + sqrtDelta) / (2 * a);
			ZZ x2 = (b - sqrtDelta) / (2 * a);
			if (x1 * x2 == n) {
				cout <<"D="<< fraction.second<< " found\n";;
				InvMod(d, e, phi);
				cout << "Real D=" << d << "\n";
				break;
			}
		}
	}
};

int main() {
	RSA instance = RSA();
	cout << "~~~~~~~~~~~~~~~~~~~~~~Key generation start~~~~~~~~~~~~~~~~~~~~~\n";
	pair<ZZ,ZZ> key=instance.generateKeys(512);
	cout << "~~~~~~~~~~~~~~~~~~~~~~Key generation stop~~~~~~~~~~~~~~~~~~~~~\n\n\n";

	string text = "plaintext_example";
	cout << "~~~~~~~~~~~~~~~~~~~~~~Encrypt start~~~~~~~~~~~~~~~~~~~~~\n";
	string cypertext=instance.encrypt(text,key);
	cout << "~~~~~~~~~~~~~~~~~~~~~~Encrypt stop~~~~~~~~~~~~~~~~~~~~~\n\n\n";

	cout << "~~~~~~~~~~~~~~~~~~~~~~Decrypt start~~~~~~~~~~~~~~~~~~~~~\n";
	string plaintext = instance.decrypt(cypertext);
	string plaintext2 = instance.decryptCRT(instance.n, instance.getP(), instance.getQ(), instance.getD(), encrypted_number);
	cout << "~~~~~~~~~~~~~~~~~~~~~~Decrypt stop~~~~~~~~~~~~~~~~~~~~~\n\n\n";
	cout << "C: " << cypertext << "\nP: " << plaintext <<"\nP2: "<<plaintext2<< "\n\n";

	cout << "~~~~~~~~~~~~~~~~~~~~~~Key generation Wiener start~~~~~~~~~~~~~~~~~~~~~\n";
	key=instance.generateWienerKeys();
	cout << "~~~~~~~~~~~~~~~~~~~~~~Key generation Wiener stop~~~~~~~~~~~~~~~~~~~~~\n\n\n";

	cout << "~~~~~~~~~~~~~~~~~~~~~~Wiener attack~~~~~~~~~~~~~~~~~~~~~\n";
	instance.wienerAttack(key.second, key.first);
	cout << "~~~~~~~~~~~~~~~~~~~~~~Wiener attack~~~~~~~~~~~~~~~~~~~~~\n";
	return 0;
}


/*
Number theory-based solution.

Fractions with prime denominators always have repeating decimals. Non-primes
never do. Factors of primes never have higher amounts of repeating decimals than
the primes they are factors of. Thus we can evaluate only all primes from 1 to
1000 for repeating digits.

Calculate the order for `10 modulo p`, where `p` is the prime number in the
expression `1/p`. This is equal to the number of repeating digits.

References:
https://en.wikipedia.org/wiki/Repeating_decimal#Fractions_with_prime_denominators
https://en.wikipedia.org/wiki/Multiplicative_order
*/

#include <iostream>
#include <math.h>
#include "Vector.h"
#include <boost/multiprecision/gmp.hpp>

using namespace std;

int target = 1000;

int main(int argc, char** argv) {
  // Lazy primes generation, since the amount we need is so low.
  Vector<int> primes;
  primes.insertBack(2);
  for (int i = primes[0]; i < target; i++) {
    bool foundFactor = false;
    for (int j = 0; j < primes.length(); j++) {
      cout << i << ", " << j << endl;
      if (i % primes[j] == 0) {
        foundFactor = true;
        break;
      }
    }
    if (!foundFactor) {
      cout << "No factors for " << i << endl;
      primes.insertBack(i);
    }
  }

  int highestPeriodIndex;
  int highestPeriod = 0;

  for (int i = 0; i < primes.length(); i++) {
    int prime = primes[i];
    if (prime == 2 || prime == 5) {
      // Special case: according to number theory, the rule doesn't apply to
      // these two, probably because they're both factors of 10.
      continue;
    }
    int a = 10;
    boost::multiprecision::mpz_int ak = a;
    int k = 1;
    while (ak % prime != 1) {
      ak *= a;
      k++;
    }
    if (k > highestPeriod) {
      highestPeriod = k;
      highestPeriodIndex = i;
    }
  }

  cout << "Number from 1 to " << target << " with highest number of repeating digits: "
       << primes[highestPeriodIndex] << " with period of " << highestPeriod << endl;

  return 0;
}


// gauss_jordan.cpp : example program comparing float vs posit matrix inversion algorithms
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-BLAS project, which is released under an MIT Open Source license.

//#include <chrono>
// Boost arbitrary precision floats
#include <boost/multiprecision/cpp_bin_float.hpp>

// configure the posit number system behavior
#define POSIT_ROUNDING_ERROR_FREE_IO_FORMAT 0
// configure the HPR-BLAS behavior
#define HPRBLAS_TRACE_ROUNDING_EVENTS 0
#include <hprblas>
#include <mtl_extensions.hpp>
// matrix generators
#include <generators/matrix_generators.hpp>
#include <utils/print_utils.hpp>

template<typename Scalar>
void GenerateNumericalAnalysisTestCase(const std::string& header, unsigned N, bool verbose = false) {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;
	using namespace sw::hprblas;

	std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" << header << std::endl;
	using Vector = mtl::vec::dense_vector<Scalar>;
	using Matrix = mtl::mat::dense2D<Scalar>;

	Matrix H(N, N);
	GenerateHilbertMatrix(H, false);
	Matrix Hinv = GaussJordanInversion(H);
	Matrix Href(N, N);
	GenerateHilbertMatrixInverse(Href);
	Matrix I(N, N);  // H * Hinv should yield the identity matrix
	// TODO: this is not clear that for posits this would be a fused matrix multiply
	matmul(I, H, Hinv);

	if (verbose) {
		printMatrix(cout, "Hilbert matrix order 5", H);
		printMatrix(cout, "Hilbert inverse", Hinv);

		printMatrix(cout, "Hilbert inverse reference", Href);

		printMatrix(cout, "H * H^-1 => I", I);
	}

	// calculate the numerical error caused by the linear algebra computation
	Vector e(N), eprime(N), eabsolute(N), erelative(N);
	e = Scalar(1);
	// TODO: it is not clear that for posits this would be a fused matrix-vector operation
	matvec(eprime, I, e);
	printVector(cout, "reference vector", e);
	printVector(cout, "error vector", eprime);
	// absolute error
	eabsolute = e - eprime;
	printVector(cout, "absolute error vector", eabsolute);
	cout << "L1 norm   " << hex_format(l1_norm(eabsolute)) << "  " << l1_norm(eabsolute) << endl;
	cout << "L2 norm   " << hex_format(l2_norm(eabsolute)) << "  " << l2_norm(eabsolute) << endl;
	cout << "Linf norm " << hex_format(linf_norm(eabsolute)) << "  " << linf_norm(eabsolute) << endl;

	// relative error
	cout << "relative error\n";
	Scalar relative_error;
	relative_error = l1_norm(eabsolute) / l1_norm(e);
	cout << "L1 norm   " << hex_format(relative_error) << "  " << relative_error << endl;
	relative_error = l2_norm(eabsolute) / l2_norm(e);
	cout << "L2 norm   " << hex_format(relative_error) << "  " << relative_error << endl;
	relative_error = linf_norm(eabsolute) / linf_norm(e);
	cout << "Linf norm " << hex_format(relative_error) << "  " << relative_error << endl;

	// error volume
	cout << "error bounding box volume\n";
	cout << "Measured in Euclidean distance    : " << error_volume(linf_norm(eabsolute), N, false) << endl;
	cout << "Measured in ULPs                  : " << error_volume(linf_norm(eabsolute), N, true) << " ulps^" << N << endl;
	Scalar ulp = numeric_limits<Scalar>::epsilon();
	cout << "L-infinitiy norm measured in ULPs : " << linf_norm(eabsolute) / ulp << " ulps" << endl;
}

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;
	using namespace sw::hprblas;
	using namespace boost::multiprecision;

	using sp = boost::multiprecision::cpp_bin_float_single;
	using dp = boost::multiprecision::cpp_bin_float_double;
	using qp = boost::multiprecision::cpp_bin_float_quad;

	unsigned N = 5;
	GenerateNumericalAnalysisTestCase< posit<32, 2> >("posit<32,2>", N);
	cout << endl;
	GenerateNumericalAnalysisTestCase< float >("IEEE single precision", N);
	cout << endl;
	GenerateNumericalAnalysisTestCase< posit<64, 3> >("posit<64,3>", N);
	cout << endl;
	GenerateNumericalAnalysisTestCase< double >("IEEE double precision", N);
	cout << endl;
	GenerateNumericalAnalysisTestCase< posit<128, 4> >("posit<128,4>", N);
	cout << endl;
	GenerateNumericalAnalysisTestCase< qp >("IEEE quad precision", N);

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "SeaWaves.h"

namespace Upp {

using namespace Eigen;

double SeaWaves::rho = 1024;
double SeaWaves::g = 9.81;

double SeaWaves::WaveNumber(double T, double h, bool exact) {
	double y = 4*M_PI*M_PI*h/g/(T*T);
	double k1 = 1 + 0.6666666666*y + 0.3555555555*pow(y,2) + 0.1608465608*pow(y,3)
				+ 0.0632098765*pow(y,4) + 0.0217540484*pow(y,5) + 0.0065407983*pow(y,6);
	double k = sqrt((y*y + y/k1))/h;

	if (!exact)
		return k;

	double w = 2*M_PI/T;

	VectorXd x(1);
	x[0] = k;
	if (SolveNonLinearEquations(x, [&](const VectorXd &x, VectorXd &residual)->int {
		residual[0] = w*w - g*k*tanh(x[0]*h);
		return 0;
	}))
		return x[0];

	return -1;
}

SeaWaves::SEA_TYPE SeaWaves::GetSeaType(double T, double h) {
    double h_L = h/WaveLength(T, h);
	if (h_L < 1./20)
		return SHALLOW;
	else if (h_L < 1./2)
		return INTERMEDIATE;
	return DEEP;
}

double SeaWaves::WaveLength(double T, double h) 	{return 2*M_PI/WaveNumber(T, h);}

double SeaWaves::Celerity(double T, double h) 	  	{return WaveLength(T, h)/T;}

double SeaWaves::GroupCelerity(double T, double h) {
    double k = WaveNumber(T, h);
    double L = 2*M_PI/k;
    double c = L/T;
    double n = 0.5*(1 + 2*k*h/sinh(2*k*h));
    return c*n;
}

double SeaWaves::Power(double Te, double Hs, double h) {
	double k = WaveNumber(Te, h);
    double L = 2*M_PI/k;
    double c = L/Te;
    double n = 0.5*(1 + 2*k*h/sinh(2*k*h));
    double Cg = c*n;
    double E = 1/16.*rho*g*Hs*Hs;
	return E*Cg/1000.;
}

double SeaWaves::JONSWAP_Spectrum(double Hm0, double Tp, double gamma, double freq) {
	double sigma_f = freq <= 1/Tp ? 0.07 : 0.09;
	double beta = 0.0624/(0.230 + 0.0336*gamma - 0.185/(1.9 + gamma))*(1.094 - 0.01915*log(gamma));

	return beta*Hm0*Hm0*pow(Tp,-4)*pow(freq, -5)*exp(-5./4.*pow(Tp*freq, -4)) * pow(gamma, exp(-pow((Tp*freq - 1), 2)/(2*sigma_f*sigma_f)));
}

bool SeaWaves::JONSWAP_Spectrum_test(double Hm0, double Tp, double gamma) {
	if ((Tp > 5*sqrt(Hm0)) || (Tp < 3.6*sqrt(Hm0)))
		return false;
	if (gamma > 7 || gamma < 1)
		return false;
	return true;
}

bool SeaWaves::Init(double Tp, double Hs, double dirM, double h, int nd, int nf, double gamma,
					double disp_ang, int seed, double fmin, double fmax) {
	if(!(Tp > 0 && nd > 0 && nf > 0 && gamma >= 1))
		return false;

	this->Tp = Tp;
	this->dirM = dirM;
	this->Hs = Hs;
	this->h = h;
	this->nd = nd;
	this->nf = nf = nf/nd;

	frecs.resize(nf);
	k_f.resize(nf);
	dirs.resize(nd);
	amp_fd.resize(nd, nf);
	fi_fd.resize(nd, nf);

	double fp = 1/Tp;
	if (nf > 1) {
		if (fmin < 0)
			fmin = 0.25/Tp;
		else if (fmin > fp)
			return false;
		if (fmax < 0)
			fmax = 4/Tp;
		else if (fmax < fp)
			return false;
	} else
		fmin = fmax = fp;

	df = 1;
	if (nf > 1)
		df = (fmax-fmin)/(nf - 1);

	for(int f = 0; f < nf; f++) {
		int n = f + 1;
	    frecs[f] = fmin + (n - 1)*df;
	    k_f[f] = WaveNumber(1/frecs[f], h);
	}

	Buffer<double> Sf_f(nf);

	if (nf > 1) {
		double beta = 0.0624/(0.230 + 0.0336*gamma - 0.185*(pow(1.9 + gamma, -1)))*(1.094 - 0.01915*log(gamma));
		for(int f = 0; f < nf; f++) {
			double sigma_f;
			if(frecs[f] <= fp)
		        sigma_f = 0.07;
			else
				sigma_f = 0.09;

			Sf_f[f] = beta*pow(Hs, 2)*pow(Tp,-4)*pow(frecs[f], -5)*exp(-1.25*pow(Tp*frecs[f], -4))
						  *pow(gamma, exp(-pow((Tp*frecs[f]-1), 2)/2/pow(sigma_f, 2)));
		}
	} else
		Sf_f[0] = 1/2.*pow(Hs/2., 2);

	if (nd > 1) {
	    double dirmin = dirM - disp_ang;
	    double dirmax = dirM + disp_ang;
	    double dd = (dirmax - dirmin)/(nd-1);

	    for (int d = 0; d < nd; ++d)
	        dirs[d] = dirmin + d*dd;

	    double L0 = g*Tp*Tp/2./M_PI;
	    double per = Hs/L0;
	    double Smax = pow(10, -1.2195*log10(per) - 0.5573);

		Buffer<double> incdir_d(nd);

		for (int d = 0; d < nd; ++d)
			incdir_d[d] = dirs[d] - dirM;

		for (int f = 0; f < nf; f++) {
			double s_f;
		    if (frecs[f] <= fp)
		        s_f = pow((frecs[f]/fp), 5)*Smax;
		    else
		        s_f = pow((frecs[f]/fp), -2.5)*Smax;

		    double G0_f = 0;
		    for (int d = 0; d < nd; d++)
		        G0_f += pow(cos(incdir_d[d]/2), 2*s_f)*dd;
		    G0_f = 1/G0_f;

	      	for (int d = 0; d < nd; d++) {
		        double G_fd = G0_f*pow(cos(incdir_d[d]/2.), 2*s_f);
		        double Sfd_fd = Sf_f[f]*G_fd;
		        amp_fd(d, f) = sqrt(2*Sfd_fd*df*dd);
		    }
		}
	} else {
	    dirs[0] = dirM;
	    for (int f = 0; f < nf; ++f)
	        amp_fd(0, f) = sqrt(2*Sf_f[f]*df);
	}

	if (nf > 1) {
		if (IsNull(seed)) {
			std::random_device rd;
			seed = rd();
		}
		std::mt19937 random_engine(seed);
		std::uniform_real_distribution<double> random_distribution(-M_PI, M_PI);
		for (int f = 0; f < nf; f++)
		    for (int d = 0; d < nd; d++)
				fi_fd(d, f) = random_distribution(random_engine);
	} else {
		for (int d = 0; d < nd; d++)
			fi_fd(d, 0) = 0;
	}

	return true;
}

void SeaWaves::Calc(double x, double y, double z, double t) {
	ASSERT(frecs.size() > 0);

    zSurf = dzSurf = vx = vy = vz = ax = ay = az = 0;

	p = -rho*g*z;

	for (int f = 0; f < nf; f++) {
    	for (int d = 0; d < nd; d++) {
    		double frec = frecs[f];
    		double sindirsd = sin(dirs[d]);
    		double cosdirsd = cos(dirs[d]);
    		double aux = k_f[f]*cosdirsd*x + k_f[f]*sindirsd*y - 2*M_PI*frec*t + fi_fd(d, f);
    		double auxcos = cos(aux);
    		double auxsin = sin(aux);

	        double AUXsl = amp_fd(d, f)*auxcos;
            zSurf += AUXsl;
            dzSurf += 2*M_PI*frec*amp_fd(d, f)*auxsin;

	        double kp     = cosh(k_f[f]*(h+z))/cosh(k_f[f]*h);
	        double kp_sin = sinh(k_f[f]*(h+z))/cosh(k_f[f]*h);

	        double AUXvh = g*amp_fd(d, f)*k_f[f]/(2*M_PI*frec)*kp*auxcos;
	        vx += AUXvh*cosdirsd;
	        vy += AUXvh*sindirsd;

	        vz += g*amp_fd(d, f)*k_f[f]/(2*M_PI*frec)*kp_sin*auxsin;

	        double AUXah = g*amp_fd(d, f)*pow((k_f[f]),2)/(2*M_PI*frec)*kp*auxsin;
	        ax += AUXah*cosdirsd;
	        ay += AUXah*sindirsd;

	        az += -g*amp_fd(d, f)*pow((k_f[f]),2)/(2*M_PI*frec)*kp_sin*auxcos;

	   		p += rho*g*kp*AUXsl;
    	}
	}
}

double SeaWaves::ZSurf(double x, double y, double z, double t) {
	ASSERT(frecs.size() > 0);

    double zSurf = 0;
	for (int f = 0; f < nf; f++)
    	for (int d = 0; d < nd; d++)
	        zSurf += amp_fd(d, f)*cos(k_f[f]*cos(dirs[d])*x + k_f[f]*sin(dirs[d])*y
	        						  - 2*M_PI*frecs[f]*t + fi_fd(d, f));
    return zSurf;
}

double SeaWaves::Pressure(double x, double y, double z, double t) {
	ASSERT(frecs.size() > 0);

	double p = -rho*g*z;
	for (int f = 0; f < nf; f++)
    	for (int d = 0 ; d < nd; d++) {
	        double KP    = cosh(k_f[f]*(h+z))/cosh(k_f[f]*h);
    		double AUXsl = amp_fd(d, f)*cos(k_f[f]*cos(dirs[d])*x + k_f[f]*sin(dirs[d])*y - 2*M_PI*frecs[f]*t + fi_fd(d, f));
    		p += rho*g*KP*AUXsl;
    	}
    return p;
}

// DNV-RP-C205, page 34
double Te_fTp(double Tp, double gamma) 	  {return Tp*(4.2 + gamma)/(5 + gamma);}
double Tp_fTe(double Te, double gamma) 	  {return Te*(5 + gamma)/(4.2 + gamma);}
double gamma_fTp_Te(double Tp, double Te) {return (Te*5 - Tp*4.2)/(Tp - Te);}
double Tp_fTm(double Tm, double gamma) 	  {return Tm/(0.7303 + 0.04936*gamma - 0.006556*pow(gamma, 2) + 0.0003610*pow(gamma, 3));}
double Tp_fTz(double Tz, double gamma) 	  {return Tz/(0.6673 + 0.05037*gamma - 0.006230*pow(gamma, 2) + 0.0003341*pow(gamma, 3));}

double gamma_fTp_Tz(double Tp, double Tz) {
	VectorXd x(1);
	x[0] = 5;
	if (SolveNonLinearEquations(x, [&](const VectorXd &x, VectorXd &residual)->int {
		double gamma = x[0];
		double gamma2 = gamma*gamma;
		residual[0] = - Tz/Tp + 0.6673 + 0.05037*gamma - 0.006230*gamma2 + 0.0003341*gamma*gamma2;
		return 0;
	}))
		return x[0];
	return -1;
}

}


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

#include "timer.h"

//! \brief Use efficient implementation A*x = bb
//! \param[in] R Matrix is nxn and upper triangular
//! \param[in] v Vector is nx1
//! \param[in] u Vector is nx1
//! \param[in] bb vector is (n+1)x1 and is stacked (b, \beta)^T =: b
//! \param[out] x solution A*bb = x
template <class Matrix, class Vector>
void solvelse(const Matrix & R, const Vector & v, const Vector & u,
              const Vector & bb, Vector & x) {
    // Size of R, which is wize of u, v, and size of bb is n+1
    unsigned int n = R.rows();
    if( n != R.cols() || n != u.size() || n != v.size() || n+1  != bb.size() ) {
        // Size check, error if do not match
        std::cerr << "Error: size mismatch!" << std::endl;
        return;
    }

    // This gets the scalar type of the matrix
    typedef typename Matrix::Scalar Scalar;
    // Tell eigen R is triangular, we use the construct R.template to avoid compilation errors
    // cf. http://eigen.tuxfamily.org/dox-devel/TopicTemplateKeyword.html
    // here auto = const Eigen::TriangularView<const Matrix, Eigen::Upper>
    auto triR = R.template triangularView<Eigen::Upper>();

    // s is the Schur's complement and, in this case, is a scalar (and so is b_s)
    // snv = s^{-1}, b_s as in lecture notes
    // sinvbs = s^{-1}*b_s
    Scalar sinv = - 1. / u.dot(triR.solve(v));
    Scalar bs = (bb(n) - u.dot(triR.solve(bb.head(n))));
    Scalar sinvbs = sinv*bs;

    // Stack the vector (z, \xi)^T =: x
    x = Vector::Zero(n+1);
    x << triR.solve(bb.head(n) - v*sinvbs), sinvbs;
}

//! \brief Use Eigen's LU-solver to solve Ax = y
//! \param[in] R Matrix is nxn and upper triangular
//! \param[in] v Vector is nx1
//! \param[in] u Vector is nx1
//! \param[in] bb vector is (n+1)x1 and is stacked (b, \beta)^T =: b
//! \param[out] x solution A*bb = x
template <class Matrix, class Vector>
void solvelse_lu(const Matrix & R, const Vector & v, const Vector & u,
                 const Vector & bb, Vector & x) {
    // Size of R, which is wize of u, v, and size of bb is n+1
    unsigned int n = R.rows();
    if( n != R.cols() && n != u.size() && n != v.size() && n+1  != bb.size() ) {
        // Size check, error if do not match
        std::cerr << "Error: size mismatch!" << std::endl;
        return;
    }

    Matrix A(n+1,n+1);
    A << R, v, u.transpose(), 0;

    x = A.partialPivLu().solve(bb);
}

int main() {
    // Bunch of random vectors/matrices
    int n = 9;
    Eigen::MatrixXd R = Eigen::MatrixXd::Random(n,n).triangularView<Eigen::Upper>();
    Eigen::VectorXd v = Eigen::VectorXd::Random(n);
    Eigen::VectorXd u = Eigen::VectorXd::Random(n);
    Eigen::VectorXd bb = Eigen::VectorXd::Random(n+1);
    Eigen::VectorXd x;

    // Check that answer is correct
    std::cout << "*** Check correctness of Block Gauss implementation" << std::endl;
    solvelse(R, v, u, bb, x);
    std::cout << "Block Gauss:" << std::endl << x << std::endl;
    // Compare with eigen partial pivot LU-solve
    solvelse_lu(R, v, u, bb, x);
    std::cout << "Eigen LU:" << std::endl << x << std::endl;

    // Compute runtime of different implementations of kron
    std::cout << "*** Runtime comparisons of Block Gauss implementation VS Eigen LU" << std::endl;
    unsigned int repeats = 3;
    timer<> tm_own, tm_eigen_lu;

    for(unsigned int p = 2; p <= 10; p++) {
        tm_own.reset();
        tm_eigen_lu.reset();
        for(unsigned int r = 0; r < repeats; ++r) {
            unsigned int M = pow(2,p);
            R = Eigen::MatrixXd::Random(M,M).triangularView<Eigen::Upper>(); // We will use only upper triangular part
            v = Eigen::VectorXd::Random(M);
            u = Eigen::VectorXd::Random(M);
            bb = Eigen::VectorXd::Random(M+1);

            tm_own.start();
            solvelse(R, v, u, bb, x);
            tm_own.stop();

            tm_eigen_lu.start();
            solvelse_lu(R, v, u, bb, x);
            tm_eigen_lu.stop();
        }

        std::cout << "Own implementation took:       " << tm_own.min().count() / 1000000. << " ms" << std::endl;
        std::cout << "Eigen solver took:             " << tm_eigen_lu.min().count() / 1000000. << " ms" << std::endl;
    }
}

Nummerical Methods for CSE/solutions/solution_2/block_lu_decomp.cpp


// Author: Haralambi Todorov <harrytodorov@gmail.com>

#include <cstdint>
#include <cassert>
#include <vector>
#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

struct fraction {
  int256_t n; // numerator
  int256_t d; // denominator
  fraction(const int256_t &_n,
           const int256_t &_d) : n(_n), d(_d) {}
};

fraction add_int_to_frac(const int256_t &i,
                         const fraction &f) {
  return fraction(i * f.d + f.n, f.d);
}

fraction int_over_frac(const int256_t &i,
                       const fraction &f) {
  return fraction(f.d * i, f.n);
}

std::vector<size_t> e_sequence(const size_t &n) {
  std::vector<size_t> res;
  res.push_back(2);
  size_t i = 1;
  while (res.size() < n) {
    if ((res.size() + 1) % 3 == false) {
      res.push_back(2 * i);
      i++;
    } else {
      res.push_back(1);
    }
  }
  return res;
}

int256_t sum_digits(const int256_t &i) {
  int256_t sum = i % 10;
  int256_t _i = i / 10;
  while (_i > 0) {
    sum += (_i % 10);
    _i /= 10;
  }
  return sum;
}

void tests() {
  assert(fraction(4, 3).n == add_int_to_frac(1, fraction(1, 3)).n);
  assert(fraction(4, 3).d == add_int_to_frac(1, fraction(1, 3)).d);

  assert(fraction(11, 4).n == add_int_to_frac(2, fraction(3, 4)).n);
  assert(fraction(11, 4).d == add_int_to_frac(2, fraction(3, 4)).d);

  assert(fraction(3, 4).n == int_over_frac(1, fraction(4, 3)).n);
  assert(fraction(3, 4).d == int_over_frac(1, fraction(4, 3)).d);

  assert(17 == sum_digits(1457));
}

int main() {
  // Generate first n numbers of the e convergence sequence.
  size_t n = 100;
  std::vector<size_t> e_seq = e_sequence(n);

  // Compute the convergence of e until n.
  fraction curr(1, e_seq[e_seq.size() - 1]);
  int i = e_seq.size() - 2;
  while (i >= 0) {
    if (i == 0) {
      curr = add_int_to_frac(e_seq[i], curr);
    } else {
      curr = int_over_frac(1, add_int_to_frac(e_seq[i], curr));
    }
    i--;
  }

  std::cout << "Result: " << curr.n << "/" << curr.d << std::endl;
  std::cout << "Sum of the digits in the numerator: " << sum_digits(curr.n)
            << std::endl;
  return 0;
}


#include <iostream>
#include <eigen3/Eigen/Dense>
#include "gnuplot-iostream.h"
#include <boost/numeric/odeint.hpp>
#include "sysfunc_exercise2_2_a.h"

using Eigen::Matrix3cd;
using Eigen::Matrix3d;
namespace odeint = boost::numeric::odeint;

int main(int argc, char const *argv[])
{
    Matrix3d A;
    A << 0, 1, 0,
        0, 0, 1,
        2, 1, -2;
    std::cout << "Here is the matrix A:\n"
              << A << std::endl;

    Eigen::EigenSolver<Matrix3d> eigenSolver(A);
    if (eigenSolver.info() != Eigen::Success)
        abort();
    std::cout << "The eigenvalues of A are:\n"
              << eigenSolver.eigenvalues() << std::endl;
    std::cout << "Here's a matrix whose columns are eigenvectors of A \n"
              << "corresponding to these eigenvalues:\n"
              << eigenSolver.eigenvectors() << std::endl;

    Matrix3cd T = eigenSolver.eigenvectors();
    Matrix3cd T_inverse = T.inverse();
    Matrix3cd D = T_inverse * A * T;

    std::cout << "The diagonlized Matrix D is:\n"
              << D << std::endl;

    // Odient Code ...
    std::vector<system_function::state_type> x_vec;
    std::vector<double> times;
    std::vector<double> position;
    {
        system_function::state_type x(3); // a vector with size 3. [x; x_dot; x_dot_dot]
        //x = {1, 1, 1};                    // start at x=1, 1, 1;
        x = {1, -1, 1};
        size_t steps = odeint::integrate(system_function::harmonic_oscillator,
                                         x, 0.0, 10.0, 0.1,
                                         system_function::push_back_state_and_time(x_vec, times)); //time should be there?

        for (size_t i = 0; i <= steps; i++)
        {
            position.push_back(x_vec[i][0]);
        }
    }

    // GNU Plot
    {
        /* output */
        Gnuplot gp;
        gp << "plot '-' using 1:2 with linespoint" << std::endl;
        gp.send1d(std::make_tuple(times, position));
    }

    return 0;
}


#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <armadillo>

using namespace  std;
using namespace arma;

vec SteepestDescent(mat A, vec b, vec x0, int dim){
  int IterMax, i;
  const double tolerance = 1.0e-14;
  vec x(dim),r(dim),t(dim);
  double c ,alpha,d;
  IterMax = 20;
  x = x0;
  r = A*x-b;
  i = 0;
  while (i <= IterMax || sqrt(dot(r,r)) < tolerance ){
    if(sqrt(dot(r,r))<tolerance){
       cerr << "An error has occurred: execution of function terminated" << endl;
       break;
    }
    t = A*r;
    c = dot(r,r);
    alpha = c/dot(r,t);
    x = x - alpha*r;
    r =  A*x-b;
    if(sqrt(dot(r,r)) < tolerance) break;
    i++;
  }
  return x;
}

//   Main function begins here
int main(int  argc, char * argv[]){
  int dim = 2;
  vec x(dim),xsd(dim), b(dim),x0(dim);
  mat A(dim,dim);

  // Set our initial guess
  x0(0) = x0(1) = 0;
  // Set the matrix
  A(0,0) =  3;    A(1,0) =  2;   A(0,1) =  2;   A(1,1) =  6;

  cout << "The Matrix A that we are using: " << endl;
  A.print();
  cout << endl;

  vec y(dim);
  y(0) = 2.;
  y(1) = -2.;

  cout << "The exact solution is: " << endl;
  y.print();
  cout << endl;
  b = A*y;

  cout << "The right hand side, b, of the expression Ax=b: " << endl;
  b.print();
  cout << endl;

  xsd = SteepestDescent(A,b,x0, dim);

  cout << "The approximate solution using Steepest Descent is: " << endl;
  xsd.print();
  cout << endl;

}


// conjugate_gradient.cpp: example program showing the conjugate gradient algorithm using posit arithmetic
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include <hprblas>
#include <universal/number/posit/posit>
#include <boost/multiprecision/cpp_bin_float.hpp>

typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<11, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -32, 31>, boost::multiprecision::expression_template_option::et_off> cpp_bin_float_half;

/*
From Wikipedia, the free encyclopedia
https://en.wikipedia.org/wiki/Conjugate_gradient_method

In mathematics, the conjugate gradient method is an algorithm for the numerical solution of particular
systems of linear equations, namely those whose matrix is symmetric and positive-definite.
The conjugate gradient method is often implemented as an iterative algorithm, applicable to
sparse systems that are too large to be handled by a direct implementation or other direct methods
such as the Cholesky decomposition. Large sparse systems often arise when numerically solving
partial differential equations or optimization problems.

The conjugate gradient method can also be used to solve unconstrained optimization problems
such as energy minimization. It was mainly developed by Magnus Hestenes and Eduard Stiefel who programmed it on the Z4.

The biconjugate gradient method provides a generalization to non-symmetric matrices.
Various nonlinear conjugate gradient methods seek minima of nonlinear equations.
 */

// Conjugate Gradient algorithm, returns the iteration number of convergence
template<typename Matrix, typename Vector, typename Real>
unsigned CG(const Matrix& A, const Vector& b, Vector& x, Real epsilon) {
	using namespace sw::hprblas;
	//using Real = Vector::value_type;
	// starting x is provided by calling context
	unsigned k = 0;
	Vector r = b;
	Real error = mtl::two_norm(r);
	Vector p = r;
	Vector Ap(size(p)); // need to create the vector to be the same structure as p as the expression (A * p) doesn't do it
	while (error > epsilon) {
		Ap = A * p;
		Real alpha = dot(r, r) / dot(p, Ap);
		x = x + alpha * p;
		Vector r_prev = r;
		r = r - alpha * A * p;
		error = mtl::two_norm(r);
		Real beta = dot(r, r) / dot(r_prev, r_prev);
		p = r + beta * p;
		std::cout << "iteration: " << std::setw(4) << k
			<< " alpha: " << std::setw(12) << alpha
			<< " beta: " << std::setw(12) << beta
			<< " error : " << error << std::endl;
		++k;
		if (k > 1.5* size(b)) break;
	}
	return k;
}

// Conjugate Gradient algorithm, returns the iteration number of convergence
template<typename Matrix, typename Vector, typename Real>
unsigned fdpCG(const Matrix& A, const Vector& b, Vector& x, Real epsilon) {
	using namespace sw::universal;
	//using Real = Vector::value_type;
	// starting x is provided by calling context
	unsigned k = 0;
	Vector r = b;
	Real error = mtl::two_norm(r);
	Vector p = r;
	Vector Ap(size(p)); // need to create the vector to be the same structure as p as the expression (A * p) doesn't do it
	while (error > epsilon) {
		Ap = A * p;
		Real alpha = fdp(r, r) / fdp(p, Ap);
		x = x + alpha * p;
		Vector r_prev = r;
		r = r - alpha * A * p;
		Real beta = fdp(r, r) / fdp(r_prev, r_prev);
		p = r + beta * p;
		error = mtl::two_norm(r);
		std::cout << "iteration: " << std::setw(4) << k
			<< " alpha: " << std::setw(12) << alpha
			<< " beta: " << std::setw(12) << beta
			<< " error : " << error << std::endl;

		++k;
		if (k > 1.5* size(b)) break;
	}
	return k;
}

// Conjugate Gradient algorithm, returns the iteration number of convergence
template<typename Matrix, typename Vector, typename Real>
unsigned fdp2CG(const Matrix& A, const Vector& b, Vector& x, Real epsilon) {
	using namespace sw::universal;
	//using Real = Vector::value_type;
	// starting x is provided by calling context
	unsigned k = 0;
	Vector r = b;
	Real error = mtl::two_norm(r);
	Vector p = r;
	Vector Ap(size(p)); // need to create the vector to be the same structure as p as the expression (A * p) doesn't do it
	while (error > epsilon) {
		sw::hprblas::matvec(Ap, A, p);   // Ap = A * p
		Real alpha = fdp(r, r) / fdp(p, Ap);
		x = x + alpha * p;
		Vector r_prev = r;
		r = r - alpha * Ap;
		Real beta = fdp(r, r) / fdp(r_prev, r_prev);
		p = r + beta * p;
		error = mtl::two_norm(r);
		std::cout << "iteration: " << std::setw(4) << k
			<< " alpha: " << std::setw(12) << alpha
			<< " beta: " << std::setw(12) << beta
			<< " error : " << error << std::endl;

		++k;
		if (k > 1.5* size(b)) break;
	}
	return k;
}

template<typename Real>
void CGdriver(unsigned N, Real epsilon) {
	using namespace std;
	using namespace mtl;
	using Matrix = mtl::dense2D<Real>;
	using Vector = mtl::dense_vector<Real>;

	Matrix A(N, N);
	mat::laplacian_setup(A, N, 1);
	//cout << A << endl;

	Vector b(N), x(N), ones(N);
	ones = Real(1);
	b = A * ones;
	x = Real(0.0);  // starting x
	cout << b << endl;
	unsigned k = CG(A, b, x, epsilon);
	cout << "solution: " << x << " at iteration: " << k << endl;

	Vector error(N);
	error = ones - x;
	cout << "exact error is: " << two_norm(error) << endl;
}

// CG with fdp applied to alpha/beta calculation only
template<size_t nbits, size_t es>
void fdpCGdriver(unsigned N, sw::universal::posit<nbits,es> epsilon) {
	using namespace std;
	using namespace mtl;
	using Real = sw::universal::posit<nbits, es>;
	using Matrix = mtl::dense2D<Real>;
	using Vector = mtl::dense_vector<Real>;

	Matrix A(N, N);
	mat::laplacian_setup(A, N, 1);
	//cout << A << endl;

	Vector b(N), x(N), ones(N);
	ones = Real(1);
	b = A * ones;
	x = Real(0.0);  // starting x
	cout << b << endl;
	unsigned k = fdpCG(A, b, x, epsilon);
	cout << "solution: " << x << " at iteration: " << k << endl;

	Vector error(N);
	error = ones - x;
	cout << "exact error is: " << two_norm(error) << endl;
}

// CG with fdp applied to alpha/beta and to matvec A * p as well
template<size_t nbits, size_t es>
void fdp2CGdriver(unsigned N, sw::universal::posit<nbits, es> epsilon) {
	using namespace std;
	using namespace mtl;
	using Real = sw::universal::posit<nbits, es>;
	using Matrix = mtl::dense2D<Real>;
	using Vector = mtl::dense_vector<Real>;

	Matrix A(N, N);
	mat::laplacian_setup(A, N, 1);
	//cout << A << endl;

	Vector b(N), x(N), ones(N);
	ones = Real(1);
	b = A * ones;
	x = Real(0.0);  // starting x
	cout << b << endl;
	unsigned k = fdp2CG(A, b, x, epsilon);
	cout << "solution: " << x << " at iteration: " << k << endl;

	Vector error(N);
	error = ones - x;
	cout << "exact error is: " << two_norm(error) << endl;
}

// CG test program
int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::universal;

	string separation_string = "=================================================================\n";
	constexpr unsigned N = 10;
	long double accuracy = 1.0e-3;

	{
		constexpr size_t nbits = 16;
		constexpr size_t es = 1;
		using Real = posit<nbits,es>;
		Real pminpos; minpos(pminpos);
		cout << "type: " << typeid(Real).name() << endl;
		cout << "minpos : " << pminpos << endl;
		Real epsilon = Real(accuracy);
		CGdriver(N, epsilon);
	}

	cout << separation_string;
	{
		using Real = cpp_bin_float_half;
		cout << "type: " << typeid(Real).name() << endl;
		Real epsilon = Real(accuracy);
		CGdriver(N, epsilon);
	}
	cout << separation_string;
	{
		constexpr size_t nbits = 16;
		constexpr size_t es = 1;
		using Real = posit<nbits, es>;
		Real pminpos; minpos(pminpos);
		cout << "type: " << typeid(Real).name() << endl;
		cout << "minpos : " << pminpos << endl;
		Real epsilon = Real(accuracy);
		CGdriver(N, epsilon);
		fdpCGdriver(N, epsilon);
		fdp2CGdriver(N, epsilon);
	}

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

/**
 * @brief Calcola la direzione normale di ogni vertice nella mesh in input.
 *
 * Data una mesh di triangoli M = (V, F), dove V e' un array di posizioni 3D
 * (i vertici) e F e' un array di triple di indici ai vertici (ogni tripla
 * rappresenta una faccia triangolare), questa funzione calcola la direzione
 * normale di ogni vertice come una somma pesata delle normali dei triangoli
 * intorno (incidenti) a tale vertice.
 * Il peso è dato dall'area del triangolo moltiplicato l'angolo incidente
 * sul vertice.
 *
 * L'area di un triangolo viene calcolata come meta' della lunghezza (norma)
 * del prodotto vettoriale tra due lati.
 * L'angolo incidente su un vertice viene calcolato come l'arcocoseno del
 * prodotto scalare tra i due lati incidenti (intesi come vettori unitari
 * centrati su di esso), oppure come l'arcotangente del seno e coseno:
 * - il seno e' proporzionale alla normal del prodotto vettoriale
 * - il cose e' proporzionale al prodotto scalare.
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @return MatrixXd La matrice restituita ha una riga per ogni vertice
 *                  (N.rows() == V.rows()) e 3 colonne. Ogni riga corrisponde
 *                  alla direzione normale del vertice corrispondente, avente
 *                  le 3 coordinate x,y,z.
 */
MatrixXd perVertexNormals(MatrixXd const &V, MatrixXi const &F)
{
    MatrixXd N = MatrixXd::Zero(V.rows(), 3);

    // per leggere/scrivere un elemento di una matrice X: X(i,j)
    // per leggere/scrivere una riga di una matrice X: X.row(i) (un vettore orizzontale)
    // il vettore da un punto p0 a un altro punto p1: Vector3d v = p1 - p0;
    // il prodotto vettoriale tra 2 vettori: Vector3d c = v1.cross(v2);
    // normalizzare un vettore, in-place: c.normalize();
    // normalizzare un vettore, in una nuova variabile: Vector3d cn = c.normalized();
    // lunghezza (norma) di un vettore: v.norm();
    // angolo tra due vettori normalizzati: std::acos(n1.dot(n2));
    // angolo tra due vettori non-normalizzati: std::atan2(v1.cross(v2).norm(), v1.dot(v2));

    for (int f = 0; f < F.rows(); ++f)
    {
        int i = F(f, 0);
        int j = F(f, 1);
        int k = F(f, 2);

        Vector3d A = V.row(i);
        Vector3d B = V.row(j);
        Vector3d C = V.row(k);

        Vector3d e0 = B - A;
        Vector3d e1 = C - B;
        Vector3d e2 = A - C;

        Vector3d c0 = e0.cross(-e2);
        Vector3d c1 = e1.cross(-e0);
        Vector3d c2 = e2.cross(-e1);

        double area = c0.norm() / 2.0;

        Vector3d n = c0.normalized();

        double a_i = std::atan2(c0.norm(), e0.dot(-e2));
        double a_j = std::atan2(c1.norm(), e1.dot(-e0));
        double a_k = std::atan2(c2.norm(), e2.dot(-e1));

        N.row(i) += area * a_i * n;
        N.row(j) += area * a_j * n;
        N.row(k) += area * a_k * n;
    }

    for (int v = 0; v < V.rows(); ++v) {
        N.row(v).normalize();
    }

    return N;
}


// Implements functions that fit conics to data using linear least squares methods.
// File: conicfit.cpp
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <rc_utils/conicfit.h>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/LU>
#include <numeric>
#include <cmath>

// Namespaces
using namespace rc_utils;

// Fitting problems can often be expressed as an overconstrained set of linear equations Ax = b. Such a set of equations is generally
// solved by treating it as a least squares problem. The task is then to find a vector xhat that has the smallest residual squared
// error in satisfying Ax = b. It is a well-known result that the required minimising solution is given by:
//   xhat = (A'A)\(A'b)               {Matlab style}
// Or equivalently:
//   xhat = (A^T * A)^-1 * A^T * b    {Explicit style}
// The exact residual sum being minimised is:
//   J(x) = norm(b - Ax)^2

// Constants
const double ConicFit::ZeroTol = 1e-12;

//
// Circle fitting
//

// Fit a circle to 2D data
void ConicFit::fitCircle(const Points2D& P, Eigen::Vector2d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi). We try to fit a circle of radius R and centre (cx,cy) to the data. Let:
	//   A = N rows of [xi yi 1]              {Nx3 matrix}
	//   x = [2*cx; 2*cy; R^2-cx^2-cy^2]      {3x1 column vector}
	//   b = N rows of [xi^2 + yi^2]          {Nx1 column vector}
	// Ax = b is equivalent to the condition that all data points lie on a circle of radius R, centered at (cx,cy).
	// Hence by solving Ax = b for x in a least squares sense, we are implicitly solving for the values of cx, cy, R
	// for which the corresponding circle most closely fits the data. We perform a mean shift on the data for
	// numerical conditioning and stability reasons.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd A(N, 3);
	Eigen::VectorXd b(N);

	// Calculate the mean of the data points
	Eigen::Vector2d mean = Eigen::Vector2d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Ax = b)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector2d p = P[i] - mean;
		A(i, 0) = p.x();
		A(i, 1) = p.y();
		A(i, 2) = 1.0;
		b(i) = p.squaredNorm();
	}

	// Call the worker function to fit the circle
	fitCircle(A, b, centre, radius);

	// Correct for the applied mean shift
	centre += mean;
}

// Fit a circle to the XY components of 3D data
void ConicFit::fitCircle(const Points3D& P, Eigen::Vector3d& centre, double& radius)
{
	// We discard the z coordinates and fit a circle as normal for 2D data.
	// The returned centre z coordinate is a plain mean of the input z coordinates.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd A(N, 3);
	Eigen::VectorXd b(N);

	// Calculate the mean of the data points
	Eigen::Vector3d mean = Eigen::Vector3d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Ax = b)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector3d p = P[i] - mean;
		A(i, 0) = p.x();
		A(i, 1) = p.y();
		A(i, 2) = 1.0;
		b(i) = p.x()*p.x() + p.y()*p.y();
	}

	// Call the worker function to fit the circle
	Eigen::Vector2d centre2D;
	fitCircle(A, b, centre2D, radius);

	// Correct for the applied mean shift
	centre.x() = mean.x() + centre2D.x();
	centre.y() = mean.y() + centre2D.y();
	centre.z() = mean.z();
}

// Worker function for fitting a circle
void ConicFit::fitCircle(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, Eigen::Vector2d& centre, double& radius)
{
	// Find the least squares solution to Ax = b using Singular Value Decomposition (linear least squares fitting)
	Eigen::VectorXd coefficients = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

	// Extract the centre and radius of the fitted circle
	centre.x() = 0.5*coefficients(0);
	centre.y() = 0.5*coefficients(1);
	radius = sqrt(coefficients(2) + centre.squaredNorm());
}

// Fit a circle of known centre to 2D data
void ConicFit::fitCircleCentred(const Points2D& P, const Eigen::Vector2d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi). The least squares solution to finding a circle of radius R of known centre
	// (cx,cy) that best fits the data points has a simple algebraic solution, given by:
	//   R = sqrt(sum((xi-cx)^2 + (yi-cy)^2, 1, N) / N)
	// In other words, the radius of best fit is the quadratic mean of the distances from the known centre to each point.

	// Declare variables
	size_t N = P.size();

	// Calculate the radius of best fit
	double Rsq = 0.0;
	for(size_t i = 0; i < N; i++)
		Rsq += (P[i] - centre).squaredNorm() / N;
	radius = sqrt(Rsq);
}

// Fit a circle of known centre to the XY components of 3D data
void ConicFit::fitCircleCentred(const Points3D& P, const Eigen::Vector3d& centre, double& radius)
{
	// We discard the z coordinates and fit a circle of known centre as normal for 2D data.

	// Declare variables
	size_t N = P.size();
	Eigen::Vector2d centre2D = centre.head<2>();

	// Calculate the radius of best fit
	double Rsq = 0.0;
	for(size_t i = 0; i < N; i++)
		Rsq += (P[i].head<2>() - centre2D).squaredNorm() / N;
	radius = sqrt(Rsq);
}

// Calculate the fitting error of a circle to certain 2D data
double ConicFit::fitCircleError(const Points2D& P, const Eigen::Vector2d& centre, double radius)
{
	// The returned error is the average distance to the circle in units of circle radii.

	// Calculate and return the required dimensionless fitting error
	size_t N = P.size();
	if(N <= 0) return 0.0;
	double err = 0.0;
	if(radius != 0.0)
	{
		for(size_t i = 0; i < N; i++)
			err += fabs((P[i] - centre).norm() - radius);
		err /= N*fabs(radius);
	}
	else
	{
		for(size_t i = 0; i < N; i++)
			err += fabs((P[i] - centre).norm());
		err /= N;
	}
	return err;
}

// Calculate the fitting error of a circle to certain 3D data
double ConicFit::fitCircleError(const Points3D& P, const Eigen::Vector3d& centre, double radius)
{
	// The returned error is the average planar XY distance to the circle in units of circle radii.

	// Calculate and return the required dimensionless fitting error
	size_t N = P.size();
	if(N <= 0) return 0.0;
	Eigen::Vector2d centre2D = centre.head<2>();
	double err = 0.0;
	if(radius != 0.0)
	{
		for(size_t i = 0; i < N; i++)
			err += fabs((P[i].head<2>() - centre2D).norm() - radius);
		err /= N*fabs(radius);
	}
	else
	{
		for(size_t i = 0; i < N; i++)
			err += fabs((P[i].head<2>() - centre2D).norm());
		err /= N;
	}
	return err;
}

//
// Weighted circle fitting
//

// Fit a weighted circle to 2D data
void ConicFit::fitCircleWeighted(const WeightedPoints2D& P, Eigen::Vector2d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi,wi). We try to fit a circle of radius R and centre (cx,cy) to the data. Let:
	//   A = N rows of [wi*xi wi*yi wi]       {Nx3 matrix}
	//   x = [2*cx; 2*cy; R^2-cx^2-cy^2]      {3x1 column vector}
	//   b = N rows of [wi*(xi^2 + yi^2)]     {Nx1 column vector}
	// Ax = b is equivalent to the weighted condition that all data points lie on a circle of radius R, centered at (cx,cy).
	// Hence by solving Ax = b for x in a least squares sense, we are implicitly solving for the values of cx, cy, R
	// for which the corresponding circle most closely fits the weighted data. We perform a mean shift on the data for
	// numerical conditioning and stability reasons.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd A(N, 3);
	Eigen::VectorXd b(N);

	// Calculate the mean of the data points
	Eigen::Vector2d mean = Eigen::Vector2d::Zero();
	for(size_t i = 0; i < N; i++)
		mean += P[i].head<2>();
	if(N >= 1)
		mean /= N;

	// Construct the linear equation to solve (Ax = b)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector2d p = P[i].head<2>() - mean;
		double w = P[i].z();
		A(i, 0) = w*p.x();
		A(i, 1) = w*p.y();
		A(i, 2) = w;
		b(i) = w*p.squaredNorm();
	}

	// Call the worker function to fit the circle
	fitCircle(A, b, centre, radius);

	// Correct for the applied mean shift
	centre += mean.head<2>();
}

// Fit a weighted circle to 2D data
void ConicFit::fitCircleWeighted(const Points2D& P, const Weights& W, Eigen::Vector2d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi,wi). We try to fit a circle of radius R and centre (cx,cy) to the data. Let:
	//   A = N rows of [wi*xi wi*yi wi]       {Nx3 matrix}
	//   x = [2*cx; 2*cy; R^2-cx^2-cy^2]      {3x1 column vector}
	//   b = N rows of [wi*(xi^2 + yi^2)]     {Nx1 column vector}
	// Ax = b is equivalent to the weighted condition that all data points lie on a circle of radius R, centered at (cx,cy).
	// Hence by solving Ax = b for x in a least squares sense, we are implicitly solving for the values of cx, cy, R
	// for which the corresponding circle most closely fits the weighted data. We perform a mean shift on the data for
	// numerical conditioning and stability reasons.

	// Error checking
	size_t N = P.size();
	if(N != W.size())
	{
		centre.setZero();
		radius = 0.0;
		return;
	}

	// Declare variables
	Eigen::MatrixXd A(N, 3);
	Eigen::VectorXd b(N);

	// Calculate the mean of the data points
	Eigen::Vector2d mean = Eigen::Vector2d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Ax = b)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector2d p = P[i] - mean;
		double w = W[i];
		A(i, 0) = w*p.x();
		A(i, 1) = w*p.y();
		A(i, 2) = w;
		b(i) = w*p.squaredNorm();
	}

	// Call the worker function to fit the circle
	fitCircle(A, b, centre, radius);

	// Correct for the applied mean shift
	centre += mean;
}

// Fit a weighted circle of known centre to 2D data
void ConicFit::fitCircleCentredWeighted(const WeightedPoints2D& P, const Eigen::Vector2d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi,wi). The least squares solution to finding a circle of radius R of known centre
	// (cx,cy) that best fits the data points has a simple algebraic solution, given by:
	//   R = sqrt(sum(wi^2*((xi-cx)^2 + (yi-cy)^2), 1, N) / sum(wi^2, 1, N))
	// In other words, the radius of best fit is the weighted quadratic mean of the distances from the known centre to each point,
	// using weights the squares of those that are passed (to be consistent with the definition of the weights in other fitCircle functions).

	// Declare variables
	size_t N = P.size();

	// Calculate the radius of best fit
	double Rsq = 0.0, wsqsum = 0.0;
	for(size_t i = 0; i < N; i++)
	{
		double w = P[i].z();
		double wsq = w*w;
		Rsq += wsq*((P[i].head<2>() - centre).squaredNorm());
		wsqsum += wsq;
	}
	if(wsqsum > 0.0)
		radius = sqrt(Rsq / wsqsum);
	else
		radius = 0.0;
}

// Fit a weighted circle of known centre to 2D data
void ConicFit::fitCircleCentredWeighted(const Points2D& P, const Weights& W, const Eigen::Vector2d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi,wi). The least squares solution to finding a circle of radius R of known centre
	// (cx,cy) that best fits the data points has a simple algebraic solution, given by:
	//   R = sqrt(sum(wi^2*((xi-cx)^2 + (yi-cy)^2), 1, N) / sum(wi^2, 1, N))
	// In other words, the radius of best fit is the weighted quadratic mean of the distances from the known centre to each point,
	// using weights the squares of those that are passed (to be consistent with the definition of the weights in other fitCircle functions).

	// Error checking
	size_t N = P.size();
	if(N != W.size())
	{
		radius = 0.0;
		return;
	}

	// Calculate the radius of best fit
	double Rsq = 0.0, wsqsum = 0.0;
	for(size_t i = 0; i < N; i++)
	{
		double w = W[i];
		double wsq = w*w;
		Rsq += wsq*((P[i] - centre).squaredNorm());
		wsqsum += wsq;
	}
	if(wsqsum > 0.0)
		radius = sqrt(Rsq / wsqsum);
	else
		radius = 0.0;
}

// Calculate the weighted fitting error of a circle to certain 2D data (the returned error is the weighted average distance of the data points to the circle in units of circle radii)
double ConicFit::fitCircleErrorWeighted(const WeightedPoints2D& P, const Eigen::Vector2d& centre, double radius)
{
	// Calculate and return the required dimensionless fitting error
	size_t N = P.size();
	if(N <= 0) return 0.0;
	radius = fabs(radius);
	double err = 0.0, wsum = 0.0;
	for(size_t i = 0; i < N; i++)
	{
		double w = fabs(P[i].z());
		err += w*fabs((P[i].head<2>() - centre).norm() - radius);
		wsum += w;
	}
	if(wsum == 0.0)
		err = 0.0;
	else
	{
		if(radius != 0.0)
			err /= radius;
		err /= wsum;
	}
	return err;
}

// Calculate the weighted fitting error of a circle to certain 2D data (the returned error is the weighted average distance of the data points to the circle in units of circle radii)
double ConicFit::fitCircleErrorWeighted(const Points2D& P, const Weights& W, const Eigen::Vector2d& centre, double radius)
{
	// Calculate and return the required dimensionless fitting error
	size_t N = P.size();
	if(N != W.size()) return 1e16;
	if(N <= 0) return 0.0;
	radius = fabs(radius);
	double err = 0.0, wsum = 0.0;
	for(size_t i = 0; i < N; i++)
	{
		double w = fabs(W[i]);
		err += w*fabs((P[i] - centre).norm() - radius);
		wsum += w;
	}
	if(wsum == 0.0)
		err = 0.0;
	else
	{
		if(radius != 0.0)
			err /= radius;
		err /= wsum;
	}
	return err;
}

//
// Ellipse fitting
//

// Fit an ellipse to 2D data
void ConicFit::fitEllipse(const Points2D& P, Eigen::Vector2d& centre, Eigen::Matrix2d& coeff)
{
	// We first compute the mean point (xm,ym) of the data points, and bias the data points by the negative of this, resulting in zero mean data.
	// Suppose that the zero mean set of points is (xi,yi). We then fit the following equation by least squares:
	//   ax^2 + by^2 + 2cxy + 2dx + 2ey = 1
	// We do this by expressing the least squares fitting problem as the matrix equation Dv = y, where:
	//   D = N rows of [xi^2 yi^2 2*xi*yi 2*xi 2*yi]    {Nx5 matrix}
	//   v = [a; b; c; d; e]                            {5x1 column vector}
	//   y = N rows of [1]                              {Nx1 column vector}
	// The centre of the ellipse is at the point where the gradient of ax^2 + by^2 + 2cxy + 2dx + 2ey is zero. Therefore, accounting for (xm,ym):
	//   [cx; cy] = [xm; ym] - [a c; c b] \ [d; e]      {Matlab style}
	// (cx,cy) is the centre of the ellipse of best fit through the original data points. The coefficient matrix is then computed by fitting an
	// ellipse of known centre (cx,cy) through the data points. If A is the resulting coefficient matrix, the ellipse of best fit is:
	//   (x-c)'A(x-c) = 1
	// where x = [x; y] and c = [cx; cy] are column vectors.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd D(N, 5);
	Eigen::VectorXd y(N);

	// Calculate the mean of the data points
	Eigen::Vector2d mean = Eigen::Vector2d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Dv = y)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector2d p = P[i] - mean;
		D(i, 0) = p.x()*p.x();
		D(i, 1) = p.y()*p.y();
		D(i, 2) = 2.0*p.x()*p.y();
		D(i, 3) = 2.0*p.x();
		D(i, 4) = 2.0*p.y();
		y(i) = 1.0;
	}

	// Compute the centre of the ellipse of best fit
	fitEllipse(mean, D, y, centre);

	// Compute the coefficient matrix of the ellipse of best fit through the nominated centre
	fitEllipseCentred(P, centre, coeff);
}

// Fit an ellipse to the XY components of 3D data
void ConicFit::fitEllipse(const Points3D& P, Eigen::Vector3d& centre, Eigen::Matrix2d& coeff)
{
	// We discard the z coordinates and fit an ellipse as normal for 2D data.
	// The returned centre z coordinate is a plain mean of the input z coordinates.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd D(N, 5);
	Eigen::VectorXd y(N);

	// Calculate the mean of the data points
	Eigen::Vector3d mean = Eigen::Vector3d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Dv = y)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector3d p = P[i] - mean;
		D(i, 0) = p.x()*p.x();
		D(i, 1) = p.y()*p.y();
		D(i, 2) = 2.0*p.x()*p.y();
		D(i, 3) = 2.0*p.x();
		D(i, 4) = 2.0*p.y();
		y(i) = 1.0;
	}

	// Compute the centre of the ellipse of best fit
	Eigen::Vector2d centre2D;
	fitEllipse(mean.head<2>(), D, y, centre2D);
	centre.x() = centre2D.x();
	centre.y() = centre2D.y();
	centre.z() = mean.z();

	// Compute the coefficient matrix of the ellipse of best fit through the nominated centre
	fitEllipseCentred(P, centre, coeff);
}

// Worker function for fitting an ellipse
void ConicFit::fitEllipse(const Eigen::Vector2d& mean, const Eigen::MatrixXd& D, const Eigen::VectorXd& y, Eigen::Vector2d& centre)
{
	// Find the least squares solution to Dv = y using Singular Value Decomposition (linear least squares fitting)
	Eigen::VectorXd vhat = D.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);

	// Construct the coefficient matrix and vector
	Eigen::Matrix2d A;
	A << vhat(0), vhat(2),
	     vhat(2), vhat(1);
	Eigen::Vector2d b(vhat(3), vhat(4));

	// Calculate the centre of the fitted ellipse
	centre = mean - A.colPivHouseholderQr().solve(b);
}

// Fit an ellipse of known centre to 2D data
void ConicFit::fitEllipseCentred(const Points2D& P, const Eigen::Vector2d& centre, Eigen::Matrix2d& coeff)
{
	// We first bias the data to be relative to the centre (cx,cy) of the ellipse to fit. We then fit the following equation by least squares:
	//   ax^2 + by^2 + 2cxy = 1
	// If we define A = [a c; c b] as the coefficient matrix, and let xc = [x; y] then this is equivalent to:
	//   xc'*A*xc = 1
	// Suppose that the i-th data point of biased data is (xi,yi). We perform the ellipse fit by expressing the least squares fitting problem
	// as the matrix equation Dv = y, where:
	//   D = N rows of [xi^2 yi^2 2*xi*yi]    {Nx3 matrix}
	//   v = [a; b; c]                        {3x1 column vector}
	//   y = N rows of [1]                    {Nx1 column vector}
	// The coefficient matrix A can be constructed from the solution v, after which the equation of the ellipse of best fit through the
	// original data becomes:
	//   (x-c)'A(x-c) = 1
	// where x = [x; y] and c = [cx; cy] are column vectors, and c is the specified centre of the ellipse.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd D(N, 3);
	Eigen::VectorXd y(N);

	// Construct the linear equation to solve (Dv = y)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector2d p = P[i] - centre;
		D(i, 0) = p.x()*p.x();
		D(i, 1) = p.y()*p.y();
		D(i, 2) = 2.0*p.x()*p.y();
		y(i) = 1.0;
	}

	// Compute the coefficient matrix of the ellipse of best fit through the nominated centre
	fitEllipseCentred(D, y, coeff);
}

// Fit an ellipse of known centre to the XY components of 3D data
void ConicFit::fitEllipseCentred(const Points3D& P, const Eigen::Vector3d& centre, Eigen::Matrix2d& coeff)
{
	// We discard the z coordinates and fit an ellipse of known centre as normal for 2D data.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd D(N, 3);
	Eigen::VectorXd y(N);

	// Construct the linear equation to solve (Dv = y)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector3d p = P[i] - centre;
		D(i, 0) = p.x()*p.x();
		D(i, 1) = p.y()*p.y();
		D(i, 2) = 2.0*p.x()*p.y();
		y(i) = 1.0;
	}

	// Compute the coefficient matrix of the ellipse of best fit through the nominated centre
	fitEllipseCentred(D, y, coeff);
}

// Worker function for fitting an ellipse with a known centre
void ConicFit::fitEllipseCentred(const Eigen::MatrixXd& D, const Eigen::VectorXd& y, Eigen::Matrix2d& coeff)
{
	// Find the least squares solution to Dv = y using Singular Value Decomposition (linear least squares fitting)
	Eigen::VectorXd vhat = D.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);

	// Construct the coefficient matrix
	coeff << vhat(0), vhat(2),
	         vhat(2), vhat(1);
}

// Calculate the fitting error of an ellipse to certain 2D data (average distance of normalised points to the unit circle)
double ConicFit::fitEllipseErrorCoeff(const Points2D& P, const Eigen::Vector2d& centre, const Eigen::Matrix2d& coeff)
{
	// Declare variables
	Eigen::Matrix2d rotmat, transform;
	Eigen::Vector2d radii;
	double angle;

	// Calculate the transformation matrix
	double failed = (double) P.size();
	if(!ellipseMatrixToAxes(coeff, rotmat, radii, angle)) return failed;
	if(!ellipseAxesToTransform(rotmat, radii, transform)) return failed;

	// Calculate the fitting error based on the centre and transformation matrix
	return fitEllipseError(P, centre, transform);
}

// Calculate the fitting error of an ellipse to certain 2D data (average distance of normalised points to the unit circle)
double ConicFit::fitEllipseError(const Points2D& P, const Eigen::Vector2d& centre, const Eigen::Matrix2d& transform)
{
	// Calculate and return the required dimensionless fitting error
	double err = 0.0;
	size_t N = P.size();
	for(size_t i = 0; i < N; i++)
		err += fabs((transform*(P[i] - centre)).norm() - 1.0)/N;
	return err;
}

// Calculate the fitting error of an ellipse to certain 3D data (average distance of normalised points to the unit circle)
double ConicFit::fitEllipseErrorCoeff(const Points3D& P, const Eigen::Vector3d& centre, const Eigen::Matrix2d& coeff)
{
	// Declare variables
	Eigen::Matrix2d rotmat, transform;
	Eigen::Vector2d radii;
	double angle;

	// Calculate the transformation matrix
	double failed = (double) P.size();
	if(!ellipseMatrixToAxes(coeff, rotmat, radii, angle)) return failed;
	if(!ellipseAxesToTransform(rotmat, radii, transform)) return failed;

	// Calculate the fitting error based on the centre and transformation matrix
	return fitEllipseError(P, centre, transform);
}

// Calculate the fitting error of an ellipse to certain 3D data (average distance of normalised points to the unit circle)
double ConicFit::fitEllipseError(const Points3D& P, const Eigen::Vector3d& centre, const Eigen::Matrix2d& transform)
{
	// Calculate and return the required dimensionless fitting error
	double err = 0.0;
	size_t N = P.size();
	Eigen::Vector2d centre2D = centre.head<2>();
	for(size_t i = 0; i < N; i++)
		err += fabs((transform*(P[i].head<2>() - centre2D)).norm() - 1.0)/N;
	return err;
}

//
// Sphere fitting
//

// Fit a sphere to 3D data
void ConicFit::fitSphere(const Points3D& P, Eigen::Vector3d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi,zi). We try to fit a sphere of radius R and centre (cx,cy,cz) to the data. Let:
	//   A = N rows of [xi yi zi 1]                     {Nx4 matrix}
	//   x = [2*cx; 2*cy; 2*cz; R^2-cx^2-cy^2-cz^2]     {4x1 column vector}
	//   b = N rows of [xi^2 + yi^2 + zi^2]             {Nx1 column vector}
	// Ax = b is equivalent to the condition that all data points lie on a sphere of radius R, centered at (cx,cy,cz). Hence,
	// by solving Ax = b for x in a least squares sense, we are implicitly solving for the values of cx, cy, cz and R for
	// which the corresponding sphere most closely fits the data. We perform a mean shift on the data for numerical
	// conditioning and stability reasons.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd A(N, 4);
	Eigen::VectorXd b(N);

	// Calculate the mean of the data points
	Eigen::Vector3d mean = Eigen::Vector3d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Ax = b)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector3d p = P[i] - mean;
		A(i, 0) = p.x();
		A(i, 1) = p.y();
		A(i, 2) = p.z();
		A(i, 3) = 1;
		b(i) = p.squaredNorm();
	}

	// Find the least squares solution to Ax = b using Singular Value Decomposition (linear least squares fitting)
	Eigen::VectorXd coefficients = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

	// Extract the centre and radius of the fitted sphere
	centre = 0.5*coefficients.head<3>();
	radius = sqrt(coefficients(3) + centre.squaredNorm());

	// Correct for the applied mean shift
	centre += mean;
}

// Fit a sphere of known centre to 3D data
void ConicFit::fitSphereCentred(const Points3D& P, const Eigen::Vector3d& centre, double& radius)
{
	// Suppose the i-th data point is (xi,yi,zi). The least squares solution to finding a sphere of radius R of known
	// centre (cx,cy,cz) that best fits the data points has a simple algebraic solution, given by:
	//   R = sqrt(sum((xi-cx)^2 + (yi-cy)^2 + (zi-cz)^2, 1, N) / N)
	// In other words, the radius of best fit is the quadratic mean of the distances from the known centre to each point.

	// Declare variables
	size_t N = P.size();

	// Calculate the radius of best fit
	double Rsq = 0.0;
	for(size_t i = 0; i < N; i++)
		Rsq += (P[i] - centre).squaredNorm() / N;
	radius = sqrt(Rsq);
}

// Calculate the fitting error of a sphere to certain 3D data (average distance to the sphere in units of sphere radii)
double ConicFit::fitSphereError(const Points3D& P, const Eigen::Vector3d& centre, double radius)
{
	// Calculate and return the required dimensionless fitting error
	double err = 0.0;
	size_t N = P.size();
	if(radius != 0.0)
	{
		for(size_t i = 0; i < N; i++)
			err += fabs((P[i] - centre).norm()/radius - 1.0)/N;
	}
	else
	{
		for(size_t i = 0; i < N; i++)
			err += fabs((P[i] - centre).norm())/N;
	}
	return err;
}

//
// Ellipsoid fitting
//

// Fit an ellipsoid to 3D data
void ConicFit::fitEllipsoid(const Points3D& P, Eigen::Vector3d& centre, Eigen::Matrix3d& coeff)
{
	// We first compute the mean point (xm,ym,zm) of the data points, and bias the data points by the negative of this, resulting in zero mean data.
	// Suppose that the zero mean set of points is (xi,yi,zi). We then fit the following equation by least squares:
	//   ax^2 + by^2 + cz^2 + 2dxy + 2exz + 2fyz + 2gx + 2hy + 2iz = 1
	// We do this by expressing the least squares fitting problem as the matrix equation Dv = y, where:
	//   D = N rows of [xi^2 yi^2 zi^2 2*xi*yi 2*xi*zi 2*yi*zi 2*xi 2*yi 2*zi]    {Nx9 matrix}
	//   v = [a; b; c; d; e; f; g; h; i]                                          {9x1 column vector}
	//   y = N rows of [1]                                                        {Nx1 column vector}
	// The centre of the ellipsoid is at the point where the gradient of the defining function is zero. Therefore, accounting for (xm,ym,zm):
	//   [cx; cy; cz] = [xm; ym; zm] - [a d e; d b f; e f c] \ [g; h; i]          {Matlab style}
	// (cx,cy,cz) is the centre of the ellipsoid of best fit through the original data points. The coefficient matrix is then computed by fitting an
	// ellipsoid of known centre (cx,cy,cz) through the data points. If A is the resulting coefficient matrix, the ellipsoid of best fit is:
	//   (x-c)'A(x-c) = 1
	// where x = [x; y; z] and c = [cx; cy; cz] are column vectors.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd D(N, 9);
	Eigen::VectorXd y(N);

	// Calculate the mean of the data points
	Eigen::Vector3d mean = Eigen::Vector3d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the linear equation to solve (Dv = y)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector3d p = P[i] - mean;
		D(i, 0) = p.x()*p.x();
		D(i, 1) = p.y()*p.y();
		D(i, 2) = p.z()*p.z();
		D(i, 3) = 2.0*p.x()*p.y();
		D(i, 4) = 2.0*p.x()*p.z();
		D(i, 5) = 2.0*p.y()*p.z();
		D(i, 6) = 2.0*p.x();
		D(i, 7) = 2.0*p.y();
		D(i, 8) = 2.0*p.z();
		y(i) = 1.0;
	}

	// Find the least squares solution to Dv = y using Singular Value Decomposition (linear least squares fitting)
	Eigen::VectorXd vhat = D.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);

	// Construct the coefficient matrix and vector
	Eigen::Matrix3d A;
	A << vhat(0), vhat(3), vhat(4),
	     vhat(3), vhat(1), vhat(5),
	     vhat(4), vhat(5), vhat(2);
	Eigen::Vector3d b(vhat(6), vhat(7), vhat(8));

	// Calculate the centre of the fitted ellipse
	centre = mean - A.colPivHouseholderQr().solve(b);

	// Compute the coefficient matrix of the ellipsoid of best fit through the nominated centre
	fitEllipsoidCentred(P, centre, coeff);
}

// Fit an ellipsoid of known centre to 3D data
void ConicFit::fitEllipsoidCentred(const Points3D& P, const Eigen::Vector3d& centre, Eigen::Matrix3d& coeff)
{
	// We first bias the data to be relative to the centre (cx,cy) of the ellipsoid to fit. We then fit the following equation by least squares:
	//   ax^2 + by^2 + cz^2 + 2dxy + 2exz + 2fyz = 1
	// If we define A = [a d e; d b f; e f c] as the coefficient matrix, and let xc = [x; y; z] then this is equivalent to:
	//   xc'*A*xc = 1
	// Suppose that the i-th data point of biased data is (xi,yi,zi). We perform the ellipsoid fit by expressing the least squares fitting problem
	// as the matrix equation Dv = y, where:
	//   D = N rows of [xi^2 yi^2 zi^2 2*xi*yi 2*xi*zi 2*yi*zi]    {Nx6 matrix}
	//   v = [a; b; c; d; e; f]                                    {6x1 column vector}
	//   y = N rows of [1]                                         {Nx1 column vector}
	// The coefficient matrix A can be constructed from the solution v, after which the equation of the ellipsoid of best fit through the original
	// data becomes:
	//   (x-c)'A(x-c) = 1
	// where x = [x; y; z] and c = [cx; cy; cz] are column vectors, and c is the specified centre of the ellipsoid.

	// Declare variables
	size_t N = P.size();
	Eigen::MatrixXd D(N, 6);
	Eigen::VectorXd y(N);

	// Construct the linear equation to solve (Dv = y)
	for(size_t i = 0; i < N; i++)
	{
		const Eigen::Vector3d p = P[i] - centre;
		D(i, 0) = p.x()*p.x();
		D(i, 1) = p.y()*p.y();
		D(i, 2) = p.z()*p.z();
		D(i, 3) = 2.0*p.x()*p.y();
		D(i, 4) = 2.0*p.x()*p.z();
		D(i, 5) = 2.0*p.y()*p.z();
		y(i) = 1.0;
	}

	// Find the least squares solution to Dv = y using Singular Value Decomposition (linear least squares fitting)
	Eigen::VectorXd vhat = D.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);

	// Construct the coefficient matrix
	coeff << vhat(0), vhat(3), vhat(4),
	         vhat(3), vhat(1), vhat(5),
	         vhat(4), vhat(5), vhat(2);
}

// Calculate the fitting error of an ellipsoid to certain 3D data (average distance of normalised points to the unit sphere)
double ConicFit::fitEllipsoidErrorCoeff(const Points3D& P, const Eigen::Vector3d& centre, const Eigen::Matrix3d& coeff)
{
	// Declare variables
	Eigen::Matrix3d rotmat, transform;
	Eigen::Vector3d radii;

	// Calculate the transformation matrix
	double failed = (double) P.size();
	if(!ellipsoidMatrixToAxes(coeff, rotmat, radii)) return failed;
	if(!ellipsoidAxesToTransform(rotmat, radii, transform)) return failed;

	// Calculate the fitting error based on the centre and transformation matrix
	return fitEllipsoidError(P, centre, transform);
}

// Calculate the fitting error of an ellipsoid to certain 3D data (average distance of normalised points to the unit sphere)
double ConicFit::fitEllipsoidError(const Points3D& P, const Eigen::Vector3d& centre, const Eigen::Matrix3d& transform)
{
	// Calculate and return the required dimensionless fitting error
	double err = 0.0;
	size_t N = P.size();
	for(size_t i = 0; i < N; i++)
		err += fabs((transform*(P[i] - centre)).norm() - 1.0)/N;
	return err;
}

//
// Ellipse functions
//

// Calculate the rotation matrix, radii and rotation angle of an ellipse defined by a coefficient matrix
bool ConicFit::ellipseMatrixToAxes(const Eigen::Matrix2d& coeff, Eigen::Matrix2d& rotmat, Eigen::Vector2d& radii, double& angle)
{
	// If the input coefficient matrix (coeff) is A, the output rotation matrix (rotmat) is Q, and the output radii (radii) are R,
	// then if we let D = diag(1./R.^2), the outputs of this function are constructed so that A = QDQ', Q^-1 = Q', |Q| = 1 and R > 0.
	// The angle is expressed as the magnitude of the rotation Q as a counterclockwise rotation.

	// Ensure the coefficient matrix is symmetric (trust the lower triangle)
	Eigen::Matrix2d A = coeff;
	A(0,1) = A(1,0);

	// Compute the eigendecomposition of the coefficient matrix (the solver assumes the matrix is symmetric and trusts the lower triangle)
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> ES(A);

	// Retrieve the eigenvalues
	Eigen::Vector2d lambda = ES.eigenvalues();

	// Check and ensure that the eigenvalues are not negative
	for(size_t i = 0; i < 2; i++)
	{
		if(lambda(i) < -ZeroTol) return false;
		if(lambda(i) < 0.0) lambda(i) = 0.0;
	}

	// Retrieve the eigenvectors (columns of the matrix Q)
	Eigen::Matrix2d Q = ES.eigenvectors();

	// Ensure that the eigenvectors are unit norm
	for(size_t i = 0; i < 2; i++)
	{
		double norm = Q.col(i).norm();
		if(norm < ZeroTol) return false;
		Q.col(i) /= norm;
	}

	// Enforce the right hand rule for the eigenvectors
	if(Q.determinant() < 0.0)
		Q.col(1) = -Q.col(1);

	// Transcribe the rotation matrix of the ellipse
	rotmat = Q;

	// Calculate the radii of the ellipse
	radii.x() = 1.0 / sqrt(lambda.x());
	radii.y() = 1.0 / sqrt(lambda.y());

	// Calculate the angle of the ellipse
	angle = atan2(rotmat(1,0) + rotmat(1,1), rotmat(0,0) + rotmat(0,1)) - M_PI/4.0;
	if(angle <= -M_PI)
		angle += 2.0*M_PI;

	// Return success
	return true;
}

// Calculate the normalisation transformation matrix of an ellipse defined by its rotation matrix and radii
bool ConicFit::ellipseAxesToTransform(const Eigen::Matrix2d& rotmat, const Eigen::Vector2d& radii, Eigen::Matrix2d& transform)
{
	// If the input rotation matrix (rotmat) is Q, the input radii (radii) are R, and the output normalisation matrix (transform) is W,
	// then the corresponding coefficient matrix is A = QDQ' where D = diag(1./R.^2). The normalisation matrix is then the positive
	// definite symmetric solution to W*W = A (i.e. W = sqrtm(A)).

	// Check that none of the radii are negative or zero
	if(radii.x() <= 0.0 || radii.y() <= 0.0) return false;

	// Construct the required normalisation transformation matrix
	Eigen::Vector2d rootLambda(1.0/radii.x(), 1.0/radii.y());
	transform = rotmat * rootLambda.asDiagonal() * rotmat.inverse();

	// Return success
	return true;
}

//
// Ellipsoid functions
//

// Calculate the rotation matrix and radii of an ellipsoid defined by a coefficient matrix
bool ConicFit::ellipsoidMatrixToAxes(const Eigen::Matrix3d& coeff, Eigen::Matrix3d& rotmat, Eigen::Vector3d& radii)
{
	// If the input coefficient matrix (coeff) is A, the output rotation matrix (rotmat) is Q, and the output radii (radii) are R,
	// then if we let D = diag(1./R.^2), the outputs of this function are constructed so that A = QDQ', Q^-1 = Q', |Q| = 1 and R > 0.

	// Ensure the coefficient matrix is symmetric (trust the lower triangle)
	Eigen::Matrix3d A = coeff;
	A(0,1) = A(1,0);
	A(0,2) = A(2,0);
	A(1,2) = A(2,1);

	// Compute the eigendecomposition of the coefficient matrix (the solver assumes the matrix is symmetric and trusts the lower triangle)
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ES(A);

	// Retrieve the eigenvalues
	Eigen::Vector3d lambda = ES.eigenvalues();

	// Check and ensure that the eigenvalues are not negative
	for(size_t i = 0; i < 3; i++)
	{
		if(lambda(i) < -ZeroTol) return false;
		if(lambda(i) < 0.0) lambda(i) = 0.0;
	}

	// Retrieve the eigenvectors (columns of the matrix Q)
	Eigen::Matrix3d Q = ES.eigenvectors();

	// Ensure that the eigenvectors are unit norm
	for(size_t i = 0; i < 3; i++)
	{
		double norm = Q.col(i).norm();
		if(norm < ZeroTol) return false;
		Q.col(i) /= norm;
	}

	// Enforce the right hand rule for the eigenvectors
	if(Q.determinant() < 0.0)
		Q.col(2) = -Q.col(2);

	// Transcribe the rotation matrix of the ellipsoid
	rotmat = Q;

	// Calculate the radii of the ellipsoid
	radii.x() = 1.0 / sqrt(lambda.x());
	radii.y() = 1.0 / sqrt(lambda.y());
	radii.z() = 1.0 / sqrt(lambda.z());

	// Return success
	return true;
}

// Calculate the normalisation transformation matrix of an ellipsoid defined by its rotation matrix and radii
bool ConicFit::ellipsoidAxesToTransform(const Eigen::Matrix3d& rotmat, const Eigen::Vector3d& radii, Eigen::Matrix3d& transform)
{
	// If the input rotation matrix (rotmat) is Q, the input radii (radii) are R, and the output normalisation matrix (transform) is W,
	// then the corresponding coefficient matrix is A = QDQ' where D = diag(1./R.^2). The normalisation matrix is then the positive
	// definite symmetric solution to W*W = A (i.e. W = sqrtm(A)).

	// Check that none of the radii are negative or zero
	if(radii.x() <= 0.0 || radii.y() <= 0.0 || radii.z() <= 0.0) return false;

	// Construct the required normalisation transformation matrix
	Eigen::Vector3d rootLambda(1.0/radii.x(), 1.0/radii.y(), 1.0/radii.z());
	transform = rotmat * rootLambda.asDiagonal() * rotmat.inverse();

	// Return success
	return true;
}

//
// Conic data generation functions
//

// Generate circle data (returns N points)
void ConicFit::genCircleData(ConicFit::Points2D& P, size_t N, const Eigen::Vector2d& centre, double radius)
{
	// Generate the required data
	P.clear();
	if(N < 3) N = 3;
	Eigen::Vector2d tmp;
	for(size_t i = 0; i < N; i++)
	{
		double t = i * (4.0*M_PI / (N - 1));
		tmp << radius*cos(t), radius*sin(t);
		tmp += centre;
		P.push_back(tmp);
	}
}

// Generate ellipse data (returns N points)
void ConicFit::genEllipseData(ConicFit::Points2D& P, size_t N, const Eigen::Vector2d& centre, const Eigen::Vector2d& radii, double angle)
{
	// Generate the required data
	P.clear();
	if(N < 3) N = 3;
	Eigen::Vector2d tmp;
	Eigen::Matrix2d rotmat;
	double cangle = cos(angle);
	double sangle = sin(angle);
	rotmat << cangle, -sangle, sangle, cangle;
	for(size_t i = 0; i < N; i++)
	{
		double t = i * (4.0*M_PI / (N - 1));
		tmp << radii.x()*cos(t), radii.y()*sin(t);
		tmp = rotmat * tmp;
		tmp += centre;
		P.push_back(tmp);
	}
}

// Generate sphere data (returns N^2 points)
void ConicFit::genSphereData(ConicFit::Points3D& PP, size_t N, const Eigen::Vector3d& centre, double radius)
{
	// Generate the required data
	PP.clear();
	if(N < 3) N = 3;
	Eigen::Vector3d tmp;
	for(size_t i = 0; i < N; i++)
	{
		double u = i * (2.0*M_PI / (N - 1));
		for(size_t j = 0; j < N; j++)
		{
			double v = j * (M_PI / (N - 1));
			tmp << radius*cos(u)*sin(v), radius*sin(u)*sin(v), radius*cos(v);
			tmp += centre;
			PP.push_back(tmp);
		}
	}
}

// Generate ellipsoid data (returns N^2 points)
void ConicFit::genEllipsoidData(ConicFit::Points3D& PP, size_t N, const Eigen::Vector3d& centre, const Eigen::Vector3d& radii, const Eigen::Matrix3d& rotmat)
{
	// Generate the required data
	PP.clear();
	if(N < 3) N = 3;
	Eigen::Vector3d tmp;
	for(size_t i = 0; i < N; i++)
	{
		double u = i * (2.0*M_PI / (N - 1));
		for(size_t j = 0; j < N; j++)
		{
			double v = j * (M_PI / (N - 1));
			tmp << radii.x()*cos(u)*sin(v), radii.y()*sin(u)*sin(v), radii.z()*cos(v);
			tmp = rotmat * tmp;
			tmp += centre;
			PP.push_back(tmp);
		}
	}
}
// EOF


/**
 *  @file eigen_quaternion.cpp
 *  @author Maximilian Harr <maximilian.harr@daimler.com>
 *  @date 17.06.2017
 *
 *  @brief Eigen quaternion computations
 *
 *
 *
 *          Coding Standard:
 *          wiki.ros.org/CppStyleGuide
 *          https://google.github.io/styleguide/cppguide.html
 *
 *
 *  @bug
 *
 *
 *  @todo
 *
 *
 */

// PRAGMA

// SYSTEM INCLUDES
#include <iostream>  /* Header that defines the standard input/output stream objects */
#include <cstdlib>  /* Header that defines several general purpose functions */
#include <string>
#include <Eigen/Dense>  /* template library for linear algebra http://eigen.tuxfamily.org */
#include <math.h> /* Defines M_PI for pi value */
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

// PROJECT INCLUDES

// LOCAL INCLUDES

// FORWARD REFERENCES

// FUNCTION PROTOTYPES

/** @brief Standard command line parameter processing.
 *  @param Pass command line parameters
 *  @return 0 if -h flag is set
 */
int cmd_check(int, char*[]);

// GLOBAL VARIABLES

using namespace Eigen;
using namespace std;

void outputAsMatrix(const Eigen::Quaterniond& q)
{
    std::cout << "R=" << std::endl << q.normalized().toRotationMatrix() << std::endl;
}

//// MAIN //////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
  /* Check command line options. Stop execution if -h is set. */
  if(!cmd_check(argc,argv)) return 0;

  /* Specify roation axis and angle */
  Eigen::Vector3d rotation_axis{0, 0, 1};
  Eigen::Vector3d pose{0, 0, 1};
  double angle = 0;
  if(argc == 2){
    angle = atof(argv[1]);}
  else{
    angle = M_PI/8.0;
  }

  /* Compute Quaternion by hand */
  auto sinA = std::sin(angle / 2);
  auto cosA = std::cos(angle / 2);
  Eigen::Quaterniond q;
  q.x() = rotation_axis(0) * sinA;
  q.y() = rotation_axis(1) * sinA;
  q.z() = rotation_axis(2) * sinA;
  q.w() = cosA;

  /* Let Eigen compute Quaternion (use Vector3d::UnitZ() instead of rotation_axis) */
  Eigen::Quaterniond q2 = Eigen::Quaterniond{ Eigen::AngleAxisd{ angle, rotation_axis } };

  /* Compare rotation matrices */
  outputAsMatrix( q  );
  outputAsMatrix( q2 );

  /* Get rotation matrix from quaternion */
  Matrix3d m;
  m = Eigen::Quaterniond(q2);
  std::cout << "Quaternion q2: " << std::endl << q2.x() << std::endl
            << q2.y() << std::endl << q2.z() << std::endl << q2.w() << std::endl;
  std::cout << "Rotation matrix m of Quaternion q2: " << std::endl << m << std::endl;

  /* Multiplication of quaternions */
  Matrix3d m2;
  m2 = Eigen::Quaterniond(q2*q2);
  std::cout << "m*m: " << std::endl << m*m << std::endl;
  std::cout << "Rotation matrix of q2*q2: " << std::endl << m2 << std::endl;

  /* Dividing Quaternions */
  Matrix3d m3;
  m3 = Eigen::Quaterniond(q2.inverse());
  std::cout << "m.inverse(): " << std::endl << m.inverse() << std::endl;
  std::cout << "Rotation matrix of q2.inverse(): " << std::endl << m3 << std::endl;

  /* Rotate Vector */
  Eigen::Vector3d x_axis = Eigen::Vector3d::UnitX();
  std::cout << "q2*x_axis: " << std::endl << q2*x_axis << std::endl;

  /* Get rotation axis of Quaternion */
  Eigen::AngleAxisd rot_axis_m(m);
  Eigen::AngleAxisd rot_axis_q2(q2);
  rot_axis_m.axis().cross(rot_axis_q2.axis());
  std::cout << "Rotation axis of m: " << std::endl << rot_axis_m.axis() << std::endl;
  std::cout << "Rotation axis of q2: " << std::endl << rot_axis_q2.axis() << std::endl;

  /* Get Euler Angles from Quaternion */
  Eigen::Vector3d euler = q2.toRotationMatrix().eulerAngles(0,1,2);
  std::cout << "Euler angles yf q2: " << std::endl << euler << std::endl;

  /* Get Quaternion from euler angles */
  Eigen::Quaterniond q3;
  q3 = AngleAxisd(euler[0], Eigen::Vector3d::UnitX())*
      AngleAxisd(euler[1], Eigen::Vector3d::UnitY())*
      AngleAxisd(euler[2], Eigen::Vector3d::UnitZ());
  std::cout << "q3 from euler angles: " << std::endl << q3.x() << std::endl
            << q3.y() << std::endl << q3.z() << std::endl << q3.w() << std::endl;

  /* Rotate covariance matrix */
  Eigen::MatrixXd covariance_q = Eigen::MatrixXd::Zero(3,3);
  Eigen::Quaterniond q4 = Eigen::Quaterniond{ Eigen::AngleAxisd{ angle, Eigen::Vector3d::UnitZ() } };
  Eigen::MatrixXd covariance_r = Eigen::MatrixXd::Zero(3,3);
  Matrix3d rotation;
  rotation = Eigen::AngleAxisd{ Eigen::AngleAxisd( angle, Eigen::Vector3d::UnitZ()) };

  covariance_q(0,0) = 1;
  covariance_q(1,1) = 2;
  covariance_q(2,2) = 3;
  covariance_r(0,0) = 1;
  covariance_r(1,1) = 2;
  covariance_r(2,2) = 3;

  covariance_q = q * covariance_q; // Rotate covariance matrix in lane coordinates
  covariance_q = covariance_q*covariance_q.transpose();
  covariance_r = rotation*covariance_r*covariance_r.transpose()*rotation.transpose();

  std::cout << "covariance_q:\n " << covariance_q << std::endl;
  std::cout << "covariance_r:\n " << covariance_r << std::endl;

}

//// FUNCTION DEFINITIONS //////////////////////////////////////////////////////////////////////////
int cmd_check(int argc, char* argv[])
{
  int option;
  /* third argument of getopt specifies valid options and whether they need input(:) */
  while((option = getopt(argc,argv,"hp:"))>=0)
  {
    switch (option)
    {
      case 'h': std::cout
                << "Usage: <filename> [options] \n\n"
                << "<desription> \n\n"
                << "Options: \n"
                << " -h                    show this help message and exit \n"
                << " -p <PARAMETER>        <description> \n"
                << " \n";
                return 0; /* do not execute main with this option */
      case 'p': std::cout << "-p = " << optarg << "\n"; /* optarg is option argument */
                break;
    }
  }
  return 1;
}


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

using namespace std;
using namespace Eigen;

int main()
{
   Matrix2f A;
   A << 1, 2, 2, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
   if (eigensolver.info() != Success) abort();
   cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
   cout << "Here's a matrix whose columns are eigenvectors of A \n"
        << "corresponding to these eigenvalues:\n"
        << eigensolver.eigenvectors() << endl;
}


#include "ode45.hpp"

#include <iostream>
#include <iomanip>

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

//! \file stabrk.cpp Solution for Problem 1, PS13, involving ode45 and matrix ODEs

//! \brief Solve matrix IVP Y' = -(Y-Y')*Y using ode45 up to time T
//! \param[in] Y0 Initial data Y(0) (as matrix)
//! \param[in] T final time of simulation
//! \return Matrix of solution of IVP at t = T
Eigen::MatrixXd matode(const Eigen::MatrixXd & Y0, double T) {
    // TODO: evolve Y0 up to T using ode45 (read ode45.hpp)
}

//! \brief Find if invariant is preserved after evolution with matode
//! \param[in] Y0 Initial data Y(0) (as matrix)
//! \param[in] T final time of simulation
//! \return true if invariant was preserved (up to round-off), i.e. if norm was less than 10*eps
bool checkinvariant(const Eigen::MatrixXd & M, double T) {
    // TODO: check if invariant is preserved applying matode
}

//! \brief Implement ONE step of explicit Euler applied to Y0, of ODE Y' = A*Y
//! \param[in] A matrix A of the ODE
//! \param[in] Y0 Initial state
//! \param[in] h step size
//! \return next step
Eigen::MatrixXd expeulstep(const Eigen::MatrixXd & A, const Eigen::MatrixXd & Y0, double h) {
    // TODO: ose step of EE
}

//! \brief Implement ONE step of implicit Euler applied to Y0, of ODE Y' = A*Y
//! \param[in] A matrix A of the ODE
//! \param[in] Y0 Initial state
//! \param[in] h step size
//! \return next step
Eigen::MatrixXd impeulstep(const Eigen::MatrixXd & A, const Eigen::MatrixXd & Y0, double h) {
    // TODO: ose step of IE
}

//! \brief Implement ONE step of implicit midpoint ruler applied to Y0, of ODE Y' = A*Y
//! \param[in] A matrix A of the ODE
//! \param[in] Y0 Initial state
//! \param[in] h step size
//! \return next step
Eigen::MatrixXd impstep(const Eigen::MatrixXd & A, const Eigen::MatrixXd & Y0, double h) {
    // TODO: ose step of IMP
}

int main() {

    double T = 1;
    unsigned int n = 3;

    Eigen::MatrixXd M(n,n);
    M << 8,1,6,3,5,7,4,9,2;

    std::cout << "SUBTASK 1. c)" << std::endl;
    // Test preservation of orthogonality

    // Build Q
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(M.rows(), M.cols());
    qr.compute(M);
    Eigen::MatrixXd Q = qr.householderQ();

    // Build A
    Eigen::MatrixXd A(n,n);
    A << 0, 1, 1, -1, 0, 1, -1, -1, 0;
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n,n);

    // TODO: compute norm of Y'Y-I for 20 steps and print table

    std::cout << "SUBTASK 1. d)" << std::endl;
    // Test implementation of ode45

    // TODO: TEST matode

    std::cout << "SUBTASK 1. g)" << std::endl;
    // Test whether invariant was preserved or not

    // TODO: TEST if matode preserves invariant using checkinvariant

    return 0;
}

Nummerical Methods for CSE/PS13/templates_ps13/matrix_ode_template.cpp