#[cfg(target_arch = "x86")]
use std::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use std::arch::x86_64::*;
use super::ac_strategy::{hsum, sse_dequantized_level_f32, sse_log2p1_f32};
use crate::color_correlation::{CFL_MAGNITUDE_BITS, CFL_NONZERO_BITS, cfl_rd_cost_scalar};
#[target_feature(enable = "sse4.1")]
pub(crate) fn cfl_rd_cost_sse41(
m: &[f32],
s: &[f32],
factor: f32,
weight: f32,
lambda: f32,
) -> f32 {
let len = m.len().min(s.len());
let (m_chunks, m_tail) = m[..len].as_chunks::<4>();
let (s_chunks, s_tail) = s[..len].as_chunks::<4>();
let factor_v = _mm_set1_ps(factor);
let weight_v = _mm_set1_ps(weight);
let zero = _mm_setzero_ps();
let one = _mm_set1_ps(1.0);
let half = _mm_set1_ps(0.5);
let sign = _mm_set1_ps(-0.0);
let nonzero_bits = _mm_set1_ps(lambda * CFL_NONZERO_BITS);
let magnitude_bits = _mm_set1_ps(lambda * CFL_MAGNITUDE_BITS);
let mut acc = zero;
for (m, s) in m_chunks.iter().zip(s_chunks) {
let mv = unsafe { _mm_loadu_ps(m.as_ptr()) };
let sv = unsafe { _mm_loadu_ps(s.as_ptr()) };
let residual = _mm_sub_ps(sv, _mm_mul_ps(factor_v, mv));
let zero_cost = _mm_mul_ps(_mm_mul_ps(weight_v, residual), residual);
let truncated = _mm_round_ps::<{ _MM_FROUND_TO_ZERO | _MM_FROUND_NO_EXC }>(residual);
let fraction = _mm_andnot_ps(sign, _mm_sub_ps(residual, truncated));
let at_least_half = _mm_cmpge_ps(fraction, half);
let signed_one = _mm_or_ps(one, _mm_and_ps(sign, residual));
let level = _mm_add_ps(truncated, _mm_and_ps(signed_one, at_least_half));
let coded_err = _mm_sub_ps(residual, sse_dequantized_level_f32(level));
let abs_level = _mm_andnot_ps(sign, level);
let log2_level = sse_log2p1_f32(_mm_max_ps(_mm_sub_ps(abs_level, one), zero));
let bits = _mm_add_ps(nonzero_bits, _mm_mul_ps(magnitude_bits, log2_level));
let coded = _mm_add_ps(_mm_mul_ps(_mm_mul_ps(weight_v, coded_err), coded_err), bits);
let nonzero = _mm_cmpgt_ps(abs_level, zero);
let best = _mm_min_ps(zero_cost, _mm_blendv_ps(zero_cost, coded, nonzero));
acc = _mm_add_ps(acc, best);
}
hsum(acc) + cfl_rd_cost_scalar(m_tail, s_tail, factor, weight, lambda)
}