///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

#ifndef BOOST_MATH_FFT_REAL_ALGORITHMS_HPP
  #define BOOST_MATH_FFT_REAL_ALGORITHMS_HPP

  #include <algorithm>
  #include <numeric>
  #include <cmath>

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

  namespace boost { namespace math {  namespace fft {

  namespace detail {

  template<class T>
  inline void real_dft_2(
    const T* in,
    T* out, int)
  {
    T o1 = in[0]+in[1], o2 = in[0]-in[1] ;
    out[0] = o1;
    out[1] = o2;
  }

  template<class RealType>
  RealType complex_root_of_unity_real(long n,long p=1)
  /*
    Computes cos(-2 pi p/n)
  */
  {
    p = modulo(p,n);

    if(p==0)
      return RealType(1);

    long g = gcd(p,n);
    n/=g;
    p/=g;
    switch(n)
    {
      case 1:
        return RealType(1);
      case 2:
        return p==0 ? RealType(1) : RealType(-1);
      case 4:
        return p==0 ? RealType(1) :
               p==1 ? RealType(0) :
               p==2 ? RealType(-1) :
                      RealType(0) ;
    }
    using std::cos;
    RealType phase = -2*p*boost::math::constants::pi<RealType>()/n;
    return RealType(cos(phase));
  }
  template<class RealType>
  RealType complex_root_of_unity_imag(long n,long p=1)
  /*
    Computes sin(-2 pi p/n)
  */
  {
    p = modulo(p,n);

    if(p==0)
      return RealType(0);

    long g = gcd(p,n);
    n/=g;
    p/=g;
    switch(n)
    {
      case 1:
        return RealType(0);
      case 2:
        return p==0 ? RealType(0) : RealType(0);
      case 4:
        return p==0 ? RealType(0) :
               p==1 ? RealType(-1) :
               p==2 ? RealType(0) :
                      RealType(1) ;
    }
    using std::sin;
    RealType phase = -2*p*boost::math::constants::pi<RealType>()/n;
    return RealType(sin(phase));
  }

  template <class T>
  void real_dft_power2(const T *in_first, const T *in_last, T* out, int/*sign*/)
  {
    /*
      Cooley-Tukey mapping, in-place Decimation in Time
    */
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (n == 1)
        return;

    // Gold-Rader bit-reversal algorithm.
    for(int i=0,j=0;i<n-1;++i)
    {
      if(i<j)
        std::swap(out[i],out[j]);
      for(int k=n>>1;!( (j^=k)&k );k>>=1);
    }

    for (int len = 2, prev_len = 1; len <= n; len <<= 1,prev_len<<=1)
    {
      for (int i = 0; i < n; i += len)
      {
        {
          // j=0;
          T* u = out + i, *v = out + i + prev_len;
          T Bu = *u, Bv = *v;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }
        for(int j=1;j < prev_len/2;++j)
        {
          T cos{ complex_root_of_unity_real<T>(len,j) },
            sin{ complex_root_of_unity_imag<T>(len,j) };

          T *ux = out + i + j,
            *uy = out + i + len - j;

          T *vx = out + i + prev_len - j,
            *vy = out + i + prev_len + j;

          T prev_ux = *ux,
            prev_uy = *uy;

          T prev_vx = *vx,
            prev_vy = *vy;

          *ux = prev_ux + cos * prev_vy + sin * prev_uy;
          *uy = prev_vx + cos * prev_uy - sin * prev_vy;
          *vx = prev_ux - cos * prev_vy - sin * prev_uy;
          *vy =-prev_vx + cos * prev_uy - sin * prev_vy;
        }
        //if(prev_len>=2)
        //{
        //  const int j = prev_len/2;
        //  T* u = out + i + j, *v = out + i + j + prev_len;
        //  T Bu = *u, Bv = *v;
        //  *u = Bu;
        //  *v = Bv;
        //}
      }
    }
  }
  template <class T>
  void real_inverse_dft_power2(const T *in_first, const T *in_last, T* out, int /*sign*/)
  {
    /*
      Cooley-Tukey mapping, in-place Decimation in Time
      Reverse flow graph of the real_dft_power2
    */
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (n == 1)
        return;

    for (int len = n, prev_len = len/2; len >= 2; len >>= 1,prev_len>>=1)
    {
      for (int i = 0; i < n; i += len)
      {
        {
          // j=0;
          T* u = out + i, *v = out + i + prev_len;
          T Bu = *u, Bv = *v;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }
        for(int j=1;j < prev_len/2;++j)
        {
          T cos{ complex_root_of_unity_real<T>(len,-j) },
            sin{ complex_root_of_unity_imag<T>(len,-j) };

          T *ux = out + i + j,
            *uy = out + i + len - j;

          T *vx = out + i + prev_len - j,
            *vy = out + i + prev_len + j;

          T prev_ux = *ux,
            prev_uy = *uy;

          T prev_vx = *vx,
            prev_vy = *vy;

          T sum_x = prev_ux + prev_vx,
            dif_x = prev_ux - prev_vx,
            sum_y = prev_uy + prev_vy,
            dif_y = prev_uy - prev_vy;

          *ux = sum_x;
          *vx = dif_y;
          *uy = cos * sum_y - sin*dif_x;
          *vy = cos * dif_x + sin*sum_y;
        }
        if(prev_len>=2)
        {
          const int j = prev_len/2;
          T* u = out + i + j, *v = out + i + j + prev_len;
          T Bu = *u, Bv = *v;
          *u = Bu + Bu;
          *v = Bv + Bv;
        }
      }
    }

    // Gold-Rader bit-reversal algorithm.
    for(int i=0,j=0;i<n-1;++i)
    {
      if(i<j)
        std::swap(out[i],out[j]);
      for(int k=n>>1;!( (j^=k)&k );k>>=1);
    }
  }

  template<class T>
  void real_dft_prime_bruteForce_outofplace(
    const T* in_first,
    const T* in_last,
    T* out, int sign)
  /*
    assumptions:
    - allocated memory in out is enough to hold distance(in_first,in_last) element,
    - out!=in
  */
  {
    const long N = static_cast<long>(std::distance(in_first,in_last));
    if(N<=0)
      return;

    out[0] = std::accumulate(in_first+1,in_last,in_first[0]);

    for(long i=1,j=N-1;i<j;++i,--j)
    {
      T sum_x{in_first[0]},sum_y = 0;
      for(long l=1;l<N; ++l)
      {
        sum_x += in_first[l] * complex_root_of_unity_real<T>(N,i*l*sign);
        sum_y += in_first[l] * complex_root_of_unity_imag<T>(N,i*l*sign);
      }
      // if(i<j) // i==j never happens for odd sizes
      out[j] = -sum_y;
      out[i] = sum_x;
    }
  }

  template<class T,class Allocator_t>
  void real_dft_prime_bruteForce_inplace(
    T* in_first,
    T* in_last,
    int sign,
    const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_dft_prime_bruteForce_outofplace(in_first,in_last,work_space.data(),sign);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_dft_prime_bruteForce(
    const T* in_first,
    const T* in_last,
    T* out,
    int sign,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      real_dft_prime_bruteForce_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_dft_prime_bruteForce_outofplace(in_first,in_last,out,sign);
  }

  template<class T>
  void real_inverse_dft_prime_bruteForce_outofplace(
    const T* in_first,
    const T* in_last,
    T* out, int sign)
  /*
    assumptions:
    - allocated memory in out is enough to hold distance(in_first,in_last) element,
    - out!=in
  */
  {
    const long N = static_cast<long>(std::distance(in_first,in_last));
    if(N<=0)
      return;

    {
      T sum_x{0.};
      for(long i=1,j=N-1;i<j;++i,--j)
      {
        sum_x += in_first[i];
      }
      // if(i<j) // i==j never happens for odd sizes
      out[0] = ( 2*sum_x + in_first[0]);
    }
    for(long l=1;l<N; ++l)
    {
      T sum_x{0.},sum_y{0.};
      for(long i=1,j=N-1;i<j;++i,--j)
      {
        sum_x += in_first[i] * complex_root_of_unity_real<T>(N,i*l*sign);
        sum_y += in_first[j] * complex_root_of_unity_imag<T>(N,i*l*sign);
      }
      // if(i<j) // i==j never happens for odd sizes
      out[l] = (2*sum_x - 2*sum_y + in_first[0]);
    }
  }

  template<class T,class Allocator_t>
  void real_inverse_dft_prime_bruteForce_inplace(
    T* in_first,
    T* in_last,
    int sign,
    const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_inverse_dft_prime_bruteForce_outofplace(in_first,in_last,work_space.data(),sign);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_inverse_dft_prime_bruteForce(
    const T* in_first,
    const T* in_last,
    T* out,
    int sign,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      real_inverse_dft_prime_bruteForce_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_inverse_dft_prime_bruteForce_outofplace(in_first,in_last,out,sign);
  }

  template <class T, class allocator_t>
  void real_dft_composite_outofplace(
            const T *in_first,
            const T *in_last,
            T* out,
            int /*sign*/,
            const allocator_t& alloc)
  {
    /*
      Cooley-Tukey mapping, intrinsically out-of-place, Decimation in Time
      composite sizes.
    */
    using allocator_type = allocator_t;
    using ComplexType = simple_complex<T>;
    using ComplexAllocator = typename std::allocator_traits<allocator_type>::template rebind_alloc<ComplexType>;

    const long n = static_cast<long>(std::distance(in_first,in_last));
    if(n <=0 )
      return;

    if (n == 1)
    {
        out[0]=in_first[0];
        return;
    }
    std::array<int,32> prime_factors;
    const int nfactors = prime_factorization(n,prime_factors.begin());

    // reorder input
    for (long i = 0; i < n; ++i)
    {
        long j = 0, k = i;
        for (int ip=0;ip<nfactors;++ip)
        {
            int p = prime_factors[ip];
            j = j * p + k % p;
            k /= p;
        }
        out[j] = in_first[i];
    }

    std::reverse(prime_factors.begin(), prime_factors.begin()+nfactors);

    //auto show = [&] (const T* beg, const T* end)
    //{
    //  for(;beg!=end;++beg)
    //  {
    //    std::cout << *beg << ", ";
    //  }
    //  std::cout << "\n";
    //};

    // butterfly pattern
    long len = 1;
    for (int ip=0;ip<nfactors;++ip)
    {
      int p = prime_factors[ip];
      long len_old = len;
      len *= p;

      std::vector<ComplexType,ComplexAllocator> tmp(p,ComplexType(),alloc);
      //std::cout << "pass " << ip << "\n";
      for (long i = 0; i < n; i += len)
      {
        //std::cout << "    i = " << i << "\n";
        for(long k=0;2*k<=len_old;++k)
        {
          if(k==0)
          {
            tmp[0] = ComplexType{out[i],0.};
            for(long j=1;j<p;++j)
              tmp[j] = ComplexType{out[i + j*len_old],0.};
          }else if(2*k == len_old)
          {
            tmp[0] = ComplexType{out[i + k ],0.};
            for(long j=1;j<p;++j)
              tmp[j] = ComplexType{out[i + j*len_old +k ],0.}
                * complex_root_of_unity<ComplexType>(len,k*j);
          }else
          {
            tmp[0] = ComplexType{out[i + k ],-out[i+len_old-k]};
            for(long j=1;j<p;++j)
              tmp[j] = ComplexType{out[i + j*len_old +k ],-out[i+j*len_old + len_old-k]}
                * complex_root_of_unity<ComplexType>(len,k*j);
          }
          if(p==2)
          {
            complex_dft_2(tmp.data(),tmp.data(),1);
          }
          else
          {
            complex_dft_prime_rader<ComplexType,ComplexAllocator>(tmp.data(),tmp.data()+p,tmp.data(),1,alloc);
          }
          for(long j=0;j<p;++j)
          {
            int posx = j*len_old + k, posy = len - k - j*len_old;

            if(posx==0)
            {
              out[i+posx] = tmp[j].real();
            }else if(posx>posy)
            {
              out[i+posx] = tmp[j].imag();
              out[i+posy] = tmp[j].real();
            }else
            {
              out[i+posy] = -tmp[j].imag();
              out[i+posx] = tmp[j].real();
            }
          }
        }
        //show(out+i,out+i+len);
      }
    }
  }

  template<class T,class Allocator_t>
  void real_dft_composite_inplace(
          T* in_first,
          T* in_last,
          int sign,
          const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_dft_composite_outofplace(in_first,in_last,work_space.data(),sign,alloc);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_dft_composite(
          const T* in_first,
          const T* in_last,
          T* out,
          int sign,
          const Allocator_t& alloc)
  {
    if(in_first==out)
      real_dft_composite_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_dft_composite_outofplace(in_first,in_last,out,sign,alloc);
  }

  template <class T, class allocator_t>
  void real_inverse_dft_composite_outofplace(
            const T *in_first,
            const T *in_last,
            T* out,
            int /*sign*/,
            const allocator_t& alloc)
  {
    /*
      Cooley-Tukey mapping, intrinsically out-of-place, Decimation in Time
      composite sizes.
      Reverse graph.
    */
    using allocator_type = allocator_t;
    using ComplexType = simple_complex<T>;
    using ComplexAllocator = typename std::allocator_traits<allocator_type>::template rebind_alloc<ComplexType>;

    const long n = static_cast<long>(std::distance(in_first,in_last));
    if(n <=0 )
      return;

    std::copy(in_first,in_last,out);
    if (n == 1)
        return;

    std::array<int,32> prime_factors;
    const int nfactors = prime_factorization(n,prime_factors.begin());

    // butterfly pattern
    for (long ip=0,len=n,prev_len = len;ip<nfactors;++ip,len = prev_len)
    {
      int p = prime_factors[ip];
      prev_len = len/p;

      std::vector<ComplexType,ComplexAllocator> tmp(p,ComplexType(),alloc);
      for (long i = 0; i < n; i += len)
      {
        for(long k=0;2*k<=prev_len;++k)
        {
          for(long j=0;j<p;++j)
          {
            int posx = j*prev_len + k, posy = len - k - j*prev_len;

            if(posx==0 || posx==posy)
            {
              tmp[j] = ComplexType {out[i+posx],0.};
            }else if(posx>posy)
            {
              tmp[j] = ComplexType{out[i+posy],out[i+posx]};
            }else // if(posx<posy)
            {
              tmp[j] = ComplexType{out[i+posx],-out[i+posy]};
            }
          }
          if(p==2)
          {
            complex_dft_2(tmp.data(),tmp.data(),-1);
          }
          else
          {
            complex_dft_prime_rader<ComplexType,ComplexAllocator>(tmp.data(),tmp.data()+p,tmp.data(),-1,alloc);
          }

          if(k==0)
          {
            out[i] = tmp[0].real();
            for(long j=1;j<p;++j)
              out[i + j*prev_len] = tmp[j].real();
          }else if(2*k == prev_len)
          {
            out[i + k ] = tmp[0].real();
            for(long j=1;j<p;++j)
              out[i + j*prev_len +k ] = (tmp[j]* complex_root_of_unity<ComplexType>(len,-k*j) ). real();
          }else
          {
            out[i+k]          =  tmp[0].real();
            out[i+prev_len-k] = -tmp[0].imag();
            for(long j=1;j<p;++j)
            {
              ComplexType cplx = tmp[j] * complex_root_of_unity<ComplexType>(len,-k*j);
              out[i + j*prev_len +k ]        = cplx.real();
              out[i+j*prev_len + prev_len-k] = -cplx.imag();
            }
          }
        }
      }
    }

    std::vector<T,allocator_type> tmp(out,out+n);
    // reorder
    for (long i = 0; i < n; ++i)
    {
        long j = 0, k = i;
        for (int ip=0;ip<nfactors;++ip)
        {
            int p = prime_factors[ip];
            j = j * p + k % p;
            k /= p;
        }
        out[i] = tmp[j];
    }
  }

  template<class T,class Allocator_t>
  void real_inverse_dft_composite_inplace(
          T* in_first,
          T* in_last,
          int sign,
          const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_inverse_dft_composite_outofplace(in_first,in_last,work_space.data(),sign,alloc);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_inverse_dft_composite(
          const T* in_first,
          const T* in_last,
          T* out,
          int sign,
          const Allocator_t& alloc)
  {
    if(in_first==out)
      real_inverse_dft_composite_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_inverse_dft_composite_outofplace(in_first,in_last,out,sign,alloc);
  }

  } // namespace detail

  } } } // namespace boost::math::fft

#endif // BOOST_MATH_FFT_REAL_ALGORITHMS_HPP


#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
using Eigen::MatrixXd;

using namespace std;
using std::setw;
using std::setprecision;

int main(int argc, char** argv)
{
	MatrixXd J_pseudoInv(2,2);     //  NULL space projection
	MatrixXd A = MatrixXd::Random(2,2);

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU |Eigen::ComputeThinV);
	J_pseudoInv = svd.matrixV()*svd.singularValues().inverse()*svd.matrixU().transpose();

	cout << "Hello World! "<<J_pseudoInv<<endl;
	cout << "J_pseudoInv*A "<<J_pseudoInv*A<<endl;

	return 0;
}


#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, const char **argv)
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));

    std::cout .precision(3);
    std::cout << "rotation matrix =\n" << rotation_vector.matrix() << std::endl;

    rotation_matrix = rotation_vector.toRotationMatrix();

    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl;

    v_rotated = rotation_matrix * v;
    std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl;

    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
    std::cout << "yaw pitch roll = " << euler_angles.transpose() << std::endl;

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    std::cout << "Transform matrix = \n" << T.matrix() << std::endl;
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    std::cout << "Transform matrix = \n" << T.matrix() << std::endl;

    Eigen::Vector3d v_transformed = T * v;
    std::cout << "v transformed = " << v_transformed.transpose() << std::endl;

    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    std::cout << "quaternion = \n" << q.coeffs() << std::endl;

    q = Eigen::Quaterniond(rotation_matrix);
    std::cout << "quaternion = \n" << q.coeffs() << std::endl;

    v_rotated = q * v;
    std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl;

    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

using std::cout;
using std::cin;
using std::endl;

using Eigen::MatrixXd;
using Eigen::MatrixXf;
using Eigen::Matrix;
using Eigen::VectorXd;
using Eigen::Matrix3d;
using Eigen::Matrix3f;
using Eigen::Vector3d;

#define LOGD(fmt, ...) fprintf(stdout, fmt, ##__VA_ARGS__)

float dotproduct_eigen(size_t len, float* va, float* vb)
{
    Eigen::Map<Eigen::Matrix<float, 1, Eigen::Dynamic, Eigen::RowMajor>> vva(va, len);
    Eigen::Map<Eigen::Matrix<float, 1, Eigen::Dynamic, Eigen::RowMajor>> vvb(vb, len);
    float res = vva.dot(vvb);
    return res;
}

static void matrix_add_f32_eigen(float* mA, float* mB, float* mC, const size_t M, const size_t N)
{
    using namespace Eigen;
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eA(mA, M, N);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eB(mB, M, N);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eC(mC, M, N);
    eC = eA + eB;
}

// 例子1：创建2x2矩阵，逐元素赋值，然后输出
static void eigen_example1()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m(2,2); // MatrixXd是最常用的Eigen数据类型，X表示任意尺寸，d表示double。这里创建的是2x2规格的矩阵。
    m(0,0) = 3; // 追元素赋值，注意是用小括号，这应该是重载了括号操作符
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);
    cout << m << endl; // 输出矩阵，直接std::cout即可，说明有重载<<操作符
}

// 矩阵乘以向量的例子。
static void eigen_example2()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m = MatrixXd::Random(3,3); //创建3x3矩阵，元素为[-1,1]之间的随机浮点数

    m = (m+MatrixXd::Constant(3, 3, 1.2)) * 50;//[-1,1] + 1.2 = [0.2, 2.2]; [0.2, 2.2]*50=[10, 110]
    // 也就是把原本在[-1,1]之间的各个元素，映射到[10, 110]之间

    cout << "m=" << endl << m << endl;

    VectorXd v(3); // 创建一个3行的列向量v

    v << 1, 2, 3; // 给列向量赋值，分别为1,2,3

    cout << "m*v=" << endl << m*v << endl; // 计算 m*v，也就是矩阵乘以向量，并输出结果
}

// eigen_example2的另一个版本：使用具体尺寸的类型，而不是MatrixXd和VectorXd
static void eigen_example2_2()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    Matrix3d m = Matrix3d::Random();
    m = (m+Matrix3d::Constant(1.2)) * 50;
    cout << "m=" << endl << m << endl;
    Vector3d v(1,2,3);
    cout << "m*v=" << endl << m*v << endl;
}

// 访问系数（元素）
static void eigen_example3()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m(2,2);
    m(0, 0) = 3;
    m(1, 0) = 2.5;
    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    cout << "Here is the matrix m:\n" << m << endl;

    VectorXd v(2);
    v(0) = 4;
    v(1) = v(0) - 1;
    cout << "Here is the vector v:\n" << v << endl;
}

// 用逗号表达式初始化
static void eigen_example4()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    Matrix3f m; // 声明

    // 初始化。注意：必须先声明，才能用逗号表达式初始化；定义时直接用逗号表达式初始化会报错
    // 因为这个初始化其实是通过重载<<操作符实现的
    m << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
    cout << m << endl;
}

// 输出维度信息、元素个数；动态尺寸矩阵/向量的resize
static void eigen_example5()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXd m(2, 5);
    m.resize(4, 3);
    printf("The matrix m is of size: rows=%d, cols=%d, num elements=%d\n",
        m.rows(), m.cols(), m.size());

    VectorXd v(2);
    v.resize(5);
    std::cout << "The vector v is of size " << v.size() << endl;
    printf("As a matrix, v is of size: rows=%d, cols=%d\n", v.rows(), v.cols());
}

// 动态尺寸矩阵赋值，如果尺寸不同，则自动把等号左边的矩阵resize
static void eigen_example5_2()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXf a(2, 2);
    printf("originally, a's size: row=%d, col=%d\n", a.rows(), a.cols());
    MatrixXf b(3, 3);
    a = b;
    printf("after assign, a's size: row=%d, col=%d\n", a.rows(), a.cols());
}

// eigen_example5_2的修改，从固定尺寸(3x3)的矩阵，赋值给动态维度的2x2矩阵，隐式resize
static void eigen_example5_3()
{
    LOGD("--- %s ---\n", __FUNCTION__);
    MatrixXf a(2, 2);
    printf("originally, a's size: row=%d, col=%d\n", a.rows(), a.cols());
    Matrix3f b;
    b << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
    a = b;
    printf("after assign, a's size: row=%d, col=%d\n", a.rows(), a.cols());
}

int main() {
    eigen_example1();
    eigen_example2();
    eigen_example2_2();
    eigen_example3();
    eigen_example4();
    eigen_example5();
    eigen_example5_2();
    eigen_example5_3();

    return 0;
}


// Copyright 2018 Hans Dembinski
//
// 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)

//[ guide_stdlib_algorithms

#include <boost/histogram.hpp>
#include <cassert>

#include <algorithm> // fill, any_of, min_element, max_element
#include <cmath>     // sqrt
#include <numeric>   // partial_sum, inner_product

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

  // make histogram that represents a probability density function (PDF)
  auto h1 = make_histogram(axis::regular<>(4, 1.0, 3.0));

  // use std::fill to set all counters to 0.25, including *flow cells
  std::fill(h1.begin(), h1.end(), 0.25);
  // reset *flow cells to zero
  h1.at(-1) = h1.at(4) = 0;

  // compute the cumulative density function (CDF), overriding cell values
  std::partial_sum(h1.begin(), h1.end(), h1.begin());

  assert(h1.at(-1) == 0.0);
  assert(h1.at(0) == 0.25);
  assert(h1.at(1) == 0.50);
  assert(h1.at(2) == 0.75);
  assert(h1.at(3) == 1.00);
  assert(h1.at(4) == 1.00);

  // use any_of to check if any cell values are smaller than 0.1,
  // and use indexed() to skip underflow and overflow cells
  auto h1_ind = indexed(h1);
  const auto any_small =
      std::any_of(h1_ind.begin(), h1_ind.end(), [](const auto& x) { return *x < 0.1; });
  assert(any_small == false); // underflow and overflow are zero, but skipped

  // find maximum element
  const auto max_it = std::max_element(h1.begin(), h1.end());
  assert(max_it == h1.end() - 2);

  // find minimum element
  const auto min_it = std::min_element(h1.begin(), h1.end());
  assert(min_it == h1.begin());

  // make second PDF
  auto h2 = make_histogram(axis::regular<>(4, 1.0, 4.0));
  h2.at(0) = 0.1;
  h2.at(1) = 0.3;
  h2.at(2) = 0.2;
  h2.at(3) = 0.4;

  // computing cosine similiarity: cos(theta) = A dot B / sqrt((A dot A) * (B dot B))
  const auto aa = std::inner_product(h1.begin(), h1.end(), h1.begin(), 0.0);
  const auto bb = std::inner_product(h2.begin(), h2.end(), h2.begin(), 0.0);
  const auto ab = std::inner_product(h1.begin(), h1.end(), h2.begin(), 0.0);
  const auto cos_sim = ab / std::sqrt(aa * bb);

  assert(std::abs(cos_sim - 0.78) < 1e-2);
}

//]


#define BOOST_TEST_MODULE test_utils

#include <boost/test/unit_test.hpp>
#include <Utils/utils.h>
#include <Utils/bshelper.h>

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_SUITE(utils_boost)

    BOOST_AUTO_TEST_CASE(annual_cap1) {
        BOOST_TEST_MESSAGE("Testing annual_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 121; // (100*(1.1)^2)

        auto calculated_value = annual_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_discount1) {
        BOOST_TEST_MESSAGE("Testing annual_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 121;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 100; // (121/(1.1)^2)

        auto calculated_value = discount_annually(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_cap1) {
        BOOST_TEST_MESSAGE("Testing period_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 110.25; // (100*(1.05)^2)

        auto calculated_value = period_capitalization(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_discount1) {
        BOOST_TEST_MESSAGE("Testing period_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 110.25;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 100; // (110.25/(1.05)^2)

        auto calculated_value = discount_by_periods(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(continuous_cap1) {
        BOOST_TEST_MESSAGE("Testing continuous_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 122.140275816; // 100 * e^(0.10*2) rounded to second

        auto calculated_value = continuous_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(continuous_discount1) {
        BOOST_TEST_MESSAGE("Testing continuous_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 122.140275816;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 100; // 122.140275816 / e^(0.10*2) rounded to second

        auto calculated_value = discount_continuously(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(fwd_rate1) {
        BOOST_TEST_MESSAGE("Testing fwd_rate1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double zero_coupon_total = 0.04;
        int years_total = 2;

        double zero_coupon_partial = 0.03;
        int years_partial = 1;

        double theoretical_value = 0.05; // (100*(1.1)^2) rounded to second

        auto calculated_value = forward_rate(zero_coupon_total, years_total, zero_coupon_partial, years_partial);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_to_cont1) {
        BOOST_TEST_MESSAGE("Testing annual_to_cont1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double annual_rate = 0.12;

        double theoretical_value = 0.1133286853; // ln(1.12)

        auto calculated_value = annual_to_continuous_rate(1, annual_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-10));
    }

    BOOST_AUTO_TEST_CASE(cont_to_annual) {
        BOOST_TEST_MESSAGE("Testing cont_to_annual");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double continuous_rate = 0.11332868531; //(e^0.1)-1

        double theoretical_value = 0.12; // (e^0.1)-1

        auto calculated_value = continuous_to_annual_rate(1, continuous_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-9));
    }

    BOOST_AUTO_TEST_CASE(total_day_count, *utf::tolerance(0.0001)) {
        BOOST_TEST_MESSAGE("Testing cont_to_annual");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double myDoubles[] = {185.0 / 360, 182.0 / 360, 182.0 / 360, 182.0 / 360};
        std::vector<double> dayCountFractionVector(myDoubles, myDoubles + sizeof(myDoubles) / sizeof(double));

        std::vector<double> calculated_values = getTotalDayCountFractionVector(dayCountFractionVector);

        double myResults[] = {0.513888888889, 1.019444444444, 1.525000000000, 2.030555555556};
        std::vector<double> expected_values(myResults, myResults + sizeof(myResults) / sizeof(double));

        BOOST_TEST(calculated_values == expected_values, boost::test_tools::per_element());

    }

    BOOST_AUTO_TEST_CASE(black_scholes_d1) {
        BOOST_TEST_MESSAGE("Testing black_scholes_d1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double asset_price = 50.0;
        double strike = 50.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double time_to_maturity = 5.0;

        double theoretical_value = 0.8251812; // (e^0.1)-1

        auto calculated_value = getD1(asset_price, strike, rate, volatility, time_to_maturity);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_d1_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_d2) {
        BOOST_TEST_MESSAGE("Testing black_scholes_d2");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double asset_price = 50.0;
        double strike = 50.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double time_to_maturity = 5.0;

        double theoretical_value = -0.5611809; // (e^0.1)-1

        auto calculated_value = getD2(asset_price, strike, rate, volatility, time_to_maturity);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_d2_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_weightAsset) {
        BOOST_TEST_MESSAGE("Testing black_scholes_weightAsset");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double asset_price = 50.0;
        double option_d1 = 0.8251812; //pre-calculated value

        double theoretical_value = 39.7682821; // (e^0.1)-1

        auto calculated_value = weightAsset(asset_price, option_d1);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_weightStrike) {
        BOOST_TEST_MESSAGE("Testing black_scholes_weightStrike");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double strike = 50.0;
        double rate = 3.66 / 100;
        double time_to_maturity = 5.0;
        double option_d2 = -0.5611809; //pre-calculated value

        double theoretical_value = 11.9642594; // (e^0.1)-1

        auto calculated_value = weightStrike(strike, option_d2, rate, time_to_maturity);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-7));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_evaluateCall) {
        BOOST_TEST_MESSAGE("Testing black_scholes_evaluateCall");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 27.804023; // (e^0.1)-1

        auto calculated_value = evaluateCall(option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_evaluatePut) {
        BOOST_TEST_MESSAGE("Testing black_scholes_evaluatePut");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 19.442431; // (e^0.1)-1

        auto calculated_value = evaluatePut(option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_blackScholesCall) {
        BOOST_TEST_MESSAGE("Testing black_scholes_blackScholesCall");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 27.804023; // (e^0.1)-1

        auto calculated_value = blackScholes(call, option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

    BOOST_AUTO_TEST_CASE(black_scholes_blackScholesPut) {
        BOOST_TEST_MESSAGE("Testing black_scholes_blackScholesPut");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double option_strike = 50.0;
        double time_to_maturity = 5.0;
        double rate = 3.66 / 100;
        double volatility = 62.0 / 100;
        double asset_price = 50.0;

        double theoretical_value = 19.442431; // (e^0.1)-1

        auto calculated_value = blackScholes(put, option_strike, time_to_maturity, rate, volatility, asset_price);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_asset_contribution: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-6));
    }

BOOST_AUTO_TEST_SUITE_END()


/// @file example_geqr2.cpp
/// @author Weslley S Pereira, University of Colorado Denver, USA
//
// Copyright (c) 2022, University of Colorado Denver. All rights reserved.
//
// This file is part of <T>LAPACK.
// <T>LAPACK is free software: you can redistribute it and/or modify it under
// the terms of the BSD 3-Clause license. See the accompanying LICENSE file.

#include <iostream>

// Must be loaded in the following order
#include <plugins/tlapack_eigen.hpp>
#include <tlapack.hpp>

#include <Eigen/Dense>
#include <Eigen/Householder>

int main( int argc, char** argv )
{
    using std::size_t;
    using pair = std::pair<size_t,size_t>;
    using namespace blas;
    using namespace lapack;
    using Eigen::Matrix;

    // Constants
    const size_t m = 5;
    const size_t n = 3;

    // Input data
    Matrix<float, m, n> A {
        { 1,  2,  3},
        { 4,  5,  6},
        { 7,  8,  9},
        {10, 11, 12},
        {13, 14, 15}
    };

    // Matrices
    Matrix<float, m, n> Q = A;
    Matrix<float, n, n> R = Matrix<float, n, n>::Zero();
    Matrix<float, m, n> QtimesR = Matrix<float, m, n>::Zero();

    std::cout << "A = " << std::endl << A << std::endl << std::endl;

    // <T>LAPACK -----------------------------------------------

    std::cout << "--- <T>LAPACK: ---" << std::endl << std::endl;

    // Allocates memory
    Matrix<float, n, 1> tau;
    Matrix<float, n-1, 1> work;
    Matrix<float, n, n> orthQ;

    // Compute QR decomposision in place
    geqr2( Q, tau, work );
    // Copy the upper triangle to R
    lacpy( upper_triangle, submatrix(Q,pair{0,n},pair{0,n}), R );
    // Generate Q
    org2r( n, Q, tau, work );

    std::cout << "Q = " << std::endl << Q << std::endl;
    std::cout << std::endl;

    std::cout << "R = " << std::endl << R << std::endl;
    std::cout << std::endl;

    // Checking A = Q R
    lacpy( general_matrix, Q, QtimesR );
    trmm( Side::Right, Uplo::Upper, Op::NoTrans, Diag::NonUnit, 1.0, R, QtimesR );
    std::cout << "QR = " << std::endl << QtimesR << std::endl;
    QtimesR -= A;
    std::cout << "\\|QR - A\\|_F/\\|A\\|_F = " << std::endl << lange( frob_norm, QtimesR ) / lange( frob_norm, A ) << std::endl;
    std::cout << std::endl;

    // Checking orthogonality of Q
    orthQ = Matrix<float, n, n>::Identity();
    syrk( Uplo::Upper, Op::Trans, 1.0, Q, -1.0, orthQ );
    std::cout << "\\|Q^t Q - I\\|_F = " << std::endl << lansy( frob_norm, upper_triangle, orthQ ) << std::endl;
    std::cout << std::endl;

    // Eigen -----------------------------------------------

    std::cout << "--- Eigen: ---" << std::endl << std::endl;

    // Compute QR decomposision in place, possibly allocating memory dynamically
    Eigen::HouseholderQR<decltype(A)> qrEigen( A );
    // Generate Q
    Q = qrEigen.householderQ() * Matrix<float, m, n>::Identity();
    // Copy the upper triangle to R
    R = qrEigen.matrixQR().block(0,0,n,n).triangularView<Eigen::Upper>();

    std::cout << "Q = " << std::endl << Q << std::endl;
    std::cout << std::endl;

    std::cout << "R = " << std::endl << R << std::endl;
    std::cout << std::endl;

    // Checking A = Q R
    QtimesR = Q * R;
    std::cout << "QR = " << std::endl << QtimesR << std::endl;
    std::cout << "\\|QR - A\\|_F/\\|A\\|_F = " << (QtimesR-A).norm() / A.norm() << std::endl;
    std::cout << std::endl;

    // Checking orthogonality of Q
    orthQ = Q.transpose() * Q - Matrix<float, n, n>::Identity();
    std::cout << "\\|Q^t Q - I\\|_F = " << std::endl << orthQ.norm() << std::endl;
    std::cout << std::endl;

    return 0;
}


// 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)

// Copyright Jeremy W. Murphy 2015.

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

//[polynomial_arithmetic_0
/*`First include the essential polynomial header (and others) to make the example:
*/
#include <boost/math/tools/polynomial.hpp>
//] [polynomial_arithmetic_0

#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/tools/assert.hpp>

#include <iostream>
#include <stdexcept>
#include <cmath>
#include <string>
#include <utility>

//[polynomial_arithmetic_1
/*`and some using statements are convenient:
*/

using std::string;
using std::exception;
using std::cout;
using std::abs;
using std::pair;

using namespace boost::math;
using namespace boost::math::tools; // for polynomial
using boost::lexical_cast;

//] [/polynomial_arithmetic_1]

template <typename T>
string sign_str(T const &x)
{
  return x < 0 ? "-" : "+";
}

template <typename T>
string inner_coefficient(T const &x)
{
  string result(" " + sign_str(x) + " ");
  if (abs(x) != T(1))
      result += lexical_cast<string>(abs(x));
  return result;
}

/*! Output in formula format.
For example: from a polynomial in Boost container storage  [ 10, -6, -4, 3 ]
show as human-friendly formula notation: 3x^3 - 4x^2 - 6x + 10.
*/
template <typename T>
string formula_format(polynomial<T> const &a)
{
  string result;
  if (a.size() == 0)
      result += lexical_cast<string>(T(0));
  else
  {
    // First one is a special case as it may need unary negate.
    unsigned i = a.size() - 1;
    if (a[i] < 0)
        result += "-";
    if (abs(a[i]) != T(1))
        result += lexical_cast<string>(abs(a[i]));

    if (i > 0)
    {
      result += "x";
      if (i > 1)
      {
          result += "^" + lexical_cast<string>(i);
          i--;
          for (; i != 1; i--)
              if (a[i])
                result += inner_coefficient(a[i]) + "x^" + lexical_cast<string>(i);

          if (a[i])
            result += inner_coefficient(a[i]) + "x";
      }
      i--;

      if (a[i])
        result += " " + sign_str(a[i]) + " " + lexical_cast<string>(abs(a[i]));
    }
  }
  return result;
} // string formula_format(polynomial<T> const &a)

int main()
{
  cout << "Example: Polynomial arithmetic.\n\n";

  try
  {
//[polynomial_arithmetic_2
/*`Store the coefficients in a convenient way to access them,
then create some polynomials using construction from an iterator range,
and finally output in a 'pretty' formula format.

[tip Although we might conventionally write a polynomial from left to right
in descending order of degree, Boost.Math stores in [*ascending order of degree].]

  Read/write for humans:    3x^3 - 4x^2 - 6x + 10
  Boost polynomial storage: [ 10, -6, -4, 3 ]
*/
  std::array<double, 4> const d3a = {{10, -6, -4, 3}};
  polynomial<double> const a(d3a.begin(), d3a.end());

  // With C++11 and later, you can also use initializer_list construction.
  polynomial<double> const b{{-2.0, 1.0}};

  // formula_format() converts from Boost storage to human notation.
  cout << "a = " << formula_format(a)
  << "\nb = " << formula_format(b) << "\n\n";

//] [/polynomial_arithmetic_2]

//[polynomial_arithmetic_3
  // Now we can do arithmetic with the usual infix operators: + - * / and %.
  polynomial<double> s = a + b;
  cout << "a + b = " << formula_format(s) << "\n";
  polynomial<double> d = a - b;
  cout << "a - b = " << formula_format(d) << "\n";
  polynomial<double> p = a * b;
  cout << "a * b = " << formula_format(p) << "\n";
  polynomial<double> q = a / b;
  cout << "a / b = " << formula_format(q) << "\n";
  polynomial<double> r = a % b;
  cout << "a % b = " << formula_format(r) << "\n";
//] [/polynomial_arithmetic_3]

//[polynomial_arithmetic_4
/*`
Division is a special case where you can calculate two for the price of one.

Actually, quotient and remainder are always calculated together due to the nature
of the algorithm: the infix operators return one result and throw the other
away.

If you are doing a lot of division and want both the quotient and remainder, then
you don't want to do twice the work necessary.

In that case you can call the underlying function, [^quotient_remainder],
to get both results together as a pair.
*/
  pair< polynomial<double>, polynomial<double> > result;
  result = quotient_remainder(a, b);
// Reassure ourselves that the result is the same.
  BOOST_MATH_ASSERT(result.first == q);
  BOOST_MATH_ASSERT(result.second == r);
//] [/polynomial_arithmetic_4]
//[polynomial_arithmetic_5
  /*
We can use the right and left shift operators to add and remove a factor of x.
This has the same semantics as left and right shift for integers where it is a
factor of 2. x is the smallest prime factor of a polynomial as is 2 for integers.
*/
    cout << "Right and left shift operators.\n";
    cout << "\n" << formula_format(p) << "\n";
    cout << "... right shift by 1 ...\n";
    p >>= 1;
    cout << formula_format(p) << "\n";
    cout << "... left shift by 2 ...\n";
    p <<= 2;
    cout << formula_format(p) << "\n";

/*
We can also give a meaning to odd and even for a polynomial that is consistent
with these operations: a polynomial is odd if it has a non-zero constant value,
even otherwise. That is:
    x^2 + 1     odd
    x^2         even
   */
    cout << std::boolalpha;
    cout << "\nPrint whether a polynomial is odd.\n";
    cout << formula_format(s) << "   odd? " << odd(s) << "\n";
    // We cheekily use the internal details to subtract the constant, making it even.
    s -= s.data().front();
    cout << formula_format(s) << "   odd? " << odd(s) << "\n";
    // And of course you can check if it is even:
    cout << formula_format(s) << "   even? " << even(s) << "\n";

    //] [/polynomial_arithmetic_5]
    //[polynomial_arithmetic_6]
    /* For performance and convenience, we can test whether a polynomial is zero
     * by implicitly converting to bool with the same semantics as int.    */
    polynomial<double> zero; // Default construction is 0.
    cout << "zero: " << (zero ? "not zero" : "zero") << "\n";
    cout << "r: " << (r ? "not zero" : "zero") << "\n";
    /* We can also set a polynomial to zero without needing a another zero
     * polynomial to assign to it. */
    r.set_zero();
    cout << "r: " << (r ? "not zero" : "zero") << "\n";
    //] [/polynomial_arithmetic_6]
}
catch (exception const &e)
{
  cout << "\nMessage from thrown exception was:\n   " << e.what() << "\n";
}
return 0;
} // int main()

/*
//[polynomial_output_1

a = 3x^3 - 4x^2 - 6x + 10
b = x - 2

//] [/polynomial_output_1]

//[polynomial_output_2

a + b = 3x^3 - 4x^2 - 5x + 8
a - b = 3x^3 - 4x^2 - 7x + 12
a * b = 3x^4 - 10x^3 + 2x^2 + 22x - 20
a / b = 3x^2 + 2x - 2
a % b = 6

//] [/polynomial_output_2]

*/


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.
// 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)

#ifdef _MSC_VER
#  pragma warning(disable: 4512) // assignment operator could not be generated.
#  pragma warning(disable: 4510) // default constructor could not be generated.
#  pragma warning(disable: 4610) // can never be instantiated - user defined constructor required.
#endif

#include <iostream>
#include <iomanip>
#include <boost/math/distributions/students_t.hpp>

void two_samples_t_test_equal_sd(
        double Sm1,
        double Sd1,
        unsigned Sn1,
        double Sm2,
        double Sd2,
        unsigned Sn2,
        double alpha)
{
   //
   // Sm1 = Sample Mean 1.
   // Sd1 = Sample Standard Deviation 1.
   // Sn1 = Sample Size 1.
   // Sm2 = Sample Mean 2.
   // Sd2 = Sample Standard Deviation 2.
   // Sn2 = Sample Size 2.
   // alpha = Significance Level.
   //
   // A Students t test applied to two sets of data.
   // We are testing the null hypothesis that the two
   // samples have the same mean and that any difference
   // if due to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda353.htm
   //
   using namespace std;
   using namespace boost::math;

   // Print header:
   cout <<
      "_______________________________________________\n"
      "Student t test for two samples (equal variances)\n"
      "_______________________________________________\n\n";
   cout << setprecision(5);
   cout << setw(55) << left << "Number of Observations (Sample 1)" << "=  " << Sn1 << "\n";
   cout << setw(55) << left << "Sample 1 Mean" << "=  " << Sm1 << "\n";
   cout << setw(55) << left << "Sample 1 Standard Deviation" << "=  " << Sd1 << "\n";
   cout << setw(55) << left << "Number of Observations (Sample 2)" << "=  " << Sn2 << "\n";
   cout << setw(55) << left << "Sample 2 Mean" << "=  " << Sm2 << "\n";
   cout << setw(55) << left << "Sample 2 Standard Deviation" << "=  " << Sd2 << "\n";
   //
   // Now we can calculate and output some stats:
   //
   // Degrees of freedom:
   double v = Sn1 + Sn2 - 2;
   cout << setw(55) << left << "Degrees of Freedom" << "=  " << v << "\n";
   // Pooled variance:
   double sp = sqrt(((Sn1-1) * Sd1 * Sd1 + (Sn2-1) * Sd2 * Sd2) / v);
   cout << setw(55) << left << "Pooled Standard Deviation" << "=  " << v << "\n";
   // t-statistic:
   double t_stat = (Sm1 - Sm2) / (sp * sqrt(1.0 / Sn1 + 1.0 / Sn2));
   cout << setw(55) << left << "T Statistic" << "=  " << t_stat << "\n";
   //
   // Define our distribution, and get the probability:
   //
   students_t dist(v);
   double q = cdf(complement(dist, fabs(t_stat)));
   cout << setw(55) << left << "Probability that difference is due to chance" << "=  "
      << setprecision(3) << scientific << 2 * q << "\n\n";
   //
   // Finally print out results of alternative hypothesis:
   //
   cout << setw(55) << left <<
      "Results for Alternative Hypothesis and alpha" << "=  "
      << setprecision(4) << fixed << alpha << "\n\n";
   cout << "Alternative Hypothesis              Conclusion\n";
   cout << "Sample 1 Mean != Sample 2 Mean       " ;
   if(q < alpha / 2)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean <  Sample 2 Mean       ";
   if(cdf(dist, t_stat) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean >  Sample 2 Mean       ";
   if(cdf(complement(dist, t_stat)) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << endl << endl;
}

void two_samples_t_test_unequal_sd(
        double Sm1,
        double Sd1,
        unsigned Sn1,
        double Sm2,
        double Sd2,
        unsigned Sn2,
        double alpha)
{
   //
   // Sm1 = Sample Mean 1.
   // Sd1 = Sample Standard Deviation 1.
   // Sn1 = Sample Size 1.
   // Sm2 = Sample Mean 2.
   // Sd2 = Sample Standard Deviation 2.
   // Sn2 = Sample Size 2.
   // alpha = Significance Level.
   //
   // A Students t test applied to two sets of data.
   // We are testing the null hypothesis that the two
   // samples have the same mean and that any difference
   // if due to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda353.htm
   //
   using namespace std;
   using namespace boost::math;

   // Print header:
   cout <<
      "_________________________________________________\n"
      "Student t test for two samples (unequal variances)\n"
      "_________________________________________________\n\n";
   cout << setprecision(5);
   cout << setw(55) << left << "Number of Observations (Sample 1)" << "=  " << Sn1 << "\n";
   cout << setw(55) << left << "Sample 1 Mean" << "=  " << Sm1 << "\n";
   cout << setw(55) << left << "Sample 1 Standard Deviation" << "=  " << Sd1 << "\n";
   cout << setw(55) << left << "Number of Observations (Sample 2)" << "=  " << Sn2 << "\n";
   cout << setw(55) << left << "Sample 2 Mean" << "=  " << Sm2 << "\n";
   cout << setw(55) << left << "Sample 2 Standard Deviation" << "=  " << Sd2 << "\n";
   //
   // Now we can calculate and output some stats:
   //
   // Degrees of freedom:
   double v = Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2;
   v *= v;
   double t1 = Sd1 * Sd1 / Sn1;
   t1 *= t1;
   t1 /=  (Sn1 - 1);
   double t2 = Sd2 * Sd2 / Sn2;
   t2 *= t2;
   t2 /= (Sn2 - 1);
   v /= (t1 + t2);
   cout << setw(55) << left << "Degrees of Freedom" << "=  " << v << "\n";
   // t-statistic:
   double t_stat = (Sm1 - Sm2) / sqrt(Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2);
   cout << setw(55) << left << "T Statistic" << "=  " << t_stat << "\n";
   //
   // Define our distribution, and get the probability:
   //
   students_t dist(v);
   double q = cdf(complement(dist, fabs(t_stat)));
   cout << setw(55) << left << "Probability that difference is due to chance" << "=  "
      << setprecision(3) << scientific << 2 * q << "\n\n";
   //
   // Finally print out results of alternative hypothesis:
   //
   cout << setw(55) << left <<
      "Results for Alternative Hypothesis and alpha" << "=  "
      << setprecision(4) << fixed << alpha << "\n\n";
   cout << "Alternative Hypothesis              Conclusion\n";
   cout << "Sample 1 Mean != Sample 2 Mean       " ;
   if(q < alpha / 2)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean <  Sample 2 Mean       ";
   if(cdf(dist, t_stat) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Sample 1 Mean >  Sample 2 Mean       ";
   if(cdf(complement(dist, t_stat)) < alpha)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << endl << endl;
}

int main()
{
   //
   // Run tests for Car Mileage sample data
   // http://www.itl.nist.gov/div898/handbook/eda/section3/eda3531.htm
   // from the NIST website http://www.itl.nist.gov.  The data compares
   // miles per gallon of US cars with miles per gallon of Japanese cars.
   //
   two_samples_t_test_equal_sd(20.14458, 6.414700, 249, 30.48101, 6.107710, 79, 0.05);
   two_samples_t_test_unequal_sd(20.14458, 6.414700, 249, 30.48101, 6.107710, 79, 0.05);

   return 0;
} // int main()

/*
Output is

------ Build started: Project: students_t_two_samples, Configuration: Debug Win32 ------
Compiling...
students_t_two_samples.cpp
Linking...
Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\students_t_two_samples.exe"
_______________________________________________
Student t test for two samples (equal variances)
_______________________________________________

Number of Observations (Sample 1)                      =  249
Sample 1 Mean                                          =  20.145
Sample 1 Standard Deviation                            =  6.4147
Number of Observations (Sample 2)                      =  79
Sample 2 Mean                                          =  30.481
Sample 2 Standard Deviation                            =  6.1077
Degrees of Freedom                                     =  326
Pooled Standard Deviation                              =  326
T Statistic                                            =  -12.621
Probability that difference is due to chance           =  5.273e-030

Results for Alternative Hypothesis and alpha           =  0.0500

Alternative Hypothesis              Conclusion
Sample 1 Mean != Sample 2 Mean       NOT REJECTED
Sample 1 Mean <  Sample 2 Mean       NOT REJECTED
Sample 1 Mean >  Sample 2 Mean       REJECTED

_________________________________________________
Student t test for two samples (unequal variances)
_________________________________________________

Number of Observations (Sample 1)                      =  249
Sample 1 Mean                                          =  20.14458
Sample 1 Standard Deviation                            =  6.41470
Number of Observations (Sample 2)                      =  79
Sample 2 Mean                                          =  30.48101
Sample 2 Standard Deviation                            =  6.10771
Degrees of Freedom                                     =  136.87499
T Statistic                                            =  -12.94627
Probability that difference is due to chance           =  1.571e-025

Results for Alternative Hypothesis and alpha           =  0.0500

Alternative Hypothesis              Conclusion
Sample 1 Mean != Sample 2 Mean       NOT REJECTED
Sample 1 Mean <  Sample 2 Mean       NOT REJECTED
Sample 1 Mean >  Sample 2 Mean       REJECTED

Build Time 0:03
Build log was saved at "file://i:\boost-06-05-03-1300\libs\math\test\Math_test\students_t_two_samples\Debug\BuildLog.htm"
students_t_two_samples - 0 error(s), 0 warning(s)
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


/**
 * @file nonlinschroedingerequation_main.cc
 * @brief NPDE homework NonLinSchroedingerEquation code
 * @author Oliver Rietmann
 * @date 22.04.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <complex>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <utility>

#include "nonlinschroedingerequation.h"
#include "propagator.h"

int main() {
  /* SAM_LISTING_BEGIN_9 */
  // Load mesh and initalize FE space and DOF handler
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(
      std::move(mesh_factory), CURRENT_SOURCE_DIR "/../meshes/square_64.msh");
  auto mesh_p = reader.mesh();
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

  // Mass matrix
  lf::assemble::COOMatrix<double> D_COO(N_dofs, N_dofs);
  NonLinSchroedingerEquation::MassElementMatrixProvider mass_emp;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, mass_emp, D_COO);
  Eigen::SparseMatrix<double> D = D_COO.makeSparse();
  Eigen::SparseMatrix<std::complex<double>> M = std::complex<double>(0, 1) * D;

  // Stiffness matrix
  lf::assemble::COOMatrix<double> A_COO(N_dofs, N_dofs);
  lf::uscalfe::LinearFELaplaceElementMatrix stiffness_emp;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, stiffness_emp, A_COO);
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();

  // Prepare timestepping
  int timesteps = 100;
  double T = 1.0;
  double tau = T / timesteps;

// Prepare inital data
  const double PI = 3.14159265358979323846;
  auto u0 = [PI](Eigen::Vector2d x) -> double {
    return 4.0 * std::cos(PI * x(0)) * std::cos(PI * x(1));
  };
  lf::mesh::utils::MeshFunctionGlobal mf_u0{u0};
  Eigen::VectorXcd mu = lf::fe::NodalProjection(*fe_space, mf_u0);

  // Prepare split-step propagator for full step $\tau$
  NonLinSchroedingerEquation::SplitStepPropagator splitStepPropagator(A, M,
                                                                      tau);

  // Arrays for storing "energies" contributing to the Hamiltonian
  Eigen::VectorXd norm(timesteps + 1);
  Eigen::VectorXd E_kin(timesteps + 1);
  Eigen::VectorXd E_int(timesteps + 1);
  // Timestepping
  for (int j = 0; j < timesteps; ++j) {
    // Compute norm and energy along the solution
    norm(j) = NonLinSchroedingerEquation::Norm(mu, D);
    E_kin(j) = NonLinSchroedingerEquation::KineticEnergy(mu, A);
    E_int(j) = NonLinSchroedingerEquation::InteractionEnergy(mu, D);
    // Timestep tau according to Strang splitting
    mu = splitStepPropagator(mu);
  }
  norm(timesteps) = NonLinSchroedingerEquation::Norm(mu, D);
  E_kin(timesteps) = NonLinSchroedingerEquation::KineticEnergy(mu, A);
  E_int(timesteps) = NonLinSchroedingerEquation::InteractionEnergy(mu, D);

  // Timegrid
  Eigen::VectorXd t = Eigen::VectorXd::LinSpaced(timesteps + 1, 0.0, T);

  // Nice output format
  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");

  // Write norm to file
  std::ofstream norm_csv;
  norm_csv.open("norm.csv");
  norm_csv << t.transpose().format(CSVFormat) << std::endl;
  norm_csv << norm.transpose().format(CSVFormat) << std::endl;
  norm_csv.close();
  /* SAM_LISTING_END_9 */

  // Call python script to plot norm
  std::cout << "Generated " CURRENT_BINARY_DIR "/norm.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_norm.py " CURRENT_BINARY_DIR
              "/norm.csv " CURRENT_BINARY_DIR "/norm.eps");

  // Write energies to file
  std::ofstream energies_csv;
  energies_csv.open("energies.csv");
  energies_csv << t.transpose().format(CSVFormat) << std::endl;
  energies_csv << E_kin.transpose().format(CSVFormat) << std::endl;
  energies_csv << E_int.transpose().format(CSVFormat) << std::endl;
  energies_csv.close();

  // Call python script to plot energies
  std::cout << "Generated " CURRENT_BINARY_DIR "/energies.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR
              "/plot_energies.py " CURRENT_BINARY_DIR
              "/energies.csv " CURRENT_BINARY_DIR "/energies.eps");

  // Write entry-wise squared modulus of $\mu$ to .vtk file
  std::cout << "Generated " CURRENT_BINARY_DIR "/solution.vtk" << std::endl;
  lf::io::VtkWriter vtk_writer(mesh_p, "solution.vtk");
  Eigen::VectorXd mu_abs2 = mu.cwiseAbs2();
  lf::fe::MeshFunctionFE mu_abs2_mf(fe_space, mu_abs2);
  vtk_writer.WritePointData("mu_abs2", mu_abs2_mf);

  return 0;
}


#ifndef _COCONUT_PULP_MATH_QUATERNION_HPP_
#define _COCONUT_PULP_MATH_QUATERNION_HPP_

#include <boost/operators.hpp>

#include "ScalarEqual.hpp"
#include "Vector.hpp"

namespace coconut {
namespace pulp {
namespace math {

template <class ScalarType, class ScalarEqualityFunc = ScalarEqual<ScalarType>>
class Quaternion :
	boost::equality_comparable<Quaternion<ScalarType, ScalarEqualityFunc>,
	boost::additive<Quaternion<ScalarType, ScalarEqualityFunc>,
	boost::multipliable<Quaternion<ScalarType, ScalarEqualityFunc>,
	boost::multiplicative<Quaternion<ScalarType, ScalarEqualityFunc>, ScalarType
	>>>>
{
public:

	using Scalar = ScalarType;

	using ScalarPart = Scalar;

	using VectorPart = Vector<Scalar, 3, ScalarEqualityFunc>;

	// --- CONSTRUCTORS AND OPERATORS

	constexpr Quaternion(ScalarPart s, VectorPart v) :
		elements_(v.x(), v.y(), v.z(), s)
	{
	}

	friend std::ostream& operator<<(std::ostream& os, const Quaternion& q) {
		return os << q.s() << " + " << q.v();
	}

	friend bool operator==(const Quaternion& lhs, const Quaternion& rhs) noexcept {
		return lhs.elements_ == rhs.elements_;
	}

	Quaternion& operator*=(const Quaternion& other) noexcept {
		const auto s1 = s();
		const auto s2 = other.s();
		const auto v1 = v();
		const auto v2 = other.v();

		const auto s = s1 * s2 - ::dot(v1, v2);
		const auto v = s1 * v2 + s2 * v1 + cross(v1, v2);

		*this = Quaternion(s, v);

		return *this;
	}

	Quaternion& operator+=(const Quaternion& other) noexcept {
		elements_ += other.elements_;
		return *this;
	}

	Quaternion& operator-=(const Quaternion& other) noexcept {
		elements_ -= other.elements_;
		return *this;
	}

	Quaternion& operator*=(const Scalar& s) noexcept {
		elements_ *= s;
		return *this;
	}

	Quaternion& operator/=(const Scalar& s) noexcept {
		elements_ /= s;
		return *this;
	}

	// --- QUATERNION-SPECIFIC OPERATIONS

	constexpr Quaternion conjugate() const noexcept {
		return Quaternion(s(), -v());
	}

	Quaternion& normalise() noexcept {
		const auto n = norm();
		if (n > Scalar(0)) {
			*this /= n;
		}
		return *this;
	}

	Quaternion normalised() const noexcept {
		auto result = *this;
		return result.normalise();
	}

	Scalar norm() const noexcept {
		return elements_.length();
	}

	Scalar normSq() const noexcept {
		return elements_.lengthSq();
	}

	Quaternion inverse() const noexcept {
		const auto n = normSq();
		assert(!ScalarEqualityFunc()(n, Scalar(0)));
		return conjugate() / n;
	}

	Scalar dot(const Quaternion& other) const noexcept {
		return elements_.dot(other.elements_);
	}

	// --- ACCESSORS

	constexpr const ScalarPart s() const noexcept {
		return elements_.w();
	}

	ScalarPart& s() noexcept {
		return elements_.w();
	}

	const VectorPart v() const noexcept { // TODO: return view
		return elements_.xyz();
	}

	const Scalar x() const noexcept {
		return elements_.x();
	}

	Scalar& x() noexcept {
		return elements_.x();
	}

	const Scalar y() const noexcept {
		return elements_.y();
	}

	Scalar& y() noexcept {
		return elements_.y();
	}

	const Scalar z() const noexcept {
		return elements_.z();
	}

	Scalar& z() noexcept {
		return elements_.z();
	}

	const Scalar w() const noexcept {
		return elements_.w();
	}

	Scalar& w() noexcept {
		return elements_.w();
	}

private:

	using Elements = Vector<Scalar, 4, ScalarEqualityFunc>;

	Elements elements_;

};

template <class S, class SEF>
S dot(const Quaternion<S, SEF>& lhs, const Quaternion<S, SEF>& rhs) noexcept {
	return lhs.dot(rhs);
}

using Quat = Quaternion<float>;

} // namespace math

using math::Quaternion;
using math::Quat;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_QUATERNION_HPP_ */


#include <iostream>
#include <filesystem>
#include <Eigen/Eigenvalues>
#include <fstream>
#include "SpectraAnalysisLinearElements.h"
#include "../PhysicalModel/ExternalForces.h"

SpectraAnalysisLinearElements::SpectraAnalysisLinearElements(const SimParameters& params, Eigen::VectorXd& q0, Eigen::VectorXd& v0, const LinearElements& model)
{
	params_ = params;
	q0_ = q0;
	v0_ = v0;
	numSpectras_ = params.numSpectra;

	if (numSpectras_ > q0.size())
		numSpectras_ = q0.size();

	model_ = model;

	initialization();
}

void SpectraAnalysisLinearElements::initialization()
{
	// form mass matrix and its inverse
	std::vector<Eigen::Triplet<double>> T, T1;
	for (int i = 0; i < model_.massVec_.size(); i++)
	{
		T.push_back({ i, i, model_.massVec_(i) });
		T1.push_back({ i, i, 1.0 / model_.massVec_(i) });
	}

	massMat_.resize(q0_.size(), q0_.size());
	massInvMat_.resize(q0_.size(), q0_.size());

	massMat_.setFromTriplets(T.begin(), T.end());
	massInvMat_.setFromTriplets(T1.begin(), T1.end());

	// compute the eigen modes
	model_.computeHessian(q0_, K_);

	Eigen::VectorXd g;
	model_.computeGradient(q0_, g);
	b_ = g - K_ * q0_;

	// check the energy
	Eigen::VectorXd testQ = q0_;
	testQ.setZero();
	double E0 = model_.computeEnergy(testQ);
	double E = model_.computeEnergy(q0_);
	double E1 = 0.5 * q0_.dot(K_ * q0_) + q0_.dot(b_) + E0;

	std::cout << "E = " << E << ", E1 = " << E1 << ", error = " << E1 - E << std::endl;

	testQ.setRandom();
	E = model_.computeEnergy(testQ);
	E1 = 0.5 * testQ.dot(K_ * testQ) + testQ.dot(b_) + E0;
	std::cout << "E = " << E << ", E1 = " << E1 << ", error = " << E1 - E << std::endl;

	if (numSpectras_ < q0_.size())
	{
		Spectra::SparseSymMatProd<double> opK(K_);
		Spectra::SparseCholesky<double> opM(massMat_);

		Spectra::SymGEigsSolver<Spectra::SparseSymMatProd<double>, Spectra::SparseCholesky<double>, Spectra::GEigsMode::Cholesky> eigs(opK, opM, numSpectras_, (2 * numSpectras_ > q0_.size()) ? q0_.size() : 2 * numSpectras_);

		eigs.init();
		int nconv = eigs.compute(Spectra::SortRule::LargestMagn);
		if (eigs.info() == Spectra::CompInfo::Successful)
		{
			std::cout << eigs.eigenvalues() << std::endl;
			eigenValues_ = eigs.eigenvalues();
			eigenVecs_ = eigs.eigenvectors();
		}
		else
		{
			std::cerr << "error in t computing the eigen values of the M^{-1} K " << std::endl;
			exit(1);
		}
	}

	else
	{
		int halfNum = numSpectras_ / 2;
		Spectra::SparseSymMatProd<double> opK(K_);
		Spectra::SparseCholesky<double> opM(massMat_);

		Spectra::SymGEigsSolver<Spectra::SparseSymMatProd<double>, Spectra::SparseCholesky<double>, Spectra::GEigsMode::Cholesky> eigs(opK, opM, halfNum, numSpectras_);

		eigs.init();
		eigs.compute(Spectra::SortRule::LargestMagn);

		int leftNum = numSpectras_ - halfNum;

		using OpType = Spectra::SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
		using BOpType = Spectra::SparseSymMatProd<double>;
		OpType op(K_, massMat_);
		BOpType Bop(massMat_);

		Spectra::SymGEigsShiftSolver<OpType, BOpType, Spectra::GEigsMode::ShiftInvert> eigs1(op, Bop, leftNum, numSpectras_, 0.0);
		eigs1.init();
		eigs1.compute(Spectra::SortRule::LargestMagn);

		if (eigs.info() == Spectra::CompInfo::Successful && eigs1.info() == Spectra::CompInfo::Successful)
		{
			std::cout << eigs.eigenvalues() << std::endl;
			std::cout << eigs1.eigenvalues() << std::endl;

			eigenValues_.resize(numSpectras_);
			eigenValues_.segment(0, halfNum) = eigs.eigenvalues();
			eigenValues_.segment(halfNum, leftNum) = eigs1.eigenvalues();

			eigenVecs_.resize(q0_.size(), numSpectras_);
			eigenVecs_.block(0, 0, q0_.size(), halfNum) = eigs.eigenvectors();
			eigenVecs_.block(0, halfNum, q0_.size(), leftNum) = eigs1.eigenvectors();
		}
		else
		{
			// super slow when size of q is large
			Eigen::GeneralizedEigenSolver<Eigen::MatrixXd> ges;
			ges.compute(K_.toDense(), massMat_.toDense());
			eigenValues_ = ges.eigenvalues().real();
			eigenVecs_ = ges.eigenvectors().real();
		}

	}

	// check the othonormality
	Eigen::MatrixXd idMat = Eigen::MatrixXd::Identity(numSpectras_, numSpectras_);
	std::cout << "error: " << (eigenVecs_.transpose() * massMat_ * eigenVecs_ - idMat).norm() << std::endl;

	// compute initial alphas and betas
	curAlphaBeta_.resize(numSpectras_);
	preAlphaBeta_.resize(numSpectras_);
	initialAlphaBeta_.resize(numSpectras_);
	curAlphaBetaTheo_.resize(numSpectras_);

	for (int i = 0; i < numSpectras_; i++)
	{
		double alpha = eigenVecs_.col(i).dot(massMat_ * q0_);
		double beta = eigenVecs_.col(i).dot(massMat_ * v0_);
		initialAlphaBeta_[i] << alpha, beta;
		preAlphaBeta_[i] = initialAlphaBeta_[i];
		curAlphaBeta_[i] = initialAlphaBeta_[i];
		curAlphaBetaTheo_[i] = initialAlphaBeta_[i];
	}

	// compute the constant part
	/*cis_.resize(numSpectras_);

	for (int i = 0; i < numSpectras_; i++)
	{
		double value = eigenVecs_.col(i).dot(b_);
		cis_[i] = value;
	}*/

	// current time
	curTime_ = 0;

	updateCis();
}

void SpectraAnalysisLinearElements::updateCis()
{
	Eigen::VectorXd extForce = ExternalForces::externalForce(q0_, curTime_, params_.impulseMag, params_.impulsePow);
	Eigen::VectorXd c = b_ - extForce;

	// compute the constant part
	cis_.resize(numSpectras_);

	for (int i = 0; i < numSpectras_; i++)
	{
		double value = eigenVecs_.col(i).dot(c);
		cis_[i] = value;
	}
}

void SpectraAnalysisLinearElements::updateAlphasBetas()
{
	double h = params_.timeStep;
	updateCis();
	for (int i = 0; i < numSpectras_; i++)
	{
		if (params_.integrator == SimParameters::TI_IMPLICIT_EULER)
		{
			Eigen::Matrix2d A;
			A << 1, -h, eigenValues_[i] * h, 1;
			Eigen::Vector2d consVec;
			consVec << 0, -h * cis_[i];

			Eigen::Matrix2d Ainv = A.inverse();

			preAlphaBeta_[i] = curAlphaBeta_[i];
			curAlphaBeta_[i] = Ainv * (preAlphaBeta_[i] + consVec);
		}
		else if (params_.integrator == SimParameters::TI_NEWMARK)
		{
			Eigen::Matrix2d A;
			double NM_beta = params_.NM_beta;
			A << 1 + h * h * eigenValues_[i] * NM_beta, 0,
				eigenValues_[i] * h / 2.0, 1;
			Eigen::Vector2d consVec;

			Eigen::Matrix2d A1;
			A1 << 1 - h * h * eigenValues_[i] * (0.5 - NM_beta), h,
				-eigenValues_[i] * h / 2.0, 1;

			consVec << -h * h * cis_[i] / 2, -params_.timeStep * cis_[i];

			Eigen::Matrix2d Ainv = A.inverse();

			preAlphaBeta_[i] = curAlphaBeta_[i];
			curAlphaBeta_[i] = Ainv * (A1 * preAlphaBeta_[i] + consVec);
		}
		else if (params_.integrator == SimParameters::TI_TR_BDF2)
		{
			Eigen::Vector2d alphabeta;
			double gamma = params_.TRBDF2_gamma;
			double gamma2 = (1 - 2 * gamma) / (2 - 2 * gamma);
			double gamma3 = (1 - gamma2) / (2 * gamma);

			Eigen::Matrix2d A, A1, A2, Ainv;
			Eigen::Vector2d consVec;

			A << 1, -gamma * h, eigenValues_[i] * gamma* h, 1;
			Ainv = A.inverse();

			A1 << 1, gamma * h, -eigenValues_[i] * gamma* h, 1;

			consVec << 0, -2 * gamma * h * cis_[i];
			alphabeta = Ainv * (A1 * curAlphaBeta_[i] + consVec);

			A << 1, -gamma2 * h, eigenValues_[i] * gamma2 * h, 1;
			Ainv = A.inverse();
			consVec << 0, -gamma2 * h * cis_[i];

			A1 << gamma3, 0, 0, gamma3;
			A2 << 1 - gamma3, 0, 0, 1 - gamma3;

			preAlphaBeta_[i] = curAlphaBeta_[i];
			curAlphaBeta_[i] = Ainv * (A1 * alphabeta + A2 * preAlphaBeta_[i] + consVec);
		}
		else if (params_.integrator == SimParameters::TI_BDF2)
		{
			if (curTime_ == 0)	// use IE for the first step
			{
				Eigen::Matrix2d A;
				A << 1, -h, eigenValues_[i] * h, 1;
				Eigen::Vector2d consVec;
				consVec << 0, -h * cis_[i];

				Eigen::Matrix2d Ainv = A.inverse();

				preAlphaBeta_[i] = curAlphaBeta_[i];
				curAlphaBeta_[i] = Ainv * (preAlphaBeta_[i] + consVec);
			}
			else
			{
				Eigen::Vector2d alphabeta;

				Eigen::Matrix2d A, A1, A2, Ainv;
				Eigen::Vector2d consVec;

				A << 1, -2.0 / 3.0 * h, eigenValues_[i] * 2.0 / 3.0 * h, 1;
				Ainv = A.inverse();
				consVec << 0, -2.0 / 3.0 * h * cis_[i];

				A1 << 4.0 / 3.0, 0, 0, 4.0 / 3.0;
				A2 << -1.0 / 3.0, 0, 0, -1.0 / 3.0;

				alphabeta = Ainv * (A1 * curAlphaBeta_[i] + A2 * preAlphaBeta_[i] + consVec);

				preAlphaBeta_[i] = curAlphaBeta_[i];
				curAlphaBeta_[i] = alphabeta;
			}
		}
	}
	curTime_ += h;

	// compute the theoretical alpha beta
	for (int i = 0; i < numSpectras_; i++)
	{
		curAlphaBetaTheo_[i](0) = (initialAlphaBeta_[i](0) + cis_[i] / eigenValues_[i]) * std::cos(std::sqrt(eigenValues_[i]) * curTime_) - cis_[i] / eigenValues_[i];
		curAlphaBetaTheo_[i](1) = (initialAlphaBeta_[i](0) + cis_[i] / eigenValues_[i]) * std::sin(std::sqrt(eigenValues_[i]) * curTime_) * std::sqrt(eigenValues_[i]);
	}
}

void SpectraAnalysisLinearElements::getCurPosVel(Eigen::VectorXd& pos, Eigen::VectorXd& vel)
{
	if (curAlphaBeta_.size() != numSpectras_)
	{
		std::cerr << "mismatch in alpha beta vector size and number of spectras." << std::endl;
		exit(1);
	}

	pos.setZero(eigenVecs_.rows());
	vel.setZero(eigenVecs_.rows());

	for (int i = 0; i < numSpectras_; i++)
	{
		pos += curAlphaBeta_[i](0) * eigenVecs_.col(i);
		vel += curAlphaBeta_[i](1) * eigenVecs_.col(i);
	}
}

void SpectraAnalysisLinearElements::getTheoPosVel(Eigen::VectorXd& pos, Eigen::VectorXd& vel)
{
	if (curAlphaBetaTheo_.size() != numSpectras_)
	{
		std::cerr << "mismatch in alpha beta vector size and number of spectras." << std::endl;
		exit(1);
	}

	pos.setZero(eigenVecs_.rows());
	vel.setZero(eigenVecs_.rows());

	for (int i = 0; i < numSpectras_; i++)
	{
		pos += curAlphaBetaTheo_[i](0) * eigenVecs_.col(i);
		vel += curAlphaBetaTheo_[i](1) * eigenVecs_.col(i);
	}
}

void SpectraAnalysisLinearElements::saveInfo(std::string outputFolder)
{
	if (!std::filesystem::exists(outputFolder))
	{
		std::cout << "create directory: " << outputFolder << std::endl;
		if (!std::filesystem::create_directories(outputFolder))
		{
			std::cout << "create folder failed." << outputFolder << std::endl;
			exit(1);
		}
	}

	std::string evalsFileName = outputFolder + "evals.txt";
    if(curTime_ == 0)
    {
        std::ofstream  efs;
        efs.open(evalsFileName, std::ofstream::out);
        for (int i = 0; i < numSpectras_; i++)
        {
            efs << eigenValues_[i] << std::endl;
        }
    }

	for (int i = 0; i < numSpectras_; i++)
	{
		std::string alphafileName = outputFolder + "alpha_" + std::to_string(i) + ".txt";
		std::string theoAlphafileName = outputFolder + "alpha_theo_" + std::to_string(i) + ".txt";

		std::string betafileName = outputFolder + "beta_" + std::to_string(i) + ".txt";
		std::string theoBetafileName = outputFolder + "beta_theo_" + std::to_string(i) + ".txt";

		std::string extFfileName = outputFolder + "c_" + std::to_string(i) + ".txt";

		std::ofstream afs, atfs, bfs, btfs, cfs;

		if (curTime_ == 0)
		{
			afs.open(alphafileName, std::ofstream::out);
			atfs.open(theoAlphafileName, std::ofstream::out);

			bfs.open(betafileName, std::ofstream::out);
			btfs.open(theoBetafileName, std::ofstream::out);

			cfs.open(extFfileName, std::ofstream::out);
		}
		else
		{
			afs.open(alphafileName, std::ofstream::out | std::ofstream::app);
			atfs.open(theoAlphafileName, std::ofstream::out | std::ofstream::app);

			bfs.open(betafileName, std::ofstream::out | std::ofstream::app);
			btfs.open(theoBetafileName, std::ofstream::out | std::ofstream::app);

			cfs.open(extFfileName, std::ofstream::out | std::ofstream::app);
		}

		afs << curAlphaBeta_[i](0) << std::endl;
		atfs << curAlphaBetaTheo_[i](0) << std::endl;

		bfs << curAlphaBeta_[i](1) << std::endl;
		btfs << curAlphaBetaTheo_[i](1) << std::endl;

		cfs << cis_[i] << std::endl;
	}
}


#include <Eigen/Dense>
#include <iostream>
#include <mplot++/mplot++.h>
#include <tuple>

namespace ei = Eigen;
namespace mp = mplotpp;

template<class T>
std::tuple<Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>,
           Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>>
meshgrid(const Eigen::Array<T, Eigen::Dynamic, 1>& x,
         const Eigen::Array<T, Eigen::Dynamic, 1>& y)
{
  Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> X(y.size(), x.size());
  Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> Y(y.size(), x.size());
  for (ssize_t i = 0; i < X.rows(); ++i) {
    X.row(i) = x;
  }
  for (ssize_t j = 0; j < Y.cols(); ++j) {
    Y.col(j) = y;
  }

  return std::make_tuple(X, Y);
}

int
main()
{
  ei::ArrayXd x = mp::arange(-1.0, 1.01, 1.0);
  ei::ArrayXd y = mp::arange(-2.0, 2.01, 1.0);
  std::cout << "x = " << x.transpose() << std::endl;
  std::cout << "y = " << y.transpose() << std::endl;

  {
    ei::ArrayXXd X(y.size(), x.size());
    for (ssize_t i = 0; i < X.rows(); ++i) {
      X.row(i) = x;
    }
    std::cout << "X =\n" << X << std::endl;

    ei::ArrayXXd Y(y.size(), x.size());
    for (ssize_t j = 0; j < Y.cols(); ++j) {
      Y.col(j) = y;
    }
    std::cout << "Y =\n" << Y << std::endl;
  }

  {
    auto [X, Y] = meshgrid(x, y);
    std::cout << "X =\n" << X << std::endl;
    std::cout << "Y =\n" << Y << std::endl;
    std::cout << "X + 1 =\n" << (X + 1) << std::endl;
    auto Z1 = (-X.pow(2) - Y.pow(2)).exp();
    std::cout << "Z1 =\n" << Z1 << std::endl;
    auto Z2 = (-(X - 1).pow(2) - (Y - 1).pow(2)).exp();
    std::cout << "Z2 =\n" << Z2 << std::endl;
    auto Z = (Z1 - Z2) * 2;
    std::cout << "Z =\n" << Z << std::endl;
  }
}


#include <Engine/MeshEdit/ASAP.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>
#include <Eigen/LU>
#include <Eigen/SVD>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

bool ASAP::Iterate()
{
	iter_count_++;
	cout << iter_count_ << "th iteration" << endl;
	UpdatePara();
	UpdateTriMesh();
	return true;
}

void ASAP::UpdatePara()
{
	MatrixXf B(nV, 2);
	for (size_t i = 0; i < nV; i++)
	{
		B.row(i) = RowVector2f::Zero();
	}

	for (size_t t = 0; t < nT; t++)
	{
		auto triangle = heMesh->Polygons().at(t);
		auto edge = triangle->HalfEdge();
		Matrix<float, 3, 2> xt = flat_tri_[t];
		Matrix<float, 3, 2> ut = Matrix<float, 3, 2>();
		ut.row(0) = para_solution_.row(heMesh->Index(edge->Origin()));
		ut.row(1) = para_solution_.row(heMesh->Index(edge->End()));
		ut.row(2) = para_solution_.row(heMesh->Index(edge->Next()->End()));
		Matrix2f J = Jacobian(xt, ut);
		JacobiSVD<Matrix2f> svd(J, ComputeThinU | ComputeThinV);
		Matrix2f U = svd.matrixU();
		Matrix2f V = svd.matrixV();
		Vector2f A = svd.singularValues();
		Matrix2f A2 = Matrix2f::Zero();
		A2(0, 0) = (A(0) + A(1)) / 2;
		A2(1, 1) = (A(0) + A(1)) / 2;
		Matrix2f L = U * V;

		for (int k = 0; k < 3; k++, edge = edge->Next())
		{
			auto vi = edge->Origin();
			auto vj = edge->End();
			auto vk = edge->Next()->End();
			int i = heMesh->Index(vi);
			int j = heMesh->Index(vj);
			vecf3 eki = vi->pos - vk->pos;
			vecf3 ekj = vj->pos - vk->pos;
			float cot_theta = eki.cos_theta(ekj) / eki.sin_theta(ekj);
			//B.row(i) += (para_solution_.row(i) - para_solution_.row(j)) * V.transpose() * V * cot_theta;
			B.row(i) += (xt.row(k) - xt.row((k + 1) % 3)) * L * cot_theta;
			//B.row(j) += (para_solution_.row(j) - para_solution_.row(i)) * V.transpose() * V * cot_theta;
			B.row(j) += (xt.row((k + 1) % 3) - xt.row(k)) * L * cot_theta;
		}
	}

	B.row(start) = RowVector2f::Zero();
	B.row(end) = RowVector2f(1, 1);
	para_solution_ = para_solver_.solve(B);
}


#include <iostream>
#include "mtao/optimization/line_search.hpp"

#include <Eigen/Dense>

struct QuadraticFunc {
    using Scalar = double;
    using Vector = Eigen::VectorXd;

    Scalar objective(const Vector& p) const {
        return p.transpose() * (A * p + b) + c;
    }
    Vector gradient(const Vector& p) const {
        Vector ret = 2 * A * p + b;
        return ret;
    }

    //Vector descent_direction(const Vector& p) const { return -gradient(p).normalized(); }
    Vector descent_direction(const Vector& p) const {

        if(trinary) {
        return (-gradient(p)).unaryExpr([](double v) -> double {
                if(v > 0) { return 1; }
                else if(v < 0) { return -1; }
                else { return 0; }
                });
        } else {
            return -gradient(p).normalized();
        }

    }
    bool trinary = false;
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Scalar c;

};

using namespace mtao::optimization;

int main(int argc, char * argv[]) {
    std::cout << "Eigen threads: " << Eigen::nbThreads() << std::endl;
    QuadraticFunc func;
    int N = 600;
    Eigen::MatrixXd A(N,N);
    Eigen::VectorXd b(N);

    if constexpr(true) {
        A.setIdentity();
        b.setOnes(N);

        func.A = A.transpose() * A;
        func.b = -2 * A.transpose() * b;
        func.c = b.dot(b);
    } else {

        A.setRandom();
        b.setRandom();
        func.A = A.transpose() * A;
        func.b = b;
        func.c = 5;
    }

    auto run = [&](auto&& opt) {
        opt.set_position(b);
        std::cout << "Optimal energy: " << opt.objective() << std::endl;
        std::cout << "Optimal gradient norm: " << opt.gradient().norm() << std::endl;
        opt.set_position(Eigen::VectorXd::Random(N));
        std::cout << "Initial energy: " << opt.objective() << std::endl;
        int iters = opt.run();
        std::cout << "Final energy: " << opt.objective() << " in " << iters << " iterations."<< std::endl;
        std::cout << std::endl;
        //std::cout << "position: " << opt.position().transpose() << std::endl;
    };
    auto run_run = [&]() {
    {
        std::cout << "Backtracking / armijo conditions" << std::endl;
        auto opt = make_backtracking_line_search(func);
        run(opt);
    }
    {
        std::cout << "weak wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_weak();
        run(opt);
    }
    {
        std::cout << "Strong wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_strong();
        run(opt);
    }
    };
    run_run();
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
    func.trinary = true;
    std::cout << "Using a `trinary` which is lazy for something in the roughly right direction" << std::endl;
    run_run();
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

////////////////////////////////////////////////////////////////////////////////
/// @file
/// Solution of the Euler Problem #3
/// (link).
////////////////////////////////////////////////////////////////////////////////

#include <stapl/utility/do_once.hpp>
#include <stapl/array.hpp>
#include <stapl/views/array_view.hpp>
#include <stapl/algorithm.hpp>
#include <boost/lexical_cast.hpp>

////////////////////////////////////////////////////////////////////////////////
/// @brief Returns truncated integer square root of given number.
/// @tparam IntType Integral type of the number.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
inline IntType sqrt_int(IntType n)
{
  return static_cast<IntType>( std::sqrt(n) );
}

////////////////////////////////////////////////////////////////////////////////
/// @brief Functor that returns the larger of @p n and @p x/n that is both a
///        factor of @p x and a prime number (0 if neither satisfies these
///        conditions).
/// @tparam IntType Integral type of the dividend @p x.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
struct largest_prime_factors_filter
{
  static_assert(std::is_integral<IntType>::value, "Integer required.");

  largest_prime_factors_filter(IntType x)
    : m_x(x)
  { }

  IntType operator()(IntType n)
  {
    if ((m_x % n) != 0)   // n is not a factor of x
      return 0;

    if (is_prime(m_x/n))  // x/n (which is larger than n) is prime
      return m_x/n;

    return is_prime(n) ? n : 0;
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_x);
  }

private:
  IntType m_x;

  static bool is_prime(IntType n)
  {
    IntType sqrt_n = sqrt_int(n);

    for (IntType i = 2; i <= sqrt_n; ++i)
    {
      if ((n % i) == 0)
        return false;
    }

    return true;
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  using value_t = std::size_t;

  if (argc < 2)
  {
    stapl::do_once( [] {
      std::cout << "Run as: mpirun -n <ncpu> pe_3a <number>" << std::endl;
    });
    return EXIT_FAILURE;
  }

  stapl::counter<stapl::default_timer> exec_timer;
  exec_timer.start();

  // Read the dividend from command line.
  value_t num = boost::lexical_cast<value_t> (argv[1]);

  // Create array container for storage and a view into it.
  stapl::array<value_t> a(sqrt_int(num)-1);
  auto a_vw = stapl::make_array_view(a);

  // Fill the array with consecutive values from 2 to sqrt(num).
  stapl::iota(a_vw, 2);

  // Replace numbers in the array that are either not factors of num or not
  // prime by 0; also replace any prime factor x by the greater value num/x if
  // the latter is also prime.
  stapl::transform(a_vw, a_vw, largest_prime_factors_filter<value_t>(num));

  // Compute and print the maximum of all numbers in the array.
  value_t max_prime_factor = stapl::max_value(a_vw);

  double t = exec_timer.stop();

  stapl::do_once( [&] {
    std::cout << "Computation finished (time taken: " << t << " s)."
              << std::endl
              << "Largest prime factor of " << num << " is: "
              << max_prime_factor << std::endl;
  });

  return EXIT_SUCCESS;
}


#include <Eigen>

#include "warp.h"

void llcv_calc_persp_transform(float *matrixData, int matrixDataSize, bool rowMajor, const vector<cv::Point> sourcePoints, const vector<cv::Point> destPoints) {

  // Set up matrices a and b so we can solve for x from ax = b
  // See http://xenia.media.mit.edu/~cwren/interpolator/ for a
  // good explanation of the basic math behind this.

  typedef Eigen::Matrix<float, 8, 8> Matrix8x8;
  typedef Eigen::Matrix<float, 8, 1> Matrix8x1;

  Matrix8x8 a;
  Matrix8x1 b;

  for(int i = 0; i < 4; i++) {
    a(i, 0) = sourcePoints[i].x;
    a(i, 1) = sourcePoints[i].y;
    a(i, 2) = 1;
    a(i, 3) = 0;
    a(i, 4) = 0;
    a(i, 5) = 0;
    a(i, 6) = -sourcePoints[i].x * destPoints[i].x;
    a(i, 7) = -sourcePoints[i].y * destPoints[i].x;

    a(i + 4, 0) = 0;
    a(i + 4, 1) = 0;
    a(i + 4, 2) = 0;
    a(i + 4, 3) = sourcePoints[i].x;
    a(i + 4, 4) = sourcePoints[i].y;
    a(i + 4, 5) = 1;
    a(i + 4, 6) = -sourcePoints[i].x * destPoints[i].y;
    a(i + 4, 7) = -sourcePoints[i].y * destPoints[i].y;

    b(i, 0) = destPoints[i].x;
    b(i + 4, 0) = destPoints[i].y;
  }

  // Solving ax = b for x, we get the values needed for our perspective
  // matrix. Table of options on the eigen site at
  // /dox/TutorialLinearAlgebra.html#TutorialLinAlgBasicSolve
  //
  // We use householderQr because it places no restrictions on matrix A,
  // is moderately fast, and seems to be sufficiently accurate.
  //
  // partialPivLu() seems to work as well, but I am wary of it because I
  // am unsure of A is invertible. According to the documenation and basic
  // performance testing, they are both roughly equivalent in speed.
  //
  // - @burnto

  Matrix8x1 x = a.householderQr().solve(b);

  // Initialize matrixData
  for (int i = 0; i < matrixDataSize; i++) {
    matrixData[i] = 0.0f;
  }
  int matrixSize = (matrixDataSize >= 16) ? 4 : 3;

  // Initialize a 4x4 eigen matrix. We may not use the final
  // column/row, but that's ok.
  Eigen::Matrix4f perspMatrix = Eigen::Matrix4f::Zero();

  // Assign a, b, d, e, and i
  perspMatrix(0, 0) = x(0, 0); // a
  perspMatrix(0, 1) = x(1, 0); // b
  perspMatrix(1, 0) = x(3, 0); // d
  perspMatrix(1, 1) = x(4, 0); // e
  perspMatrix(2, 2) = 1.0f;    // i

  // For 4x4 matrix used for 3D transform, we want to assign
  // c, f, g, and h to the fourth col and row.
  // So we use an offset for thes values
  int o = matrixSize - 3; // 0 or 1
  perspMatrix(0, 2 + o) = x(2, 0); // c
  perspMatrix(1, 2 + o) = x(5, 0); // f
  perspMatrix(2 + o, 0) = x(6, 0); // g
  perspMatrix(2 + o, 1) = x(7, 0); // h
  perspMatrix(2 + o, 2 + o) = 1.0f; // i

  // Assign perspective matrix to our matrixData buffer,
  // swapping row versus column if needed, and taking care not to
  // overflow if user didn't provide a large enough matrixDataSize.
  for(int c = 0; c < matrixSize; c++) {
    for(int r = 0; r < matrixSize; r++) {
      int index = rowMajor ? (c + r * matrixSize) : (r + c * matrixSize);
      if (index < matrixDataSize) {
        matrixData[index] = perspMatrix(r, c);
      }
    }
  }
  // TODO - instead of copying final values into matrixData return array, do one of:
  // (a) assign directly into matrixData, or
  // (b) use Eigen::Mat so that assignment goes straight into underlying matrixData
}

void llcv_unwarp(const Mat& input, const vector<cv::Point>& source_points, const cv::Rect& to_rect, Mat& output)
{
    float matrix[16];
	vector<cv::Point> dest_points;
    dest_points.push_back(cv::Point(to_rect.x, to_rect.y));
    dest_points.push_back(cv::Point(to_rect.x + to_rect.width, to_rect.y));
    dest_points.push_back(cv::Point(to_rect.x, to_rect.y + to_rect.height));
    dest_points.push_back(cv::Point(to_rect.x + to_rect.width, to_rect.y + to_rect.height));

    // Calculate row-major matrix
    llcv_calc_persp_transform(matrix, 9, true, source_points, dest_points);
    Mat cv_persp_mat = Mat(3, 3, CV_32FC1);
    for (int r = 0; r < 3; r++) {
        float* ptr = cv_persp_mat.ptr<float>(r);
        for (int c = 0; c < 3; c++) {
            ptr[c] =  matrix[3 * r + c];
        }
    }

    warpPerspective(input, output, cv_persp_mat, input.size(), CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS);
}


#pragma once
#include <Eigen/Core>
#include <numeric>

namespace mtao::linear_algebra {
    // M is matrix, B is initial vector, Q is the basis for the basis
    // N is the dimension of the subspace

    namespace internal {
        template <typename Derived, typename BDerived, typename QDerived, typename HDerived>
            auto arnoldi(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& b, Eigen::PlainObjectBase<QDerived>& Q, Eigen::PlainObjectBase<HDerived>& H) {
                const int N = H.rows() - 1;
                using Scalar = typename Derived::Scalar;
                constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
                using Vec = Eigen::Matrix<Scalar, Derived::RowsAtCompileTime, 1>;

                Q.setZero();
                H.setZero();
                Vec v;

                Q.col(0) = b.normalized();
                for(int j = 0; j <= N; ++j) {

                    auto q = Q.col(j);
                    v = M * q;
                    for(int k = 0; k <= j; ++k) {
                        auto l = Q.col(k);
                        const Scalar s = l.dot(v);
                        H(k,j) = s;
                        v -= s * l;
                    }
                    if(j < N) {
                        const Scalar n = H(j+1,j) = v.norm();
                        if(n > eps) {
                            v /= n;
                            Q.col(j+1) = v;
                            continue;
                        }
                    }
                    return;
                }
            }
    }

    template <typename Derived, typename BDerived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B, int N) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,Eigen::Dynamic>;
            using DMat = Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>;

            Mat Q(M.rows(),N);
            DMat H(N,N);

            internal::arnoldi(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <int N, typename Derived, typename BDerived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Eigen::Dynamic,N>;
            using HMat = Eigen::Matrix<Scalar,N,N>;

            Mat Q(M.rows(),N);
            HMat H(N,N);
            internal::arnoldi(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <typename Derived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M, int N) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return arnoldi(M,B,N);
        }
    template <int N, typename Derived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return arnoldi<N>(M,B);
        }
}