// VMDモーションの補間を行う
#include "interpolate.h"

#include <Eigen/Core>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
#include "VMD.h"

using namespace Eigen;

// x座標値がx_argとなる点をベジェ曲線上から探し、その点のy座標値を返す
// ベジェ曲線の制御点は(0,0), p1, p2, (1,1)とする
// 0 < x_arg < 1 とする
float bezier_y(Vector2f p1, Vector2f p2, float x_arg)
{
  // 二分法でtを探す
  float epsilon = 1.0e-5;
  float lower = 0.0;
  float upper = 1.0;
  float t;
  float x;
  float x1 = p1.x();
  float x2 = p2.x();
  const int max_iteration = 20;
  for (int i = 0; i < max_iteration; i++) {
    t = (lower + upper) / 2;
    x = bezier3(x1, x2, t);
    if (abs(x - x_arg) < epsilon) {
      // x == x_arg となる t が見つかった
      break;
    }
    if (x < x_arg) {
      // x(t)は単調増加するので、
      // 真のxがもっと大きいということは、真のtはもっと大きいので、
      // 探索範囲を上半分に絞る
      lower = t;
    } else {
      upper = t;
    }
  }
  return bezier3(p1.y(), p2.y(), t);
}

float bezier_y_vmd(uint8_t ipx1, uint8_t ipy1, uint8_t ipx2, uint8_t ipy2, float x)
{
  return bezier_y(Vector2f(float(ipx1)/127, float(ipy1)/127), Vector2f(float(ipx2)/127, float(ipy2)/127), x);
}

VMD_Frame make_intermediate_frame(const VMD_Frame& head_frame, const VMD_Frame& tail_frame, float ratio, bool bezier)
{
  VMD_Frame f;
  memcpy(f.bonename, head_frame.bonename, f.bonename_len);
  if (bezier) {
    // ベジェ曲線補間
    const uint8_t* ip = tail_frame.interpolation;
    float y;

    // X座標値の補間
    y = bezier_y_vmd(ip[0], ip[4], ip[8], ip[12], ratio);
    f.position.x() = head_frame.position.x() * (1-y) + tail_frame.position.x() * y;

    // Y座標値の補間
    y = bezier_y_vmd(ip[16], ip[20], ip[24], ip[28], ratio);
    f.position.y() = head_frame.position.y() * (1-y) + tail_frame.position.y() * y;

    // Z座標値の補間
    y = bezier_y_vmd(ip[32], ip[36], ip[40], ip[44], ratio);
    f.position.z() = head_frame.position.z() * (1-y) + tail_frame.position.z() * y;

    // 回転の補間
    y = bezier_y_vmd(ip[48], ip[52], ip[56], ip[60], ratio);
    f.rotation = head_frame.rotation.slerp(y, tail_frame.rotation);
  } else {
    // 線形補間
    f.position = head_frame.position + (tail_frame.position - head_frame.position) * ratio;
    f.rotation = head_frame.rotation.slerp(ratio, tail_frame.rotation);
  }
  return f;
}

// head_frameとtail_frameを元に、補間でframe_num番目のボーンフレームを作る
VMD_Frame interpolate_frame(const VMD_Frame& head_frame, const VMD_Frame& tail_frame, int frame_num, bool bezier)
{
  int total = tail_frame.number - head_frame.number;
  float ratio = float(frame_num - head_frame.number) / total;
  VMD_Frame f = make_intermediate_frame(head_frame, tail_frame, ratio, bezier);
  f.number = frame_num;
  return f;
}

VMD_Morph make_intermediate_morph(const VMD_Morph& head_frame, const VMD_Morph& tail_frame, float ratio)
{
  VMD_Morph m = head_frame;
  m.weight = head_frame.weight + (tail_frame.weight - head_frame.weight) * ratio;
  return m;
}

// head_frameとtail_frameを元に、補間でframe_num番目の表情フレームを作る
VMD_Morph interpolate_morph(const VMD_Morph& head_frame, const VMD_Morph& tail_frame, int frame_num)
{
  int total = tail_frame.frame - head_frame.frame;
  float ratio = float(frame_num - head_frame.frame) / total;
  VMD_Morph m = make_intermediate_morph(head_frame, tail_frame, ratio);
  m.frame = frame_num;
  return m;
}

vector<VMD_Frame> fill_bone_frame(const vector<VMD_Frame>& fv, bool bezier)
{
  vector<VMD_Frame> fv_new;
  VMD_Frame f_old = fv[0];
  f_old.number = 0;
  fv_new.push_back(f_old);
  for (VMD_Frame f : fv) {
    // もしフレーム番号が重複していたら、重複したフレームは消す
    if (f.number == f_old.number) {
      continue;
    }
    // フレーム番号が連続していない場合、途中のフレームを補間する
    for (uint32_t i = f_old.number + 1; i < f.number; i++) {
      VMD_Frame interpolated = interpolate_frame(f_old, f, i, bezier);
      fv_new.push_back(interpolated);
    }
    fv_new.push_back(f);
    f_old = f;
  }
  return fv_new;
}

vector<VMD_Morph> fill_morph_frame(vector<VMD_Morph>& mv)
{
  vector<VMD_Morph> mv_new;
  VMD_Morph m_old = mv[0];
  m_old.frame = 0;
  mv_new.push_back(m_old);
  for (VMD_Morph m : mv) {
    // もしフレーム番号が重複していたら、重複したフレームは消す
    if (m.frame == m_old.frame) {
      continue;
    }
    // フレーム番号が連続していない場合、途中のフレームを補間する
    for (uint32_t i = m_old.frame + 1; i < m.frame; i++) {
      VMD_Morph interpolated = interpolate_morph(m_old, m, i);
      mv_new.push_back(interpolated);
    }
    mv_new.push_back(m);
    m_old = m;
  }
  return mv_new;
}

// Eigen::LevenbergMarquardt で非線形最小二乗法フィッティングに使う構造体
template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
struct Functor
{
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
    };
    typedef Matrix<Scalar, InputsAtCompileTime, 1> InputType;
    typedef Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
    typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

    const int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; }
    int values() const { return m_values; }
};

struct position_functor : Functor<float>
{
  position_functor(int nparam, int nvalue, const vector<float>& x, const vector<float>& y)
  : Functor<float>(nparam, nvalue), x(x), y(y) {}

  const vector<float>& x;
  const vector<float>& y;

  // 各データ点(x[i], y[i])における誤差をf[i]に格納する
  // パラメータpはベジェ曲線の制御点の座標値
  // ベジェ曲線の制御点 = (0, 0), (p[0], p[1]), (p[2], p[3]), (1, 1) となる
  int operator() (const VectorXf& p, VectorXf& f) const
  {
    Vector2f cp1(p[0], p[1]);
    Vector2f cp2(p[2], p[3]);
    for (int i = 0; i < m_values; i++) {
      float by = bezier_y(cp1, cp2, x[i]);
      f[i] = y[0]*(1.0-by) + y[m_values - 1]*by - y[i];
    }
    return 0;
  }
};

struct rotation_functor : Functor<float>
{
  rotation_functor(int nparam, int nvalue, const vector<float>& x, const vector<Eigen::Quaternionf>& y)
  : Functor<float>(nparam, nvalue), x(x), y(y) {}

  const vector<float>& x;
  const vector<Eigen::Quaternionf>& y;

  // 各データ点における誤差をf[i]に格納する
  // パラメータpはベジェ曲線の制御点の座標値
  // ベジェ曲線の制御点 = (0, 0), (p[0], p[1]), (p[2], p[3]), (1, 1) となる
  int operator() (const VectorXf& p, VectorXf& f) const
  {
    Vector2f cp1(p[0], p[1]);
    Vector2f cp2(p[2], p[3]);
    for (int i = 0; i < m_values; i++) {
      float by = bezier_y(cp1, cp2, x[i]);
      Eigen::Quaternionf rot = y[0].slerp(by, y[m_values - 1]);
      f[i] = rot.angularDistance(y[i]);
    }
    return 0;
  }
};

// head番めからtail番目までの誤差が最小になるような補間曲線パラメータを探す
VectorXf find_bezier_parameter_pos(const vector<VMD_Frame>& v, int head, int tail, int axis)
{
  vector<float> x;
  vector<float> y;
  for (int i = head; i <= tail; i++) {
    int current = v[i].number;
    x.push_back(float(current - head)/(tail - head));
    y.push_back(v[i].position(axis));
  }
  // パラメータの初期値を与える
  // ベジェ曲線の制御点 = (0, 0), (p[0], p[1]), (p[2], p[3]), (1, 1) となる
  VectorXf p(4);
  p << 20.0/127, 20.0/127, 107.0/127, 107.0/127;
  position_functor functor(4, (tail - head + 1), x, y);
  NumericalDiff<position_functor> nd(functor);
  LevenbergMarquardt<NumericalDiff<position_functor>, float> lm(nd);
  lm.parameters.maxfev = 10;
  lm.minimize(p);
  return p;
}

// head番めからtail番目までの誤差が最小になるような補間曲線パラメータを探す
VectorXf find_bezier_parameter_rot(const vector<VMD_Frame>& v, int head, int tail)
{
  vector<float> x;
  vector<Eigen::Quaternionf> y;
  for (int i = head; i <= tail; i++) {
    int current = v[i].number;
    x.push_back(float(current - head)/(tail - head));
    y.push_back(v[i].rotation);
  }
  // パラメータの初期値を与える
  // ベジェ曲線の制御点 = (0, 0), (p[0], p[1]), (p[2], p[3]), (1, 1) となる
  VectorXf p(4);
  p << 20.0/127, 20.0/127, 107.0/127, 107.0/127;
  // パラメータpの最適化を行う
  rotation_functor functor(4, (tail - head + 1), x, y);
  NumericalDiff<rotation_functor> nd(functor);
  LevenbergMarquardt<NumericalDiff<rotation_functor>, float> lm(nd);
  lm.parameters.maxfev = 10;
  lm.minimize(p);
  return p;
}

void convert_interpolation(uint8_t* ip, const VectorXf p)
{
  for (int i = 0; i < 4; i++) {
    int k = int(p[i] * 127);
    if (k > 127) {
      ip[i] = 127;
    } else if (k < 0) {
      ip[i] = 0;
    } else {
      ip[i] = uint8_t(k);
    }
  }
}

// head番めからtail番目までの誤差が最小になるようtail_frameの補間曲線パラメータを調整する
void optimize_bezier_parameter(VMD_Frame& tail_frame, const vector<VMD_Frame>& v,
                               int head, int tail)
{
  uint8_t ip[4];
  VectorXf p;

  // Xの補間パラメータの最適化
  p = find_bezier_parameter_pos(v, head, tail, 0);
  convert_interpolation(ip, p);
  tail_frame.set_interpolation_x(ip[0], ip[1], ip[2], ip[3]);

  // Yの補間パラメータの最適化
  p = find_bezier_parameter_pos(v, head, tail, 1);
  convert_interpolation(ip, p);
  tail_frame.set_interpolation_y(ip[0], ip[1], ip[2], ip[3]);

  // Zの補間パラメータの最適化
  p = find_bezier_parameter_pos(v, head, tail, 2);
  convert_interpolation(ip, p);
  tail_frame.set_interpolation_z(ip[0], ip[1], ip[2], ip[3]);

  // 回転の補間パラメータの最適化
  p = find_bezier_parameter_rot(v, head, tail);
  convert_interpolation(ip, p);
  tail_frame.set_interpolation_r(ip[0], ip[1], ip[2], ip[3]);
}


#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <tuple>

namespace py = pybind11;

// double + Eigen

std::tuple<Eigen::VectorXd, Eigen::MatrixXd> eigsy(const Eigen::Ref<const Eigen::MatrixXd> & input_sym) {
    auto solver = Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd>(input_sym, Eigen::ComputeEigenvectors);
    return {solver.eigenvalues(), solver.eigenvectors()};
}

PYBIND11_MODULE(eigen_wrapper, m) {
    m.doc() = "Wrapper for eigenvalue computation functions";

    m.def(
        "eigsy",
        &eigsy,
        "Computes eigenvalues using the default eigenvalue solver of Eigen C++ lib\n"
        "Uses a QR iterative algorithm in O(n^3), according to Eigen's doc\n"
        "Manipulates double precision only\n"
        "\n"
        "input_sym: symmetric matrix of double (numpy.float64)\n"
        "output = (E,Q):\n"
        "   E = vector of eigenvalues\n"
        "   Q = matrix with eigenvectors as columns",
        py::arg("input_sym"));
}


#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <NTL/ZZ.h>
#include <Windows.h>

using namespace std;
using namespace NTL;

string toBinary(ZZ n)
{
	string r;
	while (n != 0) {
		r += (n % 2 == 0 ? "0" : "1");
		n /= 2;
	}
	return r;
}

ZZ modulo(ZZ a, ZZ n) {
    ZZ r = a - (a / n) * n;
    if (r < 0) {
        r = r + n;
    }
    return r;
}

class RSA {

private:
    string msg;
    string alphabet = "ABCDEFGHIJKLMNOPQRSTUVWXYZ,.-( )abcdefghijklmnopqrstuvwxyz<>*1234567890[]";
    ZZ d;
    ZZ e;
    ZZ N;
    ZZ p;
    ZZ q;

public:
    ZZ e_Receiver;
    ZZ N_Receiver;
    RSA(string directory);
    RSA(int bits);
    RSA(ZZ, ZZ);
    RSA(ZZ, ZZ, ZZ, ZZ);
    ZZ chineseRemainder(vector<vector<ZZ>>);
    string encode_message(string, ZZ, ZZ);
    string decode_message(string, ZZ, ZZ);
    string encode(string);
    string decode(string);
    long euclidAlgorithm(ZZ a, ZZ b);
    vector<ZZ> euclidExtendedAlgorithm(ZZ a, ZZ b);
    ZZ inverse(ZZ a, ZZ b);
    bool primalityTest(ZZ num);
    string block(string);
    ZZ generate_bit();
    ZZ generate_random(int bits);
    ZZ prime(int bits);
    ZZ generateRandom(int bits);
    ZZ exponenciacion(ZZ b, ZZ e);
    ZZ binaryExponentiation(ZZ a, ZZ e, ZZ n);
};

ZZ RSA::generate_bit() {

    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
    // Elapsed time
    // Elapsed time
    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

    ZZ seed(std::chrono::duration_cast<std::chrono::nanoseconds> (end - begin).count());

    ZZ seed_2 = modulo(seed, ZZ(2));

    return seed_2;
}

ZZ RSA::generate_random(int bits)
{
	ZZ random(0);
	ZZ power(2);
	random = random + (generate_bit());
	for (int i = 1;i<bits;i++) {
		random = random + (generate_bit()*power);
		power = power * 2;
	}
	return random;
}

ZZ mod(ZZ a, ZZ n) {
    ZZ r = a - (a / n) * n;
    if (r < 0) {
        r = r + n;
    }
    return r;
}

// Euclid's extended algorithm:
// Function that returns a vector that stores the values
// of the gcd between a & b, and also the x and y values
vector<ZZ> RSA::euclidExtendedAlgorithm(ZZ a, ZZ b)
{
    // Initialization
	ZZ r_1 = a;
    ZZ r_2 = b;
    ZZ s_1 = ZZ(1);
    ZZ s_2 = ZZ(0);
	ZZ t_1 = ZZ(0);
	ZZ t_2 = ZZ(1);

	while (r_2 != 0) {

        // Quotient
		ZZ quotient = r_1 / r_2;

        // Updating r
        ZZ temp = r_2;
		r_2 = r_1 - quotient * r_2;
		r_1 = temp;

        // Updating s
		temp = s_2;
		s_2 = s_1 - quotient * s_2;
		s_1 = temp;

        // Updating t
		temp = t_2;
		t_2 = t_1 - quotient * t_2;
		t_1 = temp;
	}

    // Vector to store values
	vector<ZZ> result;

    // Greatest common divisor: result[0]
	result.push_back(r_1);
    // x: result[1]
	result.push_back(s_1);
    // y: result[2]
	result.push_back(t_1);

	return result;
}

string num2String(ZZ number) {
    ostringstream stringZZ;
    stringZZ << number;
    return stringZZ.str();
}

string num2String(int number) {
    ostringstream stringZZ;
    stringZZ << number;
    return stringZZ.str();
}

int string2NumInt(string message){
    int numberZZ;
    istringstream num(message);
    num >> numberZZ;
    return numberZZ;
}

ZZ string2Num(string message){
    ZZ numberZZ;
    istringstream num(message);
    num >> numberZZ;
    return numberZZ;
}

ZZ RSA::exponenciacion(ZZ b, ZZ e){

    ZZ result(1);

    while(e>0){
        if(e % 2)
            result = (result*b);
        b =  (b * b);
        e /= 2;
    }
    return result;
}

ZZ RSA::binaryExponentiation(ZZ a, ZZ e, ZZ n) {
	ZZ A(1);
	string bin = toBinary(e);
	for (int i = bin.size(); i != -1; i--) {
		A = modulo(A * A, n);
		if (bin[i] == '1') {
			A = modulo(A * a, n);
		}
	}
	return A;
}

bool RSA::primalityTest(ZZ n){

    if ((n & 1) == 0){
        return false;
    }
    ZZ s(0);
    ZZ t = n - 1;
    while ((t & 1) == 0) {
        s++;
        t >>= 1;
    }
    ZZ a(2);
    for (int i = 0; i < 10; i++){

        ZZ x = binaryExponentiation(a, t, n);
        if (x == 1 || x == (n-1))
            continue;
        for (ZZ r(0); r < (s-1); r++){
            x = binaryExponentiation(x, to_ZZ(2), n);
            if(x == 1){
                return false;
            }
            else if(x == n - 1)
                break;
        }
        if(x != n - 1 )
            return false;
        ZZ a = RandomBnd(n-3) + 3;
    }

    return true;
}

ZZ RSA::generateRandom(int bits){

    ZZ min(exponenciacion(to_ZZ(2), to_ZZ(bits))>>1), max(exponenciacion(to_ZZ(2), to_ZZ(bits)) - 1);

    ZZ number;
    do{
        number = RandomLen_ZZ(bits);
        // number = generate_random(bits); // No funcional para Linux
    }while(number < min || number > max);

    return number;
}

ZZ RSA::prime(int bits){

    ZZ prime;

    for (;!primalityTest(prime);prime = generateRandom(bits));

    return prime;
}

ZZ RSA::inverse(ZZ a, ZZ b)
{
    ZZ s_1 = euclidExtendedAlgorithm(a, b)[1];
    if (s_1 < 0) {
        s_1 = mod(s_1, b);
    }
    return s_1;
}

RSA::RSA(string directory) {
    this->p = 13;
	this->q = 149;
    ifstream infile(directory);

    string line;
    for(int i{0}; getline(infile, line); i++)
    {
        if(i == 2) {
            this->N_Receiver = string2Num(line);
        }
        else if(i == 3) {
            this->e_Receiver = string2Num(line);
        }
        else if(i == 7) {
            this->N = string2Num(line);
        }
        else if(i == 8) {
            this->e = string2Num(line);
        }
    }
	ZZ phiN = (p - 1) * (q - 1);
	this->d = inverse(e, phiN);
}

RSA::RSA(ZZ e, ZZ N) {
    this->p = 17;
	this->q = 59;
	this->N = p * q;
	ZZ phiN = (p - 1) * (q - 1);
	this->d = inverse(e, phiN);
}

RSA::RSA(ZZ p, ZZ q, ZZ e, ZZ d){
    this->p = p;
    this->q = q;
    this->N = p * q;
    this->e = e;
    this->d = d;
}

ZZ binaryEuclidAlgorithm(ZZ a, ZZ b) {
    ZZ g = ZZ(1);
    while ((mod(a, ZZ(2)) == 0) && (mod(b, ZZ(2)) == 0)) {
        a = a / 2;
        b = b / 2;
        g = 2 * g;
    }
    while (a != 0) {
        if (mod(a, ZZ(2)) == 0) {
            a = a / 2;
        }
        else if (mod(b, ZZ(2)) == 0) {
            b = b / 2;
        }
        else {
            ZZ t = abs(a - b) / 2;
            if (a >= b) {
                a = t;
            }
            else {
                b = t;
            }
        }
    }
    return g * b;
}

RSA::RSA(int bits) {
    this-> p = prime(bits);
    this-> q = prime(bits);
    this-> N = p * q;

    ZZ phiN = (p - 1) * (q - 1);

    do{
        this-> e = generateRandom(bits);
    } while(binaryEuclidAlgorithm(this->e, phiN) != 1);

    this-> d = inverse(this->e, phiN);

    cout << "-------------------- Valores generados --------------------" << endl;
    cout << "- Valor p  -" << endl
         << p << endl << endl;
    cout << "- Valor q -" << endl
         << q << endl << endl;;
    cout << "- Valor N -" << endl
         << N << endl << endl;
    cout << "- Valor e -" << endl
         << e << endl << endl;
    cout << "- Valor d -" << endl
         << d << endl << endl;
}

void addZeros(int size, string &block) {
	string zeros(size - block.size(), '0');
	zeros += block;
	block = zeros;
}

ZZ phi(ZZ modulus)
{
    ZZ result = ZZ(1);
    for (ZZ i = ZZ(2); i < modulus; i++)
        if (binaryEuclidAlgorithm(i, ZZ(modulus)) == ZZ(1))
            result++;
    return result;
}

string convert(string message, string alphabet, int size) {
    int messageSize = message.length();
    string decodedMessage;
    int alphabetIndex = 0;
    for(int i{0}; i < messageSize; i += size) {
        alphabetIndex = string2NumInt(message.substr(i, size));
        char alphabetCharacter = alphabet[alphabetIndex];
        decodedMessage += alphabetCharacter;
    }
    return decodedMessage;
}

string RSA::block(string mensaje) {
    string mensaje_antes;
    int tamano_mensaje = mensaje.length();

    string tamano_alfabeto = num2String(alphabet.length());
    int tamano = tamano_alfabeto.length();

    for(int i = 0; i < tamano_mensaje; i++) {
        int posicion = alphabet.find(mensaje[i]);

        string tamano_posicion = num2String(posicion);
        int tamano_pos = tamano_posicion.length();

        if( tamano_pos < tamano) {
            string pos = num2String(posicion);
            addZeros(tamano, pos);
            mensaje_antes += pos;
        }
        else {
            string pos = num2String(posicion);
            mensaje_antes += pos;
        }
    }
    return mensaje_antes;
}

string RSA::encode_message(string message, ZZ exponent, ZZ modulus) {

    string codedMessage;

    string blockMessage = block(message);

    int blockSize = num2String(modulus).length() - 1;
    int blockMessageLength = blockMessage.length();

    for(int i = 0; i < blockMessageLength; i += blockSize){
        ZZ base(0);
        if(i + blockSize > blockMessageLength) {
            base = string2Num(blockMessage.substr(i, blockMessageLength - i));
        }
        else {
            base = string2Num(blockMessage.substr(i, blockSize));
        }

        ZZ power = binaryExponentiation(base, exponent, modulus);
        string powerString = num2String(power);
        int powerSize = powerString.length();

        string N_string = num2String(N);
        int N_size = N_string.length();
        if( powerSize < N_size) {
            string pos = num2String(power);
            addZeros(N_size, pos);
            codedMessage += pos;
        }
        else {
            string pos = num2String(power);
            codedMessage += pos;
        }

    }
    return codedMessage;
}

string RSA::encode(string message) {
    ifstream infile("sign.txt");
    string text;
    string line;

    for(int i{0}; getline(infile, line); i++){
        text += line;
    }

    string rubric = encode_message(text, d, N);

    string sign = encode_message(rubric, e_Receiver, N_Receiver);

    ofstream out("digital_sign.txt");
    out << sign;
    out.close();

    string code = encode_message(message, e_Receiver, N_Receiver);

    return code;
}

ZZ RSA::chineseRemainder(vector<vector<ZZ>> ecs)
{
	ZZ P(1);
	for (int i = 0; i < ecs.size(); i++) {
		ecs[i][0] = mod(ecs[i][0], ecs[i][1]);
		P *= ecs[i][1];
	}
	for (int i = 0; i < ecs.size(); i++) {
		ecs[i].push_back(P / ecs[i][1]);
	}
	ZZ x0(0);
	for (int i = 0; i < ecs.size(); i++) {
		x0 += mod(ecs[i][0], P) * mod(ecs[i][2], P) * mod(inverse(ecs[i][2], ecs[i][1]), P);
		x0 = mod(x0, P);
	}
	x0 = mod(x0, P);
	return x0;
}

string RSA::decode_message(string message, ZZ exponent, ZZ modulus) {

    string decodedCode;

    string N_string = num2String(modulus);
    int N_size = N_string.length();
    int codeLength = message.length();

    string alphabetSizeString = num2String(alphabet.length());
    int alphabetSize = alphabetSizeString.length();

    for(int i = 0; i < codeLength; i += N_size){
        ZZ base(0);
        if(i + N_size > codeLength) {
            base = string2Num(message.substr(i, codeLength - i));
        }
        else {
            base = string2Num(message.substr(i, N_size));
        }

        ZZ power = binaryExponentiation(base, exponent, modulus);

        string powerString = num2String(power);
        int powerSize = powerString.length();

        if( powerSize < N_size - 1 && i + N_size < codeLength) {
            string pos = num2String(power);
            addZeros(N_size - 1, pos);
            decodedCode += pos;
        }
        else {
            string pos = num2String(power);
            decodedCode += pos;
        }

    }
    string decodedMessage = convert(decodedCode, alphabet, alphabetSize);
    return decodedMessage;
}

string RSA::decode(string message) {
    ifstream infile("digital_sign.txt");
    string line;
    for(; getline( infile, line ); );

    string decodedSign = decode_message(line, d, N);

    ifstream indirectory("directory.txt");

    string text;
    ZZ e_Emitter;
    ZZ N_Emitter;

    for(int i{0}; getline(indirectory, text); i++)
    {
        if(i == 7) {
            N_Emitter = string2Num(text);
        }
        else if(i == 8) {
            e_Emitter = string2Num(text);
        }
    }

    string sign = decode_message(decodedSign, e_Emitter, N_Emitter);

    ofstream out("sign_test.txt");
    out << sign;
    out.close();

    string decodedCode = decode_message(message, d, N);
    return decodedCode;
}

int main() {

    int opt;

    cout << "Choose an option" << endl;
    cout << right << setw(12)
              << "Encode (1)" << endl;
    cout << right << setw(12)
              << "Decode (2)" << endl;
    cout << right << setw(12)
              << "Generate keys (3)" << endl;
    cout << "Option: ";
    cin >> opt;
    cin.ignore();

    if ((opt != 1) && (opt != 2) && (opt != 3)) {
        return 0;
    }

    // Objeto Receptor:
    // - Envía la clave pública e
    // - Envía el producto N
    // Directorio público
    RSA Receiver(ZZ(3), ZZ(1003));
    // Objeto Emisor:
    RSA Emitter("directory.txt");

    if (opt == 1) {
        /*string message;
        cout << "Enter message: ";
        (void) getline(cin, message);*/

        ifstream infile("message.txt");
        string line;
        for(; getline( infile, line ); );

        string code = Emitter.encode(line);

        cout << "\nEncoded message: " << code;

        ofstream out("encode.txt");
        out << code;
        out.close();
    }
    else if (opt == 2) {
        /*string message;
        cout << "Enter message: ";
        (void) getline(cin, message);*/

        ifstream infile("encode.txt");
        string line;
        for(; getline( infile, line ); );

        string code = Receiver.decode(line);

        cout << "\nDecoded message: " << code;

        ofstream out("decode.txt");
        out << code;
        out.close();
    }
    else if (opt == 3) {
        RSA Bits(1024);
    }

}


#ifndef RNDCMP_INCLUDE_ESN_HPP_
#define RNDCMP_INCLUDE_ESN_HPP_

#include <random>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/QR>

namespace rndcmp {
    template<typename DATA_TYPE>
    class ESN {
    public:
        using ESNMatrix = Eigen::Matrix<DATA_TYPE, Eigen::Dynamic, Eigen::Dynamic>;

        ESN(
            size_t input_size,
            size_t hidden_size,
            size_t output_size,
            double spectral_radius,
            double sparsity,
            double regularization,
            size_t seed
            ):
            _input_size(input_size),
            _hidden_size(hidden_size),
            _output_size(output_size),
            _regularization(regularization),
            _seed(seed) {
            W_in.resize(_hidden_size, _input_size);
            W.resize(_hidden_size, _hidden_size);
            W_out.resize(_output_size, _hidden_size);

            std::mt19937 rd(_seed);
            std::uniform_real_distribution<double> generator(-0.5, 0.5);

            initialize_weight_m(W_in, generator, rd, 0.0, hidden_size, input_size);
            initialize_weight_m(W, generator, rd, sparsity, hidden_size, hidden_size);
            initialize_weight_m(W_out, generator, rd, 0.0, output_size, hidden_size);

            rescale_weight_m(spectral_radius);
        }

        double fit(ESNMatrix inputs, ESNMatrix outputs) {
            ESNMatrix states;
            states.resize(inputs.rows(), _hidden_size);
            states.setZero();

            // Calculate hidden states
            for (size_t i = 0; i < inputs.rows(); i++) {
                if (i == 0) {
                    ESNMatrix initial;
                    initial.resize(1, _hidden_size);
                    initial.setZero();
                    states.row(i) = update(initial.transpose(), inputs.row(i).transpose()).transpose();

                } else {
                    states.row(i) = update(states.row(i - 1).transpose(), inputs.row(i).transpose()).transpose();
                }
            }

            // Find optimal matrix (we do it in double for numerical stability of fixed and other types)
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> states_d = states.template cast<double>();
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> states_square_d = states_d.transpose() * states_d;

            for (size_t i = 0; i < states_square_d.rows(); i++) {
                states_square_d(i, i) += _regularization;
            }
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> pinv_d = states_square_d.completeOrthogonalDecomposition().pseudoInverse();
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W_out_d = (pinv_d * states_d.transpose() * outputs.template cast<double>()).transpose();
            // ESNMatrix pinv = pinv_d.template cast<DATA_TYPE>();
            // W_out = (pinv * states.transpose() * outputs).transpose();
            W_out = W_out_d.template cast<DATA_TYPE>();

            // Train prediction
            double pred = error(states * W_out.transpose(), outputs);
            return pred;
        }

        ESNMatrix predict(ESNMatrix inputs, size_t n_future) {
            size_t n_samples = inputs.rows();
            ESNMatrix outputs;
            outputs.resize(n_samples + n_future, _output_size);
            outputs.setZero();

            ESNMatrix prev;
            prev.resize(1, _hidden_size);
            prev.setZero();

            for (size_t i = 0; i < n_samples; i++) {
                ESNMatrix temp_state =
                 update(prev.transpose(), inputs.row(i).transpose()).transpose();
                outputs.row(i) = temp_state * W_out.transpose();
                prev = temp_state;
            }

            for (size_t i = 0; i < n_future; i++) {
                size_t idx = n_samples + i;

                ESNMatrix input = outputs.row(idx - 1);
                ESNMatrix temp_state =
                 update(prev.transpose(), input.transpose()).transpose();

                outputs.row(idx) = temp_state * W_out.transpose();
                prev = temp_state;
            }
            return outputs;
        }

        ESNMatrix predict(ESNMatrix inputs) {
            return predict(inputs, 0);
        }

        double score(ESNMatrix x, ESNMatrix y) {
            ESNMatrix y_pred = predict(x);
            return error(y, y_pred);
        }

        double error(ESNMatrix y, ESNMatrix y_pred) {
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> error_m = (y - y_pred).template cast<double>();
            return sqrt((error_m.array() * error_m.array()).matrix().rowwise().sum().mean());
        }

    protected:
        ESNMatrix update(ESNMatrix state, ESNMatrix input_vector) {
            ESNMatrix preactivation = W * state.matrix() + W_in * input_vector.matrix();
            return preactivation.array().tanh();
        }

        void initialize_weight_m(ESNMatrix& m,
                                 std::uniform_real_distribution<double> generator,
                                 std::mt19937 rd,
                                 double sparsity,
                                 size_t first_size,
                                 size_t second_size) {
            std::mt19937 random_device(_seed);
            std::uniform_real_distribution<double> probability_gen(0.0, 1.0);

            for (size_t i = 0; i < first_size; i++) {
                for (size_t j = 0; j < second_size; j++) {
                    if (probability_gen(random_device) < sparsity) {
                        m(i, j) = DATA_TYPE(0.0);
                    } else {
                        m(i, j) = DATA_TYPE(generator(rd));
                    }
                }
            }
        }

        void rescale_weight_m(double spectral_radius) {
            Eigen::EigenSolver<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> solver(W.template cast<double>());
            Eigen::VectorXcd eivals = solver.eigenvalues();

            double real_sr = 0.0;
            for (size_t i = 0; i < eivals.size(); i++) {
                double temp = sqrt(pow(eivals[i].real(), 2) + pow(eivals[i].imag(), 2));
                if (temp > real_sr) {
                    real_sr = temp;
                }
            }

            W /= (real_sr / spectral_radius);
        }

        size_t _seed;

        Eigen::Matrix<DATA_TYPE, Eigen::Dynamic, Eigen::Dynamic> W_in;
        Eigen::Matrix<DATA_TYPE, Eigen::Dynamic, Eigen::Dynamic> W;
        Eigen::Matrix<DATA_TYPE, Eigen::Dynamic, Eigen::Dynamic> W_out;

        size_t _input_size;
        size_t _hidden_size;
        size_t _output_size;
        double _regularization;
    };
}

#endif  // RNDCMP_INCLUDE_ESN_HPP_


/**
 * @file    map_stats.hpp
 * @author  Chirag Jain <cjain7@gatech.edu>
 */

#ifndef MAP_STATS_HPP
#define MAP_STATS_HPP

#include <vector>
#include <algorithm>
#include <deque>
#include <cmath>

#ifdef USE_BOOST
    #include <boost/math/distributions/binomial.hpp>
    using namespace::boost::math;
#else
    #include <gsl/gsl_cdf.h>
#endif

//Own includes
#include "map/include/base_types.hpp"
#include "map/include/map_parameters.hpp"

//External includes
#include "common/murmur3.h"
#include "common/kseq.h"
#include "common/prettyprint.hpp"

namespace skch
{
  /**
   * @namespace skch::Stat
   * @brief     Implements utility functions that involve statistical computation
   */
  namespace Stat
  {
    /**
     * @brief         jaccard estimate to mash distance
     * @param[in] j   jaccard estimate
     * @param[in] k   kmer size
     * @return        mash distance [0.0 - 1.0]
     */
    inline float j2md(float j, int k)
    {
      if(j == 0)
        return 1.0; //jaccard estimate 0 -> 1.0 mash distance

      if(j == 1)
        return 0.0; //jaccard estimate 1 -> 0.0 mash distance

      float mash_dist = (-1.0 / k) * log(2.0 * j/(1+j) );
      return mash_dist;
    }

    /**
     * @brief         mash distance to jaccard estimate
     * @param[in] d   mash distance [0.0 - 1.0]
     * @param[in] k   kmer size
     * @return        jaccard estimate
     */
    inline float md2j(float d, int k)
    {
      float jaccard = 1.0 / (2.0 * exp( k*d ) - 1.0);
      return jaccard;
    }

    /**
     * @brief               Given a distance d, compute the lower bound on d within required confidence interval
     * @details             If a given match has distance d in the L2 stage, we compare its lower distance bound
     *                      against the assumed cutoff to decide its significance. This makes the mapping algorithm
     *                      more sensitive to true alignments
     * @param[in]   d       calculated mash distance
     * @param[in]   s       sketch size
     * @param[in]   k       kmer size
     * @param[in]   ci      confidence interval [0-1], example 0.9 implies 90% confidence interval
     * @return              computed lower bound on d within 'ci' confidence interval
     */
    inline float md_lower_bound(float d, int s, int k, float ci)
    {
      //One side interval probability
      float q2 = (1.0 - ci)/2;

      //Computing count of sketches using confidence interval
#ifdef USE_BOOST

      //Inverse binomial
      int x = quantile(complement(binomial(s, md2j(d,k)), q2));

#else
      //GSL
      int x = std::max( int(ceil(s * md2j(d,k))), 1 );    //Begin search from jaccard * s
      while(x <= s)
      {
        //probability of having x or more shared sketches
        double cdf_complement = gsl_cdf_binomial_Q(x-1, md2j(d,k), s);

        if (cdf_complement < q2)
        {
          x--;  //Last guess was right
          break;
        }

        x++;
      }
#endif

      float jaccard = float(x) / s;
      float low_d = j2md(jaccard, k);
      return low_d;
    }

    /**
     * @brief                 Estimate minimum number of shared sketches to achieve the desired identity
     * @param[in] s           sketch size
     * @param[in] k           kmer size
     * @param[in] identity    percentage identity [0-1]
     * @return                minimum count of hits
     */
    inline int estimateMinimumHits(int s, int k, float perc_identity)
    {
      //Compute the estimate
      float mash_dist = 1.0 - perc_identity;
      float jaccard = md2j(mash_dist, k);

      //function to convert jaccard to min hits
      //Atleast these many minimizers should match for achieving the required jaccard identity
      int minimumSharedMinimizers = ceil (1.0 * s * jaccard);

      return minimumSharedMinimizers;
    }

    /**
     * @brief                 Estimate minimum number of shared sketches
     *                        s.t. upper bound identity is >= desired identity
     *                        Upper bound is computed using the 90% confidence interval
     * @param[in] s           sketch size
     * @param[in] k           kmer size
     * @param[in] identity    percentage identity [0-1]
     * @return                count of min. shared minimizers
     */
    inline int estimateMinimumHitsRelaxed(int s, int k, float perc_identity, float confidence_interval)
    {
      // The desired value has be between [0, min  s.t. identity >= perc_identity]
      auto searchRange = std::pair<int, int>( estimateMinimumHits(s, k, perc_identity) , 0);

      int minimumSharedMinimizers_relaxed = searchRange.first;

      for(int i = searchRange.first ; i >= searchRange.second; i--)
      {
        float jaccard = 1.0 * i/s;
        float d = j2md(jaccard, k);

        float d_lower = md_lower_bound(d, s, k, confidence_interval);

        //Upper bound identity
        float id_upper = 1.0 - d_lower;

        //Check if it satisfies the criteria
        if(id_upper >= perc_identity)
          minimumSharedMinimizers_relaxed = i;
        else
          break;    //Stop the search
      }

      return minimumSharedMinimizers_relaxed;
    }

    /**
     * @brief                     calculate p-value for a given alignment identity, sketch size..
     * @param[in] s               sketch size
     * @param[in] k               kmer size
     * @param[in] alphabetSize    alphabet size
     * @param[in] identity        mapping identity cut-off
     * @param[in] lengthQuery     query length
     * @param[in] lengthReference reference length
     * @return                    p-value
     */
    inline double estimate_pvalue (int s, int k, int alphabetSize,
        float identity,
        int64_t lengthQuery, uint64_t lengthReference, float confidence_interval)
    {
      //total space size of k-mers
      double kmerSpace = pow(alphabetSize, k);

      //probability of a kmer match by random in |query| sized sequence
      double pX, pY;
      pX = pY = 1. / (1. + kmerSpace / lengthQuery);

      //Jaccard similarity of two random given sequences
      double r = pX * pY / (pX + pY - pX * pY);

      int x = estimateMinimumHitsRelaxed(s, k, identity, confidence_interval);

      //P (x or more minimizers match)
      double cdf_complement;
      if(x == 0)
      {
        cdf_complement = 1.0;
      }
      else
      {
#ifdef USE_BOOST
      cdf_complement = cdf(complement(binomial(s, r), x-1));
#else
      cdf_complement =  gsl_cdf_binomial_Q(x-1, r, s);
#endif
      }

      double pVal = lengthReference * cdf_complement;

      return pVal;
    }

    /**
     * @brief                       calculate minimum window size for sketching that satisfies
     *                              the given p-value threshold
     * @param[in] pValue_cutoff     cut off p-value threshold
     * @param[in] confidence_interval confidence interval to relax jaccard cutoff for mapping
     * @param[in] k                 kmer size
     * @param[in] alphabetSize      alphabet size
     * @param[in] identity          mapping identity cut-off
     * @param[in] segmentLength     mashmap's internal minimum query sequence length
     * @param[in] lengthReference   reference length
     * @return                      optimal window size for sketching
     */
    inline int64_t recommendedWindowSize(double pValue_cutoff, float confidence_interval,
        int k, int alphabetSize,
        float identity,
        int64_t segmentLength, uint64_t lengthReference)
    {
        int64_t lengthQuery = segmentLength;

      int optimalSketchSize;
      for (optimalSketchSize = 10; optimalSketchSize < lengthQuery; optimalSketchSize += 50) {
        //Compute pvalue
        double pVal = estimate_pvalue(optimalSketchSize, k, alphabetSize, identity, lengthQuery, lengthReference, confidence_interval);

        //Check if pvalue is <= cutoff
        if(pVal <= pValue_cutoff)
        {
          break;
        }
      }

      int64_t w =  (2.0 * lengthQuery)/optimalSketchSize;

      // 1 <= w <= lengthQuery
      return std::min(std::max(w,(int64_t)1), lengthQuery);
    }
  }
}

#endif


#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

// Print Funktion zur Vereinfachung a lá python
template <typename T>
void print(T x) {
    std::cout << x << std::endl;
}

// Bisection Verfahren Analog zu Blatt 5 nur mit Vektoren
Eigen::Vector2d bisection(double function(Eigen::Vector2d), Eigen::Vector2d x0, Eigen::Vector2d gradient) {
    // print("Start bisection method.");
    double a, b, c;

    a = 0;
    b = 10;
    c = 100;
    int iterator = 0;
    while ((function(x0 + a * gradient) < function(x0 + b * gradient)) & (iterator < 100)) {
        a--;
        iterator++;
    }
    iterator = 0;
    // print("Find a. Iterations:");
    // print(iterator);
    while ((function(x0 + c * gradient) < function(x0 + b * gradient)) & (iterator < 100)) {
        c++;
        iterator++;
    }
    // print("Find c. Iterations:");
    // print(iterator);

    if ((a < b) & (b < c) & (function(x0 + b * gradient) < function(x0 + a * gradient)) & (function(x0 + b * gradient) < function(x0 + c * gradient))) {
        int iteration = 0;
        do {
            iteration++;
            if (abs(b - a) > abs(c - b)) {
                double a_new = a;
                double b_new = (a + b) / 2;
                double c_new = b;
                if (function(x0 + b_new * gradient) < function(x0 + c_new * gradient)) {
                    a = a_new;
                    b = b_new;
                    c = c_new;
                } else {
                    a = b_new;
                    b = c_new;
                    c = c;
                }
            } else {
                double a_new = b;
                double b_new = (b + c) / 2;
                double c_new = c;
                if (function(x0 + b_new * gradient) < function(x0 + a_new * gradient)) {
                    a = a_new;
                    b = b_new;
                    c = c_new;
                } else {
                    a = a;
                    b = a_new;
                    c = b_new;
                }
            }
        } while ((abs(a - c) > 1e-9) & (iteration < 100));
        // print("Iterations: ");
        // print(iteration);
        // print("a - c = ");
        // print(abs(a - c));
    } else {
        print("First requirement of bisection is not fullfiled.");
    }
    Eigen::Vector2d x_min = x0 + a * gradient;
    // print("x_min after Bisection.");
    // print(x_min);
    return x_min;
}

// BFGS Algorithmus
Eigen::Vector2d calc_BFGS(double function(Eigen::Vector2d), Eigen::Vector2d gradient(Eigen::Vector2d), Eigen::Vector2d x0, Eigen::Matrix2d C0, double epsilon, std::string filename) {
    // Definiere alle nötigen Objekte, x_k = x_k1, x_(k-1)= x_k0, b analog
    Eigen::Vector2d x_k0, x_k1, b_k0, b_k1, x_min, s, y;
    // Neue C Matrix, C1
    Eigen::Matrix2d C1;
    // Iterator um Schritte zu zählen
    double rho, iterator;
    iterator = 0;
    // Analog zu Skript. Bestimme b_0, mit bisections-Verfahren x1 und damit b1
    x_k0 = x0;
    b_k0 = gradient(x_k0);
    x_k1 = bisection(function, x_k0, b_k0);
    b_k1 = gradient(x_k1);
    // Speichere x_k in Datei
    std::ofstream output;
    output.open(filename, std::ofstream::trunc);  // std::ofstream::trunc);
    // Starte Iteration: Bestimmte s, y Vektoren und dann C1, anschließend nach Skript das neue x_k
    do {
        s = x_k1 - x_k0;
        y = b_k1 - b_k0;
        rho = 1. / (s.transpose() * y);
        C1 = (Eigen::Matrix2d::Identity() - rho * s * y.transpose()) * C0 * (Eigen::Matrix2d::Identity() - rho * y * s.transpose()) + rho * s * s.transpose();
        x_k0 = x_k1;
        C0 = C1;
        b_k0 = b_k1;

        x_k1 = x_k0 - C0 * b_k0;
        b_k1 = gradient(x_k1);
        iterator++;
        output << std::setprecision(8) << x_k1(0) << " " << x_k1(1) << std::endl;
        // Solange die Norm des Gradienten größer als epsilon ist oder bis eine gewisse Anzahl an Iterationen durchgelaufen ist.
    } while ((b_k1.norm() > epsilon) & (iterator < 1000));
    output.close();
    print("Iterations of BFGS:");
    print(iterator);
    x_min = x_k1;
    // Gib Minimum (1,1) zurück
    return x_min;
}

// 1. C0 Matrix Variante: Inverse Hesse-Matrix
Eigen::Matrix2d inv_HesseC_a(Eigen::Vector2d x) {
    Eigen::Matrix2d Hesse_C;
    Hesse_C(0, 0) = 2 - 400 * (x(1) - pow(x(0), 2.)) + 800 * pow(x(0), 2.);  // f_x1_x1
    Hesse_C(0, 1) = -400 * x(0);                                             // f_x1_x2
    Hesse_C(1, 0) = -400 * x(0);                                             // f_x2_x1
    Hesse_C(1, 1) = 200 * x(1);                                              // f_x2_x2
    return Hesse_C.inverse();
}

// 2. C0 Matrix Variante: inverse diag. Hesse-Matrix
Eigen::Matrix2d inv_HesseC_b(Eigen::Vector2d x) {
    Eigen::Matrix2d Hesse_C;
    Hesse_C(0, 0) = 2 - 400 * (x(1) - pow(x(0), 2.)) + 800 * pow(x(0), 2.);  // f_x1_x1
    Hesse_C(0, 1) = 0;                                                       // f_x1_x2
    Hesse_C(1, 0) = 0;                                                       // f_x2_x1
    Hesse_C(1, 1) = 200 * x(1);                                              //
    return Hesse_C.inverse();
}

// 3. C0 Matrix Variante: f(x_0) * identity
Eigen::Matrix2d inv_HesseC_c(Eigen::Vector2d x, double function(Eigen::Vector2d)) {
    Eigen::Matrix2d identity = Eigen::Matrix2d::Identity(2, 2);
    return function(x) * identity;
}

// Rosenbrock Funktion zu Aufgabe 1
double function_1(Eigen::Vector2d x) {
    return pow(1 - x(0), 2.) + 100 * pow(x(1) - pow(x(0), 2.), 2);
}

// Gradient zur Aufgabe 1
Eigen::Vector2d gradient_1(Eigen::Vector2d x) {
    Eigen::Vector2d g(2);
    g(0) = -2 * (1 - x(0)) - 400 * x(0) * (x(1) - pow(x(0), 2.));
    g(1) = 200 * (x(1) - pow(x(0), 2.));
    return g;
}

// double calc_RungeKutta(double y_prime(double, double), double t, double y_n, double h){
//     double k_1 = h* y_prime(t, y_n);
//     double k_2 = h* y_prime(t+ h/2., y_n + 0.5*k_1);
//     double k_3 = h* y_prime(t+h/2., y_n + 0.5*k_2);
//     double k_4 = h* y_prime(t+h, y_n + k_3);
//     double y_n_1 = y_n + 1./6. * (k_1 + 2* k_2 + 2*k_3 + k_4);
//     return y_n_1;
// }

int main() {
    // Nr. 1 Definiere Startvektor, Gradient und die C0 Matrix mit drei verschiedenen Varianten
    Eigen::Vector2d x(2);
    x << -1., -1.;
    Eigen::Vector2d grad = gradient_1(x);
    Eigen::Matrix2d C0_a = inv_HesseC_a(x);
    Eigen::Matrix2d C0_b = inv_HesseC_b(x);
    Eigen::Matrix2d C0_c = inv_HesseC_c(x, function_1);

    // Berechne die Ergebnisse mit dem oben definierten BFGS-Algorithmus für die drei verschiedenen Methoden
    print("1 a)");
    Eigen::Vector2d x_min_a = calc_BFGS(function_1, gradient_1, x, C0_a, 1e-5, "output/output_a.txt");
    print("Result of BFGS:");
    print(x_min_a);
    print("");

    print("1 b)");
    Eigen::Vector2d x_min_b = calc_BFGS(function_1, gradient_1, x, C0_b, 1e-5, "output/output_b.txt");
    print("Result of BFGS:");
    print(x_min_b);
    print("");

    print("1 c)");
    Eigen::Vector2d x_min_c = calc_BFGS(function_1, gradient_1, x, C0_c, 1e-5, "output/output_c.txt");
    print("Result of BFGS:");
    print(x_min_c);
    print("");

    return 0;
}


/*
 * useG2O.cpp
 * Copyright (C) 2018 exbot <exbot@ubuntu>
 *
 * Distributed under terms of the MIT license.
 */

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	virtual void setToOriginImpl()
	{
		_estimate << 0,0,0;
	}
	virtual void oplusImpl (const double* update)
	{
		_estimate += Eigen::Vector3d(update);
	}
	virtual bool read( istream& in ){}
	virtual bool write( ostream& out ) const {}
};

class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	CurveFittingEdge(double x):BaseUnaryEdge(), _x(x){}
	void computeError()
	{
		const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
		const Eigen::Vector3d abc = v->estimate();
		_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x+abc(1,0)*_x+abc(2,0) );
	}
	virtual bool read( istream& in ){}
	virtual bool write( ostream& out) const {}
	double _x;
};

int main(int argc, char **argv){
	double a = 1.0, b = 2.0, c = 1.0;
	int N = 100;
	double w_sigma = 1.0;
	cv::RNG rng;

	double abc[3] = {0, 0, 0};

	vector<double> x_data, y_data;

	cout << "Generating Data: " << endl;
	for(int i = 0; i < N; i++)
	{
		double x = i/100.0;
		x_data.push_back(x);
		y_data.push_back(
				exp(a*x*x + b*x + c) + rng.gaussian(w_sigma));
		cout << x_data[i] << " " << y_data[i] << endl;
	}

	typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	Block *solver_ptr = new Block(linearSolver);
	g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);
	optimizer.setVerbose(true);

	CurveFittingVertex *v = new CurveFittingVertex();
	v->setEstimate(Eigen::Vector3d(0, 0, 0));
	v->setId(0);
	optimizer.addVertex(v);

	for(int i = 0; i < N; i++)
	{
		CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
		edge->setId(i);
		edge->setVertex(0, v);
		edge->setMeasurement( y_data[i]);
		edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity()*1/(w_sigma*w_sigma) );
		optimizer.addEdge(edge);
	}

	cout << "Start" << endl;
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	optimizer.initializeOptimization();
	optimizer.optimize(100);
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
	cout << "time cost:" << time_used.count() << endl;

	Eigen::Vector3d abc_estimate = v->estimate();
	cout << "estimated model :" << abc_estimate.transpose() << endl;

return 1;
}


#include <iostream>
#include <memory>
#include <algorithm>
#include <iomanip>
#include <math.h>
#include <cmath>
#include <mgl2/mgl.h>
#include <Eigen/Dense>

#include <data.hpp>
#include <particle.hpp>
#include <gravitysolvers.hpp>
#include <first_task.hpp>

std::unique_ptr<std::vector<Particle>> p(Data::readFromFile("data.ascii"));
std::vector<Particle> particles = *p;

float pMass = 0.0;
float totalMass = 0.0;
float radius = 0.0;
float scaleLength = 0.0;
float epsilon = 0.0; // softening
float t_relax = 0.0;
float r0 = std::numeric_limits<float>::max(); // center of system to avoid problems with dividing by 0
float rhm = 0.0; // half mass radius

float Mass(float r, bool strictlyLess = false)
{
  float M = 0.0;

  for(Particle &p : particles) {
    float r2 = p.radius2();

    if((!strictlyLess && r2 <= r*r) || (strictlyLess && r2 < r*r)) {
      M += p.m();
    }
  }

  return M;
}

float density_hernquist(float r)
{
  return (totalMass / (2 * M_PI)) * (scaleLength / r) * (1 / std::pow(r + scaleLength, 3));
}

// Computes force F where F = m*a
float force_hernquist(float r)
{
  // Assumption: G = 1
  return -totalMass * pMass / ((r + scaleLength) * (r + scaleLength));
}

void calculate_constants()
{
  pMass = particles[0].m(); // all particles have the same mass

  for(Particle &p : particles) {
    totalMass += p.m();
    radius = std::max(p.radius2(), radius);
    r0 = std::min(p.radius2(), r0);
  }

  r0 = std::sqrt(r0) + std::numeric_limits<float>::epsilon();
  radius = std::sqrt(radius);

  float dr = radius / 100000;
  for(float r = r0; r <= radius; r += dr) {
    if(Mass(r) >= totalMass * 0.5) {
      rhm = r;
      break;
    }
  }

  scaleLength = rhm / (1 + std::sqrt(2));

  // https://en.wikipedia.org/wiki/Mean_inter-particle_distance
  // after plugging in n = totalMass / (4/3*PI*r^3) in the formula most terms cancel out
  epsilon = radius / std::pow(totalMass, 1.0/3.0);
  t_relax = compute_relaxation();

  // exact mean inter-particle separation
  /*
  epsilon = .0;

  for(Particle &p : particles) {
    for(Particle &q : particles) {
      epsilon += (p.r() - q.r()).norm();
    }
  }

  long n = particles.size();
  epsilon /= (n*(n-1));
  */

  std::cout << "First Task        " << std::endl;
  std::cout << "------------------" << std::endl;
  std::cout << "  pMass:          " << pMass << std::endl;
  std::cout << "  totalMass:      " << totalMass << std::endl;
  std::cout << "  radius:         " << radius << std::endl;
  std::cout << "  r0:             " << r0 << std::endl;
  std::cout << "  rhm:            " << rhm << std::endl;
  std::cout << "  scaleLength:    " << scaleLength << std::endl;
  std::cout << "  softening:      " << epsilon << std::endl;
  std::cout << "  t_relax:        " << t_relax << std::endl;
  std::cout << "------------------" << std::endl;
}

float compute_relaxation()
{
  // Assumption: G = 1
  float N = particles.size();
  float vc = std::sqrt(totalMass * 0.5 / rhm);
  float t_cross = rhm / vc;
  float t_relax = N / (8 * std::log(N)) * t_cross;

  return t_relax;
}

void step1()
{
  r0 = 0.005;
  int numSteps = 50;

  std::vector<float> hDensity;
  std::vector<float> nDensity;
  std::vector<float> rInput;
  std::vector<float> errors;

  // creates evenly spaced intervals on a log scale on the interval [r0, radius]
  // drLinToLog(i) gives the start of the ith interval on [r0, radius]
  auto drLinToLog = [&](int i) {
    return r0 * std::pow(radius / r0, (float)i / (float)numSteps);
  };

  // transforms the numerical data into a dimension desirable for plotting
  // here we want to avoid problems with 0 on log scales since log(0) is undefined
  auto plotFit = [](float x) {
    return x + std::numeric_limits<float>::epsilon();
  };

  for(int i = 0; i <= numSteps; i++) {
      float r = drLinToLog(i);
      float r1 = drLinToLog(i + 1);
      float MShell = Mass(r1, true) - Mass(r, true);
      float VShell = 4.0/3.0*M_PI*(r1*r1*r1 - r*r*r);

      float nRho = MShell / VShell;
      float numParticlesInShell = MShell / pMass;

      // p = n*m/v, err = sqrt(n) =>
      // p_err = sqrt(n)/n*p = sqrt(n)*n*m/(n*v) = sqrt(n)*m/v
      // p = density, n = #particles, m = pMass
      float rhoError = std::sqrt(numParticlesInShell) * pMass / VShell;

      hDensity.push_back(plotFit(density_hernquist((r + r1) / 2)));
      nDensity.push_back(plotFit(nRho));
      errors.push_back(rhoError);
      rInput.push_back(r);
  }

  mglData hData;
  hData.Set(hDensity.data(), hDensity.size());

  mglData nData;
  nData.Set(nDensity.data(), nDensity.size());

  mglData rData;
  rData.Set(rInput.data(), rInput.size());

  mglData eData;
  eData.Set(errors.data(), errors.size());

  mglGraph gr(0, 1200, 800);

  float outMin = std::min(hData.Minimal(), nData.Minimal());
  float outMax = std::max(hData.Maximal(), nData.Maximal());

  gr.SetRange('x', rData);
  gr.SetRange('y', outMin, outMax);

  gr.SetFontSize(2);
  gr.SetCoor(mglLogLog);
  gr.Axis();

  gr.Label('x', "Radius [l]", 0);
  gr.Label('y', "Density [m]/[l]^3", 0);

  gr.Plot(rData, hData, "b");
  gr.AddLegend("Hernquist", "b");

  gr.Plot(rData, nData, "r .");
  gr.AddLegend("Numeric", "r .");

  gr.Error(rData, nData, eData, "qo");
  gr.AddLegend("Poissonian Error", "qo");

  gr.Legend();
  gr.WritePNG("density_profiles.png");
}

void step2()
{
  r0 = 0.005;
  int numSteps = 100;

  std::vector<float> softenings;
  std::vector<float> dAnalytic;
  std::vector<float> rInput;

  // creates evenly spaced intervals on a log scale on the interval [r0, radius]
  // drLinToLog(i) gives the start of the ith interval on [r0, radius]
  auto drLinToLog = [&](int i) {
    return r0 * std::pow(radius / r0, (float)i / (float)numSteps);
  };

  // transforms the numerical data into a dimension desirable for plotting
  // here we want to plot the magnitude of the force and add a small nonzero
  // number to the force to avoid problems with 0 on log scales since log(0) is undefined
  auto plotFit = [](float x) {
    return std::abs(x) + std::numeric_limits<float>::epsilon();
  };

  // calculate the analytical force in the hernquist model
  for(int i = 0; i <= numSteps; i++) {
    float r = drLinToLog(i);
    dAnalytic.push_back(plotFit(force_hernquist(r)));
    rInput.push_back(r);
  }

  std::unique_ptr<Gravitysolver::Direct> solver(new Gravitysolver::Direct());
  std::vector<mglData> plotData;

  for(int i = 1; i <= 7; i++) {
    solver->readData("data/direct-nbody-" + std::to_string(i) + ".txt");
    softenings.push_back(solver->softening());

    std::vector<float> dNumeric;

    // project the force vector of each particle towards the center of the system
    const MatrixData &solverData = solver->data();
    Eigen::VectorXf f_center(solverData.cols());

    float fx, fy, fz, x, y, z, norm;

    for(int i = 0; i < solverData.cols(); i++) {
      x = solverData(1, i);
      y = solverData(2, i);
      z = solverData(3, i);
      fx = solverData(7, i);
      fy = solverData(8, i);
      fz = solverData(9, i);

      norm = std::sqrt(x*x + y*y + z*z);

      // project the force vector onto the normalized sphere normal
      f_center(i) = (x*fx + y*fy + z*fz) / norm;
    }

    for(int i = 0; i <= numSteps; i++) {
      float r = drLinToLog(i);
      float r1 = drLinToLog(i + 1);
      float dr = r1 - r;

      float f = 0.0;
      int numParticles = 0;

      // calculate the average gravitational force in a shell
      for(int i = 0; i < particles.size(); i++) {
        Particle p = particles[i];

        if(r <= p.radius() && p.radius() < r1) {
          f += f_center(i);
          numParticles++;
        }
      }

      f = (numParticles == 0 ? .0 : f / numParticles);
      dNumeric.push_back(plotFit(f));
    }

    mglData cData;
    cData.Set(dNumeric.data(), dNumeric.size());
    plotData.push_back(cData);
  }

  mglData aData;
  aData.Set(dAnalytic.data(), dAnalytic.size());

  mglData rData;
  rData.Set(rInput.data(), rInput.size());

  mglGraph gr(0, 1200, 800);

  float outMin;
  float outMax;

  for(int i = 0; i < plotData.size(); i++) {
    outMin = std::max(std::min(plotData[i].Minimal(), aData.Minimal()), 50.0);
    outMax = std::max(plotData[i].Maximal(), aData.Maximal());
  }

  gr.SetRange('x', rData);
  gr.SetRange('y', outMin, outMax);

  gr.SetFontSize(2);
  gr.SetCoor(mglLogLog);
  gr.Axis();

  gr.Label('x', "Radius [l]", 0);
  gr.Label('y', "Force [m]^2[l]^{-2}", 0);

  gr.Plot(rData, aData, "b");
  gr.AddLegend("Analytic", "b");

  // colors for plotting
  const char *opt[7] = {"r +", "c +", "m +", "h +", "l +", "n +", "q +"};

  for(int i = 0; i < plotData.size(); i++) {
    std::stringstream ss;
    ss << "\\epsilon = " << std::setprecision(4) << softenings[i] << " [l]";

    gr.Plot(rData, plotData[i], opt[i]);
    gr.AddLegend(ss.str().c_str(), opt[i]);
  }

  gr.Legend();
  gr.WritePNG("forces.png");
}

void first_task()
{
  calculate_constants();
  step1();
  step2();
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2020 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Global Principal Compoment Analysis filtering                            *
***************************************************************************/

#include <itkImage.h>
#include <Eigen/Dense>
#include <EigenITK.hpp>
#include <ITKUtils.hpp>
#include <PrincipalComponentAnalysis.hpp>
#include <cstdlib>
#include <cmath>

using namespace Eigen;

int main(int argc, char *argv [])
{
    if (argc < 5)
    {
        std::cerr << "Error! Invalid number of arguments!" << std::endl << "Usage: GlobalPCADenoising inputImage maskImage variance outputImage [minComponents=5% of number of components] [maxComponents=25% of number of components] [verbose=0]" << std::endl;
        return EXIT_FAILURE;
    }

    // Typedefs
    typedef itk::Image<float, 4> ComponentsImageType;
    typedef itk::Image<unsigned char, 3> MaskType;
    try
    {
        // Get PWI
        typename ComponentsImageType::Pointer PWI = ITKUtils::ReadNIfTIImage<ComponentsImageType>(std::string(argv[1]));
        // Get mask
        typename MaskType::Pointer mask = ITKUtils::ReadNIfTIImage<MaskType>(std::string(argv[2]));
        // Get variance
        const double variance = std::strtod(argv[3], NULL);
        // Default min and max number of components
        typename ComponentsImageType::SizeType imageSize = PWI->GetLargestPossibleRegion().GetSize();
        unsigned int minComponents = std::ceil(imageSize[3] * 0.0500);
        unsigned int maxComponents = std::ceil(imageSize[3] * 0.3333);
        // Get user min number of components
        if (argc > 5)
            minComponents = std::atoi(argv[5]);
        // Get user max number of components
        if (argc > 6)
            maxComponents = std::atoi(argv[6]);
        // Get user max number of components
        bool verbose = false;
        if (argc > 7)
            verbose = (bool) std::atoi(argv[7]);
        // Print configuration
        if (verbose)
        {
            std::cout << "CONFIGURATION" << std::endl;
            std::cout << "-------------" << std::endl;
            std::cout << "Variance explained" << std::endl;
            std::cout << "\tValue: " << variance << std::endl;
            std::cout << "Number of components" << std::endl;
            std::cout << "\tMinium: " << minComponents << std::endl;
            std::cout << "\tMaximum: " << maxComponents << std::endl;
        }
        // Compute Non-Zeros mask
        typename MaskType::Pointer nonZerosMask = ITKUtils::ZerosMaskIntersect<ComponentsImageType, MaskType>(PWI, mask, true, false, 0.05);
        // Convert to Eigen Matrix
        MatrixXf dataset(EigenITK::toEigen<ComponentsImageType, MaskType>(PWI, nonZerosMask));
        // Compute PCA filtering
        PrincipalComponentAnalysis pca;
        MatrixXf PWIPCARawdata = pca.filteringVarianceExplained(dataset, variance, minComponents, maxComponents);
        if (verbose)
        {
            std::cout << "PCA" << std::endl;
            std::cout << "---" << std::endl;
            std::cout << "Reconstruction with " << pca.components() << " components out of " << imageSize[3] << std::endl;
        }
        // Correct curves with negative values
        #pragma omp parallel for
        for (int i = 0; i < PWIPCARawdata.rows(); i++)
        {
            ArrayXf row = PWIPCARawdata.row(i);
            if ((row <= 0).any())
                PWIPCARawdata.row(i) = (row - row.minCoeff() + 1);
        }
        // Convert to ITK image
        EigenITK::toITK<ComponentsImageType, MaskType>(PWIPCARawdata, nonZerosMask, PWI);
        // Save new filtered image
        ITKUtils::WriteNIfTIImage<ComponentsImageType>(PWI, std::string(argv[4]));
    }
    catch (itk::ExceptionObject & err)
    {
        std::cerr << "ExceptionObject caught !" << std::endl;
        std::cerr << err << std::endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}


//
//  myMath.cpp
//  pbeam
//
//  Created by Andrew Ning on 2/4/12.
//  Copyright (c) 2012 NREL. All rights reserved.
//

//#include <Accelerate/Accelerate.h>
#include <cstdlib> // for malloc
#include <algorithm> // for max/min
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "myMath.h"

using namespace std;

namespace myMath {

  // MARK: ------------- EIGENVALUES --------------

  // solves generalized eigenvalue problem Ax = lambda * Bx
  int generalizedEigenvalues(bool cmpVec, const Matrix &A, const Matrix &B, Vector &eig, Matrix &eig_vec){

    int flag = cmpVec ? Eigen::ComputeEigenvectors : Eigen::EigenvaluesOnly;

    Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> es(A, B, flag);

    eig = es.eigenvalues().real();
    if (cmpVec) eig_vec = es.eigenvectors().real();
    return es.info(); // != Eigen::Success) abort();
   }

  int generalizedEigenvalues(const Matrix &A, const Matrix &B, Vector &eig){
    Matrix empty(0, 0);
    return generalizedEigenvalues(false, A, B, eig, empty);
  }

  int generalizedEigenvalues(const Matrix &A, const Matrix &B, Vector &eig, Matrix &eig_vec){
    return generalizedEigenvalues(true, A, B, eig, eig_vec);
  }

  // -------------------------------------

  // MARK: -------------- LINEAR SYSTEM SOLVER -----------------

  int solveSPDBLinearSystem(const Matrix &A, const Vector &b, Vector &x){
    //x = A.llt().solve(b);
    x = A.ldlt().solve(b);
    return 0;
  }

  //TODO: add unit test for this
  int solveLinearSystem(const Matrix &A, const Vector &b, Vector &x){
    x = A.householderQr().solve(b);
    return 0;
  }

  // -------------------------------------

  // MARK: ------------ INTEGRATION -----------------
  // TODO: add unit tests

  void cumtrapz(const Vector &f, const Vector &x, Vector &y){

    y(0) = 0.0;

    for (int i = 1; i < x.size(); i++) {
      y(i) = y(i-1) + 0.5*(f(i)+f(i-1)) * (x(i)-x(i-1));
    }
  }

  double trapz(const Vector &f, const Vector &x){

    int n = (int) x.size();
    Vector y(n);
    cumtrapz(f, x, y);

    return y(n-1);
  }

  void vectorFromArray(double x[], Vector &v){

    for (int i = 0; i < v.size(); i++) {
      v(i) = x[i];
    }

  }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Point Example - showing geographic (latitude longitude) points

#include <iostream>
#include <iomanip>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/extensions/algorithms/parse.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

// Formula to get the course (direction) between two points.
// This might be a GGL-function in the future.
template <typename P1, typename P2>
inline double get_course(P1 const& p1, P2 const& p2)
{
    double const& lat1 = boost::geometry::get_as_radian<1>(p1);
    double const& lon1 = boost::geometry::get_as_radian<0>(p1);
    double const& lat2 = boost::geometry::get_as_radian<1>(p2);
    double const& lon2 = boost::geometry::get_as_radian<0>(p2);
    // http://williams.best.vwh.net/avform.htm#Crs
    return atan2(sin(lon1-lon2)*cos(lat2),
       cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2));
}

// Formula to calculate the point at a distance/angle from another point
// This might be a GGL-function in the future.
template <typename P1, typename P2>
inline void point_at_distance(P1 const& p1,
        double distance, double tc, double radius,
        P2& p2)
{
    double const two_pi = 2.0 * boost::geometry::math::pi<double>();
    double earth_perimeter = radius * two_pi;
;
    double d = (distance / earth_perimeter) * two_pi;
    double const& lat1 = boost::geometry::get_as_radian<1>(p1);
    double const& lon1 = boost::geometry::get_as_radian<0>(p1);

    // http://williams.best.vwh.net/avform.htm#LL
    double lat = asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc));
    double dlon = atan2(sin(tc)*sin(d)*cos(lat1),cos(d)-sin(lat1)*sin(lat));
    double lon = lon1 - dlon;

    boost::geometry::set_from_radian<1>(p2, lat);
    boost::geometry::set_from_radian<0>(p2, lon);
}

int main()
{
    using namespace boost::geometry;

    typedef model::ll::point<degree> latlon_point;

    latlon_point paris;

    // Assign coordinates to the latlong point, using the methods lat and lon
    // Paris 48 52' 0" N, 2 19' 59" E
    paris.lat(dms<north>(48, 52, 0));
    paris.lon(dms<east>(2, 19, 59));

    std::cout << "Paris: " << boost::geometry::dsv(paris) << std::endl;

    // Constructor using explicit latitude/longitude
    // Lima 12 2' 36" S, 77 1' 42" W
    latlon_point lima(
            latitude<>(dms<south>(12, 2, 36)),
            longitude<>(dms<west>(77, 1, 42)));

    std::cout << "Lima: " << boost::geometry::dsv(lima) << std::endl;

    // Construction with parse utiity
    latlon_point amsterdam = parse<latlon_point>("52 22'23\"N", "4 53'32\"E");
    std::cout << "Amsterdam: " << boost::geometry::dsv(amsterdam) << std::endl;

    // Calculate the distance using the default strategy (Andoyer), and Vincenty
    std::cout << std::setprecision(9);
    std::cout << "Distance Paris-Lima, Andoyer (default) "
        << 0.001 * distance(paris, lima)
        << " km" << std::endl;

    std::cout << "Distance Paris-Lima, Vincenty "
        << 0.001 * distance(paris, lima, strategy::distance::vincenty<double>())
        << " km" << std::endl;

    // Using great circle (=haversine), this is less precise because earth is not a sphere
    double const average_earth_radius = 6372795.0;
    std::cout << "Distance Paris-Lima, great circle "
        << 0.001 * distance(paris, lima, strategy::distance::haversine<double>(average_earth_radius))
        << " km" << std::endl;

    // Convert a latlong point to radians. This might be convenient, although algorithms
    // are transparent on degree/radians
    model::ll::point<radian> paris_rad;
    transform(paris, paris_rad);
    std::cout << "Paris in radians: " << boost::geometry::dsv(paris_rad) << std::endl;

    model::ll::point<radian> amsterdam_rad;
    transform(amsterdam, amsterdam_rad);
    std::cout << "Amsterdam in radians: " << boost::geometry::dsv(amsterdam_rad) << std::endl;

    std::cout << "Distance Paris-Amsterdam, (degree) " << 0.001 * distance(paris, amsterdam) << " km" << std::endl;
    std::cout << "Distance Paris-Amsterdam, (radian) " << 0.001 * distance(paris_rad, amsterdam_rad) << " km" << std::endl;

    std::cout << "Distance Paris-Amsterdam, (mixed) " << 0.001 * distance(paris, amsterdam_rad) << " km" << std::endl;

    // Other way round: have Amsterdam and go 430 km to the south (i.e. first calculate direction)
    double tc = get_course(amsterdam, paris);
    std::cout << "Course: " << (tc * boost::geometry::math::r2d) << std::endl;

    latlon_point paris_calculated;
    point_at_distance(amsterdam, 430 * 1000.0, tc, average_earth_radius, paris_calculated);
    std::cout << "Paris calculated (degree): " << boost::geometry::dsv(paris_calculated) << std::endl;

    return 0;
}


#ifndef MLT_UTILS_OPTIMIZERS_GRADIENT_DESCENT_UPDATES_HPP
#define MLT_UTILS_OPTIMIZERS_GRADIENT_DESCENT_UPDATES_HPP

#include <Eigen/Core>

#include "../../defs.hpp"

namespace mlt {
namespace utils {
namespace optimizers {
	// Implementation of the Vanilla Gradient Descent update rule
	class VanillaGradientDescentUpdate {
	public:
		void restart() {}

		auto step(double learning_rate, MatrixXdRef gradient) {
			return (-learning_rate * gradient).eval();
		}
	};

	// Implementation of the Momentum Gradient Descent update rule
	// Parameters:
	// - double mu: amount of momentum applied on each descent
	class MomentumGradientDescentUpdate {
	public:
		MomentumGradientDescentUpdate(double mu = 0.9) : _mu(mu), _init(false) {}

		void restart() { _init = false; }

		auto step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			_cache = _mu * _cache - learning_rate * gradient;
			_init = true;
			return _cache;
		}

	protected:
		bool _init;
		double _mu;
		MatrixXd _cache;
	};

	// Implementation of the Nesterov's Accelerated Momentum Gradient Descent update rule
	// Parameters:
	// - double mu: amount of momentum applied on each descent
	class NesterovMomentumGradientDescentUpdate {
	public:
		NesterovMomentumGradientDescentUpdate(double mu = 0.9) : _mu(mu), _init(false) {}

		void restart() { _init = false; }

		auto step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			VectorXd velocity_prev = _cache;
			_cache = _mu * _cache - learning_rate * gradient;
			_init = true;
			return (-_mu * velocity_prev + (1 + _mu) * _cache).eval();
		}

	protected:
		bool _init;
		double _mu;
		MatrixXd _cache;
	};

	// Implementation of the Adagrad Gradient Descent update rule
	class AdagradGradientDescentUpdate {
	public:
		AdagradGradientDescentUpdate() : _init(false) {}

		void restart() { _init = false; }

		auto step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			_cache += gradient.array().pow(2).matrix();
			_init = true;
			return (-learning_rate * (gradient.array() / (_cache.array() + 1e-8).sqrt()).matrix()).eval();
		}

	protected:
		bool _init;
		MatrixXd _cache;
	};

	// Implementation of the RMSProp Gradient Descent update rule
	// Parameters:
	// - double decay_rate: the decay rate of the moving average of squared gradients at each step
	class RMSPropGradientDescentUpdate {
	public:
		RMSPropGradientDescentUpdate(double decay_rate = 0.9) : _decay_rate(decay_rate), _init(false) {}

		void restart() { _init = false; }

		MatrixXd step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			_cache = _decay_rate * _cache + (1 - _decay_rate) * gradient.array().pow(2).matrix();
			_init = true;
			return (-learning_rate * (gradient.array() / (_cache.array() + 1e-8).sqrt()).matrix()).eval();
		}

	protected:
		bool _init;
		double _decay_rate;
		MatrixXd _cache;
	};
}
}
}
#endif


#pragma once

#include <Eigen/Dense>

#include <cmath>
#include <iostream>
#include <type_traits>

namespace pcv
{

template<class NumericType0, class NumericType1>
double getEuclideanDistance(
    const Eigen::Matrix<NumericType0, 2, 1> &point0,
    const Eigen::Matrix<NumericType1, 2, 1> &point1)
{
    return std::sqrt(
            std::pow(point0.x()-point1.x(), 2)
          + std::pow(point0.y()-point1.y(), 2));
}

template<class NumericType>
std::optional<Eigen::Vector2d> getSegmentIntersectionPoint2(
    const Eigen::Matrix<NumericType, 2, 1> &segmentOnePoint0,
    const Eigen::Matrix<NumericType, 2, 1> &segmentOnePoint1,
    const Eigen::Matrix<NumericType, 2, 1> &segmentTwoPoint0,
    const Eigen::Matrix<NumericType, 2, 1> &segmentTwoPoint1,
    double error = 1e-10)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    Eigen::Matrix<double, 2, 2> linearEquationsLhs;
    linearEquationsLhs <<
        segmentOnePoint0.y() - segmentOnePoint1.y(),
        segmentOnePoint1.x() - segmentOnePoint0.x(),
        segmentTwoPoint0.y() - segmentTwoPoint1.y(),
        segmentTwoPoint1.x() - segmentTwoPoint0.x();

    Eigen::Vector2d linearEquationsRhs;
    linearEquationsRhs <<
          segmentOnePoint0.y() * (segmentOnePoint1.x() - segmentOnePoint0.x())
        - segmentOnePoint0.x() * (segmentOnePoint1.y() - segmentOnePoint0.y()),
          segmentTwoPoint0.y() * (segmentTwoPoint1.x() - segmentTwoPoint0.x())
        - segmentTwoPoint0.x() * (segmentTwoPoint1.y() - segmentTwoPoint0.y());

    Eigen::Vector2d intersection =
        linearEquationsLhs.colPivHouseholderQr().solve(linearEquationsRhs);

    if (intersection.x() >= std::min(segmentOnePoint0.x(), segmentOnePoint1.x())-error
        && intersection.x() <= std::max(segmentOnePoint0.x(), segmentOnePoint1.x())+error
        && intersection.y() >= std::min(segmentOnePoint0.y(), segmentOnePoint1.y())-error
        && intersection.y() <= std::max(segmentOnePoint0.y(), segmentOnePoint1.y())+error
        && intersection.x() >= std::min(segmentTwoPoint0.x(), segmentTwoPoint1.x())-error
        && intersection.x() <= std::max(segmentTwoPoint0.x(), segmentTwoPoint1.x())+error
        && intersection.y() >= std::min(segmentTwoPoint0.y(), segmentTwoPoint1.y())-error
        && intersection.y() <= std::max(segmentTwoPoint0.y(), segmentTwoPoint1.y())+error)
    {
        return intersection;
    }

    return {};
}

}


#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

int main()
{
  for (int size=1; size<=4; ++size)
  {
    MatrixXi m(size,size+1);         // a (size)x(size+1)-matrix of int's
    for (int j=0; j<m.cols(); ++j)   // loop over columns
      for (int i=0; i<m.rows(); ++i) // loop over rows
        m(i,j) = i+j*m.rows();       // to access matrix coefficients,
                                     // use operator()(int,int)
    std::cout << m << "\n\n";
  }

  VectorXf v(4); // a vector of 4 float's
  // to access vector coefficients, use either operator () or operator []
  v[0] = 1; v[1] = 2; v(2) = 3; v(3) = 4;
  std::cout << "\nv:\n" << v << std::endl;
}


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

using namespace std;
using namespace Eigen;

int main() {
  Matrix3f A;
  Vector3f b;
  A << 1, 2, 3, 4, 5, 6, 7, 8, 10;
  b << 3, 3, 4;
  cout << "Here is the matrix A:\n" << A << endl;
  cout << "Here is the vector b:\n" << b << endl;
  Vector3f x = A.colPivHouseholderQr().solve(b);
  cout << "The solution is:\n" << x << endl;
}


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

template<typename vectype>
double Clenshaw1D(const vectype &c, double ind){
    int N = static_cast<int>(c.size()) - 1;
    double u_k = 0, u_kp1 = 0, u_kp2 = 0;
    for (int k = N; k >= 0; --k){
        // Do the recurrent calculation
        u_k = 2.0*ind*u_kp1 - u_kp2 + c[k];
        if (k > 0){
            // Update the values
            u_kp2 = u_kp1; u_kp1 = u_k;
        }
    }
    return (u_k - u_kp2)/2;
}

/// With STL datatypes
template<typename Mat, typename vectype>
double Clenshaw2D(const Mat& a, double x, double y, vectype& b) {
    std::size_t m = a.size() - 1;
    std::size_t n = a[0].size() - 1;
    for (auto i = 0; i < b.size(); ++i) {
        b[i] = Clenshaw1D(a[i], y);
    }
    return Clenshaw1D(b, x);
}

template<typename MatType, int Cols = MatType::ColsAtCompileTime>
auto Clenshaw1DByRow(const MatType& c, double ind) {
    int N = static_cast<int>(c.rows()) - 1;
    static Eigen::Array<double, 1, Cols> u_k, u_kp1, u_kp2;
    // Not statically sized
    if constexpr (Cols < 0) {
        int M = c.rows();
        u_k.resize(M);
        u_kp1.resize(M);
        u_kp2.resize(M);
    }
    u_k.setZero(); u_kp1.setZero(); u_kp2.setZero();

    for (int k = N; k >= 0; --k) {
        // Do the recurrent calculation
        u_k = 2.0 * ind * u_kp1 - u_kp2 + c.row(k);
        if (k > 0) {
            // Update the values
            u_kp2 = u_kp1; u_kp1 = u_k;
        }
    }
    return (u_k - u_kp2) / 2;
}

/// With Eigen datatypes
template<typename MatType>
double Clenshaw2DEigen(const MatType& a, double x, double y) {
    auto b = Clenshaw1DByRow(a, y);
    return Clenshaw1D(b.matrix(), x);
}

template<int Rows, int Cols>
void test_Eigen(int M){
    using MatType = Eigen::Array<double, Rows, Cols>;
    MatType aa;
    if constexpr ((Rows < 0) || (Cols < 0)) {
        aa.resize(M + 1, M + 1);
    }
    else{
        aa.resize(M + 1, M + 1);
    }
    aa.fill(0.0);
    for (auto i = 0; i < M + 1; ++i) {
        for (auto j = 0; j < M + 1; ++j) {
            aa(i, j) = i + j;
        }
    }
    int N = 1000 * 1000;
    volatile auto r = 0.0, x = 0.1, y = 0.7;
    auto startTime = std::chrono::system_clock::now();
    for (int i = 0; i < N; ++i) {
        auto v = Clenshaw2DEigen(aa, x, y);
        r += v;
    }
    auto endTime = std::chrono::system_clock::now();
    auto elap_us = std::chrono::duration<double>(endTime - startTime).count() / N * 1e6;
    std::cout << elap_us << " us/call. (Eigen-powered) value:" << (r / N) << std::endl;
}

int main(){
    std::vector<std::vector<double>> a;
    const int M = 8;
    for (auto i = 0; i <= M; ++i){
        a.push_back(std::vector<double>(M+1, 0.0)); // One would normally use Eigen here, but the challenge is to use only standard library elements...
    }
    {
        for (auto i = 0; i < M + 1; ++i) {
            for (auto j = 0; j < M + 1; ++j) {
                a[i][j] = i + j;
            }
        }
        std::vector<double> b(M+1, 0.0);
        int N = 1000 * 1000;
        volatile auto r = 0.0, x = 0.1, y = 0.7;
        auto startTime = std::chrono::system_clock::now();
        for (int i = 0; i < N; ++i) {
            auto v = Clenshaw2D(a, x, y, b);
            r += v;
        }
        auto endTime = std::chrono::system_clock::now();
        auto elap_us = std::chrono::duration<double>(endTime - startTime).count() / N * 1e6;
        std::cout << elap_us << " us/call. value:" << (r / N) << std::endl;
    }
    std::cout  << "Dynamic:" << std::endl;
    test_Eigen<Eigen::Dynamic, Eigen::Dynamic>(M);

    std::cout  << "Static:" << std::endl;
    test_Eigen<M+1, M+1>(M);
}


// https://projecteuler.net/problem=57
/*
Square root convergents

It is possible to show that the square root of two can be expressed as
an infinite continued fraction.

2^1/2 = 1 + 1/(2 + 1/(2 + 1/(2 + ... ))) = 1.414213...

By expanding this for the first four iterations, we get:

1 + 1/2 = 3/2 = 1.5
1 + 1/(2 + 1/2) = 7/5 = 1.4
1 + 1/(2 + 1/(2 + 1/2)) = 17/12 = 1.41666...
1 + 1/(2 + 1/(2 + 1/(2 + 1/2))) = 41/29 = 1.41379...

The next three expansions are 99/70, 239/169, and 577/408, but the eighth expansion,
1393/985, is the first example where the number of digits in the numerator exceeds
the number of digits in the denominator.

In the first one-thousand expansions, how many fractions contain a numerator
with more digits than denominator?

Solution:
*/

#include <iostream>
#include <cstdint>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>
#include <chrono>

auto compute() {
	using namespace boost::multiprecision;
	constexpr uint32_t limit = 1000;

	cpp_int natural = 3, den = 2;
	uint32_t count = 0;
	for (uint32_t t = 2; t <= limit; ++t) {
		natural += den << 1;
		den = natural - den;
		if (natural.str().length() > den.str().length()) {
			++count;
			//std::cout << t << ',';
		}
	}

	return count;
}

#ifdef _MSC_VER
	template <class T>
	inline void DoNotOptimize(const T &value) {
		__asm { lea ebx, value }
	}
#else
	template <class T>
	__attribute__((always_inline)) inline void DoNotOptimize(const T &value) {
		asm volatile("" : "+m"(const_cast<T &>(value)));
	}
#endif

int main() {
	using namespace std;
	using namespace chrono;
	auto start = high_resolution_clock::now();
	auto result = compute();
	DoNotOptimize(result);
	cout << "Done in "
		<< duration_cast<nanoseconds>(high_resolution_clock::now() - start).count() / 1e6
		<< " miliseconds." << endl;
	cout << result << endl;
}


// Ax=b
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
   Matrix3f A;
   Vector3f b;
   A << 1,2,3,  4,5,6,  7,8,10;
   b << 3, 3, 4;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector b:\n" << b << endl;
   Vector3f x = A.colPivHouseholderQr().solve(b); // QR decompsition
   // ColPivHouseholderQR<Matrix3f> dec(A);
   // Vector3f x = dec.solve(b);
   cout << "The solution is:\n" << x << endl;
}

PartialPivLU            partialPivLu()          Invertible              ++      ++      +
FullPivLU               fullPivLu()             None                    -       - -     +++
HouseholderQR           householderQr()         None                    ++      ++      +
ColPivHouseholderQR     colPivHouseholderQr()   None                    ++      -       +++
FullPivHouseholderQR    fullPivHouseholderQr()  None                    -       - -     +++
LLT                     llt()                   Positive definite       +++     +++     +
LDLT                    ldlt()                  Positive or
                                                negative semidefinite   +++     +       ++
JacobiSVD               jacobiSvd()             None                    - -     - - -   +++


/**
	@file rotation.cpp
	Utilities for expressing and converting rotations of reference frames

	@brief All of the rotation parameterizations here represent rotations
	of an initial fixed frame to a new frame. Therefore using these
	parameterizations to operate on a vector is the same as transforming
	the new frame coordinates into the original fixed frame.

	Examples if rotation describes relation of body w.r.t. inertial
	(how to rotate inertial to body):
	R: coverts body frame coordinates into inertial frame coordinates
	euler: usual roll-pitch-yaw of body frame w.r.t inertial frame

*/

#include <utils/rotation.hpp>

#include <Eigen/Dense>
#include <cmath>

/**
    @brief Converts the rotation matrix R to the Euler angles (ZYX)
    (Yaw-Pitch-Roll) (psi, theta, phi) used for aircraft conventions.
    Note to compute the Euler angles for the aircraft this should be
    the R matrix that converts a vector from body frame coordinates
    to a vector in inertial frame coordinates.

    @param[in] R  rotation matrix
    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
*/
void Rot::R_to_euler(const Eigen::Matrix3d& R, Eigen::Vector3d& e) {
    // Check for singularity that occurs when pitch = 90 deg
    if (sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) >= 1e-6) {
    	e(0) = atan2(R(2,1), R(2,2));
        e(1) = asin(-R(2,0));
        e(2) = atan2(R(1,0), R(0,0));
    }
    else {
    	e(0) = atan2(-R(1,2), R(1,1));
        e(1) = asin(-R(2,0));
        e(2) = 0.0;
    }
}

/**
    @brief Converts Euler angles (ZYX) (Yaw-Pitch-Roll) (psi, theta, phi)
    into the inertial to body rotation matrix

    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
    @param[in] R  rotation matrix
*/
void Rot::euler_to_R(const Eigen::Vector3d& e, Eigen::Matrix3d& R) {
    R(0,0) = cos(e(1))*cos(e(2));
    R(0,1) = sin(e(0))*sin(e(1))*cos(e(2)) - cos(e(0))*sin(e(2));
    R(0,2) = sin(e(0))*sin(e(2)) + cos(e(0))*sin(e(1))*cos(e(2));
    R(1,0) = cos(e(1))*sin(e(2));
    R(1,1) = cos(e(0))*cos(e(2)) + sin(e(0))*sin(e(1))*sin(e(2));
    R(1,2) = cos(e(0))*sin(e(1))*sin(e(2)) - sin(e(0))*cos(e(2));
    R(2,0) = -sin(e(1));
    R(2,1) = sin(e(0))*cos(e(1));
    R(2,2) = cos(e(0))*cos(e(1));
}

/**
    @brief Converts a unit quaternion into a rotation matrix

    @param[in] q  unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
    @param[in] R  rotation matrix
*/
void Rot::quat_to_R(const Eigen::Vector4d& q, Eigen::Matrix3d& R) {
    R(0,0) = 1.0 - 2.0*q(2)*q(2) - 2.0*q(3)*q(3);
    R(0,1) = 2.0*q(1)*q(2) - 2.0*q(3)*q(0);
    R(0,2) = 2.0*q(1)*q(3) + 2.0*q(2)*q(0);
    R(1,0) = 2.0*q(1)*q(2) + 2.0*q(3)*q(0);
    R(1,1) = 1.0 - 2.0*q(1)*q(1) - 2.0*q(3)*q(3);
    R(1,2) = 2.0*q(2)*q(3) - 2.0*q(1)*q(0);
    R(2,0) = 2.0*q(1)*q(3) - 2.0*q(2)*q(0);
    R(2,1) = 2.0*q(2)*q(3) + 2.0*q(1)*q(0);
    R(2,2) = 1.0 - 2.0*q(1)*q(1) - 2.0*q(2)*q(2);
}

/**
    @brief Converts a unit quaternion into the aircraft Euler angles
    (ZYX) (Yaw-Pitch-Roll) (psi, theta, phi)

    @param[in] q  unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
*/
void Rot::quat_to_euler(const Eigen::Vector4d& q, Eigen::Vector3d& e) {
    double R00 = 1.0 - 2.0*q(2)*q(2) - 2.0*q(3)*q(3);
    double R10 = 2.0*q(1)*q(2) + 2*q(3)*q(0);
    if (sqrt(R00*R00 + R10*R10 >= 1e-6)) {
        e(0) = atan2(2.0*q(2)*q(3) + 2.0*q(1)*q(0), 1.0 - 2.0*q(1)*q(1) - 2.0*q(2)*q(2));
        e(1) = asin(-2.0*q(1)*q(3) + 2.0*q(2)*q(0));
        e(2) = atan2(R10, R00);
    }
    else {
        e(0) = atan2(-2.0*q(2)*q(3) - 2.0*q(1)*q(0), 1.0 - 2.0*q(1)*q(1) - 2.0*q(3)*q(3));
        e(1) = asin(-2.0*q(1)*q(3) + 2.0*q(2)*q(0));
        e(2) = 0.0;
    }
}

/**
    @brief Converts aircraft Euler angles (ZYX) (Yaw-Pitch-Roll) (psi, theta, phi)
    into unit quaternion

    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
    @param[in] q  unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
*/
void Rot::euler_to_quat(const Eigen::Vector3d& e, Eigen::Vector4d& q) {
    double phi = 0.5 * e(0);
    double th = 0.5 * e(1);
    double psi = 0.5 * e(2);
    q(0) = cos(phi) * cos(th) * cos(psi) + sin(phi) * sin(th) * sin(psi);
    q(1) = sin(phi) * cos(th) * cos(psi) - cos(phi) * sin(th) * sin(psi);
    q(2) = cos(phi) * sin(th) * cos(psi) + sin(phi) * cos(th) * sin(psi);
    q(3) = cos(phi) * cos(th) * sin(psi) - sin(phi) * sin(th) * cos(psi);
}

/**
    @brief Converts an axis and angle into a unit quaternion

    @param[in] aa  axis/angle aa = (x, y, z), th = ||aa||, e = aa/th
    @param[in] q   unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
*/
void Rot::axis_to_quat(const Eigen::Vector3d& aa, Eigen::Vector4d& q) {
    double th = aa.norm();
    if (th < 0.0000001) {
        q(0) = 1.0;
        q(1) = 0.0;
        q(2) = 0.0;
        q(3) = 0.0;
    }
    else {
        Eigen::Vector3d e = aa.normalized();
        q(0) = cos(th/2.0);
        q(1) = e(0)*sin(th/2.0);
        q(2) = e(1)*sin(th/2.0);
        q(3) = e(2)*sin(th/2.0);
    }
}

/**
    @brief Converts a unit quaternion into a normalized axis and angle.

    @param[in] q   unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
    @param[in] aa  axis/angle aa = (x, y, z), th = ||aa||, e = aa/th
*/
void Rot::quat_to_axis(const Eigen::Vector4d& q, Eigen::Vector3d& aa) {
    if (q(0) > 0.9999999) { // no rotation
        aa(0) = 0.0;
        aa(1) = 0.0;
        aa(2) = 0.0;
	}
    else {
        double m = 2.0*acos(q(0))/sqrt(1.0 - q(0)*q(0));
        aa(0) = m*q(1);
        aa(1) = m*q(2);
        aa(2) = m*q(3);

        // Scale to keep magnitude less than pi
        double th = aa.norm();
        if (th > M_PI) {
            double th_new = 2 * M_PI - th;
            aa = -(th_new/th)*aa;
        }
    }
}

/**
    @brief Composes two rotations parameterized by unit quaternions
    and outputs a single quaternion representing the composed rotation.
    This performs rotation p first and then rotation q after.

    @param[in] p   unit quaternion p = (pw, px, py, pz) = pw + (px i, py j, pz k)
    @param[in] q   unit quaternion q = (qw, qx, qy, qz) = qw + (qx i, qy j, qz k)
    @param[in] o   composed unit quaternion o = (ow, ox, oy, oz) = ow + (ox i, oy j, oz k)
*/
void Rot::compose_quats(const Eigen::Vector4d& p, const Eigen::Vector4d q, Eigen::Vector4d& o) {
    o(0) = p(0)*q(0) - (p(1)*q(1) + p(2)*q(2) + p(3)*q(3));
    o(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2);
    o(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1);
    o(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0);
}

/**
    @brief Inverts a unit quaternion, gives the opposite rotation

    @param[in] q   unit quaternion q = (qw, qx, qy, qz) = qw + (qx i, qy j, qz k)
*/
void Rot::invert_quat(Eigen::Vector4d& q) {
    q(1) *= -1.0;
    q(2) *= -1.0;
    q(3) *= -1.0;
}

double Rot::rad_to_deg(double th) {
    return th*(180.0/M_PI);
}

double Rot::deg_to_rad(double th) {
    return th*(M_PI/180.0);
}

/**
    @brief Wraps angle th [rad] to range [0, 2pi]
*/
double Rot::wrap_to_2pi(double th) {
    th = fmod(th, 2.0*M_PI);
    if (th < 0) return th + 2.0 * M_PI;
    else return th;
}

/**
    @brief Wraps angle th [rad] to range [-pi, pi]
*/
double Rot::wrap_to_pi(double th) {
    return Rot::wrap_to_2pi(th + M_PI) - M_PI;
}


// Computer the winding number of a polygon at a point
// Author: Shayan Hoshyari

#ifndef polyvec_winding_number_
#define polyvec_winding_number_

#include <Eigen/Core>

namespace polyvec {
    namespace WindingNumber {
        // Input:
        // polygon: polygon 2 x numpoints matrix
        // point: the point to compute the winding number at
        // Returns:
        // winding_number: value of winding number
        // is_trustable: is the number trustable, or are we too close to the boundary?
        // NOTE: assumes that the polygon points are sorted in CCW order.
        // multiply the answer by -1 if the order is CW.
        void compute_winding ( const Eigen::Matrix2Xd& polygon, const Eigen::Vector2d& point,  double& winding_number, bool& is_trustable );

        // Input:
        // polygon: polygon 2 x numpoints matrix
        // Returns:
        // winding_number: is ccw
       void compute_orientation ( const Eigen::Matrix2Xd& polygon,  bool &is_ccw, double &area );
       void compute_orientation ( const Eigen::Matrix2Xd& polygon,  bool &is_ccw );

    }
} // end of polyvec

#endif


#include "SGFitLeastSquares.h"
#include <Eigen/Eigen>
#include <Eigen/nnls.h>

namespace Probulator
{
	SgBasis sgFitLeastSquares(const SgBasis& basis, const std::vector<RadianceSample>& samples)
	{
		using namespace Eigen;
		SgBasis result = basis;

		MatrixXf A;
		A.resize(samples.size(), basis.size());
		for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
		{
			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				A(sampleIt, lobeIt) = sgEvaluate(basis[lobeIt].p, basis[lobeIt].lambda, samples[sampleIt].direction);
			}
		}

		for (u32 channelIt = 0; channelIt < 3; ++channelIt)
		{
			VectorXf b;
			b.resize(samples.size());
			for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
			{
				b[sampleIt] = samples[sampleIt].value[channelIt];
			}

			VectorXf x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				result[lobeIt].mu[channelIt] = x[lobeIt];
			}
		}

		return result;
	}

	// Non-negative version of least squares
	SgBasis sgFitNNLeastSquares(const SgBasis& basis, const std::vector<RadianceSample>& samples)
	{
		using namespace Eigen;
		SgBasis result = basis;

		MatrixXf A;
		A.resize(samples.size(), basis.size());
		for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
		{
			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				A(sampleIt, lobeIt) = sgEvaluate(basis[lobeIt].p, basis[lobeIt].lambda, samples[sampleIt].direction);
			}
		}

		NNLS<MatrixXf> nnlssolver(A);
		for (u32 channelIt = 0; channelIt < 3; ++channelIt)
		{
			VectorXf b;
			b.resize(samples.size());
			for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
			{
				b[sampleIt] = samples[sampleIt].value[channelIt];
			}

			// -- run the solver
			nnlssolver.solve(b);
			VectorXf x = nnlssolver.x();

			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				result[lobeIt].mu[channelIt] = x[lobeIt];
			}
		}

		return result;
	}
}


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

using namespace std;

int furthest_dist(const weighted_graph &G, int s) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));

  int max_dist = 0;
  for(int i = 1; i < n; i++) {
    if(dist_map[i] > max_dist)
      max_dist = dist_map[i];
  }
  return max_dist;
}

int span_tree_weight(const weighted_graph &G, const weight_map &weights) {
  std::vector<edge_desc> mst;    // vector to store MST edges (not a property map!)

  boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst));
  int total_weight = 0;
  for (std::vector<edge_desc>::iterator it = mst.begin(); it != mst.end(); ++it) {
    total_weight += weights[*it];
  }
  return total_weight;
}

void testcase() {
  int n; cin >> n;
  int m; cin >> m;
  weighted_graph G(n);
  weight_map weights = boost::get(boost::edge_weight, G);
  edge_desc e;
  for(int i = 0; i < m; i++) {
    int u,v; cin >> u; cin >> v;
    int w; cin >> w;
    e = boost::add_edge(u, v, G).first; weights[e] = w;
  }

  cout << span_tree_weight(G, weights) << " ";
  cout << furthest_dist(G, 0) << endl;

}
int main()
{
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  for(int i = 0; i < t; i++)
    testcase();
  return 0;
}


#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <map>
#include <vector>
#include <iostream>
#include <iomanip>

static void boost_uniform( const int trials , unsigned seed )
{
  // Generate numbers from a uniforma distribution using boost
  // Roll dice and keep track of the some of the dice
  // Print the resulting distribuiton for the trial

  boost::mt19937 random_num_gen( seed );
  boost::uniform_int<> urange(1,6);
  boost::variate_generator< boost::mt19937 , boost::uniform_int<> > dice( random_num_gen , urange );

  // create a map to store the count of the number of occurrences of the sum on the dist
  // first is the dice sum the second is the count;
  std::map<int,int> throw_cnt;
  for( int i = 2 ; i <=12 ; i++ ) throw_cnt[i]=0;

  for( int i = 0; i < trials ; i++ )
    {
      int d1 = dice() , d2 = dice() , s = d1 + d2;
      std::map<int,int>::iterator itr = throw_cnt.find(s);
      (*itr).second++;
    }

  double n = static_cast<double>(trials);
  std::cout << "Dice Throw : Trials = " << std::setprecision(1) << std::scientific << n << std::endl;
  for( std::map<int,int>::iterator itr = throw_cnt.begin() ; itr != throw_cnt.end() ; ++itr )
    {
      double p = static_cast<double>((*itr).second) / n;
      std::cout << std::setw(3) << (*itr).first << "," << std::setw(10) << std::setprecision(8) << std::fixed << p << std::endl;
    }
  std::cout << std::endl;
}

static void boost_normal( const int trials , unsigned seed , double mean , double variance )
{
  // Generate Normal Random Variables using boost libraties
  // Then assess the density at +/- 2 sigma and +/- 1 sigma

  boost::mt19937 random_num_gen( seed );
  boost::normal_distribution<double> n_dist(mean,variance);
  boost::variate_generator< boost::mt19937 , boost::normal_distribution<double> > normal_rv( random_num_gen , n_dist );

  std::vector<double> dist;
  dist.reserve(trials);
  for( int i = 0 ; i < trials ; i++ )
    {
      dist.push_back( normal_rv() );
    }

  std::sort( dist.begin(), dist.end() );

  double s = sqrt(variance);

  if( dist.empty() ) return;
  std::vector<double>::iterator itr_2slb = std::lower_bound( dist.begin() , dist.end() , -2.0*s );
  std::vector<double>::iterator itr_2sub = std::lower_bound( dist.begin() , dist.end() , 2.0*s );

  std::vector<double>::iterator itr_1slb = std::lower_bound( dist.begin() , dist.end() , -1.0*s );
  std::vector<double>::iterator itr_1sub = std::lower_bound( dist.begin() , dist.end() , 1.0*s );

  double nobs_2sd = itr_2sub - itr_2slb;
  double nobs_1sd = itr_1sub - itr_1slb;
  double n = static_cast<double>( trials );
  std::cout << "Normal RV : ";
  std::cout << std::setw(8) << std::setprecision(1) << std::scientific << static_cast<double>(trials);
  std::cout << " : mean +/- 2sigma % = " << std::setw(10) << std::setprecision(6) << std::fixed << nobs_2sd/n;
  std::cout << " : mean +/- 1sigma % = " << std::setw(10) << std::setprecision(6) << std::fixed << nobs_1sd/n;
  std::cout << std::endl;
}

static void boost_number_gen()
{
  const unsigned  seed = 5489u;;
  boost_uniform(1e3,seed);
  boost_uniform(1e6,seed);
  boost_uniform(1e7,seed);

  const double mean= 0.0 , variance = 1.0 ;
  boost_normal( 1e2 , seed , mean , variance );
  boost_normal( 1e3 , seed , mean , variance );
  boost_normal( 1e6 , seed , mean , variance );
  boost_normal( 1e7 , seed , mean , variance );
}