#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>

#include "spline_kernel.h"
#include "interval.h"
#include "cardinal_spline.h"
#include "kernel\spline.h"
#include "generate_spline.h"
#include "data.h"
#include "classic_methods.h"
using namespace std;
using namespace spl;

template<typename T>
void SaveFunctionToFile(T f, spl::interval interval, int grid_size, string path)
{
	const auto [a, b] = interval;

	ofstream myfile(path + "\\example.txt");
	if (myfile.is_open())
	{

		for (int i = 0; i < grid_size; i++)
		{
			auto x = a + i * interval.get_step(grid_size);
			auto y = f(x);
			myfile << x << " " << y << std::endl;
		}

		myfile.close();
	}
	else cout << "Unable to open file";
}

void test1()
{
	using spl::data::calculate_points;
	using spl::generator::create_spline;

	auto f = [](double t)->double {return t*t + 1.; };
	interval interv = { 0., 1. };
	auto sp_f = create_spline<double, 2u>(f, interv, 10);

	const auto points = calculate_points<double>(f, interv, 100);

	for (size_t i = 0; i < 100; i++)
	{
		std::cout << points[i] << ", ";
	}
}

void test2()
{
	const double root = std::sqrt(2.);
	auto f = [root](double t)->double {
		return std::pow(t - root, 3);
	};
	interval interv = { -10., 10. };

	auto answer = spl::numeric::bisection<double>(f, interv, 1e-6,
		[](std::function<double(double)> func, interval gap)->bool {
		if (func(gap._aborder) * func(gap._bborder) < 0.)
			return true;
		return false;
		});

	if (answer)
		std::cout <<"bisection error = "<< std::abs(answer.value() - root) << std::endl;
}

int main()
{
	test2();
	test1();
	Eigen::Vector2f vec(0, 1);
	using spl::ker::spline;

	spl::interval interval(1.4, 1.0);
	spl::interval i = {0.0, 1.0};
	auto [a, b] = i;

	std::cout << a << " " << b << std::endl;
	std::cout << interval.length() << std::endl;

    std::cout << "Hello World! " << meaning_of_life() << std::endl;

	spl::cardinal::api_bsplvb(0.25, 3);

	const int size = 10;
	spl::ker::spline<double, 2u> f(size, { 0., 1. }), g(size, { 0., 1. });

	for (size_t i = 0; i < f.size(); i++)
	{
		f[i] = i + 1u;
	}

	auto h = f + g;

	std::cerr << "h(0.5) = " << h(0.5) << std::endl;
}


#include "ros/ros.h"
#include "wave_filter.h"
#include "asv_simulator.h"

// Standard libraries
#include <cmath>
#include <iostream>

// Linear algebra math
#include <Eigen/Dense>

/// Make sure angle is between [-PI, PI)
double normalize_angle(double val);

/// Makes angle compatible with angle_ref such that the numerical diference is at most PI.
double normalize_angle_diff(double angle, double angle_ref);

Vessel::Vessel()
{
  eta = Eigen::Vector3d::Zero();
  nu  = Eigen::Vector3d::Zero();
  tau_waves = Eigen::Vector3d::Zero();
  tau = Eigen::Vector3d::Zero();
}

Vessel::~Vessel() {}

void Vessel::initialize(ros::NodeHandle nh)
{
  // Get all parameters from the parameter server
  if (!nh.getParam("mass", M))
    M = 3980.0;
  if (!nh.getParam("inertia", I_z))
    I_z = 19703.0;
  if (!nh.getParam("dt", DT))
    DT = 0.05;

  if (!nh.getParam("X_udot", X_udot))
    X_udot = 0.0;
  if (!nh.getParam("Y_vdot", Y_vdot))
    Y_vdot = 0.0;
  if (!nh.getParam("Y_rdot", Y_rdot))
    Y_rdot = 0.0;
  if (!nh.getParam("N_vdot", N_vdot))
    N_vdot = 0.0;
  if (!nh.getParam("N_rdot", N_rdot))
    N_rdot = 0.0;

  if (!nh.getParam("X_u", X_u))
    X_u = -50.0;
  if (!nh.getParam("Y_v", Y_v))
    Y_v = -200.0;
  if (!nh.getParam("Y_r", Y_r))
    Y_r = 0.0;
  if (!nh.getParam("N_v", N_v))
    N_v = 0.0;
  if (!nh.getParam("N_r", N_r))
    N_r = -1281.0;

  if (!nh.getParam("X_uu", X_uu))
    X_uu = -135.0;
  if (!nh.getParam("Y_vv", Y_vv))
    Y_vv = -2000.0;
  if (!nh.getParam("N_rr", N_rr))
    N_rr = 0.0;
  if (!nh.getParam("X_uuu", X_uuu))
    X_uuu = 0.0;
  if (!nh.getParam("Y_vvv", Y_vvv))
    Y_vvv = 0.0;
  if (!nh.getParam("N_rrr", N_rrr))
    N_rrr = -3224.0;

  Eigen::Matrix3d Mtot;
  Mtot << M - X_udot, 0, 0,
    0, M-Y_vdot, -Y_rdot,
    0, -Y_rdot, I_z-N_rdot;
  Minv = Mtot.inverse();

  if (!nh.getParam("Fx_min", Fx_min))
    Fx_min = -6550.0;
  if (!nh.getParam("Fx_max", Fx_max))
    Fx_max = 13100.0;
  if (!nh.getParam("Fy_min", Fy_min))
    Fy_min = -650.0;
  if (!nh.getParam("Fy_max", Fy_max))
    Fy_max = 650.0;

  if (!nh.getParam("rudder_length", rudder_length))
      rudder_length = 4.0;

  if (!nh.getParam("Kp_u", Kp_u))
    Kp_u = 0.1;
  if (!nh.getParam("Kp_psi", Kp_psi))
    Kp_psi = 5.0;
  if (!nh.getParam("Kd_psi", Kd_psi))
    Kd_psi = 1.0;
  if (!nh.getParam("Kp_r", Kp_r))
    Kp_r = 8.0;

  if (!nh.getParam("Fx_current", Fx_current))
    Fx_current = 0.0;
  if (!nh.getParam("Fy_current", Fy_current))
    Fy_current = 0.0;

  std::vector<double> initial_state;
  if (nh.getParam("initial_state", initial_state))
    {
      // Check if the vector supplied is the right size. Else default to zeros.
      if (initial_state.size() == 6)
        {
          eta[0] = initial_state[0];
          eta[1] = initial_state[1];
          eta[2] = initial_state[2];
          nu[0]  = initial_state[3];
          nu[1]  = initial_state[4];
          nu[2]  = initial_state[5];
        }
    }

  // Initialize wave filters
  double sigma, omega0, lambda, gain;
  if (nh.getParam("sigma_x", sigma) &&
      nh.getParam("omega0_x", omega0) &&
      nh.getParam("lambda_x", lambda) &&
      nh.getParam("gain_x", gain))
    {
      if (gain > 0.0)
        {
          wave_filter_x.initialize(sigma, omega0, lambda, gain, DT);
          ROS_INFO("Enabled wave filter x!");
        }
    }
  if (nh.getParam("sigma_y", sigma) &&
      nh.getParam("omega0_y", omega0) &&
      nh.getParam("lambda_y", lambda) &&
      nh.getParam("gain_y", gain))
    {
      if (gain > 0.0)
        {
          wave_filter_y.initialize(sigma, omega0, lambda, gain, DT);
          ROS_INFO("Enabled wave filter y!");
        }
    }
  if (nh.getParam("sigma_psi", sigma) &&
      nh.getParam("omega0_psi", omega0) &&
      nh.getParam("lambda_psi", lambda) &&
      nh.getParam("gain_psi", gain))
    {
      if (gain > 0.0)
        {
          wave_filter_psi.initialize(sigma, omega0, lambda, gain, DT);
          ROS_INFO("Enabled wave filter psi!");
        }
    }

}

double Vessel::getDT()
{
  return DT;
}

void Vessel::printPose()
{
  std::cout << eta << std::endl << std::endl;
}

void Vessel::setState(Eigen::Vector3d eta_new, Eigen::Vector3d nu_new)
{
  eta = eta_new;
  nu = nu_new;
}

void Vessel::getState(Eigen::Vector3d &eta2, Eigen::Vector3d &nu2)
{
  eta2 = eta;
  nu2 = nu;
}

void Vessel::getWaveNoise(Eigen::Vector3d &wave_noise)
{
  wave_noise = tau_waves;
}

void Vessel::updateSystem(double u_d, double psi_d, double r_d)
{
  // Ensure psi_d is "compatible" with psi
  psi_d = normalize_angle_diff(psi_d, eta[2]);

  Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(eta[2], Eigen::Vector3d::UnitZ());

  // Calculate coriolis and dampening matrices according to Fossen, 2011 or Stenersen, 2014.
  Cvv[0] = (-M*nu[1] + Y_vdot*nu[1] + Y_rdot*nu[2]) * nu[2];
  Cvv[1] = ( M*nu[0] - X_udot*nu[0]) * nu[2];
  Cvv[2] = (( M*nu[1] - Y_vdot*nu[1] - Y_rdot*nu[2] ) * nu[0] +
            ( -M*nu[0] + X_udot*nu[0] ) * nu[1]);

  Dvv[0] = - (X_u + X_uu*fabs(nu[0]) + X_uuu*nu[0]*nu[0]) * nu[0];
  Dvv[1] = - ((Y_v*nu[1] + Y_r*nu[2]) +
              (Y_vv*fabs(nu[1])*nu[1] + Y_vvv*nu[1]*nu[1]*nu[1]));
  Dvv[2] = - ((N_v*nu[1] + N_r*nu[2]) +
              (N_rr*fabs(nu[2])*nu[2] + N_rrr*nu[2]*nu[2]*nu[2]));

  this->updateControlInput(u_d, psi_d, r_d);

  Eigen::Vector3d tau_const_disturbance(Fx_current, Fy_current, 0.0);
  tau_const_disturbance = rot_z.inverse()*tau_const_disturbance;

  tau_waves[0] = wave_filter_x.updateFilter();
  tau_waves[1] = wave_filter_y.updateFilter();
  tau_waves[2] = wave_filter_psi.updateFilter();

  // Integrate system
  eta += DT * (rot_z * nu);
  nu  += DT * (Minv * (tau + tau_const_disturbance + tau_waves - Cvv - Dvv));

  // Keep yaw within [-PI,PI)
  eta[2] = normalize_angle(eta[2]);
}

void Vessel::updateControlInput(double u_d, double psi_d, double r_d)
{
  double Fx = Cvv[0] + Dvv[0] + Kp_u*M*(u_d - nu[0]);
  double Fy = 0.0;

  // If psi_d == inf, then use yaw-rate controller
  if (isinf(psi_d))
    {
      Fy = Cvv[1] + Dvv[1] + I_z*Kp_r*(r_d - nu[2]);
      Fy *= 1.0 / rudder_length;
    }
  else
    {
      Fy = (Kp_psi * I_z ) * ((psi_d - eta[2]) - Kd_psi*nu[2]);
      Fy *= 1.0 / rudder_length;
    }

  // Saturate
  if (Fx < Fx_min)
    Fx = Fx_min;
  if (Fx > Fx_max)
    Fx = Fx_max;

  if (Fy < Fy_min)
    Fy = Fy_min;
  if (Fy > Fy_max)
    Fy = Fy_max;

  tau[0] = Fx;
  tau[1] = Fy;
  tau[2] = rudder_length * Fy;
}

double normalize_angle(double val)
{
  if (isinf(val))
    return val;

  while (val <= -M_PI)
    val += 2*M_PI;

  while (val > M_PI)
    val -= 2*M_PI;

  return val;
}

double normalize_angle_diff(double angle, double angle_ref)
{
  double new_angle = 0;
  double diff = angle_ref - angle;

  if (isinf(angle) || isinf(angle_ref))
    return angle;

  // Get angle within 2PI of angle_ref
  if (diff > 0)
    {
      new_angle = angle + (diff - fmod(diff, 2*M_PI));
    }
  else
    {
      new_angle = angle + (diff + fmod(-diff, 2*M_PI));
    }

  // Make sure angle is on the closest side of angle_ref
  diff = angle_ref - new_angle;
  if (diff > M_PI)
    {
      new_angle += 2*M_PI;
    }
  else if (diff < -M_PI)
    {
      new_angle -= 2*M_PI;
    }
  return new_angle;
}


#include "mandel_hp.h"

#include <cassert>
#include <cstdlib>
#include <iomanip>
#include <cmath>
#include <climits>
#include <iostream>
#include <fstream>

#include <boost/thread.hpp>
#include <boost/chrono.hpp>

#include "common.h"
#include "updater.h"

using namespace std::literals;

// ----------------------------------------------------------------------------

FltH copysign(const FltH& a,const FltH& b)
{
	bool aneg = a<0;
	bool bneg = b<0;
	if (aneg==bneg)
		return a;
	else
		return -a;
}

template<typename Flt>
std::complex<Flt> step(const std::complex<Flt>& c, const std::complex<Flt>& z)
{
	return z*z + c;
}

template std::complex<FltH> step<FltH>(const std::complex<FltH>&, const std::complex<FltH>&);
template std::complex<FltL> step<FltL>(const std::complex<FltL>&, const std::complex<FltL>&);

// ----------------------------------------------------------------------------

// *************
// *** Point ***
// *************

template<typename Flt>
bool Point<Flt>::docalc(const std::complex<Flt>& c, UL cap)
{

	#ifndef NDEBUG
	if (c != orig)
	{
		auto dx = c.real() - orig.real();
		auto dy = c.imag() - orig.imag();
		using std::sqrt;
		auto dst = sqrt(dx*dx + dy*dy);
		if (dst > (0.001 * stepsize))
		{
			std::cerr << "Error: c differs from orig\n";
			std::cerr << "    c = " << c.real() << "+" << c.imag() << "i\n";
			std::cerr << " orig = " << orig.real() << "+" << orig.imag() << "i\n";
			std::cerr << " diff = " << dx << "+" << dy << "i\n";
			throw "Error: c differs from orig\n";
		}
	}
	#endif

	UL& n = iter;
	if (n <= 1)
	{
		// first bulb
		double xld = (double)z.real();
		double yld = (double)z.imag();
		double y2 = yld * yld;
		double xldp1 = xld+1.0;
		if (((xldp1*xldp1) + y2) < 0.0625)
		{
			status = Point::in;
			pixtype = pt_black;
			return true;
		}
		// main cardoid
		double xx = xld - 0.25;
		xx *= xx;
		xx += y2;
		double pp = sqrt(xx);
		if (xld < (pp - 2.0*(pp*pp) + 0.25))
		{
			status = Point::in;
			pixtype = pt_black;
			return true;
		}
	}

	bool did_smth = false;

	while (true)
	{
		if (n >= cap)
		{
			pixtype = pt_black;
			break;
		}

		auto zre = z.real();
		Flt  re_sq = zre * zre;
		auto zim = z.imag();
		Flt  im_sq = zim * zim;
		auto az2 = (double)re_sq + (double)im_sq;
		if (az2 > 4.0)
		{
			did_smth = true;
			status = Point::out;
			over = sqrtf(az2);
			pixtype = pt_normal;
			break;
		}

		z.real(re_sq-im_sq);
		auto ab = zre * zim;
		z.imag(ab+ab);
		z += c;
		++n;
	}

	return did_smth;
}

template<typename Flt>
void Point<Flt>::init(const std::complex<Flt>& c)
{
	status = Point::calc;
	iter = 1;
	z = c;
	pixtype = 0;
	#ifndef NDEBUG
	orig = c;
	#endif
}

template<typename Flt>
void Point<Flt>::col(float mod)
{
	const Point& p = *this;
	if (p.status != Point::out)
	{
		rgbval = {0,0,0};
        return;
	}
	auto x = p.iter;

	static const float pi2 = 3.1415926536f * 2;
	static const float ilg2 = 1.0f / logf(2.0f);
	static const float c000 = 0.0f / 3.0f;
	static const float c333 = 1.0f / 3.0f;
	static const float c666 = 2.0f / 3.0f;
	float f = fmodf((float)x, mod) + 1.0f;
	f /= mod;

	f -= logf(logf(p.over) * ilg2) * ilg2 / mod;
	f *= pi2;
	float r = 0.5f + 0.5f*sinf(f + pi2 * c000);
	float g = 0.5f + 0.5f*sinf(f + pi2 * c333);
	float b = 0.5f + 0.5f*sinf(f + pi2 * c666);
	int ri = clamp(int(r*256), 0, 255);
	int gi = clamp(int(g*256), 0, 255);
	int bi = clamp(int(b*256), 0, 255);
	rgbval = {(UC)ri, (UC)gi, (UC)bi};
}

template struct Point<FltH>;
template struct Point<FltL>;

// ----------------------------------------------------------------------------

// ***********
// *** Map ***
// ***********

template<typename Flt>
void Map<Flt>::colorize(float mod)
{
	for (UL y=0; y<new_h; ++y)
	{
		for (UL x=0; x<new_w; ++x)
		{
			get(x,y).col(mod);
		}
	}
}

template<typename Flt>
Point<Flt>& Map<Flt>::get(UL x, UL y)
{
	return points[y][x];
}

template<typename Flt>
Flt Map<Flt>::to_xpos(UL x) const
{
	assert(x<new_w);
	Flt fact = x;
	fact /= (Flt)new_w;
	fact -= (Flt)0.5;
	fact *= scale_x;
	fact += center_x;
	return fact;
}

template<typename Flt>
Flt Map<Flt>::to_ypos(UL y) const
{
	assert(y<new_h);
	Flt fact = y;
	fact /= (Flt)new_h;
	fact -= (Flt)0.5;
	fact *= scale_y;
	fact += center_y;
	return fact;
}

template<typename Flt>
void Map<Flt>::setZ(Flt z)
{
	scale_x = z;
	scale_y = z * (Flt)height / (Flt)width;
}

template<typename Flt>
void Map<Flt>::generate_init()
{
	UL x,y;
	vfx.clear();
	vfy.clear();

	new_w = width; new_h = height;

	for (y=0; y<height; ++y)
		vfy.push_back(to_ypos(y));
	for (x=0; x<width; ++x)
		vfx.push_back(to_xpos(x));

	map_all_done = false;
	points.resize(height);

	for (y=0; y<height; ++y)
	{
		points[y].resize(width);
		const Flt& yld = vfy[y];
		for (x=0; x<width; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
		}
	}
}

template<typename Flt>
auto Map<Flt>::generate(UL cap, bool display, bool extrap) -> Status
{
	bool found_one = false;
	bool did_smth = false;

	UL x,y;

	for (y=0; y<height; ++y)
	{
		if (display)
		{
			float f = 100.0f;
			f /= height;
			f *= y;
			std::cout << (int)f << "%\r" << std::flush;
		}
		const Flt& yld = vfy[y];
		for (x=0; x<width; ++x)
		{
			Point<Flt>& p = get(x,y);
			if (p.status != Point<Flt>::calc)
				continue;
			found_one = true;
			if (extrap)
			{
				UL w = width-1, h = height-1;
				if (x && x<w)
				{
					auto& pp = get(x-1,y);
					auto& pn = get(x+1,y);
					if ( (pp.status == Point<Flt>::out) &&
					     (pn.status == Point<Flt>::out) &&
					     (pp.iter == pn.iter) )
					{
						p.status = Point<Flt>::out;
						p.iter = pp.iter;
						p.over = (float)(pp.over + pn.over)/2.0f;
						did_smth = true;
						continue;
					}
				}
				if (y && y<h)
				{
					Point<Flt>& pp = get(x,y-1);
					Point<Flt>& pn = get(x,y+1);
					if ( (pp.status == Point<Flt>::out) &&
					     (pn.status == Point<Flt>::out) &&
					     (pp.iter == pn.iter) )
					{
						p.status = Point<Flt>::out;
						p.iter = pp.iter;
						p.over = (pp.over + pn.over)/2.0;
						did_smth = true;
						continue;
					}
				}
			}
			std::complex<Flt> c{vfx[x], yld};
			did_smth = p.docalc(c, p.iter+cap) || did_smth;
		}
	}
	if (!found_one)
		return all_done;
	else
		return did_smth ? was_updated : no_change;
}

template<typename Flt>
Image Map<Flt>::makeimage(float mod, UL upc)
{
	Image img(width, height);
	UL x,y;
	for (y=0; y<height; ++y)
	{
		for (x=0; x<width; ++x)
		{
			auto& p = get(x,y);
			if (p.status == Point<Flt>::out)
			{
				p.col(mod);
				img.PutPixel(x,y,p.rgbval);
			} else {
				bool clc = p.status==Point<Flt>::calc;
				bool ilu = upc && (p.iter<upc);
				if (clc && ilu)
					img.PutPixel(x,y,{255,255,255});
				else
					img.PutPixel(x,y,{0,0,0});
			}
		}
	}
	return img;
}

template<typename Flt>
RGB Map<Flt>::extrapolate(float x, float y)
{
	using namespace std;
	if (x<0.0f) x=0.0f; if (x>new_w) x=new_w;
	if (y<0.0f) y=0.0f; if (y>new_h) y=new_h;
	float xr = roundf(x);
	float yr = roundf(y);
	float dx = x-xr;
	float dy = y-yr;
	UL x1,x2,y1,y2;
	double fx, fy;
	if (fabs(dx) < 0.05f) {
		x1 = x2 = roundl(x);
		fx = 0.5f;
	} else {
		x1 = floorl(x);
		x2 = ceill(x);
		fx = x2-x;
	}
	if (fabs(dy) < 0.05f) {
		y1 = y2 = roundl(y);
		fy = 0.5f;
	} else {
		y1 = floorl(y);
		y2 = ceill(y);
		fy = y2-y;
	}
	RGB pix_11 = get(x1 , y1).rgbval;
	RGB pix_12 = get(x1 , y2).rgbval;
	RGB pix_21 = get(x2 , y1).rgbval;
	RGB pix_22 = get(x2 , y2).rgbval;
	RGB pix_1 = mix(pix_11, pix_12, fy);
	RGB pix_2 = mix(pix_21, pix_22, fy);
	RGB pix   = mix(pix_1,  pix_2,  fx);
	return pix;
}

template<typename Flt>
void Map<Flt>::generate_init_rest()
{
	points.resize(new_h);
	UL x,y;
	for (y=0; y<new_h; ++y)
		points[y].resize(new_w+3);

	Flt x_start = to_xpos(0);
	Flt x_stop  = to_xpos(new_w-1);
	Flt y_start = to_ypos(0);
	Flt y_stop  = to_ypos(new_h-1);
	Flt x_step  = (x_stop-x_start) / (new_w-1);
	Flt y_step  = (y_stop-y_start) / (new_h-1);

	vfx.resize(new_w); vfy.resize(new_h);
	for (x=0; x<new_w; ++x)
		vfx[x] = (x_start + x_step*x);
	for (y=0; y<new_h; ++y)
		vfy[y] = (y_start + y_step*y);
}

FltH pow(FltH m, int e)
{
	if (e==0) return FltH{1.0};
	if (e==1) return m;
	if (e<0)  return FltH{1.0} / pow(m, -e);
	FltH ret;
	mpf_pow_ui(ret.get_mpf_t(), m.get_mpf_t(), (UL)e);
	return ret;
}

template<typename Flt>
void Map<Flt>::generate_N_init(int n, bool disp)
{
	using std::pow;

	Flt tm = pow(zoom_mul, -n);

	new_w = ceill(width  * (double)tm);
	new_h = ceill(height * (double)tm);

	if (disp)
	{
		std::cout << "Old size " << width << "x" << height << std::endl;
		std::cout << "New size " << new_w << "x" << new_h << std::endl;
	}

	generate_init_rest();
}

template<typename Flt>
UL Map<Flt>::generate_N_threaded(int n, UL cap, bool display)
{
	generate_N_init(n, display);

	Updater::Init(new_h*3+4);
	if (display) Updater::Display();

	LineCache<Flt> lc[4] = {
		{ cap, display, *this },
		{ cap,   false, *this },
		{ cap,   false, *this },
		{ cap,   false, *this },
	};

	UL i = 0;
	UL num = new_h / 4;
	UL ovr = new_h - (num*4);
	UL y = 0;
	lc[0].y_start = y; y += (lc[0].y_count = num + ovr);
	lc[1].y_start = y; y += (lc[1].y_count = num);
	lc[2].y_start = y; y += (lc[2].y_count = num);
	lc[3].y_start = y; y += (lc[3].y_count = num);

	UL maxout = 0;
	boost::thread tt[4];
	for (i=1; i<4; ++i)
	{
		tt[i] = boost::thread{&LineCache<Flt>::execute, lc+i};
	}
	LineCache<Flt>::execute(lc);
	boost::chrono::nanoseconds ns{250'000};
	int joined = 1;
	while (true)
	{
		if (display) Updater::Display();

		bool j = tt[joined].try_join_for(ns);
		if (j)
		{
			++joined;
			Updater::Tick();
			if (joined >= 4) break;
		}
	}
	UL sk = 0, is = 0;
	for (i=0; i<4; ++i)
	{
		if (lc[i].eff_cap > maxout)
			maxout = lc[i].eff_cap;
		sk += lc[i].skip_count;
		is += lc[i].inskip;
	}

	Updater::Tick();
	if (display) Updater::Display();

	if (display)
		std::cout << "skipped        : " << sk << " pixels, of wich " << is << " was inside \n";

	return maxout;
}

template<typename Flt>
Image Map<Flt>::makeimage_N(int n, ModFunc mf, OSP fr)
{
	using std::pow;

	Image img(width, height);

	Flt tpmn = pow(zoom_mul, -n);

	float myw = new_w / (double)tpmn;
	float myh = new_h / (double)tpmn;

	float xstart = (new_w-myw) / 2;
	float ystart = (new_h-myh) / 2;

	float xstep = myw / width;
	float ystep = myh / height;

	Flt quot = scale_x / tpmn;
	float mod = mf((double)quot);

	if (fr) DiffReport((double)quot, mod);

	if (fr)
	{
		(*fr) << std::setprecision(20) << std::setw(25) << std::scientific << quot << " ";
		(*fr) << std::setprecision(10) << std::setw(15) << std::fixed << mod << " ";
		(*fr) << myw << "x" << myh << " ";
		(*fr) << xstart << "+" << xstep << " ";
		(*fr) << ystart << "+" << ystep << " ";
	}

	colorize(mod);

	UL x,y;
	for (y=0; y<height; ++y)
	{
		float yf = ystart + y * ystep;
		for (x=0; x<width; ++x)
		{
			double xf = xstart + x * xstep;
			img.PutPixel(x, y, extrapolate(xf, yf));
		}
	}

	return img;
}

void adjust_factor(FltL&, int) {}

int adjustments = 0;
int epsilondbl = 0;

void adjust_factor(FltH& f, int n)
{
	FltH epsilon = 0.001;
	FltH target = pow(f, n);
	FltH diff = abs(0.5f - target);

	while (true)
	{
		while (true)
		{
			FltH up = f + epsilon;
			if (up == f) return;
			FltH new_target = pow(up, n);
			FltH new_diff = abs(0.5f - new_target);
			if (new_diff<diff)
			{
				++adjustments;
				f = up;
				target = new_target;
				diff = new_diff;
				continue;
			}
			break;
		}
		while (true)
		{
			FltH dn = f - epsilon;
			if (dn == f) return;
			FltH new_target = pow(dn, n);
			FltH new_diff = abs(0.5f - new_target);
			if (new_diff<diff)
			{
				++adjustments;
				f = dn;
				target = new_target;
				diff = new_diff;
				continue;
			}
			break;
		}
		epsilon /= 2;
		++epsilondbl;
	}
}

template<typename Flt>
void Map<Flt>::setup_dbl(Flt target, MultiLogger& logger)
{
	using std::pow;
	double n = std::log(0.5) / std::log((double)target);
	count_dlb = std::roundl(n);
	Flt factor = std::pow(0.5, 1.0/n);

	logger << std::setprecision(100) << std::fixed;
	logger << "new factor     : " << factor << std::endl;
	adjust_factor(factor, count_dlb);
	logger << "adj. factor    : " << factor << std::endl;
	logger << "new count      : " << count_dlb << std::endl;
	FltH new_target = pow(factor, count_dlb);
	logger << "new doubling   : " << new_target << std::endl;

	logger << "adj / dbl      : " << adjustments << " / " << epsilondbl << std::endl;

	zoom_mul = factor;
	width  = (width  >> 2) << 2;
	height = (height >> 2) << 2;
	new_w = width  * 2;
	new_h = height * 2;
	logger << "adjusted size  : " << width << "x" << height << std::endl;
	logger << "new size       : " << new_w << "x" << new_h << std::endl;

	generate_init_rest();

	for (UL y=0; y<new_h; ++y)
	{
		const Flt& yld = vfy[y];
		for (UL x=0; x<new_w; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
		}
	}

}

template<typename Flt>
Image Map<Flt>::dbl_makefull(UL cap)
{
	std::vector<Scanline<Flt>> saved;
	saved.swap(points);

	generate_init_rest();

	for (UL y=0; y<new_h; ++y)
	{
		const Flt& yld = vfy[y];
		for (UL x=0; x<new_w; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
			get(x,y).docalc({xld, yld}, cap);
		}
	}

	using std::swap;
	swap(width, new_w); swap(height, new_h);
	auto img = makeimage(350,INT_MAX);
	swap(width, new_w); swap(height, new_h);

	saved.swap(points);

	return img;
}

template<typename Flt>
int Map<Flt>::generate_dbl(UL cap, bool first, bool display, MultiLogger& logger)
{

	#ifndef NDEBUG

	Updater::Init(new_h*3+1);
	if (display) Updater::Display();

	LineCache<Flt> lc = { cap, true, *this, first };
	lc.y_start = 0;
	lc.y_count = new_h;

	LineCache<Flt>::execute_dbl(&lc);

	UL sk = lc.skip_count;
	UL is = lc.inskip;
	UL maxout = lc.eff_cap;

	#else

	if (first)
		Updater::Init(new_h*3+4);
	else
		Updater::Init(new_h*2+4);
	if (display) Updater::Display();

	LineCache<Flt> lc[4] = {
		{ cap,  display, *this, first },
		{ cap,   false, *this, first },
		{ cap,   false, *this, first },
		{ cap,   false, *this, first },
	};

	UL i = 0;
	UL num = new_h / 4;
	UL ovr = new_h - (num*4);
	UL y = 0;
	lc[0].y_start = y; y += (lc[0].y_count = num + ovr);
	lc[1].y_start = y; y += (lc[1].y_count = num);
	lc[2].y_start = y; y += (lc[2].y_count = num);
	lc[3].y_start = y; y += (lc[3].y_count = num);

	UL maxout = 0;
	boost::thread tt[4];
	for (i=1; i<4; ++i)
	{
		tt[i] = boost::thread{&LineCache<Flt>::execute_dbl, lc+i};
	}

	LineCache<Flt>::execute_dbl(lc);

	boost::chrono::nanoseconds ns{250'000};
	int joined = 1;
	while (true)
	{
		if (display) Updater::Display();

		bool j = tt[joined].try_join_for(ns);
		if (j)
		{
			++joined;
			Updater::Tick();
			if (joined >= 4) break;
		}
	}

	UL sk = 0, is = 0;
	for (i=0; i<4; ++i)
	{
		if (lc[i].eff_cap > maxout)
			maxout = lc[i].eff_cap;
		sk += lc[i].skip_count;
		is += lc[i].inskip;
	}

	#endif

	Updater::Tick();
	if (display) Updater::Display();

	logger << "skipped        : " << sk << " pixels, of wich " << is << " was inside \n";
	logger << "effective cap  : " << maxout << "\n";

	return count_dlb;
}

template<typename Flt>
int Map<Flt>::sh_new_xcoord(int oldx)
{
	int hw = new_w/2;
	int dfc = hw - oldx;
	return hw - 2*dfc;
}

template<typename Flt>
int Map<Flt>::sh_new_ycoord(int oldy)
{
	int hh = new_h/2;
	int dfc = hh - oldy;
	return hh - 2*dfc;
}

template<typename Flt>
void Map<Flt>::new_out(std::string fn)
{
	using std::swap;
	swap(width, new_w); swap(height, new_h);
	makeimage(350,INT_MAX).Save(fn);
	swap(width, new_w); swap(height, new_h);
}

template<typename Flt>
int Map<Flt>::shuffle_dbl()
{
	generate_init_rest();

	auto copy = points;
	for (UL y=0; y<new_h; ++y)
	{
		const Flt& yld = vfy[y];
		for (UL x=0; x<new_w; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
		}
	}

	int cpy = 0;
	for (int y=0; y<(int)new_h; ++y)
	{
		auto newy = sh_new_ycoord(y);
		if ((newy<0) || (newy>=(int)new_h)) continue;
		for (int x=0; x<(int)new_w; ++x)
		{
			auto newx = sh_new_xcoord(x);
			if ((newy==y) && (newx==x)) continue;
			if ((newx<0) || (newx>=(int)new_w)) continue;
			const Point<Flt>& src = copy[y][x];
			Point<Flt>& dst = get(newx,newy);
			dst = src;
			++cpy;
		}
	}

	return cpy;
}

template struct Map<FltH>;
template struct Map<FltL>;

// ----------------------------------------------------------------------------

// *****************
// *** LineCache ***
// *****************

template<typename Flt>
LineCache<Flt>::LineCache(UL cap, bool display, Map<Flt>& map, bool first)
	: cap(cap)
	, display(display)
	, first(first)
	, map(map)
	, vfx(map.vfx)
	, vfy(map.vfy)
{}

template<typename Flt>
void LineCache<Flt>::base_init()
{
	eff_cap = 1;
	skip_count = 0;
	inskip = 0;
	n = y_count;
	w = map.new_w;
}

template<typename Flt>
void LineCache<Flt>::init_zero()
{
	for (UL i=0; i<n; ++i)
	{
		UL y = y_start + i;
		Flt& yld = vfy[y];
		for (UL x=0; x<w; ++x)
		{
			auto& p = map.get(x,y);
			std::complex<Flt> c{vfx[x], yld};
			p.init(c);
		}
	}
}

template<typename Flt>
void LineCache<Flt>::init_lim()
{
	xlo = 0;
	xhi = map.new_w-1;
	ylo = y_start;
	yhi = y_start+n-1;
}

template<typename Flt>
void LineCache<Flt>::dc(Point<Flt>& p, const std::complex<Flt>& c)
{
	bool did = p.docalc(c, cap);
	if (did && p.iter>eff_cap)
		eff_cap = p.iter;
}

template<typename Flt>
void LineCache<Flt>::even()
{
	for (UL i=0; i<n; ++i)
	{
		Updater::Tick();
		if (display)
			Updater::Display();
		if (i%2) continue;
		UL y = y_start + i;
		Flt& yld = vfy[y];
		for (UL x=0; x<w; x+=2)
		{
			auto& p = map.get(x,y);
			std::complex<Flt> c{vfx[x], yld};
			dc(p,c);
		}
	}
}

template<typename Flt>
void LineCache<Flt>::odd()
{
	for (UL i=0; i<n; ++i)
	{
		Updater::Tick();
		if (display)
			Updater::Display();
		if (!(i%2)) continue;
		UL y = y_start + i;
		Flt& yld = vfy[y];
		for (UL x=1; x<w; x+=2)
		{
			auto& p = map.get(x,y);
			if (p.status != Point<Flt>::calc) continue;
			if (p.iter != 1) continue;
			std::complex<Flt> c{vfx[x], yld};
			dc(p,c);
		}
	}
}

template<typename Flt>
bool LineCache<Flt>::ep_xy(UL x, UL y)
{
	auto& p = map.get(x,y);
	if (p.status != Point<Flt>::calc) return false;
	if (p.iter != 1) return false;
	if (x==xlo) return false;
	if (x==xhi) return false;
	if (y==ylo) return false;
	if (y==yhi) return false;
	if (map.get(x-1,y).status != Point<Flt>::in) return false;
	if (map.get(x+1,y).status != Point<Flt>::in) return false;
	if (map.get(x,y-1).status != Point<Flt>::in) return false;
	if (map.get(x,y+1).status != Point<Flt>::in) return false;
	p.status = Point<Flt>::in;
	p.pixtype = pt_black;
	++skip_count;
	++inskip;
	return true;
}

template<typename Flt>
bool LineCache<Flt>::ep_x(UL x, UL y)
{
	auto& p = map.get(x,y);
	if (p.status != Point<Flt>::calc) return false;
	if (p.iter != 1) return false;
	if (x==xlo) return false;
	if (x==xhi) return false;
	auto& pp = map.get(x-1,y);
	if (pp.status != Point<Flt>::out) return false;
	bool pe = pp.pixtype & pt_ep_msk;
	auto& pn = map.get(x+1,y);
	if (pn.status != Point<Flt>::out) return false;
	if (pp.iter != pn.iter) return false;
	bool ne = pn.pixtype & pt_ep_msk;
	if (pe && ne) return false;
	p.status = Point<Flt>::out;
	p.iter = pp.iter;
	p.over = (pp.over + pn.over)/2.0;
	p.pixtype = pt_ep_hor;
	++skip_count;
	return true;
}

template<typename Flt>
bool LineCache<Flt>::ep_y(UL x, UL y)
{
	auto& p = map.get(x,y);
	if (p.status != Point<Flt>::calc) return false;
	if (p.iter != 1) return false;
	if (y==ylo) return false;
	if (y==yhi) return false;
	auto& pp = map.get(x,y-1);
	if (pp.status != Point<Flt>::out) return false;
	bool pe = pp.pixtype & pt_ep_msk;
	auto& pn = map.get(x,y+1);
	if (pn.status != Point<Flt>::out) return false;
	if (pp.iter != pn.iter) return false;
	bool ne = pn.pixtype & pt_ep_msk;
	if (pe && ne) return false;
	p.status = Point<Flt>::out;
	p.iter = pp.iter;
	p.over = (float)(pp.over + pn.over)/2.0f;
	p.pixtype = pt_ep_ver;
	++skip_count;
	return true;
}

template<typename Flt>
bool LineCache<Flt>::setin()
{
	bool foundin = false;

	for (UL i=0; i<n; ++i)
	{
		UL y = y_start + i;
		for (UL x=0; x<w; ++x)
		{
			auto& p = map.get(x,y);
			if (p.status ==Point<Flt>::in) foundin = true;
			if (p.status != Point<Flt>::calc) continue;
			if (p.iter < cap) continue;
			p.status = Point<Flt>::in;
			foundin = true;
		}
	}
	return foundin;
}

template<typename Flt>
void LineCache<Flt>::all()
{
	for (UL i=0; i<n; ++i)
	{
		Updater::Tick();
		if (display)
			Updater::Display();

		UL y = y_start + i;
		for (UL x=0; x<w; ++x)
		{
			auto& p = map.get(x,y);
			if (p.status != Point<Flt>::calc) continue;
			if (p.iter != 1) continue;
			std::complex<Flt> c{vfx[x], vfy[y]};
			dc(p,c);
		}
	}
}

template<typename Flt>
void LineCache<Flt>::execute(LineCache* lc)
{
	lc->base_init();
	lc->init_zero();
	lc->even();
	lc->init_lim();

	auto all_f = [&](auto f) -> void
	{
		for (UL i=0; i<lc->n; i+=1)
		{
			UL y = lc->y_start + i;
			for (UL x=0; x<lc->w; x+=1)
				f(x,y);
		}
	};

	all_f( [&](UL x, UL y) { lc->ep_x(x,y); } );
	all_f( [&](UL x, UL y) { lc->ep_y(x,y); } );

	lc->odd();

	bool foundin = lc->setin();

	if (foundin)
		all_f( [&](UL x, UL y) { lc->ep_xy(x,y); } );

	lc->all();
}

template<typename Flt>
void LineCache<Flt>::execute_dbl(LineCache* lc)
{
	lc->base_init();

	#ifndef NDEBUG
	#define IDBO(fn) lc->map.new_out(fn)
	#else
	#define IDBO(fn) (void)fn
	#endif

	IDBO("ED_0_After_BI.bmp");

	if (lc->first)
	{
		lc->even();
		IDBO("ED_1_RanEvenFrst.bmp");
	}

	lc->init_lim();

	auto all_f = [&](auto f) -> void
	{
		for (UL i=0; i<lc->n; i+=1)
		{
			UL y = lc->y_start + i;
			for (UL x=0; x<lc->w; x+=1)
				f(x,y);
		}
	};

	all_f( [&](UL x, UL y) { lc->ep_x(x,y); } );
	all_f( [&](UL x, UL y) { lc->ep_y(x,y); } );

	IDBO("ED_2_Extrapolate_X_and_Y.bmp");

	lc->odd();
	IDBO("ED_3_After_Odd.bmp");

	bool foundin = lc->setin();

	if (foundin)
	{
		IDBO("ED_4_After_Setin.bmp");
		all_f( [&](UL x, UL y) { lc->ep_xy(x,y); } );
		IDBO("ED_5_Extrapolate_XY.bmp");
	}

	lc->all();
	IDBO("ED_6_After_All.bmp");

	#undef IDBO
}

template struct LineCache<FltH>;
template struct LineCache<FltL>;

// ----------------------------------------------------------------------------

// ******************
// *** DiffReport ***
// ******************

namespace {
	bool first = true;
	int current;
	std::ofstream diffr;
	double prv_zf;
	float  prv_mod;
}

void DiffReport(int fr)
{
	if (first)
	{
		diffr.open("DiffReport.txt");
	} else {
		assert(fr == (current+1));
	}
	current = fr;
}

void DiffReport(double zf, float mod)
{
	if (!first)
	{
		diffr << std::setw(6) << current << " ";
		diffr << std::setprecision(30) << std::setw(35);
		diffr << std::fixed << (zf / prv_zf) << " ";
		diffr << std::setprecision(10) << std::setw(15);
		diffr << std::fixed << (mod / prv_mod) << std::endl << std::flush;
	}
	first = false;
	prv_zf  = zf;
	prv_mod = mod;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/GPFCorrection.h>
#include <BayesFilters/utils.h>

#include <Eigen/Cholesky>

#include <exception>

using namespace bfl;
using namespace Eigen;

GPFCorrection::GPFCorrection
(
    std::unique_ptr<LikelihoodModel> likelihood_model,
    std::unique_ptr<GaussianCorrection> gaussian_correction,
    std::unique_ptr<StateModel> state_model
) noexcept :
    GPFCorrection(std::move(likelihood_model), std::move(gaussian_correction), std::move(state_model), 1)
{ }

GPFCorrection::GPFCorrection
(
    std::unique_ptr<LikelihoodModel> likelihood_model,
    std::unique_ptr<GaussianCorrection> gaussian_correction,
    std::unique_ptr<StateModel> state_model,
    unsigned int seed
) noexcept :
    likelihood_model_(std::move(likelihood_model)),
    gaussian_correction_(std::move(gaussian_correction)),
    state_model_(std::move(state_model)),
    generator_(std::mt19937_64(seed)),
    distribution_(std::normal_distribution<double>(0.0, 1.0)),
    gaussian_random_sample_([&] { return (distribution_)(generator_); })
{ }

GPFCorrection::GPFCorrection(GPFCorrection&& correction) noexcept :
    PFCorrection(std::move(correction)),
    likelihood_model_(std::move(correction.likelihood_model_)),
    gaussian_correction_(std::move(correction.gaussian_correction_)),
    state_model_(std::move(correction.state_model_)),
    generator_(std::move(correction.generator_)),
    distribution_(std::move(correction.distribution_)),
    gaussian_random_sample_(std::move(correction.gaussian_random_sample_)),
    valid_likelihood_(correction.valid_likelihood_),
    likelihood_(std::move(correction.likelihood_))
{ }

GPFCorrection& GPFCorrection::operator=(GPFCorrection&& correction) noexcept
{
    PFCorrection::operator=(std::move(correction));

    gaussian_correction_ = std::move(correction.gaussian_correction_);

    state_model_ = std::move(correction.state_model_);

    generator_ = std::move(correction.generator_);

    distribution_ = std::move(correction.distribution_);

    gaussian_random_sample_ = std::move(correction.gaussian_random_sample_);

    valid_likelihood_ = correction.valid_likelihood_;

    likelihood_ = std::move(correction.likelihood_);

    return *this;
}

MeasurementModel& GPFCorrection::getMeasurementModel() noexcept
{
    return gaussian_correction_->getMeasurementModel();
}

LikelihoodModel& GPFCorrection::getLikelihoodModel() noexcept
{
    return *likelihood_model_;
}

std::pair<bool, Eigen::VectorXd> GPFCorrection::getLikelihood()
{
    return std::make_pair(valid_likelihood_, likelihood_);
}

void GPFCorrection::correctStep(const bfl::ParticleSet& pred_particles, bfl::ParticleSet& corr_particles)
{
    /* Propagate Gaussian belief associated to each particle. */
    gaussian_correction_->correct(pred_particles, corr_particles);

    /* Sample from the proposal distribution. */
    for (std::size_t i = 0; i < pred_particles.components; i++)
    {
        corr_particles.state(i) = sampleFromProposal(corr_particles.mean(i), corr_particles.covariance(i));
    }

    /* Evaluate the likelihood. */
    std::tie(valid_likelihood_, likelihood_) = getLikelihoodModel().likelihood(getMeasurementModel(), corr_particles.state());

    if (!valid_likelihood_)
    {
        corr_particles = pred_particles;

        return;
    }

    /* Evaluate the transition probability. */
    VectorXd transition_probability = state_model_->getTransitionProbability(pred_particles.state(), corr_particles.state());

    /* Update weights in the log space.
     * w_{k} = w_{k-1} + log(likelihood) + log(transition_probability) - log(proposal_distribution)
     */
    double eps = std::numeric_limits<double>::min();
    for (std::size_t i = 0; i < pred_particles.components; i++)
    {
        corr_particles.weight(i) = pred_particles.weight(i) + std::log(likelihood_(i) + eps) + std::log(transition_probability(i) + eps) - std::log(evaluateProposal(corr_particles.state(i), corr_particles.mean(i), corr_particles.covariance(i)) + eps);
    }
}

Eigen::VectorXd GPFCorrection::sampleFromProposal(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance)
{
    /* Evaluate the square root of the state covariance matrix using the LDL' decomposition
       (it can be used even if the covariance matrix is positive semidefinite). */
    LDLT<MatrixXd> chol_ldlt(covariance);
    MatrixXd sqrt_P = (chol_ldlt.transpositionsP() * MatrixXd::Identity(mean.size(), mean.size())).transpose() *
                       chol_ldlt.matrixL() *
                       chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();

    /* Sample i.i.d standard normal univariates. */
    VectorXd rand_vectors(mean.size());
    for (int i = 0; i < rand_vectors.size(); i++)
        rand_vectors(i) = gaussian_random_sample_();

    /* Return a sample from a normal multivariate having mean `mean` and covariance 'covariance'. */
    return mean + sqrt_P * rand_vectors;
}

double GPFCorrection::evaluateProposal
(
    const Eigen::VectorXd& state,
    const Eigen::VectorXd& mean,
    const Eigen::MatrixXd& covariance
)
{
    /* Evaluate the proposal distribution, a Gaussian centered in 'mean' and having
       covariance 'covariance', in the state 'state'. */
    return utils::multivariate_gaussian_density(state, mean, covariance).coeff(0);
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <algorithm>
#include <boost/geometry.hpp>
#include <optional>

#include "pyinterp/detail/geometry/box.hpp"
#include "pyinterp/detail/geometry/point.hpp"
#include "pyinterp/detail/math/radial_basis_functions.hpp"
#include "pyinterp/detail/math/window_functions.hpp"

namespace pyinterp::detail::geometry {

/// Index points in the Cartesian space at N dimensions.
///
/// @tparam CoordinateType The class of storage for a point's coordinates.
/// @tparam Type The type of data stored in the tree.
/// @tparam N Number of dimensions in the Cartesian space handled.
template <typename CoordinateType, typename Type, size_t N>
class RTree {
 public:
  /// Type of the point handled by this instance.
  using point_t = geometry::PointND<CoordinateType, N>;

  /// Type of distances between two points.
  using distance_t = typename boost::geometry::default_distance_result<
      point_t, geometry::PointND<CoordinateType, N>>::type;

  /// Type of query results.
  using result_t = std::pair<distance_t, Type>;

  /// Value handled by this object
  using value_t = std::pair<point_t, Type>;

  /// Spatial index used
  using rtree_t =
      boost::geometry::index::rtree<value_t, boost::geometry::index::rstar<16>>;

  /// Type of the implicit conversion between the type of coordinates and values
  using promotion_t =
      decltype(std::declval<CoordinateType>() + std::declval<Type>());

  /// Default constructor
  RTree() : tree_(new rtree_t{}) {}

  /// Default destructor
  virtual ~RTree() = default;

  /// Default copy constructor
  RTree(const RTree &) = default;

  /// Default copy assignment operator
  auto operator=(const RTree &) -> RTree & = default;

  /// Move constructor
  RTree(RTree &&) noexcept = default;

  /// Move assignment operator
  auto operator=(RTree &&) noexcept -> RTree & = default;

  /// Returns the box able to contain all values stored in the container.
  ///
  /// @returns The box able to contain all values stored in the container or an
  /// invalid box if there are no values in the container.
  virtual inline auto bounds() const
      -> std::optional<geometry::BoxND<CoordinateType, N>> {
    if (empty()) {
      return {};
    }
    return tree_->bounds();
  }

  /// Returns the number of points of this mesh
  ///
  /// @return the number of points
  [[nodiscard]] constexpr auto size() const -> size_t { return tree_->size(); }

  /// Query if the container is empty.
  ///
  /// @return true if the container is empty.
  [[nodiscard]] constexpr auto empty() const -> bool { return tree_->empty(); }

  /// Removes all values stored in the container.
  inline auto clear() -> void { tree_->clear(); }

  /// The tree is created using packing algorithm (The old data is erased before
  /// construction.)
  ///
  /// @param points
  inline auto packing(const std::vector<value_t> &points) -> void {
    *tree_ = rtree_t(points);
  }

  /// Insert new data into the search tree
  ///
  /// @param point
  inline auto insert(const value_t &value) -> void { tree_->insert(value); }

  /// Search for the K nearest neighbors of a given point.
  ///
  /// @param point Point of interest
  /// @param k The number of nearest neighbors to search.
  /// @return the k nearest neighbors:
  auto query(const point_t &point, const uint32_t k) const
      -> std::vector<result_t> {
    auto result = std::vector<result_t>();
    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&point, &result](const auto &item) {
          result.emplace_back(std::make_pair(
              boost::geometry::distance(point, item.first), item.second));
        });
    return result;
  }

  /// Search for the nearest neighbors of a given point within a radius r.
  ///
  /// @param point Point of interest
  /// @param radius distance within which neighbors are returned
  /// @return the k nearest neighbors
  auto query_ball(const point_t &point, const distance_t radius) const
      -> std::vector<result_t> {
    auto result = std::vector<result_t>();
    std::for_each(
        tree_->qbegin(boost::geometry::index::satisfies([&](const auto &item) {
          return boost::geometry::distance(item.first, point) <= radius;
        })),
        tree_->qend(), [&point, &result](const auto &item) {
          result.emplace_back(std::make_pair(
              boost::geometry::distance(point, item.first), item.second));
        });
    return result;
  }

  /// Search for the nearest K neighbors around a given point.
  ///
  /// @param point Point of interest
  /// @param k The number of nearest neighbors to search.
  /// @return the k nearest neighbors if the point is within by its
  /// neighbors.
  auto query_within(const point_t &point, const uint32_t k) const
      -> std::vector<result_t> {
    auto result = std::vector<result_t>();
    auto points = boost::geometry::model::multi_point<point_t>();
    points.reserve(k);

    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&points, &point, &result](const auto &item) {
          points.emplace_back(item.first);
          result.emplace_back(std::make_pair(
              boost::geometry::distance(point, item.first), item.second));
        });

    // Are found points located around the requested point?
    if (!boost::geometry::covered_by(
            point, boost::geometry::return_envelope<
                       boost::geometry::model::box<point_t>>(points))) {
      return {};
    }
    return result;
  }

  /// Interpolation of the value at the requested position.
  ///
  /// @param point Point of interrest
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param p the power parameter.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return a tuple containing the interpolated value and the number of
  /// neighbors used in the calculation.
  auto inverse_distance_weighting(const point_t &point, distance_t radius,
                                  uint32_t k, uint32_t p, bool within) const
      -> std::pair<distance_t, uint32_t> {
    distance_t result = 0;
    distance_t total_weight = 0;

    // We're looking for the nearest k points.
    auto nearest = within ? query_within(point, k) : query(point, k);
    uint32_t neighbors = 0;

    // For each point, the distance between the point requested and the point
    // found is calculated and the information required for the Inverse distance
    // weighting interpolation method is updated.
    for (const auto &item : nearest) {
      const auto distance = item.first;
      if (distance < 1e-6) {
        // If the user has requested a grid point, the mesh value is returned.
        return std::make_pair(item.second, k);
      }

      if (distance <= radius) {
        // If the neighbor found is within an acceptable radius it can be taken
        // into account in the calculation.
        auto wk =
            static_cast<Type>(1 / std::pow(distance, static_cast<Type>(p)));
        total_weight += wk;
        result += item.second * wk;
        ++neighbors;
      }
    }

    // Finally the interpolated value is returned if there are selected points
    // otherwise one returns an undefined value.
    return total_weight != 0
               ? std::make_pair(static_cast<distance_t>(result / total_weight),
                                neighbors)
               : std::make_pair(std::numeric_limits<distance_t>::quiet_NaN(),
                                static_cast<uint32_t>(0));
  }

  /// Search for the nearest K neighbors of a given point.
  ///
  /// @param point Point of interest
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @return A tuple containing the matrix describing the coordinates of the
  /// selected points and a vector of the values of the points. The arrays will
  /// be empty if no points are selected.
  auto nearest(const point_t &point, const distance_t radius,
               const uint32_t k) const
      -> std::tuple<Matrix<promotion_t>, Vector<promotion_t>> {
    auto coordinates = Matrix<promotion_t>(N, k);
    auto values = Vector<promotion_t>(k);
    auto jx = 0U;

    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&](const auto &item) {
          if (boost::geometry::distance(point, item.first) <= radius) {
            // If the point is not too far away, it is inserted and
            // its coordinates and value are stored.
            for (size_t ix = 0; ix < N; ++ix) {
              coordinates(ix, jx) = geometry::point::get(item.first, ix);
            }
            values(jx++) = item.second;
          }
        });

    // The arrays are resized according to the number of selected points. This
    // number can be zero.
    coordinates.conservativeResize(N, jx);
    values.conservativeResize(jx);
    return std::make_tuple(coordinates, values);
  }

  /// Search for the nearest K neighbors around a given point.
  ///
  /// @param point Point of interest
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @return A tuple containing the matrix describing the coordinates of the
  /// selected points and a vector of the values of the points. The arrays will
  /// be empty if no points are selected.
  auto nearest_within(const point_t &point, const distance_t radius,
                      const uint32_t k) const
      -> std::tuple<Matrix<promotion_t>, Vector<promotion_t>> {
    auto points = boost::geometry::model::multi_point<point_t>();
    auto coordinates = Matrix<promotion_t>(N, k);
    auto values = Vector<promotion_t>(k);
    auto jx = 0U;

    // List of selected points ()
    points.reserve(k);

    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&](const auto &item) {
          if (boost::geometry::distance(point, item.first) <= radius) {
            // If the point is not too far away, it is inserted and
            // its coordinates and value are stored.
            points.emplace_back(item.first);
            for (size_t ix = 0; ix < N; ++ix) {
              coordinates(ix, jx) = geometry::point::get(item.first, ix);
            }
            values(jx++) = item.second;
          }
        });

    // If the point is not covered by its closest neighbors, an empty set will
    // be returned.
    if (!boost::geometry::covered_by(
            point, boost::geometry::return_envelope<
                       boost::geometry::model::box<point_t>>(points))) {
      jx = 0;
    }

    // The arrays are resized according to the number of selected points. This
    // number can be zero.
    coordinates.conservativeResize(N, jx);
    values.conservativeResize(jx);
    return std::make_tuple(coordinates, values);
  }

  /// Interpolate the value of a point using a Radial Basis Function.
  ///
  /// @param point Point of interest
  /// @param rbf The radial basis function to be used.
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return A pair containing the interpolated value and the number of
  /// neighbors used in the calculation.
  auto radial_basis_function(const point_t &point,
                             const math::RBF<promotion_t> &rbf,
                             distance_t radius, uint32_t k, bool within) const
      -> std::pair<promotion_t, uint32_t> {
    auto [coordinates, values] =
        within ? nearest_within(point, radius, k) : nearest(point, radius, k);
    if (values.size() == 0) {
      return std::make_pair(std::numeric_limits<promotion_t>::quiet_NaN(), 0);
    }
    auto xi = Eigen::Matrix<promotion_t, N, 1>();
    for (size_t ix = 0; ix < N; ++ix) {
      xi(ix, 0) = geometry::point::get(point, ix);
    }
    auto interpolated = rbf.interpolate(coordinates, values, xi);
    return std::make_pair(interpolated(0),
                          static_cast<uint32_t>(values.size()));
  }

  /// Interpolate the value of a point using a Window Function.
  ///
  /// @param point Point of interest
  /// @param wf The window function to be used.
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return A pair containing the interpolated value and the number of
  /// neighbors used in the calculation.
  auto window_function(const point_t &point,
                       const math::WindowFunction<distance_t> &wf,
                       const distance_t arg, distance_t radius, uint32_t k,
                       bool within) const -> std::pair<distance_t, uint32_t> {
    distance_t result = 0;
    distance_t total_weight = 0;

    auto nearest = within ? query_within(point, k) : query(point, k);
    uint32_t neighbors = 0;

    for (const auto &item : nearest) {
      const auto distance = item.first;

      auto wk = wf(distance, radius, arg);
      total_weight += wk;
      result += item.second * wk;
      ++neighbors;
    }

    return total_weight != 0
               ? std::make_pair(static_cast<distance_t>(result / total_weight),
                                neighbors)
               : std::make_pair(std::numeric_limits<distance_t>::quiet_NaN(),
                                static_cast<uint32_t>(0));
  }

 protected:
  /// Geographic index used to store data and their searches.
  std::shared_ptr<rtree_t> tree_;
};

}  // namespace pyinterp::detail::geometry


#pragma once

#include <cstddef>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/serialization/strong_typedef.hpp>

#include <algorithm>
#include <array>
#include <cstdint>
#include <cstring>
#include <optional>
#include <set>
#include <variant>
#include <vector>

namespace koinos {

   using std::array;
   using std::optional;
   using std::set;
   using std::variant;
   using std::vector;

   typedef boost::multiprecision::int128_t  int128_t;
   typedef boost::multiprecision::uint128_t uint128_t;
   typedef boost::multiprecision::int256_t  int256_t;
   typedef boost::multiprecision::uint256_t uint256_t;

   typedef boost::multiprecision::number<
      boost::multiprecision::cpp_int_backend<
         160,
         160,
         boost::multiprecision::unsigned_magnitude,
         boost::multiprecision::unchecked, void
      >
   > uint160_t;

   typedef boost::multiprecision::number<
      boost::multiprecision::cpp_int_backend<
         160,
         160,
         boost::multiprecision::signed_magnitude,
         boost::multiprecision::unchecked, void
      >
   > int160_t;

   typedef bool      boolean;
   typedef int8_t    int8;
   typedef uint8_t   uint8;
   typedef int16_t   int16;
   typedef uint16_t  uint16;
   typedef int32_t   int32;
   typedef uint32_t  uint32;
   typedef int64_t   int64;
   typedef uint64_t  uint64;
   typedef int128_t  int128;
   typedef uint128_t uint128;
   typedef int160_t  int160;
   typedef uint160_t uint160;
   typedef int256_t  int256;
   typedef uint256_t uint256;

   using variable_blob = std::vector< char >;

   template < size_t N >
   using fixed_blob    = std::array< char, N >;

   BOOST_STRONG_TYPEDEF( uint64_t, timestamp_type );
   BOOST_STRONG_TYPEDEF( uint64_t, block_height_type );

   struct multihash
   {
      uint64_t      id = 0;
      variable_blob digest;

      bool operator ==( const multihash& other ) const
      {
         return ( id == other.id )
            && ( digest.size() == other.digest.size() )
            && ( std::memcmp( digest.data(), other.digest.data(), other.digest.size() ) == 0 );
      }

      bool operator !=( const multihash& other ) const
      {
         return !(*this == other);
      }

      bool operator <( const multihash& other ) const
      {
         int64_t res = (int64_t)id - (int64_t)other.id;
         if( res < 0 ) return true;
         if( res > 0 ) return false;
         res = digest.size() - other.digest.size();
         if( res < 0 ) return true;
         if( res > 0 ) return false;
         return std::memcmp( digest.data(), other.digest.data(), digest.size() ) < 0;
      }

      bool operator <=( const multihash& other ) const
      {
         int64_t res = (int64_t)id - (int64_t)other.id;
         if( res < 0 ) return true;
         if( res > 0 ) return false;
         res = digest.size() - other.digest.size();
         if( res < 0 ) return true;
         if( res > 0 ) return false;
         return std::memcmp( digest.data(), other.digest.data(), digest.size() ) <= 0;
      }

      bool operator >( const multihash& other ) const
      {
         return !(*this <= other);
      }

      bool operator >=( const multihash& other ) const
      {
         return !(*this < other);
      }
   };

} // koinos


#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

void solve() {
  int cols; cin >> cols;
  int rows; cin >> rows;

  graph G;
  edge_adder adder(G);

  int targetFlow = 0;
  vertex_desc source = boost::add_vertex(G);
  vertex_desc target = boost::add_vertex(G);

  vector<vector<vertex_desc>> in(rows, vector<vertex_desc>(cols));
  vector<vector<vertex_desc>> out(rows, vector<vertex_desc>(cols));
  for (int row = 0; row < rows; ++row) {
    for (int col = 0; col < cols; ++col) {
      in[row][col] = boost::add_vertex(G);
      out[row][col] = boost::add_vertex(G);
    }
  }
  for (int row = 0; row < rows; ++row) {
    for (int col = 0; col < cols; ++col) {
      char c; cin >> c;
      bool blocked = (c == 'x');
      if (blocked) {
        continue;
      }
      bool even = (row + col) % 2 == 0;
      if (even) {
        adder.add_edge(source, in[row][col], 1);

        if (row > 0)
          adder.add_edge(in[row][col], out[row - 1][col], 1);
        if (row < rows - 1)
          adder.add_edge(in[row][col], out[row + 1][col], 1);
        if (col > 0)
          adder.add_edge(in[row][col], out[row][col - 1], 1);
        if (col < cols - 1)
          adder.add_edge(in[row][col], out[row][col + 1], 1);
      }
      else {
        adder.add_edge(out[row][col], target, 1);
      }
      ++targetFlow;
    }
  }
  if (targetFlow % 2 != 0) {
    cout << "no" << endl;
    return;
  }
  targetFlow /= 2;

  long flow = boost::push_relabel_max_flow(G, source, target);
  cout << ((flow == targetFlow) ? "yes" : "no") << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


// Copyright (c) 2021 Sota Tsuji
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php

#pragma once

#include <boost/multiprecision/cpp_int.hpp>
#include <vector>

#include "graph.hpp"

namespace extraction_of_maximum_clique {
using std::tuple;
using std::vector;
using Bint = boost::multiprecision::cpp_int;
using Matrix = vector<vector<int>>;
using Polynomial = vector<Bint>;
using Polynomials = vector<Polynomial>;

enum struct Infinity { Positive, Negative };

Polynomial operator-(Polynomial pol);
Polynomial operator/(const Polynomial& pol1, const Polynomial& pol2);
Polynomial operator/(Polynomial pol, const Bint x);

Bint get_maximum_coefficient(const int n);
vector<int> get_prime_list(const int n, const Bint c);
// Polynomial get_coefficient(const WeightedGraph& S);
tuple<Polynomial, Polynomial, Bint> polynomial_division(Polynomial f,
                                                        const Polynomial& g);
int get_number_of_roots(Polynomial f);
int get_km(const WeightedGraph& S);

vector<int> sieve_of_Eratosthenes(const int k);
Matrix get_double_adjacent_matrix(const WeightedGraph& S);
// Polynomial get_coefficient_by_modulo(const WeightedGraph& S, const int p);
// Polynomial merge_coefficients(const Polynomial& f, const Polynomial& g);
Polynomial differential(const Polynomial& f);
Polynomial simplify_coefficient(const Polynomial& f);
Polynomial gcd(Polynomial f, Polynomial g);
Polynomial rem(const Polynomial& f, const Polynomial& g);
Bint substitute_into_polynomial(const Polynomial& f, const int x);
int sigma(const Polynomials& fs, const int alpha);
int sigma(const Polynomials& fs, const Infinity inf);
int get_number_of_roots_by_strum(const Polynomial& f);

/*
The following functions are tentative.
Currently, "get_coefficient" is NOT a polynomial-time algorithm.
Later, we are going to replace it with a polynomial-time algorithm.
*/
using PolynomialMatrix = vector<vector<Polynomial>>;
Polynomial get_coefficient(const WeightedGraph& S);
Polynomial calculate_determinant(const PolynomialMatrix& A);
Polynomial operator*(const Polynomial& pol1, const Polynomial& pol2);
Polynomial operator*(const int x, const Polynomial& pol);
Polynomial& operator+=(Polynomial& self, const Polynomial& other);
}  // namespace extraction_of_maximum_clique


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/numeric/fixed_point.hpp>
#include <srook/limits/numeric_limits.hpp>

BOOST_AUTO_TEST_SUITE(numeric_fixed_point_test)

BOOST_AUTO_TEST_CASE(fixed_point_operator_test1)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    fixed_point_type fp {};

    ~fp;
    +fp;
    -fp;
    !fp;
    ++fp;
    --fp;
    fp++;
    fp--;
}

BOOST_AUTO_TEST_CASE(fixed_point_operator_test2)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    auto fp = srook::numeric_limits<fixed_point_type>::max();
    try {
        fp.increment();
    } catch (std::overflow_error) {}
    try {
        fp.post_increment();
    } catch (std::overflow_error) {}
    fp = srook::numeric_limits<fixed_point_type>::min();
    try {
        fp.decrement();
    } catch (std::underflow_error) {}
    try {
        fp.post_decrement();
    } catch (std::underflow_error) {}
}

BOOST_AUTO_TEST_CASE(fixed_point_operator_test3)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    fixed_point_type fp1 = 2;
    auto fp2 = srook::numeric_limits<fixed_point_type>::max();
    auto fp3 = srook::numeric_limits<fixed_point_type>::min();

    fp1 /= fp1;
    fp1 += fp1;
    fp1 *= fp1;
    fp1 |= fp1;
    fp1 ^= fp1;
    fp1 &= fp1;
    fp1 >>= fp1;
    fp1 <<= fp1;
    fp1 -= fp1;

    try {
        fp2.iadd(fp2);
    } catch (std::overflow_error) {}
    try {
        fp3.isub(fp2);
    } catch (std::underflow_error) {}
    try {
        fp2.imul(fp2);
    } catch (std::overflow_error) {}
    fp3.idiv(fp2);
}

BOOST_AUTO_TEST_CASE(fixed_point_operator_test4)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    fixed_point_type fp1 = 2;
    fp1 + 1;
    fp1 - 1;
    fp1 * 1;
    fp1 / 1;
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Aleksey Gurtovoy 2002-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Source: /cvsroot/boost/boost/libs/mpl/test/zip_view.cpp,v $
// $Date: 2004/09/02 15:41:35 $
// $Revision: 1.4 $

#include <boost/mpl/zip_view.hpp>

#include <boost/mpl/transform_view.hpp>
#include <boost/mpl/filter_view.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/unpack_args.hpp>
#include <boost/mpl/math/is_even.hpp>

#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef transform_view<
          zip_view< vector< range_c<int,0,10>, range_c<int,10,20> > >
        , unpack_args< plus<> >
        > result;

    MPL_ASSERT(( equal<
          result
        , filter_view< range_c<int,10,30>, is_even<_> >
        , equal_to<_,_>
        > ));
}


#pragma once

#include <string>
#include <Eigen/Core>

std::string main_print()
{
    float norm = Eigen::Vector3f{0,1,2}.norm();
    return  "Hello! Eigen::Vector3f{0,1,2}.norm()=" + std::to_string(norm);
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm statistic sum
#include <limits>
#include <numeric>
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_array.h"
#include "fern/feature/core/data_customization_point/masked_scalar.h"
#include "fern/algorithm/core/test/test_utils.h"
#include "fern/algorithm/statistic/sum.h"

namespace f = fern;
namespace fa = f::algorithm;
namespace ft = f::test;

template<
    typename ExecutionPolicy,
    typename Argument,
    typename Result>
void verify_0d_0d(
    ExecutionPolicy& execution_policy,
    Argument const& value,
    Result const& result_we_want)
{
    int result_we_got{-9};
    fa::statistic::sum<>(execution_policy, value, result_we_got);
    BOOST_CHECK_EQUAL(result_we_got, result_we_want);
}

template<
    typename ExecutionPolicy>
void test_0d_0d(
    ExecutionPolicy& execution_policy)
{
    {
        int value{5};
        int result_we_want{5};
        verify_0d_0d(execution_policy, value, result_we_want);
    }

    {
        int value{-5};
        int result_we_want{-5};
        verify_0d_0d(execution_policy, value, result_we_want);
    }

    {
        int value{0};
        int result_we_want{0};
        verify_0d_0d(execution_policy, value, result_we_want);
    }
}

template<
    typename ExecutionPolicy,
    typename Argument,
    typename Result>
void verify_0d_0d_masked(
    ExecutionPolicy& execution_policy,
    Argument const& value,
    Result const& result_we_want)
{
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<bool>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;

    f::MaskedScalar<int> result_we_got{-9};

    InputNoDataPolicy input_no_data_policy{{value.mask(), true}};
    OutputNoDataPolicy output_no_data_policy(result_we_got.mask(), true);

    fa::statistic::sum<fa::sum::OutOfRangePolicy>(
        input_no_data_policy, output_no_data_policy, execution_policy,
        value, result_we_got);
    BOOST_CHECK_EQUAL(result_we_got, result_we_want);
}

template<
    typename ExecutionPolicy>
void test_0d_0d_masked(
    ExecutionPolicy& execution_policy)
{
    // Regular case.
    {
        f::MaskedScalar<int> value{5};
        f::MaskedScalar<int> result_we_want{5};
        verify_0d_0d_masked(execution_policy, value, result_we_want);
    }

    // Mask a value.
    {
        f::MaskedScalar<int> value{5, true};
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_0d_0d_masked(execution_policy, value, result_we_want);
    }
}

template<
    typename ExecutionPolicy,
    typename Argument,
    typename Result>
void verify_1d_0d(
    ExecutionPolicy& execution_policy,
    Argument const& value,
    Result const& result_we_want)
{
    int result_we_got{-9};
    fa::statistic::sum<>(execution_policy, value, result_we_got);
    BOOST_CHECK_EQUAL(result_we_got, result_we_want);
}

template<
    typename ExecutionPolicy>
void test_1d_0d(
    ExecutionPolicy& execution_policy)
{
    // Regular case.
    {
        f::Array<int, 1> value(ft::nr_elements_1d);
        std::iota(value.data(), value.data() + ft::nr_elements_1d, 0);
        int result_we_want{std::accumulate(value.data(), value.data() +
            ft::nr_elements_1d, 0)};
        verify_1d_0d(execution_policy, value, result_we_want);
    }

    // Out of range.
    {
        f::Array<int, 1> value(ft::nr_elements_1d);
        std::iota(value.data(), value.data() + ft::nr_elements_1d, 0);
        get(value, 5) = std::numeric_limits<int>::max();
        // Overflow, integer wrap.
        int result_we_want = std::accumulate(value.data(), value.data() +
            ft::nr_elements_1d, 0);
        verify_1d_0d(execution_policy, value, result_we_want);
    }

    // Empty.
    {
        f::Array<int, 1> value(0);
        int result_we_want{-9};
        verify_1d_0d(execution_policy, value, result_we_want);
    }
}

template<
    typename ExecutionPolicy,
    typename Argument,
    typename Result>
void verify_1d_0d_masked(
    ExecutionPolicy& execution_policy,
    Argument const& value,
    Result const& result_we_want)
{
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<f::Mask<1>>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;

    f::MaskedScalar<int> result_we_got{-9};

    InputNoDataPolicy input_no_data_policy{{value.mask(), true}};
    OutputNoDataPolicy output_no_data_policy(result_we_got.mask(), true);

    fa::statistic::sum<fa::sum::OutOfRangePolicy>(
        input_no_data_policy, output_no_data_policy, execution_policy,
        value, result_we_got);
    BOOST_CHECK_EQUAL(result_we_got, result_we_want);
}

template<
    typename ExecutionPolicy>
void test_1d_0d_masked(
    ExecutionPolicy& execution_policy)
{
    // Regular case.
    {
        f::MaskedArray<int, 1> value(ft::nr_elements_1d);
        std::iota(value.data(), value.data() + ft::nr_elements_1d, 0);
        f::MaskedScalar<int> result_we_want{std::accumulate(
            value.data(), value.data() + ft::nr_elements_1d, 0)};
        verify_1d_0d_masked(execution_policy, value, result_we_want);
    }

    // Mask a value.
    {
        f::MaskedArray<int, 1> value(ft::nr_elements_1d);
        std::iota(value.data(), value.data() + ft::nr_elements_1d, 0);
        get(value.mask(), 5) = true;
        f::MaskedScalar<int> result_we_want{std::accumulate(
            value.data(), value.data() + ft::nr_elements_1d, 0)};
        result_we_want -= get(value, 5);
        verify_1d_0d_masked(execution_policy, value, result_we_want);
    }

    // Mask all values.
    {
        f::MaskedArray<int, 1> value(ft::nr_elements_1d);
        value.mask().fill(true);
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_1d_0d_masked(execution_policy, value, result_we_want);
    }

    // Out of range, within a block.
    {
        f::MaskedArray<int, 1> value(ft::nr_elements_1d, 0);
        get(value, 0) = std::numeric_limits<int>::max();
        get(value, 1) = 1;
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_1d_0d_masked(execution_policy, value, result_we_want);
    }

    // Out of range, when aggregating block results.
    {
        f::MaskedArray<int, 1> value(ft::nr_elements_1d, 0);
        value.fill(0);
        get(value, 0) = std::numeric_limits<int>::max();
        get(value, ft::nr_elements_1d - 1) = 1;
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_1d_0d_masked(execution_policy, value, result_we_want);
    }

    // Empty.
    {
        f::MaskedArray<int, 1> value(0);
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_1d_0d_masked(execution_policy, value, result_we_want);
    }
}

template<
    typename ExecutionPolicy,
    typename Argument,
    typename Result>
void verify_2d_0d(
    ExecutionPolicy& execution_policy,
    Argument const& value,
    Result const& result_we_want)
{
    int result_we_got{-9};
    fa::statistic::sum<>(execution_policy, value, result_we_got);
    BOOST_CHECK_EQUAL(result_we_got, result_we_want);
}

template<
    typename ExecutionPolicy>
void test_2d_0d(
    ExecutionPolicy& execution_policy)
{
    // Regular case.
    {
        f::Array<int, 2> value(f::extents[ft::nr_rows][ft::nr_cols]);
        std::iota(value.data(), value.data() + ft::nr_elements_2d, 0);
        int result_we_want{std::accumulate(value.data(), value.data() +
            ft::nr_elements_2d, 0)};
        verify_2d_0d(execution_policy, value, result_we_want);
    }

    // Empty.
    {
        f::Array<int, 2> value(f::extents[0][0]);
        int result_we_want{-9};
        verify_2d_0d(execution_policy, value, result_we_want);
    }
}

template<
    typename ExecutionPolicy,
    typename Argument,
    typename Result>
void verify_2d_0d_masked(
    ExecutionPolicy& execution_policy,
    Argument const& value,
    Result const& result_we_want)
{
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<f::Mask<2>>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;

    f::MaskedScalar<int> result_we_got{-9};

    InputNoDataPolicy input_no_data_policy{{value.mask(), true}};
    OutputNoDataPolicy output_no_data_policy(result_we_got.mask(), true);

    fa::statistic::sum<fa::sum::OutOfRangePolicy>(
        input_no_data_policy, output_no_data_policy, execution_policy,
        value, result_we_got);
    BOOST_CHECK_EQUAL(result_we_got, result_we_want);
}

template<
    typename ExecutionPolicy>
void test_2d_0d_masked(
    ExecutionPolicy& execution_policy)
{
    // Regular case.
    {
        f::MaskedArray<int, 2> value(f::extents[ft::nr_rows][ft::nr_cols]);
        std::iota(value.data(), value.data() + ft::nr_elements_2d, 0);
        f::MaskedScalar<int> result_we_want{std::accumulate(
            value.data(), value.data() + ft::nr_elements_2d, 0)};
        verify_2d_0d(execution_policy, value, result_we_want);
    }

    // Mask a value.
    {
        f::MaskedArray<int, 2> value(f::extents[ft::nr_rows][ft::nr_cols]);
        std::iota(value.data(), value.data() + ft::nr_elements_2d, 0);
        get(value.mask(), 5) = true;
        f::MaskedScalar<int> result_we_want{std::accumulate(
            value.data(), value.data() + ft::nr_elements_2d, 0)};
        result_we_want -= get(value, 5);
        verify_2d_0d_masked(execution_policy, value, result_we_want);
    }

    // Mask all values.
    {
        f::MaskedArray<int, 2> value(f::extents[ft::nr_rows][ft::nr_cols]);
        value.mask().fill(true);
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_2d_0d_masked(execution_policy, value, result_we_want);
    }

    // Out of range, within a block.
    {
        f::MaskedArray<int, 2> value(f::extents[ft::nr_rows][ft::nr_cols], 0);
        get(value, 0) = std::numeric_limits<int>::max();
        get(value, 1) = 1;
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_2d_0d_masked(execution_policy, value, result_we_want);
    }

    // Out of range, when aggregating block results.
    {
        f::MaskedArray<int, 2> value(f::extents[ft::nr_rows][ft::nr_cols], 0);
        value.fill(0);
        get(value, 0) = std::numeric_limits<int>::max();
        get(value, ft::nr_elements_2d - 1) = 1;
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_2d_0d_masked(execution_policy, value, result_we_want);
    }

    // Empty.
    {
        f::MaskedArray<int, 2> value(f::extents[0][0]);
        f::MaskedScalar<int> result_we_want{-9, true};
        verify_2d_0d_masked(execution_policy, value, result_we_want);
    }
}

FERN_UNARY_AGGREGATE_TEST_CASES()


#ifndef STAN_MATH_PRIM_MAT_FUN_MULTIPLY_HPP
#define STAN_MATH_PRIM_MAT_FUN_MULTIPLY_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/arr/err/check_matching_sizes.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/utility/enable_if.hpp>

namespace stan {
  namespace math {

    /**
     * Return specified matrix multiplied by specified scalar.
     * @tparam R Row type for matrix.
     * @tparam C Column type for matrix.
     * @param m Matrix.
     * @param c Scalar.
     * @return Product of matrix and scalar.
     */
    template <int R, int C, typename T>
    inline
    typename boost::enable_if_c<boost::is_arithmetic<T>::value,
                                Eigen::Matrix<double, R, C> >::type
    multiply(const Eigen::Matrix<double, R, C>& m,
             T c) {
      return c * m;
    }

    /**
     * Return specified scalar multiplied by specified matrix.
     * @tparam R Row type for matrix.
     * @tparam C Column type for matrix.
     * @param c Scalar.
     * @param m Matrix.
     * @return Product of scalar and matrix.
     */
    template <int R, int C, typename T>
    inline
    typename boost::enable_if_c<boost::is_arithmetic<T>::value,
                                Eigen::Matrix<double, R, C> >::type
    multiply(T c,
             const Eigen::Matrix<double, R, C>& m) {
      return c * m;
    }

    /**
     * Return the product of the specified matrices.  The number of
     * columns in the first matrix must be the same as the number of rows
     * in the second matrix.
     * @param m1 First matrix.
     * @param m2 Second matrix.
     * @return The product of the first and second matrices.
     * @throw std::domain_error if the number of columns of m1 does not match
     *   the number of rows of m2.
     */
    template<int R1, int C1, int R2, int C2>
    inline Eigen::Matrix<double, R1, C2>
    multiply(const Eigen::Matrix<double, R1, C1>& m1,
             const Eigen::Matrix<double, R2, C2>& m2) {
      check_multiplicable("multiply",
                          "m1", m1,
                          "m2", m2);
      return m1*m2;
    }

    /**
     * Return the scalar product of the specified row vector and
     * specified column vector.  The return is the same as the dot
     * product.  The two vectors must be the same size.
     * @param rv Row vector.
     * @param v Column vector.
     * @return Scalar result of multiplying row vector by column vector.
     * @throw std::domain_error if rv and v are not the same size.
     */
    template<int C1, int R2>
    inline double multiply(const Eigen::Matrix<double, 1, C1>& rv,
                           const Eigen::Matrix<double, R2, 1>& v) {
      check_matching_sizes("multiply",
                           "rv", rv,
                           "v", v);
      return rv.dot(v);
    }

  }
}
#endif


//
// Created by dominik on 29.06.21.
//
#include "types.hpp"
#include "utils.hpp"
#include "structure_utils.hpp"
#include <cmath>
#include <cassert>
#include <stdexcept>
#include <fstream>
#include <filesystem>
#include <boost/multi_array.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <gtest/gtest.h>

using namespace boost;
using namespace sqsgenerator::utils;
using namespace boost::numeric::ublas;
namespace fs = std::filesystem;

namespace sqsgenerator::test {

    template <typename T> T convert_to (const std::string &str)
    {
        std::istringstream ss(str);
        T num;
        ss >> num;
        return num;
    }
/**
 * \brief   Return the filenames of all files that have the specified extension
 *          in the specified directory and all subdirectories.
 */
    std::vector<fs::path> get_all(std::string const &root, std::string const &ext) {
        std::vector<fs::path> paths;
        for (auto &p : fs::recursive_directory_iterator(root))
        {
            if (p.path().extension() == ext) paths.emplace_back(p.path());
        }
        return paths;
    }

    std::vector<std::string> split (std::string s, std::string delimiter) {
        // Taken from https://stackoverflow.com/questions/14265581/parse-split-a-string-in-c-using-string-delimiter-standard-c
        size_t pos_start = 0, pos_end, delim_len = delimiter.length();
        std::string token;
        std::vector<std::string> res;

        while ((pos_end = s.find (delimiter, pos_start)) != std::string::npos) {
            token = s.substr (pos_start, pos_end - pos_start);
            pos_start = pos_end + delim_len;
            res.push_back (token);
        }

        res.push_back (s.substr (pos_start));
        return res;
    }

    template<typename T, size_t NDims>
    multi_array<T, NDims> read_array(std::ifstream &fhandle, std::string const &name) {
        std::string line;
        std::string start_line = name + "::array::begin";
        while (std::getline(fhandle, line)) {
            if (line == start_line) { break; }
        }
        assert(line == start_line);
        // Read dimensions
        std::getline(fhandle, line);
        auto crumbs  = split(line, " ");
        assert(crumbs.size() == 2);
        assert(crumbs[0] == name + "::array::ndims");
        size_t ndims {std::stoul(crumbs[1])};
        assert(ndims == NDims);

        // Read shape
        std::getline(fhandle, line);
        crumbs  = split(line, " ");
        assert(crumbs.size() == ndims+1);
        assert(crumbs[0] == name + "::array::shape");
        std::vector<size_t> shape;
        for(auto it = crumbs.begin() + 1; it != crumbs.end(); ++it) shape.push_back(std::stoul(*it));
        assert(shape.size() == ndims);
        size_t num_elements = std::accumulate(shape.begin(), shape.end(), 1, std::multiplies<size_t>());

        std::getline(fhandle, line);
        crumbs  = split(line, " ");
        assert(crumbs.size() == num_elements+1);
        assert(crumbs[0] == name + "::array::data");
        std::vector<T> data;
        for(auto it = crumbs.begin() + 1; it != crumbs.end(); ++it) data.push_back(convert_to<T>(*it));
        assert(data.size() == num_elements);

        multi_array<T, NDims> result;
        auto& shape_array = reinterpret_cast<boost::array<size_t, NDims> const&>(*shape.data());
        result.resize(shape_array);
        result.assign(data.begin(), data.end());
        std::getline(fhandle, line);
        assert(line == name + "::array::end");
        return result;
    }

    struct TestCaseData {
    public:
        array_2d_t lattice;
        array_2d_t fcoords;
        array_2d_t distances;
        array_3d_t vecs;
        pair_shell_matrix_t shells;
    };

    TestCaseData read_test_data(std::string const &path) {
        std::ifstream fhandle(path);
        std::string line;
        auto lattice = read_array<double, 2>(fhandle, "lattice");
        auto fcoords = read_array<double, 2>(fhandle, "fcoords");
        auto d2 = read_array<double, 2>(fhandle, "distances");
        auto shells = read_array<shell_t, 2>(fhandle, "shells");
        auto vecs = read_array<double, 3>(fhandle, "vecs");

        fhandle.close();
        return TestCaseData {lattice, fcoords, d2, vecs, shells};
    }

    class StructureUtilsTestFixture : public ::testing::Test {
    protected:
        std::vector<TestCaseData> test_cases{};

    public:
        void SetUp() {
            // code here will execute just before the test ensues
            // std::cout << "StructureUtilsTestFixture::SetUp(): " << std::filesystem::current_path() << std::endl;
            get_all("resources", ".data");
            for (auto &p : get_all("resources", ".data")) {
                // std::cout << "StructureUtilsTestFixture::SetUp(): Found test case: " << p << std::endl;
                test_cases.emplace_back(read_test_data(p));
            }
        };

        void TearDown() {
            // code here will be called just after the test completes
            // ok to through exceptions from here if need be
        }

    };

    template<typename MultiArrayA>
    void assert_multi_array_equal(const MultiArrayA &a, const MultiArrayA &b) {
        typedef typename MultiArrayA::element T;

        ASSERT_EQ(a.num_elements(), b.num_elements());
        for (size_t i = 0; i < a.num_elements(); ++i) {
            ASSERT_NEAR(std::abs<T>(a.data()[i]), std::abs<T>(b.data()[i]), 1.0e-5);
            //EXPECT_NEAR(a.data()[i], b.data()[i], 1.0e-5);
        }
    }

    template<>
    void assert_multi_array_equal<pair_shell_matrix_t>(const pair_shell_matrix_t &a, const pair_shell_matrix_t &b) {
        ASSERT_EQ(a.num_elements(), b.num_elements());
        for (size_t i = 0; i < a.num_elements(); ++i) {
            //ASSERT_NEAR(std::abs<int>(a.data()[i]), std::abs<int>(b.data()[i]), 1.0e-5);
            EXPECT_NEAR(std::abs<int>(a.data()[i]), std::abs<int>(b.data()[i]), 1.0e-5);
            //EXPECT_NEAR(a.data()[i], b.data()[i], 1.0e-5);
        }
    }

    TEST_F(StructureUtilsTestFixture, TestPbcVectors) {
        for (TestCaseData &test_case : test_cases) {
            matrix<double> lattice (matrix_from_multi_array(test_case.lattice));
            matrix<double> fcoords (matrix_from_multi_array(test_case.fcoords));
            auto pbc_vecs = sqsgenerator::utils::pbc_shortest_vectors(lattice, fcoords, true);
            for (size_t i = 0; i < 3; i++) ASSERT_EQ(pbc_vecs.shape()[i], test_case.vecs.shape()[i]);
            assert_multi_array_equal(pbc_vecs, test_case.vecs);
        }
    }

    TEST_F(StructureUtilsTestFixture, TestDistanceMatrix) {
        typedef multi_array<double, 3> pbc_mat;
        for (TestCaseData &test_case : test_cases) {
            matrix<double> lattice (matrix_from_multi_array(test_case.lattice));
            matrix<double> fcoords (matrix_from_multi_array(test_case.fcoords));
            auto pbc_vecs = sqsgenerator::utils::pbc_shortest_vectors(lattice, fcoords, true);
            auto d2 = sqsgenerator::utils::distance_matrix(pbc_vecs);
            for (size_t i = 0; i < 2; i++)  ASSERT_EQ(d2.shape()[i], test_case.vecs.shape()[i]);
            assert_multi_array_equal(d2, test_case.distances);
            auto d2_external = sqsgenerator::utils::distance_matrix(test_case.vecs);
            assert_multi_array_equal(d2, d2_external);
        }
    }

    TEST_F(StructureUtilsTestFixture, TestShellMatrix) {
        for (TestCaseData &test_case : test_cases) {
            matrix<double> lattice (matrix_from_multi_array(test_case.lattice));
            matrix<double> fcoords (matrix_from_multi_array(test_case.fcoords));
            auto pbc_vecs = sqsgenerator::utils::pbc_shortest_vectors(lattice, fcoords, true);
            auto d2 = sqsgenerator::utils::distance_matrix(pbc_vecs);
            auto distances = sqsgenerator::utils::default_shell_distances(d2, 1.0e-3);
            auto shells = sqsgenerator::utils::shell_matrix(d2, distances, 1.0e-3);
            auto natoms {static_cast<index_t>(fcoords.size1())};
            for (size_t i = 0; i < 2; i++)  ASSERT_EQ(shells.shape()[i], test_case.shells.shape()[i]);
            assert_multi_array_equal(shells, test_case.shells);
            /*for (index_t i = 0; i < natoms; i++) {
                for (index_t j = i+1; j < natoms; j++) {
                    if (shells[i][j] != test_case.shells[i][j]) {
                        std::cout << "Different shell (" << i << ", " << j << ") = (" << static_cast<int>(shells[i][j]) << " != " << static_cast<int>(test_case.shells[i][j]) << ") = (" << d2[i][j] << ", " << distances[shells[i][j]] << ")" << std::endl;
                    } else {
                        ASSERT_EQ(shells[i][j], test_case.shells[i][j]);
                    }
                }
            }*/
            auto shells_external = sqsgenerator::utils::shell_matrix(test_case.distances, distances);
            assert_multi_array_equal(shells, shells_external);
            std::cout << format_vector(distances);
            // Make sure the main diagonal is zero
            for (index_t i = 0; i < natoms; i++) {
                ASSERT_EQ(shells[i][i], 0);
            }
            // Ensure the matrix is symmetric
            for (index_t i = 0; i < natoms; i++) {
                for (index_t j = i+1; j < natoms; j++)
                    ASSERT_EQ(shells[i][j], shells[j][i]);
            }
        }
    }

    TEST_F(StructureUtilsTestFixture, TestCreatePairListSizes) {
        for (TestCaseData &test_case : test_cases) {
            matrix<double> lattice (matrix_from_multi_array(test_case.lattice));
            matrix<double> fcoords (matrix_from_multi_array(test_case.fcoords));
            std::map<shell_t, size_t> counts;
            std::map<shell_t, double> all_weights;
            auto natoms {static_cast<index_t>(fcoords.size1())};
            auto pbc_vecs = sqsgenerator::utils::pbc_shortest_vectors(lattice, fcoords, true);
            auto d2 = sqsgenerator::utils::distance_matrix(pbc_vecs);
            auto distances = sqsgenerator::utils::default_shell_distances(d2);
            pair_shell_matrix_t shells = sqsgenerator::utils::shell_matrix(d2, distances);
            auto max_shell = static_cast<index_t>(*std::max_element(shells.origin(), shells.origin()+shells.num_elements()));
            for (auto i = 1; i <= max_shell; i++) {
                counts.insert(std::make_pair(i, 0));
                all_weights.insert(std::make_pair(i, 0.0));
            }
            // If all shells are present, the list length must be the number of pairs in the structure
            ASSERT_EQ(create_pair_list(shells, all_weights).size(), natoms*(natoms-1)/2);
            for (index_t i = 0; i < natoms; i++) {
                for (index_t j = i+1; j < natoms; j++) {
                    counts[shells[i][j]]++;
                }
            }
            // If theres only one shell it must be exactly the number of pairs in the corresponding shell
            for (auto i = 1; i <= max_shell; i++) {
                auto num_pairs = create_pair_list(shells, {{i, 0.0}}).size();
                auto should_be = counts[i];
                ASSERT_EQ(num_pairs, should_be);
            }
            // Same must be true for any combination of two shells
            for (auto i = 1; i <= max_shell; i++) {
                for (auto j = i+1; j <= max_shell; j++) {
                    auto num_pairs = create_pair_list(shells, {{i, 0.0}, {j, 0.0}}).size();
                    auto should_be = counts[i]+counts[j];
                    ASSERT_EQ(num_pairs, should_be);
                }
            }
        }
    }
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#ifndef STAN_MATH_PRIM_PROB_STD_NORMAL_RNG_HPP
#define STAN_MATH_PRIM_PROB_STD_NORMAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a standard Normal random variate using the specified
 * random number generator.
 *
 * @tparam RNG type of random number generator
 * @param rng random number generator
 * @return A standard normal random variate
 */
template <class RNG>
inline double std_normal_rng(RNG& rng) {
  using boost::normal_distribution;
  using boost::variate_generator;
  static const char* function = "std_normal_rng";

  variate_generator<RNG&, normal_distribution<>> norm_rng(
      rng, normal_distribution<>(0, 1));

  return norm_rng();
}

}  // namespace math
}  // namespace stan
#endif


#include "delsum-poly/include/poly.hh"
#include "delsum-poly/src/lib.rs.h"
#include <cstdint>
#include <climits>
#include <NTL/GF2X.h>
#include <NTL/GF2E.h>
#include <NTL/GF2XFactoring.h>
namespace poly
{
    long i64_to_l(int64_t x) {
        if (x > LONG_MAX || x < LONG_MIN) {
            std::cerr << "Polynomial too big, probably because of big input files" << std::endl
                      << "Try using smaller files or run on a platform with larger long variables" << std::endl;
            abort();
        }
        return (long)x;
    }
    Poly::Poly()
    {
        int_pol = NTL::GF2X();
    }
    Poly::Poly(const Poly &b)
    {
        int_pol = NTL::GF2X(b.int_pol);
    }
    Poly::Poly(Poly &&b)
    {
        int_pol = NTL::GF2X(b.int_pol);
    }
    bool Poly::coeff(int64_t idx) const
    {
        return NTL::IsOne(NTL::coeff(int_pol, i64_to_l(idx)));
    }
    bool Poly::eq(const Poly &b) const
    {
        return int_pol == b.int_pol;
    }
    int64_t deg(const Poly &a)
    {
        return NTL::deg(a.int_pol);
    }
    std::unique_ptr<Poly> copy_poly(const Poly &p)
    {
        return std::make_unique<Poly>(p);
    }
    std::unique_ptr<std::vector<PolyI64Pair>> factor(const Poly &p, int64_t verbosity)
    {
        auto v = std::vector<PolyI64Pair>();
        auto decomp = NTL::CanZass(p.int_pol, i64_to_l(verbosity));
        for (auto x : decomp)
        {
            auto poly = Poly();
            poly.int_pol = x.a;
            v.push_back(PolyI64Pair{.poly = std::make_unique<Poly>(std::move(poly)), .l = (int64_t)x.b});
        }
        return std::make_unique<std::vector<PolyI64Pair>>(std::move(v));
    }

    std::unique_ptr<Poly> new_poly_shifted(rust::Slice<uint8_t> bytes, int64_t shift, bool msb_first)
    {
        auto lshift = i64_to_l(shift);
        auto ret = Poly();
        ret.int_pol = NTL::GF2X();
        ret.int_pol.SetLength(lshift + bytes.length() * 8);
        for (size_t i = 0; i < bytes.length(); i++)
        {
            auto current_byte = bytes.data()[bytes.length() - 1 - i];
            for (int j = 0; j < 8; j++)
            {
                auto bit_pos = msb_first ? j : (7 - j);
                auto current_bit = (current_byte >> bit_pos) & 1;
                auto bit_index = lshift + 8 * i + j;
                ret.int_pol[bit_index] = current_bit;
            }
        }
        ret.int_pol.normalize();
        return std::make_unique<Poly>(ret);
    }

    std::unique_ptr<Poly> new_poly(rust::Slice<uint8_t> bytes)
    {
        return new_poly_shifted(bytes, 0, true);
    }
    std::unique_ptr<Poly> new_zero()
    {
        auto ret = Poly();
        return std::make_unique<Poly>(ret);
    }

    std::unique_ptr<std::vector<uint8_t>> Poly::to_bytes(int64_t min_bytes) const
    {
        auto d = NTL::deg(int_pol);
        auto n_bytes = d / 8 + 1;
        if (d < 0) {
            n_bytes = 0;
        }
        auto v = std::vector<uint8_t>();
        auto lmin = i64_to_l(min_bytes);
        auto amount_of_bytes = n_bytes > lmin ? n_bytes : lmin;
        v.reserve(amount_of_bytes);
        for (long i = 0; i < amount_of_bytes - n_bytes; i++) {
            v.push_back(0);
        }
        uint8_t current_byte = 0;
        for (long i = d; i >= 0; i--)
        {
            current_byte <<= 1;
            current_byte |= (uint8_t)NTL::rep(int_pol[i]);
            if (i % 8 == 0)
            {
                v.push_back(current_byte);
                current_byte = 0;
            }
        }
        return std::make_unique<std::vector<uint8_t>>(v);
    }
    std::unique_ptr<Poly> add(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        ret.int_pol = b.int_pol + c.int_pol;
        return std::make_unique<Poly>(ret);
    }
    void Poly::add_to(const Poly &b)
    {
        int_pol += b.int_pol;
    }
    std::unique_ptr<Poly> mul(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        ret.int_pol = b.int_pol * c.int_pol;
        return std::make_unique<Poly>(ret);
    }
    void Poly::mul_to(const Poly &b)
    {
        int_pol *= b.int_pol;
    }
    std::unique_ptr<Poly> div(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        ret.int_pol = b.int_pol / c.int_pol;
        return std::make_unique<Poly>(ret);
    }
    void Poly::div_to(const Poly &b)
    {
        int_pol /= b.int_pol;
    }
    bool Poly::div_to_checked(const Poly &b)
    {
        auto ret = NTL::divide(int_pol, b.int_pol);
        return ret == 1;
    }
    bool Poly::is_zero() const
    {
        return NTL::IsZero(int_pol);
    }
    std::unique_ptr<Poly> gcd(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        NTL::GCD(ret.int_pol, b.int_pol, c.int_pol);
        return std::make_unique<Poly>(ret);
    }
    std::unique_ptr<Poly> xgcd(Poly &x, Poly &y, const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        NTL::XGCD(ret.int_pol, x.int_pol, y.int_pol, b.int_pol, c.int_pol);
        return std::make_unique<Poly>(ret);
    }
    void Poly::gcd_to(const Poly &b)
    {
        NTL::GCD(int_pol, int_pol, b.int_pol);
    }
    std::unique_ptr<Poly> rem(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        NTL::rem(ret.int_pol, b.int_pol, c.int_pol);
        return std::make_unique<Poly>(ret);
    }
    void Poly::rem_to(const Poly &b)
    {
        NTL::rem(int_pol, int_pol, b.int_pol);
    }
    std::unique_ptr<Poly> power(const Poly &p, int64_t n)
    {
        auto q = Poly();
        q.int_pol = NTL::power(p.int_pol, i64_to_l(n));
        return std::make_unique<Poly>(q);
    }
    std::unique_ptr<Poly> shift(const Poly &p, int64_t n)
    {
        auto q = Poly();
        if (n >= 0) {
            q.int_pol = NTL::LeftShift(p.int_pol, i64_to_l(n));
        } else {
            q.int_pol = NTL::RightShift(p.int_pol, i64_to_l(-n));
        }
        return std::make_unique<Poly>(q);
    }
    void Poly::sqr()
    {
        int_pol = NTL::sqr(int_pol);
    }

    // PolyRem stuff

    PolyRem::PolyRem(const Poly &p)
    {
        int_pol = NTL::GF2E();
        int_pol.init(p.int_pol);
    }
    // copy
    PolyRem::PolyRem(const PolyRem &b)
    {
        int_pol = NTL::GF2E(b.int_pol);
    }
    // move
    PolyRem::PolyRem(PolyRem &&b)
    {
        int_pol = NTL::GF2E(b.int_pol);
    }
    PolyRem::PolyRem(const NTL::GF2E &p)
    {
        int_pol = NTL::GF2E(p);
    }

    std::unique_ptr<PolyRem> new_polyrem(const Poly &rem, const Poly &m)
    {
        auto ret = PolyRem(m);
        NTL::conv(ret.int_pol, rem.int_pol);
        return std::make_unique<PolyRem>(ret);
    }
    void PolyRem::add_to(const PolyRem &b)
    {
        int_pol += b.int_pol;
    }
    void PolyRem::mul_to(const PolyRem &b)
    {
        int_pol *= b.int_pol;
    }
    void PolyRem::div_to(const PolyRem &b)
    {
        int_pol /= b.int_pol;
    }
    void PolyRem::sqr()
    {
        int_pol = NTL::sqr(int_pol);
    }
    std::unique_ptr<Poly> PolyRem::rep() const
    {
        auto ret = Poly();
        ret.int_pol = NTL::rep(int_pol);
        return std::make_unique<Poly>(ret);
    }
    std::unique_ptr<PolyRem> powermod(const PolyRem &p, int64_t n)
    {
        auto q = PolyRem(NTL::power(p.int_pol, i64_to_l(n)));
        return std::make_unique<PolyRem>(q);
    }
    std::unique_ptr<PolyRem> copy_polyrem(const PolyRem &p)
    {
        return std::make_unique<PolyRem>(PolyRem(p));
    }
} // namespace poly