// Pulse clas implimentation
// standard includes
#include <cmath>
#include <iterator>

// my headers
#include <Pulse.hpp>
#include <boost/math/interpolators/barycentric_rational.hpp>
#include <fftw3.h>
#include <algorithm>
#include <Constants.hpp>
#include <boost/lexical_cast.hpp>
#include <DataOps.hpp>
#include <random>
#include <cassert>
#include <cstdint> // for appendwavelegth() and int32_t

using namespace Constants;
using namespace DataOps;

PulseFreq::PulseFreq(const double omcenter_in=(0.55*fsPau<double>()),const double omwidth_in=(0.15*fsPau<double>()),const double omonoff_in=(0.1*fsPau<double>()), double tspan_in=(10000.0/fsPau<double>())):
	omega_center(omcenter_in),
	omega_width(omwidth_in ),
	omega_high( std::max(4.0*(omcenter_in + omwidth_in),10.0*omcenter_in) ),
	domega( 2.0*pi<double>()/tspan_in),
	intime(false),
	infreq(true),
	m_noisescale(1e-3),
	m_sampleinterval(2),
	m_saturate(4096),
	m_gain(1000000),
	m_lamsamples(1024),
	sampleround(1000),
	cvec(NULL),
	r_vec(NULL),
	hc_vecFT(NULL),
	r_vec_2x(NULL),
	hc_vec_2xFT(NULL)
{
	std::cout << "In constructor PulseFreq()" << std::endl;
	i_low =  (unsigned)(double( atof( getenv("nu_low") ) )* twopi<double>()*fsPau<double>()/domega);
	i_high =  (unsigned)(double( atof( getenv("nu_high") ) )* twopi<double>()*fsPau<double>()/domega);
	samples = (( (unsigned)(2.0 * omega_high / domega))/sampleround + 1 ) *sampleround;// dt ~ .1fs, Dt ~ 10000fs, dom = 2*pi/1e4, omhigh = 2*pi/.1fs, samples = omhigh/dom*2
	dtime = tspan_in/double(samples);
	omega_onwidth = omega_offwidth = omega_width/2.0; // forcing sin2 gaussian spectrum
	buildvectors(samples);
	nu0=omcenter_in/(2.0*pi<double>())*fsPau<double>();
	phase_GDD=phase_TOD=phase_4th=phase_5th=0.0;
	m_lamsamples = (size_t)atoi(getenv("lamsamples"));
	m_gain = boost::lexical_cast<float>( getenv("gain"));
	m_noisescale = boost::lexical_cast<double>( getenv("noisescale") ) ;
	m_sampleinterval = boost::lexical_cast<size_t>(getenv("sampleinterval"));
	m_saturate = uint16_t( boost::lexical_cast<int>( getenv("saturate")));
	std::cout << "exiting constructor PulseFreq()" << std::endl;
}

PulseFreq::PulseFreq(const PulseFreq &rhs) // deep-ish copy constructor
	:samples(rhs.samples)
	,omega_center(rhs.omega_center)
	,omega_width(rhs.omega_width)
	,omega_high(rhs.omega_high)
	,omega_onwidth(rhs.omega_onwidth)
	,domega(rhs.domega)
	,intime(rhs.intime)
	,infreq(rhs.infreq)
	,i_low(rhs.i_low)
	,i_high(rhs.i_high)
	,m_noisescale(rhs.m_noisescale)
	,m_sampleinterval(rhs.m_sampleinterval)
	,m_saturate(rhs.m_saturate)
	,m_gain(rhs.m_gain)
	,m_lamsamples(rhs.m_lamsamples)
	,sampleround(1000)
{
	//std::cerr << "\t\t\t+++++  Copy constructor of PulseFreq::PulseFreq(PulseFreq &rhs)\n\t\tsamples = " << samples << "\n" << std::flush;
	DataOps::clone(omega,rhs.omega);
	DataOps::clone(time,rhs.time);

	startind = rhs.startind;stopind=rhs.stopind;onwidth=rhs.onwidth;offwidth=rhs.offwidth;
	tspan = rhs.tspan;
	lambda_center=rhs.lambda_center;lambda_width=rhs.lambda_width;
	phase_GDD=rhs.phase_GDD;phase_TOD=rhs.phase_TOD;phase_4th=rhs.phase_4th;phase_5th=rhs.phase_5th;

	dtime = rhs.dtime;time_center=rhs.time_center;time_wdith=rhs.time_wdith;

	nu0=rhs.nu0;
	FTplan_forwardPtr = rhs.FTplan_forwardPtr;
	FTplan_backwardPtr = rhs.FTplan_backwardPtr;
	buildvectors(samples);

	DataOps::clone(rhovec,rhs.rhovec);
	DataOps::clone(phivec,rhs.phivec);
	DataOps::clone(cvec,rhs.cvec,samples);
	DataOps::clone(r_vec,rhs.r_vec,samples);
	DataOps::clone(hc_vecFT,rhs.hc_vecFT,samples);
	DataOps::clone(r_vec_2x,rhs.r_vec_2x,2*samples);
	DataOps::clone(hc_vec_2xFT,rhs.hc_vec_2xFT,2*samples);

	DataOps::clone(modamp,rhs.modamp);
	DataOps::clone(modphase,rhs.modphase);
}

PulseFreq & PulseFreq::operator=(const PulseFreq & rhs) // shallow-ish assignment
{
	//std::cerr << "\n\n\t\t########### !!!!!!! copying into a PulseFreq, make sure same enumber of samples !!!!!! ##########\n\n" << std::flush;
	//std::cerr << "\t\t\t+++++  assignment copy of PulseFreq::operator= with " << samples << " samples and " << rhs.getsamples() << " on rhs \n" << std::flush;
	samples=rhs.samples;
	omega_center=rhs.omega_center;
	omega_width=rhs.omega_width;
	omega_high=rhs.omega_high;
	omega_onwidth=rhs.omega_onwidth;
	domega=rhs.domega;
	intime=rhs.intime;
	infreq=rhs.infreq;
	omega=rhs.omega; // these are static vectors... i'm trying not to make copies just yet
	time=rhs.time; // these are static vectors... i'm trying not to make copies just yet
	i_low=rhs.i_low;
	i_high=rhs.i_high;
	m_noisescale=rhs.m_noisescale;
	m_sampleinterval=rhs.m_sampleinterval;
	m_saturate=rhs.m_saturate;
	m_gain=rhs.m_gain;
	m_lamsamples=rhs.m_lamsamples;

	startind = rhs.startind;stopind=rhs.stopind;onwidth=rhs.onwidth;offwidth=rhs.offwidth;
	tspan = rhs.tspan;
	lambda_center=rhs.lambda_center;lambda_width=rhs.lambda_width;
	phase_GDD=rhs.phase_GDD;phase_TOD=rhs.phase_TOD;phase_4th=rhs.phase_4th;phase_5th=rhs.phase_5th;

	dtime = rhs.dtime;time_center=rhs.time_center;time_wdith=rhs.time_wdith;

	nu0=rhs.nu0;
	FTplan_forwardPtr = rhs.FTplan_forwardPtr;
	FTplan_backwardPtr = rhs.FTplan_backwardPtr;

	DataOps::clone(rhovec,rhs.rhovec);
	DataOps::clone(phivec,rhs.phivec);
	DataOps::clone(cvec,rhs.cvec,samples);
	DataOps::clone(r_vec,rhs.r_vec,samples);
	DataOps::clone(hc_vecFT,rhs.hc_vecFT,samples);
	DataOps::clone(r_vec_2x,rhs.r_vec_2x,2*samples);
	DataOps::clone(hc_vec_2xFT,rhs.hc_vec_2xFT,2*samples);

	DataOps::clone(modamp,rhs.modamp);
	DataOps::clone(modphase,rhs.modphase);
	return *this;
}

PulseFreq::~PulseFreq(void){
	killvectors();
}

void PulseFreq::rhophi2cvec(void)
{
	for (size_t i=0;i<samples;i++){
		cvec[i] = std::polar(rhovec[i],phivec[i]);
	}
}
void PulseFreq::cvec2rhophi(void)
{
	for (size_t i=0;i<samples;i++){
		rhovec[i] = std::abs(cvec[i]);
		phivec[i] = std::arg(cvec[i]);
	}
}

PulseFreq & PulseFreq::operator+=(const PulseFreq &rhs){
	DataOps::sum(cvec,rhs.cvec,samples);
	cvec2rhophi();
	return *this;
}

PulseFreq & PulseFreq::operator-=(const PulseFreq &rhs){
	DataOps::diff(cvec,rhs.cvec,samples);
	cvec2rhophi();
        return *this;
}

PulseFreq & PulseFreq::operator*=(const PulseFreq &rhs){
	DataOps::mul(cvec,rhs.cvec,samples);
	cvec2rhophi();
	return *this;
}

PulseFreq & PulseFreq::operator*=(const double s){
	DataOps::mul(cvec,s,samples);
	cvec2rhophi();
	return *this;
}

PulseFreq & PulseFreq::operator/=(const PulseFreq &rhs){
	DataOps::div(cvec,rhs.cvec,samples);
	cvec2rhophi();
        return *this;
}

PulseFreq & PulseFreq::interfere(const PulseFreq &rhs){
	*this += rhs;
        return *this;
}

PulseFreq & PulseFreq::diffamps(const PulseFreq &rhs){
	rhovec -= rhs.rhovec;
	std::fill(phivec.begin(),phivec.end(),0);
	rhophi2cvec();
        return *this;
}

PulseFreq & PulseFreq::normamps(const PulseFreq &rhs){
	rhovec /= rhs.rhovec;
	std::fill(phivec.begin(),phivec.end(),0);
	rhophi2cvec();
        return *this;
}

void PulseFreq::print_amp(std::ofstream & outfile)
{
	outfile << "# amp\n";
	outfile << rhovec << std::endl;
}
void PulseFreq::print_phase(std::ofstream & outfile)
{
	outfile << "# phase\n";
	outfile << phivec << std::endl;
}
void PulseFreq::print_phase_powerspectrum(std::ofstream & outfile)
{
/*
	double * phase = (double *) fftw_malloc(sizeof(double) * samples);
	double * phaseFT = (double *) fftw_malloc(sizeof(double) * samples);
	fftw_plan plan_r2hc = fftw_plan_r2r_1d(samples,
			phase,
			phaseFT,
			FFTW_R2HC,
			FFTW_MEASURE
			);
*/
	std::copy(phivec.begin(),phivec.end(),r_vec);
	fftw_execute_r2r(*FTplan_r2hcPtr.get(),r_vec,hc_vecFT);

	outfile << "# power spectrum of the Fourier phase\n";
	outfile << std::pow(hc_vecFT[0],int(2)) << "\n";
	for (size_t i = 1; i<samples/2;++i){
		outfile << std::pow(hc_vecFT[i],int(2)) + std::pow(hc_vecFT[samples-i],int(2)) << "\n";
	}
	outfile << std::pow(hc_vecFT[samples/2],int(2)) << std::endl;
	outfile << std::endl;
}

bool PulseFreq::addrandomphase(void)
{
	if (!infreq){
		std::cerr << "died here at addrandomphase()" << std::endl;
		return false;
	}
	size_t sz = samples*2; // doubling the vector to mirror it so that DFT hansles the phase well

	double * randphase = (double *) fftw_malloc(sizeof(double) * sz);
	double * randphaseFT = (double *) fftw_malloc(sizeof(double) * sz);
/*

	fftw_plan plan_r2hc = fftw_plan_r2r_1d(sz,
			randphase,
			randphaseFT,
			FFTW_R2HC,
			FFTW_MEASURE
			);
	fftw_plan plan_hc2r = fftw_plan_r2r_1d(sz,
			randphaseFT,
			randphase,
			FFTW_HC2R,
			FFTW_MEASURE
			);

*/

	std::uniform_real_distribution<double> distribution(
		(double(atof(getenv("randphase_mean")))-double(atof(getenv("randphase_std"))))*Constants::pi<double>(),
		(double(atof(getenv("randphase_mean")))+double(atof(getenv("randphase_std"))))*Constants::pi<double>()
		);

	double phase = distribution(rng);
	randphase[0] = phase;
	randphase[samples/2] = -phase;
	for (size_t i = 1; i<samples/2;i++){
		phase = distribution(rng);
		randphase[i] = phase;
		randphase[samples-i] = -phase;
	}
	for (size_t i=sz-1;i>sz/2-1;--i){
		randphase[i] = randphase[sz-i];
	}

	size_t lowpass = boost::lexical_cast<size_t>(atoi(getenv("phaseNoiseLowpass")));
	std::cerr << "\n======== lowpass is " << lowpass << " =======\n" << std::flush;

	fftw_execute_r2r(*FTplan_r2hc_2xPtr.get(),randphase,randphaseFT);
	std::fill(randphaseFT+lowpass,randphaseFT+sz-lowpass,0.);
	for (size_t i=1;i<lowpass;++i){
		double filter = std::pow(std::cos(double(i)/(double(lowpass)) * Constants::half_pi<double>() ),int(2));
		randphaseFT[i] *= filter;
		randphaseFT[sz-i] *= filter;
	}
	randphaseFT[sz/2] = 0.;
	fftw_execute_r2r(*FTplan_hc2r_2xPtr.get(),randphaseFT,randphase);

	for (size_t i=0;i<samples;++i){
		phivec[i] += randphase[i]/samples;
	}
	rhophi2cvec();
	return true;
}

void PulseTime::setstrength(const double in)
{
  strength = in * auenergy<double>()/Eh<double>() * std::pow(aufor10PW<double>(),int(2));
}

void PulseTime::setwidth(const double in)
{
  Ctau = in * Constants::root_pi<double>()/ Constants::fsPau<double>() / 2.0;
}

void PulseTime::sett0(const double in)
{
  t0 = in / fsPau<double>();
}

void PulseFreq::attenuate(double attenfactor){
	rhovec *= attenfactor;
	rhophi2cvec();
}
void PulseFreq::phase(double phasein){ // expects delay in units of pi , i.e. 1.0 = pi phase flip
	if(intime){
		fft_tofreq();
	}
	phivec += phasein*Constants::pi<double>();
	rhophi2cvec();
	if(intime){
		fft_totime();
	}
}
void PulseFreq::delay(double delayin){ // expects delay in fs
	if(intime){
		fft_tofreq();
	}
	for (unsigned i=0;i<samples;i++){
		phivec[i] += omega[i]*delayin/fsPau<double>();
	}
	rhophi2cvec();
	if(intime){
		fft_totime();
	}
}

void PulseFreq::printfrequency(std::ofstream * outfile){
	double nu,lambda;
	(*outfile) << ("#omega[fs^-1]\tlambda[nm]\trho\tphi\n");
	for (unsigned i = i_low;i<i_high;i+=(unsigned)(atoi(getenv("sampleinterval")))){
		nu = (omega[i]/(2.0*pi<double>())/fsPau<double>());
		lambda = C_nmPfs<double>()/nu;
		(*outfile) << nu << "\t" << lambda << "\t" << std::pow(rhovec[i],int(2)) << "\t" << phivec[i] << "\n";
	}
}
void PulseFreq::printwavelengthbins(std::ofstream * outfile)
{
	std::vector<double> x(2);
	x.front() = C_nmPfs<double>()*2.0*pi<double>()*fsPau<double>()/omega[i_low];
	x.back() = C_nmPfs<double>()*2.0*pi<double>()*fsPau<double>()/omega[i_high-1];
	double dlam = (x.front()-x.back())/double(m_lamsamples);
        for (size_t i = 0;i<m_lamsamples;++i){
		(*outfile) << x.back() + i*dlam << "\t";
        }
	(*outfile) << "\n";
	return;
}
void PulseFreq::appendwavelength(std::ofstream * outfile)
{
	std::vector<double> x(i_high-i_low);
	std::vector<double> y(i_high-i_low);
	for (size_t i=0;i<y.size();++i){
		x[i] = C_nmPfs<double>()*2.0*pi<double>()*fsPau<double>()/omega[i_low+i];
		//y[i] = std::pow(rhovec[i_low+i],int(2)) * 200000000000;
		y[i] = std::min(std::pow(rhovec[i_low+i],int(2)) * m_gain,double(m_saturate));
	}
	double dlam = (x.front()-x.back())/double(m_lamsamples);
	boost::math::barycentric_rational<double> interpolant(x.data(), y.data(), y.size());
	for (size_t i=0;i<m_lamsamples;++i){
		(*outfile) << uint16_t(interpolant(x.back()+i*dlam)) << "\t";
	}
	(*outfile) << std::endl;
	return;
}
void PulseFreq::appendwavelength_bin(std::ofstream * outfile)
{
	std::vector<double> x(i_high-i_low);
	std::vector<double> y(i_high-i_low);
	for (size_t i=0;i<y.size();++i){
		x[i] = C_nmPfs<double>()*2.0*pi<double>()*fsPau<double>()/omega[i_low+i];
		//y[i] = std::pow(rhovec[i_low+i],int(2)) * 200000000000;
		y[i] = std::min(std::pow(rhovec[i_low+i],int(2)) * m_gain,double(m_saturate));
	}
	double dlam = (x.front()-x.back())/double(m_lamsamples);
	boost::math::barycentric_rational<double> interpolant(x.data(), y.data(), y.size());
	for (size_t i=0;i<m_lamsamples;++i){
		(*outfile) << int32_t(interpolant(x.back()+i*dlam));
	}
	return;
}
void PulseFreq::appendfrequency(std::ofstream * outfile){
        for (unsigned i = i_low;i<i_high;i+=m_sampleinterval){
		uint16_t val = std::min(uint16_t(rhovec[i] * m_gain),uint16_t(m_saturate));
       		(*outfile) << std::pow(val,int(2)) << "\t";
        }
	(*outfile) << std::endl;
}

void PulseFreq::appendnoisy(std::ofstream * outfile){
	std::normal_distribution<double> norm_dist( 0.0, m_noisescale);
        for (unsigned i = i_low;i<i_high;i+=m_sampleinterval){
		double outval = std::pow(rhovec[i],int(2)) + norm_dist(rng);
       		(*outfile) << outval << "\t" ;
        }
	(*outfile) << std::endl;
}

void PulseFreq::printfrequencybins(std::ofstream * outfile){
	double nu,lam;
        for (unsigned i = i_low;i<i_high;i+=(unsigned)(atoi(getenv("sampleinterval")))){
		nu = (omega[i]/(2.0*pi<double>())/fsPau<double>());
		lam = C_nmPfs<double>()/nu;
       		(*outfile) << nu << "\t" << lam << "\n";
        }
	(*outfile) << "\n";
}
void PulseFreq::appendfrequencybins(std::ofstream * outfile){
	double nu,lam;
        for (unsigned i = i_low;i<i_high;i+=(unsigned)(atoi(getenv("sampleinterval")))){
		nu = (omega[i]/(2.0*pi<double>())/fsPau<double>());
       		(*outfile) << nu << "\t";
        }
	(*outfile) << "\n";
}
void PulseFreq::printfrequencydelay(std::ofstream * outfile, const double *delay){
	double nu,lambda,thisdelay;
	thisdelay = (*delay)*fsPau<double>();
	(*outfile) << ("#omega[fs^-1]\tlambda[nm]\trho\tphi\tdelay[fs]\n");
	for (unsigned i = i_low;i<i_high;i+=(unsigned)(atoi(getenv("sampleinterval")))){
		nu = (omega[i]/(2.0*pi<double>())/fsPau<double>());
		lambda = C_nmPfs<double>()/nu;
		(*outfile) << nu << "\t" << lambda << "\t" << rhovec[i] << "\t" << phivec[i] << "\t" << thisdelay << "\n";
	}
	(*outfile) << "\n";
}
void PulseFreq::printfrequencydelaychirp(std::ofstream * outfile, const double *delay,const double *chirp){
	double nu,lambda,thisdelay,thischirp,reltime;
	thisdelay = (*delay)*fsPau<double>();
	thischirp = (*chirp)*std::pow(fsPau<double>(),int(2));
	(*outfile) << ("#omega[fs^-1]\tlambda[nm]\trho\tphi\tdelay[fs]\tchirp[fs^2]\treldelays[fs]\n");
	for (unsigned i = i_low;i<i_high;i+=(unsigned)(atoi(getenv("sampleinterval")))){
		nu = (omega[i]/(2.0*pi<double>())/fsPau<double>());
		lambda = C_nmPfs<double>()/nu;
		reltime = (nu-nu0)*thischirp*10.0;
		(*outfile) << nu << "\t" << lambda << "\t" << rhovec[i] << "\t" << phivec[i] << "\t" << thisdelay << "\t" << thischirp << "\t" << reltime << "\n";
	}
	(*outfile) << "\n";
}

void PulseFreq::printwavelength(std::ofstream * outfile,const double *delay){
	double nu,lambda,lamlast;
	lamlast=0;
        for (unsigned i = i_high;i>i_low;i-=(unsigned)(atoi(getenv("sampleinterval")))){
                nu = (omega[i]/(2.0*pi<double>())/fsPau<double>());
                lambda = (double)((int)((C_nmPfs<double>()/nu)*10.0))/10.0;
		if (lambda>lamlast & (int)(lambda*10)%5 == 0){
                	(*outfile) << lambda << "\t" << (*delay) << "\t" << std::pow(rhovec[i],int(2)) << "\n";
			lamlast = lambda;
		}
        }
	(*outfile) << "\n";
}

void PulseFreq::printtime(std::ofstream * outfile){
	(*outfile) << ("#time\treal\timag\n");
	for (unsigned i = samples/2;i<samples;i++){
		(*outfile) << (time[i]*fsPau<double>()) << "\t" << cvec[i].real() << "\t" << cvec[i].imag() << "\n";
	}
	for (unsigned i = 0;i<samples/2;i++){
		(*outfile) << (time[i]*fsPau<double>()) << "\t" << cvec[i].real() << "\t" << cvec[i].imag() << "\n";
	}

}

void PulseFreq::buildvectors(const size_t s){
	//std::cerr << "allocating with fftw_malloc with samples = " << samples << std::endl;
	cvec = (std::complex<double> *) fftw_malloc(sizeof(std::complex<double>) * size_t(s));
        std::fill(cvec,cvec + samples,std::complex<double>(0));
	r_vec = (double *) fftw_malloc(sizeof(double) * size_t(s));
        std::fill(r_vec,r_vec + s,double(0));
	//std::cerr << "\t\t...allocated with fftw_malloc with samples = " << samples << std::endl;
	hc_vecFT = (double *) fftw_malloc(sizeof(double) * size_t(s));
        std::fill(hc_vecFT,hc_vecFT + s,double(0));
	//std::cerr << "allocating with fftw_malloc with samples = " << (2*samples) << std::endl;
	r_vec_2x = (double *) fftw_malloc(sizeof(double) * size_t(s) * 2);
        //std::fill(r_vec_2x,r_vec_2x + 2*samples,double(0));
	hc_vec_2xFT = (double *) fftw_malloc(sizeof(double) * size_t(s) * 2);
        std::fill(hc_vec_2xFT,hc_vec_2xFT + 2*s,double(0));
	//std::cerr << "\t\t...allocated with fftw_malloc with 2*samples = " << (2*samples) << std::endl;

	rhovec.resize(s,0.0);
	phivec.resize(s,0.0);
	modamp.resize(s,1.0);
	modphase.resize(s,0.0);
	omega.resize(s);
	time.resize(s);

	omega[0] = 0.0;
	time[0] = 0.0;
	omega[s/2] = -(double)(s/2)*domega;
	time[s/2] = -(double)(s/2)*dtime;

	startind = (unsigned)((omega_center-(omega_width/2.0))/domega);
	stopind = (unsigned)((omega_center+(omega_width/2.0))/domega);
	onwidth = (unsigned)(omega_onwidth/domega); // 2.0)/domega);//  /10.0)/domega);// sin^2 goes from0..1 in 0..pi/2
	offwidth = (unsigned)(omega_offwidth/domega); // 2.0)/domega);//  /10.0)/domega);// sin^2 goes from0..1 in 0..pi/2
	for (unsigned i = 1; i<startind;i++){
		omega[i] = domega*i;
		time[i] = dtime*i;
		omega[samples-i] = -domega*i;
		time[samples-i] = -dtime*i;
	}
	for (unsigned i = startind;i<startind+onwidth; i++){
		omega[i] = domega*i;
		time[i] = dtime*i;
		rhovec[i] = rising(i);
		cvec[i] = std::polar(rhovec[i],phivec[i]);
		omega[s-i] = -domega*(double)i;
		time[s-i] = -dtime*(double)i;
		rhovec[s-i] = rising(i);
		cvec[s-i] = std::polar(rhovec[s-i],phivec[s-i]);
	}
	for (unsigned i = startind+onwidth;i<stopind-offwidth; i++){
		omega[i] = domega*i;
		time[i] = dtime*i;
		rhovec[i] = 1.0;
		cvec[i] = std::polar(rhovec[i],phivec[i]);
		omega[s-i] = -domega*(double)i;
		time[s-i] = -dtime*(double)i;
		rhovec[s-i] = 1.0;
		cvec[s-i] = std::polar(rhovec[s-i],phivec[s-i]);
	}
	for (unsigned i = stopind-offwidth;i<stopind; i++){
		omega[i] = domega*i;
		time[i] = dtime*i;
		rhovec[i] = falling(i);
		cvec[i] = std::polar(rhovec[i],phivec[i]);
		omega[s-i] = -domega*i;
		time[s-i] = -dtime*i;
		rhovec[s-i] = falling(i);
		cvec[s- i] = std::polar(rhovec[s- i],phivec[s- i]);
	}
	for (unsigned i = stopind;i<s/2; i++){
		omega[i] = domega*i;
		time[i] = dtime*i;
		omega[s-i] = -domega*i;
		time[s-i] = -dtime*i;
	}

}
void PulseFreq::killvectors(void){
	fftw_free(cvec);
	fftw_free(r_vec);
	fftw_free(hc_vecFT);
	fftw_free(r_vec_2x);
	fftw_free(hc_vec_2xFT);
	cvec = NULL;
	r_vec = hc_vecFT = r_vec_2x = hc_vec_2xFT = NULL;
}

void PulseFreq::setplans(const PulseFreq & rhs)
{
	FTplan_forwardPtr = rhs.FTplan_forwardPtr;
	FTplan_backwardPtr = rhs.FTplan_backwardPtr;
}
void PulseFreq::setmasterplans(fftw_plan * const forward,fftw_plan * const backward)
{
	assert(FTplan_forwardPtr.use_count()==0 && FTplan_backwardPtr.use_count()==0);
	*forward = fftw_plan_dft_1d(samples,
			reinterpret_cast<fftw_complex*>(cvec),
			reinterpret_cast<fftw_complex*>(cvec),
			FFTW_FORWARD, FFTW_ESTIMATE);
	*backward = fftw_plan_dft_1d(samples,
			reinterpret_cast<fftw_complex*>(cvec),
			reinterpret_cast<fftw_complex*>(cvec),
			FFTW_BACKWARD, FFTW_ESTIMATE);
	FTplan_forwardPtr = std::make_shared<fftw_plan> (*forward);
	FTplan_backwardPtr = std::make_shared<fftw_plan> (*backward);
}
void PulseFreq::setancillaryplans(fftw_plan * const r2hc,fftw_plan * const hc2r,fftw_plan * const r2hc_2x,fftw_plan * const hc2r_2x)
{

	assert(FTplan_r2hcPtr.use_count()==0
			&& FTplan_hc2rPtr.use_count()==0
			&& FTplan_r2hc_2xPtr.use_count()==0
			&& FTplan_hc2r_2xPtr.use_count()==0);
	*r2hc = fftw_plan_r2r_1d(samples,
			r_vec,
			hc_vecFT,
			FFTW_R2HC,
			FFTW_MEASURE
			);
	*hc2r = fftw_plan_r2r_1d(samples,
			hc_vecFT,
			r_vec,
			FFTW_HC2R,
			FFTW_MEASURE
			);
	*r2hc_2x = fftw_plan_r2r_1d(2*samples,
			r_vec_2x,
			hc_vec_2xFT,
			FFTW_R2HC,
			FFTW_MEASURE
			);
	*hc2r_2x = fftw_plan_r2r_1d(2*samples,
			hc_vec_2xFT,
			r_vec_2x,
			FFTW_HC2R,
			FFTW_MEASURE
			);
	FTplan_r2hcPtr = std::make_shared<fftw_plan> (*r2hc);
	FTplan_hc2rPtr = std::make_shared<fftw_plan> (*hc2r);
	FTplan_r2hc_2xPtr = std::make_shared<fftw_plan> (*r2hc_2x);
	FTplan_hc2r_2xPtr = std::make_shared<fftw_plan> (*hc2r_2x);

}


// This file is part of SWGANH which is released under the MIT license.
// See file LICENSE or go to http://swganh.com/LICENSE

#include "random_generator.h"

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

using namespace swganh;
using namespace boost::random;

RandomGenerator::RandomGenerator(uniform_int_distribution<> distribution)
    : generator(rand())
    , distribution_(distribution)
{
}

RandomGenerator::RandomGenerator(int start, int end)
    : generator()
    , distribution_(uniform_int_distribution<>(start, end))
{
}

int RandomGenerator::Rand()
{
    int rand = distribution_(generator);
    last_random = rand;
    return rand;
}

float RandomGenerator::Randf()
{
	boost::uniform_real<float> u(0.0f, 1.0f);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > gen(generator, u);
	last_randomf = gen();
	return last_randomf;
}

int RandomGenerator::Rand(int start, int end)
{
    distribution_ = uniform_int_distribution<>(start, end);
    return Rand();
}

float RandomGenerator::Randf(float start, float end)
{
	boost::uniform_real<float> u(start, end);
	boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > gen(generator, u);
	last_randomf = gen();
	return last_randomf;
}

int RandomGenerator::LastRand()
{
    return last_random;
}

float RandomGenerator::LastRandf()
{
	return last_randomf;
}


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/shapes.hh"
#include "nauths/mml/tools.hh"
#include "nauths/mml/io.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  int next()
  {
    return sqrt(rand()) / 5;
  }

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::Point<mml::Num> Vector;

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

// TODO debug::shape: check operations work when nearing INT_MAX

BOOST_AUTO_TEST_SUITE(shape)

// shape: empty

BOOST_AUTO_TEST_CASE(EMPTY)
{
  Shape s0;

  BOOST_REQUIRE_EQUAL(s0.type(), mml::shape::EMPTY);
}

// shape: point

BOOST_AUTO_TEST_CASE(POINT)
{
  Shape s0(Point(next(), next()));
  Shape s1(s0);

  BOOST_REQUIRE_EQUAL(s0.type(), mml::shape::POINT);
  BOOST_REQUIRE_EQUAL(s1.type(), mml::shape::POINT);
  BOOST_REQUIRE_EQUAL(s0, s1);

  BOOST_REQUIRE_EQUAL(s0.center(), s0.point());

  int dx = next();
  int dy = next();

  s0.move_of(Vector(dx, dy));
  BOOST_REQUIRE_EQUAL(s0, s1.moved_of(Vector(dx, dy)));
  BOOST_REQUIRE_EQUAL(s0.moved_of(-dx, -dy), s1);
}

// shape: rect

BOOST_AUTO_TEST_CASE(RECT)
{
  int x0 = next();
  int x1 = next();
  int y0 = next();
  int y1 = next();

  Shape s0(Shape::rect(
             std::min(x0, x1), std::min(y0, y1),
             std::max(x0, x1), std::max(y0, y1)));
  Shape s1(s0);
  Shape s2(s0);

  BOOST_REQUIRE_EQUAL(s0.type(), mml::shape::RECT);
  BOOST_REQUIRE_EQUAL(s1.type(), mml::shape::RECT);
  BOOST_REQUIRE_EQUAL(s0, s1);

  int dx = next();
  int dy = next();
  Vector d(dx, dy);

  s1.move_of(dx, dy);
  s2.move_of(d);

  BOOST_REQUIRE_EQUAL(s0.rect().min() + Vector(dx, dy), s1.rect().min());
  BOOST_REQUIRE_EQUAL(s0.rect().max() + Vector(dx, dy), s1.rect().max());
  BOOST_REQUIRE_EQUAL(s0.rect().min(), s1.rect().min() - Vector(dx, dy));
  BOOST_REQUIRE_EQUAL(s0.rect().max(), s1.rect().max() - Vector(dx, dy));
  BOOST_REQUIRE_EQUAL(s0.moved_of(dx, dy), s1);

  BOOST_REQUIRE_EQUAL(s0.rect().min() + d, s2.rect().min());
  BOOST_REQUIRE_EQUAL(s0.rect().max() + d, s2.rect().max());
  BOOST_REQUIRE_EQUAL(s0.rect().min(), s2.rect().min() - d);
  BOOST_REQUIRE_EQUAL(s0.rect().max(), s2.rect().max() - d);
  BOOST_REQUIRE_EQUAL(s0.moved_of(d), s2);

  s1.move_to(0, 0);
  s2.move_to(Point(0, 0));

  BOOST_REQUIRE_EQUAL(s1.center(), Point(0, 0));
  BOOST_REQUIRE_EQUAL(s2.center(), Point(0, 0));
  BOOST_REQUIRE_EQUAL(s1, s2);
}

// shape: circle

BOOST_AUTO_TEST_CASE(CIRCLE)
{
  Shape s0 = Shape::circle(next(), next(), next());
  Shape s1(s0);

  BOOST_REQUIRE_EQUAL(s0.type(), mml::shape::CIRCLE);
  BOOST_REQUIRE_EQUAL(s1.type(), mml::shape::CIRCLE);
  BOOST_REQUIRE_EQUAL(s0, s1);
}

// shape: polygon

BOOST_AUTO_TEST_CASE(POLYGON)
{
  Shape s0 = Shape::hexagon(Point(-50,    0),
                            Point(-10,  -50),
                            Point( 10,  -50),
                            Point( 50,    0),
                            Point( 10,   50),
                            Point(-10,   50));
  Shape s1(s0);

  BOOST_REQUIRE_EQUAL(s0.type(), mml::shape::POLYGON);
  BOOST_REQUIRE_EQUAL(s1.type(), mml::shape::POLYGON);
  BOOST_REQUIRE_EQUAL(s0, s1);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by huangkun on 2020/9/15.
//

#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>

#include <opengv2/event_camera_calib/EventCalibIni.hpp>
#include <opengv2/event/EventFrame.hpp>
#include <opengv2/system/SystemBase.hpp>
#include <opengv2/sensor/PinholeCamera.hpp>
#include <opengv2/feature/FeatureIdentifier.hpp>

opengv2::EventCalibIni::EventCalibIni(MapBase::Ptr map, CalibrationSetting::Ptr calibrationSetting,
                                      double motionTimeStep)
        : TrackingBase(std::move(map)), calibrationSetting_(calibrationSetting), motionTimeStep_(motionTimeStep) {}

bool opengv2::EventCalibIni::initialization(opengv2::Bodyframe::Ptr bf) {
    system_->map->addFrame(bf);
    return true;
}

bool opengv2::EventCalibIni::track(opengv2::Bodyframe::Ptr bf) {
    /****** check similarity with nearby pattern ******/
    Bodyframe::Ptr bf_ref;
    system_->map->keyframeLockShared();
    auto itr = system_->map->keyframes().lower_bound(bf->timeStamp());
    if (itr != system_->map->keyframes().end())
        bf_ref = itr->second;
    else {
        if (system_->map->keyframes().empty())
            throw std::logic_error("Tracking Not Initialized!");

        bf_ref = system_->map->keyframes().rbegin()->second;
    }
    system_->map->keyframeUnlockShared();

    double duration = std::abs(bf->timeStamp() - bf_ref->timeStamp());
    // identify outliers that pattern orientation changed in a very short time
    auto ref = dynamic_cast<EventFrame *>(bf_ref->frame(0).get());
    auto cur = dynamic_cast<EventFrame *>(bf->frame(0).get());

    const auto &height = calibrationSetting_->circlePatternParameters->rows;
    const auto &width = calibrationSetting_->circlePatternParameters->cols;
    vectorofEigenMatrix<Eigen::Vector2d> refLineSet, curLineSet;
    for (int i = 0; i < height; i++) {
        Eigen::MatrixXd A(width, 3);
        A.col(2).setOnes();
        for (int j = 0; j < width; j++) {
            A.block<1, 2>(j, 0) = ref->features()[i * width + j]->location();
        }

        Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);
        refLineSet.emplace_back(svd.matrixV()(1, 2), -svd.matrixV()(0, 2)); // B -A

        Eigen::Vector2d d = A.block<1, 2>(width - 1, 0) - A.block<1, 2>(0, 0);// B -A or -B A
        if (refLineSet.back().dot(d) < 0)
            refLineSet.back() = -refLineSet.back();
    }
    for (int i = 0; i < height; i++) {
        Eigen::MatrixXd A(width, 3);
        A.col(2).setOnes();
        for (int j = 0; j < width; j++) {
            A.block<1, 2>(j, 0) = cur->features()[i * width + j]->location();
        }

        Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);
        curLineSet.emplace_back(svd.matrixV()(1, 2), -svd.matrixV()(0, 2)); // B -A

        Eigen::Vector2d d = A.block<1, 2>(width - 1, 0) - A.block<1, 2>(0, 0);// B -A or -B A
        if (curLineSet.back().dot(d) < 0)
            curLineSet.back() = -curLineSet.back();
    }

    std::vector<double> thetaSet;
    for (int i = 0; i < height; ++i) {
        double theta = std::acos(refLineSet[i].dot(curLineSet[i]) / (refLineSet[i].norm() * curLineSet[i].norm()));
        thetaSet.push_back(theta);
    }
    std::nth_element(thetaSet.begin(), thetaSet.begin() + thetaSet.size() / 2, thetaSet.end());

    if (thetaSet[thetaSet.size() / 2] / duration < (5e-4 * M_PI) / motionTimeStep_) { // Angular velocity rad/second
        system_->map->addFrame(bf);

        /*if (thetaSet[thetaSet.size() / 2] > M_PI / 4) {
            std::cerr << "Please check!" << std::endl;
            cv::waitKey();
        }*/

        if (system_->viewer != nullptr)
            system_->viewer->updateViewer();

        return true;
    }

    return false;
}

void opengv2::EventCalibIni::calcBoardCornerPositions(std::vector<cv::Point3f> &corners) {
    corners.clear();

    if (calibrationSetting_->circlePatternParameters->isAsymmetric) {
        for (int i = 0; i < calibrationSetting_->circlePatternParameters->rows; i++)
            for (int j = 0; j < calibrationSetting_->circlePatternParameters->cols; j++)
                corners.emplace_back((2 * j + i % 2) * calibrationSetting_->circlePatternParameters->squareSize,
                                     i * calibrationSetting_->circlePatternParameters->squareSize, 0);
    } else {
        for (int i = 0; i < calibrationSetting_->circlePatternParameters->rows; ++i)
            for (int j = 0; j < calibrationSetting_->circlePatternParameters->cols; ++j)
                corners.emplace_back(j * calibrationSetting_->circlePatternParameters->squareSize,
                                     i * calibrationSetting_->circlePatternParameters->squareSize, 0);
    }
}

static double computeReprojectionErrors(const std::vector<std::vector<cv::Point3f> > &objectPoints,
                                        const std::vector<std::vector<cv::Point2f> > &imagePoints,
                                        const std::vector<cv::Mat> &rvecs, const std::vector<cv::Mat> &tvecs,
                                        const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
                                        std::vector<float> &perViewErrors, bool fisheye) {
    std::vector<cv::Point2f> imagePoints2;
    size_t totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for (size_t i = 0; i < objectPoints.size(); ++i) {
        if (fisheye) {
            cv::fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix,
                                       distCoeffs);
        } else {
            projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
        }
        err = norm(imagePoints[i], imagePoints2, cv::NORM_L2);

        size_t n = objectPoints[i].size();
        perViewErrors[i] = (float) std::sqrt(err * err / n);
        totalErr += err * err;
        totalPoints += n;
    }

    return std::sqrt(totalErr / totalPoints);
}

bool opengv2::EventCalibIni::cvCalibration() {
    std::vector<cv::Mat> rvecs, tvecs;
    std::vector<float> reprojErrs;

    cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
    if (calibrationSetting_->aspectRatio != 0)
        cameraMatrix.at<double>(0, 0) = calibrationSetting_->aspectRatio;

    cv::Mat distCoeffs;
    if (calibrationSetting_->useFisheye) {
        distCoeffs = cv::Mat::zeros(4, 1, CV_64F);
    } else {
        distCoeffs = cv::Mat::zeros(8, 1, CV_64F);
    }

    int step = system_->map->frameNum() / calibrationSetting_->NumOfFrameToUse;
    if (step == 0) {
        calibrationSetting_->NumOfFrameToUse = system_->map->frameNum();
        step = 1;
    }

    // select frames for intrinsic initialization
    std::vector<std::vector<cv::Point2f>> imagePoints(calibrationSetting_->NumOfFrameToUse);
    int counter = 0;
    system_->map->keyframeLockShared();
    for (auto itr = system_->map->keyframes().cbegin();
         counter < calibrationSetting_->NumOfFrameToUse; std::advance(itr, step), counter++) {
        auto bf = itr->second;
        auto cf = dynamic_cast<EventFrame *>(bf->frame(0).get());
        for (const auto &f:cf->features()) {
            imagePoints[counter].emplace_back(f->location()[0], f->location()[1]);
        }
    }
    system_->map->keyframeUnlockShared();

    std::vector<std::vector<cv::Point3f> > objectPoints(1);
    calcBoardCornerPositions(objectPoints[0]);
    objectPoints.resize(imagePoints.size(), objectPoints[0]);

    //Find intrinsic and extrinsic camera parameters
    double rms;
    auto camera = dynamic_cast<PinholeCamera *>(system_->map->firstKeyframe()->frame(0)->sensor().get());
    cv::Size imageSize(camera->size()[0], camera->size()[1]);
    if (calibrationSetting_->useFisheye) {
        cv::Mat _rvecs, _tvecs;
        rms = cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, _rvecs,
                                     _tvecs, calibrationSetting_->flag);

        rvecs.reserve(_rvecs.rows);
        tvecs.reserve(_tvecs.rows);
        for (int i = 0; i < int(objectPoints.size()); i++) {
            rvecs.push_back(_rvecs.row(i));
            tvecs.push_back(_tvecs.row(i));
        }
    } else {
        rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs,
                              calibrationSetting_->flag | cv::CALIB_USE_LU);
    }

    std::cout << "Re-projection error reported by calibrateCamera: " << rms << std::endl;

    bool ok = cv::checkRange(cameraMatrix) && cv::checkRange(distCoeffs);
    double totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix,
                                                   distCoeffs, reprojErrs, calibrationSetting_->useFisheye);

    std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
              << ". avg re projection error = " << totalAvgErr << std::endl;

    int counter1 = 0, counter2 = 0, counter3 = 0;
    if (ok) {
        Eigen::Matrix3d K;
        cv::cv2eigen(cameraMatrix, K);
        Eigen::VectorXd distortion;
        cv::cv2eigen(distCoeffs, distortion);
        std::cout << K << std::endl;
        std::cout << distortion.transpose() << std::endl;

        // set camera intrinsics
        camera->setK(K);
        camera->distCoeffs() = distortion;

        std::map<double, Bodyframe::Ptr> buffer = map_->copyKeyframes();

        // add pattern landmarks to map
        for (int i = 0; i < objectPoints[0].size(); ++i) {
            Eigen::Vector3d Xw(objectPoints[0][i].x, objectPoints[0][i].y, objectPoints[0][i].z);
            LandmarkBase::Ptr lm = std::make_shared<LandmarkBase>(i, Xw);
            for (auto &itr : buffer) {
                auto &bf = itr.second;
                auto cf = dynamic_cast<EventFrame *>(bf->frame(0).get());
                if (i < cf->features().size()) {
                    FeatureIdentifier fi(bf->timeStamp(), 0, i, cf->features()[i]);
                    lm->addObservation(fi);
                    cf->features()[i]->setLandmark(lm);
                }
            }
            system_->map->addLandmark(lm);
        }

        // clear frames in map, lm->observations() will be cleared.
        for (auto &itr : buffer) {
            auto &bf = itr.second;
            system_->map->removeFrame(bf->timeStamp());
        }

        // solvePnPRansac: initialize frame poses
        for (auto &itr : buffer) {
            auto &bf = itr.second;
            cv::Mat rvec, tvec;
            std::vector<cv::Point2f> imageP;
            std::vector<int> inliers;
            auto cf = dynamic_cast<EventFrame *>(bf->frame(0).get());
            for (const auto &f:cf->features()) {
                imageP.emplace_back(f->location()[0], f->location()[1]);
            }
            cv::solvePnPRansac(objectPoints[0], imageP, cameraMatrix, distCoeffs, rvec, tvec, false, 50, 4.0, 0.99,
                               inliers, cv::SOLVEPNP_IPPE);

            /*// check bad frame
            if (inliers.size() < imageP.size() * 0.25) {
                counter3++;
                continue;
            }*/

            cv::Mat cvRsw;
            cv::Rodrigues(rvec, cvRsw);
            Eigen::Matrix3d Rsw;
            Eigen::Vector3d tsw;
            cv::cv2eigen(cvRsw, Rsw);
            cv::cv2eigen(tvec, tsw);
            Eigen::Quaterniond Qsw(Rsw);
            Qsw.normalize();

            // set poses
            Eigen::Vector3d twb = Qsw.conjugate() * (Bodyframe::tsb(0) - tsw);
            Eigen::Quaterniond unitQwb = Qsw.conjugate() * Bodyframe::unitQsb(0);
            bf->setPose(twb, unitQwb);

            // check pose in map
            if (checkPose(bf)) {
                std::unordered_set<int> outlierIdxs;
                outlierIdxs.reserve(imageP.size());
                for (int i = 0; i < imageP.size(); ++i) {
                    outlierIdxs.insert(i);
                }
                for (auto inlier: inliers) {
                    outlierIdxs.erase(inlier);
                }

                // refinement on outlier features
                if (cf->rectifyFeatures(outlierIdxs, Rsw, tsw)) {
                    system_->map->addFrame(bf);
                } else {
                    counter2++;
                }
            } else {
                counter1++;
            }
        }

        std::map<double, Bodyframe::Ptr> frames = map_->copyKeyframes();
        for (auto &itr : frames) {
            auto &bf = itr.second;
            for (int i = 0; i < bf->size(); ++i) {
                if (auto cf = dynamic_cast<CameraFrame *>(bf->frame(i).get())) {
                    for (int j = 0; j < cf->features().size(); j++) {
                        auto f = cf->features()[j];
                        if (auto lm = f->landmark()) {
                            FeatureIdentifier fi(bf->timeStamp(), i, j, f);
                            lm->addObservation(fi);
                        }
                    }
                }
            }
        }

        system_->viewer->updateViewer();
    }

    std::cout << counter1 << " frames discard by checkPose." << std::endl;
    std::cout << counter2 << " frames discard by rectifyFeature." << std::endl;
    //std::cout << counter3 << " frames discard by check inlierNum." << std::endl;
    return ok;
}

bool opengv2::EventCalibIni::checkPose(Bodyframe::Ptr cur) const {
    if (system_->map->empty()) {
        return true;
    }
    auto ref = system_->map->lastKeyframe();

    double duration = cur->timeStamp() - ref->timeStamp();

    double v_t = (ref->unitQwb().conjugate() * (cur->twb() - ref->twb())).norm() / duration;
    double v_R = std::abs(cur->unitQwb().angularDistance(ref->unitQwb()) / duration);

    //std::cout << "v_t: " << v_t << " v_R: " << v_R << std::endl;

    if (v_t < (2.5e-1 / motionTimeStep_) * 2 /*cm per second*/ &&
        v_R < (5e-4 * M_PI) * 2 / motionTimeStep_ /*rad per second*/)
        return true;
    else
        return false;
}


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/three.hpp>

template <typename T, int N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(int i = 0; i < N; ++i)
  {
    a1[i] = ((i%2) ? T(i*2) : T(-2*i))/T(3);
    b[i] = bs::trunc(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::trunc(aa1), bb);
}

STF_CASE_TPL("Check trunc on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" trunc real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::trunc;
  using p_t = bs::pack<T>;
  using r_t = decltype(trunc(p_t()));

  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(trunc(bs::Inf<p_t>()), bs::Inf<p_t>());
  STF_EQUAL(trunc(bs::Minf<p_t>()), bs::Minf<p_t>());
  STF_IEEE_EQUAL(trunc(bs::Nan<p_t>()), bs::Nan<p_t>());
#endif
  STF_EQUAL(trunc(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(trunc(bs::Mone<p_t>()), bs::Mone<p_t>());
  STF_EQUAL(trunc(bs::Zero<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(trunc(bs::Pi<p_t>()), bs::Three<p_t>());
  STF_EQUAL(trunc(p_t(1.4)), p_t(1));
  STF_EQUAL(trunc(p_t(1.5)), p_t(1));
  STF_EQUAL(trunc(p_t(1.6)), p_t(1));
  STF_EQUAL(trunc(p_t(2.5)), p_t(2));
  STF_EQUAL(trunc(p_t(-1.4)), p_t(-1));
  STF_EQUAL(trunc(p_t(-1.5)), p_t(-1));
  STF_EQUAL(trunc(p_t(-1.6)), p_t(-1));
  STF_EQUAL(trunc(p_t(-2.5)), p_t(-2));
} // end of test for floating_

STF_CASE_TPL (" trunc unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::trunc;
  using p_t = bs::pack<T>;
  using r_t = decltype(trunc(p_t()));

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

  // specific values tests
  STF_EQUAL(trunc(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(trunc(bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" trunc signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::trunc;
  using p_t = bs::pack<T>;
  using r_t = decltype(trunc(p_t()));

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

  // specific values tests
  STF_EQUAL(trunc(bs::Mone<p_t>()), bs::Mone<p_t>());
  STF_EQUAL(trunc(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(trunc(bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for signed_int_

STF_CASE_TPL ( "fast trunc real",  STF_IEEE_TYPES)
{

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::trunc;
  using p_t = bs::pack<T>;
  using r_t = decltype(trunc(p_t()));

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

  // specific values tests
  STF_EQUAL(bs::fast_(trunc)(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(bs::fast_(trunc)(bs::Mone<p_t>()), bs::Mone<p_t>());
  STF_EQUAL(bs::fast_(trunc)(bs::Zero<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(bs::fast_(trunc)(bs::Pi<p_t>()), bs::Three<p_t>());
  STF_EQUAL(bs::fast_(trunc)(p_t(1.4)), p_t(1));
  STF_EQUAL(bs::fast_(trunc)(p_t(1.5)), p_t(1));
  STF_EQUAL(bs::fast_(trunc)(p_t(1.6)), p_t(1));
  STF_EQUAL(bs::fast_(trunc)(p_t(2.5)), p_t(2));
  STF_EQUAL(bs::fast_(trunc)(p_t(-1.4)), p_t(-1));
  STF_EQUAL(bs::fast_(trunc)(p_t(-1.5)), p_t(-1));
  STF_EQUAL(bs::fast_(trunc)(p_t(-1.6)), p_t(-1));
  STF_EQUAL(bs::fast_(trunc)(p_t(-2.5)), p_t(-2));
} // end of test for floating_


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_default_latent_model_hpp
#define quantlib_default_latent_model_hpp

#include <ql/experimental/credit/basket.hpp>
#include <ql/experimental/math/latentmodel.hpp>
#include <ql/experimental/math/gaussiancopulapolicy.hpp>
#include <boost/dynamic_bitset.hpp>

namespace QuantLib {

    /*! \brief Default event Latent Model.

     This is a model for joint default events based on a generic Latent
      Model. It models solely the default events in a portfolio, not making any
      reference to severities, exposures, etc...
     An implicit correspondence is stablished between the variables modelled and
     the names in the basket given by the basket and model variable access
     indices.
     The class is parametric on the Latent Model copula.

     \todo Consider QL_REQUIRE(basket_, "No portfolio basket set.") test in
     debug model only for performance reasons.
    */
    template<class copulaPolicy>
    class DefaultLatentModel : public LatentModel<copulaPolicy> {
        // import template members
    protected:
        using LatentModel<copulaPolicy>::factorWeights_;
        using LatentModel<copulaPolicy>::idiosyncFctrs_;
        using LatentModel<copulaPolicy>::copula_;
    public:
        using LatentModel<copulaPolicy>::inverseCumulativeY;
        using LatentModel<copulaPolicy>::cumulativeZ;
        using LatentModel<copulaPolicy>::integratedExpectedValue;// which one?
    protected:
        // not a handle, the model doesnt keep any cached magnitudes, no need
        //  for notifications, still...
        mutable ext::shared_ptr<Basket> basket_;
        ext::shared_ptr<LMIntegration> integration_;
    private:
        typedef typename copulaPolicy::initTraits initTraits;
    public:
        /*!
        @param factorWeights Latent model independent factors weights for each
            variable.
        @param integralType Integration type.
        @param ini Copula initialization if any.

        \warning Baskets with realized defaults not tested/WIP.
        */
        DefaultLatentModel(
            const std::vector<std::vector<Real> >& factorWeights,
            LatentModelIntegrationType::LatentModelIntegrationType integralType,
            const initTraits& ini = initTraits()
            )
        : LatentModel<copulaPolicy>(factorWeights, ini),
          integration_(LatentModel<copulaPolicy>::IntegrationFactory::
            createLMIntegration(factorWeights[0].size(), integralType))
        { }
        DefaultLatentModel(
            const Handle<Quote>& mktCorrel,
            Size nVariables,
            LatentModelIntegrationType::LatentModelIntegrationType integralType,
            const initTraits& ini = initTraits()
            )
        : LatentModel<copulaPolicy>(mktCorrel, nVariables, ini),
          integration_(LatentModel<copulaPolicy>::IntegrationFactory::
            createLMIntegration(1, integralType))
        { }
        /* \todo
            Add other constructors as in LatentModel for ease of use. (less
            dimensions, factors, etcc...)
        */

        /* To interface with loss models. It is possible to change the basket
        since there are no cached magnitudes.
        */
        void resetBasket(const ext::shared_ptr<Basket> basket) const {
            basket_ = basket;
            // in the future change 'size' to 'liveSize'
            QL_REQUIRE(basket_->size() == factorWeights_.size(),
                "Incompatible new basket and model sizes.");
        }
    public:
        /*! Returns the probability of default of a given name conditional on
        the realization of a given set of values of the model independent
        factors. The date at which the probability is given is implicit in the
        probability since theres not other time dependence in this model.
        @param prob Unconditional probability of default.
        @param iName desired name.
        @param mktFactors Value of LM independent factors.
        \warning Most often it is preferred to use the method below avoiding the
        cumulative inversion.
        */
        Probability conditionalDefaultProbability(Probability prob, Size iName,
            const std::vector<Real>& mktFactors) const
        {
            // we can be called from the outside (from an integrable loss model)
            //   but we are called often at integration points. This or
            //   consider a list of friends.
        #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(basket_, "No portfolio basket set.");
        #endif
            /*Avoid redundant call to minimum value inversion (might be \infty),
            and this independently of the copula function.
            */
            if (prob < 1.e-10) return 0.;// use library macro...
            return conditionalDefaultProbabilityInvP(
                inverseCumulativeY(prob, iName), iName, mktFactors);
        }
    protected:
        void update() {
            if(basket_) basket_->notifyObservers();
            LatentModel<copulaPolicy>::update();
        }
    public:// open since users access it for performance on joint integrations.

        /*! Returns the probability of default of a given name conditional on
        the realization of a given set of values of the model independent
        factors. The date at which the probability is given is implicit in the
        probability since theres not other time dependent in this model.
        Same intention as above but provides a performance opportunity, if the
        integration is along the market factors (as usually is) avoids computing
        the inverse of the probability on each call.
        @param invCumYProb Inverse cumul of the unconditional probability of
          default, has to follow the same copula law for results to be coherent
        @param iName desired name.
        @param m Value of LM independent factors.
        */
        Probability conditionalDefaultProbabilityInvP(Real invCumYProb,
            Size iName,
            const std::vector<Real>& m) const {
            Real sumMs =
                std::inner_product(factorWeights_[iName].begin(),
                    factorWeights_[iName].end(), m.begin(), 0.);
            Real res = cumulativeZ((invCumYProb - sumMs) /
                    idiosyncFctrs_[iName] );
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE (res >= 0. && res <= 1.,
                        "conditional probability " << res << "out of range");
            #endif

            return res;
        }
    protected:
        /*! Returns the probability of default of a given name conditional on
        the realization of a given set of values of the model independent
        factors.
        @param date The date for the probability of default.
        @param iName desired name.
        @param mktFactors Value of LM independent factors.

        Same intention as the above methods. Usage of this one is typically more
        expensive because most often the date we call this method with
        repeats itself and with this one the probability can not be cached
        outside the call.
        */
        Probability conditionalDefaultProbability(const Date& date, Size iName,
            const std::vector<Real>& mktFactors) const
        {
            const ext::shared_ptr<Pool>& pool = basket_->pool();
            Probability pDefUncond =
                pool->get(pool->names()[iName]).
                defaultProbability(basket_->defaultKeys()[iName])
                  ->defaultProbability(date);
            return conditionalDefaultProbability(pDefUncond, iName, mktFactors);
        }
        /*! Conditional default probability product, intermediate step in the
            correlation calculation.*/
        Probability condProbProduct(Real invCumYProb1, Real invCumYProb2,
            Size iName1, Size iName2,
            const std::vector<Real>& mktFactors) const {
            return
                conditionalDefaultProbabilityInvP(invCumYProb1, iName1,
                    mktFactors) *
                conditionalDefaultProbabilityInvP(invCumYProb2, iName2,
                    mktFactors);
        }
        //! Conditional probability of n default events or more.
        // \todo: check the issuer has not defaulted.
        Real conditionalProbAtLeastNEvents(Size n, const Date& date,
            const std::vector<Real>& mktFactors) const;
        //! access to integration:
        const ext::shared_ptr<LMIntegration>&
            integration() const { return integration_; }
    public:
        /*! Computes the unconditional probability of default of a given name.
        Trivial method for testing
        */
        Probability probOfDefault(Size iName, const Date& d) const {
            QL_REQUIRE(basket_, "No portfolio basket set.");
            const ext::shared_ptr<Pool>& pool = basket_->pool();
            // avoid repeating this in the integration:
            Probability pUncond = pool->get(pool->names()[iName]).
                defaultProbability(basket_->defaultKeys()[iName])
                ->defaultProbability(d);
            if (pUncond < 1.e-10) return 0.;

            return integratedExpectedValue(
              ext::function<Real (const std::vector<Real>& v1)>(
                ext::bind(
                &DefaultLatentModel<copulaPolicy>
                    ::conditionalDefaultProbabilityInvP,
                this,
                inverseCumulativeY(pUncond, iName),
                iName,
                ext::placeholders::_1)
              ));
        }
        /*! Pearsons' default probability correlation.
            Users should consider specialization on the copula type for specific
            distributions since that might simplify the integrations, most
            importantly if this is to be used in calibration of observations for
            factor coefficients as it is expensive to integrate directly.
        */
        Real defaultCorrelation(const Date& d, Size iNamei, Size iNamej) const;

        /*! Returns the probaility of having a given or larger number of
        defaults in the basket portfolio at a given time.
        */
        Probability probAtLeastNEvents(Size n, const Date& date) const {
            return integratedExpectedValue(
             ext::function<Real (const std::vector<Real>& v1)>(
              ext::bind(
              &DefaultLatentModel<copulaPolicy>::conditionalProbAtLeastNEvents,
              this,
              n,
              ext::cref(date),
              ext::placeholders::_1)
             ));
        }
    };

    //---- Defines -----------------------------------------------------------

    template<class CP>
    Real DefaultLatentModel<CP>::defaultCorrelation(const Date& d,
        Size iNamei, Size iNamej) const
    {
        QL_REQUIRE(basket_, "No portfolio basket set.");

        const ext::shared_ptr<Pool>& pool = basket_->pool();
        // unconditionals:
        Probability pi = pool->get(pool->names()[iNamei]).
            defaultProbability(basket_->defaultKeys()[iNamei])
            ->defaultProbability(d);
        Probability pj = pool->get(pool->names()[iNamej]).
            defaultProbability(basket_->defaultKeys()[iNamej])
            ->defaultProbability(d);
        Real pipj = pi * pj;
        Real invPi = inverseCumulativeY(pi, iNamei);
        Real invPj = inverseCumulativeY(pj, iNamej);
        // avoid repetitive calls when i=j?
        Real E1i1j; // joint default covariance term
        if(iNamei !=iNamej) {
            E1i1j = integratedExpectedValue(
              ext::function<Real (const std::vector<Real>& v1)>(
                ext::bind(
                &DefaultLatentModel<CP>::condProbProduct,
                this, invPi, invPj, iNamei, iNamej,
                ext::placeholders::_1) ));
        }else{
            E1i1j = pi;
        }
        return (E1i1j - pipj )/std::sqrt(pipj*(1.-pi)*(1.-pj));
    }

    template<class CP>
    Real DefaultLatentModel<CP>::conditionalProbAtLeastNEvents(Size n,
        const Date& date,
        const std::vector<Real>& mktFactors) const {
            QL_REQUIRE(basket_, "No portfolio basket set.");

            /* \todo
            This algorithm traverses all permutations starting form the
            lowest one. This is inneficient, there shouldnt be any need to
            go through the invalid ones. Use combinations of n elements.

            See integration in O'Kane for homogeneous ntds.
            */
            // first position with as many defaults as desired:
            Size poolSize = basket_->size();//move to 'livesize'
            const ext::shared_ptr<Pool>& pool = basket_->pool();

            BigNatural limit =
                static_cast<BigNatural>(std::pow(2., (int)(poolSize)));

            // Precalc conditional probabilities
            std::vector<Probability> pDefCond;
            for(Size i=0; i<poolSize; i++)
                pDefCond.push_back(conditionalDefaultProbability(
                    pool->get(pool->names()[i]).
                    defaultProbability(basket_->defaultKeys()[i])->
                    defaultProbability(date), i, mktFactors));

            Probability probNEventsOrMore = 0.;
            for(BigNatural mask =
                  static_cast<BigNatural>(std::pow(2., (int)(n))-1);
                mask < limit; mask++)
            {
                // cheap permutations
                boost::dynamic_bitset<> bsetMask(poolSize, mask);
                if(bsetMask.count() >= n) {
                    Probability pConfig = 1;
                    for(Size i=0; i<bsetMask.size(); i++)
                        pConfig *=
                          (bsetMask[i] ? pDefCond[i] : (1.- pDefCond[i]));
                    probNEventsOrMore += pConfig;
                }
            }
            return probNEventsOrMore;
        }

    // often used:
    typedef DefaultLatentModel<GaussianCopulaPolicy> GaussianDefProbLM;
    typedef DefaultLatentModel<TCopulaPolicy> TDefProbLM;
}

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct Pooling2dFixture : public ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    explicit Pooling2dFixture(const char* poolingtype)
    {
        m_Prototext =  "node {\n"
            "  name: \"Placeholder\"\n"
            "  op: \"Placeholder\"\n"
            "  attr {\n"
            "    key: \"dtype\"\n"
            "    value {\n"
            "      type: DT_FLOAT\n"
            "    }\n"
            "  }\n"
            "  attr {\n"
            "    key: \"value\"\n"
            "    value {\n"
            "      tensor {\n"
            "        dtype: DT_FLOAT\n"
            "        tensor_shape {\n"
            "        }\n"
            "      }\n"
            "    }\n"
            "   }\n"
            "  }\n"
            "node {\n"
            "  name: \"";
        m_Prototext.append(poolingtype);
        m_Prototext.append("\"\n"
                               "  op: \"");
        m_Prototext.append(poolingtype);
        m_Prototext.append("\"\n"
                               "  input: \"Placeholder\"\n"
                               "  attr {\n"
                               "    key: \"T\"\n"
                               "    value {\n"
                               "      type: DT_FLOAT\n"
                               "    }\n"
                               "  }\n"
                               "  attr {\n"
                               "    key: \"data_format\"\n"
                               "    value {\n"
                               "      s: \"NHWC\"\n"
                               "    }\n"
                               "  }\n"
                               "  attr {\n"
                               "    key: \"ksize\"\n"
                               "    value {\n"
                               "      list {\n"
                               "        i: 1\n"
                               "        i: 2\n"
                               "        i: 2\n"
                               "        i: 1\n"
                               "      }\n"
                               "    }\n"
                               "  }\n"
                               "  attr {\n"
                               "    key: \"padding\"\n"
                               "    value {\n"
                               "      s: \"VALID\"\n"
                               "    }\n"
                               "  }\n"
                               "  attr {\n"
                               "    key: \"strides\"\n"
                               "    value {\n"
                               "      list {\n"
                               "        i: 1\n"
                               "        i: 1\n"
                               "        i: 1\n"
                               "        i: 1\n"
                               "      }\n"
                               "    }\n"
                               "  }\n"
                               "}\n");

        SetupSingleInputSingleOutput({ 1, 2, 2, 1 }, "Placeholder", poolingtype);
    }
};

struct MaxPoolFixture : Pooling2dFixture
{
    MaxPoolFixture() : Pooling2dFixture("MaxPool") {}
};
BOOST_FIXTURE_TEST_CASE(ParseMaxPool, MaxPoolFixture)
{
    RunTest<4>({1.0f, 2.0f, 3.0f, -4.0f}, {3.0f});
}

struct AvgPoolFixture : Pooling2dFixture
{
    AvgPoolFixture() : Pooling2dFixture("AvgPool") {}
};
BOOST_FIXTURE_TEST_CASE(ParseAvgPool, AvgPoolFixture)
{
    RunTest<4>({1.0f, 2.0f, 3.0f, 4.0f}, {2.5f});
}

BOOST_AUTO_TEST_SUITE_END()


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Use, modification and distribution is 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)
//
// Custom Box Example

#include <iostream>

#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/box.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/util/write_dsv.hpp>

struct my_point
{
    double x, y;
};

struct my_int_point
{
    int x, y;
};

struct my_box
{
    my_point ll, ur;
};

struct my_box_ltrb
{
    int left, top, right, bottom;
};

struct my_box_4
{
    double coors[4];
};

template <typename P>
struct my_box_t
{
    P ll, ur;
};

BOOST_GEOMETRY_REGISTER_POINT_2D(my_point, double, cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_POINT_2D(my_int_point, int, cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_BOX(my_box, my_point, ll, ur)
BOOST_GEOMETRY_REGISTER_BOX_TEMPLATED(my_box_t, ll, ur)
BOOST_GEOMETRY_REGISTER_BOX_2D_4VALUES(my_box_ltrb, my_int_point, left, top, right, bottom)
BOOST_GEOMETRY_REGISTER_BOX_2D_4VALUES(my_box_4, my_point, coors[0], coors[1], coors[2], coors[3])

int main()
{
    my_point p = boost::geometry::make<my_point>(3.5, 3.5);
    my_box b = boost::geometry::make<my_box>(0, 0, 2, 2);
    my_box_ltrb b1 = boost::geometry::make<my_box_ltrb>(0, 0, 3, 3);
    my_box_4 b4 = boost::geometry::make<my_box_4>(0, 0, 4, 4);
    my_box_t<my_point> bt = boost::geometry::make<my_box_t<my_point> >(0, 0, 5, 5);

    std::cout << boost::geometry::dsv(p) << " IN " << boost::geometry::dsv(b)
            << " : " << int(boost::geometry::within(p, b)) << std::endl;
    std::cout << boost::geometry::dsv(p) << " IN " << boost::geometry::dsv(b1)
            << " : " << int(boost::geometry::within(p, b1)) << std::endl;
    std::cout << boost::geometry::dsv(p) << " IN " << boost::geometry::dsv(b4)
            << " : " << int(boost::geometry::within(p, b4)) << std::endl;
    std::cout << boost::geometry::dsv(p) << " IN " << boost::geometry::dsv(bt)
            << " : " << int(boost::geometry::within(p, bt)) << std::endl;

    return 0;
}


#ifndef STAN_MATH_PRIM_FUN_BESSEL_SECOND_KIND_HPP
#define STAN_MATH_PRIM_FUN_BESSEL_SECOND_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{bessel\_second\_kind}(v, x) =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0 \\
     Y_v(x) & \mbox{if } x > 0 \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{bessel\_second\_kind}(v, x)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0 \\
     \frac{\partial\, Y_v(x)}{\partial x} & \mbox{if } x > 0 \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   Y_v(x)=\frac{J_v(x)\cos(v\pi)-J_{-v}(x)}{\sin(v\pi)}
   \f]

   \f[
   \frac{\partial \, Y_v(x)}{\partial x} = \frac{v}{x}Y_v(x)-Y_{v+1}(x)
   \f]
 *
 */
template <typename T2, require_arithmetic_t<T2>* = nullptr>
inline T2 bessel_second_kind(int v, const T2 z) {
  return boost::math::cyl_neumann(v, z);
}

/**
 * Enables the vectorised application of the bessel second kind function, when
 * the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return Bessel second kind function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto bessel_second_kind(const T1& a, const T2& b) {
  return apply_scalar_binary(a, b, [&](const auto& c, const auto& d) {
    return bessel_second_kind(c, d);
  });
}

}  // namespace math
}  // namespace stan

#endif


#include <boost/math/common_factor.hpp>
#include <iostream>

int main() {
   std::cout << "The greatest common divisor of 12 and 18 is " << boost::math::gcd(12, 18) << "!\n";
}


#include <boost/circular_buffer.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <range/v3/algorithm.hpp>
#include <range/v3/numeric.hpp>
#include <range/v3/view.hpp>
namespace views = ranges::views;

int main(int argc, char **argv)
{
  int n = 277678;
  int c = 0;
  int i = 1;
  for (;; i += 2) {
    if (n == std::clamp(n, i * i + 1, (i + 2) * (i + 2))) {
      c = (i + 2) * (i + 2) - (i * i);
      break;
    }
  }
  std::cout << i << std::endl;
  std::cout << c << std::endl;
  std::cout << n << std::endl;

  auto corner = ((i + 2) * (i + 2) - n) % (c / 4);
  std::cout << corner << std::endl;
  if (corner > c / 8) {
    corner -= (c / 8);
  } else {
    corner = (c / 8) - corner;
  }
  corner += c / 8;
  std::cout << corner << std::endl;
}


// Boost.Geometry
// This file is manually converted from PROJ4

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// This file was converted to Geometry Library by Adam Wulkiewicz

// Original copyright notice:
// Author:   Frank Warmerdam, warmerdam@pobox.com

// Copyright (c) 2000, Frank Warmerdam

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_SRS_PROJECTIONS_IMPL_PJ_APPLY_GRIDSHIFT_HPP
#define BOOST_GEOMETRY_SRS_PROJECTIONS_IMPL_PJ_APPLY_GRIDSHIFT_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/srs/projections/impl/pj_gridlist.hpp>

namespace boost { namespace geometry { namespace projections
{

namespace detail
{

// Originally implemented in nad_intr.c
template <typename CalcT>
inline void nad_intr(CalcT in_lon, CalcT in_lat,
                     CalcT & out_lon, CalcT & out_lat,
                     pj_ctable const& ct)
{
	pj_ctable::lp_t frct;
	pj_ctable::ilp_t indx;
	boost::int32_t in;

	indx.lam = int_floor(in_lon /= ct.del.lam);
	indx.phi = int_floor(in_lat /= ct.del.phi);
	frct.lam = in_lon - indx.lam;
	frct.phi = in_lat - indx.phi;
    // TODO: implement differently
	out_lon = out_lat = HUGE_VAL;
	if (indx.lam < 0) {
		if (indx.lam == -1 && frct.lam > 0.99999999999) {
			++indx.lam;
			frct.lam = 0.;
		} else
			return;
	} else if ((in = indx.lam + 1) >= ct.lim.lam) {
		if (in == ct.lim.lam && frct.lam < 1e-11) {
			--indx.lam;
			frct.lam = 1.;
		} else
			return;
	}
	if (indx.phi < 0) {
		if (indx.phi == -1 && frct.phi > 0.99999999999) {
			++indx.phi;
			frct.phi = 0.;
		} else
			return;
	} else if ((in = indx.phi + 1) >= ct.lim.phi) {
		if (in == ct.lim.phi && frct.phi < 1e-11) {
			--indx.phi;
			frct.phi = 1.;
		} else
			return;
	}
	boost::int32_t index = indx.phi * ct.lim.lam + indx.lam;
	pj_ctable::flp_t const& f00 = ct.cvs[index++];
	pj_ctable::flp_t const& f10 = ct.cvs[index];
	index += ct.lim.lam;
	pj_ctable::flp_t const& f11 = ct.cvs[index--];
	pj_ctable::flp_t const& f01 = ct.cvs[index];
    CalcT m00, m10, m01, m11;
	m11 = m10 = frct.lam;
	m00 = m01 = 1. - frct.lam;
	m11 *= frct.phi;
	m01 *= frct.phi;
	frct.phi = 1. - frct.phi;
	m00 *= frct.phi;
	m10 *= frct.phi;
	out_lon = m00 * f00.lam + m10 * f10.lam +
			  m01 * f01.lam + m11 * f11.lam;
	out_lat = m00 * f00.phi + m10 * f10.phi +
			  m01 * f01.phi + m11 * f11.phi;
}

// Originally implemented in nad_cvt.c
template <bool Inverse, typename CalcT>
inline void nad_cvt(CalcT const& in_lon, CalcT const& in_lat,
                    CalcT & out_lon, CalcT & out_lat,
                    pj_gi const& gi)
{
    static const int max_iterations = 10;
    static const CalcT tol = 1e-12;
    static const CalcT toltol = tol * tol;
    static const CalcT pi = math::pi<CalcT>();

    // horizontal grid expected
    BOOST_GEOMETRY_ASSERT_MSG(gi.format != pj_gi::gtx,
        "Vertical grid cannot be used in horizontal shift.");

    pj_ctable const& ct = gi.ct;

    // TODO: implement differently
    if (in_lon == HUGE_VAL)
    {
        out_lon = HUGE_VAL;
        out_lat = HUGE_VAL;
        return;
    }

    // normalize input to ll origin
    pj_ctable::lp_t tb;
    tb.lam = in_lon - ct.ll.lam;
    tb.phi = in_lat - ct.ll.phi;
    tb.lam = adjlon (tb.lam - pi) + pi;

    pj_ctable::lp_t t;
    nad_intr(tb.lam, tb.phi, t.lam, t.phi, ct);
    if (t.lam == HUGE_VAL)
    {
        out_lon = HUGE_VAL;
        out_lat = HUGE_VAL;
        return;
    }

    if (! Inverse)
    {
        out_lon = in_lon - t.lam;
        out_lat = in_lat - t.phi;
        return;
    }

    t.lam = tb.lam + t.lam;
    t.phi = tb.phi - t.phi;

    int i = max_iterations;
    pj_ctable::lp_t del, dif;
    do
    {
        nad_intr(t.lam, t.phi, del.lam, del.phi, ct);

        // This case used to return failure, but I have
        // changed it to return the first order approximation
        // of the inverse shift.  This avoids cases where the
        // grid shift *into* this grid came from another grid.
        // While we aren't returning optimally correct results
        // I feel a close result in this case is better than
        // no result.  NFW
        // To demonstrate use -112.5839956 49.4914451 against
        // the NTv2 grid shift file from Canada.
        if (del.lam == HUGE_VAL)
        {
            // Inverse grid shift iteration failed, presumably at grid edge. Using first approximation.
            break;
        }

        dif.lam = t.lam - del.lam - tb.lam;
        dif.phi = t.phi + del.phi - tb.phi;
        t.lam -= dif.lam;
        t.phi -= dif.phi;

    }
    while (--i && (dif.lam*dif.lam + dif.phi*dif.phi > toltol)); // prob. slightly faster than hypot()

    if (i==0)
    {
        // Inverse grid shift iterator failed to converge.
        out_lon = HUGE_VAL;
        out_lat = HUGE_VAL;
        return;
    }

    out_lon = adjlon (t.lam + ct.ll.lam);
    out_lat = t.phi + ct.ll.phi;
}

/************************************************************************/
/*                             find_grid()                              */
/*                                                                      */
/*    Determine which grid is the correct given an input coordinate.    */
/************************************************************************/

// Originally find_ctable()
// here divided into grid_disjoint(), find_grid() and load_grid()

template <typename T>
inline bool grid_disjoint(T const& lam, T const& phi,
                          pj_ctable const& ct)
{
    double epsilon = (fabs(ct.del.phi)+fabs(ct.del.lam))/10000.0;
    return ct.ll.phi - epsilon > phi
        || ct.ll.lam - epsilon > lam
        || (ct.ll.phi + (ct.lim.phi-1) * ct.del.phi + epsilon < phi)
        || (ct.ll.lam + (ct.lim.lam-1) * ct.del.lam + epsilon < lam);
}

template <typename T>
inline pj_gi * find_grid(T const& lam,
                         T const& phi,
                         std::vector<pj_gi>::iterator first,
                         std::vector<pj_gi>::iterator last)
{
    pj_gi * gip = NULL;

    for( ; first != last ; ++first )
    {
        // skip tables that don't match our point at all.
        if (! grid_disjoint(lam, phi, first->ct))
        {
            // skip vertical grids
            if (first->format != pj_gi::gtx)
            {
                gip = boost::addressof(*first);
                break;
            }
        }
    }

    // If we didn't find a child then nothing more to do
    if( gip == NULL )
        return gip;

    // Otherwise use the child, first checking it's children
    pj_gi * child = find_grid(lam, phi, first->children.begin(), first->children.end());
    if (child != NULL)
        gip = child;

    return gip;
}

template <typename T>
inline pj_gi * find_grid(T const& lam,
                         T const& phi,
                         pj_gridinfo & grids,
                         std::vector<std::size_t> const& gridindexes)
{
    pj_gi * gip = NULL;

    // keep trying till we find a table that works
    for (std::size_t i = 0 ; i < gridindexes.size() ; ++i)
    {
        pj_gi & gi = grids[gridindexes[i]];

        // skip tables that don't match our point at all.
        if (! grid_disjoint(lam, phi, gi.ct))
        {
            // skip vertical grids
            if (gi.format != pj_gi::gtx)
            {
                gip = boost::addressof(gi);
                break;
            }
        }
    }

    if (gip == NULL)
        return gip;

    // If we have child nodes, check to see if any of them apply.
    pj_gi * child = find_grid(lam, phi, gip->children.begin(), gip->children.end());
    if (child != NULL)
        gip = child;

    // if we get this far we have found a suitable grid
    return gip;
}

template <typename StreamPolicy>
inline bool load_grid(StreamPolicy const& stream_policy, pj_gi_load & gi)
{
    // load the grid shift info if we don't have it.
    if (gi.ct.cvs.empty())
    {
        typename StreamPolicy::stream_type is;
        stream_policy.open(is, gi.gridname);

        if (! pj_gridinfo_load(is, gi))
        {
            //pj_ctx_set_errno( ctx, PJD_ERR_FAILED_TO_LOAD_GRID );
            return false;
        }
    }

    return true;
}

/************************************************************************/
/*                        pj_apply_gridshift_3()                        */
/*                                                                      */
/*      This is the real workhorse, given a gridlist.                   */
/************************************************************************/

template <bool Inverse, typename CalcT, typename StreamPolicy, typename Range>
inline bool pj_apply_gridshift_3(StreamPolicy const& stream_policy,
                                 Range & range,
                                 srs::grids & grids,
                                 std::vector<std::size_t> const& gridindexes)
{
    typedef typename boost::range_size<Range>::type size_type;

    // If the grids are empty the indexes are as well
    if (gridindexes.empty())
    {
        //pj_ctx_set_errno(ctx, PJD_ERR_FAILED_TO_LOAD_GRID);
        //return PJD_ERR_FAILED_TO_LOAD_GRID;
        return false;
    }

    size_type point_count = boost::size(range);

    for (size_type i = 0 ; i < point_count ; ++i)
    {
        typename boost::range_reference<Range>::type
            point = range::at(range, i);

        CalcT in_lon = geometry::get_as_radian<0>(point);
        CalcT in_lat = geometry::get_as_radian<1>(point);

        pj_gi * gip = find_grid(in_lon, in_lat, grids.gridinfo, gridindexes);

        if ( gip != NULL )
        {
            // load the grid shift info if we don't have it.
            if (! gip->ct.cvs.empty() || load_grid(stream_policy, *gip))
            {
                // TODO: use set_invalid_point() or similar mechanism
                CalcT out_lon = HUGE_VAL;
                CalcT out_lat = HUGE_VAL;

                nad_cvt<Inverse>(in_lon, in_lat, out_lon, out_lat, *gip);

                // TODO: check differently
                if ( out_lon != HUGE_VAL )
                {
                    geometry::set_from_radian<0>(point, out_lon);
                    geometry::set_from_radian<1>(point, out_lat);
                }
            }
        }
    }

    return true;
}

/************************************************************************/
/*                        pj_apply_gridshift_2()                        */
/*                                                                      */
/*      This implementation uses the gridlist from a coordinate         */
/*      system definition.  If the gridlist has not yet been            */
/*      populated in the coordinate system definition we set it up      */
/*      now.                                                            */
/************************************************************************/

template <bool Inverse, typename Par, typename Range, typename ProjGrids>
inline bool pj_apply_gridshift_2(Par const& defn, Range & range, ProjGrids const& grids)
{
    /*if( defn->catalog_name != NULL )
        return pj_gc_apply_gridshift( defn, inverse, point_count, point_offset,
                                      x, y, z );*/

    /*std::vector<std::size_t> gridindexes;
    pj_gridlist_from_nadgrids(pj_get_param_s(defn.params, "nadgrids"),
                              grids.storage_ptr->stream_policy,
                              grids.storage_ptr->grids,
                              gridindexes);*/

    BOOST_GEOMETRY_ASSERT(grids.storage_ptr != NULL);

    // At this point the grids should be initialized
    if (grids.hindexes.empty())
        return false;

    return pj_apply_gridshift_3
            <
                Inverse, typename Par::type
            >(grids.storage_ptr->stream_policy,
              range,
              grids.storage_ptr->hgrids,
              grids.hindexes);
}

template <bool Inverse, typename Par, typename Range>
inline bool pj_apply_gridshift_2(Par const& , Range & , srs::detail::empty_projection_grids const& )
{
    return false;
}

} // namespace detail

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_SRS_PROJECTIONS_IMPL_PJ_APPLY_GRIDSHIFT_HPP


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#pragma once

#include "DriverTestHelpers.hpp"

#include <boost/test/unit_test.hpp>
#include <log/log.h>

#include <OperationsUtils.h>

BOOST_AUTO_TEST_SUITE(Convolution2DTests)

using namespace android::hardware;
using namespace driverTestHelpers;
using namespace armnn_driver;

namespace driverTestHelpers
{
#define ARMNN_ANDROID_FP16_TEST(result, fp16Expectation, fp32Expectation, fp16Enabled) \
   if (fp16Enabled) \
   { \
       BOOST_TEST((result == fp16Expectation || result == fp32Expectation), result << \
       " does not match either " << fp16Expectation << "[fp16] or " << fp32Expectation << "[fp32]"); \
   } else \
   { \
      BOOST_TEST(result == fp32Expectation); \
   }

void SetModelFp16Flag(V1_0::Model& model, bool fp16Enabled);

void SetModelFp16Flag(V1_1::Model& model, bool fp16Enabled);

template<typename HalPolicy>
void PaddingTestImpl(android::nn::PaddingScheme paddingScheme, bool fp16Enabled = false)
{
    using HalModel         = typename HalPolicy::Model;
    using HalOperationType = typename HalPolicy::OperationType;

    armnn::Compute computeDevice = armnn::Compute::GpuAcc;

#ifndef ARMCOMPUTECL_ENABLED
    computeDevice = armnn::Compute::CpuRef;
#endif

    auto driver = std::make_unique<ArmnnDriver>(DriverOptions(computeDevice, fp16Enabled));
    HalModel model = {};

    uint32_t outSize = paddingScheme == android::nn::kPaddingSame ? 2 : 1;

    // add operands
    float weightValue[] = {1.f, -1.f, 0.f, 1.f};
    float biasValue[]   = {0.f};

    AddInputOperand<HalPolicy>(model, hidl_vec<uint32_t>{1, 2, 3, 1});
    AddTensorOperand<HalPolicy>(model, hidl_vec<uint32_t>{1, 2, 2, 1}, weightValue);
    AddTensorOperand<HalPolicy>(model, hidl_vec<uint32_t>{1}, biasValue);
    AddIntOperand<HalPolicy>(model, (int32_t)paddingScheme); // padding
    AddIntOperand<HalPolicy>(model, 2); // stride x
    AddIntOperand<HalPolicy>(model, 2); // stride y
    AddIntOperand<HalPolicy>(model, 0); // no activation
    AddOutputOperand<HalPolicy>(model, hidl_vec<uint32_t>{1, 1, outSize, 1});

    // make the convolution operation
    model.operations.resize(1);
    model.operations[0].type = HalOperationType::CONV_2D;
    model.operations[0].inputs  = hidl_vec<uint32_t>{0, 1, 2, 3, 4, 5, 6};
    model.operations[0].outputs = hidl_vec<uint32_t>{7};

    // make the prepared model
    SetModelFp16Flag(model, fp16Enabled);
    android::sp<V1_0::IPreparedModel> preparedModel = PrepareModel(model, *driver);

    // construct the request
    DataLocation inloc    = {};
    inloc.poolIndex       = 0;
    inloc.offset          = 0;
    inloc.length          = 6 * sizeof(float);
    RequestArgument input = {};
    input.location        = inloc;
    input.dimensions      = hidl_vec<uint32_t>{};

    DataLocation outloc    = {};
    outloc.poolIndex       = 1;
    outloc.offset          = 0;
    outloc.length          = outSize * sizeof(float);
    RequestArgument output = {};
    output.location        = outloc;
    output.dimensions      = hidl_vec<uint32_t>{};

    V1_0::Request request = {};
    request.inputs  = hidl_vec<RequestArgument>{input};
    request.outputs = hidl_vec<RequestArgument>{output};

    // set the input data (matching source test)
    float indata[] = {1024.25f, 1.f, 0.f, 3.f, -1, -1024.25f};
    AddPoolAndSetData(6, request, indata);

    // add memory for the output
    android::sp<IMemory> outMemory = AddPoolAndGetData<float>(outSize, request);
    float* outdata = reinterpret_cast<float*>(static_cast<void*>(outMemory->getPointer()));

    // run the execution
    if (preparedModel.get() != nullptr)
    {
        Execute(preparedModel, request);
    }

    // check the result
    switch (paddingScheme)
    {
    case android::nn::kPaddingValid:
        ARMNN_ANDROID_FP16_TEST(outdata[0], 1022.f, 1022.25f, fp16Enabled)
        break;
    case android::nn::kPaddingSame:
        ARMNN_ANDROID_FP16_TEST(outdata[0], 1022.f, 1022.25f, fp16Enabled)
        BOOST_TEST(outdata[1] == 0.f);
        break;
    default:
        BOOST_TEST(false);
        break;
    }
}

} // namespace driverTestHelpers

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2018, Sunanda Bose (Neel Basu) (neel.basu.z@gmail.com)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "mathematica++ Unit Test (Module::Eigen3)"
#include <boost/test/unit_test.hpp>
#include <string>
#include <cmath>
#include <complex>
#include <map>
#include <vector>
#include <iostream>
#include <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include "boost/tuple/tuple.hpp"
#include "mathematica++/connector.h"
#include "mathematica++/m.h"
#include "mathematica++/io.h"
#include "mathematica++/declares.h"
#include "mathematica++/operators.h"
#include "mathematica++/rules.h"
#include <Eigen/Dense>
#include <boost/cstdint.hpp>

using namespace mathematica;

MATHEMATICA_DECLARE(Flatten)
MATHEMATICA_DECLARE(Map)
MATHEMATICA_DECLARE(Dimensions)

BOOST_AUTO_TEST_SUITE(eigen)

BOOST_AUTO_TEST_CASE(eigen_matrix){
    typedef std::pair<int, int> matrix_dimension_type;

    connector shell;
    BOOST_CHECK(shell.connected());
    {
        value result;
        Eigen::Matrix<int, 4, 3> matrix;
        matrix <<  1,  2,  3,
                   4,  5,  6,
                   7,  8,  9,
                  10, 11, 12;
        shell << Dimensions(matrix);
        shell >> result;

        matrix_dimension_type dims = cast<matrix_dimension_type>(result);

        BOOST_CHECK(dims.first == 4 && dims.second == 3);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<int, 1, 3> matrix;
        matrix << 1, 2, 3;
        shell << Dimensions(matrix);
        shell >> result;

        matrix_dimension_type dims = cast<matrix_dimension_type>(result);

        BOOST_CHECK(dims.first == 1 && dims.second == 3);

//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<int, 4, 1> matrix;
        matrix <<  1,
                   4,
                   7,
                  10;
        shell << Dimensions(matrix);
        shell >> result;

        matrix_dimension_type dims = cast<matrix_dimension_type>(result);

        BOOST_CHECK(dims.first == 4 && dims.second == 1);
//         std::cout << result << std::endl;
    }
    {
        value result;
        typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> dynamic_matrix_type;
//         dynamic_matrix_type matrix;
        int arr[4][3] = {{1 ,2, 3}, {4, 5, 6}, {7, 8, 9}, {10, 11, 12}};
        Eigen::Map<dynamic_matrix_type> map(&arr[0][0], 4, 3);
        dynamic_matrix_type matrix = map;

        shell << Dimensions(matrix);
        shell >> result;

        matrix_dimension_type dims = cast<matrix_dimension_type>(result);

        BOOST_CHECK(dims.first == 4 && dims.second == 3);
//         std::cout << result << std::endl;
    }
}

BOOST_AUTO_TEST_CASE(eigen_matrix_types){
    typedef std::pair<int, int> matrix_dimension_type;

    connector shell;
    BOOST_CHECK(shell.connected());
    {
        value result;
        Eigen::Matrix<boost::uint8_t, 4, 3> matrix;
        matrix <<  1,  2,  3,
                   4,  5,  6,
                   7,  8,  9,
                  10, 11, 12;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<boost::int16_t, 4, 3> matrix;
        matrix <<  1,  2,  3,
                   4,  5,  6,
                   7,  8,  9,
                  10, 11, 12;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<boost::uint16_t, 4, 3> matrix;
        matrix <<  1,  2,  3,
                   4,  5,  6,
                   7,  8,  9,
                  10, 11, 12;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<boost::uint32_t, 4, 3> matrix;
        matrix <<  1,  2,  3,
                   4,  5,  6,
                   7,  8,  9,
                  10, 11, 12;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<boost::int64_t, 4, 3> matrix;
        matrix <<  1,  2,  3,
                   4,  5,  6,
                   7,  8,  9,
                  10, 11, 12;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<float, 4, 3> matrix;
        matrix <<  1.0f,  2.0f,  3.0f,
                   4.0f,  5.0f,  6.0f,
                   7.0f,  8.0f,  9.0f,
                  10.0f, 11.0f, 12.0f;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78.0f);
//         std::cout << result << std::endl;
    }
    {
        value result;
        Eigen::Matrix<double, 4, 3> matrix;
        matrix <<  1.0f,  2.0f,  3.0f,
                   4.0f,  5.0f,  6.0f,
                   7.0f,  8.0f,  9.0f,
                  10.0f, 11.0f, 12.0f;
        shell << Total(Flatten(matrix));
        shell >> result;

        BOOST_CHECK(*result == 78.0f);
//         std::cout << result << std::endl;
    }
}

BOOST_AUTO_TEST_SUITE_END()