/*
######################################################
Introduction to Criptography - 2021 - Homework2
Last modification (18-Mar-2021)

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

Pseudo Random Number Generators:
1. Blum-Blum-Shub
2. Jacobi generator (a) with p&q b)with generalized components p1,p2,...pi)
######################################################

Resources:
1. https://pdfs.semanticscholar.org/c19b/91cdc1da67c52e606cd4752472ce0db83131.pdf
2. https://link.springer.com/content/pdf/10.1007/0-387-34799-2_13.pdf
3. https://libntl.org/ (Big number library)
4. https://en.wikipedia.org/wiki/Blum_Blum_Shub
5. https://asecuritysite.com/encryption/blum

*/
#include <NTL/ZZ.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <utility>

using namespace std;
using namespace NTL;

const int RUNS = 5;

class blum_blum_shub {
private:
	 ZZ p;
	 ZZ q;
	 ZZ m;
	 ZZ seed;
	 ZZ random;
	 int length;

	 ZZ primeCongruentTo3Mod4(int length) {
		 ZZ aux;
		 GenPrime(aux, length);
		 while (aux % 4 != 3) {
			 GenPrime(aux, length);
		 }
		 return aux;
	 }

	 ZZ seedGenerator(ZZ& p, ZZ& q) {
		 ZZ aux;
		 ZZ m = p * q;
		 aux = RandomBnd(m);
		 while (aux % p == 0 || aux % q == 0) {
			 aux = RandomBnd(m);
		 }
		 return aux;
	 }

public:
	blum_blum_shub(int& length) {
		this->length = length;
		p = primeCongruentTo3Mod4(length);
		q = primeCongruentTo3Mod4(length);
		m = p * q;
		seed = seedGenerator(p, q);
		random = seed;
	}

	ZZ random_bbs() {
		random = PowerMod(random, 2, m);
		return random;
	}

	void printAll() {
		cout << "------------------------------------------------------------------------------\n";
		cout << "P:" << p << '\n';
		cout << "Q:" << q << '\n';
		cout << "M:" << m << '\n';
		cout << "Seed:" << seed << '\n';
		cout << "Random:" << random << '\n';
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}

};

/*
1.Modulus division by a power-of-2-number can be optimized.
		 a % (power_of_2) == a & (power_of_2-1)
 2. Division and multiplication by powers of two can also be optimized by shifting bits.
		a / (power_of_2) == a >>= n where 2^n= power_of_2
 */
ZZ jacobi_calculator(ZZ a, ZZ n) {
	if (n % 2 == 0) {
		cout << "Second parameter must be odd";
		return (ZZ)0;
	}
	ZZ b = a % n;
	ZZ c = n;
	ZZ s = (ZZ)1;
	while (b >= 2) {

		while ((b & 3) == 0) {						// b % 4
			b >>= 2;								// b /= 4
		}
		if ((b & 1) == 0) {							// b % 2
			if ((c & 7) == 3 || (c & 7) == 5) {		// c % 8
				s = -s;
			}
			b >>= 1;								// b /= 2
		}
		if (b == 1) break;
		if ((b & 3) == 3 && (c & 3) == 3) {			// b % 4
			s = -s;
		}
		if (c % b == 0) {
			b = 0;
			break;
		}
		ZZ aux = b;
		b = c % b;
		c = aux;
	}
	return s * b;
}

class jacobi {
private:
	ZZ p;
	ZZ q;
	ZZ m;
	ZZ seed;
	ZZ random;
	vector<ZZ> m_components;
	int length;
	int l;
	vector<bool> generated_binary;
public:
	jacobi(int length, int l) {
		this->length = length;
		this->l = l;
		GenPrime(p, length);
		GenPrime(q, length);
		while (q % 2!=1) {
			GenPrime(q, length);
		}
		m = p * q;
	}

	jacobi(int NR_OF_PRIMES, int PRIME_LENGTH,bool second) {
		m = 1;
		for (int i = 0; i < NR_OF_PRIMES; ++i) {
			ZZ aux;
			GenPrime(aux, PRIME_LENGTH);
			m_components.push_back(aux);
			m *= aux;
		}
		l = PRIME_LENGTH * NR_OF_PRIMES;
	}

	vector<bool> generate_random(int LENGTH) {
		vector<ZZ> a_components;
		for (int i = 0; i < m_components.size(); ++i) {
			ZZ aux;
			aux = RandomBnd(m);
			a_components.push_back(aux);
		}
		ZZ seed = a_components[0];
		ZZ p = m_components[0];
		for (int i = 1; i < m_components.size(); ++i) {
			CRT(seed, p, a_components[i], m_components[i]);
		}
		seed = seed % m;
		if (seed < 0)
			seed += m;

		vector<bool> aux_vector;
		for (int i = 0; i < LENGTH; ++i) {
			ZZ aux = jacobi_calculator(seed + (ZZ)i, m);
			if (aux == -1)
				aux_vector.push_back(0);
			else if (aux == 1)
				aux_vector.push_back(1);
		}
		generated_binary = aux_vector;
		return generated_binary;
	}

	vector<bool> generate_random() {
		vector<bool> aux_vector;
		GenPrime(seed, RandomBnd(NumBits(m)));
		for (int i = 0; i < l; ++i) {
			ZZ aux = jacobi_calculator(seed + (ZZ)i, m);
			if (aux == -1)
				aux_vector.push_back(0);
			else if (aux == 1)
				aux_vector.push_back(1);
		}
		generated_binary = aux_vector;
		return generated_binary;
	}
};

vector<bool> convertToBinary(ZZ& number) {
	unsigned int number_size = NumBits(number);
	vector<bool> binary_representation(number_size);
	for (int i = 0; i < number_size; ++i) {
		binary_representation[number_size - (i + 1)] = bit(number, i);
	}
	return binary_representation;
}

void printBinary(const vector<bool>& v) {
	cout << "Binary format: ";
	for (int i = 0; i < v.size(); ++i)
		cout << v[i];
	cout << "\n\n";
}

void elementaryTest(const vector<bool>& v) {
	cout << "Elementary test:\n";
	int nr_of_0 = 0;
	for (int i = 0; i < v.size(); ++i) {
		if (v[i] == 0)
			++nr_of_0;
	}
	double percentage_of_0 = 100 * (double)nr_of_0 / v.size();
	cout << "Number of 0: " << nr_of_0<<'\n';
	cout << "Number of 1: " << v.size() - nr_of_0<<'\n';
	cout << "Percentages:\n0=" << percentage_of_0 << "%\n";
	cout << "1=" << 100 - percentage_of_0 << "%\n\n";
}

pair<string,string> printToFiles(const vector<bool>& v,const int& iteration,string generator) {
	string filename= generator + "_" + to_string(iteration)+".txt";
	string default_file = generator + "_Only1_" + to_string(iteration) + ".txt";
	ofstream fout(filename);
	ofstream f(default_file);
	for (int i = 0; i < v.size(); ++i)
	{
		fout << v[i];
		f << 1;
	}
	return make_pair(filename, default_file);
}

int getSize(ifstream& file) {
	file.seekg(0, ios::end);
	int size = file.tellg();
	file.seekg(0);
	return size;
}

void compressionTest(const vector<bool>& v,const int& i,string generator) {
	cout << "File compression test:\n";
	pair <string,string> files=printToFiles(v, i,generator);
	pair<string, string> zip_files = make_pair(files.first.substr(0, files.first.size() - 4)+".zip", files.second.substr(0, files.second.size() - 4)+".zip");
	system(("powershell Compress-Archive -Force " + files.first+ " " + zip_files.first).c_str());
	system(("powershell Compress-Archive -Force " + files.second + " " + zip_files.second).c_str());

	/*Default file size equals the generated one*/
	ifstream gin(files.first, ios::binary);
	ifstream zip_gin(zip_files.first, ios::binary);
	ifstream zip_din(zip_files.second, ios::binary);

	int g_size=getSize(gin);
	int zip_g_size=getSize(zip_gin);
	int zip_d_size=getSize(zip_din);

	double g_comp_ratio = ((double)zip_g_size / (double)g_size) * 100;
	double s_comp_ratio= ((double)zip_d_size / (double)g_size) * 100;

	printf("Initial files size: %d\nArchived file zie: %d\nDefault archived size: %d\n", g_size, zip_g_size, zip_d_size);
	printf("Generated file compression ratio: %.2f%%\n",g_comp_ratio);
	printf("Default file compression ratio: %.2f%%\n", s_comp_ratio);
}

void bbs_test(int bit_length) {
	blum_blum_shub generator(bit_length);
	ZZ a;
	generator.printAll();
	for (int i = 0; i < RUNS; ++i) {
		cout << "------------------------------------------------------------------------------\n";
		cout << "Iteration:" << i << "\t\t*bbs*\n";
		a = generator.random_bbs();
		cout << "Generated number=" << a << "\n\n";
		vector<bool> binary;
		binary = convertToBinary(a);
		printBinary(binary);
		elementaryTest(binary);
		compressionTest(binary,i,"bbs");
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}
}

void jacobi_test(int p_q_size,int nr_bit_length) {
	jacobi j(p_q_size, nr_bit_length);
	vector<bool> binary;
	for (int i = 0; i < RUNS; ++i) {
		cout << "------------------------------------------------------------------------------\n";
		cout << "Iteration:" << i << "\t*jacobi*\n";
		binary = j.generate_random();
		printBinary(binary);
		elementaryTest(binary);
		compressionTest(binary, i,"jcb");
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}
}

void jacobi_test_b(int NR_OF_PRIMES, int PRIME_LENGTH,int RANDOM_LENGTH) {
	jacobi j(NR_OF_PRIMES,PRIME_LENGTH,1);
	vector<bool> binary;
	for (int i = 0; i < RUNS; ++i) {
		cout << "------------------------------------------------------------------------------\n";
		cout << "Iteration:" << i << "\t*jacobi multiple components*\n";
		binary = j.generate_random(RANDOM_LENGTH);
		printBinary(binary);
		elementaryTest(binary);
		compressionTest(binary, i, "jcb_comp");
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}
}

void jacobi_symbol_test(int bit_length,int runs) {
	blum_blum_shub generator(bit_length);
	ZZ a = generator.random_bbs();
	ZZ b = generator.random_bbs();
	while (b % 2 != 1) { b = generator.random_bbs(); }
	for (int i = 0; i < runs; ++i)
	{
		ZZ aux1, aux2;
		if ((aux1=jacobi_calculator(a, b)) != (aux2=Jacobi(a, b))) {
			cout << "Something is wrong";
			cout << aux1 << " " << aux2<<'\n';
			cout << "A:" << a << " B:" << b;
			return;
		}
		a = generator.random_bbs();
		b = generator.random_bbs();
		while (b % 2 != 1) { b = generator.random_bbs(); }
	}
	printf("%d symbols calculated correctly\n", runs);
}

int main()
{
	bbs_test(1024);
	jacobi_test(1024,2048);
	jacobi_test_b(5,512,2048);
}


#include <svgpp/svgpp.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION >= 106400
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace svgpp;
namespace ublas = boost::numeric::ublas;

typedef ublas::matrix<double> matrix_t;

struct TransformEventsPolicy
{
  typedef matrix_t context_type;

  static void transform_matrix(matrix_t & transform, const boost::array<double, 6> & matrix)
  {
    matrix_t m(3, 3);
    m <<=
      matrix[0], matrix[2], matrix[4],
      matrix[1], matrix[3], matrix[5],
      0, 0, 1;
    transform = ublas::prod(transform, m);
  }

  static void transform_translate(matrix_t & transform, double tx, double ty)
  {
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(0, 2) = tx; m(1, 2) = ty;
    transform = ublas::prod(transform, m);
  }

  static void transform_scale(matrix_t & transform, double sx, double sy)
  {
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(0, 0) = sx; m(1, 1) = sy;
    transform = ublas::prod(transform, m);
  }

  static void transform_rotate(matrix_t & transform, double angle)
  {
    angle *= boost::math::constants::degree<double>();
    matrix_t m(3, 3);
    m <<=
      std::cos(angle), -std::sin(angle), 0,
      std::sin(angle),  std::cos(angle), 0,
      0, 0, 1;
    transform = ublas::prod(transform, m);
  }

  static void transform_skew_x(matrix_t & transform, double angle)
  {
    angle *= boost::math::constants::degree<double>();
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(0, 1) = std::tan(angle);
    transform = ublas::prod(transform, m);
  }

  static void transform_skew_y(matrix_t & transform, double angle)
  {
    angle *= boost::math::constants::degree<double>();
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(1, 0) = std::tan(angle);
    transform = ublas::prod(transform, m);
  }
};

int main()
{
  matrix_t transform(ublas::identity_matrix<double>(3, 3));
  value_parser<
    tag::type::transform_list,
    transform_policy<policy::transform::minimal>,
    transform_events_policy<TransformEventsPolicy>
  >::parse(tag::attribute::transform(), transform,
    std::string("translate(-10,-20) scale(2) rotate(45) translate(5,10)"), tag::source::attribute());
  std::cout << transform << "\n";
  return 0;
}


// Copyright 2008 Gautam Sewani

// Use, modification and distribution are 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)

#define BOOST_MATH_UNDERFLOW_ERROR_POLICY throw_on_error
#define BOOST_MATH_OVERFLOW_ERROR_POLICY throw_on_error
#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/logistic.hpp>
    using boost::math::logistic_distribution;

#include <boost/test/test_exec_monitor.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>
#include "test_out_of_range.hpp"

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;

template <class RealType>
void test_spot(RealType location, RealType scale, RealType x, RealType p, RealType q, RealType tolerance)
{
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
      logistic_distribution<RealType>(location,scale),
      x),
      p,
      tolerance); // %
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
      complement(logistic_distribution<RealType>(location,scale),
      x)),
      q,
      tolerance); // %
   if(p < 0.999)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(
         logistic_distribution<RealType>(location,scale),
         p),
         x,
         tolerance); // %
   }
   if(q < 0.999)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(
         complement(logistic_distribution<RealType>(location,scale),
         q)),
         x,
         2 * tolerance); // %
   }
}

template <class RealType>
void test_spots(RealType T)
{
   // Basic sanity checks.
   // 50 eps as a percentage, up to a maximum of double precision
   // Test data taken from Mathematica 6
   RealType tolerance = (std::max)(
      static_cast<RealType>(1e-33L),
      boost::math::tools::epsilon<RealType>());
   cout<<"Absolute tolerance:"<<tolerance<<endl;

   tolerance *= 50 * 100;
   // #  pragma warning(disable: 4100) // unreferenced formal parameter.
   // prevent his spurious warning.
   if (T != 0)
   {
      cout << "Expect parameter T == 0!" << endl;
   }
   cout << "Tolerance for type " << typeid(T).name()  << " is " << tolerance << " %" << endl;

   test_spot(
      static_cast<RealType>(1), // location
      static_cast<RealType>(0.5L), // scale
      static_cast<RealType>(0.1L), // x
      static_cast<RealType>(0.141851064900487789594278108470953L), // p
      static_cast<RealType>(0.858148935099512210405721891529047L), //q
      tolerance);

   test_spot(
      static_cast<RealType>(5), // location
      static_cast<RealType>(2), // scale
      static_cast<RealType>(3.123123123L),//x
      static_cast<RealType>(0.281215878622547904873088053477813L), // p
      static_cast<RealType>(0.718784121377452095126911946522187L), //q
      tolerance);

   test_spot(
      static_cast<RealType>(1.2345L), // location
      static_cast<RealType>(0.12345L), // scale
      static_cast<RealType>(3.123123123L),//x
      static_cast<RealType>(0.999999773084685079723328282229357L), // p
      static_cast<RealType>(2.26915314920276671717770643005212e-7L), //q
      tolerance);

   //High probability
   test_spot(
      static_cast<RealType>(1), // location
      static_cast<RealType>(0.5L), // scale
      static_cast<RealType>(10), // x
      static_cast<RealType>(0.99999998477002048723965105559179L), // p
      static_cast<RealType>(1.5229979512760348944408208801237e-8L), //q
      tolerance);

   //negative x
   test_spot(
      static_cast<RealType>(5), // location
      static_cast<RealType>(2), // scale
      static_cast<RealType>(-0.1L), // scale
      static_cast<RealType>(0.0724264853615177178439235061476928L), // p
      static_cast<RealType>(0.927573514638482282156076493852307L), //q
      tolerance);

   test_spot(
      static_cast<RealType>(5), // location
      static_cast<RealType>(2), // scale
      static_cast<RealType>(-20), // x
      static_cast<RealType>(3.72663928418656138608800947863869e-6L), // p
      static_cast<RealType>(0.999996273360715813438613911990521L), //q
      tolerance);

   //test value to check cancellation error in straight/complimented quantile
   //the subtraction in the formula location-scale*log term introduces catastrophics cancellator error if location and scale*log term are close
   //For these values, the tests fail at tolerance, but work at 100*tolerance
   test_spot(
      static_cast<RealType>(-1.2345L), // location
      static_cast<RealType>(1.4555L), // scale
      static_cast<RealType>(-0.00125796420642514024493852425918807L),//x
      static_cast<RealType>(0.7L), // p
      static_cast<RealType>(0.3L), //q
      80*tolerance);

   test_spot(
      static_cast<RealType>(1.2345L), // location
      static_cast<RealType>(0.12345L), // scale
      static_cast<RealType>(0.0012345L), // x
      static_cast<RealType>(0.0000458541039469413343331170952855318L), // p
      static_cast<RealType>(0.999954145896053058665666882904714L), //q
      80*tolerance);

   test_spot(
      static_cast<RealType>(5L), // location
      static_cast<RealType>(2L), // scale
      static_cast<RealType>(0.0012345L), // x
      static_cast<RealType>(0.0759014628704232983512906076564256L), // p
      static_cast<RealType>(0.924098537129576701648709392343574L), //q
      80*tolerance);

   //negative location
   test_spot(
      static_cast<RealType>(-123.123123L), // location
      static_cast<RealType>(2.123L), // scale
      static_cast<RealType>(3), // x
      static_cast<RealType>(0.999999999999999999999999984171276L), // p
      static_cast<RealType>(1.58287236765203121622150720373972e-26L), //q
      tolerance);
   //PDF Testing
   BOOST_CHECK_CLOSE(
      ::boost::math::pdf(
      logistic_distribution<RealType>(5,2),
         static_cast<RealType>(0.125L) ),//x
         static_cast<RealType>(0.0369500730133475464584898192104821L),              // probability
      tolerance); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::pdf(
         logistic_distribution<RealType>(static_cast<RealType>(1.2345L), static_cast<RealType>(0.12345L)),
         static_cast<RealType>(0.0012345L) ),//x
         static_cast<RealType>(0.000371421639109700748742498671686243L),              // probability
      tolerance); // %
   BOOST_CHECK_CLOSE(
      ::boost::math::pdf(
      logistic_distribution<RealType>(2,1),
         static_cast<RealType>(2L) ),//x
         static_cast<RealType>(0.25L),              // probability
      tolerance); // %

   //Extreme value testing

   if(std::numeric_limits<RealType>::has_infinity)
   {
      BOOST_CHECK_EQUAL(pdf(logistic_distribution<RealType>(), +std::numeric_limits<RealType>::infinity()), 0); // x = + infinity, pdf = 0
      BOOST_CHECK_EQUAL(pdf(logistic_distribution<RealType>(), -std::numeric_limits<RealType>::infinity()), 0); // x = - infinity, pdf = 0
      BOOST_CHECK_EQUAL(cdf(logistic_distribution<RealType>(), +std::numeric_limits<RealType>::infinity()), 1); // x = + infinity, cdf = 1
      BOOST_CHECK_EQUAL(cdf(logistic_distribution<RealType>(), -std::numeric_limits<RealType>::infinity()), 0); // x = - infinity, cdf = 0
      BOOST_CHECK_EQUAL(cdf(complement(logistic_distribution<RealType>(), +std::numeric_limits<RealType>::infinity())), 0); // x = + infinity, c cdf = 0
      BOOST_CHECK_EQUAL(cdf(complement(logistic_distribution<RealType>(), -std::numeric_limits<RealType>::infinity())), 1); // x = - infinity, c cdf = 1
   }
   BOOST_CHECK_THROW(quantile(logistic_distribution<RealType>(), static_cast<RealType>(1)), std::overflow_error); // x = + infinity, cdf = 1
   BOOST_CHECK_THROW(quantile(logistic_distribution<RealType>(), static_cast<RealType>(0)), std::overflow_error); // x = - infinity, cdf = 0
   BOOST_CHECK_THROW(quantile(complement(logistic_distribution<RealType>(), static_cast<RealType>(1))), std::overflow_error); // x = - infinity, cdf = 0
   BOOST_CHECK_THROW(quantile(complement(logistic_distribution<RealType>(), static_cast<RealType>(0))), std::overflow_error); // x = + infinity, cdf = 1
   BOOST_CHECK_EQUAL(cdf(logistic_distribution<RealType>(), +boost::math::tools::max_value<RealType>()), 1); // x = + infinity, cdf = 1
   BOOST_CHECK_EQUAL(cdf(logistic_distribution<RealType>(), -boost::math::tools::max_value<RealType>()), 0); // x = - infinity, cdf = 0
   BOOST_CHECK_EQUAL(cdf(complement(logistic_distribution<RealType>(), +boost::math::tools::max_value<RealType>())), 0); // x = + infinity, c cdf = 0
   BOOST_CHECK_EQUAL(cdf(complement(logistic_distribution<RealType>(), -boost::math::tools::max_value<RealType>())), 1); // x = - infinity, c cdf = 1
   BOOST_CHECK_EQUAL(pdf(logistic_distribution<RealType>(), +boost::math::tools::max_value<RealType>()), 0); // x = + infinity, pdf = 0
   BOOST_CHECK_EQUAL(pdf(logistic_distribution<RealType>(), -boost::math::tools::max_value<RealType>()), 0); // x = - infinity, pdf = 0

   //
   // Things that are errors:
   //1. domain errors for scale and location
   //2. x being NAN
   //3. Probabilies being outside (0,1)
   check_out_of_range<logistic_distribution<RealType> >(0, 1);
   if(std::numeric_limits<RealType>::has_infinity)
   {
      RealType inf = std::numeric_limits<RealType>::infinity();
      BOOST_CHECK_EQUAL(pdf(logistic_distribution<RealType>(0, 1), inf), 0);
      BOOST_CHECK_EQUAL(pdf(logistic_distribution<RealType>(0, 1), -inf), 0);
      BOOST_CHECK_EQUAL(cdf(logistic_distribution<RealType>(0, 1), inf), 1);
      BOOST_CHECK_EQUAL(cdf(logistic_distribution<RealType>(0, 1), -inf), 0);
      BOOST_CHECK_EQUAL(cdf(complement(logistic_distribution<RealType>(0, 1), inf)), 0);
      BOOST_CHECK_EQUAL(cdf(complement(logistic_distribution<RealType>(0, 1), -inf)), 1);
   }

   //location/scale can't be infinity
   if(std::numeric_limits<RealType>::has_infinity) {
      BOOST_CHECK_THROW(
         logistic_distribution<RealType> dist(std::numeric_limits<RealType>::infinity(),0.5),
         std::domain_error);
      BOOST_CHECK_THROW(
         logistic_distribution<RealType> dist(0.5,std::numeric_limits<RealType>::infinity()),
         std::domain_error);
   }
   //scale can't be negative or 0
   BOOST_CHECK_THROW(
      logistic_distribution<RealType> dist(0.5,-0.5),
      std::domain_error);
   BOOST_CHECK_THROW(
      logistic_distribution<RealType> dist(0.5,0),
      std::domain_error);

   logistic_distribution<RealType> dist(0.5,0.5);
   //x can't be NaN,p can't be NaN

   if (std::numeric_limits<RealType>::has_quiet_NaN)
   {
      // No longer allow x to be NaN, then these tests should throw.
      BOOST_CHECK_THROW(pdf(dist, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // x = NaN
      BOOST_CHECK_THROW(cdf(dist, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // x = NaN
      BOOST_CHECK_THROW(cdf(complement(dist, +std::numeric_limits<RealType>::quiet_NaN())), std::domain_error); // x = + infinity
      BOOST_CHECK_THROW(quantile(dist, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // p = + infinity
      BOOST_CHECK_THROW(quantile(complement(dist, +std::numeric_limits<RealType>::quiet_NaN())), std::domain_error); // p = + infinity
   }

   //p can't be outside (0,1)
   BOOST_CHECK_THROW(quantile(dist, static_cast<RealType>(1.1)), std::domain_error);
   BOOST_CHECK_THROW(quantile(dist, static_cast<RealType>(-0.1)), std::domain_error);
   BOOST_CHECK_THROW(quantile(dist, static_cast<RealType>(1)), std::overflow_error);
   BOOST_CHECK_THROW(quantile(dist, static_cast<RealType>(0)), std::overflow_error);

   BOOST_CHECK_THROW(quantile(complement(dist, static_cast<RealType>(1.1))), std::domain_error);
   BOOST_CHECK_THROW(quantile(complement(dist, static_cast<RealType>(-0.1))), std::domain_error);
   BOOST_CHECK_THROW(quantile(complement(dist, static_cast<RealType>(1))), std::overflow_error);
   BOOST_CHECK_THROW(quantile(complement(dist, static_cast<RealType>(0))), std::overflow_error);

   //Tests for mean,mode,median,variance,skewness,kurtosis
   //mean
   BOOST_CHECK_CLOSE(
      ::boost::math::mean(
      logistic_distribution<RealType>(2,1)
      ),//x
      static_cast<RealType>(2),              // probability
      tolerance); // %
   //median
   BOOST_CHECK_CLOSE(
      ::boost::math::median(
      logistic_distribution<RealType>(2,1)
      ),//x
      static_cast<RealType>(2),              // probability
      tolerance);
   //mode
   BOOST_CHECK_CLOSE(
      ::boost::math::mode(
      logistic_distribution<RealType>(2,1)
      ),//x
      static_cast<RealType>(2),              // probability
      tolerance);
   //variance
   BOOST_CHECK_CLOSE(
      ::boost::math::variance(
      logistic_distribution<RealType>(2,1)
      ),//x
      static_cast<RealType>(3.28986813369645287294483033329205L),              // probability
      tolerance);
   //skewness
   BOOST_CHECK_CLOSE(
      ::boost::math::skewness(
      logistic_distribution<RealType>(2,1)
      ),//x
      static_cast<RealType>(0),              // probability
      tolerance);
   BOOST_CHECK_CLOSE(
      ::boost::math::kurtosis_excess(
      logistic_distribution<RealType>(2,1)
      ),//x
      static_cast<RealType>(1.2L),              // probability
      tolerance);

} // template <class RealType>void test_spots(RealType)

int test_main(int, char* [])
{
  // Check that can generate logistic distribution using the two convenience methods:
   boost::math::logistic mycexp1(1.); // Using typedef
   logistic_distribution<> myexp2(1.); // Using default RealType double.

    // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif

   return 0;
} // int test_main(int, char* [])


/*!
 * \file misc.cpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 21, 2019: created
 */

#pragma once

#include <type_traits>
#include <Eigen/Dense>

/*************************
 *** Utility functions ***
 *************************/

/*
double cross2D(Eigen::Ref<Eigen::Vector2d> const &x, Eigen::Ref<Eigen::Vector2d> const &y)
{
    return x(0)*y(1)-x(1)*y(0);
}
*/

inline double cross2D(Eigen::Vector2d &&x, Eigen::Vector2d &&y)
{
    return x(0)*y(1)-x(1)*y(0);
}

template<class T, int n, size_t... is>
Eigen::Matrix<T,n+1,1> affWrap_impl(Eigen::Matrix<T,n,1> const &mat, std::index_sequence<is...>)
{
    return Eigen::Matrix<T,n+1,1>{mat(is)...,1.0};
}

template<class T,int n>
Eigen::Matrix<T,n+1,1> affWrap(Eigen::Matrix<T,n,1> const &mat)
{
    return affWrap_impl(mat, std::make_index_sequence<n>());
}

//! Check if two triangles overwraps with each other.
//! Based on the idea suggested in https://stackoverflow.com/questions/2778240/detection-of-triangle-collision-in-2d-space.
inline bool trianglesIntersection2D(std::array<Eigen::Vector2d,3> const &t1_, std::array<Eigen::Vector2d,3> const &t2_)
{
    /*** Preparation ***/
    // Rotation matrix of M_PI/2 in radian.
    Eigen::Matrix<double,2,2> mat;
    mat << 0, 1, -1, 0;

    // We may assume the vertices are given counter-clockwisely around triangles.
    bool is_ccwise1 = static_cast<double>((t1_[1]-t1_[0]).adjoint()*mat*(t1_[2]-t1_[0])) > 0;
    bool is_ccwise2 = static_cast<double>((t2_[1]-t2_[0]).adjoint()*mat*(t2_[2]-t2_[0])) > 0;
    std::array<std::array<Eigen::Vector2d const*, 3>,2> t{
        &(t1_[0]),
        is_ccwise1 ? &(t1_[1]) : &(t1_[2]),
        is_ccwise1 ? &(t1_[2]) : &(t1_[1]),
        &(t2_[0]),
        is_ccwise2 ? &(t2_[1]) : &(t2_[2]),
        is_ccwise2 ? &(t2_[2]) : &(t2_[1])
    };

    /*** The algorithm begins here ***/

    // Find an edge in one triangle which separates the opposite vertex and the vertices of the other triangle.
    // If found, this means two triangles are disjoint.
    for(size_t i = 0; i < 3; ++i) {
        Eigen::RowVector2d rv = (*(t[0][(i+1)%3])-*(t[0][i])).adjoint()*mat;
        if (static_cast<double>(rv*(*(t[1][0])-*(t[0][i]))) < 0
            && static_cast<double>(rv*(*(t[1][1])-*(t[0][i]))) < 0
            && static_cast<double>(rv*(*(t[1][2])-*(t[0][i]))) < 0)
            return false;
    }

    for(size_t i = 0; i < 3; ++i) {
        Eigen::RowVector2d rv = (*(t[1][(i+1)%3])-*(t[1][i])).adjoint()*mat;
        if (static_cast<double>(rv*(*(t[0][0])-*(t[1][i]))) < 0
            && static_cast<double>(rv*(*(t[0][1])-*(t[1][i]))) < 0
            && static_cast<double>(rv*(*(t[0][2])-*(t[1][i]))) < 0)
            return false;
    }

    return true;
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_SIGNEDVOLUME_HPP
#define MCL_SIGNEDVOLUME_HPP 1

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

namespace mcl
{

static inline double signed_triangle_area(
	const Eigen::Vector2d &p1,
	const Eigen::Vector2d &p2,
	const Eigen::Vector2d &p3);

// Returns (unscaled) first derivative of signed triangle area
static inline std::vector<Eigen::Vector2d> signed_triangle_area_gradients(
	const Eigen::Vector2d &p1,
	const Eigen::Vector2d &p2,
	const Eigen::Vector2d &p3);

static inline double triangle_perimeter(
	const Eigen::Vector2d &p1,
	const Eigen::Vector2d &p2,
	const Eigen::Vector2d &p3);

static inline double triangle_area(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3);

static inline double signed_tet_volume(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3,
	const Eigen::Vector3d &p4);

// Returns (unscaled) first derivative of signed tet volume
static inline std::vector<Eigen::Vector3d> signed_tet_volume_gradients(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3,
	const Eigen::Vector3d &p4);

static inline double tet_surface_area(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3,
	const Eigen::Vector3d &p4);

// Probably not where this belongs but oh well.
// Returns the faces of a tet
static inline std::vector<Eigen::Vector3i>
	faces_from_tet(const Eigen::RowVector4i &t);

//
// Implementation
//

inline double signed_triangle_area(const Eigen::Vector2d &p1, const Eigen::Vector2d &p2, const Eigen::Vector2d &p3)
{
	return 0.5 * ( -p2[0]*p1[1] + p3[0]*p1[1] + p1[0]*p2[1] - p3[0]*p2[1] - p1[0]*p3[1] + p2[0]*p3[1] );
}

inline std::vector<Eigen::Vector2d> signed_triangle_area_gradients(
	const Eigen::Vector2d &a, const Eigen::Vector2d &b, const Eigen::Vector2d &c)
{
	std::vector<Eigen::Vector2d> g(3);
	g[0] = 0.5 * Eigen::Vector2d(b[1]-c[1], -b[0]+c[0]);
	g[1] = 0.5 * Eigen::Vector2d(-a[1]+c[1], a[0]-c[0]);
	g[2] = 0.5 * Eigen::Vector2d(a[1]-b[1], -a[0]+b[0]);
	return g;
}

inline double triangle_perimeter(const Eigen::Vector2d &p1, const Eigen::Vector2d &p2, const Eigen::Vector2d &p3)
{
	return (p1-p2).norm() + (p2-p3).norm() + (p3-p1).norm();
}

// https://en.wikipedia.org/wiki/Heron%27s_formula
inline double triangle_area(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, const Eigen::Vector3d &p3)
{
	double a = (p1-p2).norm();
	double b = (p2-p3).norm();
	double c = (p3-p1).norm();
	double s = (a+b+c) * 0.5;
	return std::sqrt(s*(s-a)*(s-b)*(s-c));
}

inline double signed_tet_volume(
	const Eigen::Vector3d &p1, const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3, const Eigen::Vector3d &p4)
{
	Eigen::Matrix3d edges;
	edges.col(0) = p2 - p1;
	edges.col(1) = p3 - p1;
	edges.col(2) = p4 - p1;
	return (1.0/6.0) * edges.determinant();
}

inline std::vector<Eigen::Vector3d> signed_tet_volume_gradients(
	const Eigen::Vector3d &a, const Eigen::Vector3d &b,
	const Eigen::Vector3d &c, const Eigen::Vector3d &d)
{
	std::vector<Eigen::Vector3d> grads(4);
	const Eigen::Vector3d &p0 = a;
	const Eigen::Vector3d &p1 = b;
	const Eigen::Vector3d &p2 = c;
	const Eigen::Vector3d &p3 = d;
	static const double sixth = (1.0/6.0);
	grads[0] = sixth * (p1 - p2).cross(p3 - p2);
	grads[1] = sixth * (p2 - p0).cross(p3 - p0);
	grads[2] = sixth * (p0 - p1).cross(p3 - p1);
	grads[3] = sixth * (p1 - p0).cross(p2 - p0);
	return grads;
}

inline double tet_surface_area(
		const Eigen::Vector3d &p1,
		const Eigen::Vector3d &p2,
		const Eigen::Vector3d &p3,
		const Eigen::Vector3d &p4)
{
	double a1 = triangle_area(p2,p3,p4);
	double a2 = triangle_area(p2,p3,p1);
	double a3 = triangle_area(p3,p4,p1);
	double a4 = triangle_area(p4,p2,p1);
	return (a1+a2+a3+a4);
}

inline std::vector<Eigen::Vector3i> faces_from_tet(const Eigen::RowVector4i &t)
{
	using namespace Eigen;
	std::vector<Vector3i> f = {
		Vector3i(t[0], t[1], t[3]),
		Vector3i(t[0], t[2], t[1]),
		Vector3i(t[0], t[3], t[2]),
		Vector3i(t[1], t[2], t[3]) };
	return f;
}

} // ns mcl

#endif


#include <tuple>
#include <complex>
# include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

tuple<MatrixXcd, MatrixXcd> qr_reduced(const MatrixXcd& A)
{
    // reduced QR decomposition using Modified Gram-Schmidt with Reortogonalization
    // reference: W. Ganter 1980, Algorithms for QR-Decomposition, research report no.80-02
    // A is  m x n  complex matrix
    int n= A.cols();
    MatrixXcd R= MatrixXcd::Zero(n,n);
    MatrixXcd Q= A;
    for(int k= 0; k < n; ++k){
        complex<double> tt {(0.0,0.0)};
        for(int j= 0; j < 2; ++j){
            for(int i= 0; i < k; ++i){
                complex<double> s= Q.col(i).adjoint()*Q.col(k);
                if(tt == (0.0,0.0)) R(i,k)= s;
                Q.col(k)= Q.col(k)-s*Q.col(i);
            }
            tt= Q.col(k).norm();
        }
        R(k,k)= tt;
        Q.col(k)= Q.col(k)/R(k,k);
    }
    return forward_as_tuple(Q,R);
}


#include <NTL/ZZXFactoring.h>

NTL_CLIENT

long compare(const ZZX& a, const ZZX& b)
{
   if (deg(a) < deg(b))
      return 0;

   if (deg(a) > deg(b))
      return 1;

   long n = a.rep.length();
   long i;

   for (i = 0; i < n; i++) {
      if (a.rep[i] < b.rep[i]) return 0;
      if (a.rep[i] > b.rep[i]) return 1;
   }

   return 0;
}

void sort(vec_pair_ZZX_long& v)
{
   long n = v.length();
   long i, j;

   for (i = 0; i < n-1; i++)
      for (j = 0; j < n-1-i; j++)
         if (compare(v[j].a, v[j+1].a)) {
            swap(v[j].a, v[j+1].a);
            swap(v[j].b, v[j+1].b);
         }
}

int main(int argc, char **argv)
{
   ZZX f1, f;

   if (argc > 1)
      ZZXFac_MaxPrune = atoi(argv[1]);

   cin >> f;

   vec_pair_ZZX_long factors;
   ZZ c;

   double t;

   t = GetTime();
   factor(c, factors, f, 0);
   t = GetTime()-t;

   cerr << "total time: " << t << "\n";

   mul(f1, factors);
   mul(f1, f1, c);

   if (f != f1)
      Error("FACTORIZATION INCORRECT!!!");

   sort(factors);

   cout << c << "\n";
   cout << factors << "\n";

   return 0;
}


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

// Copyright (c) 2014, Oracle and/or its affiliates

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

//[num_segments
//` Get the number of segments in a geometry

#include <iostream>

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

int main()
{
    boost::geometry::model::multi_polygon
        <
            boost::geometry::model::polygon
                <
                    boost::geometry::model::d2::point_xy<double>, true, false // cw, open polygon
                >
        > mp;
    boost::geometry::read_wkt("MULTIPOLYGON(((0 0,0 10,10 0),(1 1,8 1,1 8)),((10 10,10 20,20 10)))", mp);
    std::cout << "Number of segments: " << boost::geometry::num_segments(mp) << std::endl;
    return 0;
}

//]

//[num_segments_output
/*`
Output:
[pre
 Number of segments: 9
]
*/
//]


#ifndef MLT_MODELS_REGRESSORS_LEAST_SQUARES_LINEAR_REGRESSION_HPP
#define MLT_MODELS_REGRESSORS_LEAST_SQUARES_LINEAR_REGRESSION_HPP

#include <type_traits>

#include <Eigen/Core>

#include "linear_regressor.hpp"
#include "../../utils/linear_solvers.hpp"

namespace mlt {
namespace models {
namespace regressors {
	using namespace utils::linear_solvers;

	template <class Solver = SVDSolver>
    class LeastSquaresLinearRegression : public LinearRegressor<LeastSquaresLinearRegression<Solver>> {
    public:
        explicit LeastSquaresLinearRegression(bool fit_intercept = true) : LinearRegressor(fit_intercept), _solver(Solver()) {}

        template <class S, class = enable_if<is_same<decay_t<S>, Solver>::value>>
		explicit LeastSquaresLinearRegression(const S&& solver, bool fit_intercept = true) : LinearRegressor(fit_intercept), _solver(forward<S>(solver)) {}

        Self& fit(Features input, Target target, bool = true) {
            MatrixXd input_prime(input.rows() + (_fit_intercept ? 1 : 0), input.cols());
			input_prime.topRows(input.rows()) << input;

			if (_fit_intercept) {
				input_prime.bottomRows<1>() = VectorXd::Ones(input.cols());
			}

			_set_coefficients(_solver.compute(input_prime * input_prime.transpose()).solve(input_prime * target.transpose()).transpose());

			return _self();
        }

	protected:
		Solver _solver;
    };
}
}
}
#endif


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   PlaneFitter.hpp
 * Author: Jordan McManus
 */

#ifndef PLANEFITTER_HPP
#define PLANEFITTER_HPP

#include <Eigen/Dense>

class PlaneFitter {
public:

    static void convertPlaneZform2GeneralForm(Eigen::Vector3d & zForm, Eigen::Vector4d & generalForm) {
        /*
         * Z-form is z = Ax + By + C
         * General form is ax + by + cz + d = 0 with (a*a + b*b + c*c = 1 i.e. unit normal vector)
         */

        double c = 1.0 / zForm.norm();
        double a = -zForm(0) * c;
        double b = -zForm(1) * c;
        double d = -zForm(2) * c;
        double f = sqrt(a * a + b * b + c * c);
        a = a / f;
        b = b / f;
        c = c / f;
        d = d / f;

        generalForm << a, b, c, d;
    }

    static void convertPlaneGeneralForm2Zform(Eigen::Vector4d & generalForm, Eigen::Vector3d & zForm) {
        /*
         * Z-form is z = Ax + By + C
         * General form is ax + by + cz + d = 0 with (a*a + b*b + c*c = 1 i.e. unit normal vector)
         */
        double f = 1.0 / generalForm(2);
        double A = -generalForm(0) * f;
        double B = -generalForm(1) * f;
        double C = -generalForm(3) * f;

        zForm << A, B, C;
    }

    static void calculatePlaneResidualsFromMatrix(Eigen::VectorXd & residuals, Eigen::MatrixXd & cloud, Eigen::Vector4d & planarGeneralForm) {
        /*
         * General form is ax + by + cz + d = 0 with (a*a + b*b + c*c = 1 i.e. unit normal vector)
         */

        Eigen::MatrixXd augmentedCloud(cloud.rows(), 4);
        augmentedCloud.col(0) = cloud.col(0);
        augmentedCloud.col(1) = cloud.col(1);
        augmentedCloud.col(2) = cloud.col(2);
        augmentedCloud.col(3) = Eigen::VectorXd::Ones(cloud.rows());

        residuals = augmentedCloud*planarGeneralForm;
    }

    static void fitPlane(Eigen::MatrixXd & xyz, Eigen::Vector4d & planeGeneralFormParams) {

        Eigen::MatrixXd A(xyz.rows(), 3);
        A.col(0) = xyz.col(0);
        A.col(1) = xyz.col(1);
        A.col(2) = Eigen::VectorXd::Ones(xyz.rows());

        Eigen::VectorXd b = xyz.col(2);

        Eigen::Vector3d planeParameterEstimation = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

        convertPlaneZform2GeneralForm(planeParameterEstimation, planeGeneralFormParams);
    }

    static void fitPlane(Eigen::MatrixXd & A, Eigen::VectorXd & b, Eigen::Vector4d & planeGeneralFormParams) {
        Eigen::Vector3d planeParameterEstimation = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
        convertPlaneZform2GeneralForm(planeParameterEstimation, planeGeneralFormParams);
    }

};

#endif /* PLANEFITTER_HPP */


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

//Euler angles to DCM
//e = [phi, theta, psi]
//3-1-2 rotation
//ref: Todd Humphreys euler2dcm MATLAB function, "Aerial Robotics," 2019
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f euler2dcm(Vector3f e)
{
  std::cout << "Calculating trig values..." << std::endl;
  float cPhi = cos(e(0));
  float sPhi = sin(e(0));
  float cThe = cos(e(1));
  float sThe = sin(e(1));
  float cPsi = cos(e(2));
  float sPsi = sin(e(2));
  std::cout << "Building DCM..." << std::endl;
  Matrix3f DCM;
  DCM << (cPhi*cThe - sPhi*sPsi*sThe), (cThe*sPsi + cPsi * sPhi*sThe), (-cPhi*sThe),
         (-cPhi*sPsi),                                    (cPhi*cPsi),         sPhi,
         (cPsi*sThe + cThe*sPhi*sPsi), (sPsi*sThe - cPsi*cThe*sPhi),    (cPhi*cThe);
  return DCM;
}

int main()
{
  using Eigen::Vector3f;
  using Eigen::Matrix3f;
  using Eigen::Quaternionf;
  /*Matrix3f A;
  A << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  std::cout << (A*5) << std::endl;*/
  Vector3f v(1.5708, -1.5708, 0.7854);
  Matrix3f DCM = euler2dcm(v);
  std::cout << DCM << std::endl;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
//  Copyright Paul A. Bristow 2015 - 2016.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// For additional details, see C.M. Kormanyos,
// Real-Time C++: Efficient Object-Oriented and
// Template Microcontroller Programming (Springer, Heidelberg, 2013).
// in Section 12.7 and Chapter 13.
// See http://link.springer.com/chapter/10.1007/978-3-642-34688-0_13

#define USE_BARE_METAL

#include <cstdint>

#if defined(USE_BARE_METAL)
#else
  #include <iostream>
  #include <iomanip>
#endif

// Configure Boost.Fixed_point for a bare-metal system.

#if defined(USE_BARE_METAL)
  #define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION // Do not use Boost.Multiprecision.
  #define BOOST_FIXED_POINT_DISABLE_IOSTREAM       // Do not use I/O streaming.
#endif

#include <boost/fixed_point/fixed_point.hpp>

namespace local
{
  /*! Evaluate first derivative of real_function
      using a three-point central-difference rule of O(dx^6), for more details
     \see http://www.boost.org/doc/libs/release/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/fp_eg/nd.html.

    \tparam RealValueType Type of value, for example, a fixed_point_type.
    \tparam RealFunctionType Type of parameter @c real function.
    \param x Value of x.
    \param dx Step size.
    \param real_function Real Function.
    \return First derivative at x value.
  */

  //[fixed_point_derivative_function
  template<typename RealValueType,
           typename RealFunctionType>
  RealValueType first_derivative(const RealValueType& x,
                                 const RealValueType& dx,
                                 RealFunctionType real_function)
  {
    const RealValueType dx2(dx  + dx);
    const RealValueType dx3(dx2 + dx);

    const RealValueType m1((  real_function(x + dx)
                            - real_function(x - dx))  / 2U);
    const RealValueType m2((  real_function(x + dx2)
                            - real_function(x - dx2)) / 4U);
    const RealValueType m3((  real_function(x + dx3)
                            - real_function(x - dx3)) / 6U);

    const RealValueType fifteen_m1(m1 * 15U);
    const RealValueType six_m2    (m2 *  6U);
    const RealValueType ten_dx    (dx * 10U);

    return ((fifteen_m1 - six_m2) + m3) / ten_dx;
  }
//] [/fixed_point_derivative_function]
} // namespace local

// Implement a tiny simulated subset of the mcal (microcontroller abstraction layer).
namespace mcal
{
  namespace wdg
  {
    void trigger();
  }
} // namespace mcal::wdg

void mcal::wdg::trigger()
{
  // Simulate a fake watchdog trigger mechanism doing nothing here.
}

// Declare a global Boolean test variable.
bool global_result_is_ok;

extern "C" int main()
{
//[fixed_point_derivative_coeffic
  typedef boost::fixed_point::negatable<6, -9> fixed_point_type;

  const fixed_point_type a = fixed_point_type(12) / 10;
  const fixed_point_type b = fixed_point_type(34) / 10;
  const fixed_point_type c = fixed_point_type(56) / 10;

  // Compute the approximate derivative of (a * x^2) + (b * x) + c
  // evaluated at 1/2, where the approximate values of the coefficients
  // are: a = 1.2, b = 3.4, and c = 5.6. The numerical tolerance is set
  // to a value of approximately 1/4.
//] [/fixed_point_derivative_coeffic]
  // See http://link.springer.com/chapter/10.1007/978-3-642-34688-0_12 page 219-220.

//[fixed_point_derivative_evalution
  const fixed_point_type d =
    local::first_derivative(fixed_point_type(1) / 2, // x
                            fixed_point_type(1) / 4,  // Step size dx.
                            [&a, &b, &c](const fixed_point_type& x) -> fixed_point_type
                            {
                              return (((a * x) + b) * x) + c;
                            });
//] [/fixed_point_derivative_evalution]

  // The expected result is ((2 * a) + b) = (2.4 + 3.4) = 4.6 (exact).
  // We obtain a fixed-point result of approximately 4.5938.

  // Verify that the result lies within (4.5 < result < 4.7).
  // The expected result is 4.6, so this is a wide tolerance.

//[fixed_point_verify
  global_result_is_ok = ((d > (fixed_point_type(45) / 10)) && (d < (fixed_point_type(47) / 10)));
//] [/fixed_point_verify]

  #if defined(USE_BARE_METAL)
    // We can not print the fixed-point number to the output stream
    // because I/O-streaming is disabled for fixed-point in this
    // Boost configuration.
  #else
    // But if we could print to the output stream, it might look
    // similar to the lines below. When attempting to print to
    // the output stream, however, we would need to add <iostream>
    // and deactivate #define BOOST_FIXED_POINT_DISABLE_IOSTREAM.
    std::cout << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
              << std::fixed
              << d
              << std::endl;
  #endif

  // Is the result of taking the derivative of the quadratic function OK?
  if(global_result_is_ok)
  {
    // Here we could take some action in the microcontroller
    // such as toggle a digital output port to high, indicating
    // success of the test case.
  }

  #if defined(USE_BARE_METAL)
    // In this bare-metal OS-less system, do not return from main().
    for(;;)
    {
      mcal::wdg::trigger();
    }
  #endif
}


#include <iostream>
#include <eigen3/Eigen/Dense>
#include <vector>
#include <cmath>
#include <boost/numeric/odeint.hpp>
#include "gnuplot-iostream.h"
#include "system_function.h"
namespace odeint = boost::numeric::odeint;
int main()
{
	state_type x(2); // a vector with size 2.
	x[0] = 1.0;		 // start at x=1.0, p=0.0
	x[1] = 0.0;
	std::vector<state_type> x_vec;
	std::vector<double> times;

	size_t steps = odeint::integrate(harmonic_oscillator,
									 x, 0.0, 10.0, 0.1,
									 push_back_state_and_time(x_vec, times));

	std::vector<double> position;
	for (size_t i = 0; i <= steps; i++)
	{
		position.push_back(x_vec[i][0]);
	}
	/* output */
	Gnuplot gp;
	gp << "plot '-' using 1:2 with linespoint" << std::endl;
	gp.send1d(std::make_tuple(times, position));

	// for (size_t i = 0; i <= steps; i++)
	// {
	// 	std::cout << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
	// }

	return 0;
}


#include <limits>
#include <numeric>

#include <Eigen/Core>

#include "UtilMath.hpp"

namespace RGM
{

template<typename T>
T MathUtil_<T>::median(std::vector<T> & vData, int num/*=std::numeric_limits<int>::max()*/, bool sortData/*=false*/)
{

    if (vData.size()==0 || num==0) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    num = std::max<int>(std::min<int>(vData.size(), num), 1);

    if ( vData.size()==1 || num==1 ) {
        return vData[0];
    }

    std::vector<T> * p=0;

    if ( sortData ) {
        std::sort(vData.begin(), vData.begin()+num); // Ascending

        if ( num % 2 == 0) {
            int idx = num / 2;
            return (vData[idx-1] + vData[idx]) / (T)2;
        } else {
            int idx = (num-1) / 2;
            return vData[idx];
        }
    } else {
        std::vector<T> tmp(num);
        std::copy(vData.begin(), vData.begin()+num, tmp.begin());
        std::sort(tmp.begin(), tmp.end());

        if ( num % 2 == 0) {
            int idx = num / 2;
            return (tmp[idx-1] + tmp[idx]) / (T)2;
        } else {
            int idx = (num-1) / 2;
            return tmp[idx];
        }
    }
}

template<typename T>
std::vector<float> MathUtil_<T>::pdist(const std::vector<cv::Point_<T> > & pts, int num)
{

    std::vector<float> dist;

    num = std::min<int>(pts.size(), num);

    for ( int i=0; i<num-1; ++i ) {
        const cv::Point_<T> & pt1( pts[i] );
        for ( int j=i+1; j<num; ++j ) {
            const cv::Point_<T> & pt2( pts[j] );
            dist.push_back( sqrt((float)(pt1.x-pt2.x)*(pt1.x-pt2.x)+(pt1.y-pt2.y)*(pt1.y-pt2.y)) );
        }
    }

    return dist;
}

template<typename T>
std::vector<T> MathUtil_<T>::linspace(T s, T e, T interval)
{

    std::vector<T> x;

    for ( T i=s; i<=e; i+=interval ) {
        x.push_back(i);
    }

    return x;
}

template<typename T>
std::vector<T> MathUtil_<T>::hist(std::vector<T> & y, std::vector<T> & x)
{

    std::vector<T> n(x.size(), 0);

    std::vector<T> xx(x.size()+1);
    xx[0]           = -std::numeric_limits<T>::infinity();
    std::copy(x.begin(), x.end(), xx.begin()+1);

    for ( int i=0; i<y.size(); ++i ) {

        int j=0;
        for ( ; j<xx.size()-1; ++j ) {
            if (y[i]>xx[j] && y[i]<=xx[j+1]) {
                break;
            }
        }

        n[j]++;
    }

    return n;
}

template<typename T>
std::vector<T> MathUtil_<T>::convnSame(std::vector<T> & x, std::vector<T> & filter)
{

    std::vector<T> xx(x.size()+2*filter.size(), 0);
    std::copy(x.begin(), x.end(), xx.begin()+filter.size());

    std::reverse(filter.begin(), filter.end());

    std::vector<T> result(xx.size()-filter.size());
    for ( int i=0; i<result.size(); ++i) {
        result[i] = std::inner_product(filter.begin(), filter.end(), xx.begin()+i, 0.0F);
    }

    std::vector<T> r(x.size());

    int istart = floor(filter.size()/2.0F);
    std::copy(result.begin()+istart, result.begin()+istart+x.size(), r.begin());

    return r;
}

template<typename T>
T MathUtil_<T>::calcErr(std::vector<T> & pscores, std::vector<T> & nscores, T & thr)
{
    T minerr = 1.0;

    int numpos = pscores.size();
    int numneg = nscores.size();
    int num = numpos+numneg;

    std::vector<std::pair<T, int> > scores(num);

    for ( int i=0; i<numpos; ++i ) {
        scores[i] = std::pair<T, int>(-pscores[i], i);
    }
    for ( int i=numpos, j=0; j<numneg; ++i, ++j ) {
        scores[i] = std::pair<T, int>(-nscores[j], i);
    }

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

    std::vector<int> tp(num, 0), fp(num, 0);

    for (int i=0; i<num; ++i ) {
        if ( scores[i].second < numpos ) {
            tp[i] = 1;
            fp[i] = 0;
        } else {
            fp[i] = 1;
            tp[i] = 0;
        }
    }

    Eigen::VectorXd tpr(num), fpr(num), err(num);

    tpr(0) = tp[0];
    fpr(0) = fp[0];
    for ( int i=1; i<num; ++i ) {
        tpr(i) = tpr(i-1) + tp[i];
        fpr(i) = fpr(i-1) + fp[i];
    }

    tpr /= numpos;
    // to fnr
    tpr.array() = 1 - tpr.array();
    fpr /= numneg;

    err = tpr+fpr;

    int r, c;
    minerr = err.minCoeff(&r, &c) / 2.0f;

    thr = std::max<T>(-scores[r].first, *std::min_element(pscores.begin(), pscores.end()));

    return minerr;
}

template<typename T>
T MathUtil_<T>::calcVar(std::vector<T> & pscores, T & thr)
{
    int num = pscores.size();

    //thr = std::numeric_limits<T>::max();

    T  m = 0;
    for ( int i=0; i<num; ++i) {
        m += pscores[i];
        //thr = std::min<T>(thr, pscores[i]);
    }

    m /= num;

    T v = 0;
    for ( int i=0; i<num; ++i) {
        v += std::pow(pscores[i]-m, 2.0F);
    }

    v /= (num-1);

    v = sqrt(v);

    thr = m - v;

    return v;
}

/// Specification
template class MathUtil_<int>;
template class MathUtil_<float>;
template class MathUtil_<double>;

} // namespace RGM


#pragma once

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

#include "topology.hpp"

using namespace Eigen;

/**
 * @brief Calcola la direzione normale di ogni corner di ogni triangolo nella
 * mesh in input.
 *
 * Data una mesh di triangoli M = (V, F), dove V e' un array di posizioni 3D
 * (i vertici) e F e' un array di triple di indici ai vertici (ogni tripla
 * rappresenta una faccia triangolare), questa funzione calcola la direzione
 * normale di ogni corner di ogni triangolo come una somma pesata delle normali
 * dei triangoli adiacenti nello stesso settore.
 * Ogni triangolo ha 3 corner.
 * Un settore e' costituito da un insieme di triangoli incidenti su uno stesso
 * vertice, uno adiacente al successivo, che approssimano una superficie smooth:
 * due triangoli adiacenti tra loro (incidono sullo stesso lato) fanno parte
 * dello stesso settore se l'angolo fra le loro rispettive direzioni normali e'
 * inferiore ad una data soglia (il coseno, o prodotto scalare, - e' maggiore
 * di una soglia).
 * Il peso è dato dall'area del triangolo moltiplicato l'angolo incidente sul
 * vertice.
 *
 * L'area di un triangolo viene calcolata come meta' della lunghezza (norma)
 * del prodotto vettoriale tra due lati.
 * L'angolo incidente su un vertice viene calcolato come l'arcocoseno del
 * prodotto scalare tra i due lati incidenti (intesi come vettori unitari
 * centrati su di esso), oppure come l'arcotangente del seno e coseno:
 * - il seno e' proporzionale alla normal del prodotto vettoriale
 * - il cose e' proporzionale al prodotto scalare.
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @return MatrixXd La matrice restituita ha tre righe per ogni triangolo
 *                  (N.rows() == F.rows() * 3) e 3 colonne. Ogni riga corrisponde
 *                  alla direzione normale di un corner di un triangolo, avente
 *                  le 3 coordinate x,y,z.
 */
MatrixXd perCornerNormals(MatrixXd const &V, MatrixXi const &F)
{
    MatrixXd N = MatrixXd::Zero(F.rows() * 3, 3);

    // coseno di 30 gradi:
    const double cos_thr = std::sqrt(3) / 2;

    // suggerimento: e' necessario trovare le facce adiacenti intorno ad un vertice
    // se non vuoi scrivere il codice che fa questo, puoi includere il file
    // #include "topology.hpp"
    // e chiamare le funzioni `vertex_face_adjacency()` e `face_face_adjacency()`

    std::vector<std::vector<int>> VF, VFi;
    MatrixXi FF, FFi;

    vertex_face_adjacency(V, F, VF, VFi);
    face_face_adjacency(V, F, VF, VFi, FF, FFi);

    MatrixXd FN(F.rows(), 3);
    VectorXd Fareas(F.rows());
    MatrixXd Fangles(F.rows(), 3);

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

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

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

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

        Fareas(f) = c0.norm() / 2.0;

        FN.row(f) = c0.normalized();

        Fangles(f, 0) = std::atan2(c0.norm(), e0.dot(-e2));
        Fangles(f, 1) = std::atan2(c1.norm(), e1.dot(-e0));
        Fangles(f, 2) = std::atan2(c2.norm(), e2.dot(-e1));
    }

    MatrixXd FF_cosines(F.rows(), 3);
    for (int f = 0; f < F.rows(); ++f) {
        auto const& fn = FN.row(f);
        for (int p = 0; p < 3; ++p) {
            auto const& ffn = FN.row(FF(f, p));
            FF_cosines(f, p) = fn.dot(ffn);
        }
    }

    for (int f = 0; f < F.rows(); ++f) {
        auto const& fn = FN.row(f);
        for (int p = 0; p < 3; ++p) {
            auto n = Fareas(f) * Fangles(f, p) * fn;

            N.row(3 * f + p) += n;

            // contribusci alle normali di tutte le facce adiacenti intorno
            // al corner p, il cui angolo diedrale non supera la soglia
            int nf = FF(f, (3 + p - 1) % 3);
            if (nf >= 0) {
                int nfi = FFi(f, (3 + p - 1) % 3);
                // prima le facce in senso antiorario
                do {
                    if (FF_cosines(nf, nfi) >= cos_thr) {
                        N.row(3 * nf + nfi) += n;
                        int nnf = FF(nf, (3 + nfi - 1) % 3);
                        nfi = FFi(nf, (3 + nfi - 1) % 3);
                        nf = nnf;
                    } else {
                        break;
                    }
                } while (nf >= 0 && nf != f);

                // poi le facce in senso orario, eventualmente
                if (nf != f) {
                    nf = f;
                    nfi = p;
                    while (nf >= 0 && FF_cosines(nf, nfi) >= cos_thr) {
                        int nnf = FF(nf, nfi);
                        nfi = (FFi(nf, nfi) + 1) % 3;
                        nf = nnf;
                        N.row(3 * nf + nfi) += n;
                    }
                }
            }
        }
    }

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

    return N;
}


//============================================================================
// Name        : jacobian-matrix.cpp
// Author      : Deborah Digges
// Version     :
// Copyright   : 2016
// Description : Hello World in C++, Ansi-style
//============================================================================

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

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;

MatrixXd CalculateJacobian(const VectorXd& x_state);

int main() {

	/*
	 * Compute the Jacobian Matrix
	 */

	//predicted state  example
	//px = 1, py = 2, vx = 0.2, vy = 0.4
	VectorXd x_predicted(4);
	x_predicted << 1, 2, 0.2, 0.4;

	MatrixXd Hj = CalculateJacobian(x_predicted);

	cout << "Hj:" << endl << Hj << endl;

	return 0;
}

MatrixXd CalculateJacobian(const VectorXd& x_state) {

	MatrixXd Hj(3,4);
	//recover state parameters
	float px = x_state(0);
	float py = x_state(1);
	float vx = x_state(2);
	float vy = x_state(3);

	if(px == 0 && py == 0) {
		return Hj;
	}

	float px2py2 = pow(px, 2) + pow(py, 2);

	Hj(0, 0) = px/sqrt(px2py2);
	Hj(0, 1) = py/sqrt(px2py2);

	Hj(1, 0) = -py/px2py2;
	Hj(1, 1) = px/px2py2;

	Hj(2, 0) = py* (vx*py - vy*px)/pow(px2py2, 3/2);
	Hj(2, 1) = px * (vy*px - vx*py)/pow(px2py2, 3/2);
	Hj(2, 2) = px/sqrt(px2py2);
	Hj(2, 3) = py/sqrt(px2py2);

	return Hj;
}


#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>

/**
 * Get most significant byte of n.
 * @param n
 * @return only the most significant byte of n set
 */
uint64_t msb(uint64_t n) {
    if (n == 0)
        return 0;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    return mask;
}

/**
 * Algorithm by Marc Joye and Jean-Jacques Quisquater in Efficient computation of full Lucas sequences.
 */
NTL::ZZ fibonacci1(uint64_t n) {
    if (n == 0)
        return (NTL::ZZ) 0;

    NTL::ZZ Uh;
    Uh = 1;
    NTL::ZZ Vl;
    Vl = 2;
    NTL::ZZ Vh;
    Vh = 1;
    NTL::ZZ Ql;
    Ql = 1;
    NTL::ZZ Qh;
    Qh = 1;

    uint64_t s = 0;
    while (n % 2 == 0) {
        n /= 2;
        s++;
    }

    uint64_t mask = msb(n);
    while (mask != 1) {
        Ql = Ql * Qh;
        if (n & mask) {
            Qh = -Ql;
            Uh = Uh * Vh;
            Vl = Vh * Vl - Ql;
            Vh = Vh * Vh - 2*Qh; // mistake in the original paper
        }
        else {
            Qh = Ql;
            Uh = Uh * Vl - Ql;
            Vh = Vh * Vl - Ql;
            Vl = Vl * Vl - 2 * Ql;
        }

        mask >>= (uint) 1;
    }

    Ql = Ql * Qh;
    Qh = -Ql;
    Uh = Uh * Vl - Ql;
    Vl = Vh * Vl - Ql;
    Ql = Ql * Qh;

    for (uint64_t i = 1; i <= s; i++) {
        Uh = Uh * Vl;
        Vl = Vl * Vl - 2*Ql;
        Ql = Ql * Ql;
    }

    return Uh;
}

/**
 * Algorithm by Aleksey Koval in On Lucas Sequences Computation.
 */
NTL::ZZ fibonacci2(uint64_t n) {
    NTL::ZZ V_l;
    V_l = 2;
    NTL::ZZ V_h;
    V_h = 1;

    NTL::ZZ Q_l;
    Q_l = 1;
    NTL::ZZ Q_h;
    Q_h = 1;

    uint64_t mask = msb(n);
    while (mask != 0) {
        Q_l = Q_l * Q_h;
        if (n & mask) {
            Q_h = -Q_l;
            V_l = V_h * V_l - Q_l;
            V_h = V_h * V_h - 2 * Q_h;
        }
        else {
            Q_h = Q_l;
            V_h = V_h * V_l - Q_l;
            V_l = V_l * V_l - 2 * Q_h;
        }

        mask >>= (uint) 1;
    }

    return (2 * V_h - V_l) / 5;
}

/**
 * Algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci3(uint64_t n) {
    NTL::ZZ x;
    x = 0;
    NTL::ZZ y;
    y = 1;

    NTL::ZZ tmp;

    uint64_t mask = msb(n);
    while (mask != 0) {
        tmp = x;
        x = x*(2*y-x);
        y = (tmp * tmp + y * y);

        if (n & mask) {
            tmp = x;
            x = y;
            y = (tmp + y);
        }

        mask >>= (uint) 1;
    }

    return x;
}

/**
 * Adjusted algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci4(uint64_t n) {
    NTL::ZZ x;
    x = 0;
    NTL::ZZ y;
    y = 1;

    NTL::ZZ x2;
    NTL::ZZ y2;

    uint64_t mask = msb(n);
    while (mask != 0) {
        x2 = x*x;
        y2 = y*y;

        if (n & mask) {
            y = 2*x*y + y2;
            x = x2+y2;
        }
        else {
            x = 2*x*y - x2;
            y = x2+y2;
        }

        mask >>= (uint) 1;
    }

    return x;
}

/**
 * Modular version of algorithm by Marc Joye and Jean-Jacques Quisquater in Efficient computation of full Lucas sequences.
 */
NTL::ZZ fibonacci_mod1(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    if (n == 0)
        return (NTL::ZZ) 0;

    NTL::ZZ_p Uh;
    Uh = 1;
    NTL::ZZ_p Vl;
    Vl = 2;
    NTL::ZZ_p Vh;
    Vh = 1;
    NTL::ZZ_p Ql;
    Ql = 1;
    NTL::ZZ_p Qh;
    Qh = 1;

    uint64_t s = 0;
    while (n % 2 == 0) {
        n /= 2;
        s++;
    }

    uint64_t mask = msb(n);
    while (mask != 1) {
        Ql = Ql * Qh;
        if (n & mask) {
            Qh = -Ql;
            Uh = Uh * Vh;
            Vl = Vh * Vl - Ql;
            Vh = Vh * Vh - 2*Qh; // mistake in the original paper
        }
        else {
            Qh = Ql;
            Uh = Uh * Vl - Ql;
            Vh = Vh * Vl - Ql;
            Vl = Vl * Vl - 2 * Ql;
        }

        mask >>= (uint) 1;
    }

    Ql = Ql * Qh;
    Qh = -Ql;
    Uh = Uh * Vl - Ql;
    Vl = Vh * Vl - Ql;
    Ql = Ql * Qh;

    for (uint64_t i = 1; i <= s; i++) {
        Uh = Uh * Vl;
        Vl = Vl * Vl - 2*Ql;
        Ql = Ql * Ql;
    }

    return rep(Uh);
}

/**
 * Modular version of algorithm by Aleksey Koval in On Lucas Sequences Computation.
 */
NTL::ZZ fibonacci_mod2(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    NTL::ZZ_p V_l;
    V_l = 2;
    NTL::ZZ_p V_h;
    V_h = 1;

    NTL::ZZ_p Q_l;
    Q_l = 1;
    NTL::ZZ_p Q_h;
    Q_h = 1;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    while (mask != 0) {
        Q_l = Q_l * Q_h;
        if (n & mask) {
            Q_h = -Q_l;
            V_l = V_h * V_l - Q_l;
            V_h = V_h * V_h - 2 * Q_h;
        }
        else {
            Q_h = Q_l;
            V_h = V_h * V_l - Q_l;
            V_l = V_l * V_l - 2 * Q_h;
        }

        mask >>= (uint) 1;
    }

    return rep((2 * V_h - V_l) / 5);  // fails if 5 divides m
}

/**
 * Modular version of algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci_mod3(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    NTL::ZZ_p x;
    x = 0;
    NTL::ZZ_p y;
    y = 1;

    NTL::ZZ_p tmp;

    uint64_t mask = msb(n);
    while (mask != 0) {
        tmp = x;
        x = x*(2*y-x);
        y = (tmp * tmp + y * y);

        if (n & mask) {
            tmp = x;
            x = y;
            y = (tmp + y);
        }

        mask >>= (uint) 1;
    }

    return rep(x);
}

/**
 * Modular version of adjusted algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci_mod4(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    NTL::ZZ_p x;
    x = 0;
    NTL::ZZ_p y;
    y = 1;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    NTL::ZZ_p x2;
    NTL::ZZ_p y2;

    while (mask != 0) {
        x2 = x*x;
        y2 = y*y;

        if (n & mask) {
            y = 2*x*y + y2;
            x = x2+y2;
        }
        else {
            x = 2*x*y - x2;
            y = x2+y2;
        }

        mask >>= (uint) 1;
    }

    return rep(x);
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/interpolation/linear.hpp>
#include <fcppt/math/interpolation/trigonometric.hpp>
#include <fcppt/math/vector/arithmetic.hpp>
#include <fcppt/math/vector/length.hpp>
#include <fcppt/math/vector/object_impl.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include <fcppt/config/external_end.hpp>

namespace
{
typedef
fcppt::math::vector::static_<double,2>
vector2;

double const epsilon = 0.001;
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(linear_interpolation)
{
FCPPT_PP_POP_WARNING

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::linear(
				0.0,
				1.0,
				2.0) - 1.0) < epsilon);

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::linear(
				1.0,
				1.0,
				2.0) - 2.0) < epsilon);

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::linear(
				0.5,
				1.0,
				2.0) - 1.5) < epsilon);

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::linear(
				0.25,
				1.0,
				2.0) - 1.25) < epsilon);

	// One test for vectors
	BOOST_CHECK(
		fcppt::math::vector::length(
			fcppt::math::interpolation::linear(
				0.25,
				vector2(1.0,1.0),
				vector2(2.0,2.0)) - vector2(1.25,1.25)) < epsilon);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(trigonometric_interpolation)
{
FCPPT_PP_POP_WARNING

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::trigonometric(
				0.0,
				1.0,
				2.0) - 1.0) < epsilon);

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::trigonometric(
				1.0,
				1.0,
				2.0) - 2.0) < epsilon);

	BOOST_CHECK(
		std::abs(
			fcppt::math::interpolation::trigonometric(
				0.5,
				1.0,
				2.0) - 1.5) < epsilon);

	// One test for vectors
	BOOST_CHECK(
		fcppt::math::vector::length(
			fcppt::math::interpolation::trigonometric(
				1.0,
				vector2(1.0,1.0),
				vector2(2.0,2.0)) - vector2(2.0,2.0)) < epsilon);
}


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>

TEST(AgradRev,atan2_var_var) {
  AVAR a = 1.2;
  AVAR b = 3.9;
  AVAR f = atan2(a,b);
  EXPECT_FLOAT_EQ(atan2(1.2,3.9),f.val());

  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(3.9 / (1.2 * 1.2 + 3.9 * 3.9), g[0]);
  EXPECT_FLOAT_EQ(-1.2 / (1.2 * 1.2 + 3.9 * 3.9), g[1]);
}

TEST(AgradRev,atan2_dvd) {
  AVAR sigma = 1;
  AVEC x = createAVEC(sigma);
  AVAR f = atan2(1.0,sigma) / 3.14;
  VEC g;
  f.grad(x,g);

  AVAR sigma1 = 1;
  AVEC x1 = createAVEC(sigma1);
  AVAR f1 = atan2(1.0,sigma1);
  VEC g1;
  f1.grad(x1,g1);

  EXPECT_FLOAT_EQ(3.14 * g[0],g1[0]);
}
TEST(AgradRev,atan2_var_var__integration) {
  double c = 5.0;
  AVAR a = 1.2;
  AVAR b = 3.9;
  AVAR f = atan2(a,b) * c;
  EXPECT_FLOAT_EQ(atan2(1.2,3.9)*c,f.val());

  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(3.9 / (1.2 * 1.2 + 3.9 * 3.9) * c, g[0]);
  EXPECT_FLOAT_EQ(-1.2 / (1.2 * 1.2 + 3.9 * 3.9) * c, g[1]);
}

TEST(AgradRev,atan2_var_double) {
  AVAR a = 1.2;

  double b = 3.9;
  AVAR f = atan2(a,b);
  EXPECT_FLOAT_EQ(atan2(1.2,3.9),f.val());

  AVEC x = createAVEC(a);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(3.9 / (1.2 * 1.2 + 3.9 * 3.9), g[0]);
}

TEST(AgradRev,atan2_double_var) {
  double a = 1.2;
  AVAR b = 3.9;
  AVAR f = atan2(a,b);
  EXPECT_FLOAT_EQ(atan2(1.2,3.9),f.val());

  AVEC x = createAVEC(b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(-1.2 / (1.2 * 1.2 + 3.9 * 3.9), g[0]);
}

struct atan2_fun {
  template <typename T0, typename T1>
  inline
  typename stan::return_type<T0,T1>::type
  operator()(const T0& arg1,
             const T1& arg2) const {
    return atan2(arg1,arg2);
  }
};

TEST(AgradRev, atan2_nan) {
  atan2_fun atan2_;
  test_nan(atan2_,3.0,5.0,false,true);

}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 1.2;
  AVAR b = 3.9;
  test::check_varis_on_stack(stan::math::atan2(a, b));
  test::check_varis_on_stack(stan::math::atan2(a, 3.9));
  test::check_varis_on_stack(stan::math::atan2(1.2, b));
}


#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <map>
#include <utility>
#include <vector>
// this header for all except for ch8
// MIT LICENSE
#ifndef STATISTICS_HPP
#define STATISTICS_HPP
using namespace std;
#define yes true
#define no false

double genPValue(double z_value, bool isTwoTail = false, bool tailInvertSign = false) {
    // need to add two tail test!
    // add bool isTwotail = false
    // this is the conjugate function of genZValue
    boost::math::normal Ndistribution(0, 1);
    auto P = boost::math::cdf(boost::math::complement(Ndistribution, fabs(z_value)));
    if (isTwoTail && tailInvertSign)
        throw "error range";
    if (tailInvertSign)
        P = 1 - P;
    if (isTwoTail)
        P *= 2;
    return P;
}

double genPValue(double degree, double t_value, bool isTwoTail = false, bool tailInvertSign = false) {
    // this is the conjugate function of genTValue
    // @degree: the degree of freedom
    boost::math::students_t Tdistribution(degree);
    auto P = boost::math::cdf(boost::math::complement(Tdistribution, fabs(t_value)));
    if (isTwoTail && tailInvertSign)
        throw "error range";
    if (tailInvertSign)
        P = 1 - P;
    if (isTwoTail)
        P *= 2;
    return P;
}

double genZValue(double subscript, bool isSingleTail) {
    /*
    * @subscript: the score below the "z", which means the cumulative distribution function of the area.
    *             if subscript is negative meaning it is left tail, possitive otherwise
    * @isSingleTail: is only sigle tail of the reject area
    * return the z-score which lookup from the z-table
    */
    boost::math::normal Ndistribution(0, 1);
    if (!isSingleTail)
        subscript /= 2;
    auto Z = boost::math::quantile(boost::math::complement(Ndistribution, fabs(subscript)));
    if (subscript < 0)
        Z = -Z;
    return Z;
}

double genTValue(int degree, double upperTailArea) {
    boost::math::students_t Tdistribution(degree);
    auto T = boost::math::quantile(boost::math::complement(Tdistribution, upperTailArea));
    return T;
}

template <typename Iteratable>
double genMean(Iteratable& _dataSet) {
    double sum = 0;
    for (auto i : _dataSet)
        sum += i;
    return sum / _dataSet.size();
}

template <typename Iteratable>
double genSampleStandardDeviation(Iteratable& _dataSet, double sampleMean, double size) {
    double sxx = 0;
    for (auto i : _dataSet)
        sxx += (i - sampleMean) * (i - sampleMean);
    return sqrt(1 / (size - 1) * sxx);
}

double genPercentageStandardDeviation(double p, int sampleSize) {
    return sqrt(p * (1 - p) / sampleSize);
}

int twoPopulationDegreeFreedom(double ssd1, double ssd2, int n1, int n2) {
    auto sampleVariation1 = (ssd1 * ssd1 / n1);
    auto sampleVariation2 = (ssd2 * ssd2 / n2);
    return (sampleVariation1 + sampleVariation2) * (sampleVariation1 + sampleVariation2) /
           (sampleVariation1 * sampleVariation1 / (n1 - 1) + sampleVariation2 * sampleVariation2 / (n2 - 1));
}
template <typename Iteratable>
int twoPopulationDegreeFreedom(Iteratable& _dataSet1, Iteratable& _dataSet2) {
    auto ssd1 = genSampleStandardDeviation(_dataSet1, genMean(_dataSet1), _dataSet1.size());
    auto ssd2 = genSampleStandardDeviation(_dataSet2, genMean(_dataSet2), _dataSet2.size());
    return twoPopulationDegreeFreedom(ssd1, ssd2, _dataSet1.size(), _dataSet2.size());
}

double errorRadius(vector<double>& _dataSet, int degree, int sampleSize, double upperTailArea) {
    double mean = genMean(_dataSet);
    double ssd = genSampleStandardDeviation(_dataSet, mean, sampleSize);
    return ssd / sqrt(sampleSize) * genTValue(degree, upperTailArea);
}

double errorRadius(vector<double>& _dataSet, double upperTailArea = 0.025) {
    /*
    * giving data set and the upper tail area of Student-T distribution
    * return the error radius
    * for too lazy to only input data set
    */
    int sampleSize = _dataSet.size();
    return errorRadius(_dataSet, sampleSize - 1, sampleSize, upperTailArea);
}

double errorRadius(double knownSigma, double alpha, int sampleSize, bool isSingleTail = false, bool isSample = false) {
    /*
    * @knownSigma: the population sigma, which over sqrt(n)
    * giving alpha return the error radius for known Sigma of Z distribution
    * return the error radius
    */

    if (isSample) {
        if (!isSingleTail)
            alpha /= 2;
        return knownSigma / sqrt(sampleSize) * genTValue(sampleSize - 1, alpha);
    }
    return knownSigma / sqrt(sampleSize) * genZValue(alpha, isSingleTail);
}

pair<double, double> genConfidenceInterval(double theta, double errorRadius, int tailOrient = 0) {
    /*
     * @theta: the sample variable
     * @errorRadius: a radius of error, which might greater than 1,
     *               means the standardDeviation times (z or t) over sqrt(n)
     * @tailOrient: if (tailOrient > 0) will reject right tail
     *              else if (tailOrient < 0) will reject left tail
     *              else will reject two tail
     */
    if (tailOrient > 0)
        return make_pair(-numeric_limits<double>::infinity(), theta + errorRadius);
    else if (tailOrient < 0)
        return make_pair(theta - errorRadius, numeric_limits<double>::infinity());
    else
        return make_pair(theta - errorRadius, theta + errorRadius);
}

vector<pair<double, double>> invertInterval(pair<double, double> originInterval) {
    if (originInterval.first == -numeric_limits<double>::infinity())
        return {make_pair(originInterval.second, numeric_limits<double>::infinity())};
    else if (originInterval.second == numeric_limits<double>::infinity())
        return {make_pair(-numeric_limits<double>::infinity(), originInterval.first)};
    else
        return {make_pair(-numeric_limits<double>::infinity(), originInterval.first),
                make_pair(originInterval.second, numeric_limits<double>::infinity())};
}

pair<double, double> standardlizeInterval(pair<double, double> originInterval, double mu, double standardDeviation) {
    /**
     * @originInterval: the interval need to be standardlized
     * @mu: the mean
     * @standardDeviation: the standard deviation of mu
     */
    return make_pair((originInterval.first - mu) / standardDeviation, (originInterval.second - mu) / standardDeviation);
}

double intervalProbability(pair<double, double> standardlizedInterval) {
    /*
    * this will return the probability with z distribution in some interval
    * @standardlizedInterval: the standardlized interval which want to get the probability
    */
    double leftTail = genPValue(standardlizedInterval.first, false, false),
           rightTail = genPValue(standardlizedInterval.second, false, false);
    return 1 - leftTail - rightTail;
}

template <typename T, typename S>
ostream& operator<<(ostream& os, const pair<T, S>& v) {
    pair<char, char> boundSign('[', ']');
    if (v.first == -numeric_limits<double>::infinity())
        boundSign.first = '(';
    if (v.second == numeric_limits<double>::infinity())
        boundSign.second = ')';
    os << boundSign.first << v.first << ", " << v.second << boundSign.second;
    return os;
}

namespace needingSampleSize {
int p(double alpha, double p = 0.5, double marginError = 0.95) {
    /*
    * @p: the p bar of sample proportions, which might be iid Ber(P) (Bernoulli distribution)
    * @marginError: the margin error of confidence interval
    */
    double q = 1 - p, z = genZValue(alpha, false);
    auto sampleSize = p * q * z * z / marginError / marginError;
    return ceil(sampleSize);
}

int x_bar(double alpha, double knownTheta, double marginError) {
    /*
    * @knownTheta: the sigma which is the population standard deviation
    * @marginError: the margin error of confidence interval
    */
    double z = genZValue(alpha, false);
    auto sampleSize = z * z * knownTheta * knownTheta / marginError / marginError;
    return ceil(sampleSize);
}
int hypothesis(double mu_0, double mu_a, double za, double zb, double sigma) {
    return ceil(pow((za + zb) * sigma / (mu_0 - mu_a), 2));
}
}  // namespace needingSampleSize

double testStatistic(double x_bar, double mu_0, double sd, int sampleSize, bool isSample = false) {
    /**
     * @x_bar: sampleMean
     * @sd: standard deviation
     * @mu_0: the null hypotheses
     * @sampleSize: the sample size which want to againest the H0
     * @isSample: Are we not know the population @sd?
     *          if(false): we don't know the population standard deviation
     */
    if (isSample)
        sampleSize -= 1;
    return (x_bar - mu_0) / (sd / sqrt(sampleSize));
}

template <class T>
string readSingleLineCSV(vector<T>& _dataSet, string fileName) {
    /* will return the file's title
    * read only single line csv file
    * @_dataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawdata;
    getline(inFile, title);
    while (inFile >> rawdata)
        _dataSet.push_back(rawdata);
    inFile.close();
    return title;
}

template <class T>
string readSingleLineCSV(map<T, int>& proportionDataSet, string fileName) {
    /*
    * only for reading proportion Data
    * will return the file's title
    * read only single line csv file
    * @proportionDataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawData;
    getline(inFile, title);
    while (getline(inFile, rawData)) {
        auto isInMap = proportionDataSet.find(rawData);
        if (isInMap != proportionDataSet.end())
            proportionDataSet.at(isInMap->first)++;
        else
            proportionDataSet.insert(pair<T, int>(rawData, 1));
    }
    inFile.close();
    return title;
}

string readMultiLineCSV(map<string, map<string, int>>& proportionDataSet, string fileName) {
    // @proportionDataSet: {title, {data, freq}}
    // return file name
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string titles, singleLine;
    vector<string> titlesContainer;
    getline(inFile, titles);
    for (auto index = titles.find(','); index != string::npos; index = titles.find(',')) {
        string firstSubstring(titles.substr(0, index));
        titlesContainer.push_back(firstSubstring);
        titles.erase(0, firstSubstring.length() + 1);
    }
    titlesContainer.push_back(titles);  //push last title

    while (getline(inFile, singleLine)) {
        for (auto i : titlesContainer) {
            auto index = singleLine.find(',');
            string tmp;
            if (index != string::npos)
                tmp = singleLine.substr(0, index);
            else
                tmp = singleLine;  //the last element

            if (tmp.length()) {
                auto isTitleInTable = proportionDataSet.find(i);
                if (isTitleInTable != proportionDataSet.end()) {
                    auto isInMap = isTitleInTable->second.find(tmp);
                    if (isInMap != isTitleInTable->second.end())
                        isTitleInTable->second.at(isInMap->first)++;
                    else
                        isTitleInTable->second.insert(make_pair(tmp, 1));
                } else {
                    auto inMap = make_pair(tmp, 1);
                    proportionDataSet[i].insert(inMap);
                }
            }
            singleLine.erase(0, tmp.length() + 1);
        }
    }
    inFile.close();
    return fileName;
}
#endif


#include "utils.hpp"
#include <cmath>
#include <fstream>
#include <boost/range/numeric.hpp>

using namespace std;

namespace delphi::utils {

/**
 * Returns the square of a number.
 */
double sqr(double x) { return x * x; }

/**
 * Returns the sum of a vector of doubles.
 */
double sum(std::vector<double> v) { return boost::accumulate(v, 0.0); }

/**
 * Returns the arithmetic mean of a vector of doubles.
 */
double mean(std::vector<double> v) { return sum(v) / v.size(); }

double log_normpdf(double x, double mean, double sd) {
  double var = pow(sd, 2);
  double log_denom = -0.5 * log(2 * M_PI) - log(sd);
  double log_nume = pow(x - mean, 2) / (2 * var);

  return log_denom - log_nume;
}

nlohmann::json load_json(string filename) {
  ifstream i(filename);
  nlohmann::json j = nlohmann::json::parse(i);
  return j;
}

} // namespace delphi::utils


/*
    boost::math::fft example 03.

    FFT plan-like API,
    default engine
*/

#include <boost/math/fft/bsl_backend.hpp>

#include <iostream>
#include <vector>
#include <complex>

namespace fft = boost::math::fft;

template<class T>
void print(const std::vector< std::complex<T> >& V)
{
    for(auto i=0UL;i<V.size();++i)
        std::cout << "V[" << i << "] = "
            << V[i].real() << ", " << V[i].imag() << '\n';
}

int main()
{
    std::vector< std::complex<double> > A{1.0,2.0,3.0,4.0},B(A.size());

    // default engine, create plan
    fft::bsl_dft<std::complex<double>> P(A.size());

    // forward transform, out-of-place
    P.forward(A.cbegin(),A.cend(),B.begin());

    print(B);

    // backward transform, in-place
    P.backward(B.cbegin(),B.cend(),B.begin());

    print(B);
    return 0;
}


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  // Always normalize quaternions before using them
  q1.normalize();
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  // Note here T1w is the transform from the world to frame 1
  // This will transform a point from world coordinate frame to frame 1
  // Robot poses generally mean the opposite
  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;
  return 0;
}


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of euclidean_distance class.
 */
#include "num_collect/interp/kernel/euclidean_distance.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

TEST_CASE("num_collect::interp::kernel::euclidean_distance") {
    using num_collect::interp::kernel::euclidean_distance;

    SECTION("calculate distance of double") {
        const auto dist = euclidean_distance<double>();
        constexpr double var1 = 1.234;
        constexpr double var2 = 3.1415;
        constexpr double expected = var2 - var1;
        REQUIRE_THAT(dist(var1, var2), Catch::Matchers::WithinRel(expected));
        REQUIRE_THAT(dist(var2, var1), Catch::Matchers::WithinRel(expected));
    }

    SECTION("calculate distance of vectors") {
        const auto dist = euclidean_distance<Eigen::Vector3d>();
        const auto var1 = Eigen::Vector3d(1.234, 2.345, 3.456);
        const auto var2 = Eigen::Vector3d(1.357, 2.468, 3.579);
        const double expected = (var1 - var2).norm();
        REQUIRE_THAT(dist(var1, var2), Catch::Matchers::WithinRel(expected));
        REQUIRE_THAT(dist(var2, var1), Catch::Matchers::WithinRel(expected));
    }
}


#pragma once

#include "random_engine.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/optional.hpp>

namespace cryptb
{
	class rsa
	{
		// Private key- for decrypting / digital signing
		// DON'T SHARE d WITH THE CLIENT!
		// Tends to be a number with around 2048 bits.
		boost::multiprecision::cpp_int d{ 0 };

		// It's completely safe to share e and N with the entire world.
		// In fact, you should.

		// Public key- for encrypting / verifying digital signature
		// Tends to be a very small number. Choosing the number 3 for example, is common.
		boost::multiprecision::cpp_int e{ 0 };

		// Public key- for everything. N is needed for all operations.
		// Tends to be a number with about 4096 bits.
		boost::multiprecision::cpp_int N{ 0 };

		// A pure mathematical function to solve for d such that:
		// ((e * d) modulo PhiN) == 1
		// e and PhiN must already be coprime.
		static boost::multiprecision::cpp_int findd(const boost::multiprecision::cpp_int& PhiN, const boost::multiprecision::cpp_int& e);

	public:
		rsa(const rsa&) = default;
		rsa(rsa&&) = default;
		rsa& operator=(const rsa&) = default;
		rsa& operator=(rsa&&) = default;

		// Constructor for generating RSA public-private key pair using the given random engine.
		//
		// When "num_bytes_in_prime_number" == 128 that's 2048-bit RSA
		// Should take a second and a half (very expensive function, call on an asynchronous thread).
		//
		// "num_bytes_in_prime_number" must be at least 2
		//
		rsa(random_engine& rand, const int num_bytes_in_prime_number = 128);

		// Constructor for loading RSA public-private key pairs from values
		rsa(boost::multiprecision::cpp_int&& e, boost::multiprecision::cpp_int&& d, boost::multiprecision::cpp_int&& N) :
			e(std::move(e)), d(std::move(d)), N(std::move(N)) {}

		// Private secret key, don't share.
		const boost::multiprecision::cpp_int& get_d() const
		{
			return this->d;
		}

		// Public key, no danger. Allowed to reveal to the entire world.
		const boost::multiprecision::cpp_int& get_e() const
		{
			return this->e;
		}

		// Public key, no danger. Allowed to reveal to the entire world.
		const boost::multiprecision::cpp_int& get_N() const
		{
			return this->N;
		}

		// powm(a, b, c) == power(a, b) modulo c
		// When computing powm in one operation that can reduce the
		// computation time down from millions of years to mere microseconds.
		//
		// Technically powm treats negative exponents differently than power(a, b)
		// but in the function "rsa::findd()" we made sure that the returned
		// d is positive so in our use case we're only dealing with positive numbers.
		//

		// "original_message" should either be a large random number
		// otherwise the entire algorithm will be insecure.
		// That's because if the same clear-text message is sent to e or more recipients in an encrypted way,
		// and the receivers share the same exponent e, but different p, q, and therefore n,
		// then it's easy to decrypt the original clear-text message via the Chinese remainder theorem.
		//
		// Basically: only use the "encrypt" function with an original_message that is a large
		// random number.
		//
		// You should check that:
		// 0 <= "original_message" < N
		// and that:
		// is_valid_public_key(e, N) == true
		// Otherwise the function will return boost::none
		static boost::optional<boost::multiprecision::cpp_int> encrypt(
			const boost::multiprecision::cpp_int& original_message,
			const boost::multiprecision::cpp_int& e,
			const boost::multiprecision::cpp_int& N)
		{
			if (!rsa::is_valid_public_key(e, N) || original_message >= N || original_message < 0)
				return boost::none;
			return static_cast<boost::multiprecision::cpp_int>(boost::multiprecision::powm(original_message, e, N));
		}

		// You should check that:
		// 0 <= "encrypted_message" < this->N
		// Otherwise the function will return boost::none
		boost::optional<boost::multiprecision::cpp_int> decrypt(const boost::multiprecision::cpp_int& encrypted_message)
		{
			if (encrypted_message >= this->N || encrypted_message < 0)
				return boost::none;
			return static_cast<boost::multiprecision::cpp_int>(boost::multiprecision::powm(encrypted_message, this->d, this->N));
		}

		// RSA digital signature.
		// message_hash must be a cryptographic hash of a message
		// and not the message itself.
		// Otherwise the digital signature won't be secure.
		//
		// You should check that:
		// 0 <= "message_hash" < this->N
		// Otherwise the function will return boost::none
		boost::optional<boost::multiprecision::cpp_int> sign(const boost::multiprecision::cpp_int& message_hash)
		{
			// It's the same algorithm. Isn't that convenient!
			return this->decrypt(message_hash);
		}

		// Verify an RSA digital signature.
		static bool is_valid_signature(
			const boost::multiprecision::cpp_int& message_hash,
			const boost::multiprecision::cpp_int& signature_of_hash,
			const boost::multiprecision::cpp_int& e,
			const boost::multiprecision::cpp_int& N)
		{
			// It's the same algorithm. Isn't that convenient!
			const boost::optional<boost::multiprecision::cpp_int> result = rsa::encrypt(signature_of_hash, e, N);
			if (result == boost::none)
				return false;
			// If the signature matches then it's legit.
			return result.get() == message_hash;
		}

		// Recommended to check the validity of public keys taken
		// from an untrusted source.
		// We wouldn't want to store an invalid public key
		// in the database of known public keys.
		// That would cause functions such as rsa::encrypt
		// to return boost::none
		static bool is_valid_public_key(
			const boost::multiprecision::cpp_int& e,
			const boost::multiprecision::cpp_int& N)
		{
			if (e < 2 || N < (2*3))
				return false;
			return true;
		}
	};
}


#define EIGEN_USE_MKL_ALL

#include <iostream>

#include <Eigen/Core>

#include "../misc.hpp"

#include "models/regressors/least_squares_linear_regression.hpp"
#include "models/regressors/ridge_regression.hpp"
#include "models/regressors/optimizable_linear_regressor.hpp"
#include "utils/optimizers/stochastic_gradient_descent.hpp"
#include "utils/loss_functions.hpp"

void benchmark_linear_solvers() {
	Eigen::MatrixXd XN = Eigen::MatrixXd::Random(100, 100000);
	Eigen::RowVectorXd YN = Eigen::RowVectorXd::Random(100000);

	mlt::models::regressors::LeastSquaresLinearRegression<mlt::utils::linear_solvers::SVDSolver> linear_regressor_svd(false);
	benchmark(linear_regressor_svd, XN, YN, 100);
	mlt::models::regressors::LeastSquaresLinearRegression<mlt::utils::linear_solvers::LDLTSolver> linear_regressor_ldlt(false);
	benchmark(linear_regressor_ldlt, XN, YN, 100);
	std::cout << std::endl;
	mlt::models::regressors::LeastSquaresLinearRegression<mlt::utils::linear_solvers::CGSolver> linear_regressor_cg(false);
	benchmark(linear_regressor_cg, XN, YN, 100);
	std::cout << std::endl;

	std::cout << "Diff: " << (linear_regressor_svd.coefficients() - linear_regressor_ldlt.coefficients()).squaredNorm() << std::endl;
	std::cout << "Diff: " << (linear_regressor_svd.coefficients() - linear_regressor_cg.coefficients()).squaredNorm() << std::endl;
	std::cout << "Diff: " << (linear_regressor_ldlt.coefficients() - linear_regressor_cg.coefficients()).squaredNorm() << std::endl;
}

void test_optimizable_linear_regressors() {
	auto samples = 100;
	Eigen::MatrixXd input = Eigen::MatrixXd::Random(3, samples) * 100;
	Eigen::MatrixXd output = Eigen::MatrixXd::Random(2, samples).array();
	output = (output.array() > 0.0).cast<double>();
	output.row(1) = 1 - output.row(0).array();

	mlt::utils::optimizers::StochasticGradientDescent<> sgd;
	mlt::utils::loss_functions::SquaredLoss loss;

	mlt::models::regressors::OptimizableLinearRegressor<mlt::utils::loss_functions::SquaredLoss, mlt::utils::optimizers::StochasticGradientDescent<>> model(loss, sgd, 0, false);
	eval_numerical_gradient(model, Eigen::MatrixXd::Random(2, 3) * 0.05, input, output);

	mlt::models::regressors::OptimizableLinearRegressor<mlt::utils::loss_functions::SquaredLoss, mlt::utils::optimizers::StochasticGradientDescent<>> model2(loss, sgd, 0, true);
	eval_numerical_gradient(model2, Eigen::MatrixXd::Random(2, 4) * 0.05, input, output);
}

void lr_examples() {
	benchmark_linear_solvers();

	Eigen::MatrixXd X1(2, 3);
	Eigen::MatrixXd Y1(1, 3);

	X1.row(0) << 0, 1, 2;
	X1.row(1) << 0, 1, 2;
	Y1 << 0, 1, 2;

	mlt::models::regressors::LeastSquaresLinearRegression<> linear_regressor(false);
	linear_regressor.fit(X1, Y1);

	std::cout << "LinearRegression: " << std::endl;
	std::cout << linear_regressor.coefficients() << std::endl;
	if (linear_regressor.fit_intercept()) {
	std::cout << linear_regressor.intercepts() << std::endl;
	}
	std::cout << linear_regressor.predict(X1.col(0)) << std::endl;

	Eigen::MatrixXd X2(2, 3);
	Eigen::MatrixXd Y2(1, 3);

	X2.row(0) << 0, 0, 1;
	X2.row(1) << 0, 0, 1;
	Y2 << 0, .1, 1;

	mlt::models::regressors::RidgeRegression<> ridge_regressor(0.5, true);
	ridge_regressor.fit(X2, Y2);

	std::cout << "RidgeRegression: " << std::endl;
	std::cout << ridge_regressor.coefficients() << std::endl;
	std::cout << ridge_regressor.intercepts() << std::endl;
	std::cout << ridge_regressor.predict(X2.col(0)) << std::endl;

	mlt::utils::optimizers::StochasticGradientDescent<> grad_descent(10, 2000, 0.001, 1);
	mlt::utils::loss_functions::SquaredLoss loss;
	mlt::models::regressors::OptimizableLinearRegressor<mlt::utils::loss_functions::SquaredLoss, mlt::utils::optimizers::StochasticGradientDescent<>> sgd(loss, grad_descent, 0.5, true);

	sgd.fit(X2, Y2, true);

	std::cout << "OptimizableLinearRegressor<SquaredLoss, SGD>: " << std::endl;
	std::cout << ridge_regressor.coefficients() << std::endl;
	std::cout << ridge_regressor.intercepts() << std::endl;
	std::cout << sgd.predict(X2.col(0)) << std::endl;

	std::cout << "loss with closed form: " << sgd.loss(ridge_regressor.all_coefficients(), X2, Y2) << std::endl;
	std::cout << "loss with SGD: " << sgd.loss(sgd.all_coefficients(), X2, Y2) << std::endl;
}


#include "Debug.h"
#include "SolverEigen.h"
#include "SolverTime.h"
#include "ComputerTime.h"
#include "util.h"

//#include <Eigen/SuperLUSupport>
#include <Eigen/SparseExtra>
#include <Eigen/IterativeSolvers>

extern SolverTime      solverTime;
extern ComputerTime    computerTime;

using namespace std;
using namespace Eigen;

void SolverEigen::computeConditionNumber()
{
/*
    MatrixXd  globalK;

    globalK.resize(nRow, nCol);
    globalK.setZero();

    int k, ii, jj;

    for(k=0; k<mtx.outerSize(); ++k)
    {
      for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      {
        ii = it.row();
        jj = it.col();

        //cout << ii << '\t' << jj << '\t' << it.value() << endl;

        globalK.coeffRef(ii, jj) = it.value();
      }
    }

    VectorXd sing_vals = globalK.jacobiSvd().singularValues();

    //printf("\n Matrix condition number = %12.6E \n", sing_vals(0)/sing_vals(sing_vals.size()-1) );

    //printf("\n Minimum eigenvalue = %12.6f \n", sing_vals.minCoeff() );
    //printf("\n Minimum eigenvalue = %12.6f \n", sing_vals.maxCoeff() );
    printf("\n Matrix condition number = %12.6E \n", sing_vals.maxCoeff() / sing_vals.minCoeff() );
    printf("\n\n\n\n");
*/

  //myCondNumMatlab(mtx);

  return;
}