/*
 * solvers.cpp
 *
 *  Created on: Sep 29, 2016
 *      Author: george
 */

#include "solvers.hpp"

#include <gecode/int.hh>
#include <gecode/float.hh>
#include <gecode/search.hh>
#include <gecode/gist.hh>

#include <boost/numeric/ublas/io.hpp>

using namespace Gecode;

class LinearEqSysSolver : public Space {
protected:
  IntVarArray x;
public:
  LinearEqSysSolver(
      const std::vector<std::vector<int>>& A,
      const std::vector<int>& y) throw (InvalidArgumentException)
    : x(*this, y.size(), 1, Int::Limits::max) {

    const size_t n_actors = y.size();
    const size_t n_channels = A.size();

    for (size_t i=0; i<n_channels; i++ ) {
      IntArgs c(n_actors);
      IntVarArgs xs(n_actors);
      for (size_t j=0; j<n_actors; j++ ) {
        c[j]  = A[i][j];
        xs[j] = x[j];
      }

      linear(*this, c, xs, IRT_EQ, y[i]);
    }
    branch(*this, x, INT_VAR_SIZE_MIN(), INT_VAL_MIN());
  }

  LinearEqSysSolver(bool share, LinearEqSysSolver& s) : Space(share, s) {
    x.update(*this, share, s.x);
  }

  virtual Space* copy(bool share) {
    return new LinearEqSysSolver(share,*this);
  }

  void print(void) const {
    std::cout << x << std::endl;
  }

  vector<int> getSolution(void) const {
    vector<int> tmp_sol;
    for (int i=0; i<x.size(); i++) {
      //if x[i].assigned()
      tmp_sol.push_back(x[i].val());
      //...else something is wrong
    }
    return tmp_sol;
  }

  // constrain function
  virtual void constrain(const Space& _b) {
    const LinearEqSysSolver& b = static_cast<const LinearEqSysSolver&>(_b);

    int sum = 0;
    IntVarArgs xs(b.x.size());
    for (int i=0; i<b.x.size(); i++) {
      sum += b.x[i].val();
      xs[i] = x[i];
    }
    linear(*this, xs, IRT_LE, sum);
  }
};

class FloatSysSolver : public Space {
protected:
  FloatVarArray x;
public:
  FloatSysSolver(
      const std::vector<std::vector<int>>& A,
      const std::vector<int>& y) throw (InvalidArgumentException)
    : x(*this, y.size(), 1, Float::Limits::max) {

    const size_t n_actors = y.size();
    const size_t n_channels = A.size();

    for (size_t i=0; i<n_channels; i++ ) {
      FloatValArgs c(n_actors);
      FloatVarArgs xs(n_actors);
      for (size_t j=0; j<n_actors; j++ ) {
        c[j]  = A[i][j];
        xs[j] = x[j];
      }

      linear(*this, c, xs, FRT_EQ, y[i]);
    }
    branch(*this, x, FLOAT_VAR_SIZE_MIN(), FLOAT_VAL_SPLIT_MIN());
  }

  FloatSysSolver(bool share, FloatSysSolver& s) : Space(share, s) {
    x.update(*this, share, s.x);
  }

  virtual Space* copy(bool share) {
    return new FloatSysSolver(share,*this);
  }

  void print(void) const {
    std::cout << x << std::endl;
  }

  vector<double> getSolution(void) const {
    vector<double> tmp_sol;
    for (int i=0; i<x.size(); i++) {
      //if x[i].assigned()
      tmp_sol.push_back(x[i].val().min());
      //...else something is wrong
    }
    return tmp_sol;
  }

  // constrain function
  virtual void constrain(const Space& _b) {
    const FloatSysSolver& b = static_cast<const FloatSysSolver&>(_b);

    double sum = 0;
    FloatVarArgs xs(b.x.size());
    for (int i=0; i<b.x.size(); i++) {
      sum += b.x[i].val().max();
      xs[i] = x[i];
    }
    linear(*this, xs, FRT_LE, sum);
  }
};

vector<int> tools::solveLinearEqSys(
    const std::vector<std::vector<int>>& A,
    const std::vector<int>& y) throw (InvalidArgumentException) {

  LinearEqSysSolver* m = new LinearEqSysSolver(A,y);
  //Gist::bab(m);
  BAB<LinearEqSysSolver> e(m);
  delete m;

  std::vector<int> solutions;
  while (LinearEqSysSolver* s = e.next()) {
    s->print();
    solutions = s->getSolution();
    delete s;
  }
  //if solutions.size() = 0, SDFG is probably not consistent!
  return solutions;
}

vector<double> tools::solveFloatSys(
    const std::vector<std::vector<int>>& A,
    const std::vector<int>& y) throw (InvalidArgumentException) {

  FloatSysSolver* m = new FloatSysSolver(A,y);
  //Gist::bab(m);
  BAB<FloatSysSolver> e(m);
  delete m;

  std::vector<double> solutions;
  while (FloatSysSolver* s = e.next()) {
    s->print();
    solutions = s->getSolution();
    delete s;
  }
  //if solutions.size() = 0, SDFG is probably not consistent!
  return solutions;
}


// Copyright (c) 2017 Evan S Weinberg
// Test code for a real operator.

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

#include "blas/generic_vector.h"

#include "square_laplace.h"

using namespace std;
using namespace Eigen;

typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;
typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;

int main(int argc, char** argv)
{
  double *rhs_real;
  double *rhs_real_indef;
  complex<double> *rhs_cplx;
  complex<double> *rhs_cplx_indef;
  complex<double> *gauge_links;

  // Set output precision to be long.
  cout << setprecision(10);

  // RNG related things.
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.
  double inv_variance = 6.0; // inverse of variance for gaussian non-compact U(1) links.

  // Basic information about the lattice.
  int length = 8;
  double m_sq = 0.001;

  // Some start-up.
  int volume = length*length;
  int wilson_volume = 2*volume;

  // Create a random compact U(1) link.
  gauge_links = allocate_vector<complex<double>>(2*length*length);
  gaussian_real(gauge_links, 2*length*length, generator, 1.0/inv_variance);
  polar(gauge_links, 2*length*length);

  // Vectors.
  rhs_real = allocate_vector<double>(volume);
  rhs_real_indef = allocate_vector<double>(volume);
  rhs_cplx = allocate_vector<complex<double>>(volume);
  rhs_cplx_indef = allocate_vector<complex<double>>(wilson_volume);

  // Zero out the vector.
  zero_vector(rhs_real, length*length);
  zero_vector(rhs_real_indef, length*length);
  zero_vector(rhs_cplx, length*length);
  zero_vector(rhs_cplx_indef, 2*length*length);

  //////////////////////////
  // REAL, SYMMETRIC CASE //
  //////////////////////////

  std::cout << "Real, Symmetric case.\n\n";

  // Structure which gets passed to the function.
  laplace_struct lapstr;
  lapstr.length = length;
  lapstr.m_sq = m_sq;

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real = dMatrix::Zero(volume, volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_real, volume);
    rhs_real[i] = 1.0;

    // Where we put the result of the matrix element.
    double* mptr = &(mat_real(i*volume));

    square_laplacian(mptr, rhs_real, &lapstr);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 16)
  {
    std::cout << mat_real << "\n";
  }

  // Get the eigenvalues.
  SelfAdjointEigenSolver<dMatrix> eigsolve_real(volume);
  eigsolve_real.compute(mat_real);

  std::cout << "The eigenvalues are:\n" << eigsolve_real.eigenvalues() << "\n";

  /////////////////////////////
  // COMPLEX, HERMITIAN CASE //
  /////////////////////////////

  std::cout << "\n\nComplex, Symmetric case.\n\n";

  // Structure which gets passed to the function.
  laplace_gauged_struct lapstr_gauged;
  lapstr_gauged.length = length;
  lapstr_gauged.m_sq = m_sq;
  lapstr_gauged.gauge_links = gauge_links;

  // Allocate a sufficiently gigantic matrix.
  cMatrix mat_cplx = cMatrix::Zero(volume, volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_cplx, volume);
    rhs_cplx[i] = 1.0;

    // Where we put the result of the matrix element.
    complex<double>* mptr = &(mat_cplx(i*volume));

    square_laplacian_gauged(mptr, rhs_cplx, &lapstr_gauged);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 9)
  {
    std::cout << mat_cplx << "\n";
  }

  // Get the eigenvalues.
  SelfAdjointEigenSolver<cMatrix> eigsolve_cplx(volume);
  eigsolve_cplx.compute(mat_cplx);

  std::cout << "The eigenvalues are:\n" << eigsolve_cplx.eigenvalues() << "\n";

  ///////////////////////////
  // REAL, INDEFINITE CASE //
  ///////////////////////////

  std::cout << "\n\nReal, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real_indef = dMatrix::Zero(volume, volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_real_indef, volume);
    rhs_real_indef[i] = 1.0;

    // Where we put the result of the matrix element.
    double* mptr = &(mat_real_indef(i*volume));

    square_staggered(mptr, rhs_real_indef, &lapstr);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 16)
  {
    std::cout << mat_real_indef << "\n";
  }

  // Get the eigenvalues.
  EigenSolver<dMatrix> eigsolve_real_indef(wilson_volume);
  eigsolve_real_indef.compute(mat_real_indef);

  std::cout << "The eigenvalues are:\n" << eigsolve_real_indef.eigenvalues() << "\n";

  //////////////////////////////
  // COMPLEX, INDEFINITE CASE //
  //////////////////////////////

  std::cout << "\n\nComplex, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  cMatrix mat_cplx_indef = cMatrix::Zero(wilson_volume, wilson_volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < wilson_volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_cplx_indef, wilson_volume);
    rhs_cplx_indef[i] = 1.0;

    // Where we put the result of the matrix element.
    complex<double>* mptr = &(mat_cplx_indef(i*wilson_volume));

    square_wilson_gauged(mptr, rhs_cplx_indef, &lapstr_gauged);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 9)
  {
    std::cout << mat_cplx_indef << "\n";
  }

  // Get the eigenvalues.
  ComplexEigenSolver<cMatrix> eigsolve_cplx_indef(wilson_volume);
  eigsolve_cplx_indef.compute(mat_cplx_indef);

  std::cout << "The eigenvalues are:\n" << eigsolve_cplx_indef.eigenvalues() << "\n";

  //////////////
  // CLEAN UP //
  //////////////

  // Free the lattice.
  //delete[] lattice;
  deallocate_vector(&rhs_real);
  deallocate_vector(&rhs_real_indef);
  deallocate_vector(&rhs_cplx);
  deallocate_vector(&rhs_cplx_indef);
  deallocate_vector(&gauge_links);
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Tests for the cylindrical bessel function of (fixed_point) for a small digit range.

#define BOOST_TEST_MODULE test_negatable_math_cyl_bessel_j_small
#define BOOST_LIB_DIAGNOSTIC

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_math_cyl_bessel_j_small)
{
  typedef boost::fixed_point::negatable<  std::numeric_limits<int>::digits,
                                        -(std::numeric_limits<long double>::digits - std::numeric_limits<int>::digits)>
  fixed_point_type;

  typedef fixed_point_type::float_type float_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 6);

  BOOST_CONSTEXPR int i_max = 4;

  // Check small arguments in the region of Taylor series expansion.
  for(int i = 1; i <= i_max; ++i)
  {
    // Use an integer-valued order wrapped in the fixed-point type in order
    // to force a high level of compiler work but simultaneously avoid
    // potentially lossy tgamma calculations (requiring Bernoulli numbers).

    const fixed_point_type x = boost::math::cyl_bessel_j(fixed_point_type(2), fixed_point_type(i) / boost::math::constants::e<fixed_point_type>());
    const fixed_point_type y = boost::math::cyl_bessel_j(float_point_type(2), float_point_type(i) / boost::math::constants::e<float_point_type>());

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }
}


#ifndef TRIUMF_STATISTICAL_MECHANICS_FERMI_DIRAC_HPP
#define TRIUMF_STATISTICAL_MECHANICS_FERMI_DIRAC_HPP

#include <cmath>

#include <boost/math/constants/constants.hpp>

#include <triumf/constants/codata_2018.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace statistical_mechanics {

// Fermi-Dirac statistics
namespace fermi_dirac {

// chemical potential of a Fermi gas
template <typename T> T chemical_potential(T temperature, T E_0, T E_F) {
  /*
  // Sommerfeld expansion
  constexpr T k_B =
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  T fraction = (k_B * temperature) / E_F;
  return E_0 +
         E_F * (1.0 -
                std::pow(boost::math::constants::pi<T>() * fraction, 2) / 12.0 -
                std::pow(boost::math::constants::pi<T>() * fraction, 4) / 80.0);
  */
  return E_0 + E_F;
}

// Fermi-Dirac distribution
template <typename T> T distribution(T temperature, T energy, T E_0, T E_F) {
  constexpr T k_B =
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  T mu = chemical_potential<T>(temperature, E_0, E_F);
  T arg = (energy - mu) / (k_B * temperature);
  return 1.0 / (std::exp(arg) + 1.0);
}

// Fermi-Dirac function
template <typename T> T function(T temperature, T energy, T E_0, T E_F) {
  T f_E = distribution<T>(temperature, energy, E_0, E_F);
  return f_E * (1.0 - f_E);
}

} // namespace fermi_dirac

} // namespace statistical_mechanics

} // namespace triumf

#endif // TRIUMF_STATISTICAL_MECHANICS_FERMI_DIRAC_HPP


/// \file
// --------------------------------------------------------------------------
// This file is part of the reference implementation for the paper
//    QFib: Fast and Efficient Brain Tractogram Compression
//    C. Mercier*, S. Rousseau*, P. Gori, I. Bloch and T. Boubekeur
//    NeuroInformatics 2020
//    DOI: 10.1007/s12021-020-09452-0
//
// All rights reserved. Use of this source code is governed by a
// MIT license that can be found in the LICENSE file.
// --------------------------------------------------------------------------
#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <bitset>

#include "oct.hpp"
#include "sfibonacci.h"
#include "bitvalue.hpp"
#include "parallel.hpp"

using namespace Eigen;

//struct CompressedVector {Bitvalue<T>pv;};
template<typename T>
using CompressedVector = Bitvalue<T>;

template<typename T>
struct CompressedFiber
{
	Vector3f origin;
	Vector3f second;
	std::vector<CompressedVector<T>> data;
};

float epsMapping = 0.08f;

enum QuantizationMethod
{
	OCTAHEDRAL,
	SPHERICALFIBONACCI
};

QuantizationMethod quantizationMethod = QuantizationMethod::SPHERICALFIBONACCI;

namespace fc
{
template<typename T>
Vector3f unquantize(const CompressedVector<T> & cvec)
{
	switch(quantizationMethod)
	{
	float vec[3];
	case(QuantizationMethod::OCTAHEDRAL):
		octDecode(cvec, vec);
		return Vector3f(vec[0], vec[1], vec[2]).normalized();
	case(QuantizationMethod::SPHERICALFIBONACCI):
		static uint32_t nbFiboPoints = 1 << (bitcount);
		return fib::SF(cvec, nbFiboPoints);
	default :
		std::cerr << "ERROR: Quantization Method not handled !" << std::endl;
		exit(EXIT_FAILURE);
		break;

	}
}

template<typename T>
void quantize(const Vector3f & vec, CompressedVector<T> & cvec)
{
	switch(quantizationMethod)
	{
	case(QuantizationMethod::OCTAHEDRAL) :
		octEncode(vec.data(), cvec);
		break;
	case(QuantizationMethod::SPHERICALFIBONACCI):
		static uint32_t nbFiboPoints = 1 << (bitcount);
		cvec.setCombinedValue(fib::inverseSF<T>(vec, nbFiboPoints));
		break;
	default:
		std::cerr << "ERROR: Quantization Method not handled !" << std::endl;
		exit(EXIT_FAILURE);
		break;
	}
}

///
/// \brief uniformMapping Uniform mapping from uniquant (Fast lossy compression of 3d unit vectors)
/// \param v Vector to map
/// \param axis Axis to use for the spherical cap
/// \param ratio Ratio to use for the mapping
/// \return The mapped vector
///
Vector3f uniformMapping(const Vector3f & v, const Vector3f & axis, const float ratio)
{
	Vector3d dv = v.cast<double>();
	Vector3d daxis = axis.cast<double>();
	dv.normalize();
	daxis.normalize();

	double K = 1.0 / ratio;
	double c = dv.dot(daxis);

	// numerical instabilities
	if (c > 1.0)
		c = 1.0;
	if (c < -1.0)
		c = -1.0;
	Vector3d p1 = (dv - c * daxis).normalized();
	double delta = (1.0 - ((1.0 - c) / K));
	return  ((p1 * std::sqrt(1.0 - delta * delta) + delta * daxis).cast<float>()).normalized();
}

///
/// \brief inverseUniformMapping Inverse uniform mapping from uniquant (Fast lossy compression of 3d unit vectors)
/// \param v Vector to unmap
/// \param axis Axis to use for the spherical cap
/// \param ratio Ratio to use for the mapping
/// \return The unmapped vector
///

Vector3f inverseUniformMapping(const Vector3f & v, const Vector3f & axis, const float ratio)
{
	//return v;
	Vector3d v2 = v.cast<double>();
	Vector3d axis2 = axis.cast<double>();
	v2.normalize();
	axis2.normalize();

	double K = ratio;
	double c = v2.dot(axis2);

	/// numerical instabilities
	if (c > 1.0)
		c = 1.0;
	if (c < -1.0)
		c = -1.0;
	Vector3d p1 = (v2 - c * axis2).normalized();
	double delta = (1.0 - ((1.0 - c) / K));
	return  ((p1 * std::sqrt(1.0 - delta * delta) + delta * axis2).cast<float>()).normalized();
}

///
/// \brief computeRatio Compute the minimum dot product of the input fiber (only one)
/// \param fiber Fiber from which to compute the minimum dot product
/// \return Minimum dot product of the unique fiber
///

float computeRatio(const std::vector<Vector3f> & fibers)
{
	Vector3f v1, v2;
	float mindot = 2.f;
	v1 = (fibers[1] - fibers[0]).normalized();
	for(unsigned p = 2; p < fibers.size(); ++p)
	{
		v2 = (fibers[p] - fibers[p-1]).normalized();
		float dot = v2.dot(v1);
		mindot = (dot <  mindot) ? dot : mindot;
		v1 = v2;
	}

	return mindot;
}

///
/// \brief computeRatioVerif Compute the ratio of the input fibers and verify that stepsizes are almost constant
/// \param fibers Fibers from which to compute the ratio
/// \param verbose Parameter to display the maximum angle if true
/// \param totalNbPts Variable that will contain the total number of points in the bundle, used for average error computation
/// \return Ratio of the bundle of fibers
///

float computeRatioVerif(const std::vector<std::vector<Vector3f> > & fibers, bool verbose, uint64_t & totalNbPts)
{
	unsigned maxThreads = std::max(unsigned(1), std::thread::hardware_concurrency());
	std::vector<uint64_t> nbPts(maxThreads, 0);
	std::vector<float> mindot(maxThreads, 2.0f);
	std::vector<float> maxStep(maxThreads, 0.0f);
	std::vector<float> minStep(maxThreads, std::numeric_limits<float>::max());
#ifdef USE_OPENMP
#pragma omp parallel for
	for(int f = 0; f < fibers.size(); ++f)
	{
		nbPts[omp_get_thread_num()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]);
			v2norm = v2.norm();
			maxStep[omp_get_thread_num()] = (v2norm > maxStep[omp_get_thread_num()]) ? v2norm : maxStep[omp_get_thread_num()];
			minStep[omp_get_thread_num()] = (v2norm < minStep[omp_get_thread_num()]) ? v2norm : minStep[omp_get_thread_num()];
			float dot = (v2 / v2norm).dot(v1);
			mindot[omp_get_thread_num()] = (dot <  mindot[omp_get_thread_num()]) ? dot : mindot[omp_get_thread_num()];
			v1 = v2 / v2norm;
		}
	}
#else
	parallel.For<unsigned>(0, fibers.size(), 1, [&](unsigned f)
	{
		nbPts[parallel.getID()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]);
			v2norm = v2.norm();
			maxStep[parallel.getID()] = (v2norm > maxStep[parallel.getID()]) ? v2norm : maxStep[parallel.getID()];
			minStep[parallel.getID()] = (v2norm < minStep[parallel.getID()]) ? v2norm : minStep[parallel.getID()];
			float dot = (v2 / v2norm).dot(v1);
			mindot[parallel.getID()] = (dot <  mindot[parallel.getID()]) ? dot : mindot[parallel.getID()];
			v1 = v2 / v2norm;
		}
	});
#endif
	float mdot = mindot[0], maxS = maxStep[0], minS = minStep[0];
	totalNbPts=nbPts[0];
	for (unsigned int i=1; i<mindot.size(); i++)
	{
		totalNbPts += nbPts[i];
		mdot = (mindot[i] < mdot) ? mindot[i] : mdot;
		maxS = (maxStep[i] > maxS) ? maxStep[i] : maxS;
		minS = (minStep[i] < minS) ? minStep[i] : minS;
	}
	float diff = maxS - minS;

	if (verbose)
	{
		std::cout << "Max angle: " << acos(mdot) * 180 / fib::PI << "°" << std::endl;
		std::cout << "Max length between segments: " << maxS << std::endl;
		std::cout << "Min length between segments: " << minS << std::endl;
		std::cout << "Difference: " << diff << std::endl;
	}

	if (diff > 0.1f * (maxS + minS) / 2.0f)
	{
		std::cerr << std::endl << "-------------!ERROR!--------------" << std::endl;
		std::cerr << "Difference in segments length too big (" << diff << "), no compression possible" << std::endl;
		std::cerr << "Try to resample your data with a constant stepsize before compression" << std::endl;
		std::cerr << "-------------!ERROR!--------------" << std::endl << std::endl;
		exit(EXIT_FAILURE);
	}

	epsMapping = 0.f;
	for(unsigned i = 0; i < 20; ++i)
		epsMapping = 3 * sqrt(2) * pow(2, -bitcount/2.f) * ((1 - (mdot))/2+epsMapping);
	//Error maximization proportionaly to itself (epsMapping < 1)
	epsMapping = sqrt(epsMapping);
	return std::min(1.f, (1.f - mdot) / 2.f + epsMapping);
}

///
/// \brief computeRatioVerif Compute the ratio of the input fibers without verifying that stepsizes are almost constant
/// \param fibers Fibers from which to compute the ratio
/// \param verbose Parameter to display the maximum angle if true
/// \param totalNbPts Variable that will contain the total number of points in the bundle, used for average error computation
/// \return Ratio of the bundle of fibers
///

float computeRatio(const std::vector<std::vector<Vector3f> > & fibers, bool verbose, uint64_t & totalNbPts)
{
	unsigned maxThreads = std::max(unsigned(1), std::thread::hardware_concurrency());
	std::vector<uint64_t> nbPts(maxThreads, 0);
	std::vector<float> mindot(maxThreads, 2.0f);

#ifdef USE_OPENMP
#pragma omp parallel for
	for(int f = 0; f < fibers.size(); ++f)
	{
		nbPts[omp_get_thread_num()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]).normalized();
			float dot = v2.dot(v1);
			mindot[omp_get_thread_num()] = (dot <  mindot[omp_get_thread_num()]) ? dot : mindot[omp_get_thread_num()];
			v1 = v2;
		}
	}
#else
	parallel.For<unsigned>(0, fibers.size(), 1, [&](unsigned f)
	{
		nbPts[parallel.getID()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]).normalized();
			float dot = v2.dot(v1);
			mindot[parallel.getID()] = (dot <  mindot[parallel.getID()]) ? dot : mindot[parallel.getID()];
			v1 = v2;
		}
	});
#endif
	float mdot=mindot[0];
	totalNbPts=nbPts[0];
	for (unsigned int i=1; i<mindot.size(); i++)
	{
		totalNbPts += nbPts[i];
		mdot = (mindot[i] < mdot) ? mindot[i] : mdot;
	}

	if (verbose)
		std::cout << "Max angle: " << acos(mdot) * 180 / fib::PI << "°" << std::endl;

	epsMapping = 0.f;
	for(unsigned i = 0; i < 20; ++i)
		epsMapping = 3 * sqrt(2) * pow(2, -bitcount/2.f) * ((1 - (mdot))/2+epsMapping);
	//Error maximization proportionaly to itself (epsMapping < 1)
	epsMapping = sqrt(epsMapping);
	return std::min(1.f, (1.f - mdot) / 2.f + epsMapping);
}

///
/// \brief compressFiber Compression of the fiber considering its ratio
/// \param fiber Fiber to compress
/// \param ratio Ratio to use for the compression
/// \param cfiber Fiber compressed
/// \param template T is the type of the compressed data, either int8 or int16 depending on the precision asked
///

template<typename T>
void compressFiber(const std::vector<Vector3f> & fiber, const float ratio, CompressedFiber<T> & cfiber)
{
	assert(fiber.size() > 1);

	//encode the first two points
	cfiber.origin = fiber[0];
	cfiber.second = fiber[1];

	cfiber.data.resize(fiber.size() - 2);

	//compute the first unit vector and the stepsize
	Vector3f axis = (fiber[1] - fiber[0]).normalized();
	float stepsize = (fiber[1] - fiber[0]).norm();
	Vector3f currentpoint = fiber[1];

	//compress the rest of the fiber
	Vector3f v;
	constexpr float eps = 0.00000001;
	for(unsigned i = 2; i < fiber.size(); ++i)
	{
		v = (fiber[i] - currentpoint).normalized();
		if(((1.f - v.dot(axis)) / 2.f) > ratio) //Conservative mapping in case the epsilon is not enough
		{
			fc::quantize((-((1.0 - eps) * axis) + (eps * v)).normalized(), cfiber.data[i-2]);
		}
		else //Otherwise, general case
		{
			if(abs(v.dot(axis)) < 1.f)
				fc::quantize(fc::inverseUniformMapping(v, axis, ratio), cfiber.data[i-2]);
			else
			{
				fc::quantize(v, cfiber.data[i-2]);
			}
		}
		axis = fc::uniformMapping(fc::unquantize(cfiber.data[i-2]), axis, ratio);
		currentpoint += (stepsize * axis);
		//currentpoint = fiber[i]; //Uncomment if you want to disable error propagation !!
	}
}

///
/// \brief compressFiber Compression of the fiber considering its ratio, computing the obtained error at the same time
/// \param fiber Fiber to compress
/// \param ratio Ratio to use for the compression
/// \param cfiber Fiber compressed
/// \param maxError Maximum error (at point level) when compressing the fiber
/// \param meanError Average error (at point level) when compressing the fiber
/// \param template T is the type of the compressed data, either int8 or int16 depending on the precision asked
///

template<typename T>
void compressFiber(const std::vector<Vector3f> & fiber, const float ratio, CompressedFiber<T> & cfiber, float & maxError, double & meanError)
{
	assert(fiber.size() > 1);

	//encode the first two points
	cfiber.origin = fiber[0];
	cfiber.second = fiber[1];

	cfiber.data.resize(fiber.size() - 2);

	//compute the first unit vector and the stepsize
	Vector3f axis = (fiber[1] - fiber[0]).normalized();
	float stepsize = (fiber[1] - fiber[0]).norm();
	Vector3f currentpoint = fiber[1];

	//compress the rest of the fiber
	Vector3f v;
	constexpr float eps = 0.00000001;
	for(unsigned i = 2; i < fiber.size(); ++i)
	{
		v = (fiber[i] - currentpoint).normalized();
		if(((1.f - v.dot(axis)) / 2.f) > ratio) //Conservative mapping in case the epsilon is not enough
		{
			fc::quantize((-((1.0 - eps) * axis) + (eps * v)).normalized(), cfiber.data[i-2]);
		}
		else //Otherwise, general case
		{
			if(abs(v.dot(axis)) < 1.f)
				fc::quantize(fc::inverseUniformMapping(v, axis, ratio), cfiber.data[i-2]);
			else
				fc::quantize(v, cfiber.data[i-2]);
		}
		axis = fc::uniformMapping(fc::unquantize(cfiber.data[i-2]), axis, ratio);
		currentpoint += (stepsize * axis);
		float err = (fiber[i] - currentpoint).norm();
		maxError = err > maxError ? err : maxError;
		meanError += err;
	}
}

///
/// \brief decompressFiber Decompress the input fiber
/// \param cfiber Compressed fiber to decompress
/// \param ratio Ratio to use for decompression (same than for compression)
/// \param fiber Decompressed fiber
///

template<typename T>
void decompressFiber(const CompressedFiber<T> & cfiber, const float ratio, std::vector<Vector3f> & fiber)
{
	fiber.reserve(cfiber.data.size() + 2);
	fiber.push_back(cfiber.origin);
	fiber.push_back(cfiber.second);

	float stepsize = (cfiber.second - cfiber.origin).norm();
	Vector3f axis = (fiber[1] - fiber[0]).normalized();

	for(unsigned i = 0; i < cfiber.data.size(); ++i)
	{
		Vector3f v = fc::unquantize(cfiber.data[i]);
		if(abs(v.dot(axis)) < 1.f)
			v = fc::uniformMapping(v, axis, ratio);

		fiber.push_back(fiber[i+1] + stepsize * v);
		axis = v;
	}
}
}


/*
 * Copyright Nick Thompson, 2021
 * 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)
 */

#include "math_unit_test.hpp"
#include <random>
#include <boost/math/tools/quartic_roots.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::tools::quartic_roots;
using std::cbrt;
using std::sqrt;

template<class Real>
void test_zero_coefficients()
{
    Real a = 0;
    Real b = 0;
    Real c = 0;
    Real d = 0;
    Real e = 0;
    auto roots = quartic_roots(a,b,c,d,e);
    CHECK_EQUAL(roots[0], Real(0));
    CHECK_EQUAL(roots[1], Real(0));
    CHECK_EQUAL(roots[2], Real(0));
    CHECK_EQUAL(roots[3], Real(0));

    b = 1;
    e = 1;
    // x^3 + 1 = 0:
    roots = quartic_roots(a,b,c,d,e);
    CHECK_EQUAL(roots[0], Real(-1));
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);
    CHECK_NAN(roots[3]);
    e = -1;
    // x^3 - 1 = 0:
    roots = quartic_roots(a,b,c,d,e);
    CHECK_EQUAL(roots[0], Real(1));
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);
    CHECK_NAN(roots[3]);

    e = -2;
    // x^3 - 2 = 0
    roots = quartic_roots(a,b,c,d,e);
    CHECK_ULP_CLOSE(roots[0], cbrt(Real(2)), 2);
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);
    CHECK_NAN(roots[3]);

    // x^4 -1 = 0
    // x = \pm 1:
    roots = quartic_roots<Real>(1, 0, 0, 0, -1);
    CHECK_ULP_CLOSE(Real(-1), roots[0], 3);
    CHECK_ULP_CLOSE(Real(1), roots[1], 3);
    CHECK_NAN(roots[2]);
    CHECK_NAN(roots[3]);

    // x^4 - 2 = 0 \implies x = \pm sqrt(sqrt(2))
    roots = quartic_roots<Real>(1,0,0,0,-2);
    CHECK_ULP_CLOSE(-sqrt(sqrt(Real(2))), roots[0], 3);
    CHECK_ULP_CLOSE(sqrt(sqrt(Real(2))), roots[1], 3);
    CHECK_NAN(roots[2]);
    CHECK_NAN(roots[3]);

    // x(x-1)(x-2)(x-3) = x^4 - 6x^3 + 11x^2 - 6x
    roots = quartic_roots(Real(1), Real(-6), Real(11), Real(-6), Real(0));
    CHECK_ULP_CLOSE(roots[0], Real(0), 2);
    CHECK_ULP_CLOSE(roots[1], Real(1), 2);
    CHECK_ULP_CLOSE(roots[2], Real(2), 2);
    CHECK_ULP_CLOSE(roots[3], Real(3), 2);

     // (x-1)(x-2)(x-3)(x-4) = x^4 - 10x^3 + 35x^2 - (2*3*4 + 1*3*4 + 1*2*4 + 1*2*3)x + 1*2*3*4
    roots = quartic_roots<Real>(1, -10, 35, -24 - 12 - 8 - 6, 1*2*3*4);
    CHECK_ULP_CLOSE(Real(1), roots[0], 2);
    CHECK_ULP_CLOSE(Real(2), roots[1], 2);
    CHECK_ULP_CLOSE(Real(3), roots[2], 2);
    CHECK_ULP_CLOSE(Real(4), roots[3], 2);

    // Double root:
    // (x+1)^2(x-2)(x-3) = x^4 - 3x^3 -3x^2 + 7x + 6
    // Note: This test is unstable wrt to perturbations!
    roots = quartic_roots(Real(1), Real(-3), Real(-3), Real(7), Real(6));
    CHECK_ULP_CLOSE(Real(-1), roots[0], 2);
    CHECK_ULP_CLOSE(Real(-1), roots[1], 2);
    CHECK_ULP_CLOSE(Real(2), roots[2], 2);
    CHECK_ULP_CLOSE(Real(3), roots[3], 2);

    std::uniform_real_distribution<Real> dis(-2,2);
    std::mt19937 gen(12343);
    // Expected roots
    std::array<Real, 4> r;
    int trials = 10;
    for (int i = 0; i < trials; ++i) {
        // Mathematica:
        // Expand[(x - r0)*(x - r1)*(x - r2)*(x-r3)]
        // r0 r1 r2 r3 - (r0 r1 r2 + r0 r1 r3 + r0 r2 r3 + r1r2r3)x
        // + (r0 r1 + r0 r2 + r0 r3 + r1 r2 + r1r3 + r2 r3)x^2 - (r0 + r1 + r2 + r3) x^3 + x^4
        for (auto & root : r) {
            root = static_cast<Real>(dis(gen));
        }
        std::sort(r.begin(), r.end());
        Real a = 1;
        Real b = -(r[0] + r[1] + r[2] + r[3]);
        Real c = r[0]*r[1] + r[0]*r[2] + r[0]*r[3] + r[1]*r[2] + r[1]*r[3] + r[2]*r[3];
        Real d = -(r[0]*r[1]*r[2] + r[0]*r[1]*r[3] + r[0]*r[2]*r[3] + r[1]*r[2]*r[3]);
        Real e = r[0]*r[1]*r[2]*r[3];

        auto roots = quartic_roots(a, b, c, d, e);
        // I could check the condition number here, but this is fine right?
        CHECK_ULP_CLOSE(r[0], roots[0], 160);
        CHECK_ULP_CLOSE(r[1], roots[1], 260);
        CHECK_ULP_CLOSE(r[2], roots[2], 160);
        CHECK_ULP_CLOSE(r[3], roots[3], 160);
    }
}

int main()
{
    test_zero_coefficients<float>();
    test_zero_coefficients<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_zero_coefficients<long double>();
#endif
    return boost::math::test::report_errors();
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
#include <fstream>
#include "CommonCurve.h" //std::left std::setw std::setfill
#include "CeresCost.h"
#include <boost/program_options.hpp>
#include <algorithm>
#include <iterator>
#include <filesystem>
namespace fs = std::filesystem;
using namespace boost;
namespace po = boost::program_options;

int main ( int argc, char** argv )
{
    std::string dataFile = "readDataCeres.txt";
    std::string parameter = "parametersCeres.txt";
    std::string path = "./result/";
    // create directory if not exist
    struct stat info;
    if( stat( path.c_str(), &info ) != 0 )fs::create_directories(path);
    std::string pathRealData = path + za::getCurrentTime() + dataFile;
    std::string pathParameter = path + za::getCurrentTime() + parameter;

    // True parameter value to be estimated
    double a, b, c;

    // Total number of data point
    int N, iterate ;

    // Noise Sigma value
    double w_sigma;
    // Cammand line parser
	try
		{
			po::options_description desc("Allowed options");
			desc.add_options()
				("help,h", "produce help message")
				("first,a", po::value<double>(),
					"first parameter")
                ("second,b", po::value<double>(),
					"second parameter")
				("third,c", po::value<double>(),
					"third parameter")
                ("number,n", po::value<int>(&N)->default_value(100)->implicit_value(500),
					"number of data")
                ("iteration,i", po::value<int>(&iterate)->default_value(100)->implicit_value(200),
					"number of iterations")
				("noise,s", po::value<double>(&w_sigma)->default_value(1)->implicit_value(5),
					"noise added");

			/* Point out that all unknown values should be converted to the value of the "input-file" option.
					* Also use the command_line_parser class instead of parse_command_line */
			po::variables_map vm;
			po::store(po::parse_command_line(argc, argv, desc), vm);
			po::notify(vm);

			if (vm.count("help"))
			{
				std::cout << "Usage: options_description [options]\n";
				std::cout << desc;
				return 0;
			}
 			if (vm.count("first") )
        	{

                a =  vm["first"].as<double>();
        	}
        	else
       		{
            	std::cout << "First parameter not set.\n";
				return -1;
        	}
 			if (vm.count("second") )
        	{

                b =  vm["second"].as<double>();
        	}
        	else
       		{
            	std::cout << "Second parameter not set.\n";
				return -1;
        	}
 			if (vm.count("third") )
        	{

                a =  vm["third"].as<double>();
        	}
        	else
       		{
            	std::cout << "Third parameter not set.\n";
				return -1;
        	}
        }// end of try

        catch(std::exception& e)
		{
			std::cout << e.what() << "\n";
			return 1;
		}

    // OpenCV random number generator
    cv::RNG rng;

    // abc Estimated value of the parameter
    double abc[3] = {0,0,0};

   // set of x and y data
    std::vector<double> x_data, y_data;
    std::ofstream myfile;
    const int nameWidth     = 6;
    const int numWidth      = 8;
    myfile.open (&pathRealData[0]);
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    za::printElement("|", 1, myfile);
    za::printElement("N", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("X", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("Y", 10, myfile);
    za::printElement("|", 1, myfile);
    myfile <<"\n";
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    std::cout<<"Generating data: \n";

    for ( int i=0; i<N; i++ )
    {
        // x data
        double x = i/100.0;
        x_data.push_back ( x );

        // y data
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );

        za::printElement("|", 1, myfile);
        za::printElement(i+1, nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(x_data[i], nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(y_data[i], 10, myfile);
        za::printElement("|", 1, myfile);
        myfile <<"\n";
        for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
        myfile <<"\n";

    }
    myfile.close();
    // Constructing the Least Squares Problem
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
    // Add an error term to the question
    // Use automatic derivation, template parameters:
    // error type, output dimension, input dimension, dimension should be consistent with the previous struct
        problem.AddResidualBlock (
            new ceres::AutoDiffCostFunction<za::CURVE_FITTING_COST, 1, 3> (
                new za::CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // Kernel function, not used here, empty
            abc                 // Parameters to be estimated
        );
    }

    //Configure solver

    ceres::Solver::Options options;

    // How to solve the incremental equation
    options.linear_solver_type = ceres::DENSE_QR;

    // Output to std::cout
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = iterate;

    // Optimization information
    ceres::Solver::Summary summary;

    // Timer to compute solving time
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

    // Start to optimize
    ceres::Solve ( options, &problem, &summary );
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>( t2-t1 );
    std::cout<<"solve time cost = "<<time_used.count()<<" seconds. \n";
    std::ofstream mySol;
    mySol.open (&pathParameter[0]);
    // Output result
    std::cout<<summary.BriefReport() << "\n";
    mySol<<"Estimated a,b,c =";
    for ( auto val:abc ) mySol<<val<<" ";
    mySol<<"\n";
    mySol.close();

    return 0;
}


/**
 * math lib and lapack test
 * @author Tobias Weber <tweber@ill.fr>
 * @date feb-19
 * @license GPLv3, see 'LICENSE' file
 *
 * g++-10 -std=c++20 -DUSE_LAPACK -I.. -I/usr/include/lapacke -I/usr/local/opt/lapack/include -L/usr/local/opt/lapack/lib -o mat0 mat0.cpp -llapacke
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Mat0
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_mat0, t_real, t_types)
{
	using namespace tl2_ops;

	using t_cplx = std::complex<t_real>;
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;
	using t_vec_cplx = tl2::vec<t_cplx, std::vector>;
	using t_mat_cplx = tl2::mat<t_cplx, std::vector>;

	t_real eps = std::pow(std::numeric_limits<t_real>::epsilon(), 1./2.);
	std::cout << "eps = " << eps << std::endl;

	auto M = tl2::create<t_mat>({1, 2, 3, 3, 2, 6, 4, 2, 4});
	auto Z = tl2::create<t_mat_cplx>({1, 2, 3, 3, 2, 6, 4, 2, 4});
	std::cout << "M = " << M << std::endl;
	std::cout << "Z = " << Z << std::endl;

	{
		auto [ok, Q, R] = tl2::qr<t_mat, t_vec>(M);
		t_real d = tl2::det<t_mat>(Q);
		auto QR = Q*R;

		std::cout << "\nok = " << std::boolalpha << ok << std::endl;
		std::cout << "Q = " << Q << std::endl;
		std::cout << "R = " << R << std::endl;
		std::cout << "det(Q) = " << d << std::endl;
		std::cout << "QR = " << QR << std::endl;

		BOOST_TEST(ok);
		BOOST_TEST(tl2::equals<t_real>(d, 1, eps));
		BOOST_TEST(tl2::equals(QR, M, eps));

#ifdef USE_LAPACK
		auto [ok2, P, L, U] = tl2_la::lu<t_mat>(M);
		auto PLU = P*L*U;

		std::cout << "\nok2 = " << std::boolalpha << ok2 << std::endl;
		std::cout << "P = " << P << std::endl;
		std::cout << "L = " << L << std::endl;
		std::cout << "U = " << U << std::endl;
		std::cout << "PLU = " << PLU << std::endl;

		BOOST_TEST(ok2);
		BOOST_TEST(tl2::equals(PLU, M, eps));
#endif
	}

	{
		auto [ok, Q, R] = tl2::qr<t_mat_cplx, t_vec_cplx>(Z);
		t_cplx d = tl2::det<t_mat_cplx>(Q);
		auto QR = Q*R;

		std::cout << "\nok = " << std::boolalpha << ok << std::endl;
		std::cout << "Q = " << Q << std::endl;
		std::cout << "R = " << R << std::endl;
		std::cout << "det(Q) = " << d << std::endl;
		std::cout << "QR = " << QR << std::endl;

		BOOST_TEST(ok);
		BOOST_TEST(tl2::equals<t_cplx>(d, 1, eps));
		BOOST_TEST(tl2::equals(QR, Z, eps));

#ifdef USE_LAPACK
		auto [ok2, P, L, U] = tl2_la::lu<t_mat_cplx>(Z);
		auto PLU = P*L*U;

		std::cout << "\nok2 = " << std::boolalpha << ok2 << std::endl;
		std::cout << "P = " << P << std::endl;
		std::cout << "L = " << L << std::endl;
		std::cout << "U = " << U << std::endl;
		std::cout << "PLU = " << PLU << std::endl;

		BOOST_TEST(ok2);
		BOOST_TEST(tl2::equals(PLU, Z, eps));
#endif
	}

#ifdef USE_LAPACK
	{
		auto [ok, evals, evecs] =
			tl2_la::eigenvec<t_mat_cplx, t_vec_cplx, t_cplx>(Z, 0, 0, 1);
		std::cout << "\nok = " << std::boolalpha << ok << std::endl;
		for(std::size_t i=0; i<evals.size(); ++i)
			std::cout << "eval: " << evals[i] << ", evec: " << evecs[i] << std::endl;

		auto [ok2, U, Vh, vals] = tl2_la::singval<t_mat_cplx>(Z);
		std::cout << "\nok = " << std::boolalpha << ok2 << std::endl;
		std::cout << "singvals: ";
		for(std::size_t i=0; i<vals.size(); ++i)
			std::cout << vals[i] << " ";
		std::cout << std::endl;
		std::cout << "U = " << U << "\nVh = " << Vh << std::endl;

		std::cout << "diag{vals} * UVh = " << U*tl2::diag<t_mat_cplx>(vals)*Vh << std::endl;

		auto [inva, ok3a] = tl2_la::pseudoinv<t_mat_cplx>(Z);
		auto [invb, ok3b] = tl2::inv<t_mat_cplx>(Z);
		std::cout << "\nok = " << std::boolalpha << ok3a << ", " << ok3b << std::endl;
		std::cout << "pseudoinv = " << inva << std::endl;
		std::cout << "      inv  = " << invb << std::endl;

		BOOST_TEST(ok);
		BOOST_TEST(ok2);
		BOOST_TEST(ok3a);
		BOOST_TEST(ok3b);

		auto ident = tl2::unit<t_mat_cplx>(Z.size1(), Z.size2());
		auto mata1 = inva*Z;
		auto mata2 = Z*inva;
		auto matb1 = invb*Z;
		auto matb2 = Z*invb;
		BOOST_TEST(tl2::equals(mata1, ident, eps));
		BOOST_TEST(tl2::equals(matb1, ident, eps));
		BOOST_TEST(tl2::equals(mata2, ident, eps));
		BOOST_TEST(tl2::equals(matb2, ident, eps));
	}

	{
		auto [ok, evals_re, evals_im, evecs_re, evecs_im] =
			tl2_la::eigenvec<t_mat, t_vec, t_real>(M, 0, 0, 1);
		std::cout << "\nok = " << std::boolalpha << ok << std::endl;
		for(std::size_t i=0; i<evals_re.size(); ++i)
			std::cout << "eval: " << evals_re[i] << " + i*" << evals_im[i]
			<< ", evec: " << evecs_re[i] << " +i*" << evecs_im[i] << std::endl;

		auto [ok2, U, Vt, vals] = tl2_la::singval<t_mat>(M);
		std::cout << "\nok = " << std::boolalpha << ok2 << std::endl;
		std::cout << "singvals: ";
		for(std::size_t i=0; i<vals.size(); ++i)
			std::cout << vals[i] << " ";
		std::cout << std::endl;
		std::cout << "U = " << U << "\nVt = " << Vt << std::endl;

		std::cout << "diag{vals} * UVt = " << U*tl2::diag<t_mat>(vals)*Vt << std::endl;

		auto [inva, ok3a] = tl2_la::pseudoinv<t_mat>(M);
		auto [invb, ok3b] = tl2::inv<t_mat>(M);
		std::cout << "\nok = " << std::boolalpha << ok3a << ", " << ok3b << std::endl;
		std::cout << "pseudoinv = " << inva << std::endl;
		std::cout << "      inv  = " << invb << std::endl;

		BOOST_TEST(ok);
		BOOST_TEST(ok2);
		BOOST_TEST(ok3a);
		BOOST_TEST(ok3b);

		auto ident = tl2::unit<t_mat>(M.size1(), M.size2());
		auto mata = inva*M;
		auto matb = invb*M;
		BOOST_TEST(tl2::equals(mata, ident, eps));
		BOOST_TEST(tl2::equals(matb, ident, eps));
	}

	// test rotation
	for(std::size_t iteration=0; iteration<1000; ++iteration)
	{
		t_vec axis = tl2::create<t_vec>({
			tl2::get_rand<t_real>(-10., 10.),
			tl2::get_rand<t_real>(-10., 10.),
			tl2::get_rand<t_real>(-10., 10.) });
		axis /= tl2::norm<t_vec>(axis);
		t_real angle = tl2::get_rand<t_real>(-tl2::pi<t_real>, tl2::pi<t_real>);
		t_mat rot = tl2::rotation<t_mat, t_vec>(axis, angle, 1);

		auto [ok, evals_re, evals_im, evecs_re, evecs_im] =
			tl2_la::eigenvec<t_mat, t_vec, t_real>(rot, 0, 0, 1);
		//std::cout << "\nok = " << std::boolalpha << ok << std::endl;

		bool axis_found = false;
		for(std::size_t i=0; i<evals_re.size(); ++i)
		{
			if(tl2::equals<t_real>(evals_re[i], 1., eps) && tl2::equals_0<t_real>(evals_im[i], eps))
			{
				bool evec_ok = (tl2::equals<t_vec>(axis, evecs_re[i], eps) ||
					tl2::equals<t_vec>(-axis, evecs_re[i], eps)) &&
					tl2::equals_0<t_vec>(evecs_im[i], eps);
				axis_found = true;
				BOOST_TEST((evec_ok));

				if(!evec_ok)
				{
					std::cerr << "rotation axis: " << axis << ", angle: " << angle << std::endl;
					for(std::size_t j=0; j<evals_re.size(); ++j)
					{
						std::cerr << "eval: " << evals_re[j] << " + i*" << evals_im[j]
							<< ", evec: " << evecs_re[j] << " +i*" << evecs_im[j]
							<< std::endl;
					}
				}
			}
		}

		BOOST_TEST((axis_found));
		if(!axis_found)
		{
			std::cerr << "Error: axis not found!" << std::endl;
			std::cerr << "rotation axis: " << axis << ", angle: " << angle << std::endl;
			for(std::size_t i=0; i<evals_re.size(); ++i)
			{
				std::cerr << "eval: " << evals_re[i] << " + i*" << evals_im[i]
					<< ", evec: " << evecs_re[i] << " +i*" << evecs_im[i]
					<< std::endl;
			}
		}
	}
#endif

	std::cout << "--------------------------------------------------------------------------------\n" << std::endl;
}


#include <cstdint>
#include <memory>
#include <vector>
#include <string>
#include <numeric>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <omp.h>
#include <valarray>

#include <typeinfo>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <posit/posit>
// Posit Arithmetic FPGA accelerator library
#include <positarith.h>

#include "main.hpp"
#include "defines.hpp"
#include "utils.hpp"
#include "blas.hpp"
#include "vector_utils.hpp"
#include "matrix_utils.hpp"

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

void normalizeVector(vector<posit<NBITS,ES>>& vec) {
    posit<NBITS,ES> sum;
    for(posit<NBITS,ES>& el : vec) {
        sum += el * el;
    }
    double sq = sqrt((double)sum);
    for(posit<NBITS,ES>& el : vec) {
        el = el / sq;
    }
}

template<typename T>
void normalizeVector(valarray<T>& vec) {
    T sum;
    for(T& el : vec) {
        sum += el * el;
    }
    T sq = sqrt((T)sum);
    for(T& el : vec) {
        el = el / sq;
    }
}

vector<posit<NBITS, ES> > sumProj(vector<vector<posit<NBITS,ES> > >& v, vector<vector<posit<NBITS,ES> > >& u, int i) {
        int k = 0;
        vector<posit<NBITS,ES> > result(u[0].size());

        while (k < i)
        {
                posit<NBITS,ES> dot_u_u;
                vector_dot(u[k], u[k], dot_u_u);

                posit<NBITS,ES> dot_v_u;
                vector_dot(v[i], u[k], dot_v_u);

                posit<NBITS,ES> factor = dot_v_u / dot_u_u;

                vector<posit<NBITS,ES> > factor_u;
                vector_mult(u[k], factor, factor_u);
                vector_add(result, factor_u, result);

                k++;
        }

        return result;
}

template<typename T>
valarray<T> sumProj(valarray<valarray<T>> v, valarray<valarray<T>> u, int i) {
    int k = 0;
    valarray<T> result(v[0].size());

    while (k < i)
    {
        T dot_u_u = (u[k] * u[k]).sum();
        T dot_v_u = (v[i] * u[k]).sum();
        T factor = dot_v_u / dot_u_u;
        valarray<T> factor_u = factor * u[k];
        result += factor * u[k];
        k++;
    }

    return result;
}

// valarray<posit<NBITS,ES>> sumProj(valarray<valarray<posit<NBITS,ES>>> v, valarray<valarray<posit<NBITS,ES>>> u, int i) {
//     int k = 0;
//     valarray<posit<NBITS,ES>> result(v[0].size());
//
//     while (k < i)
//     {
//         posit<NBITS,ES> dot_u_u = (u[k] * u[k]).sum();
//         posit<NBITS,ES> dot_v_u = (v[i] * u[k]).sum();
//         posit<NBITS,ES> factor = dot_v_u / dot_u_u;
//         valarray<posit<NBITS,ES>> factor_u = factor * u[k];
//         result += factor * u[k];
//
//         k++;
//     }
//
//     return result;
// }

int main(int argc, char ** argv)
{
    vector<int> lengths = {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000, 2000, 3000, 4000, 5000, 10000, 20000, 30000, 40000, 50000, 100000, 200000, 300000, 400000, 500000, 1000000};

    std::string t_dot, t_sum, t_add, t_add_scalar, t_subtract, t_subtract_scalar, t_mult, t_mult_scalar;

    for(int length : lengths) {
        t_dot = test_dot_product(length);
        t_sum = test_sum(length);
        t_add = test_add(length);
        t_add_scalar = test_add_scalar(length);
        t_subtract = test_subtract(length);
        t_subtract_scalar = test_subtract_scalar(length);
        t_mult = test_mult(length);
        t_mult_scalar = test_mult_scalar(length);

        ofstream outfile("positarith_es" + std::to_string(ES) + "_" + std::to_string(length) + ".txt", ios::out);
        outfile << t_dot << endl;
        outfile << t_sum << endl;
        outfile << t_add << endl;
        outfile << t_add_scalar << endl;
        outfile << t_subtract << endl;
        outfile << t_subtract_scalar << endl;
        outfile << t_mult << endl;
        outfile << t_mult_scalar << endl;
        outfile.close();
    }

    // vector<int> vectors_vec = {2, 5, 10, 50, 100};
    // vector<int> gram_length = {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000, 2000, 3000, 4000, 5000, 10000, 20000, 30000, 40000, 50000, 100000};
    //
    // std::string t_gram;
    //
    // for(int vectors : vectors_vec) {
    //     for(int length : gram_length) {
    //         t_gram = test_gram(3, length);
    //
    //         ofstream outfile("positarith_gram_es" + std::to_string(ES) + "_" + std::to_string(vectors) + "_" + std::to_string(length) + ".txt", ios::out);
    //         outfile << t_gram << endl;
    //         outfile.close();
    //     }
    // }

    return 0;
}

std::string test_dot_product(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Dot Product
    std::vector<posit<NBITS,ES>> vec1, vec2;
    posit<NBITS,ES> result;

    for(i = 0; i < length; i++) {
        vec1.push_back(1);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_dot(vec1, vec2, result);

    start = omp_get_wtime();
    posit<NBITS,ES> res = 0;
    // for(i = 0; i < length; i++) {
    //     res = res + vec1[i] * vec2[i];
    // }
    res = sw::hprblas::dot(length, vec1, 1, vec2, 1);
    stop = omp_get_wtime();
    t_sw = stop - start;

    float res_f = 0;
    std::vector<float> vec1_f, vec2_f;
    for(i = 0; i < length; i++) {
        vec1_f.push_back(1);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        #pragma omp for reduction(+:res_f)
        for(i = 0; i < length; i++) {
            res_f += vec1_f[i] * vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(1);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec = res_dec + vec1_dec[i] * vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_add(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Add
    std::vector<posit<NBITS,ES>> vec1, vec2;
    std::vector<posit<NBITS,ES>> result;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_add(vec1, vec2, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] + vec2[i];
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f, vec2_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] + vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] * vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_add_scalar(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Add Scalar
    std::vector<posit<NBITS,ES>> vec1;
    std::vector<posit<NBITS,ES>> result;
    posit<NBITS,ES> scalar;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    scalar = 5;
    t_fpga = 0;//vector_add(vec1, scalar, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] + scalar;
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] + 5.0;
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] + 5.0;
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_subtract(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Subtract
    std::vector<posit<NBITS,ES>> vec1, vec2;
    std::vector<posit<NBITS,ES>> result;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_sub(vec1, vec2, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] - vec2[i];
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f, vec2_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] - vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] - vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_subtract_scalar(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Subtract Scalar
    std::vector<posit<NBITS,ES>> vec1;
    std::vector<posit<NBITS,ES>> result;
    posit<NBITS,ES> scalar;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    scalar = 5;
    t_fpga = 0;//vector_sub(vec1, scalar, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] - scalar;
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] - 5.0;
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] - 5.0;
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_sum(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Sum
    std::vector<posit<NBITS,ES>> vec1;
    posit<NBITS,ES> result;

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    t_fpga = 0;//vector_sum(vec1, result);

    start = omp_get_wtime();
    // for(i = 0; i < length; i++) {
    //     result = result + vec1[i];
    // }
    result = sw::hprblas::asum(length, vec1, 1);
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f;
    float res_f = 0.0;
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f = res_f + vec1_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec;
    // cpp_dec_float_100 res_dec = 0.0;
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec = res_dec + vec1_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_mult(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Multiplication
    std::vector<posit<NBITS,ES>> vec1, vec2;
    std::vector<posit<NBITS,ES>> result;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_mult(vec1, vec2, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] * vec2[i];
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f, vec2_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] * vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] * vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_mult_scalar(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double start, stop;
    // Vector Multiplication Scalar
    std::vector<posit<NBITS,ES>> vec1;
    std::vector<posit<NBITS,ES>> result;
    posit<NBITS,ES> scalar;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    scalar = 5;
    t_fpga = 0;//vector_mult(vec1, scalar, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++){
        result[i] = vec1[i] * scalar;
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec_f;
    std::vector<float> result_f;
    result_f.resize(length);
    for(i = 0; i < length; i++){
        vec_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++){
            result_f[i] = vec_f[i] * 5.0;
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec_dec;
    // std::vector<cpp_dec_float_100> result_dec;
    // result_dec.resize(length);
    // for(i = 0; i < length; i++){
    //     vec_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++){
    //     result_dec[i] = vec_dec[i] * 5.0;
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

// template<typename T>
// cpp_dec_float_100 aggregateGram(vector<vector<T > >& u) {
//     cpp_dec_float_100 a = 0.0;
//     for(vector<T>& u_vec : u) {
//         a += (cpp_dec_float_100)u_vec.sum();
//     }
//     return a;
// }
//
// template<typename T>
// cpp_dec_float_100 aggregateGram(valarray<valarray<T > >& u) {
//     cpp_dec_float_100 a = 0.0;
//     for(valarray<T>& u_vec : u) {
//         a += (cpp_dec_float_100)u_vec.sum();
//     }
// }

// std::string test_gram(int n, int m) {
//     int i;
//     double stop, start;
//     double t_sw = 0.0, t_fpga = 0.0, t_float = 0.0;
//     cpp_dec_float_100 a_dec = 0.0, a_sw = 0.0, a_fpga = 0.0, a_float = 0.0;
//     cpp_dec_float_100 da_sw = 0.0, da_hw = 0.0, da_float = 0.0;
//
//     // Gram Matrix Test
//
//     // POSIT FPGA
//     vector<vector<posit<NBITS, ES> > > v(n);
//     vector<vector<posit<NBITS, ES> > > u(n); // Orthogonal set
//
//     for(i = 0; i < n; i++) {
//         u[i].resize(m);
//     }
//
//     // // Fill vectors
//     // for(i = 0; i < n; i++) {
//     //     vector<posit<NBITS, ES>> vec(m);
//     //     for(int j = 0; j < m; j++) {
//     //         vec[j] = sqrt(2)/2;
//     //     }
//     //     v[i] = vec;
//     // }
//     //
//     // // Start
//     // start = omp_get_wtime();
//     // u[0] = v[0];
//     // i = 0;
//     // do {
//     //         vector<posit<NBITS,ES> > sum = sumProj(v, u, i);
//     //         vector_sub(v[i], sum, u[i]);
//     //         i++;
//     // } while(i < n);
//     // stop = omp_get_wtime();
//     // t_fpga = stop - start;
//     //
//     // a_fpga = aggregateGram(u);
//
//     // POSIT SW
//     valarray<valarray<posit<NBITS,ES> > > v_posit(n);
//     valarray<valarray<posit<NBITS,ES> > > u_posit(n);
//     u_posit.resize(n);
//     // Fill vectors
//     for(i = 0; i < n; i++) {
//         valarray<posit<NBITS,ES>> vec(m);
//         for(int j = 0; j < m; j++) {
//             vec[j] = sqrt(2)/2;
//         }
//         v_posit[i] = vec;
//     }
//
//     for(i = 0; i < n; i++) {
//         u_posit[i].resize(m);
//     }
//
//     start = omp_get_wtime();
//     u_posit[0] = v_posit[0];
//     i = 0;
//     do {
//             valarray<posit<NBITS,ES>> sum = sumProj(v_posit, u_posit, i);
//             u_posit[i] = v_posit[i] - sum;
//             i++;
//     } while(i < n);
//     stop = omp_get_wtime();
//     t_sw = stop - start;
//
//     a_sw = aggregateGram(u_posit);
//
//     // FLOAT
//     valarray<valarray<float > > v_float(n);
//     valarray<valarray<float > > u_float(n);
//
//     // Fill vectors
//     for(i = 0; i < n; i++) {
//         valarray<float> vec(m);
//         for(int j = 0; j < m; j++) {
//             vec[j] = sqrt(2)/2;
//         }
//         v_float[i] = vec;
//     }
//
//     for(i = 0; i < n; i++) {
//         u_float[i].resize(m);
//     }
//
//     start = omp_get_wtime();
//     u_float[0] = v_float[0];
//     i = 0;
//     do {
//             valarray<float> sum = sumProj(v_float, u_float, i);
//             u_float[i] = v_float[i] - sum;
//             i++;
//     } while(i < n);
//     stop = omp_get_wtime();
//     t_float = stop - start;
//
//     a_float = aggregateGram(u_float);
//
//     // cpp_dec_float_100
//     valarray<valarray<cpp_dec_float_100 > > v_dec(n);
//     valarray<valarray<cpp_dec_float_100 > > u_dec(n);
//
//     // Fill vectors
//     for(i = 0; i < n; i++) {
//         valarray<cpp_dec_float_100> vec(m);
//         for(int j = 0; j < m; j++) {
//             vec[j] = sqrt(2)/2;
//         }
//         v_dec[i] = vec;
//     }
//
//     for(i = 0; i < n; i++) {
//         u_dec[i].resize(m);
//     }
//
//     u_dec[0] = v_dec[0];
//     i = 0;
//     do {
//             valarray<cpp_dec_float_100> sum = sumProj(v_dec, u_dec, i);
//             u_dec[i] = v_dec[i] - sum;
//             i++;
//     } while(i < n);
//
//     a_dec = aggregateGram(u_dec);
//
//     // Calculate decimal accuracy
//     da_hw = decimal_accuracy(a_dec, a_fpga);
//     da_sw = decimal_accuracy(a_dec, a_sw);
//     da_float = decimal_accuracy(a_dec, a_float);
//
//     return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float) + "," + to_string_precision(da_hw) + "," + to_string_precision(da_sw) + "," + to_string_precision(da_float);
// }


#include "nth_prime.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <stdexcept>

BOOST_AUTO_TEST_CASE(first)
{
    BOOST_REQUIRE_EQUAL(2, prime::nth(1));
}

BOOST_AUTO_TEST_CASE(second)
{
    BOOST_REQUIRE_EQUAL(3, prime::nth(2));
}

BOOST_AUTO_TEST_CASE(sixth)
{
    BOOST_REQUIRE_EQUAL(13, prime::nth(6));
}

BOOST_AUTO_TEST_CASE(big_prime)
{
    BOOST_REQUIRE_EQUAL(104743, prime::nth(10001));
}

BOOST_AUTO_TEST_CASE(weird_case)
{
    BOOST_REQUIRE_THROW(prime::nth(0), std::domain_error);
}

#if defined(EXERCISM_RUN_ALL_TESTS)
#endif


/**
 * @file
 * @author lkoppel
 */

#ifndef WAVE_GEOMETRY_UTIL_MATH_HPP
#define WAVE_GEOMETRY_UTIL_MATH_HPP

#include <Eigen/Core>
#include <random>

namespace wave {

/** Generate a random real number on the closed interval [a, b] */
template <typename Real>
Real uniformRandom(Real a, Real b) {
    // Use Mersenne Twister pseudo-random number generator, seeded by system random device
    static std::mt19937 rng{std::random_device{}()};

    // The distribution is normally open on one end [a, b). Do this to close it
    const auto closed_b = std::nextafter(b, std::numeric_limits<Real>::max());

    // Pick a real from the distribution
    return std::uniform_real_distribution<Real>{a, closed_b}(rng);
}

/** Generate a random unit quaternion on SO(3)
 *
 * Implements Algorithm 2 from Kuffner, James J. "Effective Sampling and Distance Metrics
 * for 3D Rigid Body Path Planning."
 * https://www.ri.cmu.edu/pub_files/pub4/kuffner_james_2004_1/kuffner_james_2004_1.pdf
 */
template <typename Real>
Eigen::Quaternion<Real> randomQuaternion() {
    const Real s = uniformRandom(Real{0}, Real{1});
    const Real s1 = std::sqrt(1 - s);
    const Real s2 = std::sqrt(s);
    const Real t1 = Real{2 * M_PI} * uniformRandom(Real{0}, Real{1});
    const Real t2 = Real{2 * M_PI} * uniformRandom(Real{0}, Real{1});
    return Eigen::Quaternion<Real>{
      std::cos(t2) * s2, std::sin(t1) * s1, std::cos(t1) * s1, std::sin(t2) * s2};
}

/** Go from a skew-symmetric (cross) matrix to a compact vector
 *
 * Also known as the "vee" operator.
 */
template <typename Derived>
auto uncrossMatrix(const Eigen::MatrixBase<Derived> &skew)
  -> Eigen::Matrix<typename Eigen::internal::traits<Derived>::Scalar, 3, 1> {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
    return Eigen::Matrix<typename Eigen::internal::traits<Derived>::Scalar, 3, 1>{
      skew(2, 1), skew(0, 2), skew(1, 0)};
}

}  // namespace wave

#endif  // WAVE_GEOMETRY_UTIL_MATH_HPP


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file testQPSolver.cpp
 * @brief Test simple QP solver for a linear inequality constraint
 * @date Apr 10, 2014
 * @author Duy-Nguyen Ta
 */

#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp>

#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolver.h>

using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;

static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);

/* ************************************************************************* */
/**
 * min -x1-x2
 * s.t.   x1 + 2x2 <= 4
 *       4x1 + 2x2 <= 12
 *       -x1 +  x2 <= 1
 *       x1, x2 >= 0
 */
LP simpleLP1() {
  LP lp;
  lp.cost = LinearCost(1, Vector2(-1., -1.));  // min -x1-x2 (max x1+x2)
  lp.inequalities.push_back(
      LinearInequality(1, Vector2(-1, 0), 0, 1));  // x1 >= 0
  lp.inequalities.push_back(
      LinearInequality(1, Vector2(0, -1), 0, 2));  //  x2 >= 0
  lp.inequalities.push_back(
      LinearInequality(1, Vector2(1, 2), 4, 3));  //  x1 + 2*x2 <= 4
  lp.inequalities.push_back(
      LinearInequality(1, Vector2(4, 2), 12, 4));  //  4x1 + 2x2 <= 12
  lp.inequalities.push_back(
      LinearInequality(1, Vector2(-1, 1), 1, 5));  //  -x1 + x2 <= 1
  return lp;
}

/* ************************************************************************* */
namespace gtsam {

TEST(LPInitSolver, infinite_loop_single_var) {
  LP initchecker;
  initchecker.cost = LinearCost(1, Vector3(0, 0, 1));  // min alpha
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(-2, -1, -1), -2, 1));  //-2x-y-alpha <= -2
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(-1, 2, -1), 6, 2));  // -x+2y-alpha <= 6
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(-1, 0, -1), 0, 3));  // -x - alpha <= 0
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(1, 0, -1), 20, 4));  // x - alpha <= 20
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(0, -1, -1), 0, 5));  // -y - alpha <= 0
  LPSolver solver(initchecker);
  VectorValues starter;
  starter.insert(1, Vector3(0, 0, 2));
  VectorValues results, duals;
  boost::tie(results, duals) = solver.optimize(starter);
  VectorValues expected;
  expected.insert(1, Vector3(13.5, 6.5, -6.5));
  CHECK(assert_equal(results, expected, 1e-7));
}

TEST(LPInitSolver, infinite_loop_multi_var) {
  LP initchecker;
  Key X = symbol('X', 1);
  Key Y = symbol('Y', 1);
  Key Z = symbol('Z', 1);
  initchecker.cost = LinearCost(Z, kOne);  // min alpha
  initchecker.inequalities.push_back(
      LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
                       1));  //-2x-y-alpha <= -2
  initchecker.inequalities.push_back(
      LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
                       2));  // -x+2y-alpha <= 6
  initchecker.inequalities.push_back(LinearInequality(
      X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3));  // -x - alpha <= 0
  initchecker.inequalities.push_back(LinearInequality(
      X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4));  // x - alpha <= 20
  initchecker.inequalities.push_back(LinearInequality(
      Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5));  // -y - alpha <= 0
  LPSolver solver(initchecker);
  VectorValues starter;
  starter.insert(X, kZero);
  starter.insert(Y, kZero);
  starter.insert(Z, Vector::Constant(1, 2.0));
  VectorValues results, duals;
  boost::tie(results, duals) = solver.optimize(starter);
  VectorValues expected;
  expected.insert(X, Vector::Constant(1, 13.5));
  expected.insert(Y, Vector::Constant(1, 6.5));
  expected.insert(Z, Vector::Constant(1, -6.5));
  CHECK(assert_equal(results, expected, 1e-7));
}

TEST(LPInitSolver, initialization) {
  LP lp = simpleLP1();
  LPInitSolver initSolver(lp);

  GaussianFactorGraph::shared_ptr initOfInitGraph =
      initSolver.buildInitOfInitGraph();
  VectorValues x0 = initOfInitGraph->optimize();
  VectorValues expected_x0;
  expected_x0.insert(1, Vector::Zero(2));
  CHECK(assert_equal(expected_x0, x0, 1e-10));

  double y0 = initSolver.compute_y0(x0);
  double expected_y0 = 0.0;
  DOUBLES_EQUAL(expected_y0, y0, 1e-7);

  Key yKey = 2;
  LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
  LP expectedInitLP;
  expectedInitLP.cost = LinearCost(yKey, kOne);
  expectedInitLP.inequalities.push_back(LinearInequality(
      1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1));  // -x1 - y <= 0
  expectedInitLP.inequalities.push_back(LinearInequality(
      1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2));  // -x2 - y <= 0
  expectedInitLP.inequalities.push_back(
      LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4,
                       3));  //  x1 + 2*x2 - y <= 4
  expectedInitLP.inequalities.push_back(
      LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12,
                       4));  //  4x1 + 2x2 - y <= 12
  expectedInitLP.inequalities.push_back(
      LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1,
                       5));  //  -x1 + x2 - y <= 1
  CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
  LPSolver lpSolveInit(*initLP);
  VectorValues xy0(x0);
  xy0.insert(yKey, Vector::Constant(1, y0));
  VectorValues xyInit = lpSolveInit.optimize(xy0).first;
  VectorValues expected_init;
  expected_init.insert(1, Vector::Ones(2));
  expected_init.insert(2, Vector::Constant(1, -1));
  CHECK(assert_equal(expected_init, xyInit, 1e-10));

  VectorValues x = initSolver.solve();
  CHECK(lp.isFeasible(x));
}
}

/* ************************************************************************* */
/**
 * TEST gtsam solver with an over-constrained system
 *  x + y = 1
 *  x - y = 5
 *  x + 2y = 6
 */
TEST(LPSolver, overConstrainedLinearSystem) {
  GaussianFactorGraph graph;
  Matrix A1 = Vector3(1, 1, 1);
  Matrix A2 = Vector3(1, -1, 2);
  Vector b = Vector3(1, 5, 6);
  JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
  graph.push_back(factor);

  VectorValues x = graph.optimize();
  // This check confirms that gtsam linear constraint solver can't handle
  // over-constrained system
  CHECK(factor.error(x) != 0.0);
}

TEST(LPSolver, overConstrainedLinearSystem2) {
  GaussianFactorGraph graph;
  graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, I_1x1, kOne,
                                 noiseModel::Constrained::All(1));
  graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, -I_1x1, 5 * kOne,
                                 noiseModel::Constrained::All(1));
  graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
                                 noiseModel::Constrained::All(1));
  VectorValues x = graph.optimize();
  // This check confirms that gtsam linear constraint solver can't handle
  // over-constrained system
  CHECK(graph.error(x) != 0.0);
}

/* ************************************************************************* */
TEST(LPSolver, simpleTest1) {
  LP lp = simpleLP1();
  LPSolver lpSolver(lp);
  VectorValues init;
  init.insert(1, Vector::Zero(2));

  VectorValues x1 =
      lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize();
  VectorValues expected_x1;
  expected_x1.insert(1, Vector::Ones(2));
  CHECK(assert_equal(expected_x1, x1, 1e-10));

  VectorValues result, duals;
  boost::tie(result, duals) = lpSolver.optimize(init);
  VectorValues expectedResult;
  expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
  CHECK(assert_equal(expectedResult, result, 1e-10));
}

/* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) {
  LP lp = simpleLP1();
  LPSolver lpSolver(lp);
  VectorValues result, duals, expectedResult;
  expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
  boost::tie(result, duals) = lpSolver.optimize();
  CHECK(assert_equal(expectedResult, result));
}

/**
 * TODO: More TEST cases:
 * - Infeasible
 * - Unbounded
 * - Underdetermined
 */
/* ************************************************************************* */
TEST(LPSolver, LinearCost) {
  LinearCost cost(1, Vector3(2., 4., 6.));
  VectorValues x;
  x.insert(1, Vector3(1., 3., 5.));
  double error = cost.error(x);
  double expectedError = 44.0;
  DOUBLES_EQUAL(expectedError, error, 1e-100);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#include <stan/math/rev/arr.hpp>
#include <gtest/gtest.h>

#include <iostream>
#include <sstream>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include <test/unit/math/rev/arr/functor/util.hpp>

#include <test/unit/math/prim/arr/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/arr/functor/lorenz.hpp>

template <typename F, typename T_y0, typename T_theta>
void sho_value_test(F harm_osc,
                    std::vector<double>& y0,
                    double t0,
                    std::vector<double>& ts,
                    std::vector<double>& theta,
                    std::vector<double>& x,
                    std::vector<int>& x_int) {

  using stan::math::var;
  using stan::math::promote_scalar;

  std::vector<std::vector<var> >  ode_res_vd
    = stan::math::integrate_ode_rk45(harm_osc, promote_scalar<T_y0>(y0), t0,
                                     ts, promote_scalar<T_theta>(theta), x, x_int,
                                     0);
  EXPECT_NEAR(0.995029, ode_res_vd[0][0].val(), 1e-5);
  EXPECT_NEAR(-0.0990884, ode_res_vd[0][1].val(), 1e-5);

  EXPECT_NEAR(-0.421907, ode_res_vd[99][0].val(), 1e-5);
  EXPECT_NEAR(0.246407, ode_res_vd[99][1].val(), 1e-5);

}

void sho_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;

  test_ode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8,1e-4);

  sho_value_test<harm_osc_ode_fun,double,var>(harm_osc, y0, t0, ts, theta, x, x_int);
  sho_value_test<harm_osc_ode_fun,var,double>(harm_osc, y0, t0, ts, theta, x, x_int);
  sho_value_test<harm_osc_ode_fun,var,var>(harm_osc, y0, t0, ts, theta, x, x_int);
}

void sho_data_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3,1);
  std::vector<int> x_int(2,0);

  test_ode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8,1e-4);

  sho_value_test<harm_osc_ode_data_fun,double,var>(harm_osc, y0, t0, ts, theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun,var,double>(harm_osc, y0, t0, ts, theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun,var,var>(harm_osc, y0, t0, ts, theta, x, x_int);

}

TEST(StanAgradRevOde_integrate_ode_rk45, harmonic_oscillator_finite_diff) {
  sho_finite_diff_test(0);
  sho_finite_diff_test(1.0);
  sho_finite_diff_test(-1.0);

  sho_data_finite_diff_test(0);
  sho_data_finite_diff_test(1.0);
  sho_data_finite_diff_test(-1.0);
}

TEST(StanAgradRevOde_integrate_ode_rk45, lorenz_finite_diff) {
  lorenz_ode_fun lorenz;

  std::vector<double> y0;
  std::vector<double> theta;
  double t0;
  std::vector<double> ts;

  t0 = 0;

  theta.push_back(10.0);
  theta.push_back(28.0);
  theta.push_back(8.0/3.0);
  y0.push_back(10.0);
  y0.push_back(1.0);
  y0.push_back(1.0);

  std::vector<double> x;
  std::vector<int> x_int;

  for (int i = 0; i < 100; i++)
    ts.push_back(0.1*(i+1));

  test_ode(lorenz, t0, ts, y0, theta, x, x_int, 1e-8, 1e-1);
}


//
//  EVD.hpp
//
//  Created by r. on 08/05/14
//

#ifndef round1_EVD_hpp
#define round1_EVD_hpp

// GSL
#include <gsl/gsl_eigen.h>

// Boost
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

//#include <boost/numeric/ublas/vector_sparse.hpp>
//#include <boost/numeric/ublas/vector_proxy.hpp>
//#include <boost/numeric/ublas/matrix_sparse.hpp>
//#include <boost/numeric/ublas/operation.hpp>
//#include <boost/numeric/ublas/operation_sparse.hpp>

namespace gsl
{
	struct EVD
	{
	public:
		typedef boost::numeric::ublas::matrix<double> matrix;
		typedef boost::numeric::ublas::vector<double> vector;
	public:
		// These should not be changed from outside
		unsigned int n;
		matrix V;
		vector d;
	public:
		EVD() : n(0), V(0,0), d(0) { }
	private:
		/*//
		 void testV(const matrix& AE, const matrix& ME) const
		 {
		 namespace ublas = boost::numeric::ublas;

		 vector u(n);
		 for (unsigned int i = 0; i != u.size(); ++i) u(i) = i;
		 double j = 1, k = 2;

		 cout << "A: " << AE << endl;
		 cout << "M: " << ME << endl;
		 cout << "V: " << V << endl;
		 cout << "d: " << d << endl;
		 matrix W = ublas::trans(V);
		 matrix AV = ublas::prod(AE, V), MV = ublas::prod(ME, V);
		 cout << "Vt A V: " << ublas::prod(W, AV) << endl;
		 cout << "Vt M V: " << ublas::prod(W, MV) << endl;

		 vector f1(n);
		 {
		 f1 = ublas::prod(j*AE + k*ME, u);
		 }
		 vector f2(n);
		 {
		 f2 = ublas::prod(ME, u);
		 f2 = ublas::prod(ublas::trans(V), f2);
		 for (unsigned int i = 0; i != f2.size(); ++i)
		 {
		 f2(i) *= (j * d(i) + k);
		 }
		 f2 = ublas::prod(V, f2);
		 f2 = ublas::prod(ME, f2);
		 }
		 cout << f1 << endl;
		 cout << f2 << endl;
		 cout << "testV residual: " << ublas::norm_inf(f1 - f2) << endl;
		 assert(ublas::norm_inf(f1 - f2) <= 1e-6);
		 }
		 //*/

	public:
		EVD(const matrix& AE, const matrix& ME)
		:
		n(0), V(0,0), d(0)
		// Pre: AE and ME are square matrices of equal size
		{
			// Thanks to:
			// http://sector7.xray.aps.anl.gov/~dohnarms/programming/gsl/gsl-ref.pdf
			// http://www.ryolab.com/soft/aper/dd/d99/_2utility_2algorithm_2eigen_2eigentest_2main_8cpp-example.html

			n = (unsigned int)(AE.size1());

			assert((AE.size1() == n) && (AE.size2() == n));
			assert((ME.size1() == n) && (ME.size2() == n));

			gsl_eigen_gensymmv_workspace * w = gsl_eigen_gensymmv_alloc(n);

			matrix A(n, n); A = AE;
			matrix M(n, n); M = ME;
			gsl_matrix_view a = gsl_matrix_view_array(&A.data()[0], n, n);
			gsl_matrix_view m = gsl_matrix_view_array(&M.data()[0], n, n);
			gsl_matrix * U = gsl_matrix_alloc(n , n);
			gsl_vector * c = gsl_vector_alloc(n);
			gsl_eigen_gensymmv(&a.matrix, &m.matrix, c, U, w);
			gsl_eigen_gensymmv_sort(c, U, GSL_EIGEN_SORT_ABS_ASC);

			V = matrix(n, n);
			d = vector(n);
			std::copy(U->data, U->data + (n*n), &(V.data()[0]));
			std::copy(c->data, c->data + n, &(d.data()[0]));

			gsl_vector_free(c);
			gsl_matrix_free(U);
			gsl_eigen_gensymmv_free(w);

			// M-normalize V
			for (unsigned int i = 0; i != n; ++i)
			{
				namespace ublas = boost::numeric::ublas;
				auto c = ublas::column(V, i);
				double norm = ublas::inner_prod(c, ublas::prod(ME, c));
				//cout << "norm: " << norm << endl;
				c *= (1/sqrt(norm));
			}

			//testV(AE, ME);
		}

		EVD& operator=(const EVD&) = default;
	};
} // namespace tools

#endif


#include <iostream>
#include <math.h>
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
using namespace boost::multiprecision;

void printString(cpp_int n,cpp_int m)
{
    /*
    long long int Mm = 0;
    long long int Res = (n*m);
    long long int new_m = m;
    */
    cpp_int Mm = 0;
    cpp_int Res = (n*m);
    cpp_int new_m = m;

    cpp_dec_float<100> temp = 0.0;

//cout<<"Res "<<Res<<endl;

    while(Res>=n)
    {
        temp = 1.0*new_m;
        Mm = floor(log10(temp )/log10(2));

        //Mm = floor(log10(1.0*new_m )/log10(2));

        Res -= (n<<Mm);
        new_m = Res/n;

        if(Res > 0) cout<<"("<<n<<"<<"<<Mm<<") + ";

        if(Res == 0) cout<<"("<<n<<"<<"<<Mm<<")\n";
    }
}

int main()
{
    long int T;
    //long long int N;
    //long long int M;
    cpp_int N;
    cpp_int M;

    cin >> T;

    for(long int tt=0;tt<T;tt++)
    {
        cin >> N >> M;
        printString(N,M);
    }

    //cout<<"("<<N<<"<<|log("<<M<<")|) "<<(N<<Mm) <<endl<<endl;

    //cout<<"("<<N<<"*"<<M<<") "<< N*M <<endl;

}


/**
 * crystal matrix text
 * @author Tobias Weber <tweber@ill.fr>
 * @date feb-19
 * @license GPLv3, see 'LICENSE' file
 *
 * g++ -std=c++20 -o cryst cryst.cpp
 * g++ -std=c++20 -DUSE_LAPACK -I.. -I/usr/include/lapacke -I/usr/local/opt/lapack/include -L/usr/local/opt/lapack/lib -o cryst cryst.cpp -llapacke
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Xtal Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"
using namespace tl2_ops;

#ifdef USE_LAPACK
	using t_types = std::tuple<double, float>;
#else
	using t_types = std::tuple<long double, double, float>;
#endif

BOOST_AUTO_TEST_CASE_TEMPLATE(test_xtal, t_real, t_types)
{
	std::cout << "Test 1" << std::endl;

	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;
	//using t_cplx = std::complex<t_real>;
	//using t_vec_cplx = std::vector<t_cplx>;
	//using t_mat_cplx = tl2::mat<t_cplx, std::vector>;

	auto A = tl2::A_matrix<t_mat, t_real>(
		4.56, 4.56, 4.56,
		90. / 180. * tl2::pi<t_real>,
		90. / 180. * tl2::pi<t_real>,
		90. / 180. * tl2::pi<t_real>);
	auto B = tl2::B_matrix<t_mat, t_real>(
		4.56, 4.56, 4.56,
		90. / 180. * tl2::pi<t_real>,
		90. / 180. * tl2::pi<t_real>,
		90. / 180. * tl2::pi<t_real>);
	auto [B2, ok] = tl2::inv<t_mat>(A);
	B2 = 2.*tl2::pi<t_real> * tl2::trans<t_mat>(B2);
	auto G = tl2::metric<t_mat>(B);

	t_vec vec1 = tl2::create<t_vec>({1, 1, 0});
	t_vec vec2 = tl2::create<t_vec>({1, -1, 0});
	t_vec vec3 = tl2::cross<t_mat, t_vec>(B, vec1, vec2);
	t_mat UB = tl2::UB_matrix<t_mat, t_vec>(B, vec1, vec2, vec3);

	std::cout << "A  = " << A << std::endl;
	std::cout << "B  = " << B << std::endl;
	std::cout << "B2 = " << B2 << std::endl;
	std::cout << "G = " << G << std::endl;
	std::cout << "UB = " << UB << std::endl;
	//std::cout << tl2::levi<t_mat>(B, {0,1,2}) << std::endl;
	//std::cout << tl2::levi<t_mat>(B, {0,2,1}) << std::endl;
	//std::cout << tl2::levi<t_mat>(B, {1,2,1}) << std::endl;

	t_real ki = 1.5;
	t_real kf = 1.4;
	t_vec Q = tl2::create<t_vec>({1, -1, 0});
	auto [anglesok, a3, a4, dist] = tl2::calc_tas_a3a4<t_mat, t_vec>(B, ki, kf, Q, vec1, vec3);
	std::cout << "a3 = " << a3 / tl2::pi<t_real> * 180.
		<< ", a4 = " << a4 / tl2::pi<t_real> * 180.
		<< std::endl;
	std::cout << "distance of Q to scattering plane: " << dist << std::endl;

	// calculate back to Q
	t_real Qlen = tl2::calc_tas_Q_len<t_real>(ki, kf, a4);
	std::optional<t_vec> Qhkl = tl2::calc_tas_hkl<t_mat, t_vec>(B, ki, kf, Qlen, a3, vec1, vec3);
	std::cout << "Q = " << *Qhkl << std::endl;

	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals(B, B2, std::numeric_limits<t_real>::epsilon()*1e2));
	BOOST_TEST(tl2::equals<t_real>(dist, 0, std::numeric_limits<t_real>::epsilon()*1e2));
	BOOST_TEST(tl2::equals<t_real>(a3/tl2::pi<t_real>*180., 44.359, 1e-3));
	BOOST_TEST(tl2::equals<t_real>(a4/tl2::pi<t_real>*180., 84.359, 1e-3));
	BOOST_TEST((Qhkl.operator bool()));
	BOOST_TEST(tl2::equals<t_vec>(Q, *Qhkl, 1e-3));

	std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_xtal2, t_real, t_types)
{
	std::cout << "Test 2" << std::endl;

	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;

	auto B = tl2::B_matrix<t_mat, t_real>(
		5., 6., 7.,
		60. / 180. * tl2::pi<t_real>,
		60. / 180. * tl2::pi<t_real>,
		60. / 180. * tl2::pi<t_real>);

	t_vec vec1 = tl2::create<t_vec>({1, 0, 0});
	t_vec vec2 = tl2::create<t_vec>({0, 1, 0});
	t_vec vec3 = tl2::cross<t_mat, t_vec>(B, vec1, vec2);
	t_mat UB = tl2::UB_matrix<t_mat, t_vec>(B, vec1, vec2, vec3);
	std::cout << "vec1 = " << vec1 << std::endl;
	std::cout << "vec2 = " << vec2 << std::endl;
	std::cout << "vec3 = " << vec3 << std::endl;
	std::cout << "UB = " << UB << std::endl;

	t_real ki = 1.4;
	t_real kf = 1.5;
	t_vec Q = tl2::create<t_vec>({1, 1, 0});
	t_real a3_offs = tl2::pi<t_real>*0.5;
	auto [anglesok, a3, a4, dist] = tl2::calc_tas_a3a4<t_mat, t_vec, t_real>(
		B, ki, kf, Q, vec1, vec3, 1., a3_offs);
	std::cout << "a3 = " << a3 / tl2::pi<t_real> * 180.
		<< ", a4 = " << a4 / tl2::pi<t_real> * 180.
		<< std::endl;
	std::cout << "distance of Q to scattering plane: " << dist << std::endl;

	// calculate back to Q
	t_real Qlen = tl2::calc_tas_Q_len<t_real>(ki, kf, a4);
	std::optional<t_vec> Qhkl = tl2::calc_tas_hkl<t_mat, t_vec, t_real>(
		B, ki, kf, Qlen, a3, vec1, vec3, 1., a3_offs);
	std::cout << "Q = " << *Qhkl << std::endl;

	BOOST_TEST(tl2::equals<t_real>(dist, 0, std::numeric_limits<t_real>::epsilon()*1e2));
	BOOST_TEST(tl2::equals<t_real>(a3/tl2::pi<t_real>*180., -15.84, 1e-3));
	BOOST_TEST(tl2::equals<t_real>(a4/tl2::pi<t_real>*180., 68.895, 1e-3));
	BOOST_TEST((Qhkl.operator bool()));
	BOOST_TEST(tl2::equals<t_vec>(Q, *Qhkl, 1e-3));

	std::cout << std::endl;
}


// Copyright 2020 Josh Pieper, jjp@pobox.com.
//
// 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.

#include "base/fit_plane.h"

#include <Eigen/Dense>

namespace mjmech {
namespace base {

Plane FitPlane(const std::vector<Eigen::Vector3d>& points) {
  Eigen::MatrixXd A(points.size(), 3);
  Eigen::MatrixXd B(points.size(), 1);

  for (size_t i = 0; i < points.size(); i++) {
    A(i, 0) = points[i].x();
    A(i, 1) = points[i].y();
    A(i, 2) = 1.0;
    B(i) = points[i].z();
  }

  Eigen::MatrixXd result = A.bdcSvd(
      Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
  return Plane{result(0), result(1), result(2)};
}

}
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"

namespace math = boost::math::constants;

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

struct err
{
  double infty=0.;
  double one=0.;
  double dt=0. , dx=0. , dv=0.;
  std::size_t n=16;
  std::size_t nb_iter=0;
};

std::ostream &
operator << ( std::ostream & os , err const& e ) {
  os << e.n << " " << " " << e.nb_iter << " " << e.dx << " " << e.dt << " " << e.one << " " << e.infty;
  return os;
}

err
rotation ( std::size_t N , int q=-2 )
{
	std::size_t Nx = N, Nv = N;
	field<double,1> f(boost::extents[Nv][Nx]);
  field<double,1> f_sol(boost::extents[Nv][Nx]);

	f.range.v_min = -10.; f.range.v_max = 10;
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;
	f.range.x_min = -10.; f.range.x_max = 10.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;

	const double dt = (math::pi<double>()*std::pow(2,q))*f.step.dv/f.range.v_max;

	ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] =  Vk(k); }
  ublas::vector<double> E (Nx,0.); for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = -Xi(i); }

	const double l = f.range.x_max-f.range.x_min;
	ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i] = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] = std::exp(-SQ(Xi(i)-3)/0.5 - SQ(Vk(k))/2.);
      f_sol[k][i] = f[k][i];
    }
  }

  double Tf = 2*math::pi<double>();
  int i_t=0;

  while ( i_t*dt < Tf ) {
    field<double,1> Edvf = weno::trp_v(f,E);
    field<double,1> f1=f,f2=f;
	  fft::spectrum hf(Nx),hf1(Nx),hf2(Nx),hEdvf(Nx);

	  for ( auto k=0 ; k<f.size(0) ; ++k ) {
	  	hf.fft(&(f[k][0]));
	  	hEdvf.fft(&(Edvf[k][0]));

	  	for ( auto i=0 ; i<Nx ; ++i ) {
	  		auto re = hf[i][fft::re] , im = hf[i][fft::im];
	  		hf1[i][fft::re] = std::cos(v(k)*kx[i]*dt)*(re-dt*hEdvf[i][fft::re]) + std::sin(v(k)*kx[i]*dt)*(im-dt*hEdvf[i][fft::im]);
	  		hf1[i][fft::im] = std::cos(v(k)*kx[i]*dt)*(im-dt*hEdvf[i][fft::im]) - std::sin(v(k)*kx[i]*dt)*(re-dt*hEdvf[i][fft::re]);
	  	}
	  	hf1.ifft(&(f1[k][0]));
	  }

    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        auto re = hf[i][fft::re] , im = hf[i][fft::im];
        hf2[i][fft::re] = 0.75*(std::cos(0.5*v(k)*kx[i]*dt)*re + std::sin(0.5*v(k)*kx[i]*dt)*im) + 0.25*( std::cos(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::re]-dt*hEdvf[i][fft::re]) - std::sin(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::im]-dt*hEdvf[i][fft::im]) );
        hf2[i][fft::im] = 0.75*(std::cos(0.5*v(k)*kx[i]*dt)*im - std::sin(0.5*v(k)*kx[i]*dt)*re) + 0.25*( std::cos(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::im]-dt*hEdvf[i][fft::im]) + std::sin(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::re]-dt*hEdvf[i][fft::re]) );
      }
      hf2.ifft(&(f2[k][0]));
    }

    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        auto re = hf[i][fft::re] , im = hf[i][fft::im];
        hf[i][fft::re] = (1./3.)*(std::cos(v(k)*kx[i]*dt)*re + std::sin(v(k)*kx[i]*dt)*im) + (2./3.)*( std::cos(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::re]-dt*hEdvf[i][fft::re]) + std::sin(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::im]-dt*hEdvf[i][fft::im]) );
        hf[i][fft::im] = (1./3.)*(std::cos(v(k)*kx[i]*dt)*im - std::sin(v(k)*kx[i]*dt)*re) + (2./3.)*( std::cos(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::im]-dt*hEdvf[i][fft::im]) - std::sin(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::re]-dt*hEdvf[i][fft::re]) );
      }
      hf.ifft(&(f[k][0]));
    }

    ++i_t;
	}

  err e;
  e.dt = dt;
  e.dx = f.step.dx;
  e.dv = f.step.dv;
  e.n = N;
  e.nb_iter = i_t;
  for ( auto k=0 ; k<Nv ; ++k ) {
    for (auto i=0 ; i<Nx ; ++i ) {
      e.one += std::abs(f[k][i]-f_sol[k][i])*f.step.dv*f.step.dx;
      e.infty = std::max(std::abs(f[k][i]-f_sol[k][i]),e.infty);
    }
  }

	return e;
}

int
main ( int argc , char** argv )
{
  for ( std::size_t exp=5 ; exp<11 ; ++exp ) {
    std::size_t n=1<<exp;
    err e = rotation(n);
    std::cout << e << std::endl;
  }

  return 0;
}