#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>

using namespace std::string_literals;

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

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

struct iter_s {
  std::size_t iter;
  double dt;
  double current_time;
  double Lhfh;
  double LE;
};

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

auto
maxwellian ( double rho , double u , double T ) {
  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[])
{
  const double sigma = 1.733;

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

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

/* --------------------------------------------------------------- */
  field<double,1> fh(boost::extents[c.Nv][c.Nx]);
  complex_field<double,1> hfh(boost::extents[c.Nv][c.Nx]);

  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/c.Nv;

  double Kx = 0.5;
  fh.range.x_min = 0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/c.Nx;

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

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

  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 ) {
      // tb
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
      //fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1.);
    }
    fft::fft(&(fh[k][0]),&(fh[k][c.Nx-1])+1,&(hfh[k][0]));
  }
  fh.write( c.output_dir / "init_vhll.dat" );

  iteration::iteration<double> iter;
  iter.iter = 0;
  iter.current_time = 0.;
  iter.dt = 0.5*fh.step.dv;

  ublas::vector<double> uc(c.Nx,0.);
  ublas::vector<double> E (c.Nx,0.);

  std::vector<double> ee;   ee.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> iterations;         iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> success_iterations; success_iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  auto save_data = [&] ( std::string && suffix ) {
    suffix = c.name + suffix;

    // save iterations informations
    auto writer_iter = [] ( auto const & it ) {
      std::stringstream ss; ss << it;
      return ss.str();
    };
    c << monitoring::data( "iterations_"+suffix+".dat"         , iterations         , writer_iter );
    c << monitoring::data( "success_iterations_"+suffix+".dat" , success_iterations , writer_iter );

    // save temporel data
    auto dt_y = [&,count=0] (auto const& y) mutable {
      std::stringstream ss; ss<<times[count++]<<" "<<y;
      return ss.str();
    };
    c << monitoring::data( "ee_"+suffix+".dat"   , ee   , dt_y );
    c << monitoring::data( "Emax_"+suffix+".dat" , Emax , dt_y );
    c << monitoring::data( "H_"+suffix+".dat"    , H    , dt_y );

    // save distribution function
    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_"+suffix+".dat") );

  };

  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("_SIGINT");
  });

  const double rho_c = 1.-c.alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);
  {
    poisson<double> poisson_solver(c.Nx,fh.range.len_x());
    ublas::vector<double> rho(c.Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<c.Nx ; ++i ) { rho[i] += (1.-c.alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    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);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
    times.push_back( 0. );
  }

  // initialize memory for all temporary variables
  ublas::vector<double> J(c.Nx);
  fft::spectrum_ d(c.Nx);
  field<double,1> Edvf(tools::array_view<const std::size_t>(fh.shape(),2));
  ublas::vector<double> uc1(c.Nx) , uc2(c.Nx) , uc3(c.Nx) , uc4(c.Nx) , uc5(c.Nx) , uc6(c.Nx) , uc7(c.Nx),
                        E1(c.Nx)  , E2(c.Nx)  , E3(c.Nx)  , E4(c.Nx)  , E5(c.Nx)  , E6(c.Nx)  , E7(c.Nx);
  complex_field<double,1> hfh1(boost::extents[c.Nv][c.Nx]),hfh2(boost::extents[c.Nv][c.Nx]),hfh3(boost::extents[c.Nv][c.Nx]),hfh4(boost::extents[c.Nv][c.Nx]),hfh5(boost::extents[c.Nv][c.Nx]),hfh6(boost::extents[c.Nv][c.Nx]),hfh7(boost::extents[c.Nv][c.Nx]);

  while (  iter.current_time < c.Tf ) {
    std::cout << "\r" << iteration::time(iter) << std::flush;

/////////////////////////////////////////////////////////////////////
/**
/////////////////////////////////////////////////////////////////////
    // RK(3,3) classical

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c1 = std::cos(dt*sqrt_rho_c), s1 = std::sin(dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - dt*J[i]*s1/sqrt_rho_c;
        E1[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - dt*J[i]*c1;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) - dt*d[i]*std::exp(-I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c2 = std::cos(0.5*dt*sqrt_rho_c), s2 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] =  0.75*uc[i]*c2 + 0.75*E[i]*s2/sqrt_rho_c + 0.25*uc1[i]*c2 - 0.25*E1[i]*s2/sqrt_rho_c + 0.25*dt*J[i]*s2/sqrt_rho_c;
        E2[i]  = -0.75*uc[i]*s2*sqrt_rho_c + 0.75*E[i]*c2 + 0.25*uc1[i]*s2*sqrt_rho_c + 0.25*E1[i]*c2 - 0.25*dt*J[i]*c2;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = 0.75*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*0.5*dt) + 0.25*hfh1[k][i]*std::exp(I*kx[i]*Vk(k)*0.5*dt) - 0.25*dt*d[i]*std::exp(I*kx[i]*Vk(k)*0.5*dt);
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1 = std::cos(dt*sqrt_rho_c), s1 = std::sin(dt*sqrt_rho_c);
      double c2 = std::cos(0.5*dt*sqrt_rho_c), s2 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        double tmp_uc =  (1./3.)*uc[i]*c1 + (1./3.)*E[i]*s1/sqrt_rho_c + (2./3.)*uc2[i]*c2 + (2./3.)*E2[i]*s2/sqrt_rho_c - (2./3.)*dt*J[i]*s2/sqrt_rho_c;
        E[i]          = -(1./3.)*uc[i]*s1*sqrt_rho_c + (1./3.)*E[i]*c1 - (2./3.)*uc2[i]*s2*sqrt_rho_c + (2./3.)*E2[i]*c2 - (2./3.)*dt*J[i]*c2;
        uc[i] = tmp_uc;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh[k][i] = (1./3.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) + (2./3.)*hfh2[k][i]*std::exp(-I*kx[i]*Vk(k)*0.5*dt) - (2./3.)*dt*d[i]*std::exp(-I*kx[i]*Vk(k)*0.5*dt);
        }
      }
    } // end stage 3

/////////////////////////////////////////////////////////////////////
**
/////////////////////////////////////////////////////////////////////
    // RK(3,3) NSSP

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c49 = std::cos((4./9.)*dt*sqrt_rho_c), s49 = std::sin((4./9.)*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c49 - E[i]*s49/sqrt_rho_c - (4./9.)*dt*J[i]*s49/sqrt_rho_c;
        E1[i]  =  uc[i]*s49*sqrt_rho_c + E[i]*c49 + (4./9.)*dt*J[i]*c49;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp((4./9.)*I*kx[i]*Vk(k)*dt) + (4./9.)*dt*d[i]*std::exp((4./9.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c23  = std::cos((2./3.)*dt*sqrt_rho_c)  , s23  = std::sin((2./3.)*dt*sqrt_rho_c) ,
             c109 = std::cos((10./9.)*dt*sqrt_rho_c) , s109 = std::sin((10./9.)*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] = (29./8.)*(  uc[i]*c23 + E[i]*s23/sqrt_rho_c ) - (21./8.)*(  uc1[i]*c109 + E1[i]*s109/sqrt_rho_c ) + 0.5*dt*J[i]*s109/sqrt_rho_c;
        E2[i]  = (29./8.)*( -uc[i]*s23*sqrt_rho_c + E[i]*c23 ) - (21./8.)*( -uc1[i]*s109*sqrt_rho_c + E1[i]*c109 ) + 0.5*dt*J[i]*c109;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = (29./8.)*hfh[k][i]*std::exp(-(2./3.)*I*kx[i]*Vk(k)*dt) - (21./8.)*hfh1[k][i]*std::exp(-(10./9.)*I*kx[i]*Vk(k)*dt) + 0.5*dt*d[i]*std::exp(-(10./9.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1   = std::cos(dt*sqrt_rho_c)          , s1   = std::sin(dt*sqrt_rho_c)         ,
             c13  = std::cos((1./3.)*dt*sqrt_rho_c)  , s13  = std::sin((1./3.)*dt*sqrt_rho_c) ,
             c139 = std::cos((13./9.)*dt*sqrt_rho_c) , s139 = std::sin((13./9.)*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        double tmp_uc =  (25./16.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) - (9./16.)*(  uc1[i]*c139 + E1[i]*s139/sqrt_rho_c ) - (3./4.)*dt*J[i]*s13/sqrt_rho_c;
        E[i]          =  (25./16.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) - (9./16.)*( -uc1[i]*s139*sqrt_rho_c + E1[i]*c139 ) - (3./4.)*dt*J[i]*c13;
        uc[i] = tmp_uc;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh[k][i] = (25./16.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) - (9./16.)*hfh1[k][i]*std::exp(-(13./9.)*I*kx[i]*Vk(k)*dt) - (3./4.)*dt*d[i]*std::exp(-(1./3.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 3

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

    // DP4(3)
    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c - 0.5*iter.dt*J[i]*s05/sqrt_rho_c;
        E1[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) - 0.5*iter.dt*d[i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c;
        E2[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) - 0.5*iter.dt*d[i];
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc3[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - iter.dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh3[k][i] = hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*iter.dt) - iter.dt*d[i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt);
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc4[i] = -(1./3.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./3.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./3.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/3.;
        E4[i]  = -(1./3.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./3.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./3.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/3. - (1./6.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh4[k][i] = -(1./3.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*iter.dt) + (1./3.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + (2./3.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + hfh3[k][i]/3. - (1./6.)*iter.dt*d[i];
        }
      }
    } // end stage 4

/*
    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc5[i] = -(1./5.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./5.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./5.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/5. + (2./5.)*uc4[i];
        E5[i]  = -(1./5.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./5.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./5.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/5. + (2./5.)*E4[i] - (1./10.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh5[k][i] = -(1./5.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*iter.dt) + (1./5.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + (2./5.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + hfh3[k][i]/5. + (2./5.)*hfh4[k][i] - 0.1*iter.dt*d[i];
        }
      }
    } // end stage 5
*/

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

    //DP5
    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c02 = std::cos(0.2*dt*sqrt_rho_c) , s02 = std::sin(0.2*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c02 + E[i]*s02/sqrt_rho_c - 0.2*dt*J[i]*s02/sqrt_rho_c;
        E1[i]  = -uc[i]*s02*sqrt_rho_c + E[i]*c02 - 0.2*dt*J[i]*c02;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - 0.2*dt*d[i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c01 = std::cos(0.1*sqrt_rho_c*dt) , s01 = std::sin(0.1*sqrt_rho_c*dt),
             c03 = std::cos(0.3*sqrt_rho_c*dt) , s03 = std::sin(0.3*sqrt_rho_c*dt);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] = 0.625*(  uc[i]*c03 + E[i]*s03/sqrt_rho_c ) + 0.375*(  uc1[i]*c01 + E1[i]*s01/sqrt_rho_c ) - 0.225*dt*J[i]*s01/sqrt_rho_c;
        E2[i]  = 0.625*( -uc[i]*s03*sqrt_rho_c + E[i]*c03 ) + 0.375*( -uc1[i]*sqrt_rho_c*s01 + E1[i]*c01 ) - 0.225*dt*J[i]*c01;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = 0.625*hfh[k][i]*std::exp(-0.3*I*kx[i]*Vk(k)*dt) + 0.375*hfh1[k][i]*std::exp(-0.1*I*kx[i]*Vk(k)*dt) - 0.225*dt*d[i]*std::exp(-0.1*I*kx[i]*Vk(k));
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c08 = std::cos(0.8*dt*sqrt_rho_c) , s08 = std::sin(0.8*dt*sqrt_rho_c) ,
             c06 = std::cos(0.6*dt*sqrt_rho_c) , s06 = std::sin(0.6*dt*sqrt_rho_c) ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc3[i] = (175./27.)*(  uc[i]*c08 + E[i]*s08/sqrt_rho_c ) + (100./9.)*(  uc1[i]*c06 + E1[i]*s06/sqrt_rho_c ) - (448./27.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) - (32./9.)*dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = (175./27.)*( -uc[i]*s08*sqrt_rho_c + E[i]*c08 ) + (100./9.)*( -uc1[i]*s06*sqrt_rho_c + E1[i]*c06 ) - (448./27.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) - (32./9.)*dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh3[k][i] = (175./27.)*hfh[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) + (100./9.)*hfh1[k][i]*std::exp(-0.6*I*kx[i]*Vk(k)*dt) - (448./27.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*dt) - (32./9.)*dt*d[i]*std::exp(-0.5*I*kx[i]*Vk(k));
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c89    = std::cos((8./9.)*sqrt_rho_c*dt)    , s89    = std::sin((8./9.)*sqrt_rho_c*dt)    ,
             c3145  = std::cos((31./45.)*sqrt_rho_c*dt)  , s3145  = std::sin((31./45.)*sqrt_rho_c*dt)  ,
             c5390  = std::cos((53./90.)*sqrt_rho_c*dt)  , s5390  = std::sin((53./90.)*sqrt_rho_c*dt)  ,
             c44827 = std::cos((448./27.)*sqrt_rho_c*dt) , s44827 = std::sin((448./27.)*sqrt_rho_c*dt) ,
             c445   = std::cos((4./45.)*sqrt_rho_c*dt)   , s445   = std::sin((4./45.)*sqrt_rho_c*dt)   ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc4[i] = (3551./6561.)*(  uc[i]*c89 + E[i]*s89/sqrt_rho_c ) + (7420./2187.)*(  uc1[i]*c3145 + E1[i]*s3145/sqrt_rho_c ) - (37376./6561.)*(  uc2[i]*c5390 + E2[i]*s5390/sqrt_rho_c ) + (2014./729.)*(  uc3[i]*c445 + E3[i]*s445/sqrt_rho_c ) + (212./729.)*dt*J[i]*s445/sqrt_rho_c;
        E4[i]  = (3551./6561.)*( -uc[i]*s89*sqrt_rho_c + E[i]*c89 ) + (7420./2187.)*( -uc1[i]*s3145*sqrt_rho_c + E1[i]*c3145 ) - (37376./6561.)*( -uc2[i]*s5390*sqrt_rho_c + E2[i]*c5390 ) + (2014./729.)*( -uc3[i]*s445*sqrt_rho_c + E3[i]*c445 ) + (212./729.)*dt*J[i]*c445;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh4[k][i] = (3551./6561.)*hfh[k][i]*std::exp(-(8./9.)*I*kx[i]*Vk(k)*dt) + (7420./2187.)*hfh1[k][i]*std::exp(-(31./45.)*I*kx[i]*Vk(k)*dt) - (37376./6561.)*hfh2[k][i]*std::exp(-(53./90.)*I*kx[i]*Vk(k)*dt) + (2014./729.)*hfh3[k][i]*std::exp(-(4./45.)*I*kx[i]*Vk(k)*dt) + (212./729.)*dt*d[i]*std::exp(-(4./45.)*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 4

    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(sqrt_rho_c*dt)         , s1  = std::sin(sqrt_rho_c*dt)         ,
             c08 = std::cos(0.8*sqrt_rho_c*dt)     , s08 = std::sin(0.8*sqrt_rho_c*dt)     ,
             c07 = std::cos(0.7*sqrt_rho_c*dt)     , s07 = std::sin(0.7*sqrt_rho_c*dt)     ,
             c02 = std::cos(0.2*sqrt_rho_c*dt)     , s02 = std::sin(0.2*sqrt_rho_c*dt)     ,
             c19 = std::cos((1./9.)*sqrt_rho_c*dt) , s19 = std::sin((1./9.)*sqrt_rho_c*dt) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc5[i] = (313397./335808.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (424025./55968.)*(  uc1[i]*c08 + E1[i]*s08/sqrt_rho_c ) - (61400./5247.)*(  uc2[i]*c07 + E2[i]*s07/sqrt_rho_c ) + (96075./18656.)*(  uc3[i]*c02 + E3[i]*s02/sqrt_rho_c ) - (35721./37312.)*(  uc4[i]*c19 + E4[i]*s19/sqrt_rho_c ) + (5103./18656.)*dt*J[i]*s19/sqrt_rho_c;
        E5[i]  = (313397./335808.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (424025./55968.)*( -uc1[i]*s08*sqrt_rho_c + E1[i]*c08 ) - (61400./5247.)*( -uc2[i]*s07*sqrt_rho_c + E2[i]*c07 ) + (96075./18656.)*( -uc3[i]*s02*sqrt_rho_c + E3[i]*c02 ) - (35721./37312.)*( -uc4[i]*s19*sqrt_rho_c + E4[i]*c19 ) + (5103./18656.)*dt*J[i]*c19;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh5[k][i] = (313397./335808.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) + (424025./55968.)*hfh1[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) - (61400./5247.)*hfh2[k][i]*std::exp(-0.7*I*kx[i]*Vk(k)*dt) + (96075./18656.)*hfh3[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - (35721./37312.)*hfh4[k][i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt) + (5103./18656.)*dt*d[i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 5

    // STAGE 6
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh5[k].begin(),hfh5[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E5);

      double c1  = std::cos(sqrt_rho_c*dt)         , s1  = std::sin(sqrt_rho_c*dt)         ,
             c08 = std::cos(0.8*sqrt_rho_c*dt)     , s08 = std::sin(0.8*sqrt_rho_c*dt)     ,
             c07 = std::cos(0.7*sqrt_rho_c*dt)     , s07 = std::sin(0.7*sqrt_rho_c*dt)     ,
             c02 = std::cos(0.2*sqrt_rho_c*dt)     , s02 = std::sin(0.2*sqrt_rho_c*dt)     ,
             c19 = std::cos((1./9.)*sqrt_rho_c*dt) , s19 = std::sin((1./9.)*sqrt_rho_c*dt) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc6[i] = -(563./3456.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) - (575./252.)*(  uc1[i]*c08 + E1[i]*s08/sqrt_rho_c ) + (31400./10017.)*(  uc2[i]*c07 + E2[i]*s07/sqrt_rho_c ) + (325./1344.)*(  uc3[i]*c02 + E3[i]*s02/sqrt_rho_c ) - (7533./6784.)*(  uc4[i]*c19 + E4[i]*s19/sqrt_rho_c ) + (33./28.)*uc5[i];
        E6[i]  = -(563./3456.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) - (575./252.)*( -uc1[i]*s08*sqrt_rho_c + E1[i]*c08 ) + (31400./10017.)*( -uc2[i]*s07*sqrt_rho_c + E2[i]*c07 ) + (325./1344.)*( -uc3[i]*s02*sqrt_rho_c + E3[i]*c02 ) - (7533./6784.)*( -uc4[i]*s19*sqrt_rho_c + E4[i]*c19 ) + (33./28.)*E5[i] - (11./84.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh6[k][i] = -(563./3456.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) - (575./252.)*hfh1[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) + (31400./10017.)*hfh2[k][i]*std::exp(-0.7*I*kx[i]*Vk(k)*dt) + (325./1344.)*hfh3[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - (7533./6784.)*hfh4[k][i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt) + (33./28.)*hfh5[k][i] - (11./84.)*dt*d[i];
        }
      }
    } // end stage 6

    // STAGE 7
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh6[k].begin(),hfh6[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E6);

      double c1  = std::cos(sqrt_rho_c*dt)         , s1  = std::sin(sqrt_rho_c*dt)         ,
             c08 = std::cos(0.8*sqrt_rho_c*dt)     , s08 = std::sin(0.8*sqrt_rho_c*dt)     ,
             c07 = std::cos(0.7*sqrt_rho_c*dt)     , s07 = std::sin(0.7*sqrt_rho_c*dt)     ,
             c02 = std::cos(0.2*sqrt_rho_c*dt)     , s02 = std::sin(0.2*sqrt_rho_c*dt)     ,
             c19 = std::cos((1./9.)*sqrt_rho_c*dt) , s19 = std::sin((1./9.)*sqrt_rho_c*dt) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc7[i] = (8813./172800.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (41./180.)*(  uc1[i]*c08 + E1[i]*s08/sqrt_rho_c ) - (4294./10017.)*(  uc2[i]*c07 + E2[i]*s07/sqrt_rho_c ) + (263./384.)*(  uc3[i]*c02 + E3[i]*s02/sqrt_rho_c ) - (137781./339200.)*(  uc4[i]*c19 + E4[i]*s19/sqrt_rho_c ) + (803./4200)*uc5[i] + (17./25.)*uc6[i];
        E7[i]  = (8813./172800.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (41./180.)*( -uc1[i]*s08*sqrt_rho_c + E1[i]*c08 ) - (4294./10017.)*( -uc2[i]*s07*sqrt_rho_c + E2[i]*c07 ) + (263./384.)*( -uc3[i]*s02*sqrt_rho_c + E3[i]*c02 ) - (137781./339200.)*( -uc4[i]*s19*sqrt_rho_c + E4[i]*c19 ) + (803./4200)*E5[i] + (17./25.)*E6[i] - (1./40.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh7[k][i] = (8813./172800.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) + (41./180.)*hfh1[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) - (4294./10017.)*hfh2[k][i]*std::exp(-0.7*I*kx[i]*Vk(k)*dt) + (263./384.)*hfh3[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - (137781./339200.)*hfh4[k][i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt) + (803./4200)*hfh5[k][i] + (17./25.)*hfh6[k][i] - (1./40.)*dt*d[i];
        }
      }
    } // end stage 7
/////////////////////////////////////////////////////////////////////
/**/
/////////////////////////////////////////////////////////////////////
    // end of time loop

    // CHECK local error for compute next time step
    iter.E_error(E5,E4,fh.step.dx);
    iter.hfh_error(hfh5,hfh4,fh.step.dx*fh.step.dv);
    std::cout << " -- " << iteration::error(iter) << std::flush;

    iter.success = std::abs(iter.error() - c.tol) <= c.tol;
    //if ( iter.success ) // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    {
      iter.success = true;
      // SAVE TIME STEP
      std::copy( uc4.begin()  , uc4.end()  , uc.begin()  );
      std::copy( E4.begin()   , E4.end()   , E.begin()   );
      std::copy( hfh4.begin() , hfh4.end() , hfh.begin() );

      // MONITORING

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

      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      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 );

      // increment time
      iter.current_time += iter.dt;
      times.push_back( iter.current_time );
      success_iterations.push_back( iter );
    }
    iterations.push_back( iter );

    ++iter.iter;

    //iter.dt = std::pow( c.tol/iter.Lhfh , 0.25 )*iter.dt;// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    if ( iter.current_time+iter.dt > c.Tf ) { iter.dt = c.Tf - iter.current_time; }
  } // while (  i_t*dt < Tf )
  std::cout << "\r" << time(iter) << std::endl;

  save_data("");

  auto dx_y = [&,count=0](auto const& y) mutable {
    std::stringstream ss; ss<< fh.step.dx*(count++) <<" "<<y;
    return ss.str();
  };
  c << monitoring::data( "E_"+c.name+".dat" , E , dx_y );

  ublas::vector<double> rho  (c.Nx,0.);
  ublas::vector<double> rhoc (c.Nx,0.);
  {
    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];
    }
    hrhoc.ifft(&rhoc[0]);
    rho = rhoc + rhoh;
  }

  c << monitoring::data( "rho_"+c.name+".dat" , rho , dx_y );
  c << monitoring::data( "uc_"+c.name+".dat"  , uc  , dx_y );

  auto Jh = fh.courant();
  ublas::vector<double> Jc (c.Nx,0.), Jtot (c.Nx,0.);
  for ( auto i=0 ; i<c.Nx ; ++i ) {
    Jc[i] = rhoc[i]*uc[i];
  }
  Jtot = Jc + Jh;
  c << monitoring::data( "J_"+c.name+".dat" , Jtot , dx_y );

  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/detail/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/detail/constant/pio_4lo.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

   template < class A0,
             class unit_tag,
             class style,
             class base_A0 = bd::scalar_of_t<A0>
  >
  struct invtrig_base{};

  template < class A0 >
  struct invtrig_base<A0,tag::radian_tag,tag::not_simd_type, float>
  {
    static BOOST_FORCEINLINE A0 asin(A0 a0) BOOST_NOEXCEPT
    {
      A0 sign, x, z;
      x = bs::abs(a0);
      sign = bitofsign(a0);
      if ((x < Constant<A0,0x38d1b717>())) return a0; //1.0e-4
      if ((x >  One<A0>())) return Nan<A0>();
      auto bx_larger_05    = (x > Half<A0>());
      if (bx_larger_05)
      {
        z = Half<A0>()*oneminus(x);
        x =  sqrt(z);
      }
      else
      {
        z = sqr(x);
      }
      A0 z1 = horn<A0,
        0x3e2aaae4,
        0x3d9980f6,
        0x3d3a3ec7,
        0x3cc617e3,
        0x3d2cb352
        > (z);
      z1 = fma(z1, z*x, x);
      if(bx_larger_05)
      {
        z1 = z1+z1;
        z1 = Pio_2<A0>()-z1;
      }
      return bitwise_xor(z1, sign);
    }

    static BOOST_FORCEINLINE A0 acos(const  A0& a0) BOOST_NOEXCEPT
    {
      if (a0 < Mhalf<A0>())
        return Pi<A0>()-asin( sqrt(inc(a0)*Half<A0>()))*Two<A0>();
      else if (a0 > Half<A0>())
        return asin( sqrt(oneminus(a0)*Half<A0>()))*Two<A0>();
      return (Pio_2<A0>()-asin(a0));
    }

    static BOOST_FORCEINLINE A0 atan(A0 a0) BOOST_NOEXCEPT
    {
      A0 x  = abs(a0);
      return bitwise_xor(kernel_atan(x, rec(x)), bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 acot(A0 a0) BOOST_NOEXCEPT
    {
      A0 x  = abs(a0);
      return bitwise_xor(kernel_atan(rec(x), x), bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 kernel_atan(A0 x, A0 recx) BOOST_NOEXCEPT
    {
      if (is_eqz(x))  return Zero<A0>();
      if (x == Inf<A0>())  return Pio_2<A0>();
      A0 y = 0.0;
      A0 more = Zero<A0>();
      if( x > Tan_3pio_8<A0>())
      {
        y = Pio_2<A0>();
        more = Pio_2lo<A0>();
        x = -recx;
      }
      else if( x > Tanpio_8<A0>())
      {
        y = Pio_4<A0>();
        more =  Pio_4lo<A0>();
        x = dec(x)/inc(x);
      }
      A0 z = sqr(x);
      A0 z1 = horn<A0
        , 0xbeaaaa2aul  // -3.3333293e-01
        , 0x3e4c925ful  //  1.9991724e-01
        , 0xbe0e1b85ul  // -1.4031009e-01
        , 0x3da4f0d1ul  //  8.5460119e-02
        > (z);
    z1 = fma(x, z1*z, x);

      return y+(z1+more);
    }
  };
}
} }
#endif


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <cmath>
#include <memory>

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "MeshLib/Elements/Line.h"
#include "MeshLib/Mesh.h"

#include "ProcessLib/LIE/Common/Utils.h"

namespace
{

std::unique_ptr<MeshLib::Mesh> createLine(
    std::array<double, 3> const& a, std::array<double, 3> const& b)
{
    MeshLib::Node** nodes = new MeshLib::Node*[2];
    nodes[0] = new MeshLib::Node(a);
    nodes[1] = new MeshLib::Node(b);
    MeshLib::Element* e = new MeshLib::Line(nodes);

    return std::unique_ptr<MeshLib::Mesh>(
                new MeshLib::Mesh("",
                                  std::vector<MeshLib::Node*>{nodes[0], nodes[1]},
                                  std::vector<MeshLib::Element*>{e})
                );
}

std::unique_ptr<MeshLib::Mesh> createX()
{
    return createLine({{-1.0, 0.0, 0.0}}, {{1.0,  0.0, 0.0}});
}

std::unique_ptr<MeshLib::Mesh> createY()
{
    return createLine({{0.0, -1.0, 0.0}}, {{0.0,  1.0, 0.0}});
}

std::unique_ptr<MeshLib::Mesh> createXY()
{
    // 45degree inclined
    return createLine({{0.0, 0.0, 0.0}}, {{2./sqrt(2), 2./sqrt(2), 0.0}});
}

const double eps = std::numeric_limits<double>::epsilon();

}

TEST(LIE, rotationMatrixX)
{
    auto msh(createX());
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, nv);
    ASSERT_EQ(0., nv[0]);
    ASSERT_EQ(1., nv[1]);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(2,2);
    ProcessLib::LIE::computeRotationMatrix(nv, 2, R);

    ASSERT_NEAR(1., R(0,0), eps);
    ASSERT_NEAR(0., R(0,1), eps);
    ASSERT_NEAR(0., R(1,0), eps);
    ASSERT_NEAR(1., R(1,1), eps);
}

TEST(LIE, rotationMatrixY)
{
    auto msh(createY());
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, nv);
    ASSERT_EQ(-1., nv[0]);
    ASSERT_EQ(0., nv[1]);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(2,2);
    ProcessLib::LIE::computeRotationMatrix(nv, 2, R);

    ASSERT_NEAR(0., R(0,0), eps);
    ASSERT_NEAR(1., R(0,1), eps);
    ASSERT_NEAR(-1., R(1,0), eps);
    ASSERT_NEAR(0., R(1,1), eps);
}

TEST(LIE, rotationMatrixXY)
{
    auto msh(createXY());
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, nv);
    ASSERT_NEAR(-1./sqrt(2), nv[0], eps);
    ASSERT_NEAR(1./sqrt(2), nv[1], eps);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(2,2);
    ProcessLib::LIE::computeRotationMatrix(nv, 2, R);

    ASSERT_NEAR(1./sqrt(2), R(0,0), eps);
    ASSERT_NEAR(1./sqrt(2), R(0,1), eps);
    ASSERT_NEAR(-1./sqrt(2), R(1,0), eps);
    ASSERT_NEAR(1./sqrt(2), R(1,1), eps);
}


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#define BOOST_TEST_MODULE ecliptic_coord_test

#include <iostream>
#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/astronomy/coordinate/coord_sys/ecliptic_coord.hpp>

#include <boost/test/unit_test.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::astronomy::coordinate;

namespace bud = boost::units::degree;

BOOST_AUTO_TEST_SUITE(ecliptic_coord_constructors)

BOOST_AUTO_TEST_CASE(ecliptic_coord_default_constructor) {
    ecliptic_coord<
            double,
            quantity<bud::plane_angle>,
            quantity<bud::plane_angle>>
            ec;

    //Check set_lat_lon
        ec.set_lat_lon(45.0 * bud::degrees, 18.0 * bud::degrees);

    //Check values
    BOOST_CHECK_CLOSE(ec.get_lat().value(), 45.0, 0.001);
    BOOST_CHECK_CLOSE(ec.get_lon().value(), 18.0, 0.001);

    //Quantities stored as expected?
    BOOST_TEST((std::is_same<decltype(ec.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(ec.get_lon()), quantity<bud::plane_angle>>::value));
}

BOOST_AUTO_TEST_CASE(ecliptic_coord_quantities_constructor) {
    //Make Ecliptic Coordinate Check
    auto ec1 = make_ecliptic_coord
            (15.0 * bud::degrees, 39.0 * bud::degrees);

    //Check values
    BOOST_CHECK_CLOSE(ec1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(ec1.get_lon().value(), 39.0, 0.001);

    //Quantities stored as expected?
    BOOST_TEST((std::is_same<decltype(ec1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(ec1.get_lon()), quantity<bud::plane_angle>>::value));

    //Ecliptic Coordinate constructor
    ecliptic_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
            ec2(1.5 * bud::degrees, 9.0 * bud::degrees);

    //Check values
    BOOST_CHECK_CLOSE(ec2.get_lat().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(ec2.get_lon().value(), 9.0, 0.001);

    //Quantities stored as expected?
    BOOST_TEST((std::is_same<decltype(ec2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(ec2.get_lon()), quantity<bud::plane_angle>>::value));
}

BOOST_AUTO_TEST_SUITE_END()


// 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 <string>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

// Function is instantiated 108 times
template <typename T, typename U>
void inline test3(string type1, T x, string type2, U y)
{
    cout << type1 << " @ " << type2 << '\n';
    // cout << "As typeid: " << typeid(x).name() << " @ " << typeid(y).name() << '\n';
    // cout << x << " @ " << y << '\n';

    cout << x << " + " << y << " = " << x + y << '\n';
    MTL_THROW_IF(x + y != 8, mtl::runtime_error("Result of addition must be 8."));  // Integer numbers less then 17 should not have rounding errors

    cout << x << " - " << y << " = " << x - y << '\n';
    MTL_THROW_IF(x - y != 4, mtl::runtime_error("Result of subtraction must be 4."));

    cout << x << " * " << y << " = " << x * y << '\n';
    MTL_THROW_IF(x * y != 12, mtl::runtime_error("Result of subtraction must be 12."));

    cout << x << " / " << y << " = " << x / y << '\n';
    MTL_THROW_IF(x / y != 3, mtl::runtime_error("Result of subtraction must be 3."));

    cout << '\n';
}

template <typename T, typename U>
void inline test2(const char* type1, T x, const char* type2, U y)
{
    cout << type1 << " @ " << type2 << '\n';
    cout << "different_non_complex<T, U> is " << (mtl::traits::different_non_complex<T, U>::value ? "true (use my extension)\n " : "false (use standard)\n") << "----\n";
    string ctype1(string("complex<") + type1 + ">"), ctype2(string("complex<") + type2 + ">");
    std::complex<T> cx(x);
    std::complex<U> cy(y);

    test3(type1, x, ctype2, cy);
    test3(ctype1, cx, type2, y);
    test3(ctype1, cx, ctype2, cy);
}

template <typename T>
void inline test(const char* type1, T x)
{
#ifdef ___clang__
    test2(type1, x, "int", 2);
    test2(type1, x, "long", 2l);
#  if 0 // causes warnings in visual studion and g++ with -pedantic
    test2(type1, x, "unsigned", 2u);
#  endif

#else
    std::cerr << "Warning complex<int> not supported on certain clang compilers.\n";
#endif

    test2(type1, x, "float", 2.f);
    test2(type1, x, "double", 2.);
    test2(type1, x, "long double", 2.l);
}

int main(int, char**)
{
    using namespace mtl;

    test("int", 6);
    test("long", 6l);
#if 0// causes warnings in visual studion and g++ with -pedantic
    test("unsigned", 6u);
#endif
    test("float", 6.f);
    test("double", 6.);
    test("long double", 6.l);

    return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sind.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

STF_CASE_TPL (" sind",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sind;

  using r_t = decltype(sind(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sind(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sind(-T(180)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(-T(45)), -bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(-T(90)), -bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(bs::Zero<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(T(180)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(T(45)), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sind(T(90)), bs::One<r_t>(), 0.5);
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                          */
/* Universidad Politecnica de Valencia, Spain                              */
/*                                                                         */
/* Copyright (C) 2014 Javier Juan Albarracin                               */
/*                                                                         */
/***************************************************************************
* Eigen Utils                                                              *
***************************************************************************/

#ifndef EIGENUTILS_HPP
#define EIGENUTILS_HPP

#include <Eigen/Dense>

using namespace Eigen;

class EigenUtils
{
public:
	template <typename Derived>
	inline static void removeRow(DenseBase<Derived> &matrix, size_t rowToRemove);
	template <typename Derived>
	inline static void removeColumn(DenseBase<Derived> &matrix, size_t colToRemove);
};

template <typename Derived>
void EigenUtils::removeRow(DenseBase<Derived> &matrix, size_t rowToRemove)
{
    size_t numRows = matrix.rows() - 1;
    size_t numCols = matrix.cols();

    if (rowToRemove < numRows)
        matrix.block(rowToRemove, 0, numRows - rowToRemove, numCols) = matrix.block(rowToRemove + 1, 0, numRows - rowToRemove, numCols);

    matrix.derived().conservativeResize(numRows, numCols);
}

template <typename Derived>
void EigenUtils::removeColumn(DenseBase<Derived> &matrix, size_t colToRemove)
{
    size_t numRows = matrix.rows();
    size_t numCols = matrix.cols() - 1;

    if (colToRemove < numCols)
        matrix.block(0, colToRemove, numRows, numCols - colToRemove) = matrix.block(0, colToRemove + 1, numRows, numCols - colToRemove);

    matrix.derived().conservativeResize(numRows, numCols);
}

#endif


// Copyright Paul A. Bristow 2015
// 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)

//! \file
//!\brief Basic tests for fixed_point.

#include <iomanip>
#include <sstream>
#include <string>

#define BOOST_TEST_MODULE test_negatable_basic_basic_ops
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_basic_basic_ops)
{
  {
    // Plain fixed-point
    typedef boost::fixed_point::negatable<32, -16> fixed_point_type;

    const fixed_point_type x = fixed_point_type(-100) / 100;

    std::ostringstream os;

    os << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
       << std::fixed
       << x;

    const std::string reference_string =
        std::string("-1.")
      + std::string(std::string::size_type(std::numeric_limits<fixed_point_type>::digits10), char('0'));

    BOOST_CHECK_EQUAL(os.str(), reference_string);
  }

  {
    // Plain fixed-point
    typedef boost::fixed_point::negatable<2, -13> fixed_point_type;

    const fixed_point_type x = fixed_point_type(-1) / 3;

    std::ostringstream os;

    os << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
       << std::fixed
       << x;

    const std::string reference_string =
        std::string("-0.")
      + std::string(std::string::size_type(std::numeric_limits<fixed_point_type>::digits10), char('3'));

    BOOST_CHECK_EQUAL(os.str(), reference_string);
  }

  {
    // fastest round
    typedef boost::fixed_point::negatable<13, -2> fixed_point_type_fastest_round;

    const fixed_point_type_fastest_round x = fixed_point_type_fastest_round(-1.26);

    std::ostringstream os;

    os << std::setprecision(std::numeric_limits<fixed_point_type_fastest_round>::digits10)
       << std::fixed
       << x;

    const std::string reference_string =
        std::string("-1.25")
      + std::string(std::string::size_type(std::numeric_limits<fixed_point_type_fastest_round>::digits10 - int(std::string("25").size())), char('0'));

    BOOST_CHECK_EQUAL(os.str(), reference_string);
  }
}


#include "electronthresholds.h"
#include "python/pyreader.h"

#include <algorithm>
#include <cmath>
#include <random>

#include <Eigen/Dense>
#include <unsupported/Eigen/LevenbergMarquardt>

namespace stempy {

template <typename T>
double calculateMean(std::vector<T>& values)
{
  return std::accumulate(values.begin(), values.end(), 0.0) / values.size();
}

template <typename T>
double calculateVariance(std::vector<T>& values, double mean)
{
  double v1 = 0;
  double sigma2;

  for (size_t i = 0; i < values.size(); i++) {
    v1 += pow(values[i] - mean, 2.0);
  }
  sigma2 = v1 / (values.size() - 1.0);

  return sigma2;
}

struct GaussianErrorFunctor : Eigen::DenseFunctor<double>
{

  GaussianErrorFunctor(const Eigen::VectorXd& bins,
                       const Eigen::VectorXd& histogram)
    : Eigen::DenseFunctor<double>(3, bins.rows()), m_bins(bins),
      m_histogram(histogram)
  {}

  int operator()(const InputType& x, ValueType& fvec)
  {
    auto num = -(m_bins - ValueType::Constant(values(), x[1])).array().square();
    auto variance = pow(x[2], 2);

    fvec = x[0] * (num / (2 * variance)).exp() - m_histogram.array();

    return 0;
  }

  int df(const InputType& x, JacobianType& jacobian)
  {
    auto means = ValueType::Constant(values(), x[1]);
    auto tmp = (m_bins - means).array();
    auto num = -tmp.square();
    auto variance = pow(x[2], 2);
    auto den = 2 * variance;

    auto j0 = (num / den).exp();
    auto j1 = x[0] * tmp * j0 / variance;

    jacobian.col(0) = j0;
    jacobian.col(1) = j1;
    jacobian.col(2) = tmp * j1 / x[2];

    return 0;
  }

  Eigen::VectorXd m_bins;
  Eigen::VectorXd m_histogram;
};

template <typename BlockType, typename FrameType, bool dark>
CalculateThresholdsResults<FrameType> calculateThresholds(
  std::vector<BlockType>& blocks, const float darkReference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[])
{
  auto frameDimensions = blocks[0].header.frameDimensions;
  auto numberOfPixels = frameDimensions.first * frameDimensions.second;

  // Setup random number engine
  std::random_device randomDevice;
  std::default_random_engine randomEngine(randomDevice());

  int numberSamplePixels =
    frameDimensions.first * frameDimensions.second * numberOfSamples;
  std::vector<FrameType> samples(numberSamplePixels, 0);
  for (int i = 0; i < numberOfSamples; i++) {
    std::uniform_int_distribution<int> randomBlockDist(0, blocks.size() - 1);
    auto randomBlockIndex = randomBlockDist(randomEngine);
    auto randomBlock = blocks[randomBlockIndex];
    std::uniform_int_distribution<int> randomFrameDist(
      0, randomBlock.header.imagesInBlock - 1);
    auto randomFrameIndex = randomFrameDist(randomEngine);
    auto blockData = randomBlock.data.get();

    for (unsigned j = 0; j < numberOfPixels; j++) {
      // For now just use the index, the image number don't seem to work, in the
      // current data set. In the future we should be using the image number.

      // This will be evaluated a compile time.
      if (std::is_integral<FrameType>::value) {
        auto value = blockData[randomFrameIndex * numberOfPixels + j];
        static_if<dark>(
          [&]() { value -= static_cast<int16_t>(darkReference[j]); })();
        samples[i * numberOfPixels + j] = value;
      }
      // if not integral type then we know we have gain and need to multiple
      else {
        auto value = blockData[randomFrameIndex * numberOfPixels + j] * gain[j];
        static_if<dark>(
          [&]() { value -= static_cast<float>(darkReference[j]); })();
        samples[i * numberOfPixels + j] = value;
      }
    }
  }

  // Calculate stats
  auto mean = calculateMean(samples);
  auto variance = calculateVariance(samples, mean);
  auto stdDev = sqrt(variance);
  auto xrayThreshold = mean + xRayThresholdNSigma * stdDev;

  // Now generate a histograms
  auto minMax = std::minmax_element(samples.begin(), samples.end());
  auto minSample = *minMax.first;
  auto maxSample = static_cast<FrameType>(std::ceil(*minMax.second));
  auto maxBin = std::min(static_cast<int>(maxSample),
                         static_cast<int>(mean + xrayThreshold * stdDev));
  auto minBin = std::max(static_cast<int>(minSample),
                         static_cast<int>(mean - xrayThreshold * stdDev));

  auto numberOfBins = maxBin - minBin;
  std::vector<double> histogram(numberOfBins, 0.0);
  std::vector<double> bins(numberOfBins);

  auto binEdge = minBin;
  for (int i = 0; i < numberOfBins; i++) {
    bins[i] = binEdge++;
  }

  // Bin the values
  for (int i = 0; i < numberSamplePixels; i++) {
    auto binIndex = static_cast<int>(samples[i] - minBin);
    // Skip values outside range
    if (binIndex >= numberOfBins) {
      continue;
    }
    histogram[binIndex] += 1;
  }

  auto b = Eigen::Map<Eigen::VectorXd>(bins.data(), bins.size());
  auto h = Eigen::Map<Eigen::VectorXd>(histogram.data(), histogram.size());

  GaussianErrorFunctor gef(b, h);
  Eigen::VectorXd state(3);
  auto indexOfMaxElement =
    std::max_element(histogram.begin(), histogram.end()) - histogram.begin();
  state << static_cast<double>(histogram[indexOfMaxElement]), mean, stdDev;

  Eigen::LevenbergMarquardt<GaussianErrorFunctor> solver(gef);
  solver.minimize(state);

  if (solver.info() != Eigen::ComputationInfo::Success) {
    throw std::runtime_error("Optimization did not converge");
  }

  auto optimizedMean = state[1];
  auto optimizedStdDev = fabs(state[2]);

  auto backgroundThreshold =
    optimizedMean + optimizedStdDev * backgroundThresholdNSigma;

  CalculateThresholdsResults<FrameType> ret;
  ret.numberOfSamples = numberOfSamples;
  ret.minSample = minSample;
  ret.maxSample = maxSample;
  ret.mean = mean;
  ret.variance = variance;
  ret.stdDev = stdDev;
  ret.numberOfBins = numberOfBins;
  ret.xRayThresholdNSigma = xRayThresholdNSigma;
  ret.backgroundThresholdNSigma = backgroundThresholdNSigma;
  ret.xRayThreshold = xrayThreshold;
  ret.backgroundThreshold = backgroundThreshold;
  ret.optimizedMean = optimizedMean;
  ret.optimizedStdDev = optimizedStdDev;

  return ret;
}

// Without gain
template <typename BlockType>
CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<BlockType>& blocks, Image<float>& darkreference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma)
{
  return calculateThresholds<BlockType, uint16_t>(
    blocks, darkreference.data.get(), numberOfSamples,
    backgroundThresholdNSigma, xRayThresholdNSigma, nullptr);
}

template <typename BlockType>
CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<BlockType>& blocks, const float darkreference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma)
{
  return calculateThresholds<BlockType, uint16_t>(
    blocks, darkreference, numberOfSamples, backgroundThresholdNSigma,
    xRayThresholdNSigma, nullptr);
}

// With gain
template <typename BlockType>
CalculateThresholdsResults<float> calculateThresholds(
  std::vector<BlockType>& blocks, Image<float>& darkreference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[])
{
  return calculateThresholds<BlockType, float>(
    blocks, darkreference.data.get(), numberOfSamples,
    backgroundThresholdNSigma, xRayThresholdNSigma, gain);
}

// Without gain, without darkreference
template <typename BlockType>
CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<BlockType>& blocks, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma)
{
  return calculateThresholds<BlockType, uint16_t, false>(
    blocks, nullptr, numberOfSamples, backgroundThresholdNSigma,
    xRayThresholdNSigma, nullptr);
}

// With gain
template CalculateThresholdsResults<float> calculateThresholds<Block>(
  std::vector<Block>& blocks, Image<float>& darkReference, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma,
  const float gain[]);
template CalculateThresholdsResults<float> calculateThresholds<PyBlock>(
  std::vector<PyBlock>& blocks, Image<float>& darkReference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[]);
template CalculateThresholdsResults<float> calculateThresholds<Block, float, true>(
  std::vector<Block>& blocks, const float darkReference[], int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma,
  const float gain[]);
template CalculateThresholdsResults<uint16_t> calculateThresholds<Block>(
  std::vector<Block>& blocks, const float darkReference[], int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma,
  const float gain[]);
template CalculateThresholdsResults<float> calculateThresholds<PyBlock, float, true>(
  std::vector<PyBlock>& blocks, const float darkReference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[]);

// No gain
template CalculateThresholdsResults<uint16_t> calculateThresholds<Block>(
  std::vector<Block>& blocks, Image<float>& darkReference, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);
template CalculateThresholdsResults<uint16_t> calculateThresholds<PyBlock>(
  std::vector<PyBlock>& blocks, Image<float>& darkReference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma);
template CalculateThresholdsResults<uint16_t> calculateThresholds<Block>(
  std::vector<Block>& blocks, const float darkReference[], int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);
template CalculateThresholdsResults<uint16_t> calculateThresholds<PyBlock>(
  std::vector<PyBlock>& blocks, const float darkReference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma);

template CalculateThresholdsResults<uint16_t>
calculateThresholds<Block, uint16_t, false>(std::vector<Block>& blocks,
                                            const float darkReference[],
                                            int numberOfSamples,
                                            double backgroundThresholdNSigma,
                                            double xRayThresholdNSigma,
                                            const float gain[]);
template CalculateThresholdsResults<uint16_t>
calculateThresholds<PyBlock, uint16_t, false>(std::vector<PyBlock>& blocks,
                                              const float darkReference[],
                                              int numberOfSamples,
                                              double backgroundThresholdNSigma,
                                              double xRayThresholdNSigma,
                                              const float gain[]);

template CalculateThresholdsResults<float>
calculateThresholds<Block, float, false>(std::vector<Block>& blocks,
                                         const float darkReference[],
                                         int numberOfSamples,
                                         double backgroundThresholdNSigma,
                                         double xRayThresholdNSigma,
                                         const float gain[]);
template CalculateThresholdsResults<float>
calculateThresholds<PyBlock, float, false>(std::vector<PyBlock>& blocks,
                                           const float darkReference[],
                                           int numberOfSamples,
                                           double backgroundThresholdNSigma,
                                           double xRayThresholdNSigma,
                                           const float gain[]);

// No gain or dark reference
template CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<Block>& blocks, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);

template CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<PyBlock>& blocks, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);
}


#include <stdio.h>
#include <FileLoop.h>
#include <FileWvOut.h>
#include <Fir.h>
#include <string.h>
#include <math.h>
#include <vector>
#include <Eigen/Eigen>
#include <Eigen/Dense>

#include "kiss_fftr.h"
#include "FIRFilterCode.h"
#include "eigen_to_image.h"

#include "ChunkStats.h"
#include "Chunk.h"

using namespace stk;

using Eigen::MatrixXf;
using Eigen::MatrixXi;
using std::list;

//main test
void process_song(std::string fn_in, std::string fn_out);

//thing with fourier transform
MatrixXf TFD_extract(FileWvIn input, int nfft, int total_slices, int samples_per_slice, int channels);
void TFDI_extract(kiss_fft_cpx* fft_input, int nfft, int samples_per_slice, StkFrames output);

//filtering methods
//using stk
void make_fir_bandpass_filter(double* coefs, int taps, int freq_center, int freq_margin,  int max_freq);
stk::Fir create_fir_from_coefs(double* coefs, int len);
stk::Fir create_1d_gaussian_filter(int length, double amplitude, double center, double stddev);
//using eigen
MatrixXf create_1d_gaussian_filter_col(int length, double amplitude, double stddev);
MatrixXf create_1d_gaussian_filter_row(int length, double amplitude, double stddev);
MatrixXf one_d_convolve(MatrixXf mat, MatrixXf kern);
MatrixXf dog(MatrixXf full, MatrixXf gauss);

//chunking functions
double snaz(MatrixXf filt, int snazr);
MatrixXi chunkify(MatrixXf tfd, int vert_range, int horz_range);
//list(Chunk) cull_chunks(MatrixXi chunk_ids, ChunkStats stats)
list<Chunk> cull_chunks(ChunkStats stats);

int main() {
  std::string file_in = "./sound/tsu.wav";
  std::string file_out = "./pet.wav";

  process_song(file_in, file_out);

  return 0;
}

void process_song(std::string fn_in, std::string fn_out) {
  FileRead song;
  FileWvIn input;
  FileWvOut output;
  fn_in = "./sound/tsu.wav";
  fn_out = "pet.wav";
  song.open(fn_in, 2);
  input.openFile(fn_in);
  output.openFile(fn_out, 2, FileWrite::FILE_WAV, Stk::STK_SINT16);

  int channels = input.channelsOut();
  long samples = song.fileSize();
  long samples_length = samples / channels;
  double file_rate = input.getFileRate();
  double len_in_sec = samples_length / file_rate;

  int samples_per_slice = 1024;
  double slices_per_second = file_rate / samples_per_slice;
  int total_slices = ceil(samples_length / samples_per_slice);

  int max_freq = file_rate / 2;
  //kiss_fft mentions that the fftr only populates nfft/2 + 1 things
  //guessing that means I initialize with nfft, then only look at nfft/2 + 1 results?
  //actualyl nfft should just be length of sample. is also the resulting frequency resolution.

  bool filter_test = false;
  if (filter_test) {
    FileWvOut filteredOut;
    filteredOut.openFile("hi.wav", 2, FileWrite::FILE_WAV, Stk::STK_SINT16);

    StkFrames slice(samples_per_slice, channels);
    int taps = 60;
    double coefs[taps];
    make_fir_bandpass_filter(coefs, taps, max_freq / 2, max_freq / 64, max_freq);
    stk::Fir bp = create_fir_from_coefs(coefs, taps);
    input.reset();

    for (int slice_i = 0; slice_i < total_slices; slice_i++) {
      input.tick(slice);
      for (int chan_i = 0; chan_i < channels; chan_i++) {
	bp.tick(slice, chan_i);
      }
      filteredOut.tick( slice );
    }
    input.reset();
  }

  int nfft = samples_per_slice;

  //generate time-frequency distribution
  MatrixXf tfd = TFD_extract(input, nfft, total_slices, samples_per_slice, channels);

  write_eigen_to_file("some_spectogram.png", tfd);
  //test out convolution code
  MatrixXf guass_col = create_1d_gaussian_filter_col(16, 1, 1);
  MatrixXf guass_row = create_1d_gaussian_filter_row(16, 1, 1);
  MatrixXf sobel_x (1,3);
  MatrixXf sobel_y (3,1);
  //3x3 sobels but I only implemented 1d convolution :(
  //sobel_x << -1, 0, 1, -3, 0, 3, -1, 0, 1;
  //sobel_y << -1, -3, -1, 0, 0, 0, 1, 3, 1;
  sobel_x << -1, 0, 1;
  sobel_y << -1, 0, 1;

  MatrixXf tfd_y = one_d_convolve(tfd, sobel_y);
  MatrixXf tfd_x = one_d_convolve(tfd, sobel_x);

  write_eigen_to_file("y_sobel_spectogram.png", tfd_y);
  write_eigen_to_file("x_sobel_spectogram.png", tfd_x);

  MatrixXf sobel = one_d_convolve(tfd, sobel_y);
  sobel = one_d_convolve(sobel, sobel_x);
  write_eigen_to_file("abs_sobel_spectogram.png", sobel);

  //chunkify(sobel, 1,2);

  MatrixXf result = one_d_convolve(tfd, guass_col);
  result = one_d_convolve(result, guass_row);
  write_eigen_to_file("gauss_spectogram.png", result);
  MatrixXf dogg = dog(tfd, result);
  write_eigen_to_file("dog_spectogram.png", dogg);

  MatrixXf x_sobeled_dog = one_d_convolve(dogg, sobel_x);
  MatrixXf y_sobeled_dog = one_d_convolve(dogg, sobel_y);

  write_eigen_to_file("x_sobel_dog_spectogram.png", x_sobeled_dog * 100);
  write_eigen_to_file("y_sobel_dog_spectogram.png", y_sobeled_dog * 100);
  int vert = 1;
  int horz = 2;
  MatrixXi chunk_ids = chunkify(y_sobeled_dog, vert,horz);
  //MatrixXi stats = chunk_stats(chunk_ids);
  ChunkStats stats = ChunkStats(chunk_ids);
  list<Chunk> y_imps = cull_chunks(stats);
  chunk_ids = chunkify(x_sobeled_dog, vert,horz);
  stats = ChunkStats(chunk_ids);
  list<Chunk> x_imps = cull_chunks(stats);
  list<Chunk> all_imps;
  all_imps.insert(all_imps.begin(), x_imps.begin(), x_imps.end());
  all_imps.insert(all_imps.begin(), y_imps.begin(), y_imps.end());
  std::cout << "length of joined list is " << all_imps.size() << "\n";

    /// close files
  song.close();
  input.closeFile();
  output.closeFile();
}

//extract the time-frequency distribution of a song
//going to have it return eigen
MatrixXf TFD_extract(FileWvIn input, int nfft, int total_slices, int samples_per_slice, int channels) {
  bool is_inverse = false;
  //numbe of binds fft puts things into. apparently it's the same as nfft.
  //since nothing is placed in the upper half, divide by 2
  //though if you want to play around with inverse fft, just set to nfft I think?
  int freq_chunks = nfft / 2;
  kiss_fftr_cfg cfg = kiss_fftr_alloc( nfft ,is_inverse,0,0 );
  size_t input_size = sizeof(kiss_fft_scalar) * samples_per_slice;
  size_t output_size = sizeof(kiss_fft_cpx) * freq_chunks;
  kiss_fft_scalar* fft_input =(kiss_fft_scalar*)KISS_FFT_MALLOC(input_size);
  kiss_fft_cpx* fft_output =(kiss_fft_cpx*)KISS_FFT_MALLOC(output_size);
  bool print = false;
  StkFrames slice(samples_per_slice, channels);
  StkFrames single_chan(samples_per_slice, 1);

  MatrixXf tfd = MatrixXf(freq_chunks, total_slices);
  tfd.setZero();
  for (int slice_i = 0; slice_i < total_slices; slice_i++) {
    input.tick(slice);
    for (int i = 0; i < channels; i++) {
      slice.getChannel(i, single_chan, 0);
      for (int i = 0; i < samples_per_slice; i++) {
	fft_input[i] = single_chan[i];
      }
      kiss_fftr(cfg, fft_input, fft_output);
      for (int freq_i = 0; freq_i < freq_chunks; freq_i++) {
	kiss_fft_cpx a = fft_output[freq_i];
	double modulus =  pow(a.r, 2) + pow(a.i, 2);
	if (print) {
	  std::cout << modulus << " ";
	}
	//plus equals to get both channels
	tfd(freq_i, slice_i) += modulus;
      }
      if (print) {
	std::cout << "\n";
      }
    }
  }
  std::cout << tfd(tfd.rows() / 4, tfd.cols() - 1) << "\n";
  kiss_fftr_free(cfg);
  return tfd;
}

//do the inverse of the TFD, which I guess just returns the original input amplitudes?
//call this inside the regular TFD extract, hand in cpx, output some scalar value into some passed in stk-frame output, then in tfd, tick that to some output file
void TFDI_extract(kiss_fft_cpx* fft_input, int nfft, int samples_per_slice, StkFrames output) {
  bool print = true;
  bool is_inverse = true;
  int freq_chunks = nfft / 2 + 1;
  kiss_fftr_cfg cfg = kiss_fftr_alloc( nfft ,is_inverse,0,0 );
  //just swapped the input/output sizes
  //actually don't even need the input size since data is handed in
  //size_t input_size = sizeof(kiss_fft_cpx) * freq_chunks;
  size_t output_size = sizeof(kiss_fft_scalar) * samples_per_slice;

  kiss_fft_scalar* fft_output =(kiss_fft_scalar*)KISS_FFT_MALLOC(output_size);

  //kiss_fftri(cfg, fft_input, fft_output);
  //output is a bunch of scalars, put into some stack frame
  for (int freq_i = 0; freq_i < samples_per_slice; freq_i++) {
    kiss_fft_scalar a = fft_output[freq_i];
    if (print) {
      std::cout << a << " ";
    }
    output[freq_i] = a;
  }
  if (print) {
    std::cout << "\n";
  }
  kiss_fftr_free(cfg);
}

//creates a 1-dimensional gaussian filter, as an stk finite impulse response filter
//  int filter_size = 20;
//  Fir gaussian_filt = create_1d_gaussian_filter(20, 1, filter_size / 2, 1);
stk::Fir create_1d_gaussian_filter(int length, double amplitude, double center, double stddev) {
  std::vector<StkFloat> kernel(0);
  StkFloat val = 0;
  double exp = 0;
  for (int i = 0; i < length; i++) {
    exp = -1 * pow(i - center, 2) / stddev;
    val = amplitude * pow(M_E, exp);
    kernel.insert(kernel.begin(), val);
  }
  Fir ret (kernel);
  return ret;
}

MatrixXf create_1d_gaussian_filter_col(int length, double amplitude, double stddev) {
  MatrixXf kern (1, length);
  float val = 0;
  double exp = 0;
  double center = (double)length / 2;
  for (int i = 0; i < length; i++) {
    exp = -1 * pow(i - center, 2) / stddev;
    val = amplitude * pow(M_E, exp);
    kern(0,i) = val;
  }
  return kern;;
}

MatrixXf create_1d_gaussian_filter_row(int length, double amplitude, double stddev) {
  return create_1d_gaussian_filter_col(length, amplitude, stddev).transpose();
}

void make_fir_bandpass_filter(double* coefs, int taps, int freq_center, int freq_margin,  int max_freq) {
  double rel_center = (double)freq_center / max_freq;
  double rel_margin = (double)freq_margin / max_freq;
  TFIRPassTypes band = firBPF;
  TWindowType wt = wtSINC;
  double some_window_param = 10;
  RectWinFIR(coefs, taps, band, rel_center, rel_margin);
  FIRFilterWindow(coefs, taps, wt, some_window_param);
}

stk::Fir create_fir_from_coefs(double* coefs, int len) {
  std::vector<StkFloat> kernel(0);
  StkFloat val = 0;
  for (int i = 0; i < len; i++) {
    val = coefs[i];
    kernel.insert(kernel.begin(), val);
  }
  Fir ret (kernel);
  return ret;
}

MatrixXf one_d_convolve(MatrixXf mat, MatrixXf kern) {
  //perform valid 1d convolution on matrix
  //or cross-corelation, planning on using symetric kernels so doesn't matter
  int res_dim;
  int kern_dim = -1;
  MatrixXf temp, result;
  int d_start, d_end, d_width;

  if (kern.rows() == 1) {
    result = MatrixXf(mat.rows(), mat.cols() - (kern.cols() - 1));
    result.setZero();
    res_dim = result.cols();
    d_width = mat.rows();
    kern_dim = kern.cols();
    for (int i = 0; i < kern_dim; i++) {
      d_start = i;
      d_end = res_dim;
      temp = mat.block(0,d_start, d_width, d_end);
      result += temp * kern(0,i);
    }
  }
  else if (kern.cols() == 1) {
    result = MatrixXf(mat.rows() - (kern.rows() - 1), mat.cols());
    result.setZero();
    res_dim = result.rows();
    d_width = mat.cols();
    kern_dim = kern.rows();
    for (int i = 0; i < kern_dim; i++) {
      d_start = i;
      d_end = res_dim;
      temp = mat.block(d_start, 0, d_end, d_width);
      result += temp * kern(i, 0);
    }
  }
  else {
    //error, kernel is not 1d
  }
  if (kern_dim % 2 == 0) {
    //kernel has even dim and doesn't have a perfect center index, funky stuff may happen
  }
  return result;
}

MatrixXf dog(MatrixXf full, MatrixXf gauss) {
  int f_rows = full.rows();
  int f_cols = full.cols();
  int g_rows = gauss.rows();
  int g_cols = gauss.cols();
  if (f_rows == g_rows && f_cols == g_cols) {
    return full - gauss;
  }
  else {
    MatrixXf temp = full.block((f_rows - g_rows) / 2, (f_cols - g_cols) / 2, g_rows, g_cols);
    temp = temp - gauss;
    return temp.cwiseAbs();
  }
}

//stands for sub nonzero-average zeroing
//take average of all non-zero elements, zero anything below average
//takes matrix and rounds to apply snaz, zeros values in matrix
//returns average of penultimate round
//having snazr as anything above 1 is typically excessive, consider at most using 2
double snaz(MatrixXf filt, int snazr) {
  int freq_range = filt.rows();
  int time_range = filt.cols();
  double nz_tot = 0;
  double nz_avg = 0;
  int nz_count = 0;
  float val = 0;
  for (int i = 0; i <= snazr; i++) {
    nz_tot = 0;
    nz_count = 0;
    //don't want to include zeros in average, can't use builtin sum methods, have to traverse :(
    //combined two tasks in one, do the sub_average zeroing then compute new average
    //need to to 1 final loop after outermost for loop finishes to do final sub_average zeroing
    //accomplished by using i <= snazr and only recomputing nz_avg if i < snazr
    for (int freq_i = 0; freq_i < freq_range; freq_i++) {
      for (int time_i = 0; time_i < time_range; time_i++) {
	val = filt(freq_i, time_i);
	if (val < nz_avg) {
	  filt(freq_i, time_i) = 0.0;
	}
	else {
	  if (i < snazr) {
	    nz_tot += val;
	    nz_count++;
	  }
	}
      }
    }
    if (i < snazr) {
      nz_avg = nz_tot / nz_count;
      std::cout << "non-zero average for round " << i << " is " << nz_avg << "\n";
    }
  }
  return nz_avg;
}

MatrixXi chunkify(MatrixXf tfd, int vert_range, int horz_range) {
  //chunkify the input matrix
  //defining chunkify as assign groups of sound in tfd that are signifigant together
  //do this by locally grouping cells within proximity in matrix
  //output is a new matrix, same dimensions, values are positive intergers
  //zero indicated cell wasn't assigned a chunk
  //positive integer represents some group id to which the cell got assigned to
  int chunk_id = 0;
  int freq_range = tfd.rows();
  int time_range = tfd.cols();
  int temp_i = -1;
  int temp = 0;
  float nz_avg = 0;
  MatrixXf filt (freq_range, time_range);
  MatrixXi chunk_ids (freq_range, time_range);
  filt << tfd;
  chunk_ids.setZero( );
  //threshold values < average to zero
  //do this to not care about very minor sounds
  //sub-nonzero-average-zeroing rounds
  nz_avg = snaz(filt, 1);

  //isn't a perfect grouping algorithim, since the local grouping is kind of naive
  //hoping further culling methods down the line will get rid of bad groups
  //but loop could be improved by having a more robust/expensive joining loop(s)
  for (int time_i = 0; time_i < time_range; time_i++) {
    for (int freq_i = 0; freq_i < freq_range; freq_i++) {
      if (filt(freq_i, time_i) > nz_avg) {
	//check if you want to join it vertically
	for (int freq_off = 1; freq_off <= vert_range; freq_off++) {
	  //group into chunks within freq_off indexes below current cell
	  temp_i = freq_i - freq_off;
	  if (temp_i >= 0) {
	    temp = chunk_ids(temp_i, time_i);
	    if (temp != 0) {
	      chunk_ids(freq_i, time_i) = temp;
	      break;
	    }
	  }
	  else {
	    break;
	  }
	}
	//check if you want to join it horizontally, potentially overwriting vertical joins
	for (int time_off = 1; time_off <= horz_range; time_off++) {
	  temp_i = time_i - time_off;
	  if (temp_i >= 0) {
	    temp = chunk_ids(freq_i, temp_i);
	    if (temp != 0) {
	      chunk_ids(freq_i, time_i) = temp;
	      break;
	    }
	  }
	  else {
	    break;
	  }
	}
	//else assigning to a new group
	if (chunk_ids(freq_i, time_i) == 0) {
	  chunk_ids(freq_i, time_i) = ++chunk_id;
	}
      }
    }
  }
  //basic error checking, making sure sufficiently low values aren't being assigned chunk_ids
  for (int time_i = 0; time_i < time_range; time_i++) {
    for (int freq_i = 0; freq_i < freq_range; freq_i++) {
      if (chunk_ids(freq_i, time_i) != 00 && filt(freq_i, time_i) < nz_avg) {
	std::cout << filt(freq_i, time_i) <<  "bad! ";
      }
    }
  }
  return chunk_ids;
}

list<Chunk> cull_chunks(ChunkStats stats) {
  //wait I don't even need the chunkids anymore

  //thinking of returning a linked list of chunk objects
  //because working with matrixes will probably be annoying past this point

  list<Chunk> chunk_list;
  MatrixXi sizes = stats.get_size();
  int chunks = sizes.rows();
  //ignoring chunk_id 0
  sizes = stats.get_size().block(1,0, chunks - 1,1);
  chunks = sizes.rows();

  double average_size = (float)sizes.sum() / sizes.sum();
  MatrixXi minf = stats.get_min_freq();
  MatrixXi maxf = stats.get_max_freq();
  MatrixXi mint = stats.get_min_time();
  MatrixXi maxt = stats.get_max_time();
  int chunk_count = 0;
  for (int i = 0; i < chunks; i++) {
    if (sizes(i) > average_size) {
      //gather min, max, delta of time and frequency, construct a chunk
      //find a better name for chunks to many things have chunk in the name
      chunk_count++;
      Chunk temp = Chunk(minf(i), maxf(i), mint(i), maxt(i));
      chunk_list.insert(chunk_list.begin(), temp);
    }
  }
  std::cout << "original size was " << chunks << " culled size is " << chunk_count << "\n";
  return chunk_list;
}


#include <cstdlib>
#include <iostream>
#include <boost/lexical_cast.hpp>

#include "model.h"
#include "param.h"

using namespace std;
using namespace boost;
using namespace boost::random;

namespace nplm
{

    void model::resize(int ngram_size,
        int input_vocab_size,
        int output_vocab_size,
        int input_embedding_dimension,
        int num_hidden,
        int output_embedding_dimension)
{
    input_layer.resize(input_vocab_size, input_embedding_dimension, ngram_size-1);
    first_hidden_linear.resize(num_hidden, input_embedding_dimension*(ngram_size-1));
    first_hidden_activation.resize(num_hidden);
    second_hidden_linear.resize(output_embedding_dimension, num_hidden);
    second_hidden_activation.resize(output_embedding_dimension);
    output_layer.resize(output_vocab_size, output_embedding_dimension);
    this->ngram_size = ngram_size;
    this->input_vocab_size = input_vocab_size;
    this->output_vocab_size = output_vocab_size;
    this->input_embedding_dimension = input_embedding_dimension;
    this->num_hidden = num_hidden;
    this->output_embedding_dimension = output_embedding_dimension;
    premultiplied = false;
}

void model::initialize(mt19937 &init_engine, bool init_normal, double init_range, double init_bias)
{
    input_layer.initialize(init_engine, init_normal, init_range);
    output_layer.initialize(init_engine, init_normal, init_range, init_bias);
    first_hidden_linear.initialize(init_engine, init_normal, init_range);
    second_hidden_linear.initialize(init_engine, init_normal, init_range);
}

void model::premultiply()
{
    // Since input and first_hidden_linear are both linear,
    // we can multiply them into a single linear layer *if* we are not training
    int context_size = ngram_size-1;
    Matrix<double,Dynamic,Dynamic> U = first_hidden_linear.U;
    first_hidden_linear.U.resize(num_hidden, input_vocab_size * context_size);
    for (int i=0; i<context_size; i++)
        first_hidden_linear.U.middleCols(i*input_vocab_size, input_vocab_size) = U.middleCols(i*input_embedding_dimension, input_embedding_dimension) * input_layer.W->transpose();
    input_layer.W->resize(1,1); // try to save some memory
    premultiplied = true;
}

void model::readConfig(ifstream &config_file)
{
    string line;
    vector<string> fields;
    int ngram_size, vocab_size, input_embedding_dimension, num_hidden, output_embedding_dimension;
    activation_function_type activation_function = this->activation_function;
    while (getline(config_file, line) && line != "")
    {
        splitBySpace(line, fields);
	if (fields[0] == "ngram_size")
	    ngram_size = lexical_cast<int>(fields[1]);
	else if (fields[0] == "vocab_size")
	    input_vocab_size = output_vocab_size = lexical_cast<int>(fields[1]);
	else if (fields[0] == "input_vocab_size")
	    input_vocab_size = lexical_cast<int>(fields[1]);
	else if (fields[0] == "output_vocab_size")
	    output_vocab_size = lexical_cast<int>(fields[1]);
	else if (fields[0] == "input_embedding_dimension")
	    input_embedding_dimension = lexical_cast<int>(fields[1]);
	else if (fields[0] == "num_hidden")
	    num_hidden = lexical_cast<int>(fields[1]);
	else if (fields[0] == "output_embedding_dimension")
	    output_embedding_dimension = lexical_cast<int>(fields[1]);
	else if (fields[0] == "activation_function")
	    activation_function = string_to_activation_function(fields[1]);
	else if (fields[0] == "version")
	{
	    int version = lexical_cast<int>(fields[1]);
	    if (version != 1)
	    {
		cerr << "error: file format mismatch (expected 1, found " << version << ")" << endl;
		exit(1);
	    }
	}
	else
	    cerr << "warning: unrecognized field in config: " << fields[0] << endl;
    }
    resize(ngram_size,
        input_vocab_size,
        output_vocab_size,
        input_embedding_dimension,
        num_hidden,
        output_embedding_dimension);
    set_activation_function(activation_function);
}

void model::readConfig(const string &filename)
{
    ifstream config_file(filename.c_str());
    if (!config_file)
    {
        cerr << "error: could not open config file " << filename << endl;
	exit(1);
    }
    readConfig(config_file);
    config_file.close();
}

void model::read(const string &filename)
{
    vector<string> input_words;
    vector<string> output_words;
    read(filename, input_words, output_words);
}

void model::read(const string &filename, vector<string> &input_words, vector<string> &output_words)
{
    ifstream file(filename.c_str());
    if (!file) throw runtime_error("Could not open file " + filename);

    param myParam;
    string line;

    while (getline(file, line))
    {
	if (line == "\\config")
	{
	    readConfig(file);
	}

	else if (line == "\\vocab")
	{
	    input_words.clear();
	    readWordsFile(file, input_words);
	    output_words = input_words;
	}

	else if (line == "\\input_vocab")
	{
	    input_words.clear();
	    readWordsFile(file, input_words);
	}

	else if (line == "\\output_vocab")
	{
	    output_words.clear();
	    readWordsFile(file, output_words);
	}

	else if (line == "\\input_embeddings")
	    input_layer.read(file);
	else if (line == "\\hidden_weights 1")
	    first_hidden_linear.read(file);
	else if (line == "\\hidden_weights 2")
	    second_hidden_linear.read(file);
	else if (line == "\\output_weights")
	    output_layer.read_weights(file);
	else if (line == "\\output_biases")
	    output_layer.read_biases(file);
	else if (line == "\\end")
	    break;
	else if (line == "")
	    continue;
	else
	{
	    cerr << "warning: unrecognized section: " << line << endl;
	    // skip over section
	    while (getline(file, line) && line != "") { }
	}
    }
    file.close();
}

    void model::write(const string &filename, const vector<string> &input_words, const vector<string> &output_words)
{
    write(filename, &input_words, &output_words);
}

void model::write(const string &filename)
{
    write(filename, NULL, NULL);
}

    void model::write(const string &filename, const vector<string> *input_pwords, const vector<string> *output_pwords)
{
    ofstream file(filename.c_str());
    if (!file) throw runtime_error("Could not open file " + filename);

    file << "\\config" << endl;
    file << "version 1" << endl;
    file << "ngram_size " << ngram_size << endl;
    file << "input_vocab_size " << input_vocab_size << endl;
    file << "output_vocab_size " << output_vocab_size << endl;
    file << "input_embedding_dimension " << input_embedding_dimension << endl;
    file << "num_hidden " << num_hidden << endl;
    file << "output_embedding_dimension " << output_embedding_dimension << endl;
    file << "activation_function " << activation_function_to_string(activation_function) << endl;
    file << endl;

    if (input_pwords)
    {
        file << "\\input_vocab" << endl;
	writeWordsFile(*input_pwords, file);
	file << endl;
    }

    if (output_pwords)
    {
        file << "\\output_vocab" << endl;
	writeWordsFile(*output_pwords, file);
	file << endl;
    }

    file << "\\input_embeddings" << endl;
    input_layer.write(file);
    file << endl;

    file << "\\hidden_weights 1" << endl;
    first_hidden_linear.write(file);
    file << endl;

    file << "\\hidden_weights 2" << endl;
    second_hidden_linear.write(file);
    file << endl;

    file << "\\output_weights" << endl;
    output_layer.write_weights(file);
    file << endl;

    file << "\\output_biases" << endl;
    output_layer.write_biases(file);
    file << endl;

    file << "\\end" << endl;
    file.close();
}

} // namespace nplm


#pragma once

#include <boost/numeric/ublas/matrix.hpp>
#include "ADNConstants.hpp"
#include "ADNVectorMath.hpp"
#include "ADNLogger.hpp"

using LatticeType = ADNConstants::LatticeType;

namespace ublas = boost::numeric::ublas;

struct LatticeCell {
  double x_;
  double y_;
};

class DASLattice
{
public:
  DASLattice() = default;
  DASLattice(LatticeType type, double edgeDistance, int maxRows, int maxCols);
  ~DASLattice() = default;

  LatticeCell GetLatticeCell(unsigned int row, unsigned int column);

  size_t GetNumberRows();
  size_t GetNumberCols();

private:
  ublas::matrix<LatticeCell> mat_;

  void CreateSquareLattice(int maxRows, int maxCols);
  void CreateHoneycombLattice(int maxRows, int maxCols);

  double edgeDistance_;
};