"#include "pch.h" #define XOR_RAND(state, result_...

"#include "pch.h"

#define XOR_RAND(state, result_var)
do {
unsigned s = (state);
s ^= s << 13;
s ^= s >> 17;
s ^= s << 5;
(state) = s;
float tmp = (float)((double)(s) * (1.0 / 4294967296.0));
result_var = tmp;
} while (0)

#define XOR_RAND_GRSH(state, result_var)
do {
unsigned s = (state);
s ^= s << 13;
s ^= s >> 17;
s ^= s << 5;
(state) = s;
result_var = fmaf((float)(int)s, 0x1.0p-31f, -1.0f);
} while (0)

#define FABE13_COS(x, result_var)
do {
const float ax = fabsf(x);
float r = fmodf(ax, 6.28318530718f);
if (r > 3.14159265359f)
r = 6.28318530718f - r;
if (r < 1.57079632679f) {
const float t2 = r * r;
const float t4 = t2 * t2;
result_var = fmaf(t4,
fmaf(t2, -0.0013888889f, 0.0416666667f),
fmaf(t2, -0.5f, 1.0f));
} else {
r = 3.14159265359f - r;
const float t2 = r * r;
const float t4 = t2 * t2;
result_var = -fmaf(t4,
fmaf(t2, -0.0013888889f, 0.0416666667f),
fmaf(t2, -0.5f, 1.0f));
}
} while (0)

#define FABE13_SIN(x, result_var)
do {
const float x = (x);
const float ax = fabsf(x);
float r = fmodf(ax, 6.28318530718f);
bool sfl = r > 3.14159265359f;
if (sfl)
r = 6.28318530718f - r;
bool cfl = r > 1.57079632679f;
if (cfl)
r = 3.14159265359f - r;
const float t2 = r * r;
float _s = fmaf(t2,
fmaf(t2,
fmaf(t2, -0.0001984127f, 0.0083333333f),
-0.16666666f),
1.0f) *
r;
result_var = ((x < 0.0f) ^ sfl) ? -_s : _s;
} while (0)

#define FABE13_SINCOS(in, sin_out, cos_out, n)
do {
int i = 0;
const int limit = (n) & ~7;
if ((n) >= 8) {
static __declspec(align(16)) const __m256 VEC_TWOPI =
_mm256_set1_ps(6.28318530718f);
static __declspec(align(16)) const __m256 VEC_PI =
_mm256_set1_ps(3.14159265359f);
static __declspec(align(16)) const __m256 VEC_PI_2 =
_mm256_set1_ps(1.57079632679f);
static __declspec(align(16)) const __m256 INV_TWOPI =
_mm256_set1_ps(0.15915494309189535f);
static __declspec(align(16)) const __m256 BIAS =
_mm256_set1_ps(12582912.0f);
static __declspec(align(16)) const __m256 VEC_COS_P5 =
_mm256_set1_ps(-0.0013888889f);
static __declspec(align(16)) const __m256 VEC_COS_P3 =
_mm256_set1_ps(0.0416666667f);
static __declspec(align(16)) const __m256 VEC_COS_P1 =
_mm256_set1_ps(-0.5f);
static __declspec(align(16)) const __m256 VEC_COS_P0 =
_mm256_set1_ps(1.0f);
static __declspec(align(16)) const __m256 VEC_SIN_P5 =
_mm256_set1_ps(-0.0001984127f);
static __declspec(align(16)) const __m256 VEC_SIN_P3 =
_mm256_set1_ps(0.0083333333f);
static __declspec(align(16)) const __m256 VEC_SIN_P1 =
_mm256_set1_ps(-0.16666666f);
static __declspec(align(16)) const __m256 VEC_SIN_P0 =
_mm256_set1_ps(1.0f);
static __declspec(align(16)) const __m256 VEC_ZERO =
_mm256_setzero_ps();
while (i < limit) {
const __m256 vx = _mm256_load_ps(&(in)[i]);
const __m256 vax =
_mm256_andnot_ps(_mm256_set1_ps(-0.0f), vx);
__m256 q = _mm256_fmadd_ps(vax, INV_TWOPI, BIAS);
q = _mm256_sub_ps(q, BIAS);
const __m256 r = _mm256_fnmadd_ps(VEC_TWOPI, q, vax);
const __m256 r1 =
_mm256_min_ps(r, _mm256_sub_ps(VEC_TWOPI, r));
const __m256 r2 =
_mm256_min_ps(r1, _mm256_sub_ps(VEC_PI, r1));
const __m256 t2 = _mm256_mul_ps(r2, r2);
const __m256 cosv =
_mm256_fmadd_ps(t2,
_mm256_fmadd_ps(
t2,
_mm256_fmadd_ps(t2, VEC_COS_P5,
VEC_COS_P3),
VEC_COS_P1),
VEC_COS_P0);
const __m256 sinv =
_mm256_mul_ps(
_mm256_fmadd_ps(
t2,
_mm256_fmadd_ps(t2,
_mm256_fmadd_ps(t2, VEC_SIN_P5,
VEC_SIN_P3),
VEC_SIN_P1),
VEC_SIN_P0),
r2);
const __m256 cflip =
_mm256_cmp_ps(r1, VEC_PI_2, _CMP_GT_OQ);
const __m256 sflip =
_mm256_xor_ps(_mm256_cmp_ps(vx, VEC_ZERO, _CMP_LT_OQ),
_mm256_cmp_ps(r, VEC_PI, _CMP_GT_OQ));
_mm256_store_ps(&(cos_out)[i],
_mm256_blendv_ps(cosv,
_mm256_sub_ps(VEC_ZERO, cosv),
cflip));
_mm256_store_ps(&(sin_out)[i],
_mm256_blendv_ps(sinv,
_mm256_sub_ps(VEC_ZERO, sinv),
sflip));
i += 8;
}
}
while (i < (n)) {
const float x = (in)[i];
const float ax = fabsf(x);
float q = fmaf(ax, 0.15915494309189535f, 12582912.0f);
q -= 12582912.0f;
float r = fmaf(-6.28318530718f, q, ax);
const bool sflip = r > 3.14159265359f;
if (sflip)
r = 6.28318530718f - r;
const bool cflip = r > 1.57079632679f;
if (cflip)
r = 3.14159265359f - r;
const float t2 = r * r;
const float c =
fmaf(t2,
fmaf(t2, fmaf(t2, -0.0013888889f, 0.0416666667f),
-0.5f),
1.0f);
const float s =
fmaf(t2,
fmaf(t2, fmaf(t2, -0.0001984127f, 0.0083333333f),
-0.16666666f),
1.0f) *
r;
(cos_out)[i] = cflip ? -c : c;
(sin_out)[i] = ((x < 0.0f) ^ sflip) ? -s : s;
++i;
}
} while (0)

enum List : unsigned {
Top = 0b00u,
Down = 0b01u,
Left = 0b10u,
Right = 0b11u
};

__declspec(align(16)) struct Step final {
const unsigned next;
const unsigned dx;
const unsigned dy;

text
__forceinline Step(unsigned n, unsigned x, unsigned y) noexcept
	: next(n), dx(x), dy(y) {
}

};

__declspec(align(16)) struct InvStep final {
const unsigned q;
const unsigned next;

text
__forceinline InvStep(unsigned q_val, unsigned n) noexcept
	: q(q_val), next(n) {
}

};

__declspec(align(16)) static const Step g_step_tbl[4][4] = {
{Step(Right, 0u, 0u),
Step(Top, 0u, 1u),
Step(Top, 1u, 1u),
Step(Left, 1u, 0u)},
{Step(Left, 1u, 1u),
Step(Down, 1u, 0u),
Step(Down, 0u, 0u),
Step(Right, 0u, 1u)},
{Step(Down, 1u, 1u),
Step(Left, 0u, 1u),
Step(Left, 0u, 0u),
Step(Top, 1u, 0u)},
{Step(Top, 0u, 0u),
Step(Right, 1u, 0u),
Step(Right, 1u, 1u),
Step(Down, 0u, 1u)} };

__declspec(align(16)) static const InvStep g_inv_tbl[4][4] = {
{InvStep(0u, Right),
InvStep(1u, Top),
InvStep(3u, Left),
InvStep(2u, Top)},
{InvStep(2u, Down),
InvStep(3u, Right),
InvStep(1u, Down),
InvStep(0u, Left)},
{InvStep(2u, Left),
InvStep(1u, Left),
InvStep(3u, Top),
InvStep(0u, Down)},
{InvStep(0u, Top),
InvStep(3u, Down),
InvStep(1u, Right),
InvStep(2u, Right)} };

static const boost::mpi::environment* g_env;
static const boost::mpi::communicator* g_world;
static const pybind11::scoped_interpreter* g_pyInterpreter;
static const pybind11::module_* g_pyOptimizerBridge;
static std::string g_exeDirCache;

__declspec(align(16)) struct CrossMsg final {
float s_x1;
float s_x2;
float e_x1;
float e_x2;
float Rtop;

text
template <typename Archive>
__declspec(noalias) __forceinline void serialize(Archive& ar,
	unsigned int) noexcept {
	ar& s_x1& s_x2& e_x1& e_x2& Rtop;
}

};

__declspec(align(16)) struct MultiCrossMsg final {
float intervals[35];
unsigned count;

text
template <typename Archive>
__declspec(noalias) __forceinline void serialize(Archive& ar,
	unsigned int) noexcept {
	ar& intervals& count;
}

};

__declspec(align(16)) struct BestSolutionMsg final {
float bestF;
float bestX;
float bestY;
float bestQ[32];
unsigned dim;

text
template <typename Archive>
__declspec(noalias) __forceinline void serialize(Archive& ar,
	unsigned int) noexcept {
	ar& bestF& bestX& bestY& bestQ& dim;
}

};

__declspec(align(16)) struct Slab final sealed{
char* const base;
char* current;
char* const end;

text
__forceinline Slab(void* memory, size_t usable) noexcept
		: base((char*)memory),
			current(base),
			end(base + (usable & ~(size_t)63u)) {
}

};

static thread_local tbb::enumerable_thread_specific<Slab*> tls( noexcept {
void* memory = _aligned_malloc(16777216u, 16u);
Slab* slab = (Slab*)_aligned_malloc(sizeof(Slab), 16u);
new (slab) Slab(memory, 16777216u);
char* p = slab->base;
#pragma loop ivdep
while (p < slab->end) {
*p = 0;
p += 4096u;
}
return slab;
});

__declspec(align(16)) struct Peano2DMap final sealed{
const int levels;
const float a;
const float b;
const float c;
const float d;
const float lenx;
const float leny;
const float inv_lenx;
const unsigned scale;
const unsigned start;

text
__forceinline Peano2DMap(int L,
												 float _a,
												 float _b,
												 float _c,
												 float _d,
												 unsigned st) noexcept
		: levels(L),
			a(_a),
			b(_b),
			c(_c),
			d(_d),
			lenx(_b - _a),
			leny(_d - _c),
			inv_lenx(1.0f / (_b - _a)),
			scale((unsigned)1u << (L << 1)),
			start(st) {
}

};

static Peano2DMap gActiveMap(0, 0, 0, 0, 0, 0);

__declspec(align(16)) struct Interval1D final sealed{
const float x1;
const float x2;
const float y1;
const float y2;
const float delta_y;
const float ordinate_factor;
const float N_factor;
const float quadratic_term;
const float M;
float R;

text
static __declspec(noalias) __forceinline void* operator new(size_t) noexcept {
	Slab* s = tls.local();
	char* r = s->current;
	s->current += 64u;
	return r;
}

__declspec(noalias) __forceinline Interval1D(float _x1,
																						 float _x2,
																						 float _y1,
																						 float _y2,
																						 float _N) noexcept
		: x1(_x1),
			x2(_x2),
			y1(_y1),
			y2(_y2),
			delta_y(_y2 - _y1),
			ordinate_factor(-(y1 + y2) * 2.0f),
			N_factor(_N == 1.0f ? _x2 - _x1 : sqrtf(_x2 - _x1)),
			quadratic_term(fmaf((1.0f / N_factor)* delta_y, delta_y, 0.0f)),
			M(fabsf(delta_y) / N_factor) {
}

__declspec(noalias) __forceinline void ChangeCharacteristic(float _m) noexcept {
	R = fmaf(1.0f / _m, quadratic_term, fmaf(_m, N_factor, ordinate_factor));
}

};

static __declspec(noalias) __forceinline bool ComparePtr1D(const Interval1D* a,
const Interval1D* b) noexcept {
return a->R < b->R;
}

static __declspec(noalias) __forceinline void RecomputeR_ConstM_AVX2_1D(
Interval1D* const* arr,
size_t n,
float m) noexcept {
const __m256 vm = _mm256_set1_ps(m);
__m256 vinvm = _mm256_rcp_ps(vm);
vinvm =
_mm256_mul_ps(vinvm, _mm256_fnmadd_ps(vm, vinvm, _mm256_set1_ps(2.0f)));

text
size_t i = 0;
size_t limit = n & ~7ull;

alignas(16) float q[8];
alignas(16) float nf[8];
alignas(16) float od[8];
alignas(16) float out[8];

#pragma loop ivdep
while (i < limit) {
int k = 0;
#pragma loop ivdep
while (k < 8) {
const Interval1D* p = arr[i + k];
q[k] = p->quadratic_term;
nf[k] = p->N_factor;
od[k] = p->ordinate_factor;
++k;
}

text
	const __m256 vq = _mm256_load_ps(q);
	const __m256 vnf = _mm256_load_ps(nf);
	const __m256 vod = _mm256_load_ps(od);

	const __m256 t = _mm256_fmadd_ps(vm, vnf, vod);
	const __m256 res = _mm256_fmadd_ps(vq, vinvm, t);
	_mm256_store_ps(out, res);

	k = 0;

#pragma loop ivdep
while (k < 8) {
arr[i + k]->R = out[k];
++k;
}

text
	i += 8;
}

while (i < n) {
	arr[i]->ChangeCharacteristic(m);
	++i;
}

}

static __declspec(noalias) __forceinline void RecomputeR_AffineM_AVX2_1D(
Interval1D* const* arr,
size_t n,
float GF,
float alpha) noexcept {
const __m256 vGF = _mm256_set1_ps(GF);
const __m256 va = _mm256_set1_ps(alpha);

text
size_t i = 0;
size_t limit = n & ~7ull;

alignas(16) float ln[8];
alignas(16) float Mv[8];
alignas(16) float q[8];
alignas(16) float nf[8];
alignas(16) float od[8];
alignas(16) float out[8];

#pragma loop ivdep
while (i < limit) {
int k = 0;
#pragma loop ivdep
while (k < 8) {
const Interval1D* p = arr[i + k];
ln[k] = p->x2 - p->x1;
Mv[k] = p->M;
q[k] = p->quadratic_term;
nf[k] = p->N_factor;
od[k] = p->ordinate_factor;
++k;
}

text
	const __m256 vln = _mm256_load_ps(ln);
	const __m256 vM = _mm256_load_ps(Mv);
	const __m256 vq = _mm256_load_ps(q);
	const __m256 vnf = _mm256_load_ps(nf);
	const __m256 vod = _mm256_load_ps(od);

	const __m256 vm = _mm256_fmadd_ps(vGF, vln, _mm256_mul_ps(va, vM));
	__m256 vinvm = _mm256_rcp_ps(vm);
	vinvm = _mm256_mul_ps(
		vinvm, _mm256_fnmadd_ps(vm, vinvm, _mm256_set1_ps(2.0f)));

	const __m256 t = _mm256_fmadd_ps(vm, vnf, vod);
	const __m256 res = _mm256_fmadd_ps(vq, vinvm, t);
	_mm256_store_ps(out, res);

	k = 0;

#pragma loop ivdep
while (k < 8) {
arr[i + k]->R = out[k];
++k;
}

text
	i += 8;
}

while (i < n) {
	const Interval1D* p = arr[i];
	const float mi =
		fmaf(GF, fmaf(-p->x1, 1.0f, p->x2), fmaf(p->M, alpha, 0.0f));
	arr[i]->R = fmaf(1.0f / mi,
		p->quadratic_term,
		fmaf(mi, p->N_factor, p->ordinate_factor));
	++i;
}

}

__declspec(align(16)) struct IntervalND final sealed{
const float x1;
const float x2;
const float y1;
const float y2;
const float delta_y;
const float ordinate_factor;
float N_factor;
float quadratic_term;
float M;
float R;
unsigned long long i1;
unsigned long long i2;
float diam;
int span_level;

text
static __declspec(noalias) __forceinline void* operator new(size_t) noexcept {
	Slab* s = tls.local();
	char* r = s->current;
	s->current += 64u;
	return r;
}

__declspec(noalias) __forceinline IntervalND(float _x1,
																						 float _x2,
																						 float _y1,
																						 float _y2) noexcept
		: x1(_x1),
			x2(_x2),
			y1(_y1),
			y2(_y2),
			delta_y(fmaf(_y2, 1.0f, -_y1)),
			ordinate_factor(
					fmaf(fmaf(-y1, 1.0f, -y2), 2.0f, 0.0f)),
			N_factor(0),
			quadratic_term(0),
			M(0),
			R(0),
			i1(0),
			i2(0),
			diam(0),
			span_level(0) {
}

__declspec(noalias) __forceinline void compute_span_level(
		const struct MortonND& map) noexcept;

__declspec(noalias) __forceinline void set_metric(float d_alpha) noexcept {
	N_factor = d_alpha;
	quadratic_term = fmaf(1.0f / N_factor,
												fmaf(delta_y, delta_y, 0.0f),
												0.0f);
	M = fmaf(1.0f / N_factor, fabsf(delta_y), 0.0f);
}

__declspec(noalias) __forceinline void ChangeCharacteristic(float _m) noexcept {
	R = fmaf(1.0f / _m, quadratic_term, fmaf(_m, N_factor, ordinate_factor));
}

};

static __declspec(noalias) __forceinline bool ComparePtrND(const IntervalND* a,
const IntervalND* b) noexcept {
return a->R < b->R;
}

static __declspec(noalias) __forceinline void RecomputeR_ConstM_AVX2_ND(
IntervalND* const* arr,
size_t n,
float m) noexcept {
const __m256 vm = _mm256_set1_ps(m);
__m256 vinvm = _mm256_rcp_ps(vm);
vinvm =
_mm256_mul_ps(vinvm, _mm256_fnmadd_ps(vm, vinvm, _mm256_set1_ps(2.0f)));

text
size_t i = 0;
size_t limit = n & ~7ull;

alignas(16) float q[8];
alignas(16) float nf[8];
alignas(16) float od[8];
alignas(16) float out[8];

#pragma loop ivdep
while (i < limit) {
int k = 0;
#pragma loop ivdep
while (k < 8) {
const IntervalND* p = arr[i + k];
q[k] = p->quadratic_term;
nf[k] = p->N_factor;
od[k] = p->ordinate_factor;
++k;
}

text
	const __m256 vq = _mm256_load_ps(q);
	const __m256 vnf = _mm256_load_ps(nf);
	const __m256 vod = _mm256_load_ps(od);

	const __m256 t = _mm256_fmadd_ps(vm, vnf, vod);
	const __m256 res = _mm256_fmadd_ps(vq, vinvm, t);
	_mm256_store_ps(out, res);

	k = 0;

#pragma loop ivdep
while (k < 8) {
arr[i + k]->R = out[k];
++k;
}

text
	i += 8;
}

while (i < n) {
	arr[i]->ChangeCharacteristic(m);
	++i;
}

}

static __declspec(noalias) __forceinline void RecomputeR_AffineM_AVX2_ND(
IntervalND* const* arr,
size_t n,
float GF,
float alpha) noexcept {
const __m256 vGF = _mm256_set1_ps(GF);
const __m256 va = _mm256_set1_ps(alpha);

text
size_t i = 0;
size_t limit = n & ~7ull;

alignas(16) float ln[8];
alignas(16) float Mv[8];
alignas(16) float q[8];
alignas(16) float nf[8];
alignas(16) float od[8];
alignas(16) float out[8];

#pragma loop ivdep
while (i < limit) {
int k = 0;
#pragma loop ivdep
while (k < 8) {
const IntervalND* p = arr[i + k];
ln[k] = p->x2 - p->x1;
Mv[k] = p->M;
q[k] = p->quadratic_term;
nf[k] = p->N_factor;
od[k] = p->ordinate_factor;
++k;
}

text
	const __m256 vln = _mm256_load_ps(ln);
	const __m256 vM = _mm256_load_ps(Mv);
	const __m256 vq = _mm256_load_ps(q);
	const __m256 vnf = _mm256_load_ps(nf);
	const __m256 vod = _mm256_load_ps(od);

	const __m256 vm = _mm256_fmadd_ps(vGF, vln, _mm256_mul_ps(va, vM));
	__m256 vinvm = _mm256_rcp_ps(vm);
	vinvm = _mm256_mul_ps(
		vinvm, _mm256_fnmadd_ps(vm, vinvm, _mm256_set1_ps(2.0f)));

	const __m256 t = _mm256_fmadd_ps(vm, vnf, vod);
	const __m256 res = _mm256_fmadd_ps(vq, vinvm, t);
	_mm256_store_ps(out, res);

	k = 0;

#pragma loop ivdep
while (k < 8) {
arr[i + k]->R = out[k];
++k;
}

text
	i += 8;
}

while (i < n) {
	const IntervalND* p = arr[i];
	const float mi =
		fmaf(GF, fmaf(-p->x1, 1.0f, p->x2), fmaf(p->M, alpha, 0.0f));
	arr[i]->R = fmaf(1.0f / mi,
		p->quadratic_term,
		fmaf(mi, p->N_factor, p->ordinate_factor));
	++i;
}

}

static __declspec(noalias) __forceinline float fast_pow_int(float v,
int n) noexcept {
float r;
switch (n) {
case 3: {
const float v2 = fmaf(v, v, 0.0f);
r = fmaf(v2, v, 0.0f);
} break;
case 4: {
const float v2 = fmaf(v, v, 0.0f);
r = fmaf(v2, v2, 0.0f);
} break;
case 5: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
r = fmaf(fmaf(v4, v2, 0.0f), v, 0.0f);
} break;
case 6: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
r = fmaf(v4, v2, 0.0f);
} break;
case 7: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
r = fmaf(fmaf(v4, v2, 0.0f), v, 0.0f);
} break;
case 8: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
r = fmaf(v4, v4, 0.0f);
} break;
case 9: {
const float v3 = fmaf(fmaf(v, v, 0.0f), v, 0.0f);
const float v6 = fmaf(v3, v3, 0.0f);
r = fmaf(v6, v3, 0.0f);
} break;
case 10: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
const float v8 = fmaf(v4, v4, 0.0f);
r = fmaf(v8, v2, 0.0f);
} break;
case 11: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
const float v8 = fmaf(v4, v4, 0.0f);
r = fmaf(fmaf(v8, v2, 0.0f), v, 0.0f);
} break;
case 12: {
const float v3 = fmaf(fmaf(v, v, 0.0f), v, 0.0f);
const float v6 = fmaf(v3, v3, 0.0f);
r = fmaf(v6, v6, 0.0f);
} break;
case 13: {
const float v3 = fmaf(fmaf(v, v, 0.0f), v, 0.0f);
const float v6 = fmaf(v3, v3, 0.0f);
r = fmaf(fmaf(v6, v6, 0.0f), v, 0.0f);
} break;
case 14: {
const float v7 = fmaf(
fmaf(fmaf(fmaf(fmaf(fmaf(v, v, 0.0f), v, 0.0f), v, 0.0f), v, 0.0f),
v,
0.0f),
v,
0.0f);
r = fmaf(v7, v7, 0.0f);
} break;
case 15: {
const float v7 = fmaf(
fmaf(fmaf(fmaf(fmaf(fmaf(v, v, 0.0f), v, 0.0f), v, 0.0f), v, 0.0f),
v,
0.0f),
v,
0.0f);
r = fmaf(fmaf(v7, v7, 0.0f), v, 0.0f);
} break;
default: {
const float v2 = fmaf(v, v, 0.0f);
const float v4 = fmaf(v2, v2, 0.0f);
const float v8 = fmaf(v4, v4, 0.0f);
r = fmaf(v8, v8, 0.0f);
}
}
return r;
}

static __declspec(noalias) __forceinline float step(float _m,
float x1,
float x2,
float y1,
float y2,
float _N,
float _r) noexcept {
const float diff = fmaf(y2, 1.0f, -y1);
const unsigned sign_mask =
((reinterpret_cast<const unsigned>(&diff)) & 0x80000000u) ^
0x80000000u;
const float sign_mult = reinterpret_cast<const float>(&sign_mask);

text
if (_N == 1.0f) {
	return fmaf(fmaf(-(1.0f / _m), diff, x1 + x2), 0.5f, 0.0f);
}

if (_N == 2.0f) {
	return fmaf(fmaf(fmaf(fmaf(1.0f / fmaf(_m, _m, 0.0f), sign_mult, 0.0f),
		fmaf(fmaf(diff, diff, 0.0f), _r, 0.0f),
		0.0f),
		1.0f,
		fmaf(x1, 1.0f, x2)),
		0.5f,
		0.0f);
}

return fmaf(
	fmaf(
		fmaf(
			fmaf(1.0f / fast_pow_int(_m, (int)_N), sign_mult, 0.0f),
			fast_pow_int(fabsf(diff), (int)_N),
			0.0f),
		_r,
		0.0f),
	1.0f,
	fmaf(x1, 1.0f, x2)) *
	0.5f;

}

__declspec(align(16)) struct MortonCachePerRank final sealed{
std::vector<int> permCache;
std::vector<unsigned long long> invMaskCache;
unsigned baseSeed;
};

static thread_local MortonCachePerRank g_mc;

static __declspec(noalias) __forceinline unsigned long long gray_encode(
unsigned long long x) noexcept {
return x ^ (x >> 1);
}

static __declspec(noalias) __forceinline long long gray_decode(
unsigned long long g) noexcept {
g ^= g >> 32;
g ^= g >> 16;
g ^= g >> 8;
g ^= g >> 4;
g ^= g >> 2;
g ^= g >> 1;
return (long long)g;
}

__declspec(align(16)) struct MortonND final sealed{
const int dim;
const int levels;
const int eff_levels;
const int extra_levels;
const int chunks;

text
std::vector<int> chunk_bits;
std::vector<unsigned long long> chunk_bases;
unsigned long long scale;

std::vector<float> low;
std::vector<float> high;
std::vector<float> step;
std::vector<float> invStep;
std::vector<float> baseOff;

std::vector<int> perm;
std::vector<unsigned long long> invMask;
std::vector<unsigned long long> pextMask;
std::vector<unsigned long long> pextMaskChunks;

const float invScaleLevel;
const bool use_gray;

static __declspec(noalias) __forceinline unsigned long long make_mask(
		int dim,
		int Lc,
		int d) noexcept {
	unsigned long long m = 0ull;
	unsigned long long bitpos = (unsigned long long)d;
	int b = 0;

#pragma loop ivdep
while (b < Lc) {
m |= 1ull << bitpos;
bitpos += (unsigned long long)dim;
++b;
}
return m;
}

text
__declspec(noalias) __forceinline MortonND(int D,
																					 int L,
																					 const float* lows,
																					 const float* highs,
																					 const MortonCachePerRank& mc)
		: dim(D),
			levels(L),
			eff_levels((std::max)(1,
														(int)(63 / (D ? D : 1)))),
			extra_levels((L > eff_levels) ? (L - eff_levels) : 0),
			chunks((extra_levels > 0)
								 ? (1 + (extra_levels + eff_levels - 1) / eff_levels)
								 : 1),
			low(lows, lows + D),
			high(highs, highs + D),
			step(D, 0.0f),
			invStep(D, 0.0f),
			baseOff(D, 0.0f),
			perm(mc.permCache.begin(), mc.permCache.begin() + D),
			invMask(mc.invMaskCache.begin(), mc.invMaskCache.begin() + D),
			invScaleLevel(1.0f /
										(float)((unsigned long long)1 << L)),
			use_gray(true) {
	int d = 0;

#pragma loop ivdep
while (d < dim) {
const float rng = high[d] - low[d];
const float st = rng * invScaleLevel;
step[d] = st;
invStep[d] = 1.0f / st;
baseOff[d] = fmaf(0.5f, st, low[d]);
++d;
}

text
	chunk_bits.resize(chunks);
	pextMaskChunks.resize((size_t)chunks * (size_t)dim);
	chunk_bases.resize(chunks);

	int remaining = levels;
	int c = 0;
	while (c < chunks) {
		const int Lc =
				(c == 0) ? (std::min)(eff_levels, remaining)
								 : (std::min)(eff_levels, remaining);
		chunk_bits[c] = Lc;
		remaining -= Lc;

		const unsigned long long baseC =
				(unsigned long long)1 << (dim * Lc);
		chunk_bases[c] = baseC;

		d = 0;

#pragma loop ivdep
while (d < dim) {
pextMaskChunks[(size_t)c * (size_t)dim + (size_t)d] =
make_mask(dim, Lc, d);
++d;
}

text
		++c;
	}

	pextMask.resize(dim);
	d = 0;

#pragma loop ivdep
while (d < dim) {
pextMask[d] = make_mask(dim, chunk_bits[0], d);
++d;
}

text
	scale = (unsigned long long)1 << (dim * chunk_bits[0]);
}

__declspec(noalias) __forceinline float block_diameter(
		unsigned long long i1,
		unsigned long long i2) const noexcept {
	if (i1 > i2) {
		std::swap(i1, i2);
	}

	float s2 = 0.0f;
	int d = 0;

#pragma loop ivdep
while (d < dim) {
const int pd = perm[d];
const unsigned long long varying =
(i1 ^ i2) & pextMask[d];
const int nfree_hi = _mm_popcnt_u64(varying);
const int nfree_total =
nfree_hi + levels - chunk_bits[0];
const float range =
fmaf(step[pd],
fmaf(ldexpf(1.0f, nfree_total), 1.0f, -1.0f),
0.0f);
s2 = fmaf(range, range, s2);
++d;
}
return sqrtf(s2);
}

text
__declspec(noalias) __forceinline void map01ToPoint(
		float t,
		float* __restrict out) const noexcept {
	unsigned long long accBits[32] = {0ull};
	int accShifted[32] = {0};

	int c = 0;
	while (c < chunks) {
		const int Lc = chunk_bits[c];
		const unsigned long long baseC = chunk_bases[c];

		const float scaled = t * (float)baseC;
		unsigned long long idxc =
				(scaled >= (float)baseC)
						? (baseC - 1ull)
						: (unsigned long long)scaled;

		if (use_gray) {
			idxc = gray_encode(idxc);
		}

		int shift_from_top = 0;
		int k = 0;
		while (k <= c) {
			shift_from_top += chunk_bits[k];
			++k;
		}

		const int inv_shift = levels - shift_from_top;

		int d = 0;

#pragma loop ivdep
while (d < dim) {
const int pd = perm[d];
const unsigned long long mask =
pextMaskChunks[(size_t)c * (size_t)dim + (size_t)d];
unsigned long long bits = _pext_u64(idxc, mask);

text
			if (inv_shift >= 0) {
				unsigned long long invMaskSegment = 0ull;
				if (chunk_bits[c] < 63) {
					const unsigned long long take =
							((unsigned long long)1 << chunk_bits[c]) - 1ull;
					invMaskSegment =
							(invMask[pd] >> inv_shift) & take;
				}
				bits ^= invMaskSegment;
			}

			accBits[pd] = (accBits[pd] << Lc) | bits;
			accShifted[pd] += Lc;
			++d;
		}

		++c;
	}

	int d = 0;

#pragma loop ivdep
while (d < dim) {
out[d] = fmaf(step[d], (float)accBits[d], baseOff[d]);
++d;
}
}

text
__declspec(noalias) __forceinline float pointToT(
		const float* __restrict q) const noexcept {
	const int bitsFull = levels;
	const int bitsCoarse = chunk_bits[0];

	unsigned long long idx0 = 0ull;

	int d = 0;

#pragma loop ivdep
while (d < dim) {
const int pd = perm[d];
const float v =
(q[pd] - baseOff[pd]) * invStep[pd];
const long long cell = (long long)_mm_cvt_ss2si(
_mm_round_ss(_mm_setzero_ps(),
_mm_set_ss(v),
_MM_FROUND_TO_NEAREST_INT |
_MM_FROUND_NO_EXC));

text
		const long long maxv =
				((long long)1 << bitsFull) - 1;
		(void)maxv;

		unsigned long long b =
				(unsigned long long)cell >>
				(bitsFull - bitsCoarse);

		unsigned long long invMask0 = 0ull;
		if (bitsCoarse < 63) {
			const unsigned long long take =
					((unsigned long long)1 << bitsCoarse) - 1ull;
			invMask0 =
					(invMask[pd] >> (bitsFull - bitsCoarse)) &
					take;
		}
		b ^= invMask0;

		idx0 |= _pdep_u64(b, pextMask[d]);
		++d;
	}

	if (use_gray) {
		idx0 = (unsigned long long)gray_decode(idx0);
	}

	return fmaf(1.0f / (float)scale,
							fmaf((float)idx0, 1.0f, 0.5f),
							0.0f);
}

};

__declspec(noalias) __forceinline void IntervalND::compute_span_level(
const MortonND& map) noexcept {
span_level = 0;
int d = 0;
#pragma loop ivdep
while (d < map.dim) {
const unsigned long long varying =
(i1 ^ i2) & map.pextMask[d];
span_level += _mm_popcnt_u64(varying);
++d;
}
span_level += (map.levels - map.chunk_bits[0]) * map.dim;
span_level = (std::min)(span_level, 11);
}

__declspec(align(16)) struct ManipCost final sealed{
const int n;
const bool variableLen;
const float targetX;
const float targetY;
const float minTheta;
const float archBiasW;
const float archBiasK;
const float sharpW;

text
__declspec(noalias) __forceinline ManipCost(int _n,
																						bool _variableLen,
																						float _targetX,
																						float _targetY,
																						float _minTheta) noexcept
		: n(_n),
			variableLen(_variableLen),
			targetX(_targetX),
			targetY(_targetY),
			minTheta(_minTheta),
			archBiasW(0.02f),
			archBiasK(3.0f),
			sharpW(0.05f) {
}

__declspec(noalias) __forceinline float operator()(
		const float* __restrict q,
		float& out_x,
		float& out_y) const noexcept {
	const float* __restrict th = q;
	const float* __restrict L = variableLen ? (q + n) : nullptr;

	alignas(16) float phi[32];
	alignas(16) float s_arr[32];
	alignas(16) float c_arr[32];

	float x = 0.0f;
	float y = 0.0f;
	float phi_acc = 0.0f;
	float penC = 0.0f;
	float archPen = 0.0f;

	int i = 0;

#pragma loop ivdep
while (i < n) {
phi_acc += th[i];
phi[i] = phi_acc;
++i;
}

text
	FABE13_SINCOS(phi, s_arr, c_arr, n);

	const float sharpScale = 2.0f / (minTheta + 1.0e-6f);
	const float Lc = 1.0f;

	if (variableLen) {
		i = 0;
		while (i < n) {
			const float Li = L[i];
			x = fmaf(Li, c_arr[i], x);
			y = fmaf(Li, s_arr[i], y);
			++i;
		}
	}

else {
i = 0;
while (i < n) {
x = fmaf(Lc, c_arr[i], x);
y = fmaf(Lc, s_arr[i], y);
++i;
}
}

i = 0;
#pragma loop ivdep
while (i < n) {
const float theta = th[i];
const float ai = fabsf(theta);
const float v = fmaf(ai, 1.0f, -minTheta);

text
		if (v > 0.0f) {
			const float scale = fmaf(sharpScale, v, 0.0f);
			const float arg =
					fmaf(scale,
							 0.69314718055994530941723212145818f,
							 0.0f);
			const float exp2_val =
					fmaf(arg,
							 fmaf(arg,
										fmaf(arg,
												 fmaf(arg, 0.00833333377f, 0.0416666679f),
												 0.16666667f),
										0.5f),
							 1.0f);
			penC = fmaf(sharpW, fmaf(exp2_val, 1.0f, -1.0f), penC);
		}

		const float t = fmaf(-theta, archBiasK, 0.0f);
		float sp;
		if (t > 10.0f) {
			sp = t;
		}

else {
const float exp_val =
fmaf(t,
fmaf(t,
fmaf(t,
fmaf(t, 0.00833333377f, 0.0416666679f),
0.16666667f),
0.5f),
1.0f);
sp = log1pf(exp_val);
}
archPen = fmaf(archBiasW, sp, archPen);
++i;
}

const float dx = fmaf(x, 1.0f, -targetX);
const float dy = fmaf(y, 1.0f, -targetY);
const float dist = sqrtf(fmaf(dx, dx, dy * dy));

out_x = x;
out_y = y;
return fmaf(dist, 1.0f, fmaf(penC, 1.0f, archPen));
}
};

static __declspec(noalias) __forceinline void HitTest2D_analytic(
float x_param,
float& out_x1,
float& out_x2) noexcept {
const float a = gActiveMap.a;
const float inv_lenx = gActiveMap.inv_lenx;
const unsigned scale = gActiveMap.scale;
const unsigned scale_minus_1 = scale - 1u;
const float lenx = gActiveMap.lenx;
const float leny = gActiveMap.leny;
const float c = gActiveMap.c;
const unsigned start = gActiveMap.start;
const int levels = gActiveMap.levels;

text
float norm = (x_param - a) * inv_lenx;
norm = fminf(fmaxf(norm, 0.0f), 0x1.fffffep-1f);

unsigned idx = (unsigned)(norm * (float)scale);
idx = idx > scale_minus_1 ? scale_minus_1 : idx;

float sx = lenx;
float sy = leny;
float x1 = a;
float x2 = c;
unsigned type = start;

int l = levels - 1;
while (l >= 0) {
	const unsigned q = (idx >> (l * 2)) & 3u;
	const Step s = g_step_tbl[type][q];
	type = s.next;
	sx *= 0.5f;
	sy *= 0.5f;
	x1 += s.dx ? sx : 0.0f;
	x2 += s.dy ? sy : 0.0f;
	--l;
}

out_x1 = x1 + sx * 0.5f;
out_x2 = x2 + sy * 0.5f;

}

static __declspec(noalias) __forceinline float FindX2D_analytic(
float px,
float py) noexcept {
const float a = gActiveMap.a;
const float b = gActiveMap.b;
const float c = gActiveMap.c;
const float d = gActiveMap.d;
const float lenx = gActiveMap.lenx;
const float leny = gActiveMap.leny;
const unsigned scale = gActiveMap.scale;
const unsigned start = gActiveMap.start;
const int levels = gActiveMap.levels;

text
const float clamped_px = fminf(fmaxf(px, a), b);
const float clamped_py = fminf(fmaxf(py, c), d);

float sx = lenx;
float sy = leny;
float x0 = a;
float y0 = c;
unsigned idx = 0u;
unsigned type = start;

int l = 0;
while (l < levels) {
	sx *= 0.5f;
	sy *= 0.5f;

	const float mx = x0 + sx;
	const float my = y0 + sy;

	const unsigned tr =
		(unsigned)((clamped_px > mx) & (clamped_py > my));
	const unsigned tl =
		(unsigned)((clamped_px < mx) & (clamped_py > my));
	const unsigned dl =
		(unsigned)((clamped_px < mx) & (clamped_py < my));
	const unsigned none =
		(unsigned)(1u ^ (tr | tl | dl));
	const unsigned dd = (tr << 1) | tr | tl | (none << 1);

	const InvStep inv = g_inv_tbl[type][dd];
	type = inv.next;
	idx = (idx << 2) | inv.q;

	const unsigned dx = dd >> 1;
	const unsigned dy = dd & 1u;
	x0 += dx ? sx : 0.0f;
	y0 += dy ? sy : 0.0f;

	++l;
}

const float scale_recip = 1.0f / (float)scale;
return fmaf((float)idx * scale_recip, lenx, a);

}

static __declspec(noalias) __forceinline int generate_lhs_seeds_lite(
const MortonND& map,
int dim,
float* __restrict S,
int stride,
unsigned seed) noexcept {
int temp_dim = dim;
const int ns =
(int)fmaf((float)(--temp_dim),
fmaf((float)temp_dim,
fmaf((float)temp_dim,
fmaf((float)temp_dim,
0.00833333377f,
0.0416666679f),
0.16666667f),
0.5f),
1.0f);

text
unsigned st = seed;

alignas(16) int permutations[32][256];

int d = 0;

#pragma loop ivdep
while (d < dim) {
int s = 0;
#pragma loop ivdep
while (s < ns) {
permutations[d][s] = s;
++s;
}

text
	s = ns - 1;
	while (s > 0) {
		st ^= st << 13;
		st ^= st >> 17;
		st ^= st << 5;

		const int j = (int)(st % (unsigned)(s + 1));
		const int t = permutations[d][s];
		permutations[d][s] = permutations[d][j];
		permutations[d][j] = t;

		--s;
	}

	++d;
}

int s2 = 0;

#pragma loop ivdep
while (s2 < ns) {
d = 0;
#pragma loop ivdep
while (d < dim) {
st ^= st << 13;
st ^= st >> 17;
st ^= st << 5;

text
		const float u = (st & 0xFFFFFF) * 5.9604645e-8f;
		const int stratum = permutations[d][s2];
		const float pos =
			((float)stratum + u) / (float)ns;
		const int pd = map.perm[d];
		const float lo = map.low[pd];
		const float hi = map.high[pd];

		S[s2 * stride + d] = fmaf(pos, hi - lo, lo);
		++d;
	}
	++s2;
}

return ns;

}

static __declspec(noalias) __forceinline int generate_heuristic_seeds(
const ManipCost& cost,
const MortonND& map,
int dim,
float* __restrict S,
int stride,
unsigned seed) noexcept {
const int n = cost.n;
const bool VL = cost.variableLen;
const float tx = cost.targetX;
const float ty = cost.targetY;

text
int total_seeds = 0;

{
	float* s0 = S + total_seeds * stride;

	float sin_phi;
	float cos_phi;
	const float rho = sqrtf(fmaf(tx, tx, ty * ty));
	FABE13_SINCOS(&tx, &sin_phi, &cos_phi, 1);
	const float phi =
		(fabsf(sin_phi) > 0.0f)
		? atan2f(ty, tx)
		: 0.0f;
	const float len = fminf(fmaxf((1.0f / (float)n) * rho, 0.5f), 2.0f);

	int i = 0;

#pragma loop ivdep
while (i < n) {
s0[i] = (1.0f / (float)n) * phi;
++i;
}

text
	if (VL) {
		i = 0;
		while (i < n) {
			s0[n + i] = len;
			++i;
		}
	}

	++total_seeds;
}

{
	float* s1 = S + total_seeds * stride;

	float sin_phi;
	float cos_phi;
	FABE13_SINCOS(&tx, &sin_phi, &cos_phi, 1);
	const float phi =
		(fabsf(sin_phi) > 0.0f)
		? atan2f(ty, tx)
		: 0.0f;

	int i = 0;

#pragma loop ivdep
while (i < n) {
s1[i] = 0.5f * phi * ((i & 1) ? -1.0f : 1.0f);
++i;
}

text
	if (VL) {
		i = 0;
		while (i < n) {
			s1[n + i] =
				fmaf(0.4f,
					(float)i / (float)n,
					0.8f);
			++i;
		}
	}

	++total_seeds;
}

{
	float* s2 = S + total_seeds * stride;
	const float inv =
		(n > 1) ? 1.0f / (float)(n - 1) : 0.0f;

	float sin_phi;
	float cos_phi;
	FABE13_SINCOS(&tx, &sin_phi, &cos_phi, 1);
	const float phi =
		(fabsf(sin_phi) > 0.0f)
		? atan2f(ty, tx)
		: 0.0f;

	int i = 0;

#pragma loop ivdep
while (i < n) {
const float pr = (float)i * inv;
s2[i] = fmaf(phi, fmaf(-0.3f, pr, 1.0f), 0.0f);
++i;
}

text
	if (VL) {
		int j = 0;
		while (j < n) {
			float si;
			FABE13_SIN(fmaf(1.5f, (float)j, 0.0f), si);
			s2[n + j] = fmaf(0.2f, si, 1.0f);
			++j;
		}
	}

	++total_seeds;
}

const int lhs_count =
	generate_lhs_seeds_lite(map,
		dim,
		S + total_seeds * stride,
		stride,
		seed);
total_seeds += lhs_count;

return total_seeds;

}

static __declspec(noalias) void agp_run_branch_mpi(
const MortonND& map,
const ManipCost& cost,
int maxIter,
float r,
bool adaptive,
float eps,
unsigned seed,
std::vector<IntervalND*,
boost::alignment::aligned_allocator<IntervalND*, 16u>>&H,
std::vector<float,
boost::alignment::aligned_allocator<float, 16u>>&bestQ,
float& bestF,
float& bestX,
float& bestY,
size_t& out_iterations,
float& out_achieved_epsilon,
float M_prior) noexcept
{
const int n = cost.n;
const int dim = n + (cost.variableLen ? n : 0);
const float dim_f = (float)dim;

text
unsigned exchange_counter = 0;
unsigned exchange_counter_T = 0;

alignas(16) float M_by_span[12];
int msi = 0;
while (msi < 12) {
	M_by_span[msi++] = M_prior;
}

float Mmax = M_prior;

alignas(16) float q_local[32];
alignas(16) float phi[32];
alignas(16) float s_arr[32];
alignas(16) float c_arr[32];
alignas(16) float sum_s[32];
alignas(16) float sum_c[32];
alignas(16) float q_try[32];

bestQ.reserve((size_t)dim);

float x = 0.0f;
float y = 0.0f;
int no_improve = 0;

auto t_to_idx = [&](float t) -> unsigned long long {
	unsigned long long idx =
		(unsigned long long)fmaf(t, (float)map.scale, 0.0f);
	return idx;
	};

auto update_pockets_and_Mmax = [&](IntervalND* I) {
	const int k = I->span_level;
	if (I->M > M_by_span[k]) {
		M_by_span[k] = I->M;
	}
	if (M_by_span[k] > Mmax) {
		Mmax = M_by_span[k];
	}
	};

const float a = 0.0f;
const float b = 1.0f;

float p = 0.0f;
float dmax = fmaf(b, 1.0f, -a);
const float initial_len = dmax;

float exp_arg_threshold =
	fmaf(1.0f /
		fmaf(dim_f,
			fmaf(dim_f,
				fmaf(dim_f,
					fmaf(dim_f,
						fmaf(dim_f, -0.2f, 0.25f),
						-0.33333333f),
					0.5f),
				-1.0f),
			dim_f),
		0.415888308336f,
		0.0f);
const float log_arg_threshold =
	fmaf(dim_f, 0.1f, -0.105f);
const float adaptive_coeff_addition_threshold =
	fmaf(
		fmaf(exp_arg_threshold,
			fmaf(exp_arg_threshold,
				fmaf(exp_arg_threshold,
					fmaf(exp_arg_threshold,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f),
		1.0f /
		fmaf(log_arg_threshold,
			fmaf(log_arg_threshold,
				fmaf(log_arg_threshold,
					fmaf(log_arg_threshold,
						fmaf(log_arg_threshold,
							0.164056f,
							-0.098462f),
						0.240884f),
					-0.351834f),
				0.999996f),
			log_arg_threshold),
		0.17814618538f);
float adaptive_coeff_threshold =
	adaptive_coeff_addition_threshold + 1.0f;

const float log_arg = dim_f + 5.0f;
const float A_dim =
	fmaf(1.0f /
		fmaf(log_arg,
			fmaf(log_arg,
				fmaf(log_arg,
					fmaf(log_arg,
						fmaf(log_arg,
							0.164056f,
							-0.098462f),
						0.240884f),
					-0.351834f),
				0.999996f),
			log_arg),
		3.0f,
		0.0f);
const float B_dim = fmaf(A_dim, 0.65f, 0.0f);
const float log_argument = A_dim - 2.03f;
const float C_dim =
	fmaf(log_argument,
		fmaf(log_argument,
			fmaf(log_argument,
				fmaf(log_argument,
					fmaf(log_argument,
						0.164056f,
						-0.098462f),
					0.240884f),
				-0.351834f),
			0.999996f),
		log_argument) -
	B_dim;
const float adaptive_coeff_addition =
	fmaf(C_dim,
		fmaf(C_dim,
			fmaf(C_dim,
				fmaf(C_dim,
					0.00833333377f,
					0.0416666679f),
				0.16666667f),
			0.55f),
		1.0f);
float adaptive_coeff = A_dim - adaptive_coeff_addition;

int it = 0;

auto evalAt = [&](float t) -> float {
	map.map01ToPoint(t, q_local);
	float f = cost(q_local, x, y);

	const float step_arg =
		fmaf(-dim_f, 0.825f, 3.3f);
	float eta =
		fmaf(fmaf(step_arg,
			fmaf(step_arg,
				fmaf(step_arg,
					fmaf(step_arg,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f),
			0.0685f,
			0.0f);

	if ((((f < fmaf(bestF, 2.03f - adaptive_coeff, 0.0f) && p > 0.55f) ||
		(f < fmaf(bestF, adaptive_coeff, 0.0f) &&
			((p > 0.7f && !(it % 3)) || p > 0.9f))) && dim > 2) || (f < fmaf(bestF, adaptive_coeff, 0.0f) && dim < 3)) {
		float acc = 0.0f;
		int ii = 0;
		while (ii < n) {
			acc = fmaf(q_local[ii], 1.0f, acc);
			phi[ii] = acc;
			++ii;
		}

		FABE13_SINCOS(phi, s_arr, c_arr, n);

		float as = 0.0f;
		float ac = 0.0f;
		int k = n - 1;
		while (k >= 0) {
			const float Lk =
				cost.variableLen ? q_local[n + k] : 1.0f;
			as = fmaf(Lk, s_arr[k], as);
			ac = fmaf(Lk, c_arr[k], ac);
			sum_s[k] = as;
			sum_c[k] = ac;
			--k;
		}

		const float dx = fmaf(x, 1.0f, -cost.targetX);
		const float dy = fmaf(y, 1.0f, -cost.targetY);
		const float dist = sqrtf(fmaf(dx, dx, dy * dy)) + 1.0e-8f;

		int stepI = 0;
		const float adaptive_window =
			fmaf(1.0f / fmaf(bestF, adaptive_coeff_threshold, 0.0f),
				fmaf(bestF, adaptive_coeff_threshold, -f),
				0.0f);
		const float curse_dim_arg =
			fmaf(dim_f, 0.825f, 0.0f);
		const int threshold_iters_grad =
			(int)fmaf(
				fmaf(adaptive_window,
					adaptive_window,
					1.0f),
				fmaf(fmaf(curse_dim_arg,
					fmaf(curse_dim_arg,
						fmaf(curse_dim_arg,
							fmaf(curse_dim_arg,
								0.00833333377f,
								0.0416666679f),
							0.16666667f),
						0.5f),
					1.0f),
					0.0685f,
					0.0f),
				0.0f);

		int no_improve_steps = 0;
		float f_at_check_start = f;
		int steps_since_check = 0;
		const int check_interval =
			(int)fmaf(1.0f /
				log2f(fmaf(dim_f, 0.5f, 1.0f)),
				(float)(threshold_iters_grad >> 2),
				0.0f);

		while (stepI < threshold_iters_grad) {
			int i = 0;

#pragma loop ivdep
while (i < n) {
float gpen = 0.0f;

text
				{
					const float ai = fabsf(q_local[i]);
					const float v =
						fmaf(ai, 1.0f, -cost.minTheta);
					if (v > 0.0f) {
						const float scale_arg =
							fmaf(2.0f / (cost.minTheta + 1.0e-6f),
								fmaf(v,
									0.69314718055994530941723212145818f,
									0.0f),
								0.0f);
						const float exp_val =
							fmaf(scale_arg,
								fmaf(scale_arg,
									fmaf(scale_arg,
										fmaf(scale_arg,
											0.00833333377f,
											0.0416666679f),
										0.16666667f),
									0.5f),
								1.0f);
						const float dpen_dtheta =
							fmaf(cost.sharpW,
								fmaf(exp_val,
									0.69314718055994530941723212145818f *
									(2.0f / (cost.minTheta + 1.0e-6f)),
									0.0f),
								copysignf(1.0f, q_local[i]));
						gpen = fmaf(dpen_dtheta, 1.0f, gpen);
					}
				}

				{
					const float tsg =
						fmaf(-q_local[i], cost.archBiasK, 0.0f);
					const float exp_arg = -tsg;
					const float exp_val =
						fmaf(exp_arg,
							fmaf(exp_arg,
								fmaf(exp_arg,
									fmaf(exp_arg,
										0.00833333377f,
										0.0416666679f),
									0.16666667f),
								0.5f),
							1.0f);
					const float sig = 1.0f /
						fmaf(exp_val, 1.0f, 1.0f);
					gpen = fmaf(
						-fmaf(cost.archBiasW, cost.archBiasK, 0.0f),
						sig,
						gpen);
				}

				const float g =
					fmaf(fmaf(dx, -sum_s[i],
						fmaf(dy, sum_c[i], 0.0f)),
						1.0f / dist,
						gpen);
				q_try[i] = fmaf(-eta, g, q_local[i]);

				const float lo =
					(i == 0) ? -1.0471975511965977461542144610932f
					: -2.6179938779914943653855361527329f;
				const float hi =
					2.6179938779914943653855361527329f;

				if (q_try[i] < lo) {
					q_try[i] = lo;
				}
				else if (q_try[i] > hi) {
					q_try[i] = hi;
				}

				++i;
			}

			if (cost.variableLen) {
				int j = 0;

#pragma loop ivdep
while (j < n) {
const float g =
fmaf(fmaf(dx, c_arr[j],
fmaf(dy, s_arr[j], 0.0f)),
1.0f / dist,
0.0f);
float v = fmaf(-eta, g, q_local[n + j]);

text
					if (v < 0.5f) {
						v = 0.5f;
					}
					else if (v > 2.0f) {
						v = 2.0f;
					}

					q_try[n + j] = v;
					++j;
				}
			}

			float x2;
			float y2;
			const float f2 = cost(q_try, x2, y2);
			const float rel_improvement =
				fmaf(-1.0f / f, f2, 1.0f);

			if (f2 < f) {
				memcpy(q_local, q_try, (size_t)dim * sizeof(float));
				f = f2;
				x = x2;
				y = y2;
				eta = fmaf(
					eta,
					fmaf(10.0f / sqrtf(dim_f),
						1.0f / (1.0f + rel_improvement) *
						rel_improvement,
						1.0f),
					0.0f);
			}
			else {
				const float caution_coeff =
					5.0f / sqrtf(dim_f) * rel_improvement;
				eta = fmaf(
					eta,
					fmaf(caution_coeff,
						fmaf(caution_coeff,
							fmaf(caution_coeff,
								fmaf(caution_coeff,
									0.00833333377f,
									0.0416666679f),
								0.16666667f),
							0.5f),
						1.0f),
					0.0f);
				memcpy(q_try, q_local, (size_t)dim * sizeof(float));
				++no_improve_steps;
			}

			++steps_since_check;
			if (steps_since_check == check_interval) {
				if (fmaf(-1.0f / f_at_check_start, f, 1.0f) <
					1e-3f) {
					break;
				}
				f_at_check_start = f;
				steps_since_check = 0;
			}

			if (no_improve_steps >
				threshold_iters_grad >> 2) {
				break;
			}

			++stepI;
		}
	}

	if (f < bestF) {
		if (0.95f < p || dim < 3) {
			const int last = n - 1;
			const float lo =
				(last == 0) ? -1.0471975511965977461542144610932f
				: -2.6179938779914943653855361527329f;
			const float hi =
				2.6179938779914943653855361527329f;

			float bestLocF = f;
			float saved = q_local[last];
			float delta = eta;

			while (delta > fmaf(delta, 0.15f, 0.0f)) {
				int sgn = -1;
				while (sgn < 2) {
					float cand =
						fmaf((float)sgn, delta, saved);
					if (cand < lo) {
						cand = lo;
					}
					else if (cand > hi) {
						cand = hi;
					}

					const float backup = q_local[last];
					q_local[last] = cand;

					float x2;
					float y2;
					const float f2 = cost(q_local, x2, y2);

					if (f2 < bestLocF) {
						bestLocF = f2;
						x = x2;
						y = y2;
						saved = cand;
					}

					q_local[last] = backup;
					sgn += 2;
				}

				delta = fmaf(delta, 0.5f, 0.0f);
			}

			if (bestLocF < f) {
				q_local[last] = saved;
				f = bestLocF;
			}
		}

		bestF = f;
		bestQ.assign(q_local, q_local + dim);
		bestX = x;
		bestY = y;
		no_improve = 0;
	}
	else {
		++no_improve;
	}

	return f;
	};

const float f_a = evalAt(a);
const float f_b = evalAt(b);

const float Kf = fminf(fmaxf(2.0f * dim_f, 8.0f), 128.0f);
const int K = (int)Kf;

H.reserve((size_t)maxIter + (size_t)K + 16u);

const int rank = g_world->rank();
const int world = g_world->size();

alignas(16) float seeds[256 * 32];

const int seedCnt = generate_heuristic_seeds(
	cost,
	map,
	dim,
	seeds,
	32,
	(unsigned)fmaf((float)rank, 7919.0f, (float)seed));

int i = 0;
while (i < seedCnt) {
	const float* s =
		seeds + (size_t)fmaf((float)i, 32.0f, 0.0f);
	const float t_seed = map.pointToT(s);

	const float interval_size =
		(i < 3)
		? fmaf(0.0004f, (float)dim, 0.0f)
		: fmaf(
			fmaf(0.00031f, (float)dim, 0.0f),
			exp2f(
				(1.0f /
					(float)(seedCnt - 4)) *
				log2f(fmaf(0.00025f,
					1.0f / 0.00031f,
					0.0f)) *
				(float)(i - 3)),
			0.0f);

	const float t1 =
		fmaxf(a, fmaf(-interval_size, 1.0f, t_seed));
	const float t2 =
		fminf(b, fmaf(interval_size, 1.0f, t_seed));

	if (t2 > t1) {
		alignas(16) float q1[32];
		alignas(16) float q2[32];
		float x1;
		float y1;
		float x2;
		float y2;

		map.map01ToPoint(t1, q1);
		const float f1 = cost(q1, x1, y1);

		map.map01ToPoint(t2, q2);
		const float f2 = cost(q2, x2, y2);

		IntervalND* I =
			new IntervalND(t1, t2, f1, f2);
		I->i1 = t_to_idx(t1);
		I->i2 = t_to_idx(t2);
		I->diam = map.block_diameter(I->i1, I->i2);
		I->compute_span_level(map);
		I->set_metric(I->diam);
		update_pockets_and_Mmax(I);
		I->ChangeCharacteristic(fmaf(r, Mmax, 0.0f));

		if (i < 3) {
			I->R = fmaf(I->R,
				fmaf(0.01f, (float)dim, 0.85f),
				0.0f);
		}
		else {
			const float start_mult =
				fmaf(0.214f, (float)dim, 0.0f);
			const float end_mult =
				fmaf(0.174f, (float)dim, 0.0f);
			const float mult =
				fmaf(start_mult,
					exp2f((1.0f /
						(float)(seedCnt - 4)) *
						log2f(fmaf(end_mult,
							1.0f / start_mult,
							0.0f)) *
						(float)(i - 3)),
					0.0f);
			I->R = fmaf(I->R, mult, 0.0f);
		}

		H.emplace_back(I);
		std::push_heap(H.begin(), H.end(), ComparePtrND);

		if (f1 < bestF) {
			bestF = f1;
			bestQ.assign(q1, q1 + dim);
			bestX = x1;
			bestY = y1;
		}
		if (f2 < bestF) {
			bestF = f2;
			bestQ.assign(q2, q2 + dim);
			bestX = x2;
			bestY = y2;
		}
	}

	++i;
}

float prev_t = a;
float prev_f = f_a;

int k = 1;
while (k <= K) {
	const float t = fmaf(
		fmaf(b - a,
			(float)k / (float)(K + 1),
			a),
		1.0f,
		(float)rank /
		(float)(world * (K + 1)));
	const float f = evalAt(t);

	IntervalND* I =
		new IntervalND(prev_t, t, prev_f, f);
	I->i1 = t_to_idx(prev_t);
	I->i2 = t_to_idx(t);
	I->diam = map.block_diameter(I->i1, I->i2);
	I->compute_span_level(map);
	I->set_metric(I->diam);
	update_pockets_and_Mmax(I);
	I->ChangeCharacteristic(fmaf(r, Mmax, 0.0f));

	H.emplace_back(I);
	std::push_heap(H.begin(), H.end(), ComparePtrND);

	prev_t = t;
	prev_f = f;
	++k;
}

IntervalND* tail =
	new IntervalND(prev_t, b, prev_f, f_b);
tail->i1 = t_to_idx(prev_t);
tail->i2 = t_to_idx(b);
tail->diam = map.block_diameter(tail->i1, tail->i2);
tail->compute_span_level(map);
tail->set_metric(tail->diam);
update_pockets_and_Mmax(tail);
tail->ChangeCharacteristic(fmaf(r, Mmax, 0.0f));
H.emplace_back(tail);
std::push_heap(H.begin(), H.end(), ComparePtrND);

const int kickEveryDim = static_cast<int>(fmaf(22.5f, exp2f(-0.295f * dim_f), 0.0f));
const int noImproveThrDim = static_cast<int>(fmaf(32.5f, exp2f(-0.1f * dim_f), 0.0f));

while (it < maxIter) {
	p = fmaf(-1.0f / initial_len, dmax, 1.0f);

	const float p_arg =
		fmaf(p, 2.3f, -2.9775f);
	const float r_eff =
		fmaf(-fmaf(p_arg,
			fmaf(p_arg,
				fmaf(p_arg,
					fmaf(p_arg,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f) +
			1.05f,
			r,
			0.0f);

	const bool staganition = no_improve > noImproveThrDim;

	if (!(it % kickEveryDim) ||
		staganition) {
		const float t_best = map.pointToT(bestQ.data());
		const float log_arg = fmaf(dim_f, 1.0f, -1.0f);
		const float adaptive_diameter =
			fmaf(
				fmaf(log_arg,
					fmaf(log_arg,
						fmaf(log_arg,
							fmaf(log_arg,
								fmaf(log_arg,
									0.164056f,
									-0.098462f),
								0.240884f),
							-0.351834f),
						0.999996f),
					log_arg),
				dim > 2 ? fmaf(adaptive_coeff, 0.00475f, 0.0f) : 0.00545f,
				0.0f);
		int ii = 0;
		while (ii < 2) {
			const float off =
				(ii == 0) ? fmaf(adaptive_diameter, 2.0f, 0.0f) : fmaf(-adaptive_diameter, 2.0f, 0.0f);
			const float t_seed =
				fminf(b,
					fmaxf(a,
						fmaf(off, 1.0f, t_best)));
			const float f_seed = evalAt(t_seed);

			IntervalND* J =
				new IntervalND(fmaf(-adaptive_diameter, 1.0f, t_seed),
					fmaf(adaptive_diameter, 1.0f, t_seed),
					f_seed,
					f_seed);
			J->i1 = t_to_idx(fmaf(-adaptive_diameter, 1.0f, t_seed));
			J->i2 = t_to_idx(fmaf(adaptive_diameter, 1.0f, t_seed));
			J->diam = map.block_diameter(J->i1, J->i2);
			J->compute_span_level(map);
			J->set_metric(J->diam);
			update_pockets_and_Mmax(J);
			J->ChangeCharacteristic(
				fmaf(r_eff, Mmax, 0.0f));
			J->R = fmaf(J->R, 2.03f - adaptive_coeff, 0.0f);

			H.emplace_back(J);
			std::push_heap(H.begin(), H.end(), ComparePtrND);
			++ii;
		}
		no_improve = 0;
	}

	exp_arg_threshold =
		fmaf(-exp_arg_threshold * p, 10.0f, 0.0f);
	adaptive_coeff_threshold =
		fmaf(fmaf(exp_arg_threshold,
			fmaf(exp_arg_threshold,
				fmaf(exp_arg_threshold,
					fmaf(exp_arg_threshold,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f),
			adaptive_coeff_addition_threshold,
			1.0f);

	const float exp_arg =
		fmaf(B_dim, p, 0.0f);
	adaptive_coeff =
		fmaf(-fmaf(exp_arg,
			fmaf(exp_arg,
				fmaf(exp_arg,
					fmaf(exp_arg,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f),
			adaptive_coeff_addition,
			A_dim);

	if (dim < 3) {
		const float adaptive_coeff_arg = adaptive_coeff - 1.0f;
		const float adaptive_coeff_threshold_arg = adaptive_coeff_threshold - 1.0f;
		adaptive_coeff = fmaf(adaptive_coeff_arg, fmaf(adaptive_coeff_arg, fmaf(adaptive_coeff_arg, fmaf(adaptive_coeff_arg, fmaf(adaptive_coeff_arg, 0.164056f, -0.098462f), 0.240884f), -0.351834f), 0.999996f), adaptive_coeff_arg);
		adaptive_coeff_threshold = fmaf(adaptive_coeff_threshold_arg, fmaf(adaptive_coeff_threshold_arg, fmaf(adaptive_coeff_threshold_arg, fmaf(adaptive_coeff_threshold_arg, fmaf(adaptive_coeff_threshold_arg, 0.164056f, -0.098462f), 0.240884f), -0.351834f), 0.999996f), adaptive_coeff_threshold_arg);
	}

	const float exp_argument =
		fmaf(-0.2925f, dim_f, 0.0f);
	const float exp2_exp_arg =
		fmaf(exp_argument *
			0.69314718055994530941723212145818f,
			fmaf(exp_argument *
				0.69314718055994530941723212145818f,
				fmaf(exp_argument *
					0.69314718055994530941723212145818f,
					fmaf(exp_argument *
						0.69314718055994530941723212145818f,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f);

	const float A =
		fmaf(39.995f, exp2_exp_arg, 0.0f);
	const int T = static_cast<int>(fmaf(fmaf(boost::math::pdf(boost::math::normal(0.5f, 0.17f), p), sqrtf(fmaf(dim_f, 1.815f, 0.0f)), 0.0f), fmaf(-(exp2_exp_arg - 1.0f), A, A), 0.0f));

	std::pop_heap(H.begin(), H.end(), ComparePtrND);
	IntervalND* cur = H.back();
	H.pop_back();

	const float x1 = cur->x1;
	const float x2 = cur->x2;
	const float y1 = cur->y1;
	const float y2 = cur->y2;

	float m = fmaf(r_eff, Mmax, 0.0f);
	float tNew =
		step(m, x1, x2, y1, y2, dim_f, r_eff);
	const float bestFOld = bestF;
	const float fNew = evalAt(tNew);

	IntervalND* L = new IntervalND(x1, tNew, y1, fNew);
	IntervalND* Rv = new IntervalND(tNew, x2, fNew, y2);

	L->i1 = t_to_idx(x1);
	L->i2 = t_to_idx(tNew);
	Rv->i1 = t_to_idx(tNew);
	Rv->i2 = t_to_idx(x2);

	L->diam = map.block_diameter(L->i1, L->i2);
	Rv->diam = map.block_diameter(Rv->i1, Rv->i2);

	L->compute_span_level(map);
	Rv->compute_span_level(map);
	L->set_metric(L->diam);
	Rv->set_metric(Rv->diam);

	const float Mloc = fmaxf(L->M, Rv->M);
	update_pockets_and_Mmax(L);
	update_pockets_and_Mmax(Rv);

	const float prevMmax = Mmax;
	if (Mloc > Mmax) {
		Mmax = Mloc;
	}
	m = fmaf(r_eff, Mmax, 0.0f);

	if (adaptive) {
		const float len1 =
			fmaf(tNew, 1.0f, -x1);
		const float len2 =
			fmaf(x2, 1.0f, -tNew);

		if (fmaf(len1, 1.0f, len2) == dmax) {
			dmax = fmaxf(len1, len2);
			for (auto pI : H) {
				const float Ls =
					fmaf(pI->x2, 1.0f, -pI->x1);
				if (Ls > dmax) {
					dmax = Ls;
				}
			}
		}

		if ((p > 0.7f && !(it % 3) && dmax < 0.7f) ||
			p > 0.9f) {
			const float alpha = p * p;
			const float beta =
				fmaf(-alpha, 1.0f, 2.0f);

			const float MULT =
				(1.0f / dmax) * Mmax;
			const float global_coeff =
				fmaf(MULT, r_eff, -MULT);
			const float GF = beta * global_coeff;

			L->ChangeCharacteristic(
				fmaf(GF, len1, fmaf(L->M, alpha, 0.0f)));
			Rv->ChangeCharacteristic(
				fmaf(GF, len2, fmaf(Rv->M, alpha, 0.0f)));

			const size_t sz = H.size();
			RecomputeR_AffineM_AVX2_ND(
				H.data(),
				sz,
				GF,
				alpha);
			std::make_heap(H.begin(), H.end(), ComparePtrND);
		}
		else {
			if (Mloc > prevMmax) {
				L->ChangeCharacteristic(m);
				Rv->ChangeCharacteristic(m);

				if (Mloc > fmaf(adaptive_coeff, prevMmax, -0.03f)) {
					const size_t sz = H.size();
					RecomputeR_ConstM_AVX2_ND(
						H.data(),
						sz,
						m);
					std::make_heap(H.begin(), H.end(), ComparePtrND);
				}
			}
			else {
				L->ChangeCharacteristic(m);
				Rv->ChangeCharacteristic(m);
			}
		}
	}
	else {
		if (Mloc > prevMmax) {
			L->ChangeCharacteristic(m);
			Rv->ChangeCharacteristic(m);

			if (Mloc > fmaf(adaptive_coeff, prevMmax, -0.03f)) {
				const size_t sz = H.size();
				RecomputeR_ConstM_AVX2_ND(
					H.data(),
					sz,
					m);
				std::make_heap(H.begin(), H.end(), ComparePtrND);
			}
		}
		else {
			L->ChangeCharacteristic(m);
			Rv->ChangeCharacteristic(m);
		}
	}

	H.emplace_back(L);
	std::push_heap(H.begin(), H.end(), ComparePtrND);
	H.emplace_back(Rv);
	std::push_heap(H.begin(), H.end(), ComparePtrND);

	_mm_prefetch((const char*)H[0], _MM_HINT_T0);
	_mm_prefetch((const char*)H[1], _MM_HINT_T0);

	IntervalND* const top = H.front();
	const float interval_len = dim > 1 ? fmaf(top->x2, 1.0f, -top->x1) : top->diam;

	if ((dim > 1 ? exp2f((1.0f / dim_f) * log2f(interval_len)) : interval_len) < eps || (it == maxIter)) {
		out_iterations = (size_t)it;
		out_achieved_epsilon = interval_len;
		return;
	}

	if (!(it % T) || staganition) {
		MultiCrossMsg out;
		const unsigned intervals_to_send = static_cast<unsigned>(sqrtf(fmaf(dim_f, 5.5f, 0.0f)));
		out.count = intervals_to_send;

		float* dest = out.intervals;

		unsigned ii = 0;
		while (ii < 3) {
			IntervalND* Tt = H[ii];
			dest[0] = Tt->x1;
			dest[1] = 0.0f;
			dest[2] = Tt->x2;
			dest[3] = 0.0f;
			dest[4] = Tt->R;
			dest += 5;
			++ii;
		}

		const size_t iterations =
			std::bit_width((size_t)(world - 1));
		bool active = true;
		const bool invert_T =
			((int)fmaf((float)exchange_counter_T, 1.0f, 1.0f)) & 1;

		size_t ii2 = 0;
		while (ii2 < iterations && active) {
			const size_t step = 1ULL << ii2;
			const int partner =
				rank ^ (int)step;
			if (partner < world) {
				const bool am_sender =
					((!!(rank & (int)step)) ^ invert_T);
				if (am_sender) {
					g_world->isend(partner, 0, out);
					active = false;
				}
			}
			++ii2;
		}

		++exchange_counter_T;
	}

	if (bestF < fmaf(bestFOld, 2.03f - adaptive_coeff, 0.0f) && !(it % 2)) {
		BestSolutionMsg out;
		out.bestF = bestF;
		out.bestX = bestX;
		out.bestY = bestY;
		out.dim = (unsigned)bestQ.size();
		memcpy(out.bestQ,
			bestQ.data(),
			bestQ.size() * sizeof(float));

		const size_t iterations =
			std::bit_width((size_t)(world - 1));
		bool active = true;
		const bool invert_T =
			((int)fmaf((float)exchange_counter, 1.0f, 1.0f)) & 1;

		size_t ii2 = 0;
		while (ii2 < iterations && active) {
			const size_t step = 1ULL << ii2;
			const int partner =
				rank ^ (int)step;
			if (partner < world) {
				const bool am_sender =
					((!!(rank & (int)step)) ^ invert_T);
				if (am_sender) {
					g_world->isend(partner, 2, out);
					active = false;
				}
			}
			++ii2;
		}

		++exchange_counter;
	}

	while (g_world->iprobe(boost::mpi::any_source, 0)) {
		MultiCrossMsg in;
		g_world->recv(boost::mpi::any_source, 0, in);
		const MultiCrossMsg& mX = in;

		unsigned ii = 0;
		while (ii < mX.count) {
			const float* d = &mX.intervals[ii * 5];
			float sx = d[0];
			float ex = d[2];

			if (ex > sx) {
				alignas(16) float tmp[32];
				float tx;
				float ty;
				map.map01ToPoint(sx, tmp);
				const float y1i = cost(tmp, tx, ty);
				map.map01ToPoint(ex, tmp);
				const float y2i = cost(tmp, tx, ty);

				IntervalND* inj =
					new IntervalND(sx, ex, y1i, y2i);
				inj->i1 = t_to_idx(sx);
				inj->i2 = t_to_idx(ex);
				inj->diam =
					map.block_diameter(inj->i1, inj->i2);
				inj->compute_span_level(map);
				inj->set_metric(inj->diam);
				update_pockets_and_Mmax(inj);
				inj->ChangeCharacteristic(
					fmaf(r_eff, Mmax, 0.0f));

				_mm_prefetch((const char*)H[0], _MM_HINT_T0);
				_mm_prefetch((const char*)H[1], _MM_HINT_T0);

				IntervalND* topH = H.front();

				if (inj->R >
					fmaf(adaptive_coeff, topH->R, -0.03f)) {
					const float poly =
						fmaf(
							fmaf(p, 0.69314718055994530941723212145818f, 0.0f),
							fmaf(
								fmaf(p, 0.69314718055994530941723212145818f, 0.0f),
								fmaf(
									fmaf(p, 0.69314718055994530941723212145818f, 0.0f),
									fmaf(
										fmaf(p, 0.69314718055994530941723212145818f, 0.0f),
										0.00833333377f,
										0.0416666679f),
									0.16666667f),
								0.5f),
							1.0f) -
						1.0f;

					const float kf =
						staganition
						? fmaf(0.5819767068693265f,
							poly,
							0.4f)
						: fmaf(0.3891860241215959f,
							poly,
							0.5f);

					inj->R = fmaf(d[4], kf, 0.0f);
					H.emplace_back(inj);
					std::push_heap(H.begin(), H.end(), ComparePtrND);
				}
			}

			++ii;
		}
	}

	while (g_world->iprobe(boost::mpi::any_source, 2)) {
		BestSolutionMsg bm;
		g_world->recv(boost::mpi::any_source, 2, bm);

		if (bm.bestF < fmaf(bestF, adaptive_coeff, 0.0f) || staganition) {
			alignas(16) float tmp_q[32];
			memcpy(tmp_q, bm.bestQ, bm.dim * sizeof(float));
			const float t_best = map.pointToT(tmp_q);
			const float log_arg = fmaf(dim_f, 1.0f, -1.0f);
			const float adaptive_diameter =
				fmaf(
					fmaf(log_arg,
						fmaf(log_arg,
							fmaf(log_arg,
								fmaf(log_arg,
									fmaf(log_arg,
										0.164056f,
										-0.098462f),
									0.240884f),
								-0.351834f),
							0.999996f),
						log_arg),
					dim > 2 ? fmaf(adaptive_coeff, 0.00475f, 0.0f) : 0.00545f,
					0.0f);

			const float t1 =
				fmaxf(a, fmaf(-adaptive_diameter, 0.2f, t_best));
			const float t2 =
				fminf(b, fmaf(adaptive_diameter, 0.2f, t_best));

			if (t2 > t1) {
				alignas(16) float tq1[32];
				alignas(16) float tq2[32];
				float xx1;
				float yy1;
				float xx2;
				float yy2;

				map.map01ToPoint(t1, tq1);
				const float f1 = cost(tq1, xx1, yy1);

				map.map01ToPoint(t2, tq2);
				const float f2 = cost(tq2, xx2, yy2);

				IntervalND* I =
					new IntervalND(t1, t2, f1, f2);
				I->i1 = t_to_idx(t1);
				I->i2 = t_to_idx(t2);
				I->diam =
					map.block_diameter(I->i1, I->i2);
				I->compute_span_level(map);
				I->set_metric(I->diam);
				update_pockets_and_Mmax(I);
				I->ChangeCharacteristic(
					fmaf(r_eff, Mmax, 0.0f));
				I->R =
					fmaf(I->R, 2.03f - adaptive_coeff, 0.0f);

				H.emplace_back(I);
				std::push_heap(H.begin(), H.end(), ComparePtrND);
			}

			if (bm.bestF < bestF) {
				bestF = bm.bestF;
				bestX = bm.bestX;
				bestY = bm.bestY;
				bestQ.assign(bm.bestQ, bm.bestQ + bm.dim);
			}
		}
	}

	++it;
}

}

static __declspec(noalias) __forceinline float PivotCalculation(
std::vector<IntervalND*>::iterator first,
std::vector<IntervalND*>::iterator last) noexcept {
const auto mid = first + ((last - first) >> 1);
float pivot_value = NAN;

text
if (last - first < 199) {
	pivot_value = (*mid)->R;
}
else {
	if ((*first)->R < (*mid)->R) {
		if ((*mid)->R < (*last)->R) {
			pivot_value = (*mid)->R;
		}
		else {
			pivot_value = std::max((*first)->R, (*last)->R);
		}
	}
	else {
		if ((*first)->R < (*last)->R) {
			pivot_value = (*first)->R;
		}
		else {
			pivot_value = std::max((*mid)->R, (*last)->R);
		}
	}
}

return pivot_value;

}

static __declspec(noalias) __forceinline void HoaraSort(
std::vector<IntervalND*>::iterator first,
std::vector<IntervalND*>::iterator last) noexcept {
if (first >= last) {
return;
}

text
const float pivot_value = PivotCalculation(first, last);
auto left = first;
auto right = last;

do {
	while (left < last && (*left)->R < pivot_value) {
		++left;
	}
	while (right > first && (*right)->R > pivot_value) {
		--right;
	}

	if ((*left)->R == (*right)->R && left != right) {
		if ((*left)->R < (*(left + 1))->R) {
			++left;
		}
		else {
			--right;
		}
	}

	std::iter_swap(left, right);
} while (left != right);

if (last - first < 199) {
	HoaraSort(first, right);
	HoaraSort(left + 1, last);
}
else {
	oneapi::tbb::parallel_invoke(
		[&first, &right]() { HoaraSort(first, right); },
		[&left, &last]() { HoaraSort(left + 1, last); });
}

}

extern "C" __declspec(dllexport) __declspec(noalias)
void AGP_Manip2D(int nSegments,
bool variableLengths,
float minTheta,
float targetX,
float targetY,
int maxIterPerBranch,
float r,
bool adaptiveMode,
float epsilon,
unsigned int seed,
float baseLength,
float stretchFactor,
float** out_bestQ,
size_t* out_bestQLen,
float* out_bestX,
float* out_bestY,
float* out_bestF,
size_t* out_iterations,
float* out_achieved_epsilon) noexcept {
Slab* slab = tls.local();
slab->current = slab->base;

text
while (g_world->iprobe(boost::mpi::any_source, 0)) {
	MultiCrossMsg dummy;
	g_world->recv(boost::mpi::any_source, 0, dummy);
}
while (g_world->iprobe(boost::mpi::any_source, 2)) {
	BestSolutionMsg dummy;
	g_world->recv(boost::mpi::any_source, 2, dummy);
}

const int dim =
	nSegments + (variableLengths ? nSegments : 0);

g_mc.permCache.resize((size_t)dim);
int i = 0;
while (i < dim) {
	g_mc.permCache[i] = i;
	++i;
}

unsigned s = g_mc.baseSeed;
i = dim - 1;
while (i > 0) {
	s ^= s << 13;
	s ^= s >> 17;
	s ^= s << 5;
	const unsigned j = s % (unsigned)(i + 1);
	std::swap(g_mc.permCache[i], g_mc.permCache[j]);
	--i;
}

g_mc.invMaskCache.resize((size_t)dim);
int k = 0;
while (k < dim) {
	s ^= s << 13;
	s ^= s >> 17;
	s ^= s << 5;
	g_mc.invMaskCache[k] = (unsigned long long)s;
	++k;
}

std::vector<float, boost::alignment::aligned_allocator<float, 16u>> low;
std::vector<float, boost::alignment::aligned_allocator<float, 16u>> high;
low.reserve((size_t)dim);
high.reserve((size_t)dim);

i = 0;
while (i < nSegments) {
	low.emplace_back(
		i == 0
		? -1.0471975511965977461542144610932f
		: -2.6179938779914943653855361527329f);
	high.emplace_back(
		2.6179938779914943653855361527329f);
	++i;
}

if (variableLengths) {
	i = 0;
	const float lengthLower = baseLength / stretchFactor;
	const float lengthUpper = baseLength * stretchFactor;
	while (i < nSegments) {
		low.emplace_back(lengthLower);
		high.emplace_back(lengthUpper);
		++i;
	}
}

const ManipCost cost(nSegments,
	variableLengths,
	targetX,
	targetY,
	minTheta);

const int rank = g_world->rank();
const int world = g_world->size();

std::vector<float, boost::alignment::aligned_allocator<float, 16u>> bestQ;
float bestF = FLT_MAX;
float bx = 0.0f;
float by = 0.0f;

const float exp_arg_lvls = fmaf(-static_cast<float>(dim), 0.455f, 0.0f);
const float exp_arg_lvl0 = fmaf(-static_cast<float>(dim), 0.08f, 0.0f);
const int peanoLevels = static_cast<int>(fminf(fmaf(fmaf(exp_arg_lvls, fmaf(exp_arg_lvls, fmaf(exp_arg_lvls, fmaf(exp_arg_lvls, 0.00833333377f, 0.0416666679f), 0.16666667f), 0.5f), 1.0f), 30.0f, 6.125f), 13.0f));
const int levels0 = static_cast<int>(fminf(fmaf(-fminf(fmaf(fmaf(exp_arg_lvl0, fmaf(exp_arg_lvl0, fmaf(exp_arg_lvl0, fmaf(exp_arg_lvl0, 0.00833333377f, 0.0416666679f), 0.16666667f), 0.5f), 1.0f), 21.7f, -11.3f), 0.0f), 1.0f, static_cast<float>(peanoLevels)), 8.0f));

const MortonND map0(dim, levels0, low.data(), high.data(), g_mc);

std::vector<IntervalND*,
	boost::alignment::aligned_allocator<IntervalND*, 16u>>
	H_coarse;
std::vector<float, boost::alignment::aligned_allocator<float, 16u>>
	bestQ_coarse;

float bestF_coarse = FLT_MAX;
float bx_coarse = 0.0f;
float by_coarse = 0.0f;

size_t total_oi = 0u;
float total_oe = 0.0f;

size_t oi = 0u;
float oe = 0.0f;

const float M_arg = ldexpf(1.0f, -levels0);
const float M_prior =
	fmaf(fmaf(variableLengths ? 2.8284271f : 2.0f, static_cast<float>(nSegments),
		0.0f),
		fmaf(M_arg, fmaf(M_arg, fmaf(M_arg, fmaf(M_arg, fmaf(M_arg, 0.164056f, -0.098462f), 0.240884f), -0.351834f), 0.999996f), M_arg),
		0.0f);

agp_run_branch_mpi(map0,
	cost,
	maxIterPerBranch >> 1,
	r,
	adaptiveMode,
	epsilon,
	seed,
	H_coarse,
	bestQ_coarse,
	bestF_coarse,
	bx_coarse,
	by_coarse,
	oi,
	oe,
	M_prior);

total_oi += oi;
total_oe = oe;

if (bestF_coarse < bestF) {
	bestF = bestF_coarse;
	bestQ = std::move(bestQ_coarse);
	bx = bx_coarse;
	by = by_coarse;
}

while (g_world->iprobe(boost::mpi::any_source, 0)) {
	MultiCrossMsg dummy;
	g_world->recv(boost::mpi::any_source, 0, dummy);
}
while (g_world->iprobe(boost::mpi::any_source, 2)) {
	BestSolutionMsg dummy;
	g_world->recv(boost::mpi::any_source, 2, dummy);
}

const MortonND map1(dim,
	peanoLevels,
	low.data(),
	high.data(),
	g_mc);

std::vector<IntervalND*,
	boost::alignment::aligned_allocator<IntervalND*, 16u>>
	H_fine;
std::vector<float, boost::alignment::aligned_allocator<float, 16u>>
	bestQ_fine = bestQ;

float bestF_fine = bestF;
float bx_fine = bx;
float by_fine = by;

size_t oi_fine = 0u;
float oe_fine = 0.0f;

const float M_prior_fine =
	fmaf(1.0f / static_cast<float>(peanoLevels),
		fmaf(static_cast<float>(levels0), M_prior, 0.0f),
		0.0f);

HoaraSort(H_coarse.begin(), H_coarse.end() - 1);

const float inv_dim =
	1.0f / (float)(dim + 1);
size_t ci =
	(size_t)fmaf(
		(float)H_coarse.size(),
		fmaf(inv_dim,
			fmaf(inv_dim,
				fmaf(inv_dim,
					fmaf(inv_dim,
						0.00833333377f,
						0.0416666679f),
					0.16666667f),
				0.5f),
			1.0f) +
		1.0f,
		-0.7f);

while (ci < H_coarse.size()) {
	const IntervalND* C = H_coarse[ci];

	alignas(16) float q1[32];
	alignas(16) float q2[32];
	float x1;
	float y1;
	float x2;
	float y2;

	map1.map01ToPoint(C->x1, q1);
	const float f1 = cost(q1, x1, y1);

	map1.map01ToPoint(C->x2, q2);
	const float f2 = cost(q2, x2, y2);

	IntervalND* I =
		new IntervalND(C->x1, C->x2, f1, f2);
	I->i1 = (unsigned long long)fmaf(
		C->x1, (float)map1.scale, 0.0f);
	I->i2 = (unsigned long long)fmaf(
		C->x2, (float)map1.scale, 0.0f);
	I->diam =
		map1.block_diameter(I->i1, I->i2);
	I->set_metric(I->diam);

	H_fine.emplace_back(I);

	if (f1 < bestF_fine) {
		bestF_fine = f1;
		bestQ_fine.assign(q1, q1 + dim);
		bx_fine = x1;
		by_fine = y1;
	}
	if (f2 < bestF_fine) {
		bestF_fine = f2;
		bestQ_fine.assign(q2, q2 + dim);
		bx_fine = x2;
		by_fine = y2;
	}

	++ci;
}

std::make_heap(H_fine.begin(), H_fine.end(), ComparePtrND);

agp_run_branch_mpi(map1,
	cost,
	maxIterPerBranch >> 1,
	r,
	adaptiveMode,
	epsilon,
	seed,
	H_fine,
	bestQ_fine,
	bestF_fine,
	bx_fine,
	by_fine,
	oi_fine,
	oe_fine,
	M_prior_fine);

total_oi += oi_fine;
total_oe = oe_fine;

if (bestF_fine < bestF) {
	bestF = bestF_fine;
	bestQ = std::move(bestQ_fine);
	bx = bx_fine;
	by = by_fine;
}

BestSolutionMsg best;
best.bestF = bestF;
best.bestX = bx;
best.bestY = by;
best.dim = (unsigned)bestQ.size();
memcpy(best.bestQ,
	bestQ.data(),
	(size_t)best.dim * sizeof(float));

const size_t iterations =
	std::bit_width((size_t)(world - 1));
bool active = true;

size_t itx = 0;
while (itx < iterations && active) {
	const size_t step = 1ULL << itx;
	const int partner =
		rank ^ (int)step;
	if (partner < world) {
		const bool am_sender =
			(rank & (int)step) != 0;
		if (am_sender) {
			g_world->isend(partner, 3, best);
			active = false;
		}
		else {
			BestSolutionMsg in;
			g_world->recv(partner, 3, in);
			if (in.bestF < best.bestF) {
				best = in;
			}
		}
	}
	++itx;
}

if (rank == 0) {
	*out_bestQLen = (size_t)best.dim;
	*out_bestQ =
		(float*)CoTaskMemAlloc(sizeof(float) *
			(*out_bestQLen));
	memcpy(*out_bestQ,
		best.bestQ,
		sizeof(float) * (*out_bestQLen));
	*out_bestX = best.bestX;
	*out_bestY = best.bestY;
	*out_bestF = best.bestF;
	*out_iterations = total_oi;
	*out_achieved_epsilon = total_oe;
}

}

extern "C" __declspec(dllexport) __declspec(noalias) __forceinline int
AgpInit(int peanoLevel,
float a,
float b,
float c,
float d) noexcept {
g_env = new boost::mpi::environment();
g_world = new boost::mpi::communicator();

text
_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);

const int rank = g_world->rank();
const int world_size = g_world->size();

if (world_size == 4) {
	new (&gActiveMap)
		Peano2DMap(peanoLevel, a, b, c, d, rank & 3);
}

g_mc.baseSeed =
	(unsigned)fmaf(0x9E3779B9u,
		(float)rank,
		0x9E3779B9u);

if (rank == 0) {
	wchar_t buf[MAX_PATH]{};
	GetModuleFileNameW(nullptr, buf, MAX_PATH);
	std::wstring ws(buf);
	auto pos = ws.find_last_of(L"\\/");
	if (pos != std::wstring::npos) {
		ws.resize(pos);
	}

	int n = WideCharToMultiByte(
		CP_UTF8,
		0,
		ws.c_str(),
		-1,
		nullptr,
		0,
		nullptr,
		nullptr);

	g_exeDirCache.resize(n, '\0');
	WideCharToMultiByte(CP_UTF8,
		0,
		ws.c_str(),
		-1,
		g_exeDirCache.data(),
		n,
		nullptr,
		nullptr);

	if (!g_exeDirCache.empty() &&
		g_exeDirCache.back() == '\0') {
		g_exeDirCache.pop_back();
	}

	g_pyInterpreter = new pybind11::scoped_interpreter{};
	pybind11::module_ sys =
		pybind11::module_::import("sys");
	pybind11::list path = sys.attr("path");

	const std::string& exeDir = g_exeDirCache;
	path.attr("insert")(0,
		pybind11::str(
			exeDir +
			"\\env\\Lib\\site-packages"));
	path.attr("insert")(0,
		pybind11::str(
			exeDir + "\\env\\Scripts"));
	path.attr("insert")(0,
		pybind11::str(exeDir + "\\env"));
	path.attr("append")(pybind11::str(exeDir));

	pybind11::module_::import("warnings")
		.attr("filterwarnings")("ignore");

	g_pyOptimizerBridge =
		new pybind11::module_(
			pybind11::module_::import(
				"optimizer_bridge"));
}

return rank;

}

static __declspec(noalias) __forceinline float ShekelFunc(
float x,
float seed) noexcept {
int i = 0;
float st = seed;
float r1;
float r2;
float res = 0.0f;

#pragma loop ivdep
while (i < 10) {
XOR_RAND(st, r1);
const float xp = fmaf(-r1, 10.0f, x);
XOR_RAND(st, r1);
XOR_RAND(st, r2);
float d =
fmaf(fmaf(r1, 20.0f, 5.0f), xp * xp,
fmaf(r2, 0.2f, 1.0f));
d = copysignf(fmaxf(fabsf(d), FLT_MIN), d);
res -= (1.0f / d) * 1.0f;
++i;
}

return res;

}

static __declspec(noalias) __forceinline float RastriginFunc(
float x1,
float x2) noexcept {
const float t = fmaf(x1, x1, x2 * x2);
float c1;
float c2;
FABE13_COS(6.28318530717958647692f * x1, c1);
FABE13_COS(6.28318530717958647692f * x2, c2);
return (t - fmaf(c1 + c2, 10.0f, -14.6f)) *
fmaf(-t, 0.25f, 18.42f);
}

static __declspec(noalias) __forceinline float HillFunc(
float x,
float seed) noexcept {
int j = 0;
alignas(16) float ang[14];

text
const float st_ang =
	6.28318530717958647692f * x;
while (j < 14) {
	ang[j] = st_ang * (float)(j + 1);
	++j;
}

alignas(16) float sv[14];
alignas(16) float cv[14];
FABE13_SINCOS(ang, sv, cv, 14);

float state = seed;
float r1;
float r2;
XOR_RAND(state, r1);

float res = fmaf(r1, 2.0f, -1.1f);

--j;

#pragma loop ivdep
while (j >= 0) {
XOR_RAND(state, r1);
XOR_RAND(state, r2);
res += fmaf(fmaf(r1, 2.0f, -1.1f), sv[j],
fmaf(r2, 2.0f, -1.1f) * cv[j]);
--j;
}

return res;

}

static __declspec(noalias) __forceinline float GrishaginFunc(
float x1,
float x2,
float seed) noexcept {
int j = 0;

text
alignas(16) float aj[8];
alignas(16) float ak[8];

#pragma loop ivdep
while (j < 8) {
const float pj =
3.14159265358979323846f * (float)(j + 1);
aj[j] = pj * x1;
ak[j] = pj * x2;
++j;
}

text
alignas(16) float sj[8];
alignas(16) float cj[8];
alignas(16) float sk[8];
alignas(16) float ck[8];

FABE13_SINCOS(aj, sj, cj, 8);
FABE13_SINCOS(ak, sk, ck, 8);

--j;
float p1 = 0.0f;
float p2 = 0.0f;
float st = seed;
float r1;
float r2;

#pragma loop ivdep
while (j >= 0) {
size_t k2 = 0u;
while (k2 < 8u) {
const float s = sj[j] * sj[j];
const float c = ck[k2] * ck[k2];

text
		XOR_RAND_GRSH(st, r1);
		XOR_RAND_GRSH(st, r2);
		p1 = fmaf(r1, s, fmaf(r2, c, p1));

		XOR_RAND_GRSH(st, r1);
		XOR_RAND_GRSH(st, r2);
		p2 = fmaf(-r1, c, fmaf(r2, s, p2));
		++k2;
	}
	--j;
}

return -sqrtf(fmaf(p1, p1, p2 * p2));

}

extern "C" __declspec(dllexport) __declspec(noalias)
void AGP_1D(float global_iterations,
float a,
float b,
float r,
bool mode,
float epsilon,
float seed,
float** out_data,
size_t* out_len) noexcept {
Slab* slab = tls.local();
slab->current = slab->base;

text
int counter = 0;
const float initial_length = b - a;
float dmax = initial_length;

const float threshold_03 =
	0.3f * initial_length;
const float inv_threshold_03 =
	1.0f / threshold_03;

const float start_val = ShekelFunc(a, seed);
float best_f = ShekelFunc(b, seed);

float x_Rmax_1 = a;
float x_Rmax_2 = b;
float y_Rmax_1 = start_val;
float y_Rmax_2 = best_f;

std::vector<float, boost::alignment::aligned_allocator<float, 16u>> Extr;
std::vector<Interval1D*,
	boost::alignment::aligned_allocator<Interval1D*, 16u>>
	R;

Extr.reserve((size_t)global_iterations << 2u);
R.reserve((size_t)global_iterations << 1u);

R.emplace_back(
	new Interval1D(a, b, start_val, best_f, 1.0f));

float Mmax = R.front()->M;
float m = r * Mmax;

while (true) {
	const float new_point =
		step(m,
			x_Rmax_1,
			x_Rmax_2,
			y_Rmax_1,
			y_Rmax_2,
			1.0f,
			r);
	const float new_value = ShekelFunc(new_point, seed);

	if (new_value < best_f) {
		best_f = new_value;
		Extr.emplace_back(best_f);
		Extr.emplace_back(new_point);
	}

	std::pop_heap(R.begin(), R.end(), ComparePtr1D);
	const Interval1D* pro = R.back();

	const float new_x1 = pro->x1;
	const float new_x2 = pro->x2;
	const float len2 = new_x2 - new_point;
	const float len1 = new_point - new_x1;

	const float interval_len =
		(len1 < len2 ? len1 : len2);

	if (++counter == (int)global_iterations ||
		interval_len < epsilon) {
		Extr.emplace_back((float)counter);
		Extr.emplace_back(interval_len);

		*out_len = Extr.size();
		*out_data = (float*)CoTaskMemAlloc(
			sizeof(float) * (*out_len));
		memcpy(*out_data,
			Extr.data(),
			sizeof(float) * (*out_len));
		return;
	}

	Interval1D* curr =
		new Interval1D(new_x1,
			new_point,
			pro->y1,
			new_value,
			1.0f);
	Interval1D* curr1 =
		new Interval1D(new_point,
			new_x2,
			new_value,
			pro->y2,
			1.0f);

	const float currM =
		curr->M > curr1->M ? curr->M : curr1->M;

	const size_t r_size = R.size();

	if (mode) {
		if (len2 + len1 == dmax) {
			dmax = len2 > len1 ? len2 : len1;
			for (auto pI : R) {
				const float L = pI->x2 - pI->x1;
				if (L > dmax) {
					dmax = L;
				}
			}
		}

		if ((threshold_03 > dmax && !(counter % 3)) ||
			10.0f * dmax < initial_length) {
			if (currM > Mmax) {
				Mmax = currM;
				m = r * Mmax;
			}

			const float progress =
				fmaf(-inv_threshold_03, dmax, 1.0f);
			const float alpha = progress * progress;
			const float betta = 2.0f - alpha;

			const float MULT =
				(1.0f / dmax) * Mmax;
			const float global_coeff =
				fmaf(MULT, r, -MULT);
			const float GF = betta * global_coeff;

			curr->ChangeCharacteristic(
				fmaf(GF, len1, curr->M * alpha));
			curr1->ChangeCharacteristic(
				fmaf(GF, len2, curr1->M * alpha));

			RecomputeR_AffineM_AVX2_1D(
				R.data(),
				r_size,
				GF,
				alpha);
			std::make_heap(R.begin(), R.end(), ComparePtr1D);
		}
		else {
			if (currM > Mmax) {
				if (currM < 1.15f * Mmax) {
					Mmax = currM;
					m = r * Mmax;
					curr->ChangeCharacteristic(m);
					curr1->ChangeCharacteristic(m);
				}
				else {
					Mmax = currM;
					m = r * Mmax;
					curr->ChangeCharacteristic(m);
					curr1->ChangeCharacteristic(m);
					RecomputeR_ConstM_AVX2_1D(
						R.data(),
						r_size,
						m);
					std::make_heap(R.begin(),
						R.end(),
						ComparePtr1D);
				}
			}
			else {
				curr->ChangeCharacteristic(m);
				curr1->ChangeCharacteristic(m);
			}
		}
	}
	else {
		if (currM > Mmax) {
			if (currM < 1.15f * Mmax) {
				Mmax = currM;
				m = r * Mmax;
				curr->ChangeCharacteristic(m);
				curr1->ChangeCharacteristic(m);
			}
			else {
				Mmax = currM;
				m = r * Mmax;
				curr->ChangeCharacteristic(m);
				curr1->ChangeCharacteristic(m);
				RecomputeR_ConstM_AVX2_1D(
					R.data(),
					r_size,
					m);
				std::make_heap(
					R.begin(), R.end(), ComparePtr1D);
			}
		}
		else {
			curr->ChangeCharacteristic(m);
			curr1->ChangeCharacteristic(m);
		}
	}

	R.back() = curr;
	std::push_heap(R.begin(), R.end(), ComparePtr1D);
	R.emplace_back(curr1);
	std::push_heap(R.begin(), R.end(), ComparePtr1D);

	const Interval1D* top = R.front();
	x_Rmax_1 = top->x1;
	x_Rmax_2 = top->x2;
	y_Rmax_1 = top->y1;
	y_Rmax_2 = top->y2;
}

}

extern "C" __declspec(dllexport) __declspec(noalias)
void AGP_2D(float N,
float global_iterations,
float a,
float b,
float c,
float d,
float r,
bool mode,
float epsilon,
float seed,
float** out_data,
size_t* out_len) noexcept {
Slab* slab = tls.local();
slab->current = slab->base;

text
int counter = 0;
int no_improve = 0;

const int rank = g_world->rank();
const int world_size = g_world->size();

while (g_world->iprobe(boost::mpi::any_source, 0)) {
	MultiCrossMsg dummy;
	g_world->recv(boost::mpi::any_source, 0, dummy);
}

const float inv_divider =
	ldexpf(1.0f, -((gActiveMap.levels << 1) + 1));
const float x_addition =
	(b - a) * inv_divider;
const float y_addition =
	(d - c) * inv_divider;

const float true_start = a + x_addition;
const float true_end = b - x_addition;

float x_Rmax_1 = true_start;
float x_Rmax_2 = true_end;
const float initial_length =
	x_Rmax_2 - x_Rmax_1;
float dmax = initial_length;

const float threshold_03 =
	0.3f * initial_length;
const float inv_threshold_03 =
	1.0f / threshold_03;

const float start_val =
	(rank % 3)
	? RastriginFunc(true_end, d - y_addition)
	: RastriginFunc(true_start, c + y_addition);
float best_f =
	(rank % 2)
	? RastriginFunc(true_start, d - y_addition)
	: RastriginFunc(true_end, c + y_addition);

float y_Rmax_1 = start_val;
float y_Rmax_2 = best_f;

std::vector<float, boost::alignment::aligned_allocator<float, 16u>> Extr;
std::vector<Interval1D* __restrict,
	boost::alignment::aligned_allocator<
	Interval1D* __restrict, 16u>>
	R;

Extr.reserve((size_t)global_iterations << 2u);
R.reserve((size_t)global_iterations << 1u);

R.emplace_back(new Interval1D(true_start,
	true_end,
	start_val,
	best_f,
	2.0f));

const Interval1D* __restrict top_ptr = R.front();

float Mmax = R.front()->M;
float m = r * Mmax;

while (true) {
	const float interval_len =
		x_Rmax_2 - x_Rmax_1;
	const bool stagnation =
		no_improve > 100 && counter > 270;
	const float p =
		fmaf(-1.0f / initial_length, dmax, 1.0f);

	while (g_world->iprobe(boost::mpi::any_source, 0)) {
		MultiCrossMsg in;
		g_world->recv(boost::mpi::any_source, 0, in);
		const MultiCrossMsg& mX = in;

		unsigned ii = 0;
		while (ii < mX.count) {
			const float* d2 = &mX.intervals[ii * 5];
			float sx = d2[0];
			float ex = d2[2];

			if (ex > sx) {
				Interval1D* __restrict injected =
					new Interval1D(sx,
						ex,
						RastriginFunc(d2[0], d2[1]),
						RastriginFunc(d2[2], d2[3]),
						2.0f);
				injected->ChangeCharacteristic(m);

				if (injected->R > 1.15f * top_ptr->R) {
					const float poly =
						fmaf(p *
							0.69314718055994530941723212145818f,
							fmaf(p *
								0.69314718055994530941723212145818f,
								fmaf(p *
									0.69314718055994530941723212145818f,
									fmaf(p *
										0.69314718055994530941723212145818f,
										0.00833333377f,
										0.0416666679f),
									0.16666667f),
								0.5f),
							1.0f) -
						1.0f;

					const float k =
						stagnation
						? fmaf(0.5819767068693265f,
							poly,
							0.3f)
						: fmaf(0.3491860241215959f,
							poly,
							0.6f);

					injected->R = d2[4] * k;
					R.emplace_back(injected);
					std::push_heap(
						R.begin(), R.end(), ComparePtr1D);
				}
			}

			++ii;
		}
	}

	const int T =
		(int)fmaf(
			-(fmaf(p *
				0.69314718055994530941723212145818f,
				fmaf(p *
					0.69314718055994530941723212145818f,
					fmaf(p *
						0.69314718055994530941723212145818f,
						fmaf(p *
							0.69314718055994530941723212145818f,
							0.00833333377f,
							0.0416666679f),
						0.16666667f),
					0.5f),
				1.0f) -
				1.0f),
			264.0f,
			277.0f);

	const bool want_term =
		interval_len < epsilon ||
		counter == (int)global_iterations;

	if (!(++counter % T) || stagnation) {
		if (!want_term) {
			MultiCrossMsg out;
			float s_x1;
			float s_x2;
			float e_x1;
			float e_x2;

			HitTest2D_analytic(top_ptr->x1, s_x1, s_x2);
			HitTest2D_analytic(top_ptr->x2, e_x1, e_x2);

			out.intervals[0] = s_x1;
			out.intervals[1] = s_x2;
			out.intervals[2] = e_x1;
			out.intervals[3] = e_x2;
			out.intervals[4] = top_ptr->R;
			out.count = 1;

			int i2 = 0;
			while (i2 < world_size) {
				if (i2 != rank) {
					g_world->isend(i2, 0, out);
				}
				++i2;
			}
		}
	}

	if (want_term) {
		if (!rank) {
			Extr.emplace_back((float)counter);
			Extr.emplace_back(interval_len);

			*out_len = Extr.size();
			*out_data =
				(float* __restrict)CoTaskMemAlloc(
					sizeof(float) * (*out_len));
			memcpy(*out_data,
				Extr.data(),
				sizeof(float) * (*out_len));
		}
		return;
	}

	const float new_point =
		step(m,
			x_Rmax_1,
			x_Rmax_2,
			y_Rmax_1,
			y_Rmax_2,
			2.0f,
			r);

	float new_x1_val;
	float new_x2_val;
	HitTest2D_analytic(new_point, new_x1_val, new_x2_val);

	const float new_value =
		RastriginFunc(new_x1_val, new_x2_val);

	if (new_value < best_f) {
		best_f = new_value;
		Extr.emplace_back(best_f);
		Extr.emplace_back(new_x1_val);
		Extr.emplace_back(new_x2_val);
		no_improve = 0;
	}
	else {
		++no_improve;
	}

	std::pop_heap(R.begin(), R.end(), ComparePtr1D);
	Interval1D* __restrict intermediate = R.back();

	const float segment_x1 = intermediate->x1;
	const float segment_x2 = intermediate->x2;
	const float len2 = segment_x2 - new_point;
	const float len1 = new_point - segment_x1;

	Interval1D* __restrict curr =
		new Interval1D(segment_x1,
			new_point,
			intermediate->y1,
			new_value,
			2.0f);
	Interval1D* __restrict curr1 =
		new Interval1D(new_point,
			segment_x2,
			new_value,
			intermediate->y2,
			2.0f);

	const float currM =
		(std::max)(curr->M, curr1->M);
	const size_t r_size = R.size();

	if (mode) {
		if (len2 + len1 == dmax) {
			dmax = (std::max)(len1, len2);
			for (auto pI : R) {
				const float L = pI->x2 - pI->x1;
				if (L > dmax) {
					dmax = L;
				}
			}
		}

		if ((threshold_03 > dmax && !(counter % 3)) ||
			10.0f * dmax < initial_length) {
			if (currM > Mmax) {
				Mmax = currM;
				m = r * Mmax;
			}

			const float progress =
				fmaf(-inv_threshold_03, dmax, 1.0f);
			const float alpha = progress * progress;
			const float betta = 2.0f - alpha;

			const float MULTIPLIER =
				(1.0f / dmax) * Mmax;
			const float global_coeff =
				fmaf(MULTIPLIER, r, -MULTIPLIER);
			const float GLOBAL_FACTOR =
				betta * global_coeff;

			curr->ChangeCharacteristic(
				fmaf(GLOBAL_FACTOR, len1, curr->M * alpha));
			curr1->ChangeCharacteristic(
				fmaf(GLOBAL_FACTOR, len2, curr1->M * alpha));

			RecomputeR_AffineM_AVX2_1D(
				R.data(),
				r_size,
				GLOBAL_FACTOR,
				alpha);
			std::make_heap(
				R.begin(), R.end(), ComparePtr1D);
		}
		else {
			if (currM > Mmax) {
				if (currM < 1.15f * Mmax) {
					Mmax = currM;
					m = r * Mmax;
					curr->ChangeCharacteristic(m);
					curr1->ChangeCharacteristic(m);
				}
				else {
					Mmax = currM;
					m = r * Mmax;
					curr->ChangeCharacteristic(m);
					curr1->ChangeCharacteristic(m);
					RecomputeR_ConstM_AVX2_1D(
						R.data(),
						r_size,
						m);
					std::make_heap(
						R.begin(), R.end(), ComparePtr1D);
				}
			}
			else {
				curr->ChangeCharacteristic(m);
				curr1->ChangeCharacteristic(m);
			}
		}
	}
	else {
		if (currM > Mmax) {
			if (currM < 1.15f * Mmax) {
				Mmax = currM;
				m = r * Mmax;
				curr->ChangeCharacteristic(m);
				curr1->ChangeCharacteristic(m);
			}
			else {
				Mmax = currM;
				m = r * Mmax;
				curr->ChangeCharacteristic(m);
				curr1->ChangeCharacteristic(m);
				RecomputeR_ConstM_AVX2_1D(
					R.data(),
					r_size,
					m);
				std::make_heap(
					R.begin(), R.end(), ComparePtr1D);
			}
		}
		else {
			curr->ChangeCharacteristic(m);
			curr1->ChangeCharacteristic(m);
		}
	}

	R.back() = curr;
	std::push_heap(R.begin(), R.end(), ComparePtr1D);
	R.emplace_back(curr1);
	std::push_heap(R.begin(), R.end(), ComparePtr1D);

	top_ptr = R.front();
	x_Rmax_1 = top_ptr->x1;
	x_Rmax_2 = top_ptr->x2;
	y_Rmax_1 = top_ptr->y1;
	y_Rmax_2 = top_ptr->y2;
}

}

__declspec(align(16)) struct RunParams final sealed{
int nSegments;
unsigned varLen;
float minTheta;
float tx;
float ty;
int maxIter;
float r;
unsigned adaptive;
float eps;
unsigned seed;
float baseLength;
float stretchFactor;

text
template <typename Archive>
void serialize(Archive& ar, unsigned int) {
	ar& nSegments& varLen& minTheta& tx& ty& maxIter& r&
			adaptive& eps& seed& baseLength& stretchFactor;
}

};

extern "C" __declspec(dllexport) __declspec(noalias) __forceinline void
AgpStartManipND(int nSegments,
bool variableLengths,
float minTheta,
float targetX,
float targetY,
int maxIterPerBranch,
float r,
bool adaptiveMode,
float epsilon,
unsigned int seed,
float baseLength,
float stretchFactor) noexcept {
RunParams p;
p.nSegments = nSegments;
p.varLen = (unsigned)variableLengths;
p.minTheta = minTheta;
p.tx = targetX;
p.ty = targetY;
p.maxIter = maxIterPerBranch;
p.r = r;
p.adaptive = (unsigned)adaptiveMode;
p.eps = epsilon;
p.seed = seed;
p.baseLength = baseLength;
p.stretchFactor = stretchFactor;

text
int i = 1;
const int world = g_world->size();
while (i < world) {
	g_world->isend(i, 1, p);
	++i;
}

}

extern "C" __declspec(dllexport) __declspec(noalias) __forceinline void
AgpWaitStartAndRun() noexcept {
RunParams p;
float* __restrict q;
size_t qlen;
float bx;
float by;
float bf;
size_t oi;
float oa;

text
while (true) {
	if (g_world->iprobe(0, 1)) {
		g_world->recv(0, 1, p);
		AGP_Manip2D(p.nSegments,
			(bool)p.varLen,
			p.minTheta,
			p.tx,
			p.ty,
			p.maxIter,
			p.r,
			(bool)p.adaptive,
			p.eps,
			p.seed,
			p.baseLength,
			p.stretchFactor,
			&q,
			&qlen,
			&bx,
			&by,
			&bf,
			&oi,
			&oa);
	}
	Sleep(0);
}

}

extern "C" __declspec(dllexport) __declspec(noalias) __forceinline void
AgpWaitStartAndRun2D() noexcept {
int dummy;
float* __restrict buf;
size_t len;

text
while (true) {
	if (g_world->iprobe(0, 1)) {
		g_world->recv(0, 1, dummy);
		AGP_2D(2.0f,
			10000.0f,
			-2.2f,
			1.8f,
			-2.2f,
			1.8f,
			2.5f,
			false,
			0.00001f,
			(float)GetTickCount(),
			&buf,
			&len);
	}
	Sleep(0);
}

}

extern "C" __declspec(dllexport) __declspec(noalias) __forceinline void
AgpStartWorkers() noexcept {
int i = 1;
const int world = g_world->size();
while (i < world) {
g_world->isend(i, 1, 0);
++i;
}
}

extern "C" __declspec(dllexport) __declspec(noalias) __forceinline void
AGP_Free(float* p) noexcept {
CoTaskMemFree(p);
}

extern "C" __declspec(dllexport) __declspec(noalias)
void RunPythonOptimization(
const char* backend,
int n_seg,
bool var_len,
float min_theta,
float tx,
float ty,
int levels,
int max_iter,
float r_param,
float eps,
bool adaptive,
float baseLength,
float stretchFactor,
float* __restrict out_bestF,
float* __restrict out_bestX,
float* __restrict out_bestY,
float* __restrict out_angles,
float* __restrict out_lengths,
int* __restrict out_iterations,
float* __restrict out_eps,
float* __restrict out_micros) noexcept {
pybind11::gil_scoped_acquire gil;

text
pybind11::dict result;

if (backend && strcmp(backend, "optuna") == 0) {
	result = (*g_pyOptimizerBridge)
		.attr("run_optuna")(n_seg,
			var_len,
			min_theta,
			tx,
			ty,
			max_iter,
			baseLength,
			stretchFactor);
}
else {
	result = (*g_pyOptimizerBridge)
		.attr("run_iopt")(n_seg,
			var_len,
			min_theta,
			tx,
			ty,
			levels,
			max_iter,
			r_param,
			eps,
			adaptive,
			baseLength,
			stretchFactor);
}

*out_bestF = result["BEST_F"].cast<float>();
*out_bestX = result["BEST_X"].cast<float>();
*out_bestY = result["BEST_Y"].cast<float>();
*out_iterations = result["ITERATIONS"].cast<int>();
*out_eps = result["EPS"].cast<float>();
*out_micros = result["TIME"].cast<float>();

std::vector<float> angles_vec =
	result["ANGLES"].cast<std::vector<float>>();
std::vector<float> lengths_vec =
	result["LENGTHS"].cast<std::vector<float>>();

const size_t n = (size_t)n_seg;

if (out_angles) {
	const size_t m = angles_vec.size();
	const size_t limit = m < n ? m : n;
	const float* __restrict src =
		angles_vec.data();

	size_t i = 0;

#pragma loop ivdep
while (i < limit) {
out_angles[i] = src[i];
++i;
}
}

text
if (out_lengths) {
	if (var_len) {
		const size_t mL = lengths_vec.size();
		const size_t limitL = mL < n ? mL : n;
		const float* __restrict srcL =
			lengths_vec.data();

		size_t j = 0;

#pragma loop ivdep
while (j < limitL) {
out_lengths[j] = srcL[j];
++j;
}
}
else {
const float one = 1.0f;
size_t j = 0;
#pragma loop ivdep
while (j < n) {
out_lengths[j] = one;
++j;
}
}
}
}" - этот код нужно красиво отформатировать в соответствии со Стэнфордским стилем, нужно также удалить лишние скобки в выражениях и условиях которые лишены смысловой нагрузки и без которых можно обойтись, ВАЖНО!!! код должен остаться полностью функционально эквивалентным, грубо говоря сам код ты трогать не должен вообще, только форматирование НО!!! над чем можно подумать так это над оптимальным размещением блоков кода, блоков переменных, блоков объявлений, инициализации, блоков работы с памятью - тебе разрешено перемещать и группировать некоторые блоки близко друг к другу и перетасовывать отдельные строчки если это не скажется на работе кода но может помочь в улучшении локальности данных и потенциально дать небольшой выигрыш в производительности, также например операции с памятью можно ставить ближе друг к другу - чтобы лучше загрузить за один присест устройство работы с памятью, аналогично можно группировать вместе блоки строк отвечающих за интенсивное умножение чтобы лучше загрузить float умножители процессора чтобы за один присест сразу были проделаны все вычисления и уже потом устройство отвечающее за float умножение было освобождено, диаграмма выполнения потоков для этой программы должна быть близка к оптимальной насколько это возможно, но все эти условия вторичны - потому что самое главное не менять функциональность кода, она не должна никак пострадать - даже минимально! ЭТО ОЧЕНЬ ВАЖНО!!! Мне выдай полный отформатированный код без пропусков и комментариев одним сообщений весь код сразу чтобы я мог его скопировать