#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <complex>
#include <tuple>

#include <chrono>

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

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/splitting.h"
#include "vlasovpp/lagrange5.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"

struct iter_s {
  std::size_t iter;
  double dt;
  double current_time;
  double Lhfh;
  double LE;
};
/*
struct time_stages {
  std::chrono::duration<double> step;
  std::chrono::duration<double> stage_1;
  std::chrono::duration<double> stage_2;
  std::chrono::duration<double> stage_3;
  std::chrono::duration<double> stage_4;
  std::chrono::duration<double> stage_5;
};
*/
#define save(data,dir,suffix,x_y) {\
  std::stringstream filename; filename << #data << "_" << suffix << ".dat"; \
  std::ofstream of( dir / filename.str() );\
  std::transform( data.begin() , data.end() , std::ostream_iterator<std::string>(of,"\n") , x_y );\
  of.close();\
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

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

#define ping(X) std::cerr << __LINE__ << " " << #X << ":" << X << std::endl
int debug = 0;

auto
maxwellian ( double rho , double u , double T ) {
  //std::cout << rho << " " << u << " " << T << std::endl;
  //std::cout << rho/(std::sqrt(2.*math::pi<double>()*T)) << std::endl;
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main(int argc, char const *argv[])
{

  std::string p("config.init");
  if ( argc > 1 ) {
    p = argv[1];
  }
  auto c = config(p);
  c.name = "vhls";

  c.create_output_directory();
  std::cout << c << std::endl;
  std::ofstream oconfig( c.output_dir / "config.init" );
  oconfig << c << std::endl;
  oconfig.close();

/* --------------------------------------------------------------- */
  std::size_t Nx = c.Nx, Nv = c.Nv;

  // $(u_c,E,\hat{f}_h)$ and $f_h$
  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);
  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);
  ublas::vector<double> uc1(Nx) , uc2(Nx) , uc3(Nx) , uc4(Nx) , ucn(Nx),
                        E1 (Nx) , E2 (Nx) , E3 (Nx) , E4 (Nx) , En (Nx);
  complex_field<double,1> hfh1(boost::extents[Nv][Nx]) , hfh2(boost::extents[Nv][Nx]) ,
                          hfh3(boost::extents[Nv][Nx]) , hfh4(boost::extents[Nv][Nx]) ,
                          hfhn(boost::extents[Nv][Nx]) ;

  const double Kx = 0.5;
  // phase-space domain
  fh.range.v_min = -12.; fh.range.v_max = 12.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();

  // compute dx, dv
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/Nv;
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/Nx;

  double dt = 0.04; //0.5*fh.step.dv;

  // velocity and frequency
  ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  const double l = fh.range.x_max-fh.range.x_min;
  ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  // initial condition
  auto tb_M1 = maxwellian(0.5*c.alpha,c.ui,1.) , tb_M2 = maxwellian(0.5*c.alpha,-c.ui,1.);
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      //fh[k][i] = ( 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-c.ui)) + 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+c.ui)) )*(1.+0.04*std::cos(0.3*Xi(i)));
      //fh[k][i] = ( 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-c.ui)) + 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+c.ui)) )*(1.+0.04*std::cos(Kx*Xi(i)));

      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(&(fh[k][0]),&(fh[k][Nx-1])+1,&(hfh[k][0]));
  }
  fh.write( c.output_dir / "init_vhls.dat" );
/*
  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << fh.range.v_min << "\n";
  std::cout << "v_max: " << fh.range.v_max << "\n";
  std::cout << "x_min: " << fh.range.x_min << "\n";
  std::cout << "x_max: " << fh.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << fh.step.dx << "\n";
  std::cout << "dv: " << fh.step.dv << "\n";
  std::cout << "Tf: " << c.Tf << "\n";
  std::cout << "f_0: " << "\"tb\"" << "\n";
  std::cout << std::endl;
*/
  const double rho_c = 1.-c.alpha;
  // init E (electric field) with Poisson
  {
    poisson<double> poisson_solver(Nx,l);
    ublas::vector<double> rho(Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += rho_c; } // add (1-alpha) for cold particules
    E = poisson_solver(rho);
  }

  // monitoring data
  std::vector<iter_s> iterations; iterations.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<iter_s> success_iterations; success_iterations.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> ee;
  std::vector<double> Emax;
  std::vector<double> H;
  std::vector<double> times;

  //std::vector<time_stages> durations_s; durations_s.reserve(int(std::ceil(c.Tf/dt))+1);

  auto save_data = [&] ( std::string suffix ) {
    save(iterations,c.output_dir,suffix,[](auto const& it) { std::stringstream ss; ss << it.iter << " " << it.dt << " " << it.current_time << " " << it.Lhfh << " " << it.LE; return ss.str(); })
    save(success_iterations,c.output_dir,suffix,[](auto const& it) { std::stringstream ss; ss << it.iter << " " << it.dt << " " << it.current_time << " " << it.Lhfh << " " << it.LE; return ss.str(); })

    auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };
    save(ee,c.output_dir,suffix,dt_y);
    save(Emax,c.output_dir,suffix,dt_y);
    save(H,c.output_dir,suffix,dt_y);

    /*
    auto writer_times = [&,count=0] (auto const & t ) mutable {
      std::stringstream ss;
      ss<<times[count++]<<" "<<t.step.count()<<" "<<t.stage_1.count()<<" "<<t.stage_2.count()<<" "<<t.stage_3.count()<<" "<<t.stage_4.count()<<" "<<t.stage_5.count();
      return ss.str();
    };
    c << monitoring::data( "times_"+suffix+".dat" , durations_s , writer_times );
    */
  };

  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[36msave data...\033[0m\n";

    save_data("vhls_SIGINT");
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    fh.write( c.output_dir / "vp_vhls_SIGINT.dat" );
  });

  /*
  signal_handler::signal_handler<SIGINT>::function_handler = [&](int signal) {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[36msave data...\033[0m\n" ;

    save_data("vhls_SIGINT");
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][0])+c.Nx,&(fh[k][0])); }
    fh.write( c.output_dir / "vp_vhls_SIGINT.dat" );
  };
  signal_handler::signal_handler<SIGINT>::signal();
  */

  times.push_back(0);
  {
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
  }
  Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

  //U_type<double,1> U({uc,E,hfh};
  splitting<double,1> Lie( fh , l , rho_c );

  std::size_t i_t = 0;
  double current_time = 0.;
  const double alpha1=1./(4.-std::cbrt(4.)), alpha2=alpha1, alpha3=1./(1.-SQ(std::cbrt(4.)));

  const double g1 = alpha1, g2 = alpha1+alpha2;
  const double w1 = (g2*(1.-g2))/(g1*(g1-1.)-g2*(g2-1.)) , w2 = 1.-w1 , w3 = w2 , w4 = w1;

  while ( current_time < c.Tf ) {
    std::cout<<"\r ["<<std::setw(6)<<i_t<<"] "<< std::setw(8) << current_time << " (" << std::setw(9) << dt << ")"<<std::flush;
    /**
    // Strang classique
    Lie.phi_a(0.5*dt,uc,E,hfh);
    Lie.phi_b(0.5*dt,uc,E,hfh);
    Lie.phi_c(dt,uc,E,hfh);
    Lie.phi_b(0.5*dt,uc,E,hfh);
    Lie.phi_a(0.5*dt,uc,E,hfh);
    /**/

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfhn.origin() );
    std::copy(  E.begin() ,  E.end() ,  En.begin() );
    std::copy( uc.begin() , uc.end() , ucn.begin() );

    //auto start = std::chrono::high_resolution_clock::now();
    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    //auto end_s1 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh1.origin() );
    std::copy(  E.begin() ,  E.end() ,  E1.begin() );
    std::copy( uc.begin() , uc.end() , uc1.begin() );

    //auto start_s2 = std::chrono::high_resolution_clock::now();
    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    //auto end_s2 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh2.origin() );
    std::copy(  E.begin() ,  E.end() ,  E2.begin() );
    std::copy( uc.begin() , uc.end() , uc2.begin() );

    //auto start_s3 = std::chrono::high_resolution_clock::now();
    // Strang alpha3
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha3*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);
    //auto end_s3 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh3.origin() );
    std::copy(  E.begin() ,  E.end() ,  E3.begin() );
    std::copy( uc.begin() , uc.end() , uc3.begin() );

    //auto start_s4 = std::chrono::high_resolution_clock::now();
    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    //auto end_s4 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh4.origin() );
    std::copy(  E.begin() ,  E.end() ,  E4.begin() );
    std::copy( uc.begin() , uc.end() , uc4.begin() );

   //auto start_s5 = std::chrono::high_resolution_clock::now();
    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    //auto end_s5 = std::chrono::high_resolution_clock::now();

    //durations_s.push_back({ end_s5-start , end_s1-start , end_s2-start_s2 , end_s3-start_s3 , end_s4-start_s4 , end_s5-start_s5 });

    /**/

    double L_hfh = 0.;
    for ( auto k=0 ; k<Nv ; ++k ) {
      for ( auto i=0 ; i<Nx ; ++i ) {
        auto hfhtile_ik = -hfhn[k][i] + w1*(hfh1[k][i]+hfh4[k][i]) + w2*(hfh2[k][i]+hfh3[k][i]);
        L_hfh += SQ( std::abs( hfh[k][i]-hfhtile_ik ) )*fh.step.dx*fh.step.dv;
      }
    }
    L_hfh = std::sqrt(L_hfh);

    double L_E = 0.;
    for ( auto i=0 ; i<Nx ; ++i ) {
      double Etile_ik = -En[i] + w1*(E1[i]+E4[i]) + w2*(E2[i]+E3[i]);
      L_E += SQ( std::abs( E[i]-Etile_ik ) )*fh.step.dx;
    }
    L_E = std::sqrt(L_E);

    std::cout << " -- " << std::setw(10) << L_hfh << "        " << std::flush;

    //if ( std::abs(L_hfh - c.tol) <= c.tol ) // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    {
      // SAVE TIME STEP

      Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      double electric_energy = 0.;
      for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
      ee.push_back( std::sqrt(electric_energy) );
      double total_energy = energy(fh,E);
      {
        auto rhoh = fh.density();
        fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(&rhoh[0]);
        fft::spectrum_ hE(c.Nx); hE.fft(&E[0]);
        fft::spectrum_ hrhoc(c.Nx);
        hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
        for ( auto i=1 ; i<c.Nx ; ++i ) {
          hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
        }
        ublas::vector<double> rhoc (c.Nx,0.); hrhoc.ifft(&rhoc[0]);

        for ( auto i=0 ; i<c.Nx ; ++i ) {
          total_energy += rhoc[i]*uc[i]*uc[i];
        }
      }
      H.push_back( total_energy );

      current_time += dt;
      times.push_back( current_time );
      success_iterations.push_back( { i_t , dt , current_time , L_hfh } );
    }
    /*else {
      // REMAKE THE STEP
      std::copy( hfhn.origin() , hfhn.origin()+hfhn.num_elements() , hfh.origin() );
      std::copy( En.begin()  , En.end()  , E.begin() );
      std::copy( ucn.begin() , ucn.end() , uc.begin() );
    }*/

    iterations.push_back( { i_t , dt , current_time , L_hfh } );
    ++i_t;

    //dt = std::pow( c.tol/L_hfh , 0.25 )*dt;
    if ( current_time+dt > c.Tf ) { dt = c.Tf - current_time; }
  } // while current_time < c.Tf
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<<std::endl;

  std::ofstream of;
  std::size_t count = 0;

  /*
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };

  of.open( c.output_dir / "ee_vhls.dat" );
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "Emax_vhls.dat" );
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "H_vhls.dat" );
  std::transform( H.begin() , H.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();
  */
  save_data("vhls_suzuki");

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][Nx-1])+1,&(fh[k][0])); }
  fh.write( c.output_dir / "vp_vhls_suzuki.dat" );

  auto dx_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<< fh.step.dx*(count++) <<" "<<y; return ss.str(); };
  save(E,c.output_dir,"vhls",dx_y);
  save(uc,c.output_dir,"vhls",dx_y);

  return 0;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/left.hpp>
#include <boost/numeric/bindings/right.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    {
      size_type m=6, n=8;
      matrix A(m, m);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<m; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*ublas::prod(A, B)+beta*C);
      matrix C2(C);
      blas::symm(blas::left(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing boost::ublas containers\n"
		<< "using ublas (left multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (left multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
    {
      size_type m=6, n=8;
      matrix A(n, n);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*ublas::prod(B, A)+beta*C);
      matrix C2(C);
      blas::symm(blas::right(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing boost::ublas containers\n"
		<< "using ublas (right multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (right multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
  }
  {
    typedef std::complex<double> complex;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    {
      size_type m=6, n=8;
      matrix A(m, m);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<m; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*A*B+beta*C);
      matrix C2(C);
      blas::symm(blas::left(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing Eigen containers\n"
		<< "using Eigen (left multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (left multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
    {
      size_type m=6, n=8;
      matrix A(n, n);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*B*A+beta*C);
      matrix C2(C);
      blas::symm(blas::right(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing Eigen containers\n"
		<< "using Eigen (right multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (right multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
  }
  return EXIT_SUCCESS;
}


#include <iostream>
#include <cmath>
#include <vector>
#include <array>
#include <math.h>
#include "heatEquation.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void printQuadrature();
double f( double x);
double g (double x);
double h (double x);
const double M_PI = 2*acos(0);

int main(int argc, char* argv[])
{

SpaceMesh smesh;
//smesh.GenerateDefaultSpaceMesh();
//smesh.GloballyBisectSpaceMesh();
smesh.GenerateSpaceMesh({0, 0.15, 0.25, 0.5, 1});

//smesh.GloballyBisectSpaceMesh();
//smesh.GloballyBisectSpaceMesh();

TimeMesh tmesh;
tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize(), 2), 1.0);

HeatEquation heat;
heat.SetSpaceTimeMesh( smesh, tmesh, "soultion1.txt");
heat.Solve();

smesh.PrintSpaceNodes();
heat.PrintSolution();

}

Simple Heat Equation solver class/Driver for simple heat equation classes.cpp


#include <iostream>

#include "LinearStateSpaceModel.h"

#include <Eigen/Core>

int main()
{
	// Define the initial state of the system
	const int n = 2;
	const int q = 1;
	const int p = 1;
	float states[n] = { 1, 2 };
	float input[p] = { 0.6f };
	float state_mtx[n * n] = { 0, 2, -1, -3 };
	float input_mtx[n * p] = { 5, 0 };
	float output_mtx[q * n] = { 1, 0 };
	float feedforward_gain[q * p] = { 0 };

	// Convert input to eigen datatypes
	Eigen::Matrix<float, n, 1> x(states);
	Eigen::Matrix<float, p, 1> u(input);
	Eigen::Matrix<float, n, n, Eigen::RowMajor> A(state_mtx);
	Eigen::Matrix<float, n, p> B(input_mtx);
	Eigen::Matrix<float, q, n> C(output_mtx);
	Eigen::Matrix<float, q, p> D(feedforward_gain);

	// Create state space model
	auto ss = LinearStateSpaceModel<float, n, q, p>(A, B, C, D);
	ss.set_state(x);
	ss.set_input(u);

	// Iterate for 10 loops
	for (int i = 0; i < 10; i++)
	{
		// Calculate
		ss.propogate();

		// Output
		std::cout << "State Estimate @ t = " << (i + 1) << ": \n" << ss.x << std::endl;
		std::cout << "Output Estimate @ t = " << (i + 1) << ": " << ss.y << std::endl;
	}
}


#include <math.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <sstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>

#include "util.h"
#include "gestureDetection.h"

#define DTW_INFINITY 1e30

double maximum(double a, double b, double c) {
	if(a>b && a>c) {
		return a;
	}
	else if(b>a && b>c) {
		return b;
	}
	else return c;
}

double measure_deviation(std::vector<double> arr1, std::vector<double> arr2) {

	/*
		Function to find deviation/difference between two measured vectors having size = 3.

		@params:
		arr1	Input array 1
		arr2	Input array 2
	*/

	return maximum(std::fabs(arr1[0] - arr2[0]), std::fabs(arr1[1] - arr2[1]), std::fabs(arr1[2] - arr2[2]));
}

double minimum(double a, double b, double c) {
	if(a<b && a<c) {
		return a;
	}
	else if(b<a && b<c) {
		return b;
	}
	else return c;
}

double DTWScore(std::vector<std::vector<double> > arr1, std::vector<std::vector<double> > arr2) {

	/*
		Function to estimate how close two measurements are. The closeness is denoted by the score, which is computed
		using the DTW(Dynamic Time Warping) Algorithm. Lower the score, more close they are.

		@params:
		arr1	Input array 1
		arr2	Input array 2
	*/

	int m = arr1.size() - 1;
	int n = arr2.size() - 1;

	double DTW[m+1][n+1], dev;

	for(int i=1; i<=m; i++) {
		DTW[i][0] = DTW_INFINITY;
	}
	for(int j=1; j<=n; j++) {
		DTW[0][j] = DTW_INFINITY;
	}
	DTW[0][0] = 0;

	for(int i=1; i<=m; i++) {
		for(int j=1; j<=n; j++) {
			dev = measure_deviation(arr1[i], arr2[j]);
			DTW[i][j] = dev + minimum(DTW[i-1][j], DTW[i][j-1], DTW[i-1][j-1]);
		}
	}

	//DTW[m][n] is the score.
	return DTW[m][n];
}

void FixedBin::assign(int _size) {
	filled = 0;
	size = _size;
	bin.resize(_size);
}

void FixedBin::push(std::vector<double> vec) {
	if(filled == size) {
		for(int i=0;i<filled-1;i++) {
			bin[i] = bin[i+1];
		}
		bin[filled - 1] = vec;
	}
	else {
		bin.at(filled) = vec;
		++filled;
	}
}

int FixedBin::get_size() {
	return size;
}

int FixedBin::get_filled() {
	return filled;
}

void FixedBin::get(int pos, std::vector<double>& vec) {
	vec.resize(3);
	vec = bin.at(pos);
}

std::vector<std::vector<double> > FixedBin::clone() {
	std::vector<std::vector<double> > vec(filled);

	//vec.empty();
	//vec.resize(filled);
	for(int i=0;i<filled;i++) {
		vec.at(i) = bin.at(i);
	}
	return vec;
}

void FaceGesture::assign(int normal_size) {
	normal = new FixedBin();
	normal->assign(normal_size);
}


#pragma once
#pragma ide diagnostic ignored "modernize-use-nodiscard"
#pragma ide diagnostic ignored "NotImplementedFunctions"
#pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection"
#pragma ide diagnostic ignored "OCUnusedStructInspection"
#pragma ide diagnostic ignored "OCUnusedTypeAliasInspection"

#include <Eigen/Geometry>

namespace fvlam
{
// ==============================================================================
// Translate2 class
// ==============================================================================

  class Translate2
  {
  public:
    using MuVector = Eigen::Vector2d;
    using TangentVector = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, 1>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;

  private:
    MuVector t_;

  public:
    Translate2() :
      t_{MuVector::Zero()}
    {}

    explicit Translate2(MuVector t) :
      t_(std::move(t))
    {}

    Translate2(double x, double y) :
      t_(x, y)
    {}

    const auto &x() const
    { return t_.x(); } //
    const auto &y() const
    { return t_.y(); } //

    const auto &t() const
    { return t_; }

    MuVector mu() const
    { return t_; }

    template<class T>
    static Translate2 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Translate2 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    Translate2 operator+(const Translate2 &other) const
    {
      return Translate2(t_ + other.t_);
    }

    Translate2 operator*(double factor) const
    {
      return Translate2(t_ * factor);
    }
  };

// ==============================================================================
// Translate3 class
// ==============================================================================

  class Translate3
  {
  public:
    using MuVector = Eigen::Vector3d;
    using TangentVector = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, 1>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;

  private:
    MuVector t_;

  public:
    Translate3() :
      t_{MuVector::Zero()}
    {}

    explicit Translate3(MuVector t) :
      t_(std::move(t))
    {}

    Translate3(double x, double y, double z) :
      t_(x, y, z)
    {}

    const auto &x() const
    { return t_.x(); } //
    const auto &y() const
    { return t_.y(); } //
    const auto &z() const
    { return t_.z(); } //

    const auto &t() const
    { return t_; }

    MuVector mu() const
    { return t_; }

    template<class T>
    static Translate3 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Translate3 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    /// Exponential map at identity - create a translation from canonical coordinates \f$ [T_x,T_y,T_z] \f$
    static Translate3 Expmap(const TangentVector &x)
    { return Translate3(x); }

    /// Log map at identity - return the canonical coordinates \f$ [T_x,T_y,T_z] \f$ of this translation
    static TangentVector Logmap(const Translate3 &translate3)
    { return translate3.t_; }

    Translate3 cross(const Translate3 &v) const
    {
      return Translate3{t_.cross(v.t_)};
    }

    Translate3 operator+(const Translate3 &other) const
    {
      return Translate3(t_ + other.t_);
    }

    Translate3 operator*(double factor) const
    {
      return Translate3(t_ * factor);
    }
  };

// ==============================================================================
// Translate3WithCovariance class
// ==============================================================================

  class Translate3WithCovariance
  {
  public:
    using MuVector = Eigen::Matrix<double, Translate3::MuVector::MaxSizeAtCompileTime +
                                           Translate3::CovarianceMatrix::MaxSizeAtCompileTime, 1>;

  private:
    bool is_valid_;
    bool is_cov_valid_;
    Translate3 t_;
    Translate3::CovarianceMatrix cov_;

  public:
    Translate3WithCovariance() :
      is_valid_{false}, is_cov_valid_{false}, t_{}, cov_{Translate3::CovarianceMatrix::Zero()}
    {}

    explicit Translate3WithCovariance(Translate3 t) :
      is_valid_{true}, is_cov_valid_{false}, t_{std::move(t)}, cov_{Translate3::CovarianceMatrix::Zero()}
    {}

    Translate3WithCovariance(Translate3 t, Translate3::CovarianceMatrix cov) :
      is_valid_{true}, is_cov_valid_{true}, t_{std::move(t)}, cov_{std::move(cov)}
    {}

    auto is_valid() const
    { return is_valid_; }

    auto is_cov_valid() const
    { return is_cov_valid_; }

    const auto &t() const
    { return t_; }

    const auto &cov() const
    { return cov_; }

    template<class T>
    static Translate3WithCovariance from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    std::string to_string() const;

    bool equals(const Translate3WithCovariance &other, double tol = 1.0e-9, bool check_relative_also = true) const
    {
      return t_.equals(other.t_, tol, check_relative_also) &&
             Translate3::cov_equals(cov_, other.cov_, tol, check_relative_also);
    }
  };

// ==============================================================================
// Rotate3 class
// ==============================================================================

  class Rotate3
  {
  public:
    using MuVector = Eigen::Matrix<double, 3, 1>;
    using TangentVector = Eigen::Matrix<double, 3, 1>;
    using RotationMatrix = Eigen::Matrix<double, 3, 3>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;
    using Derived = Eigen::Quaterniond;

  private:
    Derived q_{Derived::Identity()};
    RotationMatrix debug_r_{RotationMatrix::Zero()};
    MuVector debug_xyz_{MuVector::Zero()};

    static MuVector xyz(const Derived &q);

  public:
    Rotate3() = default;

    explicit Rotate3(const Derived &q) :
      q_(q), debug_r_{q_.toRotationMatrix()}, debug_xyz_{xyz(q_)}
    {}

    explicit Rotate3(const RotationMatrix &rotation_matrix) :
      q_(rotation_matrix), debug_r_{q_.toRotationMatrix()}, debug_xyz_{xyz(q_)}
    {}

    static Rotate3 Rx(double x)
    { return Rotate3{Derived{Eigen::AngleAxisd{x, Eigen::Vector3d::UnitX()}}}; }

    static Rotate3 Ry(double y)
    { return Rotate3{Derived{Eigen::AngleAxisd{y, Eigen::Vector3d::UnitY()}}}; }

    static Rotate3 Rz(double z)
    { return Rotate3{Derived{Eigen::AngleAxisd{z, Eigen::Vector3d::UnitZ()}}}; }

    static Rotate3 RzRyRx(double x, double y, double z)
    {
      return Rotate3{Derived{Eigen::AngleAxisd{z, Eigen::Vector3d::UnitZ()}} *
                     Derived{Eigen::AngleAxisd{y, Eigen::Vector3d::UnitY()}} *
                     Derived{Eigen::AngleAxisd{x, Eigen::Vector3d::UnitX()}}};
    }

    static Rotate3 Ypr(double y, double p, double r)
    { return RzRyRx(r, p, y); }

    const auto &q() const
    { return q_; }

    RotationMatrix rotation_matrix() const
    { return q_.toRotationMatrix(); }

    MuVector xyz() const
    { return xyz(q_); }

    MuVector mu() const
    { return xyz(q_); }

    template<class T>
    static Rotate3 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Rotate3 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    Rotate3 inverse() const
    {
      return Rotate3(q_.inverse());
    }

    /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z] \f$
    static Rotate3 Expmap(const TangentVector &x);

    /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation
    static TangentVector Logmap(const Rotate3 &rotate3);

    struct ChartAtOrigin
    {
      static Rotate3 retract(const TangentVector &v)
      { return Expmap(v); } //
      static TangentVector local(const Rotate3 &r)
      { return Logmap(r); } //
    };

    Rotate3 compose(const Rotate3 &other) const
    { return *this * other; } //
    Rotate3 between(const Rotate3 &other) const
    { return (*this).inverse() * other; } //
    Rotate3 slerp(const Rotate3 &other, double t) const
    { return compose(Expmap(t * Logmap(between(other)))); } //

    Rotate3 retract(const TangentVector &v) const
    { return compose(ChartAtOrigin::retract(v)); } //
    TangentVector local_coordinates(const Rotate3 &other) const
    { return ChartAtOrigin::local(between(other)); } //

    Rotate3 operator*(const Rotate3 &other) const
    {
      return Rotate3(q_ * other.q_);
    }

    Translate3 operator*(const Translate3 &other) const
    {
      return Translate3(q_ * other.t());
    }
  };

// ==============================================================================
// Transform3 class
// ==============================================================================

  class Transform3
  {
  public:
    using MuVector = Eigen::Matrix<double,
      Rotate3::MuVector::MaxSizeAtCompileTime +
      Translate3::MuVector::MaxSizeAtCompileTime, 1>;
    using TangentVector = Eigen::Matrix<double,
      Rotate3::MuVector::MaxSizeAtCompileTime +
      Translate3::MuVector::MaxSizeAtCompileTime, 1>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;

  private:
    bool is_valid_;
    Rotate3 r_;
    Translate3 t_;

  public:
    Transform3() :
      is_valid_{false}, r_{}, t_{}
    {}

    Transform3(Rotate3 r, Translate3 t) :
      is_valid_{true}, r_(std::move(r)), t_(std::move(t))
    {}

    Transform3(double rx, double ry, double rz, double tx, double ty, double tz) :
      is_valid_{true}, r_{Rotate3::RzRyRx(rx, ry, rz)}, t_(Translate3{tx, ty, tz})
    {}

    explicit Transform3(const MuVector &mu) :
      is_valid_{true},
      r_(Rotate3::RzRyRx(mu(0), mu(1), mu(2))),
      t_(Translate3(mu(3), mu(4), mu(5)))
    {}

    const auto &r() const
    { return r_; }

    const auto &t() const
    { return t_; }

    auto is_valid() const
    { return is_valid_; }

    MuVector mu() const
    { return (MuVector() << r_.mu(), t_.mu()).finished(); }

    template<class T>
    static Transform3 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Transform3 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    Transform3 inverse() const
    {
      auto qi = r_.q().inverse();
      return Transform3(Rotate3(qi), Translate3(qi * -t_.t()));
    }

    /// Exponential map at identity - create a transform from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
    static Transform3 Expmap(const TangentVector &x);

    /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this transform
    static TangentVector Logmap(const Transform3 &transform3);

    struct ChartAtOrigin
    {
      static Transform3 retract(const TangentVector &v); //
      static TangentVector local(const Transform3 &pose); //
    };

    Transform3 compose(const Transform3 &other) const
    { return *this * other; } //
    Transform3 between(const Transform3 &other) const
    { return (*this).inverse() * other; } //

    Transform3 retract(const TangentVector &v) const
    { return compose(ChartAtOrigin::retract(v)); } //
    TangentVector local_coordinates(const Transform3 &other) const
    { return ChartAtOrigin::local(between(other)); } //

    Translate3 operator*(const Translate3 &other) const
    {
      return Translate3(r_ * other + t_);
    }

    Transform3 operator*(const Transform3 &other) const
    {
      return Transform3{r_ * other.r_, t_ + r_ * other.t_};
    }
  };

// ==============================================================================
// Transform3WithCovariance class
// ==============================================================================

  class Transform3WithCovariance
  {
  public:
    using MuVector = Eigen::Matrix<double, Transform3::MuVector::MaxSizeAtCompileTime +
                                           Transform3::CovarianceMatrix::MaxSizeAtCompileTime, 1>;

  private:
    bool is_cov_valid_;
    Transform3 tf_;
    Transform3::CovarianceMatrix cov_;

  public:
    Transform3WithCovariance() :
      is_cov_valid_{false}, tf_{}, cov_{Transform3::CovarianceMatrix::Zero()}
    {}

    explicit Transform3WithCovariance(Transform3 tf) :
      is_cov_valid_{false}, tf_(std::move(tf)), cov_(Transform3::CovarianceMatrix::Zero())
    {}

    Transform3WithCovariance(Transform3 tf, Transform3::CovarianceMatrix cov) :
      is_cov_valid_{true}, tf_(std::move(tf)), cov_(std::move(cov))
    {}

    auto is_valid() const
    { return tf_.is_valid(); }

    auto is_cov_valid() const
    { return is_cov_valid_; }

    const auto &tf() const
    { return tf_; }

    const auto &cov() const
    { return cov_; }

    template<class T>
    static Transform3WithCovariance from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    std::string to_string() const;

    bool equals(const Transform3WithCovariance &other, double tol = 1.0e-9, bool check_relative_also = true) const;
  };
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef GEOMETRY_CIRCLE_2_INTEGRATED_FLUX_HPP
#define GEOMETRY_CIRCLE_2_INTEGRATED_FLUX_HPP

#include "rjmcmc/geometry/Circle_2.hpp"
#include <boost/gil/image.hpp>
#include <boost/gil/extension/matis/float_images.hpp>

template<typename View>
void Add1CirclePoints(const View& view, double cx, double cy, double dx, double dy, double d, double & res, double & w)
{
//    typedef View::pixel_t pixel_t;
    typedef boost::gil::dev2n32F_pixel_t pixel_t;
	int i = (int) (cx + dx);
	int j = (int) (cy + dy);
	if(i<0 || j<0 || i>=view.width() || j>=view.height()) return;
	const pixel_t& grad = view(i,j);
	res += boost::gil::at_c<0>(grad) * dx + boost::gil::at_c<1>(grad) * dy;
	w   += d;
}

template<typename View>
void Add4CirclePoints(const View& view, double cx, double cy, double d, double & res, double & w)
{
	Add1CirclePoints(view, cx, cy, 0, d, d, res, w);
	Add1CirclePoints(view, cx, cy, 0,-d, d, res, w);
	Add1CirclePoints(view, cx, cy, d, 0, d, res, w);
	Add1CirclePoints(view, cx, cy,-d, 0, d, res, w);
}

template<typename View>
void Add8CirclePoints(const View& view, double cx, double cy, double dx, double dy, double & res, double & w)
{
	double d = sqrt(dx*dx+dy*dy);
	Add1CirclePoints(view, cx, cy, dx, dy, d, res, w);
	Add1CirclePoints(view, cx, cy,-dx, dy, d, res, w);
	Add1CirclePoints(view, cx, cy,-dx,-dy, d, res, w);
	Add1CirclePoints(view, cx, cy, dx,-dy, d, res, w);
	Add1CirclePoints(view, cx, cy, dy, dx, d, res, w);
	Add1CirclePoints(view, cx, cy,-dy, dx, d, res, w);
	Add1CirclePoints(view, cx, cy,-dy,-dx, d, res, w);
	Add1CirclePoints(view, cx, cy, dy,-dx, d, res, w);
}

template<typename OrientedImage, typename K>
double integrated_flux(const OrientedImage& v, const geometry::Circle_2<K> &c)
{
        typedef typename OrientedImage::view_t view_t;
    view_t view(v.view());
	int x0 = v.x0();
	int y0 = v.y0();

	double cx = c.center().x() - x0;
	double cy = c.center().y() - y0;
	double r  = geometry::radius(c);
	double res = 0., w = 0.;
	double dx = 0;
	double dy = r;
	double p = 3 - 2*r;
	Add4CirclePoints(view, cx, cy, dy, res, w);
	while (dx < dy) {
		if (p < 0) {
			p += 4*dx+6;
		} else {
			--dy;
			Add8CirclePoints(view, cx, cy, dx, dy, res, w);
			p += 4*(dx-dy)+10;
		}
		++dx;
		Add8CirclePoints(view, cx, cy, dx, dy, res, w);
	}
	if(w==0) return 0.;
	return (res * geometry::perimeter(c)) / w;
}

#endif // GEOMETRY_CIRCLE_2_INTEGRATED_FLUX_HPP


//
// Created by haohanwang on 3/25/16.
//

#include "LinearRegression.h"

#include <Eigen/Sparse>

//#include "ModelOptions.hpp"

using namespace Eigen;

LinearRegression::LinearRegression() {
    L1_reg = 0;
    L2_reg = 0;
};

LinearRegression::LinearRegression(const ModelOptions_t& options) {
    L1_reg = 0;
    L2_reg = 0;
}

void LinearRegression::setL1_reg(float l1) { L1_reg = l1; };

void LinearRegression::setL2_reg(float l2) { L2_reg = l2; };

float LinearRegression::cost() {
    return 0.5 * (y - X * beta).squaredNorm()/X.rows() + L1_reg * beta.cwiseAbs().sum() + L2_reg * beta.squaredNorm();
};

SparseMatrix<float> LinearRegression::derivative() {
    return ((-1.0 * X.transpose() * (y - X * beta)).array() + L1_reg * (beta.array() / beta.cwiseAbs().array()).sum() +
            L2_reg * beta.sum()).matrix();
};

SparseMatrix<float> LinearRegression::proximal_derivative() {
    return -1.0 * X.transpose() * (y - X * beta);
};

SparseMatrix<float> LinearRegression::proximal_operator(SparseMatrix<float> in, float lr) {
    if (L1_reg == 0 && L2_reg == 0){
        return in;
    }
    if (L1_reg != 0 && L2_reg == 0){
        VectorXf sign = ((in.array()>0).matrix()).cast<float>();//sign
        sign += -1.0*((in.array()<0).matrix()).cast<float>();
        in = ((in.array().abs()-lr*L1_reg).max(0)).matrix();//proximal
        return (in.array()*sign.array()).matrix();//proximal multipled back with sign
    }
    else if (L2_reg != 0){
        return in/(1+2*lr*L2_reg);
    }
    else{
        VectorXf sign = ((in.array()>0).matrix()).cast<float>();
        sign += -1.0*((in.array()<0).matrix()).cast<float>();
        in = ((in.array().abs()-lr*L1_reg).max(0)).matrix();
        in = in.array()*sign.array()/(1+2*lr*L2_reg);
        return in.matrix();
    }
}


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

#include <ndt_generic/eigen_utils.h>

#include <iostream>
#include <iomanip>

using namespace std;

int main()
{
    // Simple check of the normalization
    {
        Eigen::VectorXd x(6);
        x << 1, 2, 3, 0.1, 0.2, 2.7;

        std::cout << "x: " << x << std::endl;

        Eigen::Affine3d T = ndt_generic::vectorToAffine3d(x);

        std::cout << "T : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T.rotation() : " << T.rotation() << std::endl;

        x = ndt_generic::affine3dToVector(T);

        std::cout << "x: " << x << std::endl;

        std::cout << "------------------------- The two T matrices should be the same below ------------------" << std::endl;

        x(3) += M_PI;

        std::cout << "x : " << x << std::endl;
        T = ndt_generic::vectorToAffine3d(x);
        std::cout << "T : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T.rotation() : " << T.rotation() << std::endl;

        ndt_generic::normalizeEulerAngles6dVec(x);

        std::cout << "x : " << x << std::endl;
        T = ndt_generic::vectorToAffine3d(x);
        std::cout << "T : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T.rotation() : " << T.rotation() << std::endl;
    }

    // Test the fusion
    {
        std::cout << "---------------------------------------------" << std::endl;
        Eigen::Matrix3d covA;
        covA.setIdentity();

        Eigen::Matrix3d covB;
        covB.setIdentity();

        Eigen::Vector3d a(1,2,3);
        Eigen::Vector3d b(0,1,0);

        Eigen::Vector3d weighted = ndt_generic::getWeightedPoint(a,covA,b,covB);

        std::cout << "weighted : " << ndt_generic::getWeightedPoint(a,covA,b,covB);
        std::cout << "should be : 0.5, 1.5, 1.5" << std::endl;
        covB *= 10.;
        std::cout << "weighted : " << ndt_generic::getWeightedPoint(a,covA,b,covB);
        std::cout << "should be : 0.9091, 1.9091, 2.7273" << std::endl;
    }

    {
        std::cout << "---------------------------------------------" << std::endl;
        Eigen::Affine3d a = Eigen::Translation<double,3>(1,2,3)*
        Eigen::AngleAxis<double>(0.1,Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis<double>(0.2,Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis<double>(0.3,Eigen::Vector3d::UnitZ()) ;

        Eigen::Affine3d b = Eigen::Translation<double,3>(0,1,0)*
        Eigen::AngleAxis<double>(0.0,Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis<double>(0.1,Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis<double>(0.0,Eigen::Vector3d::UnitZ()) ;

        Eigen::MatrixXd covA(6,6);
        covA.setIdentity();

        Eigen::MatrixXd covB(6,6);
        covB.setIdentity();

        std::cout << "weighted : " << ndt_generic::affine3dToStringRPY(ndt_generic::getWeightedPose(a, covA, b, covB)) << std::endl;
    }

}


///////////////////////////////////////////////////////////////////////////////
// accumulator::statistics::percentage_effective_sample_size.hpp             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2009
#include <boost/parameter/binding.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/statistics/detail/importance_sampling/statistics/variance_of_mean_normalized.hpp>
#include <boost/accumulators/framework/depends_on.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace accumulator{
namespace impl
{

    // Var(w/c) = Var(w) / c^2, where c = mean(w)
    template<typename T>
    class percentage_effective_sample_size
            : public boost::accumulators::accumulator_base
        {
        typedef boost::accumulators::dont_care dont_care_;

        typedef tag::variance_of_mean_normalized tag_vmn_;
        typedef boost::accumulators::tag::accumulator tag_acc_;

        public:
        typedef T result_type;
        percentage_effective_sample_size(){}
        percentage_effective_sample_size(dont_care_){}
        void operator()(dont_care_)const{}

        template<typename Args>
        result_type result(const Args& args) const
        {

            typedef
                typename boost::parameter::binding<Args,tag_acc_>::type cref_;
            cref_ acc = args[boost::accumulators::accumulator];

            T vmn = accumulators::extract_result<tag_vmn_>(acc);
            return one / (one+vmn);
        }

        static const T one;
    };

    template<typename T>
    const T percentage_effective_sample_size<T>::one = static_cast<T>(1);

}//impl

///////////////////////////////////////////////////////////////////////////////
// tag::percentage_effective_sample_size
namespace tag
{
    struct percentage_effective_sample_size
      : boost::accumulators::depends_on<tag::variance_of_mean_normalized>
    {
      typedef statistics::detail::accumulator::impl
        ::percentage_effective_sample_size<boost::mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::percentage_effective_sample_size
namespace extract
{

  template<typename AccSet>
  typename
    boost::mpl::apply<
        AccSet,
        tag::percentage_effective_sample_size
    >::type::result_type
  percentage_effective_sample_size(AccSet const& acc){
    typedef tag::percentage_effective_sample_size the_tag;
    return boost::accumulators::extract_result<the_tag>(acc);
  }

}

using extract::percentage_effective_sample_size;

}// accumulator
}// detail
}// statistics
}// boost

#endif


/*
The MATLAB gateway function to BBFMM2D developed by Sivaram.
The code provides a O(N) solution to a kernel matrix-vector product.
Written by Judith Yue Li 10/02/2013
*/

#include <iostream>
#include "math.h"
#include "mex.h"
#include "matrix.h"
#include "environment.hpp"
#include "BBFMM2D.hpp"
#include <Eigen/Core>
#include "kernelfun.hpp"

using namespace Eigen;
using namespace std;
double pi 	=	4.0*atan(1.0);
extern void _main();

#define IS_REAL_2D_FULL_DOUBLE(P) (!mxIsComplex(P) && mxGetNumberOfDimensions(P) == 2 && !mxIsSparse(P) && mxIsDouble(P))
#define IS_REAL_SCALAR(P) (IS_REAL_2D_FULL_DOUBLE(P) && mxGetNumberOfElements(P) == 1)

// Pass location from matlab to C
void read_location(const mxArray* x, const mxArray* y, vector<Point>& location){
    unsigned long N;
    double *xp, *yp;
    N = mxGetM(x);
    xp = mxGetPr(x);
    yp = mxGetPr(y);
    for (unsigned long i = 0; i < N; i++){
        Point new_Point;
        new_Point.x = xp[i];
        new_Point.y = yp[i];
        location.push_back(new_Point);
    }
}

void mexFunction(int nlhs,mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // Macros for the output and input arguments
    #define QH_OUT          plhs[0]
    #define QHexact_OUT     plhs[1]
    #define x_IN            prhs[0]
    #define y_IN            prhs[1]
    #define H_IN            prhs[2]
    #define nCheb_IN        prhs[3]
    #define print_IN        prhs[4]

    unsigned long N;
    unsigned m;
    // Instruction
    char errmsg[1023 + 1];
    sprintf(errmsg,"Calling sequence is\n\tQH = %s(xloc,yloc,H,nCheb,PrintFlag); or\n\t[QH QHexact] = %s(xloc,yloc,H,nCheb,PrintFlag);\n", mexFunctionName(), mexFunctionName());

    // Argument Checking:
    // Check number of argument
    if(nrhs != 5) {
        mexPrintf(errmsg);
        mexErrMsgTxt("Wrong number of input arguments");
    }else if(nlhs > 2){
        mexErrMsgTxt("Too many output arguments");
    }

    if( !IS_REAL_2D_FULL_DOUBLE(x_IN)) {
        mexErrMsgTxt("Third input argument is not a real 2D full double array.");
    }
    if( !IS_REAL_2D_FULL_DOUBLE(y_IN)) {
        mexErrMsgTxt("Third input argument is not a real 2D full double array.");
    }
    if( !IS_REAL_2D_FULL_DOUBLE(H_IN)) {
        mexErrMsgTxt("Third input argument is not a real 2D full double array.");
    }
    if( !IS_REAL_SCALAR(nCheb_IN)){
        mexErrMsgTxt("nChebnotes must be a real double scalar");
    }
    if( mxGetM(x_IN)!= mxGetM(y_IN) || mxGetM(x_IN) != mxGetM(H_IN)){
        mexErrMsgTxt("The dimension of the input matrices is wrong");
    }

    //processing on input arguments
    N = mxGetM(H_IN); // get the first dimension of H
    m = mxGetN(H_IN); // get the second dimension of H
    unsigned short nChebNodes = *mxGetPr(nCheb_IN);
    bool print = *mxGetPr(print_IN);
    vector<Point> location;
    read_location(x_IN,y_IN,location);
    double *charges;
    charges = mxGetPr(H_IN);
    // Load data to local array using Eigen <Map>
    MatrixXd H = Map<MatrixXd>(charges, N, m); // Map<MatrixXd> H(charges,N,m);

    // Compute Fast matrix vector product
    // 1. Build Tree
    clock_t startBuild  = clock();
    H2_2D_Tree Atree(nChebNodes, charges, location, N, m, print); //Build the fmm tree
    clock_t endBuild = clock();

    double FMMTotalTimeBuild = double(endBuild-startBuild)/double(CLOCKS_PER_SEC);
    if(print)
        mexPrintf("\nTime taken for FMM(build tree) is: %.4g\n",FMMTotalTimeBuild);

    // 2.Calculateing potential
    clock_t startA = clock();
    // Create the output matrix
    QH_OUT = mxCreateDoubleMatrix(N, m, mxREAL);
    // Get a pointer to the real data in the output matrix
    double *QHp;
    QHp = mxGetPr(QH_OUT);
    myKernel A;
    A.calculate_Potential(Atree, QHp);
    clock_t endA = clock();
    double FMMTotalTimeA = double(endA-startA)/double(CLOCKS_PER_SEC);
    if(print){
        mexPrintf("\nTime taken for FMM(calculating potential) is: %.4g\n",FMMTotalTimeA);
        mexPrintf("\nTotal time taken for FMM is: %.4g\n",FMMTotalTimeA+FMMTotalTimeBuild);
    }

    /*///////////////////////////////
    // Compute exact covariance Q //
    ///////////////////////////////*/

    if(nlhs == 2){
    if(print){
        mexPrintf("\nStarting exact computation...\n");
    }
    clock_t start = clock();
    MatrixXd Q;
    A.kernel_2D(N, location, N, location, Q);// Q is initialized inside function A.kernel_2D
    clock_t end = clock();
    double exactAssemblyTime = double(end-start)/double(CLOCKS_PER_SEC);

    // Compute exact Matrix vector product
    start = clock();
    QHexact_OUT = mxCreateDoubleMatrix(N, m, mxREAL);
    double *QHexactp;
    QHexactp = mxGetPr(QHexact_OUT);
    Map<MatrixXd> QHT(QHexactp,N,m);
    QHT = Q*H;
    end = clock();
    double exactComputingTime = double(end-start)/double(CLOCKS_PER_SEC);
    if(print){
        mexPrintf("\nThe total exact computation time is: %.4g\n",exactAssemblyTime + exactComputingTime);
    }

    // Compute the difference
    MatrixXd QHfast = Map<MatrixXd>(QHp, N, m);
    MatrixXd error = QHfast - QHT;
    double absoluteError = error.norm();
    double relativeError = absoluteError/QHT.norm();
    if(print){
        mexPrintf("The relative difference is: %13.6E \n", relativeError);
    }

    }

    return;
}


#include "interface.h"

#include <cmath>
#include <cstdio>
#include <cstring>
#include <cstdlib>

#include <fstream>
#include <iostream>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>
#include <Eigen/IterativeLinearSolvers>

constexpr bool DEBUG = false;

using namespace Eigen;

constexpr int N = WIDTH * HEIGHT;

void print_total_velocities(int line, Grid *grid) {
    double total_sum_x = 0.0;
    double total_sum_y = 0.0;
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        total_sum_x += std::abs(grid->velocity_x[y][x]);
        total_sum_y += std::abs(grid->velocity_y[y][x]);
    }
    if (DEBUG) std::cout << "total at line " << line << ": " << total_sum_x << ", " << total_sum_y << std::endl;
}

#define PV do { if (DEBUG) print_total_velocities(__LINE__, grid); } while (false)

static SimParams params;
static SparseMatrix<double> laplacian;
static ConjugateGradient<SparseMatrix<double>, Lower|Upper, IncompleteCholesky<double>> solver;
// static SimplicialLDLT<SparseMatrix<double>> solver;

static void step(Grid *grid, const Grid *prev);

void sim_init_grid(Grid *grid) {
    std::memset(grid, 0, sizeof *grid);
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        grid->velocity_x[y][x] = 0.0;
        grid->velocity_y[y][x] = 0.0;
    }
    for (int y = 0; y < 20; y++) for (int x = 0; x < 20; x++) {
        grid->density[y + HEIGHT - 40][WIDTH / 2 - 10 + x] = 1.0;
        grid->temperature[y + HEIGHT - 40][WIDTH / 2 - 10 + x] = 20;
    }
}

inline int INDEX(int x, int y) { return x + y * WIDTH; }

bool is_valid(int x, int y) {
    if (x < 0 || x >= WIDTH) return false;
    if (y < 0 || y >= HEIGHT) return false;
    if (params.obstacle_enabled
     && x >= params.obstacle_xmin && x < params.obstacle_xmax
     && y >= params.obstacle_ymin && y < params.obstacle_ymax) {
        return false;
    }
    return true;
}

void sim_init() {
    solver.setMaxIterations(40);
    solver.setTolerance(1e-10);
    std::vector<Eigen::Triplet<double>> rows;
    rows.reserve(5 * N);
    // fill in Laplacian matrix
    int NEIGHBOR_OFFSETS[][2] = {
        {-1, 0},
        { 1, 0},
        { 0,-1},
        { 0, 1},
    };
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        if (!is_valid(x, y)) {
            rows.emplace_back(INDEX(x, y), INDEX(x, y), 1.0);
            continue;
        }
        int neighbor_count = 0;
        for (const int (&d)[2] : NEIGHBOR_OFFSETS) {
            int dx = x + d[0], dy = y + d[1];
            if (is_valid(dx, dy)) {
                rows.emplace_back(INDEX(x, y), INDEX(dx, dy), -1.0);
                neighbor_count += 1;
            }
        }
        // rows.emplace_back(INDEX(x, y), INDEX(x, y), neighbor_count);
        rows.emplace_back(INDEX(x, y), INDEX(x, y), neighbor_count);
    }
    laplacian.resize(N, N);
    laplacian.setFromTriplets(rows.begin(), rows.end());
    rows.clear();
    solver.compute(laplacian);
}

extern "C" {
    double glfwGetTime();
}

std::atomic<TimeNode *> timer_root(nullptr);

void sim_main() {
    TimeNode *prev_root = timer_root.exchange(nullptr);
    while (prev_root) {
        TimeNode *next = prev_root->next;
        delete prev_root;
        prev_root = next;
    }
    Grid *prev = grids.get_current(WRITER);
    while (running.load(std::memory_order_relaxed)) {
        // update FPS counter
        TimeNode *root = timer_root.load();
        TimeNode *node = new TimeNode();
        node->time = glfwGetTime();
        node->next = root;
        timer_root.store(node);
        Grid *next = grids.swap(WRITER);
        auto new_params = param_buf.swap(READER);
        if (new_params->updated) {
            params = *new_params;
            new_params->updated = false;
        }
        step(next, prev);
        next->updated = true;
        /*
        for (int y = 0; y < HEIGHT; y++) {
            for (int x = 0; x < WIDTH; x++) {
                std::cout << (next->temperature[y][x] > 10.0f ? 'A' : ' ');
            }
            std::cout << "\n";
        }
        std::cout << std::endl;
        */
        prev = next;
    }
}

template<int W, int H>
double querySafe(const double (&values)[H][W], int x, int y) {
    if (x < 0 || y < 0 || x >= W || y >= H) return 0.0;
    return values[y][x];
}

template<int W, int H>
double interpolate(const double (&values)[H][W], Vector2d position) {
    int ix = (int) std::floor(position(0)),
        iy = (int) std::floor(position(1));
    double fx = position(0) - ix, fy = position(1) - iy;
    double vx0 = querySafe(values, ix, iy  ) * (1.0 - fx) + querySafe(values, ix+1, iy  ) * fx;
    double vx1 = querySafe(values, ix, iy+1) * (1.0 - fx) + querySafe(values, ix+1, iy+1) * fx;
    return vx0 * (1.0 - fy) + vx1 * fy;
}

// estimate velocity at position by interpolating nearest neighbors
Vector2d interpolateVelocity(const Grid &grid, Vector2d position) {
    double vx = interpolate(grid.velocity_x, position + Vector2d(0.5, 0.0));
    double vy = interpolate(grid.velocity_y, position + Vector2d(0.0, 0.5));
    return Vector2d(vx, vy);
}

void process_forces(Grid *grid) {
    memset(grid->force_x, 0, sizeof grid->force_x);
    memset(grid->force_y, 0, sizeof grid->force_y);
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        grid->force_y[y][x] += -params.alpha * grid->density[y][x] + params.beta * grid->temperature[y][x];
    }
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++)
    {
      Vector2d vel_u = interpolateVelocity(*grid, Vector2d(x,y+1));
      Vector2d vel_d = interpolateVelocity(*grid, Vector2d(x,y-1));
      Vector2d vel_l = interpolateVelocity(*grid, Vector2d(x+1,y));
      Vector2d vel_r = interpolateVelocity(*grid, Vector2d(x-1,y));

      //Since 2-D we only need z direction of omega
      grid->vorticity[y][x] = 0.5 * (vel_r(1) - vel_l(1) - vel_u(0) + vel_d(0));
    }
    for (int y = 1; y < HEIGHT - 1; y++) for (int x = 1; x < WIDTH - 1; x++)
    {
      Vector2d N;
      //Used for normalization
      using std::abs;
      N(0) = 0.5 * (abs(grid->vorticity[y][x+1]) - abs(grid->vorticity[y][x-1]));
      N(1) = 0.5 * (abs(grid->vorticity[y+1][x]) - abs(grid->vorticity[y-1][x]));
      N /= N.norm() + 1e-5;
      grid->force_x[y][x] += N(1) * grid->vorticity[y][x] * params.epsilon;
      grid->force_y[y][x] -= N(0) * grid->vorticity[y][x] * params.epsilon;
    }
}

void apply_force(Grid *grid) {
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        grid->velocity_x[y][x] += params.timestep * interpolate(grid->force_x, Vector2d(x - 0.5, y));
        grid->velocity_y[y][x] += params.timestep * interpolate(grid->force_y, Vector2d(x, y - 0.5));
    }
}

void calculate_pressure(Grid *grid) {
    VectorXd b(N);
    double sum_b = 0.0;
    double dot_b = 0.0;
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        int n = INDEX(x, y);
        b(n) = -grid->velocity_x[y][x] + grid->velocity_x[y][x + 1]
             + -grid->velocity_y[y][x] + grid->velocity_y[y + 1][x];
        b(n) *= -1.0;
        sum_b += std::abs(b(n));
        dot_b += b(n);
    }
    if (DEBUG) std::cout << "sum of b's: " << sum_b << std::endl;
    if (DEBUG) std::cout << "dot b with 1: " << dot_b << std::endl;
    Map<VectorXd> pressureMap((double *) grid->pressure, N);
    VectorXd temp = solver.solve(b);
    if (DEBUG) std::cout << "solver error: " << ((laplacian * temp) - b).norm() << std::endl;
    // if (b.norm() < 1e-10) {
    //     temp.setZero();
    // }
    double sum_p = 0.0;
    for (int i = 0; i < N; i++) {
        sum_p += std::abs(temp(i));
    }
    if (DEBUG) std::cout << "sum of P: " << sum_p << std::endl;
    pressureMap = temp;
}

bool have_exported = false;

void step(Grid *grid, const Grid *prev) {
    *grid = *prev;
    PV;
    process_forces(grid);
    PV;
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        // update velocity to obtain u*
        grid->velocity_x[y][x] = interpolate(
            prev->velocity_x,
            Vector2d(x, y) - params.timestep *
                interpolateVelocity(*prev, Vector2d(x - 0.5, y))
        );
        grid->velocity_y[y][x] = interpolate(
            prev->velocity_y,
            Vector2d(x, y) - params.timestep *
                interpolateVelocity(*prev, Vector2d(x, y - 0.5))
        );
    }
    apply_force(grid);
    PV;
    calculate_pressure(grid);
    {
        double sum_div = 0.0;
        for (int y = 1; y < HEIGHT - 1; y++) for (int x = 1; x < WIDTH - 1; x++) {
            double div = -grid->velocity_x[y][x] + grid->velocity_x[y][x+1]
                         -grid->velocity_y[y][x] + grid->velocity_y[y+1][x];
            sum_div += std::abs(div);
            // if (std::fabs(div) > 1e-2) std::cout << "+++" << div << std::endl;
        }
        if (DEBUG) std::cout << "sum of div before: " << sum_div << std::endl;
    }
    for (int y = 1; y < HEIGHT; y++) for (int x = 1; x < WIDTH; x++) {
        // update velocity based on pressure
        grid->velocity_x[y][x] -= (grid->pressure[y][x] - grid->pressure[y][x - 1]);
        grid->velocity_y[y][x] -= (grid->pressure[y][x] - grid->pressure[y - 1][x]);
    }
    PV;
    {
        double sum_div = 0.0;
        for (int y = 1; y < HEIGHT - 1; y++) for (int x = 1; x < WIDTH - 1; x++) {
            double div = -grid->velocity_x[y][x] + grid->velocity_x[y][x+1]
                         -grid->velocity_y[y][x] + grid->velocity_y[y+1][x];
            sum_div += std::abs(div);
            // if (std::fabs(div) > 1e-2) std::cout << "+++" << div << std::endl;
        }
        if (DEBUG) std::cout << "sum of div after: " << sum_div << std::endl;
    }
    for (int i = 0; i <= WIDTH; i++) {
        grid->velocity_y[0][i] = 0;
        grid->velocity_y[HEIGHT][i] = 0;
    }
    for (int i = 0; i <= HEIGHT; i++) {
        grid->velocity_y[i][0] = 0;
        grid->velocity_y[i][WIDTH] = 0;
    }
    if (params.obstacle_enabled) {
        for (int y = params.obstacle_ymin; y <= params.obstacle_ymax; y++) {
            for (int x = params.obstacle_xmin; x <= params.obstacle_xmax; x++) {
                if (!(x == params.obstacle_xmin || x == params.obstacle_xmax - 1)) {
                    grid->velocity_y[y][x] = 0;
                }
                if (!(y == params.obstacle_ymin || y == params.obstacle_ymax - 1)) {
                    grid->velocity_x[y][x] = 0;
                }
            }
        }
    }
    // advect temperature and density
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        Vector2d pt = Vector2d(x, y) - params.timestep *
            interpolateVelocity(*grid, Vector2d(x, y));
        grid->density[y][x] = interpolate(prev->density, pt);
        if (grid->density[y][x] < 0.0) {
            std::cout << "NEGATIVE DENSITY!" << std::endl;
        }
        grid->temperature[y][x] = interpolate(prev->temperature, pt);
        // heat transfer
        double old = grid->temperature[y][x];
        grid->temperature[y][x] -= params.kappa * grid->temperature[y][x] * params.timestep;
        if (old * grid->temperature[y][x] < 0.0) {
            grid->temperature[y][x] = 0.0;
        }
    }
    PV;
    if (params.emitter_density > 1e-6) {
        for (int z = 0; z < 10; z++) {
            int y = 2;
            int x = (WIDTH - 10) / 2 + z;
            double d = grid->density[y][x];
            double dens = params.emitter_density;
            grid->density[y][x] += dens;
            grid->temperature[y][x] = (grid->temperature[y][x] * d + dens * params.emitter_temp) / grid->density[y][x];
            // grid->velocity_y[y][x] = 1.0;
        }
    }
    if (params.obstacle_enabled) {
        for (int y = params.obstacle_ymin; y < params.obstacle_ymax; y++) {
            for (int x = params.obstacle_xmin; x < params.obstacle_xmax; x++) {
                grid->pressure[y][x] = 0;
                grid->temperature[y][x] = 0;
                grid->density[y][x] = 0;
            }
        }
    }
    PV;
    if (params.want_to_export && !have_exported) {
        have_exported = true;
        std::ofstream f("out.csv");
        for (int y = 0; y <= HEIGHT; y++) for (int x = 0; x <= WIDTH; x++) {
            f << x << "," << y << "," << grid->velocity_x[y][x] << "," << grid->velocity_y[y][x] << std::endl;
        }
    }
}


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_DATA_MATRIX_HPP
#define UTILITIES_DATA_MATRIX_HPP

#include "Vector.hpp"
#include "../UtilitiesAPI.hpp"
#include "../core/Logger.hpp"

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

namespace openstudio{

  /// Matrix
  typedef boost::numeric::ublas::matrix<double> Matrix;

//////////////////////////////////////////////////////////////////////////
// Begin SWIG'able, copy and paste into Matrix.i
//////////////////////////////////////////////////////////////////////////

  /// new operators

  UTILITIES_API bool operator==(const Matrix& lhs, const Matrix& rhs);
  UTILITIES_API bool operator!=(const Matrix& lhs, const Matrix& rhs);

  /// common methods

  /// linear interpolation of the function v = f(x, y) at point xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API double interp(const Vector& x, const Vector& y, const Matrix& v, double xi, double yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, double yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Matrix& v, double xi, const Vector& yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API Matrix interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, const Vector& yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// matrix product
  UTILITIES_API Matrix prod(const Matrix& lop, const Matrix& rop);

  /// vector product
  UTILITIES_API Vector prod(const Matrix& m, const Vector& v);

  /// outer product
  UTILITIES_API Matrix outerProd(const Vector& lhs, const Vector& rhs);

  /// take the natural logarithm of Matrix elements, componentwise
  UTILITIES_API Matrix log(const Matrix& v);

  /// take the logarithm of Matrix elements with respect to base, componentwise
  UTILITIES_API Matrix log(const Matrix& v, double base);

  /// generates a M x N Matrix whose elements come from the uniform distribution on [a,b].
  UTILITIES_API Matrix randMatrix(double a, double b, unsigned M, unsigned N);

  /// sum of all elements
  UTILITIES_API double sum(const Matrix& matrix);

  /// maximum of all elements
  UTILITIES_API double maximum(const Matrix& matrix);

  /// minimum of all elements
  UTILITIES_API double minimum(const Matrix& matrix);

  /// mean of all elements
  UTILITIES_API double mean(const Matrix& matrix);

  /// get the connected components from an NxN adjacency matrix (1.0 for i-j connected, 0.0 for i-j not connected)
  UTILITIES_API std::vector<std::vector<unsigned> > findConnectedComponents(const Matrix& matrix);

  // from the boost vault:
  // The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipes in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.
  /// Matrix inversion routine, using lu_factorize and lu_substitute in uBLAS to invert a matrix */
 template<class T>
 bool invert(const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse) {

   // create a working copy of the input
   boost::numeric::ublas::matrix<T> A(input);

   // create a permutation matrix for the LU-factorization
   boost::numeric::ublas::permutation_matrix<std::size_t> pm(A.size1());

   // perform LU-factorization
   typename boost::numeric::ublas::matrix<T>::size_type res = boost::numeric::ublas::lu_factorize(A, pm);
   if( res != 0 ){
     LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_factorize returned res = " << res <<
                    ", A = " << A << ", pm = " << pm << " for input = " << input);
     return false;
   }

   // create identity matrix of "inverse"
   inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

   // backsubstitute to get the inverse
   try {
     boost::numeric::ublas::lu_substitute(A, pm, inverse);
   }catch (std::exception& e){
     LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_substitute threw exception '" << e.what() <<
                    "' for A = " << A << ", pm = " << pm);
     return false;
   }

   return true;
 }

} // openstudio

#endif //UTILITIES_DATA_MATRIX_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>

using namespace std;

template <typename MatrixA>
void test(MatrixA& A, unsigned dim1, unsigned dim2, const char* name)
{
    const unsigned max_print_size= 25;
    cout << "\n" << name << "\n";
    laplacian_setup(A, dim1, dim2);

    unsigned size= dim1 * dim2;
    mtl::dense_vector<double> v(size);
    for (unsigned i= 0; i < num_cols(A); i++)
	v[i]= A[12][i];

    // Resulting vector has same value type as matrix
    typedef typename mtl::Collection<MatrixA>::value_type rvalue_type;
    mtl::dense_vector<rvalue_type> w(size), w2;

    w= A * v;
    //mult(A, v, w);

    if (size <= max_print_size)
	cout << "A= \n" << A << "\n\nv= " << v << "\n\nA*v= " << w << "\n";

    // Same test as in matrix product: resulting vector corresponds to column 12
    // Check for stencil below in the middle of the matrix
    //        1
    //     2 -8  2
    //  1 -8 20 -8  1
    //     2 -8  2
    //        1
    if (dim1 == 5 && dim2 == 5) {
	rvalue_type twenty(20.0), two(2.0), one(1.0), zero(0.0), minus_eight(-8.0);
	MTL_THROW_IF(w[12] != twenty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(w[13] != minus_eight, mtl::runtime_error("wrong east neighbor"));
	MTL_THROW_IF(w[14] != one, mtl::runtime_error("wrong east east neighbor"));
	MTL_THROW_IF(w[15] != zero, mtl::runtime_error("wrong zero-element"));
	MTL_THROW_IF(w[17] != minus_eight, mtl::runtime_error("wrong south neighbor"));
	MTL_THROW_IF(w[18] != two, mtl::runtime_error("wrong south east neighbor"));
	MTL_THROW_IF(w[22] != one, mtl::runtime_error("wrong south south neighbor"));
    }

    w+= A * v;

    if (size <= max_print_size)
	cout << "w+= A*v= \n\n" << w << "\n";

    // Check for stencil, must be doubled now
    if (dim1 == 5 && dim2 == 5) {
	rvalue_type forty(40.0), four(4.0);
	MTL_THROW_IF(w[12] != forty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(w[18] != four, mtl::runtime_error("wrong south east neighbor"));
    }

    w-= A * v;

    if (size <= max_print_size)
	cout << "w-= A*v= \n\n" << w << "\n";

    // Check for stencil, must be A*v now
    if (dim1 == 5 && dim2 == 5) {
	rvalue_type twenty(20.0), two(2.0);
	MTL_THROW_IF(w[12] != twenty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(w[18] != two, mtl::runtime_error("wrong south east neighbor"));
    }

#if 0
    rvalue_type dotexp= dot(w, v), dotres;
    mtl::with_dot(w2, dotres)= A * v;

    w2-= w;
    if (two_norm(w2) > 0.001)
	throw "Vector result wrong in with_dot computation.";
    if (std::abs(dotres - dotexp) > 0.001)
	throw "Dot result wrong in with_dot computation.";
#endif
}

int main(int argc, char* argv[])
{
    using namespace mtl;

    unsigned dim1= 5, dim2= 5;

    if (argc > 2) {dim1= atoi(argv[1]); dim2= atoi(argv[2]);}
    unsigned size= dim1 * dim2;

    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);

    test(cr, dim1, dim2, "Row-major sparse");
#if 1
    test(cc, dim1, dim2, "Column-major sparse");

    test(dr, dim1, dim2, "Row-major dense");
    test(dc, dim1, dim2, "Column-major dense");
#endif
    return 0;
}


#ifndef DG_BASIS_HPP
#define DG_BASIS_HPP

#include <tuple>
#include <map>
#include <boost/math/special_functions/legendre.hpp>

namespace DGHydro {

  template<int nDim, int nDeg>
  class BasisFunctions {
  public:
    BasisFunctions() {
      total_number = 0;

      for (int i = 0; i <= nDeg; i++) {
        for (int j = 0; j <= nDeg*(nDim > 1); j++) {
          for (int k = 0; k <= nDeg*(nDim > 2); k++) {
            if (i + j + k <= nDeg) {
              basisMap.insert({total_number, std::make_tuple(i, j, k)});

              //std::cout << "Added " << i << " " << j << " " << k << std::endl;
              total_number++;
            }
          }
        }
      }
    };

    ~BasisFunctions(void) {};

    double operator()(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      return boost::math::legendre_p(a, x)*boost::math::legendre_p(b, y)*boost::math::legendre_p(c, z);
    }

    double x_derivative(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      double p_prime = a*(x*boost::math::legendre_p(a, x) -
                          boost::math::legendre_p(a - 1, x))/(x*x - 1.0);

      return p_prime*boost::math::legendre_p(b, y)*boost::math::legendre_p(c, z);
    }

    double y_derivative(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      double p_prime = b*(y*boost::math::legendre_p(b, y) -
                          boost::math::legendre_p(b - 1, y))/(y*y - 1.0);

      return boost::math::legendre_p(a, x)*p_prime*boost::math::legendre_p(c, z);
    }

    double z_derivative(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      double p_prime = c*(z*boost::math::legendre_p(c, z) -
                          boost::math::legendre_p(c - 1, z))/(z*z - 1.0);

      return boost::math::legendre_p(a, x)*boost::math::legendre_p(b, y)*p_prime;
    }

    int total_number;
  private:
    std::map<int, std::tuple<int, int, int>> basisMap;

  };

} // namespace DGHydro

#endif  // DG_BASIS_HPP