Сейчас пришлю блок кода, задача оптимизировать его...

Сейчас пришлю блок кода, задача оптимизировать его по скорости, убрать проверки безопасности, такие как проверки на непустоту кучи, проверки ограничений (все проверки типа clamp, убери вообще все обрезания по диапазону типа clamp, везде где встретишь clamp или более длинные названия, включающие clamp или являющиеся clamp по смыслу - убери обрезку - ни одно значение в коде обрезаться не должно и ни в одном месте кода пустота кучи не должна проверяться), убери неиспользуемые поля/константы/методы/функции, удали комментарии, упрости лямбды если там есть лишние усложнения, лишние проверки, которые никогда не выполнятся, лишние условия, маловероятные защиты от крашей, которые тоже выполнятся с малой вероятностью - всё это ты должен убрать и по ходу всех правок кода ты должен следовать парадигме максимальной производительности и полностью отключить настройки безопасности для всего кода, активно используй fmaf - даже для умножения двух чисел типа x * y используй fmaf(x, y, 0.0f), активно используй сложные вложенные fmaf, несколько fmaf подряд позволяют плотно загружать float умножители процессора - так что располагай блоки вычислений fmaf друг за другом, почти для всех вычислений используй fmaf - исключения только два - это чистое сложение/вычитание, для деления x/y используй fmaf вида fmaf(1.0f/y, x, 0.0f), помни, что деление дорогая операция - поэтому ставь её вперёд: fmaf(1.0f/y, x, 0.0f). Активно используй кеширование, предвычисления, предвыделение памяти в кеше, заботься о том, чтобы плотно загрузить кеш-линии, минимизировать обращения к памяти так как это дорогая операция, которая надолго заберёт время потока, минимум хранения лишних переменных для вычислений - максимум inline вычислений в одну строку - используй длинные сложные с большой вложенностью и большой когнитивной сложностью формулы для вычислений в одну строку, встраивай оператор ?: прямо в вычисления - так как это быстрее прямого ветвления, там где компилятор скорее всего сгенерирует jump-table - используй switch, активно используй ключевые слова для оптимизации компилятором, такие как forceline - используй именно forceline, а не inline, так как собираюсь под MCVS, и forceline является более строгим указанием, используй выровненный доступ к данным - используй выравнивание по кеш-линиям - используется 64-битная архитектура, рассмотри возможность использования AVX2 интструкций и интринсиков для векторизации вычислений, унифицируй стиль но нейминг не меняй - при это функционал должен полностью сохраниться, ты должен вносить чисто программные оптимизации не меняя смысл всего этого участка кода, код, который ты вернёшь должен быть ПОЛНОСТЬЮ ФУНКЦИОНАЛЬНО ЭКВИВАЛЕНТЕН тому коду, который пришлю сейчас, позаботься также о локальности данных - чтобы данные одного типа и данные которые совместно используются располагались в коде близко друг к другу, я ставлю рекорд гинесса по скорости программ оптимизации - так что используй другие методы увеличения скорости работы, которые здесь не упомянул, чтобы добавить к скорости, каждая наносекунда на счету, используй всё, даже то, что может дать лишь чисто гипотетический мизерный выигрыш, рассмотри возможность tbb параллелизма, pragma loop, векторизации, ручного разворачивания циклов с небольшим числом итераций, ТАКЖЕ ВАЖНО - ИСПОЛЬЗУЙ БИТОВЫЕ ТРЮКИ И ИНТРИНСИКИ ДЛЯ УСКОРЕНИЯ ОПЕРАЦИЙ С ЦЕЛЫМИ ЧИСЛАМИ, также используй оптимальные по скорости типы данных для целых чисел где это может дать выигрыш по скорости для 64-битной архитектуры, ты должен будешь вернуть мне полный исправленный блок кода полностью эквивалентный функционально коду, который пришлю, в полном объёме, без пропусков, также унифицируй стиль оформления кода предпочитая стэнфордский стиль оформления, нейминг не меняй, используй выровненный данные, аллокаторы, различные программистские трюки, битовые маски и прочее - всё что может дать даже лишь гипотетический выигрыш в скорости и то, что в теории должно быть быстрее, и обязательно используй то, что точно будет быстрее на практике, замени все for циклы циклами while, абсолютно каждое моё требование важно, так что ты должен выполнить их все без исключения, думай как можно дольше - используй свой максимальный порог для размышления чтобы предоставить максимально качественный ответ, итак код, который нужно оптимизировать: "static constexpr int AGP_MAX_STATE_DIM = 64;
static constexpr int AGP_MAX_SEED_STRIDE = 64;
static constexpr int AGP_MAX_GENERATED_SEEDS = 1024;
static constexpr int AGP_SOBOL_BASE_DIMS = 32;

static __declspec(noalias) __forceinline unsigned agp_splitmix32(unsigned x) noexcept
{
x += 0x9E3779B9u;
x = (x ^ (x >> 16)) * 0x85EBCA6Bu;
x = (x ^ (x >> 13)) * 0xC2B2AE35u;
x ^= (x >> 16);
return x;
}

static __declspec(noalias) __forceinline float agp_radical_inverse_scrambled(
unsigned long long index,
unsigned base,
unsigned scramble) noexcept
{
const float inv_base = 1.0f / static_cast<float>(base);
float inv_pow = inv_base;
float value = 0.0f;
unsigned state = (scramble % base);

text
while (index > 0ull) {
	const unsigned digit = static_cast<unsigned>(index % static_cast<unsigned long long>(base));
	const unsigned perm = (digit + state) % base;
	value = fmaf(static_cast<float>(perm), inv_pow, value);
	inv_pow *= inv_base;
	index /= static_cast<unsigned long long>(base);
	state = (state * 1664525u + 1013904223u) % base;
}

if (value >= 1.0f)
	value = 0.99999994f;

return value;

}

static __declspec(noalias) __forceinline float agp_clamp_to_range(float v, float lo, float hi) noexcept
{
if (v < lo) return lo;
if (v > hi) return hi;
return v;
}

static __declspec(noalias) __forceinline void agp_copy_state_clamped_to_map(
const float* __restrict src,
const MortonND& map,
int dim,
float* __restrict dst) noexcept
{
const int dmax = (dim < AGP_MAX_STATE_DIM) ? dim : AGP_MAX_STATE_DIM;

int d = 0;

#pragma loop ivdep
while (d < dmax) {
dst[d] = agp_clamp_to_range(src[d], map.low[d], map.high[d]);
++d;
}

text
while (d < AGP_MAX_STATE_DIM) {
	dst[d] = 0.0f;
	++d;
}

}

static __declspec(noalias) __forceinline int generate_sobol_seeds_limited(
const MortonND& map,
int dim,
float* __restrict S,
int stride,
unsigned seed,
int max_points) noexcept
{
if (max_points <= 0 || S == nullptr || stride <= 0 || dim <= 0)
return 0;

text
int dim_eff = dim;
if (dim_eff > AGP_MAX_STATE_DIM)
	dim_eff = AGP_MAX_STATE_DIM;

const int write_dim = (stride < dim_eff) ? stride : dim_eff;
if (write_dim <= 0)
	return 0;

int temp_dim = dim_eff;
--temp_dim;
if (temp_dim < 0)
	temp_dim = 0;

const int ns_orig = static_cast<int>(fmaf(
	static_cast<float>(temp_dim),
	fmaf(
		static_cast<float>(temp_dim),
		fmaf(
			static_cast<float>(temp_dim),
			fmaf(static_cast<float>(temp_dim), 0.00833333377f, 0.0416666679f),
			0.16666667f),
		0.5f),
	1.0f));

int ns_pow2 = 1;
while (ns_pow2 < ns_orig)
	ns_pow2 <<= 1;

if (ns_pow2 > max_points)
	ns_pow2 = max_points;

if (ns_pow2 <= 0)
	return 0;

static const unsigned int sobol_dir[32][32] = {
		{ 1u, 3u, 5u, 7u, 9u, 11u, 13u, 15u, 17u, 19u, 21u, 23u, 25u, 27u, 29u, 31u,
			33u, 35u, 37u, 39u, 41u, 43u, 45u, 47u, 49u, 51u, 53u, 55u, 57u, 59u, 61u, 63u },
		{ 1u, 1u, 7u, 11u, 13u, 19u, 25u, 37u, 59u, 47u, 61u, 55u, 41u, 67u, 97u, 91u,
			109u, 103u, 115u, 131u, 193u, 137u, 217u, 197u, 229u, 199u, 241u, 229u, 257u, 307u, 277u, 313u },
		{ 1u, 3u, 5u, 7u, 9u, 11u, 13u, 15u, 17u, 19u, 21u, 23u, 25u, 27u, 29u, 31u,
			33u, 35u, 37u, 39u, 41u, 43u, 45u, 47u, 49u, 51u, 53u, 55u, 57u, 59u, 61u, 63u },
		{ 1u, 1u, 5u, 3u, 13u, 15u, 9u, 7u, 21u, 23u, 29u, 31u, 17u, 19u, 25u, 27u,
			53u, 55u, 49u, 51u, 37u, 39u, 45u, 47u, 63u, 61u, 57u, 59u, 41u, 43u, 33u, 35u },
		{ 1u, 3u, 3u, 7u, 13u, 3u, 13u, 7u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 1u, 5u, 5u, 5u, 7u, 11u, 13u, 15u, 15u, 21u, 23u, 25u, 27u, 29u, 31u,
			31u, 37u, 39u, 41u, 43u, 45u, 47u, 49u, 51u, 53u, 55u, 57u, 59u, 61u, 63u, 63u },
		{ 1u, 3u, 5u, 5u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 1u, 7u, 5u, 13u, 13u, 17u, 19u, 21u, 23u, 25u, 27u, 29u, 31u, 35u, 37u,
			39u, 41u, 43u, 45u, 49u, 51u, 53u, 55u, 57u, 59u, 61u, 63u, 65u, 67u, 69u, 71u },
		{ 1u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 1u, 5u, 3u, 5u, 5u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 3u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 1u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 3u, 3u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 1u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 3u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 1u, 3u, 3u, 9u, 9u, 15u, 15u, 21u, 21u, 27u, 27u, 33u, 33u, 39u, 39u,
			45u, 45u, 51u, 51u, 57u, 57u, 63u, 63u, 69u, 69u, 75u, 75u, 81u, 81u, 87u, 87u },
		{ 1u, 3u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 1u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 1u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 3u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 1u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 3u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 1u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 1u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 3u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 1u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 3u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u },
		{ 1u, 1u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u,
			7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u, 7u },
		{ 1u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u,
			3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u, 3u },
		{ 1u, 1u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u,
			5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u, 5u }
};

static const unsigned halton_primes[32] = {
		131u, 137u, 139u, 149u, 151u, 157u, 163u, 167u,
		173u, 179u, 181u, 191u, 193u, 197u, 199u, 211u,
		223u, 227u, 229u, 233u, 239u, 241u, 251u, 257u,
		263u, 269u, 271u, 277u, 281u, 283u, 293u, 307u
};

unsigned scramble_mask[AGP_MAX_STATE_DIM];
int d = 0;

#pragma loop ivdep
while (d < dim_eff) {
seed = agp_splitmix32(seed ^ (0x9E3779B9u + static_cast<unsigned>(d) * 0x85EBCA6Bu));
scramble_mask[d] = seed;
++d;
}

text
const unsigned long long start_idx = 1ull;
const int sobol_dims = (dim_eff < AGP_SOBOL_BASE_DIMS) ? dim_eff : AGP_SOBOL_BASE_DIMS;
unsigned cur_x[AGP_SOBOL_BASE_DIMS]{};

d = 0;

#pragma loop ivdep
while (d < sobol_dims) {
unsigned x = 0u;
unsigned long long ii = start_idx;
int b = 0;
while (ii && b < 32) {
if (ii & 1ull)
x ^= sobol_dir[d][b];
ii >>= 1ull;
++b;
}
x ^= scramble_mask[d];
cur_x[d] = x;
++d;
}

text
auto write_point = [&](int row, unsigned long long index) noexcept {
	int dd = 0;

#pragma loop ivdep
while (dd < sobol_dims) {
const float u = static_cast<float>(cur_x[dd]) * 2.3283064e-10f;
const int pd = map.perm[dd];
const float lo = map.low[pd];
const float hi = map.high[pd];
S[row * stride + dd] = fmaf(u, hi - lo, lo);
++dd;
}

text
	while (dd < write_dim) {
		const unsigned base = halton_primes[(dd - AGP_SOBOL_BASE_DIMS) & 31];
		const float u = agp_radical_inverse_scrambled(index, base, scramble_mask[dd]);
		const int pd = map.perm[dd];
		const float lo = map.low[pd];
		const float hi = map.high[pd];
		S[row * stride + dd] = fmaf(u, hi - lo, lo);
		++dd;
	}

	while (dd < stride) {
		S[row * stride + dd] = 0.0f;
		++dd;
	}
	};

write_point(0, start_idx);

unsigned long long prev_gray = start_idx ^ (start_idx >> 1ull);
int j = 1;
while (j < ns_pow2) {
	const unsigned long long i = start_idx + static_cast<unsigned long long>(j);
	const unsigned long long gray = i ^ (i >> 1ull);
	const unsigned long long diff = gray ^ prev_gray;

	if (sobol_dims > 0) {
		const int bit = _tzcnt_u64(diff);
		d = 0;

#pragma loop ivdep
while (d < sobol_dims) {
cur_x[d] ^= sobol_dir[d][bit];
++d;
}
}

text
	prev_gray = gray;
	write_point(j, i);
	++j;
}

return ns_pow2;

}

static __declspec(noalias) __forceinline int generate_sobol_seeds(
const MortonND& map,
int dim,
float* __restrict S,
int stride,
unsigned seed) noexcept
{
return generate_sobol_seeds_limited(map, dim, S, stride, seed, AGP_MAX_GENERATED_SEEDS);
}

static __declspec(noalias) __forceinline void ccd_ik(
float targetX, float targetY,
const float* lengths, int n,
float* angles, int max_iter) noexcept
{
const int nn = (n < 32) ? n : 32;

text
__declspec(align(16)) float x[32];
__declspec(align(16)) float y[32];
__declspec(align(16)) float cum_angles[32];
__declspec(align(16)) float s_arr[32];
__declspec(align(16)) float c_arr[32];

int iter = 0;
while (iter < max_iter) {
	float acc = 0.0f;
	int i = 0;

#pragma loop ivdep
while (i < nn) {
acc += angles[i];
cum_angles[i] = acc;
++i;
}

text
	FABE13_SINCOS(cum_angles, s_arr, c_arr, nn);

	float curX = 0.0f;
	float curY = 0.0f;
	x[0] = 0.0f;
	y[0] = 0.0f;

	i = 0;

#pragma loop ivdep
while (i < nn) {
curX = fmaf(lengths[i], c_arr[i], curX);
curY = fmaf(lengths[i], s_arr[i], curY);
x[i + 1] = curX;
y[i + 1] = curY;
++i;
}

text
	i = nn - 1;
	while (i >= 0) {
		const float toEndX = x[nn] - x[i];
		const float toEndY = y[nn] - y[i];
		const float toTargetX = targetX - x[i];
		const float toTargetY = targetY - y[i];
		const float dot = fmaf(toEndX, toTargetX, toEndY * toTargetY);
		const float det = fmaf(toEndX, toTargetY, -toEndY * toTargetX);
		const float angle = atan2f(det, dot);

		angles[i] += angle;

		const float lo = (i == 0) ? -1.0471975511965976f : -2.6179938779914944f;
		const float hi = 2.6179938779914944f;
		if (angles[i] < lo) angles[i] = lo;
		if (angles[i] > hi) angles[i] = hi;

		--i;
	}

	++iter;
}

}

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 min_stride = (dim < AGP_MAX_SEED_STRIDE) ? dim : AGP_MAX_SEED_STRIDE;
if (S == nullptr || stride < min_stride)
return 0;

text
const int n = (cost.n < 32) ? cost.n : 32;
const bool VL = cost.variableLen;
const float tx = cost.targetX;
const float ty = cost.targetY;
const float phi = atan2f(ty, tx);

const bool is_transition = (cost.solveMode == ManipSolveMode::Transition) &&
	cost.has_transition_reference_angles();

float dist_to_target = sqrtf(tx * tx + ty * ty);
float max_reach = 0.0f;
if (VL) {
	for (int i = 0; i < n; ++i)
		max_reach += map.high[n + i];
}
else {
	max_reach = static_cast<float>(n) * cost.fixedLength;
}

const float ratio = dist_to_target / fmaxf(max_reach, 1.0e-20f);
const bool prefer_extended = (ratio > 0.7f);
const bool prefer_compact = (ratio < 0.4f);
const bool use_ik = !(ratio > 0.4f && ratio < 0.7f);

int total_seeds = 0;

auto emit_seed = [&](const float* q_src) noexcept -> bool {
	if (total_seeds >= AGP_MAX_GENERATED_SEEDS)
		return false;

	float* dst = S + static_cast<size_t>(total_seeds) * static_cast<size_t>(stride);
	agp_copy_state_clamped_to_map(q_src, map, dim, dst);
	++total_seeds;
	return true;
	};

if (is_transition) {
	alignas(16) float q_ref[AGP_MAX_STATE_DIM]{};
	alignas(16) float q_blend[AGP_MAX_STATE_DIM]{};
	alignas(16) float q_taper[AGP_MAX_STATE_DIM]{};

	const int full_dim = cost.full_state_dim();

	int d = 0;

#pragma loop ivdep
while (d < full_dim) {
if (d < cost.referenceStateCount) {
q_ref[d] = cost.referenceState[d];
}
else if (d < n) {
q_ref[d] = 0.0f;
}
else {
q_ref[d] = cost.fixedLength;
}
++d;
}

text
	while (d < AGP_MAX_STATE_DIM) {
		q_ref[d] = 0.0f;
		++d;
	}

	emit_seed(q_ref);

	d = 0;

#pragma loop ivdep
while (d < full_dim) {
q_blend[d] = q_ref[d];
q_taper[d] = q_ref[d];
++d;
}

text
	const float per_joint_phi = (n > 0) ? (phi / static_cast<float>(n)) : 0.0f;
	d = 0;

#pragma loop ivdep
while (d < n) {
const float t01 = (n > 1) ? (static_cast<float>(d) / static_cast<float>(n - 1)) : 0.0f;
const float blend = fmaf(0.20f, t01, 0.08f);
q_blend[d] = fmaf(blend, per_joint_phi - q_ref[d], q_ref[d]);
q_taper[d] = q_ref[d] * fmaf(-0.12f, t01, 1.0f);
++d;
}

text
	emit_seed(q_blend);
	emit_seed(q_taper);

	if (use_ik && prefer_extended && total_seeds < AGP_MAX_GENERATED_SEEDS) {
		alignas(16) float q_ccd[AGP_MAX_STATE_DIM]{};
		float angles[32]{};
		float lengths[32]{};

		if (VL) {
			const float avg_len = 0.5f * (map.low[n] + map.high[n]);
			for (int i = 0; i < n; ++i)
				lengths[i] = avg_len;
		}
		else {
			for (int i = 0; i < n; ++i)
				lengths[i] = cost.fixedLength;
		}

		ccd_ik(tx, ty, lengths, n, angles, 10);

		for (int i = 0; i < n; ++i) {
			const float blend = 0.30f;
			q_ccd[i] = fmaf(blend, angles[i] - q_ref[i], q_ref[i]);
		}

		if (VL) {
			for (int i = 0; i < n; ++i) {
				q_ccd[n + i] = fmaf(0.30f, lengths[i] - q_ref[n + i], q_ref[n + i]);
			}
		}

		emit_seed(q_ccd);
	}

	const int remaining = AGP_MAX_GENERATED_SEEDS - total_seeds;
	if (remaining > 0)
		total_seeds += generate_sobol_seeds_limited(
			map,
			dim,
			S + static_cast<size_t>(total_seeds) * static_cast<size_t>(stride),
			stride,
			seed,
			remaining);

	return total_seeds;
}

{
	alignas(16) float q0[AGP_MAX_STATE_DIM]{};
	const float rho = sqrtf(fmaf(tx, tx, ty * ty));
	const float len = fmaf(1.0f / static_cast<float>(fmaxf(static_cast<float>(n), 1.0f)), rho, 0.0f);

	int i = 0;

#pragma loop ivdep
while (i < n) {
q0[i] = agp_clamp_to_range((1.0f / static_cast<float>(n)) * phi, map.low[i], map.high[i]);
++i;
}

text
	if (VL) {
		i = 0;

#pragma loop ivdep
while (i < n) {
q0[n + i] = agp_clamp_to_range(len, map.low[n + i], map.high[n + i]);
++i;
}
}

text
	emit_seed(q0);
}

{
	alignas(16) float q1[AGP_MAX_STATE_DIM]{};

	int i = 0;

#pragma loop ivdep
while (i < n) {
q1[i] = agp_clamp_to_range(
0.5f * phi * ((i & 1) ? -1.0f : 1.0f),
map.low[i],
map.high[i]);
++i;
}

text
	if (VL) {
		i = 0;

#pragma loop ivdep
while (i < n) {
q1[n + i] = agp_clamp_to_range(
fmaf(0.4f, static_cast<float>(i) / static_cast<float>(fmaxf(static_cast<float>(n), 1.0f)), 0.8f),
map.low[n + i],
map.high[n + i]);
++i;
}
}

text
	emit_seed(q1);
}

{
	alignas(16) float q2[AGP_MAX_STATE_DIM]{};
	const float inv = (n > 1) ? 1.0f / static_cast<float>(n - 1) : 0.0f;

	int i = 0;

#pragma loop ivdep
while (i < n) {
const float pr = static_cast<float>(i) * inv;
q2[i] = agp_clamp_to_range(
fmaf(phi, fmaf(-0.3f, pr, 1.0f), 0.0f),
map.low[i],
map.high[i]);
++i;
}

text
	if (VL) {
		int j = 0;

#pragma loop ivdep
while (j < n) {
float si;
FABE13_SIN(fmaf(1.5f, static_cast<float>(j), 0.0f), si);
q2[n + j] = agp_clamp_to_range(
fmaf(0.2f, si, 1.0f),
map.low[n + j],
map.high[n + j]);
++j;
}
}

text
	emit_seed(q2);
}

if (use_ik && prefer_extended && total_seeds < AGP_MAX_GENERATED_SEEDS) {
	alignas(16) float q3[AGP_MAX_STATE_DIM]{};
	float angles[32]{};
	float lengths[32]{};

	if (VL) {
		const float avg_len = 0.5f * (map.low[n] + map.high[n]);
		for (int i = 0; i < n; ++i)
			lengths[i] = avg_len;
	}
	else {
		for (int i = 0; i < n; ++i)
			lengths[i] = cost.fixedLength;
	}

	ccd_ik(tx, ty, lengths, n, angles, 10);

	int i = 0;

#pragma loop ivdep
while (i < n) {
q3[i] = agp_clamp_to_range(angles[i], map.low[i], map.high[i]);
++i;
}

text
	if (VL) {
		i = 0;

#pragma loop ivdep
while (i < n) {
q3[n + i] = agp_clamp_to_range(lengths[i], map.low[n + i], map.high[n + i]);
++i;
}
}

text
	emit_seed(q3);
}

if (use_ik && prefer_compact && total_seeds < AGP_MAX_GENERATED_SEEDS) {
	alignas(16) float q4[AGP_MAX_STATE_DIM]{};
	float angles_fabrik[32]{};
	float lengths_fabrik[32]{};

	if (VL) {
		const float avg_len = 0.5f * (map.low[n] + map.high[n]);
		for (int i = 0; i < n; ++i)
			lengths_fabrik[i] = avg_len;
	}
	else {
		for (int i = 0; i < n; ++i)
			lengths_fabrik[i] = cost.fixedLength;
	}

	float targetX_fab = tx;
	float targetY_fab = ty;

	for (int iter_fab = 0; iter_fab < 3; ++iter_fab) {
		float prevX = targetX_fab;
		float prevY = targetY_fab;

		for (int j = n - 1; j >= 0; --j) {
			const float len = lengths_fabrik[j];
			const float angle_to_target = atan2f(prevY, prevX);
			angles_fabrik[j] = angle_to_target;
			float s_val;
			float c_val;
			FABE13_SINCOS(&angle_to_target, &s_val, &c_val, 1);
			prevX = prevX - len * c_val;
			prevY = prevY - len * s_val;
		}
	}

	int i = 0;

#pragma loop ivdep
while (i < n) {
q4[i] = agp_clamp_to_range(angles_fabrik[i], map.low[i], map.high[i]);
++i;
}

text
	if (VL) {
		i = 0;

#pragma loop ivdep
while (i < n) {
q4[n + i] = agp_clamp_to_range(lengths_fabrik[i], map.low[n + i], map.high[n + i]);
++i;
}
}

text
	emit_seed(q4);
}

const int remaining = AGP_MAX_GENERATED_SEEDS - total_seeds;
if (remaining > 0)
	total_seeds += generate_sobol_seeds_limited(
		map,
		dim,
		S + static_cast<size_t>(total_seeds) * static_cast<size_t>(stride),
		stride,
		seed,
		remaining);

return total_seeds;

}

enum AGPHJOrderMode : int {
AGP_HJ_ORDER_NATURAL = 0, // текущее поведение: 0..dim-1
AGP_HJ_ORDER_REVERSE = 1, // dim-1..0
AGP_HJ_ORDER_INTERLEAVED_PROXIMAL = 2, // angle0,len0, angle1,len1, ...
AGP_HJ_ORDER_INTERLEAVED_DISTAL = 3, // angleN-1,lenN-1, ...
AGP_HJ_ORDER_LENGTHS_FIRST = 4 // len0..lenN-1, angle0..angleN-1
};

enum AGPHJSeedMode : int {
AGP_HJ_SEED_INDEXED_THEN_FEASIBLE = 0, // текущее поведение
AGP_HJ_SEED_FEASIBLE_THEN_INDEXED = 1,
AGP_HJ_SEED_INDEXED_ONLY = 2,
AGP_HJ_SEED_FEASIBLE_ONLY = 3
};

constexpr int AGP_HJ_MAX_PROGRESS_RULES = 8;

struct AGPHJIterationScheduleRule {
float progress_lt = 1.0f;
int period = 1;
int phase = 0;
};

struct AGPHJCoordScaleProfile {
float angle_scale_proximal = 1.4f;
float angle_scale_distal = 0.6f;
float length_scale_proximal = 1.4f;
float length_scale_distal = 0.6f;
};

struct AGPHJCoordScaleRule {
float progress_lt = 1.0f;
AGPHJCoordScaleProfile profile{};
};

struct AGPHJOrderScheduleRule {
float progress_lt = 1.0f;
int order_mode = AGP_HJ_ORDER_INTERLEAVED_DISTAL;
};

struct AGPHookeJeevesParams {
bool enabled = true;
bool require_adaptive_flag = true;

text
// Расписание запуска HJ по прогрессу p.
int iteration_schedule_count = 0;
AGPHJIterationScheduleRule iteration_schedule[AGP_HJ_MAX_PROGRESS_RULES] = {
	{ 1.0f, 2, 0 }
};

// Фолбэк, если progress-schedule отключён.
int iteration_period = 2;
int iteration_phase = 0;

// текущее поведение: сначала +delta, потом -delta, принимаем первое улучшение
bool plus_first = true;
bool opportunistic_axis = true;

// текущее поведение: pattern-step включён
bool use_pattern = true;

// текущее поведение: delta не разгоняется после успеха
bool expand_delta_on_success = false;

// Порядок обхода координат можно переключать по прогрессу p.
// По умолчанию schedule выключен, используется order_mode ниже.
int order_schedule_count = 2;
AGPHJOrderScheduleRule order_schedule[AGP_HJ_MAX_PROGRESS_RULES] = {
	{ 0.95f, AGP_HJ_ORDER_INTERLEAVED_PROXIMAL },
	{ 1.0f, AGP_HJ_ORDER_INTERLEAVED_DISTAL }
};

// Фолбэк, если progress-schedule отключён.
int order_mode = AGP_HJ_ORDER_INTERLEAVED_DISTAL;

int seed_mode = AGP_HJ_SEED_INDEXED_THEN_FEASIBLE;

// текущее поведение:
// hj_delta[d] = max(span * 0.05f, 0.0025f)
float init_delta_frac = 0.07f;
float init_delta_abs = 0.0025f;

// текущее поведение:
// при провале всей HJ-итерации delta[d] *= 0.5f, но не ниже 1e-4f
float min_delta = 1.0e-4f;
float shrink_factor = 0.55f;

// если захочешь включать разгон шага после успеха
float expand_factor = 1.0f;

// текущее поведение:
// q_pattern = current + 1.0 * (current - base_before)
float pattern_gain = 0.7f;

// Профиль шагов по координатам можно задавать явно
// через проксимальные/дистальные соотношения и, при желании,
// тоже переключать по прогрессу p.
// По умолчанию schedule выключен, используются поля ниже.
int coord_profile_schedule_count = 2;
AGPHJCoordScaleRule coord_profile_schedule[AGP_HJ_MAX_PROGRESS_RULES] = {
	{ 0.95f, { 1.4f, 0.6f, 1.4f, 0.6f } },
	{ 1.0f, { 1.3f, 0.7f, 1.3f, 0.7f } }
};

// Фолбэк, если progress-schedule отключён.
// first = proximal, last = distal.
float angle_scale_first = 1.4f;
float angle_scale_last = 0.6f;
float length_scale_first = 1.4f;
float length_scale_last = 0.6f;

};

static inline AGPHookeJeevesParams MakeDefaultAGPHookeJeevesParams() noexcept {
AGPHookeJeevesParams p{};
return p;
}

// Глобальный набор параметров HJ.
// Меняешь его — меняется поведение HJ.
// Дефолтно: period-schedule уже настроен как p < 0.9f -> 2, иначе 3;
// order/profile schedule по умолчанию выключены и используются фолбэки ниже.
static AGPHookeJeevesParams g_agp_hj_params = MakeDefaultAGPHookeJeevesParams();

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
{
struct TrialPoint {
alignas(16) float q[AGP_MAX_STATE_DIM];
float t = 0.0f;
float f = FLT_MAX;
float x = 0.0f;
float y = 0.0f;
unsigned idx = 0u;
bool feasible = false;
};

text
using AlignedFloatVec = std::vector<float, boost::alignment::aligned_allocator<float, 16u>>;

constexpr float T_EPS = 1.0e-7f;
constexpr float NUM_EPS = 1.0e-12f;

const int n = cost.n;
const int dim = n + (cost.variableLen ? n : 0);
const float dim_f = static_cast<float>(dim);
const unsigned fullConstraintIndex = cost.feasible_index();
const bool hasHardConstraints = (fullConstraintIndex != 0u);

if (n > 32 || dim > AGP_MAX_STATE_DIM) {
	bestQ.clear();
	bestF = FLT_MAX;
	bestX = 0.0f;
	bestY = 0.0f;
	out_iterations = 0u;
	out_achieved_epsilon = 0.0f;
	return;
}

unsigned exchange_counter = 0u;
unsigned exchange_counter_T = 0u;

alignas(16) float M_by_span[12];
for (int i = 0; i < 12; ++i)
	M_by_span[i] = M_prior;

float Mmax = M_prior;

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

bestQ.reserve(static_cast<size_t>(dim));

AlignedFloatVec bestQIndexed;
bestQIndexed.reserve(static_cast<size_t>(dim));

AlignedFloatVec transitionReferenceSeed;
float transitionRefF = FLT_MAX;
unsigned transitionRefIdx = 0u;
float transitionRefX = 0.0f;
float transitionRefY = 0.0f;
bool haveTransitionRefSeed = false;

if (cost.solveMode == ManipSolveMode::Transition && cost.has_transition_reference_angles()) {
	const int refDim = cost.full_state_dim();
	transitionReferenceSeed.assign(cost.referenceState, cost.referenceState + refDim);

	float xr = 0.0f;
	float yr = 0.0f;
	unsigned idxr = 0u;
	float fr = 0.0f;
	const bool ref_ok = cost.evaluate_indexed(transitionReferenceSeed.data(), xr, yr, idxr, fr);

	transitionRefF = fr;
	transitionRefIdx = idxr;
	transitionRefX = xr;
	transitionRefY = yr;
	haveTransitionRefSeed = ref_ok;
}

unsigned bestIndexFound = 0u;
float bestIndexValue = FLT_MAX;
float bestIndexedX = 0.0f;
float bestIndexedY = 0.0f;

BestSolutionMsg lastSentBestMsg;
InitBestSolutionMsg(lastSentBestMsg);

int no_improve = 0;

const float a = 0.0f;
const float b = 1.0f;
float p = 0.0f;
float dmax = b - a;
const float initial_len = dmax;

const float A_dim = fmaf(1.0f / sqrtf(dim_f + 6.75f), 5.535f, 0.0f);
const float A_dim__ = fmaf(1.0f / sqrtf(dim_f + 6.75f), 3.425f, 0.0f);
const float B_dim = fmaf(A_dim, 0.7f, 0.0f);
const float B_dim__ = fmaf(A_dim__, 4.325f, 0.0f);
const float log_argument = A_dim - 2.03f;
const float log_argument__ = A_dim__ - 2.0f;

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 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.5f),
	1.0f);

const float adaptive_coeff_addition__ = fmaf(
	C_dim__,
	fmaf(
		C_dim__,
		fmaf(
			C_dim__,
			fmaf(C_dim__, 0.00833333377f, 0.0416666679f),
			0.16666667f),
		0.5f),
	1.0f);

float adaptive_coeff = A_dim - adaptive_coeff_addition;
float adaptive_coeff__ = A_dim__ - adaptive_coeff_addition__;
const float A_dim_clone = fmaf(
	A_dim - fmaf(
		-fmaf(B_dim, fmaf(B_dim, fmaf(B_dim, fmaf(B_dim, 0.00833333377f, 0.0416666679f), 0.16666667f), 0.5f), 1.0f),
		adaptive_coeff_addition,
		A_dim),
	0.5f,
	0.0f);

(void)A_dim_clone;
(void)NUM_EPS;
(void)eps;

int it = 0;
int stag_boost_remaining = 0;
float stag_r_multiplier = 0.0f;
int last_send_T = 0;
const int send_interval_T = 7;
int last_send_best = 0;
const int send_interval_best = 2;
const int n_stag_iters = static_cast<int>(3.0f + 2.045f * sqrtf(dim_f));
const int noImproveThrDim = static_cast<int>(fmaf(7.5f, exp2f(-0.1f * sqrtf(dim_f)), 0.0f));

const AGPHookeJeevesParams& hj_cfg = g_agp_hj_params;

auto clamp01 = [](float v) noexcept -> float {
	if (v < 0.0f) return 0.0f;
	if (v > 1.0f) return 1.0f;
	return v;
	};

auto lerp_hj = [](float a0, float a1, float t) noexcept -> float {
	return fmaf(t, a1 - a0, a0);
	};

auto select_hj_iteration_schedule = [&](float progress, int& period_out, int& phase_out) noexcept {
	period_out = hj_cfg.iteration_period;
	phase_out = hj_cfg.iteration_phase;

	int count = hj_cfg.iteration_schedule_count;
	if (count <= 0)
		return;

	if (count > AGP_HJ_MAX_PROGRESS_RULES)
		count = AGP_HJ_MAX_PROGRESS_RULES;

	const float p01 = clamp01(progress);

	for (int i = 0; i < count; ++i) {
		const AGPHJIterationScheduleRule& rule = hj_cfg.iteration_schedule[i];
		if (p01 < rule.progress_lt || i + 1 == count) {
			period_out = rule.period;
			phase_out = rule.phase;
			break;
		}
	}

	if (period_out < 1)
		period_out = 1;

	phase_out %= period_out;
	if (phase_out < 0)
		phase_out += period_out;
	};

auto select_hj_coord_profile = [&](float progress) noexcept -> AGPHJCoordScaleProfile {
	int count = hj_cfg.coord_profile_schedule_count;
	if (count > AGP_HJ_MAX_PROGRESS_RULES)
		count = AGP_HJ_MAX_PROGRESS_RULES;

	const float p01 = clamp01(progress);

	if (count > 0) {
		for (int i = 0; i < count; ++i) {
			const AGPHJCoordScaleRule& rule = hj_cfg.coord_profile_schedule[i];
			if (p01 < rule.progress_lt || i + 1 == count)
				return rule.profile;
		}
	}

	AGPHJCoordScaleProfile fallback{};
	fallback.angle_scale_proximal = hj_cfg.angle_scale_first;
	fallback.angle_scale_distal = hj_cfg.angle_scale_last;
	fallback.length_scale_proximal = hj_cfg.length_scale_first;
	fallback.length_scale_distal = hj_cfg.length_scale_last;
	return fallback;
	};

auto select_hj_order_mode = [&](float progress) noexcept -> int {
	int order_mode = hj_cfg.order_mode;

	int count = hj_cfg.order_schedule_count;
	if (count <= 0)
		return order_mode;

	if (count > AGP_HJ_MAX_PROGRESS_RULES)
		count = AGP_HJ_MAX_PROGRESS_RULES;

	const float p01 = clamp01(progress);

	for (int i = 0; i < count; ++i) {
		const AGPHJOrderScheduleRule& rule = hj_cfg.order_schedule[i];
		if (p01 < rule.progress_lt || i + 1 == count) {
			order_mode = rule.order_mode;
			break;
		}
	}

	return order_mode;
	};

auto coord_profile_scale = [&](int d, const AGPHJCoordScaleProfile& profile) noexcept -> float {
	if (d < n) {
		const float t01 = (n > 1) ? (static_cast<float>(d) / static_cast<float>(n - 1)) : 0.0f;
		return lerp_hj(profile.angle_scale_proximal, profile.angle_scale_distal, t01);
	}

	if (cost.variableLen) {
		const int j = d - n;
		const float t01 = (n > 1) ? (static_cast<float>(j) / static_cast<float>(n - 1)) : 0.0f;
		return lerp_hj(profile.length_scale_proximal, profile.length_scale_distal, t01);
	}

	return 1.0f;
	};

alignas(16) float hj_delta[AGP_MAX_STATE_DIM];
alignas(16) float hj_profile_scale_applied[AGP_MAX_STATE_DIM];

{
	const AGPHJCoordScaleProfile initial_hj_profile = select_hj_coord_profile(0.0f);

	for (int d = 0; d < dim; ++d) {
		const float span = fmaf(map.high[d], 1.0f, -map.low[d]);
		const float scale = fmaxf(coord_profile_scale(d, initial_hj_profile), 1.0e-12f);
		hj_profile_scale_applied[d] = scale;
		hj_delta[d] = fmaxf(span * hj_cfg.init_delta_frac * scale, hj_cfg.init_delta_abs);
	}
}

auto apply_hj_coord_profile = [&](float progress) noexcept {
	const AGPHJCoordScaleProfile profile = select_hj_coord_profile(progress);

	for (int d = 0; d < dim; ++d) {
		const float new_scale = fmaxf(coord_profile_scale(d, profile), 1.0e-12f);
		const float old_scale = fmaxf(hj_profile_scale_applied[d], 1.0e-12f);

		if (fabsf(new_scale - old_scale) > 1.0e-12f) {
			hj_delta[d] = fmaxf(hj_delta[d] * (new_scale / old_scale), hj_cfg.min_delta);
			hj_profile_scale_applied[d] = new_scale;
		}
	}
	};

auto clampPoint = [&](float* q) noexcept {
	int i = 0;

#pragma loop ivdep
while (i < n) {
float lo = (i == 0) ? -1.0471975511965977f : -2.6179938779914944f;
float hi = 2.6179938779914944f;

text
		if (cost.solveMode == ManipSolveMode::Transition &&
			cost.has_transition_reference_angles() &&
			i < cost.referenceAngleCount)
		{
			const float ref = cost.referenceState[i];
			const float lo_ref = ref - cost.transitionMaxAngleStep;
			const float hi_ref = ref + cost.transitionMaxAngleStep;

			if (lo_ref > lo) lo = lo_ref;
			if (hi_ref < hi) hi = hi_ref;
		}

		if (cost.use_joint_angle_limit_constraints()) {
			const float hard = fabsf(cost.minTheta);
			const float lo_hard = -hard;
			const float hi_hard = hard;

			if (lo_hard > lo) lo = lo_hard;
			if (hi_hard < hi) hi = hi_hard;
		}

		if (q[i] < lo) q[i] = lo;
		else if (q[i] > hi) q[i] = hi;

		if (cost.solveMode == ManipSolveMode::Transition &&
			cost.transitionPreserveJointMagnitudeFloor &&
			cost.has_transition_reference_angles() &&
			i < cost.referenceAngleCount)
		{
			const float ref = cost.referenceState[i];
			if (fabsf(ref) > cost.transitionJointSignDeadband) {
				const float min_abs = cost.min_joint_abs_angle_bound(i);
				if (fabsf(q[i]) < min_abs) {
					q[i] = copysignf(min_abs, ref);
					if (q[i] < lo) q[i] = lo;
					else if (q[i] > hi) q[i] = hi;
				}
			}
		}

		++i;
	}

	if (cost.variableLen) {
		i = 0;

#pragma loop ivdep
while (i < n) {
float lo = 0.5f;
float hi = 2.0f;

text
			if (cost.solveMode == ManipSolveMode::Transition &&
				cost.has_transition_reference_lengths() &&
				i < cost.referenceLengthCount)
			{
				const float refL = cost.referenceState[n + i];
				const float dL = cost.max_length_step(i);
				const float lo_ref = refL - dL;
				const float hi_ref = refL + dL;

				if (lo_ref > lo) lo = lo_ref;
				if (hi_ref < hi) hi = hi_ref;
			}

			if (q[n + i] < lo) q[n + i] = lo;
			else if (q[n + i] > hi) q[n + i] = hi;
			++i;
		}
	}
	};

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

auto finalize_interval_geometry = [&](IntervalND* I) noexcept {
	I->i1 = t_to_idx(I->x1);
	I->i2 = t_to_idx(I->x2);
	I->diam = map.block_diameter(I->i1, I->i2);
	I->compute_span_level(map);
	I->set_metric(I->diam);
	};

auto update_pockets_and_Mmax = [&](IntervalND* I) noexcept {
	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];
	};

auto recompute_dmax = [&]() noexcept {
	float new_dmax = 0.0f;
	for (auto* pI : H) {
		const float Ls = fmaf(pI->x2, 1.0f, -pI->x1);
		if (Ls > new_dmax)
			new_dmax = Ls;
	}
	dmax = (new_dmax > 0.0f) ? new_dmax : initial_len;
	};

auto recompute_heap_constM = [&](float m_cur) noexcept {
	const size_t sz = H.size();
	if (sz == 0u)
		return;

	if (!hasHardConstraints)
		RecomputeR_ConstM_AVX2_ND(H.data(), sz, m_cur);
	else
		RecomputeR_ConstM_Mixed_ND(H.data(), sz, m_cur);

	std::make_heap(H.begin(), H.end(), ComparePtrND);
	};

auto update_best_from_trial = [&](const TrialPoint& tr) noexcept {
	if (better_indexed(tr.idx, tr.f, bestIndexFound, bestIndexValue)) {
		bestIndexFound = tr.idx;
		bestIndexValue = tr.f;
		bestQIndexed.assign(tr.q, tr.q + dim);
		bestIndexedX = tr.x;
		bestIndexedY = tr.y;
	}

	if (tr.feasible) {
		if (tr.f < bestF) {
			bestF = tr.f;
			bestQ.assign(tr.q, tr.q + dim);
			bestX = tr.x;
			bestY = tr.y;
			no_improve = 0;
		}
		else {
			++no_improve;
		}
	}
	};

auto refine_trial = [&](float* q_inout,
	float& x_io,
	float& y_io,
	unsigned& idx_io,
	float& f_io,
	float t_lo,
	float t_hi,
	bool enforce_t_bounds) noexcept
	{
		if (!(f_io < fmaf(bestF, adaptive_coeff, 0.0f)))
			return;

		const float f_start = f_io;
		const float c1 = 1e-4f;
		const float tau = 0.5f;
		const int max_outer_iters = static_cast<int>(50.0f * (1.0f + 0.65f * p));
		const int max_backtrack = static_cast<int>(20.0f * (1.0f + 0.65f * p));
		const float lbfgs_trigger = 0.6f;
		const int m_lbfgs = 9;
		const int max_lbfgs_iters = static_cast<int>(25.0f * (1.0f + 0.65f * p));
		const float eps_lbfgs_curv = 1e-6f;
		const float eps_descent = 1e-12f;
		float eta = 2.0f / sqrtf(dim_f);

		auto computeGrad = [&](const float* q_in,
			const float x_in,
			const float y_in,
			float* grad_out,
			float& grad_norm2_out) noexcept
			{
				float phi_acc_local = 0.0f;
				int ii = 0;

#pragma loop ivdep
while (ii < n) {
phi_acc_local = fmaf(q_in[ii], 1.0f, phi_acc_local);
phi[ii] = phi_acc_local;
++ii;
}

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

				float acc_s = 0.0f;
				float acc_c = 0.0f;
				int kk = n - 1;
				while (kk >= 0) {
					const float Lk = cost.link_length(q_in, kk);
					acc_s = fmaf(Lk, s_arr[kk], acc_s);
					acc_c = fmaf(Lk, c_arr[kk], acc_c);
					sum_s[kk] = acc_s;
					sum_c[kk] = acc_c;
					--kk;
				}

				alignas(16) float prefix_tail_grad[AGP_MAX_STATE_DIM]{};

				if (cost.solveMode == ManipSolveMode::Transition &&
					cost.has_transition_reference_angles() &&
					cost.transitionPrefixEnergyWeight > 0.0f)
				{
					alignas(16) float prefix_delta[AGP_MAX_STATE_DIM]{};
					float phi_prefix = 0.0f;
					int jj = 0;

#pragma loop ivdep
while (jj < cost.referenceAngleCount) {
phi_prefix += q_in[jj];
prefix_delta[jj] = ManipCost::wrap_pi(phi_prefix - cost.referencePrefixAngles[jj]);
++jj;
}

text
					float tail = 0.0f;
					jj = cost.referenceAngleCount - 1;
					while (jj >= 0) {
						tail += prefix_delta[jj];
						prefix_tail_grad[jj] = tail;
						--jj;
					}
				}

				const float dx = fmaf(x_in, 1.0f, -cost.targetX);
				const float dy = fmaf(y_in, 1.0f, -cost.targetY);
				const float dist2 = fmaf(dx, dx, dy * dy);
				const float dist = sqrtf(dist2);
				const float inv_dist = (dist > 1.0e-20f) ? (1.0f / dist) : 0.0f;

				grad_norm2_out = 0.0f;

				int i = 0;

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

text
					if (cost.solveMode == ManipSolveMode::Transition) {
						const float dtheta = (i < cost.referenceAngleCount)
							? cost.wrapped_angle_delta(q_in, i)
							: 0.0f;

						gpen = 2.0f * cost.transitionEnergyWeight * dtheta;
						gpen = fmaf(2.0f * cost.transitionPrefixEnergyWeight, prefix_tail_grad[i], gpen);

						g_main = 2.0f * cost.transitionCaptureWeight *
							fmaf(dx, -sum_s[i], dy * sum_c[i]);
					}
					else {
						{
							const float tsg = fmaf(-q_in[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 / (exp_val + 1.0f);
							gpen = fmaf(-cost.archBiasW * cost.archBiasK, sig, gpen);
						}

						g_main = fmaf(dx, -sum_s[i], dy * sum_c[i]) * inv_dist;
					}

					const float gi = g_main + gpen;
					grad_out[i] = gi;
					grad_norm2_out = fmaf(gi, gi, grad_norm2_out);
					++i;
				}

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

#pragma loop ivdep
while (j < n) {
float gi = 0.0f;
if (cost.solveMode == ManipSolveMode::Transition) {
float gpenL = 0.0f;
if (cost.has_transition_reference_lengths() && j < cost.referenceLengthCount) {
gpenL = 2.0f * cost.transitionLengthEnergyWeight *
(q_in[n + j] - cost.referenceState[n + j]);
}

text
							gi = 2.0f * cost.transitionCaptureWeight *
								fmaf(dx, c_arr[j], dy * s_arr[j]) + gpenL;
						}
						else {
							gi = fmaf(dx, c_arr[j], dy * s_arr[j]) * inv_dist;
						}

						grad_out[n + j] = gi;
						grad_norm2_out = fmaf(gi, gi, grad_norm2_out);
						++j;
					}
				}
			};

		auto armijoLineSearch = [&](const float* q_base,
			const float f_base,
			const float* grad_base,
			const float* dir,
			const float gtd,
			float& alpha_io,
			float* q_out,
			float& f_out,
			float& x_out,
			float& y_out,
			unsigned& idx_out,
			bool& clipped) noexcept -> bool
			{
				(void)grad_base;
				float alpha = alpha_io;
				int backtrack = 0;

				while (backtrack < max_backtrack) {
					int i = 0;

#pragma loop ivdep
while (i < dim) {
q_out[i] = fmaf(alpha, dir[i], q_base[i]);
++i;
}

text
					float q_before_clamp[AGP_MAX_STATE_DIM];
					std::memcpy(q_before_clamp, q_out, static_cast<size_t>(dim) * sizeof(float));
					clampPoint(q_out);

					clipped = false;
					i = 0;

#pragma loop ivdep
while (i < dim) {
if (fabsf(q_out[i] - q_before_clamp[i]) > 1e-12f) {
clipped = true;
break;
}
++i;
}

text
					float x2 = 0.0f;
					float y2 = 0.0f;
					unsigned idx_try = 0u;
					float val_try = 0.0f;
					const bool feasible_try = cost.evaluate_indexed(q_out, x2, y2, idx_try, val_try);

					if (!feasible_try || !(val_try == val_try)) {
						alpha *= tau;
						++backtrack;
						continue;
					}

					const float t_try = clamp01(map.pointToT(q_out));
					if (enforce_t_bounds) {
						if (t_hi - t_lo > 2.0f * T_EPS) {
							if (!(t_try > t_lo + T_EPS && t_try < t_hi - T_EPS)) {
								alpha *= tau;
								++backtrack;
								continue;
							}
						}
						else {
							if (fabsf(t_try - t_lo) > T_EPS) {
								alpha *= tau;
								++backtrack;
								continue;
							}
						}
					}

					if (val_try <= f_base + c1 * alpha * gtd) {
						alpha_io = alpha;
						f_out = val_try;
						x_out = x2;
						y_out = y2;
						idx_out = idx_try;
						return true;
					}

					alpha *= tau;
					++backtrack;
				}

				return false;
			};

		bool lbfgs_already_tried = false;
		int outer = 0;

		while (outer < max_outer_iters) {
			float grad[AGP_MAX_STATE_DIM];
			float grad_norm2 = 0.0f;
			computeGrad(q_inout, x_io, y_io, grad, grad_norm2);

			if (!(grad_norm2 == grad_norm2) || grad_norm2 < 1e-12f)
				break;

			float dir_gd[AGP_MAX_STATE_DIM];
			int i = 0;

#pragma loop ivdep
while (i < dim) {
dir_gd[i] = -grad[i];
++i;
}

text
			const float gtd_gd = -grad_norm2;
			float eta_trial = eta;
			float f_new = f_io;
			float x_new = x_io;
			float y_new = y_io;
			unsigned idx_new = idx_io;
			bool clipped_gd = false;

			const bool found = armijoLineSearch(
				q_inout,
				f_io,
				grad,
				dir_gd,
				gtd_gd,
				eta_trial,
				q_try,
				f_new,
				x_new,
				y_new,
				idx_new,
				clipped_gd);

			if (!found)
				break;

			std::memcpy(q_inout, q_try, static_cast<size_t>(dim) * sizeof(float));
			f_io = f_new;
			x_io = x_new;
			y_io = y_new;
			idx_io = idx_new;
			eta = eta_trial;

			const float rel_impr = (f_start > 1.0e-20f) ? ((f_start - f_io) / f_start) : 0.0f;

			if (!lbfgs_already_tried && rel_impr >= lbfgs_trigger) {
				lbfgs_already_tried = true;

				float q_resume[AGP_MAX_STATE_DIM];
				std::memcpy(q_resume, q_inout, static_cast<size_t>(dim) * sizeof(float));
				const float f_resume = f_io;
				const float x_resume = x_io;
				const float y_resume = y_io;
				const unsigned idx_resume = idx_io;
				const float eta_resume = eta;

				float q_best_lbfgs[AGP_MAX_STATE_DIM];
				std::memcpy(q_best_lbfgs, q_inout, static_cast<size_t>(dim) * sizeof(float));
				float f_best_lbfgs = f_io;
				float x_best_lbfgs = x_io;
				float y_best_lbfgs = y_io;
				unsigned idx_best_lbfgs = idx_io;

				float s_hist[m_lbfgs][AGP_MAX_STATE_DIM];
				float y_hist[m_lbfgs][AGP_MAX_STATE_DIM];
				float rho_hist[m_lbfgs];
				float alpha_hist[m_lbfgs];
				int hist_size = 0;

				float gk[AGP_MAX_STATE_DIM];
				float gk_norm2 = 0.0f;
				computeGrad(q_inout, x_io, y_io, gk, gk_norm2);

				bool lbfgs_ok = true;
				float alpha_k = 2.0f / sqrtf(dim_f);
				int it_lbfgs = 0;

				while (it_lbfgs < max_lbfgs_iters) {
					if (!(gk_norm2 == gk_norm2) || gk_norm2 < 1e-12f)
						break;

					float dir[AGP_MAX_STATE_DIM];

					if (hist_size == 0) {
						int d = 0;

#pragma loop ivdep
while (d < dim) {
dir[d] = -gk[d];
++d;
}
}
else {
float q_vec[AGP_MAX_STATE_DIM];
int d = 0;
#pragma loop ivdep
while (d < dim) {
q_vec[d] = gk[d];
++d;
}

text
						for (int jj = hist_size - 1; jj >= 0; --jj) {
							float dot_sq = 0.0f;
							d = 0;

#pragma loop ivdep
while (d < dim) {
dot_sq = fmaf(s_hist[jj][d], q_vec[d], dot_sq);
++d;
}
const float a_coeff = dot_sq * rho_hist[jj];
alpha_hist[jj] = a_coeff;

							d = 0;

#pragma loop ivdep
while (d < dim) {
q_vec[d] = fmaf(-a_coeff, y_hist[jj][d], q_vec[d]);
++d;
}
}

text
						float gamma = 1.0f;
						{
							const int last = hist_size - 1;
							float yy = 0.0f;
							int d = 0;

#pragma loop ivdep
while (d < dim) {
yy = fmaf(y_hist[last][d], y_hist[last][d], yy);
++d;
}
const float ys = 1.0f / rho_hist[last];
if (yy > 0.0f)
gamma = ys / yy;
}

text
						float r_vec[AGP_MAX_STATE_DIM];
						d = 0;

#pragma loop ivdep
while (d < dim) {
r_vec[d] = gamma * q_vec[d];
++d;
}

text
						for (int jj = 0; jj < hist_size; ++jj) {
							float dot_yr = 0.0f;
							int d = 0;

#pragma loop ivdep
while (d < dim) {
dot_yr = fmaf(y_hist[jj][d], r_vec[d], dot_yr);
++d;
}
const float b_coeff = dot_yr * rho_hist[jj];
const float coeff = alpha_hist[jj] - b_coeff;

							d = 0;

#pragma loop ivdep
while (d < dim) {
r_vec[d] = fmaf(coeff, s_hist[jj][d], r_vec[d]);
++d;
}
}

						d = 0;

#pragma loop ivdep
while (d < dim) {
dir[d] = -r_vec[d];
++d;
}
}

text
					float gtd = 0.0f;
					int d = 0;

#pragma loop ivdep
while (d < dim) {
gtd = fmaf(gk[d], dir[d], gtd);
++d;
}

text
					if (!(gtd == gtd) || gtd >= -eps_descent) {
						d = 0;

#pragma loop ivdep
while (d < dim) {
dir[d] = -gk[d];
++d;
}
gtd = -gk_norm2;
if (gtd >= -eps_descent) {
lbfgs_ok = false;
break;
}
}

text
					float q_old[AGP_MAX_STATE_DIM];
					std::memcpy(q_old, q_inout, static_cast<size_t>(dim) * sizeof(float));

					float g_old[AGP_MAX_STATE_DIM];
					std::memcpy(g_old, gk, static_cast<size_t>(dim) * sizeof(float));

					float alpha_try = alpha_k;
					float f_try = f_io;
					float x_try = x_io;
					float y_try = y_io;
					unsigned idx_try = idx_io;
					bool clipped_lbfgs = false;

					const bool step_ok = armijoLineSearch(
						q_inout,
						f_io,
						gk,
						dir,
						gtd,
						alpha_try,
						q_try,
						f_try,
						x_try,
						y_try,
						idx_try,
						clipped_lbfgs);

					if (!step_ok) {
						lbfgs_ok = false;
						break;
					}

					std::memcpy(q_inout, q_try, static_cast<size_t>(dim) * sizeof(float));
					f_io = f_try;
					x_io = x_try;
					y_io = y_try;
					idx_io = idx_try;
					alpha_k = alpha_try;

					computeGrad(q_inout, x_io, y_io, gk, gk_norm2);
					if (!(gk_norm2 == gk_norm2)) {
						lbfgs_ok = false;
						break;
					}

					if (f_io < f_best_lbfgs) {
						f_best_lbfgs = f_io;
						std::memcpy(q_best_lbfgs, q_inout, static_cast<size_t>(dim) * sizeof(float));
						x_best_lbfgs = x_io;
						y_best_lbfgs = y_io;
						idx_best_lbfgs = idx_io;
					}

					float ys = 0.0f;
					float s_new[AGP_MAX_STATE_DIM];
					float y_new[AGP_MAX_STATE_DIM];
					d = 0;

#pragma loop ivdep
while (d < dim) {
const float sd = q_inout[d] - q_old[d];
const float yd = gk[d] - g_old[d];
s_new[d] = sd;
y_new[d] = yd;
ys = fmaf(yd, sd, ys);
++d;
}

text
					if (!(ys == ys) || ys <= eps_lbfgs_curv) {
						hist_size = 0;
					}
					else {
						if (hist_size < m_lbfgs) {
							const int idx_hist = hist_size;
							rho_hist[idx_hist] = 1.0f / ys;
							d = 0;

#pragma loop ivdep
while (d < dim) {
s_hist[idx_hist][d] = s_new[d];
y_hist[idx_hist][d] = y_new[d];
++d;
}
++hist_size;
}
else {
for (int jj = 0; jj < m_lbfgs - 1; ++jj) {
rho_hist[jj] = rho_hist[jj + 1];
d = 0;
#pragma loop ivdep
while (d < dim) {
s_hist[jj][d] = s_hist[jj + 1][d];
y_hist[jj][d] = y_hist[jj + 1][d];
++d;
}
}

text
							const int idx_hist = m_lbfgs - 1;
							rho_hist[idx_hist] = 1.0f / ys;
							d = 0;

#pragma loop ivdep
while (d < dim) {
s_hist[idx_hist][d] = s_new[d];
y_hist[idx_hist][d] = y_new[d];
++d;
}
hist_size = m_lbfgs;
}
}

text
					if (gk_norm2 < 1e-12f)
						break;

					++it_lbfgs;
				}

				if (lbfgs_ok) {
					std::memcpy(q_inout, q_best_lbfgs, static_cast<size_t>(dim) * sizeof(float));
					f_io = f_best_lbfgs;
					x_io = x_best_lbfgs;
					y_io = y_best_lbfgs;
					idx_io = idx_best_lbfgs;
					break;
				}
				else {
					if (f_best_lbfgs < f_resume) {
						std::memcpy(q_inout, q_best_lbfgs, static_cast<size_t>(dim) * sizeof(float));
						f_io = f_best_lbfgs;
						x_io = x_best_lbfgs;
						y_io = y_best_lbfgs;
						idx_io = idx_best_lbfgs;
					}
					else {
						std::memcpy(q_inout, q_resume, static_cast<size_t>(dim) * sizeof(float));
						f_io = f_resume;
						x_io = x_resume;
						y_io = y_resume;
						idx_io = idx_resume;
					}
					eta = eta_resume;
				}
			}

			++outer;
		}
	};

auto evaluate_trial_from_q = [&](const float* q_seed,
	float t_lo,
	float t_hi,
	bool enforce_t_bounds) noexcept -> TrialPoint
	{
		TrialPoint tr{};
		std::memcpy(tr.q, q_seed, static_cast<size_t>(dim) * sizeof(float));

		tr.feasible = cost.evaluate_indexed(tr.q, tr.x, tr.y, tr.idx, tr.f);

		if (tr.feasible) {
			refine_trial(tr.q, tr.x, tr.y, tr.idx, tr.f, t_lo, t_hi, enforce_t_bounds);
		}

		tr.t = clamp01(map.pointToT(tr.q));
		if (enforce_t_bounds) {
			if (tr.t < t_lo) tr.t = t_lo;
			if (tr.t > t_hi) tr.t = t_hi;
		}

		update_best_from_trial(tr);
		return tr;
	};

auto evaluate_trial_from_t = [&](float t_base,
	float t_lo,
	float t_hi,
	bool enforce_t_bounds) noexcept -> TrialPoint
	{
		alignas(16) float q_seed[AGP_MAX_STATE_DIM];
		map.map01ToPoint(t_base, q_seed);
		return evaluate_trial_from_q(q_seed, t_lo, t_hi, enforce_t_bounds);
	};

auto make_interval_from_trials = [&](const TrialPoint& Lp, const TrialPoint& Rp) noexcept -> IntervalND* {
	if (!(Lp.t + T_EPS < Rp.t))
		return nullptr;

	IntervalND* I = new IntervalND(Lp.t, Rp.t, Lp.f, Rp.f, Lp.idx, Rp.idx);
	finalize_interval_geometry(I);
	return I;
	};

auto find_container_index = [&](float t) noexcept -> int {
	int best_pos = -1;
	float best_len = FLT_MAX;

	for (size_t i = 0u; i < H.size(); ++i) {
		IntervalND* I = H[i];
		if (t > I->x1 + T_EPS && t < I->x2 - T_EPS) {
			const float len = fmaf(I->x2, 1.0f, -I->x1);
			if (len < best_len) {
				best_len = len;
				best_pos = static_cast<int>(i);
			}
		}
	}

	if (best_pos >= 0)
		return best_pos;

	for (size_t i = 0u; i < H.size(); ++i) {
		IntervalND* I = H[i];
		if (t >= I->x1 - T_EPS && t <= I->x2 + T_EPS) {
			const float len = fmaf(I->x2, 1.0f, -I->x1);
			if (len < best_len) {
				best_len = len;
				best_pos = static_cast<int>(i);
			}
		}
	}

	return best_pos;
	};

auto inject_trial_into_queue = [&](const TrialPoint& tr, float r_eff_cur, float adaptive_coeff_) noexcept {
	if (H.empty())
		return;

	const int pos = find_container_index(tr.t);
	if (pos < 0)
		return;

	IntervalND* src = H[static_cast<size_t>(pos)];
	const float src_len = fmaf(src->x2, 1.0f, -src->x1);

	if (!(tr.t > src->x1 + T_EPS && tr.t < src->x2 - T_EPS))
		return;

	IntervalND* L = new IntervalND(src->x1, tr.t, src->y1, tr.f, src->idx1, tr.idx);
	IntervalND* Rv = new IntervalND(tr.t, src->x2, tr.f, src->y2, tr.idx, src->idx2);

	finalize_interval_geometry(L);
	finalize_interval_geometry(Rv);

	const float prevMmax = Mmax;

	H[static_cast<size_t>(pos)] = H.back();
	H.pop_back();
	delete src;

	if (!H.empty())
		std::make_heap(H.begin(), H.end(), ComparePtrND);

	update_pockets_and_Mmax(L);
	update_pockets_and_Mmax(Rv);

	const float m_cur = fmaf(r_eff_cur, Mmax, 0.0f);
	L->ChangeCharacteristic(m_cur);
	Rv->ChangeCharacteristic(m_cur);

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

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

	if (Mmax > fmaf(adaptive_coeff_, prevMmax, 0.0f))
		recompute_heap_constM(m_cur);

	if (src_len + T_EPS >= dmax)
		recompute_dmax();
	};

auto progress_outgoing = [&]() noexcept {
	for (auto itReq = g_pendingMulti.begin(); itReq != g_pendingMulti.end();) {
		if (itReq->req.test())
			itReq = g_pendingMulti.erase(itReq);
		else
			++itReq;
	}

	for (auto itReq = g_pendingBest.begin(); itReq != g_pendingBest.end();) {
		if (itReq->req.test())
			itReq = g_pendingBest.erase(itReq);
		else
			++itReq;
	}
	};

auto build_hj_order = [&](int order_mode, int* order_out) noexcept {
	int pos = 0;

	switch (order_mode) {
	default:
	case AGP_HJ_ORDER_NATURAL:
		for (int d = 0; d < dim; ++d)
			order_out[pos++] = d;
		break;

	case AGP_HJ_ORDER_REVERSE:
		for (int d = dim - 1; d >= 0; --d)
			order_out[pos++] = d;
		break;

	case AGP_HJ_ORDER_INTERLEAVED_PROXIMAL:
		if (cost.variableLen) {
			for (int j = 0; j < n; ++j) {
				order_out[pos++] = j;
				order_out[pos++] = n + j;
			}
		}
		else {
			for (int j = 0; j < n; ++j)
				order_out[pos++] = j;
		}
		break;

	case AGP_HJ_ORDER_INTERLEAVED_DISTAL:
		if (cost.variableLen) {
			for (int j = n - 1; j >= 0; --j) {
				order_out[pos++] = j;
				order_out[pos++] = n + j;
			}
		}
		else {
			for (int j = n - 1; j >= 0; --j)
				order_out[pos++] = j;
		}
		break;

	case AGP_HJ_ORDER_LENGTHS_FIRST:
		if (cost.variableLen) {
			for (int j = 0; j < n; ++j)
				order_out[pos++] = n + j;
		}
		for (int j = 0; j < n; ++j)
			order_out[pos++] = j;
		break;
	}
	};

auto select_hj_seed = [&](const AlignedFloatVec*& seed_vec,
	float& seed_f,
	unsigned& seed_idx,
	float& seed_x,
	float& seed_y) noexcept -> bool
	{
		auto take_indexed = [&]() noexcept -> bool {
			if (bestQIndexed.empty())
				return false;
			seed_vec = &bestQIndexed;
			seed_f = bestIndexValue;
			seed_idx = bestIndexFound;
			seed_x = bestIndexedX;
			seed_y = bestIndexedY;
			return true;
			};

		auto take_feasible = [&]() noexcept -> bool {
			if (bestQ.empty())
				return false;
			seed_vec = &bestQ;
			seed_f = bestF;
			seed_idx = fullConstraintIndex;
			seed_x = bestX;
			seed_y = bestY;
			return true;
			};

		auto take_reference = [&]() noexcept -> bool {
			if (!haveTransitionRefSeed)
				return false;
			seed_vec = &transitionReferenceSeed;
			seed_f = transitionRefF;
			seed_idx = transitionRefIdx;
			seed_x = transitionRefX;
			seed_y = transitionRefY;
			return true;
			};

		seed_vec = nullptr;
		seed_f = FLT_MAX;
		seed_idx = 0u;
		seed_x = 0.0f;
		seed_y = 0.0f;

		if (cost.solveMode == ManipSolveMode::Transition)
			return take_feasible() || take_reference() || take_indexed();

		return take_indexed() || take_feasible();
	};

auto should_run_hj_this_iter = [&](float progress) noexcept -> bool {
	if (!hj_cfg.enabled)
		return false;

	if (hj_cfg.require_adaptive_flag && !adaptive)
		return false;

	int period = 1;
	int phase = 0;
	select_hj_iteration_schedule(progress, period, phase);

	if (period <= 1)
		return true;

	return (it % period) == phase;
	};

auto run_hooke_jeeves_iteration = [&](float r_eff_cur, float adaptive_coeff_, float progress) noexcept {
	if (H.empty())
		return;

	const AlignedFloatVec* seedQ = nullptr;
	float seedF = FLT_MAX;
	float seedX = 0.0f;
	float seedY = 0.0f;
	unsigned seedIdx = 0u;

	if (!select_hj_seed(seedQ, seedF, seedIdx, seedX, seedY))
		return;

	TrialPoint base{};
	std::memcpy(base.q, seedQ->data(), static_cast<size_t>(dim) * sizeof(float));
	base.t = clamp01(map.pointToT(base.q));
	base.f = seedF;
	base.idx = seedIdx;
	base.x = seedX;
	base.y = seedY;
	base.feasible = (base.idx == fullConstraintIndex);

	auto evaluate_hj_candidate = [&](const float* q_candidate) noexcept -> TrialPoint {
		const float t_guess = clamp01(map.pointToT(q_candidate));
		const int pos = find_container_index(t_guess);

		float lo = 0.0f;
		float hi = 1.0f;

		if (pos >= 0) {
			IntervalND* I = H[static_cast<size_t>(pos)];
			lo = I->x1;
			hi = I->x2;
		}

		TrialPoint tr = evaluate_trial_from_q(q_candidate, lo, hi, true);
		inject_trial_into_queue(tr, r_eff_cur, adaptive_coeff_);
		return tr;
		};

	apply_hj_coord_profile(progress);

	const int hj_order_mode = select_hj_order_mode(progress);

	alignas(16) int hj_order[AGP_MAX_STATE_DIM];
	build_hj_order(hj_order_mode, hj_order);

	TrialPoint current = base;
	TrialPoint base_before = base;
	bool improved_any = false;

	for (int ord = 0; ord < dim; ++ord) {
		const int d = hj_order[ord];
		const float step = hj_delta[d];

		const TrialPoint axis_base = current;

		auto try_direction = [&](int sgn, TrialPoint& accepted_tp) noexcept -> bool {
			float q_candidate[AGP_MAX_STATE_DIM];
			std::memcpy(q_candidate, axis_base.q, static_cast<size_t>(dim) * sizeof(float));
			q_candidate[d] = fmaf(static_cast<float>(sgn), step, q_candidate[d]);
			clampPoint(q_candidate);

			if (fabsf(q_candidate[d] - axis_base.q[d]) <= 1.0e-12f)
				return false;

			const TrialPoint tp = evaluate_hj_candidate(q_candidate);
			if (better_indexed(tp.idx, tp.f, axis_base.idx, axis_base.f)) {
				accepted_tp = tp;
				return true;
			}

			return false;
			};

		const int first_sign = hj_cfg.plus_first ? +1 : -1;
		const int second_sign = -first_sign;

		bool improved_this_axis = false;
		TrialPoint best_axis_tp{};
		bool have_axis_tp = false;

		TrialPoint tp_try{};
		if (try_direction(first_sign, tp_try)) {
			best_axis_tp = tp_try;
			have_axis_tp = true;
			if (hj_cfg.opportunistic_axis) {
				current = best_axis_tp;
				improved_any = true;
				improved_this_axis = true;
			}
		}

		if (!hj_cfg.opportunistic_axis || !have_axis_tp) {
			if (try_direction(second_sign, tp_try)) {
				if (!have_axis_tp || better_indexed(tp_try.idx, tp_try.f, best_axis_tp.idx, best_axis_tp.f)) {
					best_axis_tp = tp_try;
					have_axis_tp = true;
				}
			}

			if (have_axis_tp) {
				current = best_axis_tp;
				improved_any = true;
				improved_this_axis = true;
			}
		}

		if (improved_this_axis && hj_cfg.expand_delta_on_success) {
			hj_delta[d] = fmaxf(hj_cfg.min_delta, hj_delta[d] * hj_cfg.expand_factor);
		}
	}

	if (improved_any) {
		if (hj_cfg.use_pattern) {
			float q_pattern[AGP_MAX_STATE_DIM];
			for (int d = 0; d < dim; ++d)
				q_pattern[d] = fmaf(hj_cfg.pattern_gain, current.q[d] - base_before.q[d], current.q[d]);

			clampPoint(q_pattern);

			TrialPoint pat_tp = evaluate_hj_candidate(q_pattern);
			if (better_indexed(pat_tp.idx, pat_tp.f, current.idx, current.f))
				current = pat_tp;
		}
	}
	else {
		for (int d = 0; d < dim; ++d)
			hj_delta[d] = fmaxf(hj_delta[d] * hj_cfg.shrink_factor, hj_cfg.min_delta);
	}
	};

auto ensure_queue_nonempty = [&](float r_eff_cur) noexcept -> bool {
	if (!H.empty())
		return true;

	const AlignedFloatVec* seedQ = nullptr;
	float seedF = FLT_MAX;
	float seedX = 0.0f;
	float seedY = 0.0f;
	unsigned seedIdx = 0u;

	if (!select_hj_seed(seedQ, seedF, seedIdx, seedX, seedY))
		return false;

	(void)seedF;
	(void)seedX;
	(void)seedY;
	(void)seedIdx;

	const float t_center = clamp01(map.pointToT(seedQ->data()));
	const float half = fmaxf(1.0e-4f * dim_f, 1.0e-6f);
	const float t1b = clamp01(t_center - half);
	const float t2b = clamp01(t_center + half);
	const float mid = 0.5f * (t1b + t2b);

	const TrialPoint left_tp = evaluate_trial_from_t(t1b, t1b, mid, true);
	const TrialPoint right_tp = evaluate_trial_from_t(t2b, mid, t2b, true);

	IntervalND* I = make_interval_from_trials(left_tp, right_tp);
	if (I == nullptr)
		return false;

	update_pockets_and_Mmax(I);
	I->ChangeCharacteristic(fmaf(r_eff_cur, Mmax, 0.0f));

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

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 seedStride = AGP_MAX_SEED_STRIDE;
AlignedFloatVec seeds(static_cast<size_t>(AGP_MAX_GENERATED_SEEDS) * static_cast<size_t>(seedStride), 0.0f);

const int seedCnt = generate_heuristic_seeds(
	cost,
	map,
	dim,
	seeds.data(),
	seedStride,
	static_cast<unsigned>(fmaf(static_cast<float>(g_world->rank()), 7919.0f, static_cast<float>(seed))));

const int K_raw = static_cast<int>(fmaf(-fmaf(sqrtf(dim_f), dim_f, 0.0f), 0.725f, 10.95f));
const int K = (K_raw > 2) ? K_raw : 2;
H.reserve(static_cast<size_t>(maxIter) + static_cast<size_t>(K) + static_cast<size_t>(seedCnt) + 32u);
const int rank = g_world->rank();
const int world = g_world->size();

for (int i = 0; i < seedCnt; ++i) {
	const float* s = seeds.data() + static_cast<size_t>(i) * static_cast<size_t>(seedStride);
	const float t_seed = clamp01(map.pointToT(s));

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

	const float t1b = clamp01(fmaf(-interval_size, 1.0f, t_seed));
	const float t2b = clamp01(fmaf(interval_size, 1.0f, t_seed));
	const float mid = 0.5f * (t1b + t2b);

	const TrialPoint left_tp = evaluate_trial_from_t(t1b, t1b, mid, true);
	const TrialPoint right_tp = evaluate_trial_from_t(t2b, mid, t2b, true);

	IntervalND* I = make_interval_from_trials(left_tp, right_tp);
	if (I == nullptr)
		continue;

	update_pockets_and_Mmax(I);
	I->ChangeCharacteristic(fmaf(r, Mmax, 0.0f));

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

std::vector<float> grid_base;
grid_base.reserve(static_cast<size_t>(K) + 2u);
grid_base.emplace_back(a);

for (int k = 1; k <= K; ++k) {
	const float t = clamp01(
		fmaf(
			fmaf(b - a, static_cast<float>(k) / static_cast<float>(K + 1), a),
			1.0f,
			static_cast<float>(rank) / static_cast<float>(world * (K + 1))));
	grid_base.emplace_back(t);
}

grid_base.emplace_back(b);

std::vector<TrialPoint> grid_trials(grid_base.size());

for (size_t i = 0u; i < grid_base.size(); ++i) {
	float lo = grid_base[i];
	float hi = grid_base[i];

	if (i == 0u) {
		lo = hi = a;
	}
	else if (i + 1u == grid_base.size()) {
		lo = hi = b;
	}
	else {
		lo = 0.5f * (grid_base[i - 1u] + grid_base[i]);
		hi = 0.5f * (grid_base[i] + grid_base[i + 1u]);
	}

	grid_trials[i] = evaluate_trial_from_t(grid_base[i], lo, hi, true);
}

for (size_t i = 1u; i < grid_trials.size(); ++i) {
	IntervalND* I = make_interval_from_trials(grid_trials[i - 1u], grid_trials[i]);
	if (I == nullptr)
		continue;

	update_pockets_and_Mmax(I);
	I->ChangeCharacteristic(fmaf(r, Mmax, 0.0f));
	H.emplace_back(I);
	std::push_heap(H.begin(), H.end(), ComparePtrND);
}

recompute_dmax();
if (haveTransitionRefSeed) {
	TrialPoint ref_tp = evaluate_trial_from_q(transitionReferenceSeed.data(), 0.0f, 1.0f, false);
	if (!H.empty()) {
		inject_trial_into_queue(ref_tp, r, 1.0f);
		recompute_dmax();
	}
	else {
		ensure_queue_nonempty(r);
	}
}

g_world->barrier();

while (true) {
	p = fmaf(-1.0f / initial_len, dmax, 1.0f);

	stag_r_multiplier = 1.4f - 1.1f * fmaf(
		0.65f * p - 0.45f,
		fmaf(
			0.65f * p - 0.45f,
			fmaf(
				0.65f * p - 0.45f,
				fmaf(
					0.65f * p - 0.45f,
					fmaf(0.65f * p - 0.45f, 0.164056f, -0.098462f),
					0.240884f),
				-0.351834f),
			0.999996f),
		0.65f * p - 0.45f);

	const float p_arg = fmaf(p, 2.3f, -2.9775f);

	float current_r = r;
	if (stag_boost_remaining > 0) {
		current_r = r * stag_r_multiplier;
		--stag_boost_remaining;
	}

	const float exp_arg = fmaf(B_dim, p, 0.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);

	const float first_sqrt = sqrtf(fmaf(1.0f / dim_f, 2.0f, 0.0f));
	const float second_sqrt = sqrtf(fmaf(1.0f / (dim_f + 7.0f), 5.0f, 0.0f));
	const float third_sqrt = sqrtf(fmaf(1.0f / (dim_f + 7.0f), 9.0f, 0.0f));
	const float fourth_sqrt = sqrtf(fmaf(1.0f / (dim_f + 7.0f), 6.5f, 0.0f));
	const float rr = sqrtf(fmaf(-p, 1.0f, 1.0f));
	const float xx = p * p;
	const float tt = fmaf(500.0f, p, -486.95472f);

	const float adaptive_coeff_ =
		(p < 0.95f)
		? fmaf(fmaf(first_sqrt, xx, 0.0f), 0.0130349902f,
			fmaf(-0.04f, p, fmaf(fmaf(first_sqrt, rr, 0.0f), 0.15f, 1.1f)))
		: (p < 0.97390944f)
		? fmaf(second_sqrt, rr, 0.9396f)
		: (p < 0.97590944f)
		? fmaf(
			fmaf(
				fmaf(fmaf(third_sqrt, tt, 0.0f), tt, 0.0f),
				fmaf(-2.0f, tt, 3.0f),
				0.0f),
			fmaf(0.25f, rr, -0.0396f),
			fmaf(fmaf(third_sqrt, rr, 0.0f), 0.75f, 0.9396f))
		: fmaf(fourth_sqrt, rr, 0.925f);

	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__,
		2.0f - A_dim__);

	float grad_norm2_best = 0.0f;
	if (no_improve > 0 && !bestQ.empty()) {
		float acc_best = 0.0f;
		float phi_best[AGP_MAX_STATE_DIM];
		int ii_best = 0;

#pragma loop ivdep
while (ii_best < n) {
acc_best = fmaf(bestQ[ii_best], 1.0f, acc_best);
phi_best[ii_best] = acc_best;
++ii_best;
}

text
		float s_best[AGP_MAX_STATE_DIM];
		float c_best[AGP_MAX_STATE_DIM];
		FABE13_SINCOS(phi_best, s_best, c_best, n);

		float as_best = 0.0f;
		float ac_best = 0.0f;
		float sum_s_best[AGP_MAX_STATE_DIM];
		float sum_c_best[AGP_MAX_STATE_DIM];

		int k_best = n - 1;
		while (k_best >= 0) {
			const float Lk = cost.variableLen ? bestQ[n + k_best] : cost.fixedLength;
			as_best = fmaf(Lk, s_best[k_best], as_best);
			ac_best = fmaf(Lk, c_best[k_best], ac_best);
			sum_s_best[k_best] = as_best;
			sum_c_best[k_best] = ac_best;
			--k_best;
		}

		alignas(16) float prefix_tail_best[AGP_MAX_STATE_DIM]{};
		if (cost.solveMode == ManipSolveMode::Transition &&
			cost.has_transition_reference_angles() &&
			cost.transitionPrefixEnergyWeight > 0.0f)
		{
			alignas(16) float prefix_delta_best[AGP_MAX_STATE_DIM]{};
			float phi_pref = 0.0f;
			int jj = 0;

#pragma loop ivdep
while (jj < cost.referenceAngleCount) {
phi_pref += bestQ[jj];
prefix_delta_best[jj] = ManipCost::wrap_pi(phi_pref - cost.referencePrefixAngles[jj]);
++jj;
}

text
			float tail = 0.0f;
			jj = cost.referenceAngleCount - 1;
			while (jj >= 0) {
				tail += prefix_delta_best[jj];
				prefix_tail_best[jj] = tail;
				--jj;
			}
		}

		const float dx_best = fmaf(bestX, 1.0f, -cost.targetX);
		const float dy_best = fmaf(bestY, 1.0f, -cost.targetY);
		const float dist_best = sqrtf(fmaf(dx_best, dx_best, dy_best * dy_best));
		const float inv_dist_best = (dist_best > 1.0e-20f) ? (1.0f / dist_best) : 0.0f;

		int i_best = 0;

#pragma loop ivdep
while (i_best < n) {
float gpen_best = 0.0f;
float g_main_best = 0.0f;

text
			if (cost.solveMode == ManipSolveMode::Transition) {
				const float dtheta_best = (i_best < cost.referenceAngleCount)
					? cost.wrapped_angle_delta(bestQ.data(), i_best)
					: 0.0f;

				gpen_best = 2.0f * cost.transitionEnergyWeight * dtheta_best;
				gpen_best = fmaf(2.0f * cost.transitionPrefixEnergyWeight, prefix_tail_best[i_best], gpen_best);

				g_main_best = 2.0f * cost.transitionCaptureWeight *
					fmaf(dx_best, -sum_s_best[i_best], dy_best * sum_c_best[i_best]);
			}
			else {
				{
					const float tsg = fmaf(-bestQ[i_best], cost.archBiasK, 0.0f);
					const float exp_arg_g = -tsg;
					const float exp_val = fmaf(
						exp_arg_g,
						fmaf(
							exp_arg_g,
							fmaf(
								exp_arg_g,
								fmaf(exp_arg_g, 0.00833333377f, 0.0416666679f),
								0.16666667f),
							0.5f),
						1.0f);
					const float sig = 1.0f / (exp_val + 1.0f);
					gpen_best = fmaf(-cost.archBiasW * cost.archBiasK, sig, gpen_best);
				}

				g_main_best = fmaf(dx_best, -sum_s_best[i_best], dy_best * sum_c_best[i_best]) * inv_dist_best;
			}

			const float gi_best = g_main_best + gpen_best;
			grad_norm2_best = fmaf(gi_best, gi_best, grad_norm2_best);
			++i_best;
		}

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

#pragma loop ivdep
while (j_best < n) {
float gi_best = 0.0f;
if (cost.solveMode == ManipSolveMode::Transition) {
float gpenL_best = 0.0f;
if (cost.has_transition_reference_lengths() && j_best < cost.referenceLengthCount) {
gpenL_best = 2.0f * cost.transitionLengthEnergyWeight *
(bestQ[n + j_best] - cost.referenceState[n + j_best]);
}

text
					gi_best = 2.0f * cost.transitionCaptureWeight *
						fmaf(dx_best, c_best[j_best], dy_best * s_best[j_best]) + gpenL_best;
				}
				else {
					gi_best = fmaf(dx_best, c_best[j_best], dy_best * s_best[j_best]) * inv_dist_best;
				}
				grad_norm2_best = fmaf(gi_best, gi_best, grad_norm2_best);
				++j_best;
			}
		}
	}

	const bool stagnation = (no_improve > noImproveThrDim) && (grad_norm2_best < 0.5e-1f);

	const float r_eff = (dim > 2)
		? 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,
			fmaf(sqrtf(dim_f - 1), current_r, 0.0f),
			0.0f)
		: 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,
			current_r,
			0.0f);

	if (stagnation) {
		stag_boost_remaining = n_stag_iters;
		float t_seeds[32];
		int seed_count = 0;

		if (cost.solveMode == ManipSolveMode::Transition && cost.has_transition_reference_angles()) {
			alignas(16) float q_center[AGP_MAX_STATE_DIM];
			std::memcpy(q_center, cost.referenceState, static_cast<size_t>(dim) * sizeof(float));
			t_seeds[seed_count++] = clamp01(map.pointToT(q_center));

			if (!bestQ.empty() && seed_count < 32)
				t_seeds[seed_count++] = clamp01(map.pointToT(bestQ.data()));

			unsigned st_local = seed + static_cast<unsigned>(it) + 911u;
			const int local_variants = 2 + static_cast<int>(sqrtf(dim_f));

			for (int v = 0; v < local_variants && seed_count < 32; ++v) {
				float q_temp[AGP_MAX_STATE_DIM];
				std::memcpy(q_temp, q_center, static_cast<size_t>(dim) * sizeof(float));

				st_local ^= st_local << 13;
				st_local ^= st_local >> 17;
				st_local ^= st_local << 5;
				const float rnd0 = static_cast<float>(st_local & 0xFFFFFFu) * 5.9604645e-8f;

				const int aidx = v % n;
				q_temp[aidx] = fmaf((2.0f * rnd0 - 1.0f), 0.25f * cost.transitionMaxAngleStep, q_temp[aidx]);

				if (cost.variableLen && cost.has_transition_reference_lengths()) {
					st_local ^= st_local << 13;
					st_local ^= st_local >> 17;
					st_local ^= st_local << 5;
					const float rnd1 = static_cast<float>(st_local & 0xFFFFFFu) * 5.9604645e-8f;

					const int lidx = v % n;
					q_temp[n + lidx] = fmaf((2.0f * rnd1 - 1.0f), 0.25f * cost.max_length_step(lidx), q_temp[n + lidx]);
				}

				clampPoint(q_temp);
				t_seeds[seed_count++] = clamp01(map.pointToT(q_temp));
			}
		}
		else {
			const int num_ik = 1 + static_cast<int>(sqrtf(dim_f));
			float dist_to_target = sqrtf(cost.targetX * cost.targetX + cost.targetY * cost.targetY);
			float max_reach = 0.0f;

			if (cost.variableLen) {
				for (int ii = 0; ii < n; ++ii)
					max_reach += map.high[n + ii];
			}
			else {
				max_reach = static_cast<float>(n) * cost.fixedLength;
			}

			const float ratio = dist_to_target / fmaxf(max_reach, 1.0e-20f);
			const bool prefer_extended = (ratio > 0.7f);
			const bool prefer_compact = (ratio < 0.4f);
			const bool use_ik = !(ratio > 0.4f && ratio < 0.7f);

			if (!use_ik) {
				AlignedFloatVec temp_S(static_cast<size_t>(AGP_MAX_GENERATED_SEEDS) * static_cast<size_t>(AGP_MAX_SEED_STRIDE), 0.0f);
				const int sobol_gen = generate_sobol_seeds_limited(
					map,
					dim,
					temp_S.data(),
					AGP_MAX_SEED_STRIDE,
					seed + static_cast<unsigned>(it),
					AGP_MAX_GENERATED_SEEDS);

				const int num_sobol = num_ik;
				for (int kk = 0; kk < num_sobol && kk < sobol_gen && seed_count < 32; ++kk) {
					const float* s = temp_S.data() + static_cast<size_t>(kk) * static_cast<size_t>(AGP_MAX_SEED_STRIDE);
					t_seeds[seed_count++] = clamp01(map.pointToT(s));
				}
			}
			else {
				float angles_ccd[32] = { 0.0f };
				float lengths_ccd[32];

				if (cost.variableLen) {
					const float len_low = map.low[n];
					const float len_high = map.high[n];
					const float avg_len = 0.5f * (len_low + len_high);
					for (int ii = 0; ii < n; ++ii)
						lengths_ccd[ii] = avg_len;
				}
				else {
					for (int ii = 0; ii < n; ++ii)
						lengths_ccd[ii] = cost.fixedLength;
				}

				ccd_ik(cost.targetX, cost.targetY, lengths_ccd, n, angles_ccd, 10);

				float angles_fabrik[32] = { 0.0f };
				float lengths_fabrik[32];

				if (cost.variableLen) {
					for (int ii = 0; ii < n; ++ii)
						lengths_fabrik[ii] = lengths_ccd[ii];
				}
				else {
					for (int ii = 0; ii < n; ++ii)
						lengths_fabrik[ii] = cost.fixedLength;
				}

				float targetX_fab = cost.targetX;
				float targetY_fab = cost.targetY;

				for (int iter_fab = 0; iter_fab < 3; ++iter_fab) {
					float prevX = targetX_fab;
					float prevY = targetY_fab;
					for (int j = n - 1; j >= 0; --j) {
						const float len = lengths_fabrik[j];
						const float angle_to_target = atan2f(prevY, prevX);
						angles_fabrik[j] = angle_to_target;
						float s_val;
						float c_val;
						FABE13_SINCOS(&angle_to_target, &s_val, &c_val, 1);
						prevX = prevX - len * c_val;
						prevY = prevY - len * s_val;
					}
				}

				if (prefer_extended) {
					{
						float q_ccd[AGP_MAX_STATE_DIM];
						for (int ii = 0; ii < n; ++ii)
							q_ccd[ii] = angles_ccd[ii];
						if (cost.variableLen) {
							for (int ii = 0; ii < n; ++ii)
								q_ccd[n + ii] = lengths_ccd[ii];
						}
						t_seeds[seed_count++] = clamp01(map.pointToT(q_ccd));
					}

					unsigned st_ik = seed + static_cast<unsigned>(it) + 222u;
					const int remaining = num_ik - 1;

					for (int v = 0; v < remaining && seed_count < 32; ++v) {
						float noisy_angles[32];
						float noisy_lengths[32];

						for (int ii = 0; ii < n; ++ii) {
							st_ik ^= st_ik << 13;
							st_ik ^= st_ik >> 17;
							st_ik ^= st_ik << 5;
							const float rnd = static_cast<float>(st_ik & 0xFFFFFFu) * 5.9604645e-8f;
							noisy_angles[ii] = angles_ccd[ii] + (2.0f * rnd - 1.0f) * 0.1f;
							const float lo = (ii == 0) ? -1.0471975511965976f : -2.6179938779914944f;
							const float hi = 2.6179938779914944f;
							if (noisy_angles[ii] < lo) noisy_angles[ii] = lo;
							if (noisy_angles[ii] > hi) noisy_angles[ii] = hi;
						}

						if (cost.variableLen) {
							for (int ii = 0; ii < n; ++ii) {
								st_ik ^= st_ik << 13;
								st_ik ^= st_ik >> 17;
								st_ik ^= st_ik << 5;
								const float rnd = static_cast<float>(st_ik & 0xFFFFFFu) * 5.9604645e-8f;
								noisy_lengths[ii] = lengths_ccd[ii] + (2.0f * rnd - 1.0f) * 0.05f;
								if (noisy_lengths[ii] < 0.5f) noisy_lengths[ii] = 0.5f;
								if (noisy_lengths[ii] > 2.0f) noisy_lengths[ii] = 2.0f;
							}
						}

						float q_temp[AGP_MAX_STATE_DIM];
						for (int ii = 0; ii < n; ++ii)
							q_temp[ii] = noisy_angles[ii];
						if (cost.variableLen) {
							for (int ii = 0; ii < n; ++ii)
								q_temp[n + ii] = noisy_lengths[ii];
						}

						t_seeds[seed_count++] = clamp01(map.pointToT(q_temp));
					}
				}
				else if (prefer_compact) {
					{
						float q_fabrik[AGP_MAX_STATE_DIM];
						for (int ii = 0; ii < n; ++ii)
							q_fabrik[ii] = angles_fabrik[ii];
						if (cost.variableLen) {
							for (int ii = 0; ii < n; ++ii)
								q_fabrik[n + ii] = lengths_fabrik[ii];
						}
						t_seeds[seed_count++] = clamp01(map.pointToT(q_fabrik));
					}

					unsigned st_ik = seed + static_cast<unsigned>(it) + 222u;
					const int remaining = num_ik - 1;

					for (int v = 0; v < remaining && seed_count < 32; ++v) {
						float noisy_angles[32];
						float noisy_lengths[32];

						for (int ii = 0; ii < n; ++ii) {
							st_ik ^= st_ik << 13;
							st_ik ^= st_ik >> 17;
							st_ik ^= st_ik << 5;
							const float rnd = static_cast<float>(st_ik & 0xFFFFFFu) * 5.9604645e-8f;
							noisy_angles[ii] = angles_fabrik[ii] + (2.0f * rnd - 1.0f) * 0.06f;
							const float lo = (ii == 0) ? -1.0471975511965976f : -2.6179938779914944f;
							const float hi = 2.6179938779914944f;
							if (noisy_angles[ii] < lo) noisy_angles[ii] = lo;
							if (noisy_angles[ii] > hi) noisy_angles[ii] = hi;
						}

						if (cost.variableLen) {
							for (int ii = 0; ii < n; ++ii) {
								st_ik ^= st_ik << 13;
								st_ik ^= st_ik >> 17;
								st_ik ^= st_ik << 5;
								const float rnd = static_cast<float>(st_ik & 0xFFFFFFu) * 5.9604645e-8f;
								noisy_lengths[ii] = lengths_fabrik[ii] + (2.0f * rnd - 1.0f) * 0.03f;
								if (noisy_lengths[ii] < 0.5f) noisy_lengths[ii] = 0.5f;
								if (noisy_lengths[ii] > 2.0f) noisy_lengths[ii] = 2.0f;
							}
						}

						float q_temp[AGP_MAX_STATE_DIM];
						for (int ii = 0; ii < n; ++ii)
							q_temp[ii] = noisy_angles[ii];
						if (cost.variableLen) {
							for (int ii = 0; ii < n; ++ii)
								q_temp[n + ii] = noisy_lengths[ii];
						}

						t_seeds[seed_count++] = clamp01(map.pointToT(q_temp));
					}
				}
			}
		}

		for (int s = 0; s < seed_count; ++s) {
			const float t_center = t_seeds[s];
			const float interval_size = fmaf(0.00031f, static_cast<float>(dim), 0.0f);
			const float t1b = clamp01(t_center - 0.5f * interval_size);
			const float t2b = clamp01(t_center + 0.5f * interval_size);
			const float mid = 0.5f * (t1b + t2b);

			const TrialPoint left_tp = evaluate_trial_from_t(t1b, t1b, mid, true);
			const TrialPoint right_tp = evaluate_trial_from_t(t2b, mid, t2b, true);

			IntervalND* I = make_interval_from_trials(left_tp, right_tp);
			if (I == nullptr)
				continue;

			update_pockets_and_Mmax(I);
			I->ChangeCharacteristic(fmaf(r_eff, Mmax, 0.0f));
			I->R = fmaf(I->R, fmaf(0.01f, dim_f, 0.85f), 0.0f);
			H.emplace_back(I);
			std::push_heap(H.begin(), H.end(), ComparePtrND);
		}

		recompute_dmax();
		no_improve = 0;
	}

	const float bestFOld = bestF;

	bool local_idle = false;
	if (H.empty())
		local_idle = !ensure_queue_nonempty(r_eff);

	const bool do_hj_this_iter = should_run_hj_this_iter(p);

	if (!local_idle) {
		if (do_hj_this_iter) {
			run_hooke_jeeves_iteration(r_eff, adaptive_coeff_, p);
		}
		else {
			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);
			const float tBase = step(m, x1, x2, y1, y2, dim_f, r_eff, cur->idx1, cur->idx2);

			const TrialPoint trNew = evaluate_trial_from_t(tBase, x1, x2, true);

			IntervalND* L = new IntervalND(x1, trNew.t, y1, trNew.f, cur->idx1, trNew.idx);
			IntervalND* Rv = new IntervalND(trNew.t, x2, trNew.f, y2, trNew.idx, cur->idx2);

			finalize_interval_geometry(L);
			finalize_interval_geometry(Rv);

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

			m = fmaf(r_eff, Mmax, 0.0f);

			const float len1 = fmaf(trNew.t, 1.0f, -x1);
			const float len2 = fmaf(x2, 1.0f, -trNew.t);

			if (fmaf(len1, 1.0f, len2) >= dmax - T_EPS) {
				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 / fmaxf(dmax, 1.0e-20f)) * 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();
				if (sz > 0u) {
					if (!hasHardConstraints)
						RecomputeR_AffineM_AVX2_ND(H.data(), sz, GF, alpha);
					else
						RecomputeR_AffineM_Mixed_ND(H.data(), sz, GF, alpha);

					std::make_heap(H.begin(), H.end(), ComparePtrND);
				}
			}
			else {
				L->ChangeCharacteristic(m);
				Rv->ChangeCharacteristic(m);

				if (Mloc > prevMmax && Mloc > fmaf(adaptive_coeff_, prevMmax, 0.0f))
					recompute_heap_constM(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);

			delete cur;
		}
	}

	if (!H.empty()) {
		_mm_prefetch(reinterpret_cast<const char*>(H[0]), _MM_HINT_T0);
		if (H.size() > 1u)
			_mm_prefetch(reinterpret_cast<const char*>(H[1]), _MM_HINT_T0);
	}

	if (!H.empty() &&
		(stagnation || bestF < fmaf(fmaf(bestFOld, 0.6f, 0.0f), adaptive_coeff__, 0.0f)) &&
		it - last_send_T >= send_interval_T)
	{
		last_send_T = it;
		progress_outgoing();

		const size_t scan_count = H.size();
		unsigned intervals_to_send = (dim < 12)
			? static_cast<unsigned>(sqrtf(fmaf(dim_f, 5.5f, 0.0f)))
			: 7u;

		if (intervals_to_send > scan_count)
			intervals_to_send = static_cast<unsigned>(scan_count);

		if (intervals_to_send > 0u) {
			const float alpha = 0.63f - sqrtf(fmaxf(-0.113f * (p - 1.0f), 0.0f));
			const float beta = 1.0f - alpha;

			const float w_pos = 3.085f * (-sqrtf(fmaxf(0.007f * p, 0.0f)) + 1.0f);
			const float w_size = 5.085f - w_pos;
			const int num_bins = (p < 1.0f - 1.0e-9f) ? static_cast<int>(1.0f / (1.0f - p)) : 1;

			std::vector<float> pos_metric(scan_count);
			std::vector<float> size_metric(scan_count);
			std::vector<float> R_val(scan_count);

			float R_max = -FLT_MAX;
			float R_min = FLT_MAX;
			float size_max = -FLT_MAX;
			float size_min = FLT_MAX;
			float pos_max = -FLT_MAX;
			float pos_min = FLT_MAX;

			for (size_t idx = 0u; idx < scan_count; ++idx) {
				IntervalND* I = H[idx];
				const float center = 0.5f * (I->x1 + I->x2);
				const float len = fmaf(I->x2, 1.0f, -I->x1);
				const float size = len * static_cast<float>(1 << I->span_level);

				if (p < 0.95f && num_bins > 1) {
					float bin = floorf(center * static_cast<float>(num_bins));
					if (bin >= static_cast<float>(num_bins))
						bin = static_cast<float>(num_bins - 1);
					pos_metric[idx] = bin / static_cast<float>(num_bins - 1);
				}
				else {
					pos_metric[idx] = center;
				}

				size_metric[idx] = size;
				R_val[idx] = I->R;

				if (pos_metric[idx] > pos_max) pos_max = pos_metric[idx];
				if (pos_metric[idx] < pos_min) pos_min = pos_metric[idx];
				if (size > size_max) size_max = size;
				if (size < size_min) size_min = size;
				if (I->R > R_max) R_max = I->R;
				if (I->R < R_min) R_min = I->R;
			}

			auto norm = [](float val, float minv, float maxv) noexcept -> float {
				const float denom = maxv - minv;
				if (fabsf(denom) <= 1.0e-20f)
					return 0.0f;
				return (val - minv) / denom;
				};

			std::vector<float> R_norm(scan_count);
			std::vector<float> pos_norm(scan_count);
			std::vector<float> size_norm(scan_count);

			for (size_t idx = 0u; idx < scan_count; ++idx) {
				R_norm[idx] = norm(R_val[idx], R_min, R_max);
				pos_norm[idx] = norm(pos_metric[idx], pos_min, pos_max);
				size_norm[idx] = norm(size_metric[idx], size_min, size_max);
			}

			std::vector<int> selected;
			selected.reserve(intervals_to_send);
			std::vector<bool> used(scan_count, false);

			selected.push_back(0);
			used[0] = true;

			while (selected.size() < intervals_to_send) {
				int best_idx = -1;
				float best_score = -FLT_MAX;

				for (size_t idx = 0u; idx < scan_count; ++idx) {
					if (used[idx])
						continue;

					float min_dist = FLT_MAX;
					for (int s : selected) {
						float d2 = 0.0f;
						float d = pos_norm[idx] - pos_norm[static_cast<size_t>(s)];
						d2 += w_pos * d * d;
						d = size_norm[idx] - size_norm[static_cast<size_t>(s)];
						d2 += w_size * d * d;
						if (d2 < min_dist)
							min_dist = d2;
					}

					const float novelty = sqrtf(fmaxf(min_dist, 0.0f)) / sqrtf(fmaxf(w_pos + w_size, 1.0e-20f));
					const float score = alpha * R_norm[idx] + beta * novelty;

					if (score > best_score) {
						best_score = score;
						best_idx = static_cast<int>(idx);
					}
				}

				if (best_idx < 0)
					break;

				selected.push_back(best_idx);
				used[static_cast<size_t>(best_idx)] = true;
			}

			MultiCrossMsg out{};
			out.count = static_cast<unsigned>(selected.size());

			float* dest = out.intervals;
			for (unsigned s = 0u; s < out.count; ++s) {
				IntervalND* Tt = H[static_cast<size_t>(selected[s])];
				dest[0] = Tt->x1;
				dest[1] = 0.0f;
				dest[2] = Tt->x2;
				dest[3] = 0.0f;
				dest[4] = Tt->R;
				dest += 5;
			}

			const size_t iterations = std::bit_width(static_cast<size_t>(world - 1));
			bool active = true;
			const bool invert_T = ((static_cast<int>(exchange_counter_T) + 1) & 1) != 0;

			size_t ii2 = 0u;
			while (ii2 < iterations && active) {
				const size_t step = 1ULL << ii2;
				const int partner = rank ^ static_cast<int>(step);
				if (partner < world) {
					const bool am_sender = ((!!(rank & static_cast<int>(step))) ^ invert_T);
					if (am_sender) {
						g_pendingMulti.emplace_back(*g_world, partner, out);
						if (g_pendingMulti.size() > MAX_INFLIGHT_MULTI) {
							g_pendingMulti.front().req.wait();
							g_pendingMulti.pop_front();
						}
						active = false;
					}
				}
				++ii2;
			}

			++exchange_counter_T;
		}
	}

	const bool indexedBestImprovedSinceLastSend =
		!bestQIndexed.empty() &&
		better_indexed(bestIndexFound, bestIndexValue, lastSentBestMsg.bestIndex, lastSentBestMsg.bestF);

	if (indexedBestImprovedSinceLastSend && it - last_send_best >= send_interval_best) {
		last_send_best = it;
		progress_outgoing();

		const size_t iterations = std::bit_width(static_cast<size_t>(world - 1));
		bool active = true;
		const bool invert_best = ((static_cast<int>(exchange_counter) + 1) & 1) != 0;

		size_t ii2 = 0u;
		while (ii2 < iterations && active) {
			const size_t step = 1ULL << ii2;
			const int partner = rank ^ static_cast<int>(step);
			if (partner < world) {
				const bool am_sender = ((!!(rank & static_cast<int>(step))) ^ invert_best);
				if (am_sender) {
					BestSolutionMsg outMsg;
					InitBestSolutionMsg(outMsg);

					if (FillBestSolutionMsg(
						outMsg,
						bestIndexFound,
						bestIndexValue,
						bestIndexedX,
						bestIndexedY,
						bestQIndexed) &&
						better_indexed(
							outMsg.bestIndex,
							outMsg.bestF,
							lastSentBestMsg.bestIndex,
							lastSentBestMsg.bestF))
					{
						g_pendingBest.emplace_back(*g_world, partner, outMsg);
						lastSentBestMsg = outMsg;
					}

					if (g_pendingBest.size() > MAX_INFLIGHT_BEST) {
						g_pendingBest.front().req.wait();
						g_pendingBest.pop_front();
					}
					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);

		for (unsigned ii = 0u; ii < in.count; ++ii) {
			const float* d = &in.intervals[ii * 5u];
			const float sx = d[0];
			const float ex = d[2];

			alignas(16) float ql[AGP_MAX_STATE_DIM];
			float tx = 0.0f;
			float ty = 0.0f;

			unsigned v1i = 0u;
			unsigned v2i = 0u;
			float y1i = 0.0f;
			float y2i = 0.0f;

			map.map01ToPoint(sx, ql);
			cost.evaluate_indexed(ql, tx, ty, v1i, y1i);

			map.map01ToPoint(ex, ql);
			cost.evaluate_indexed(ql, tx, ty, v2i, y2i);

			IntervalND* inj = new IntervalND(sx, ex, y1i, y2i, v1i, v2i);
			finalize_interval_geometry(inj);

			const float prevMmax = Mmax;
			const float candidateMmax = fmaxf(Mmax, inj->M);
			const float m_inj = fmaf(r_eff, candidateMmax, 0.0f);
			inj->ChangeCharacteristic(m_inj);

			if (H.empty() || inj->R > fmaf(adaptive_coeff, H.front()->R, 0.0f) || stagnation) {
				update_pockets_and_Mmax(inj);
				H.emplace_back(inj);
				std::push_heap(H.begin(), H.end(), ComparePtrND);

				if (Mmax > fmaf(adaptive_coeff_, prevMmax, 0.0f))
					recompute_heap_constM(fmaf(r_eff, Mmax, 0.0f));
			}
			else {
				delete inj;
			}
		}
	}

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

		UpdateIndexedBestFromMessage(
			incoming,
			fullConstraintIndex,
			bestIndexFound,
			bestIndexValue,
			bestQIndexed,
			bestIndexedX,
			bestIndexedY,
			bestF,
			bestQ,
			bestX,
			bestY);
	}

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

	if ((dim > 1 ? exp2f((1.0f / dim_f) * log2f(interval_len)) : interval_len) < eps || it == maxIter - 1) {
		for (auto& s : g_pendingMulti)
			s.req.wait();
		for (auto& s : g_pendingBest)
			s.req.wait();

		g_pendingMulti.clear();
		g_pendingBest.clear();

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

			UpdateIndexedBestFromMessage(
				incoming,
				fullConstraintIndex,
				bestIndexFound,
				bestIndexValue,
				bestQIndexed,
				bestIndexedX,
				bestIndexedY,
				bestF,
				bestQ,
				bestX,
				bestY);
		}

		g_world->barrier();

		BestSolutionMsg bestMsg;
		InitBestSolutionMsg(bestMsg);

		if (!bestQIndexed.empty()) {
			FillBestSolutionMsg(
				bestMsg,
				bestIndexFound,
				bestIndexValue,
				bestIndexedX,
				bestIndexedY,
				bestQIndexed);
		}

		const size_t iterations = std::bit_width(static_cast<size_t>(world - 1));
		bool active = true;
		size_t itx = 0u;

		while (itx < iterations && active) {
			const size_t step = 1ULL << itx;
			const int partner = rank ^ static_cast<int>(step);
			if (partner < world) {
				const bool am_sender = (rank & static_cast<int>(step)) != 0;
				if (am_sender) {
					g_world->send(partner, 3, bestMsg);
					active = false;
				}
				else {
					BestSolutionMsg inBest;
					InitBestSolutionMsg(inBest);
					g_world->recv(partner, 3, inBest);
					if (better_indexed(inBest.bestIndex, inBest.bestF, bestMsg.bestIndex, bestMsg.bestF))
						bestMsg = inBest;
				}
			}
			++itx;
		}

		if (rank == 0) {
			UpdateIndexedBestFromMessage(
				bestMsg,
				fullConstraintIndex,
				bestIndexFound,
				bestIndexValue,
				bestQIndexed,
				bestIndexedX,
				bestIndexedY,
				bestF,
				bestQ,
				bestX,
				bestY);
		}

		const bool haveFeasibleBest = (bestIndexFound == fullConstraintIndex) && !bestQ.empty();

		if (haveFeasibleBest && cost.solveMode != ManipSolveMode::Transition) {
			alignas(16) float q_local[AGP_MAX_STATE_DIM];
			std::memcpy(q_local, bestQ.data(), static_cast<size_t>(dim) * sizeof(float));

			float x_final = bestX;
			float y_final = bestY;
			float f_final = bestF;

			const int last = n - 1;
			const float lo = (last == 0)
				? -1.0471975511965977461542144610932f
				: -2.6179938779914943653855361527329f;
			const float hi = 2.6179938779914943653855361527329f;

			float bestLocF = f_final;
			float saved = q_local[last];
			float delta = 0.05f;

			while (delta >= 0.00625f) {
				int sgn = -1;
				while (sgn < 2) {
					float cand = fmaf(static_cast<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_loc = 0.0f;
					float y2_loc = 0.0f;
					unsigned idx2 = 0u;
					float val2 = 0.0f;
					const bool feasible2 = cost.evaluate_indexed(q_local, x2_loc, y2_loc, idx2, val2);

					if (feasible2 && val2 < bestLocF) {
						bestLocF = val2;
						x_final = x2_loc;
						y_final = y2_loc;
						saved = cand;
					}

					q_local[last] = backup;
					sgn += 2;
				}
				delta *= 0.5f;
			}

			if (bestLocF < f_final) {
				q_local[last] = saved;
				bestF = bestLocF;
				bestX = x_final;
				bestY = y_final;
				bestQ.assign(q_local, q_local + dim);

				bestIndexFound = fullConstraintIndex;
				bestIndexValue = bestF;
				bestQIndexed = bestQ;
				bestIndexedX = bestX;
				bestIndexedY = bestY;
			}
		}
		else if (haveFeasibleBest) {
			bestIndexFound = fullConstraintIndex;
			bestIndexValue = bestF;
			bestQIndexed = bestQ;
			bestIndexedX = bestX;
			bestIndexedY = bestY;
		}
		else {
			bestQ = bestQIndexed;
			bestF = bestIndexValue;
			bestX = bestIndexedX;
			bestY = bestIndexedY;
		}

		out_iterations = static_cast<size_t>(it);
		out_achieved_epsilon = interval_len;
		return;
	}

	++it;
}

}".