volk_32f_cos_32f.h

Go to the documentation of this file.

/* -*- c++ -*- */
/*
 * Copyright 2014 Free Software Foundation, Inc.
 *
 * This file is part of VOLK
 *
 * SPDX-License-Identifier: LGPL-3.0-or-later
 */

 
#include <inttypes.h>
#include <math.h>
#include <stdio.h>
 
#ifndef INCLUDED_volk_32f_cos_32f_a_H
#define INCLUDED_volk_32f_cos_32f_a_H
 
#ifdef LV_HAVE_GENERIC
 
static inline void
volk_32f_cos_32f_generic(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
    unsigned int number = 0;
 
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_GENERIC */
 
#ifdef LV_HAVE_GENERIC
#include <volk/volk_common.h>
 
static inline void
volk_32f_cos_32f_polynomial(float* bVector, const float* aVector, unsigned int num_points)
{
    for (unsigned int number = 0; number < num_points; number++) {
        *bVector++ = volk_cos(*aVector++);
    }
}
#endif /* LV_HAVE_GENERIC */
 
#ifdef LV_HAVE_AVX512F
#include <immintrin.h>
#include <volk/volk_avx512_intrinsics.h>
 
static inline void volk_32f_cos_32f_a_avx512f(float* cosVector,
                                              const float* inVector,
                                              unsigned int num_points)
{
    float* cosPtr = cosVector;
    const float* inPtr = inVector;
 
    unsigned int number = 0;
    unsigned int sixteenPoints = num_points / 16;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m512 two_over_pi = _mm512_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m512 pi_over_2_hi = _mm512_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m512 pi_over_2_lo = _mm512_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m512i ones = _mm512_set1_epi32(1);
    const __m512i twos = _mm512_set1_epi32(2);
    const __m512i sign_bit = _mm512_set1_epi32(0x80000000);
 
    for (; number < sixteenPoints; number++) {
        __m512 x = _mm512_load_ps(inPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m512 n_f = _mm512_roundscale_ps(_mm512_mul_ps(x, two_over_pi),
                                          _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m512i n = _mm512_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m512 r = _mm512_fnmadd_ps(n_f, pi_over_2_hi, x);
        r = _mm512_fnmadd_ps(n_f, pi_over_2_lo, r);
 
        // Evaluate both sin and cos polynomials
        __m512 sin_r = _mm512_sin_poly_avx512(r);
        __m512 cos_r = _mm512_cos_poly_avx512(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m512i n_and_1 = _mm512_and_si512(n, ones);
        __m512i n_plus_1_and_2 = _mm512_and_si512(_mm512_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __mmask16 swap_mask = _mm512_cmpeq_epi32_mask(n_and_1, ones);
        __m512 result = _mm512_mask_blend_ps(swap_mask, cos_r, sin_r);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result (use integer xor for
        // AVX512F)
        __mmask16 neg_mask = _mm512_cmpeq_epi32_mask(n_plus_1_and_2, twos);
        result = _mm512_castsi512_ps(_mm512_mask_xor_epi32(_mm512_castps_si512(result),
                                                           neg_mask,
                                                           _mm512_castps_si512(result),
                                                           sign_bit));
 
        _mm512_store_ps(cosPtr, result);
        inPtr += 16;
        cosPtr += 16;
    }
 
    number = sixteenPoints * 16;
    for (; number < num_points; number++) {
        *cosPtr++ = cosf(*inPtr++);
    }
}
#endif /* LV_HAVE_AVX512F */
 
#if LV_HAVE_AVX2 && LV_HAVE_FMA
#include <immintrin.h>
#include <volk/volk_avx2_fma_intrinsics.h>
 
static inline void
volk_32f_cos_32f_a_avx2_fma(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
 
    unsigned int number = 0;
    unsigned int eighthPoints = num_points / 8;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m256i ones = _mm256_set1_epi32(1);
    const __m256i twos = _mm256_set1_epi32(2);
    const __m256 sign_bit = _mm256_set1_ps(-0.0f);
 
    for (; number < eighthPoints; number++) {
        __m256 x = _mm256_load_ps(aPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
                                     _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m256i n = _mm256_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m256 r = _mm256_fnmadd_ps(n_f, pi_over_2_hi, x);
        r = _mm256_fnmadd_ps(n_f, pi_over_2_lo, r);
 
        // Evaluate both sin and cos polynomials
        __m256 sin_r = _mm256_sin_poly_avx2_fma(r);
        __m256 cos_r = _mm256_cos_poly_avx2_fma(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m256i n_and_1 = _mm256_and_si256(n, ones);
        __m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __m256 swap_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
        __m256 result = _mm256_blendv_ps(cos_r, sin_r, swap_mask);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result
        __m256 neg_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
        result = _mm256_xor_ps(result, _mm256_and_ps(neg_mask, sign_bit));
 
        _mm256_store_ps(bPtr, result);
        aPtr += 8;
        bPtr += 8;
    }
 
    number = eighthPoints * 8;
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_AVX2 && LV_HAVE_FMA */
 
#ifdef LV_HAVE_AVX2
#include <immintrin.h>
#include <volk/volk_avx2_intrinsics.h>
 
static inline void
volk_32f_cos_32f_a_avx2(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
 
    unsigned int number = 0;
    unsigned int eighthPoints = num_points / 8;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m256i ones = _mm256_set1_epi32(1);
    const __m256i twos = _mm256_set1_epi32(2);
    const __m256 sign_bit = _mm256_set1_ps(-0.0f);
 
    for (; number < eighthPoints; number++) {
        __m256 x = _mm256_load_ps(aPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
                                     _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m256i n = _mm256_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m256 r = _mm256_sub_ps(x, _mm256_mul_ps(n_f, pi_over_2_hi));
        r = _mm256_sub_ps(r, _mm256_mul_ps(n_f, pi_over_2_lo));
 
        // Evaluate both sin and cos polynomials
        __m256 sin_r = _mm256_sin_poly_avx2(r);
        __m256 cos_r = _mm256_cos_poly_avx2(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m256i n_and_1 = _mm256_and_si256(n, ones);
        __m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __m256 swap_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
        __m256 result = _mm256_blendv_ps(cos_r, sin_r, swap_mask);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result
        __m256 neg_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
        result = _mm256_xor_ps(result, _mm256_and_ps(neg_mask, sign_bit));
 
        _mm256_store_ps(bPtr, result);
        aPtr += 8;
        bPtr += 8;
    }
 
    number = eighthPoints * 8;
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_AVX2 */
 
#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>
#include <volk/volk_sse_intrinsics.h>
 
static inline void
volk_32f_cos_32f_a_sse4_1(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
 
    unsigned int number = 0;
    unsigned int quarterPoints = num_points / 4;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m128 two_over_pi = _mm_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m128 pi_over_2_hi = _mm_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m128 pi_over_2_lo = _mm_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m128i ones = _mm_set1_epi32(1);
    const __m128i twos = _mm_set1_epi32(2);
    const __m128 sign_bit = _mm_set1_ps(-0.0f);
 
    for (; number < quarterPoints; number++) {
        __m128 x = _mm_load_ps(aPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m128 n_f = _mm_round_ps(_mm_mul_ps(x, two_over_pi),
                                  _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m128i n = _mm_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m128 r = _mm_sub_ps(x, _mm_mul_ps(n_f, pi_over_2_hi));
        r = _mm_sub_ps(r, _mm_mul_ps(n_f, pi_over_2_lo));
 
        // Evaluate both sin and cos polynomials
        __m128 sin_r = _mm_sin_poly_sse(r);
        __m128 cos_r = _mm_cos_poly_sse(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m128i n_and_1 = _mm_and_si128(n, ones);
        __m128i n_plus_1_and_2 = _mm_and_si128(_mm_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __m128 swap_mask = _mm_castsi128_ps(_mm_cmpeq_epi32(n_and_1, ones));
        __m128 result = _mm_blendv_ps(cos_r, sin_r, swap_mask);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result
        __m128 neg_mask = _mm_castsi128_ps(_mm_cmpeq_epi32(n_plus_1_and_2, twos));
        result = _mm_xor_ps(result, _mm_and_ps(neg_mask, sign_bit));
 
        _mm_store_ps(bPtr, result);
        aPtr += 4;
        bPtr += 4;
    }
 
    number = quarterPoints * 4;
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_SSE4_1 */
 
#endif /* INCLUDED_volk_32f_cos_32f_a_H */
 
 
#ifndef INCLUDED_volk_32f_cos_32f_u_H
#define INCLUDED_volk_32f_cos_32f_u_H
 
#ifdef LV_HAVE_AVX512F
#include <immintrin.h>
#include <volk/volk_avx512_intrinsics.h>
 
static inline void volk_32f_cos_32f_u_avx512f(float* cosVector,
                                              const float* inVector,
                                              unsigned int num_points)
{
    float* cosPtr = cosVector;
    const float* inPtr = inVector;
 
    unsigned int number = 0;
    unsigned int sixteenPoints = num_points / 16;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m512 two_over_pi = _mm512_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m512 pi_over_2_hi = _mm512_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m512 pi_over_2_lo = _mm512_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m512i ones = _mm512_set1_epi32(1);
    const __m512i twos = _mm512_set1_epi32(2);
    const __m512i sign_bit = _mm512_set1_epi32(0x80000000);
 
    for (; number < sixteenPoints; number++) {
        __m512 x = _mm512_loadu_ps(inPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m512 n_f = _mm512_roundscale_ps(_mm512_mul_ps(x, two_over_pi),
                                          _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m512i n = _mm512_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m512 r = _mm512_fnmadd_ps(n_f, pi_over_2_hi, x);
        r = _mm512_fnmadd_ps(n_f, pi_over_2_lo, r);
 
        // Evaluate both sin and cos polynomials
        __m512 sin_r = _mm512_sin_poly_avx512(r);
        __m512 cos_r = _mm512_cos_poly_avx512(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m512i n_and_1 = _mm512_and_si512(n, ones);
        __m512i n_plus_1_and_2 = _mm512_and_si512(_mm512_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __mmask16 swap_mask = _mm512_cmpeq_epi32_mask(n_and_1, ones);
        __m512 result = _mm512_mask_blend_ps(swap_mask, cos_r, sin_r);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result (use integer xor for
        // AVX512F)
        __mmask16 neg_mask = _mm512_cmpeq_epi32_mask(n_plus_1_and_2, twos);
        result = _mm512_castsi512_ps(_mm512_mask_xor_epi32(_mm512_castps_si512(result),
                                                           neg_mask,
                                                           _mm512_castps_si512(result),
                                                           sign_bit));
 
        _mm512_storeu_ps(cosPtr, result);
        inPtr += 16;
        cosPtr += 16;
    }
 
    number = sixteenPoints * 16;
    for (; number < num_points; number++) {
        *cosPtr++ = cosf(*inPtr++);
    }
}
#endif /* LV_HAVE_AVX512F */
 
#if LV_HAVE_AVX2 && LV_HAVE_FMA
#include <immintrin.h>
#include <volk/volk_avx2_fma_intrinsics.h>
 
static inline void
volk_32f_cos_32f_u_avx2_fma(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
 
    unsigned int number = 0;
    unsigned int eighthPoints = num_points / 8;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m256i ones = _mm256_set1_epi32(1);
    const __m256i twos = _mm256_set1_epi32(2);
    const __m256 sign_bit = _mm256_set1_ps(-0.0f);
 
    for (; number < eighthPoints; number++) {
        __m256 x = _mm256_loadu_ps(aPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
                                     _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m256i n = _mm256_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m256 r = _mm256_fnmadd_ps(n_f, pi_over_2_hi, x);
        r = _mm256_fnmadd_ps(n_f, pi_over_2_lo, r);
 
        // Evaluate both sin and cos polynomials
        __m256 sin_r = _mm256_sin_poly_avx2_fma(r);
        __m256 cos_r = _mm256_cos_poly_avx2_fma(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m256i n_and_1 = _mm256_and_si256(n, ones);
        __m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __m256 swap_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
        __m256 result = _mm256_blendv_ps(cos_r, sin_r, swap_mask);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result
        __m256 neg_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
        result = _mm256_xor_ps(result, _mm256_and_ps(neg_mask, sign_bit));
 
        _mm256_storeu_ps(bPtr, result);
        aPtr += 8;
        bPtr += 8;
    }
 
    number = eighthPoints * 8;
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_AVX2 && LV_HAVE_FMA */
 
#ifdef LV_HAVE_AVX2
#include <immintrin.h>
#include <volk/volk_avx2_intrinsics.h>
 
static inline void
volk_32f_cos_32f_u_avx2(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
 
    unsigned int number = 0;
    unsigned int eighthPoints = num_points / 8;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m256 two_over_pi = _mm256_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m256 pi_over_2_hi = _mm256_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m256 pi_over_2_lo = _mm256_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m256i ones = _mm256_set1_epi32(1);
    const __m256i twos = _mm256_set1_epi32(2);
    const __m256 sign_bit = _mm256_set1_ps(-0.0f);
 
    for (; number < eighthPoints; number++) {
        __m256 x = _mm256_loadu_ps(aPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m256 n_f = _mm256_round_ps(_mm256_mul_ps(x, two_over_pi),
                                     _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m256i n = _mm256_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m256 r = _mm256_sub_ps(x, _mm256_mul_ps(n_f, pi_over_2_hi));
        r = _mm256_sub_ps(r, _mm256_mul_ps(n_f, pi_over_2_lo));
 
        // Evaluate both sin and cos polynomials
        __m256 sin_r = _mm256_sin_poly_avx2(r);
        __m256 cos_r = _mm256_cos_poly_avx2(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m256i n_and_1 = _mm256_and_si256(n, ones);
        __m256i n_plus_1_and_2 = _mm256_and_si256(_mm256_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __m256 swap_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_and_1, ones));
        __m256 result = _mm256_blendv_ps(cos_r, sin_r, swap_mask);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result
        __m256 neg_mask = _mm256_castsi256_ps(_mm256_cmpeq_epi32(n_plus_1_and_2, twos));
        result = _mm256_xor_ps(result, _mm256_and_ps(neg_mask, sign_bit));
 
        _mm256_storeu_ps(bPtr, result);
        aPtr += 8;
        bPtr += 8;
    }
 
    number = eighthPoints * 8;
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_AVX2 */
 
#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>
#include <volk/volk_sse_intrinsics.h>
 
static inline void
volk_32f_cos_32f_u_sse4_1(float* bVector, const float* aVector, unsigned int num_points)
{
    float* bPtr = bVector;
    const float* aPtr = aVector;
 
    unsigned int number = 0;
    unsigned int quarterPoints = num_points / 4;
 
    // Constants for Cody-Waite argument reduction
    // n = round(x * 2/pi), then r = x - n * pi/2
    const __m128 two_over_pi = _mm_set1_ps(0x1.45f306p-1f);    // 2/pi
    const __m128 pi_over_2_hi = _mm_set1_ps(0x1.921fb6p+0f);   // pi/2 high
    const __m128 pi_over_2_lo = _mm_set1_ps(-0x1.777a5cp-25f); // pi/2 low
 
    const __m128i ones = _mm_set1_epi32(1);
    const __m128i twos = _mm_set1_epi32(2);
    const __m128 sign_bit = _mm_set1_ps(-0.0f);
 
    for (; number < quarterPoints; number++) {
        __m128 x = _mm_loadu_ps(aPtr);
 
        // Argument reduction: n = round(x * 2/pi)
        __m128 n_f = _mm_round_ps(_mm_mul_ps(x, two_over_pi),
                                  _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
        __m128i n = _mm_cvtps_epi32(n_f);
 
        // r = x - n * (pi/2), using extended precision
        __m128 r = _mm_sub_ps(x, _mm_mul_ps(n_f, pi_over_2_hi));
        r = _mm_sub_ps(r, _mm_mul_ps(n_f, pi_over_2_lo));
 
        // Evaluate both sin and cos polynomials
        __m128 sin_r = _mm_sin_poly_sse(r);
        __m128 cos_r = _mm_cos_poly_sse(r);
 
        // Reconstruct cos(x) based on quadrant (n mod 4):
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 != 0: negative
        __m128i n_and_1 = _mm_and_si128(n, ones);
        __m128i n_plus_1_and_2 = _mm_and_si128(_mm_add_epi32(n, ones), twos);
 
        // swap_mask: where n&1 != 0, we use sin instead of cos
        __m128 swap_mask = _mm_castsi128_ps(_mm_cmpeq_epi32(n_and_1, ones));
        __m128 result = _mm_blendv_ps(cos_r, sin_r, swap_mask);
 
        // neg_mask: where (n+1)&2 != 0, we negate the result
        __m128 neg_mask = _mm_castsi128_ps(_mm_cmpeq_epi32(n_plus_1_and_2, twos));
        result = _mm_xor_ps(result, _mm_and_ps(neg_mask, sign_bit));
 
        _mm_storeu_ps(bPtr, result);
        aPtr += 4;
        bPtr += 4;
    }
 
    number = quarterPoints * 4;
    for (; number < num_points; number++) {
        *bPtr++ = cosf(*aPtr++);
    }
}
 
#endif /* LV_HAVE_SSE4_1 */
 
 
#ifdef LV_HAVE_NEON
#include <arm_neon.h>
#include <volk/volk_neon_intrinsics.h>
 
/* NEON polynomial-based cos using Cody-Waite argument reduction */
static inline void
volk_32f_cos_32f_neon(float* bVector, const float* aVector, unsigned int num_points)
{
    // Cody-Waite argument reduction: n = round(x * 2/pi), r = x - n * pi/2
    const float32x4_t two_over_pi = vdupq_n_f32(0x1.45f306p-1f);    // 2/pi
    const float32x4_t pi_over_2_hi = vdupq_n_f32(0x1.921fb6p+0f);   // pi/2 high
    const float32x4_t pi_over_2_lo = vdupq_n_f32(-0x1.777a5cp-25f); // pi/2 low
 
    const int32x4_t ones = vdupq_n_s32(1);
    const int32x4_t twos = vdupq_n_s32(2);
    const float32x4_t sign_bit = vdupq_n_f32(-0.0f);
    const float32x4_t half = vdupq_n_f32(0.5f);
    const float32x4_t neg_half = vdupq_n_f32(-0.5f);
    const float32x4_t fzeroes = vdupq_n_f32(0.0f);
 
    unsigned int number = 0;
    const unsigned int quarterPoints = num_points / 4;
 
    for (; number < quarterPoints; number++) {
        float32x4_t x = vld1q_f32(aVector);
        aVector += 4;
 
        // n = round(x * 2/pi) - emulate round-to-nearest for ARMv7
        float32x4_t scaled = vmulq_f32(x, two_over_pi);
        uint32x4_t is_neg = vcltq_f32(scaled, fzeroes);
        float32x4_t adj = vbslq_f32(is_neg, neg_half, half);
        float32x4_t n_f = vcvtq_f32_s32(vcvtq_s32_f32(vaddq_f32(scaled, adj)));
        int32x4_t n = vcvtq_s32_f32(n_f);
 
        // r = x - n * (pi/2) using extended precision
        float32x4_t r = vmlsq_f32(x, n_f, pi_over_2_hi);
        r = vmlsq_f32(r, n_f, pi_over_2_lo);
 
        // Evaluate sin and cos polynomials
        float32x4_t sin_r = _vsin_poly_f32(r);
        float32x4_t cos_r = _vcos_poly_f32(r);
 
        // Quadrant-based reconstruction for cos:
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 == 2: negative
        int32x4_t n_and_1 = vandq_s32(n, ones);
        int32x4_t n_plus_1_and_2 = vandq_s32(vaddq_s32(n, ones), twos);
 
        uint32x4_t swap_mask = vceqq_s32(n_and_1, ones);
        float32x4_t result = vbslq_f32(swap_mask, sin_r, cos_r);
 
        uint32x4_t neg_mask = vceqq_s32(n_plus_1_and_2, twos);
        result = vreinterpretq_f32_u32(
            veorq_u32(vreinterpretq_u32_f32(result),
                      vandq_u32(neg_mask, vreinterpretq_u32_f32(sign_bit))));
 
        vst1q_f32(bVector, result);
        bVector += 4;
    }
 
    for (number = quarterPoints * 4; number < num_points; number++) {
        *bVector++ = cosf(*aVector++);
    }
}
#endif /* LV_HAVE_NEON */
 
#ifdef LV_HAVE_NEONV8
#include <arm_neon.h>
#include <volk/volk_neon_intrinsics.h>
 
/* NEONv8 polynomial-based cos using Cody-Waite argument reduction with FMA */
static inline void
volk_32f_cos_32f_neonv8(float* bVector, const float* aVector, unsigned int num_points)
{
    // Cody-Waite argument reduction: n = round(x * 2/pi), r = x - n * pi/2
    const float32x4_t two_over_pi = vdupq_n_f32(0x1.45f306p-1f);    // 2/pi
    const float32x4_t pi_over_2_hi = vdupq_n_f32(0x1.921fb6p+0f);   // pi/2 high
    const float32x4_t pi_over_2_lo = vdupq_n_f32(-0x1.777a5cp-25f); // pi/2 low
 
    const int32x4_t ones = vdupq_n_s32(1);
    const int32x4_t twos = vdupq_n_s32(2);
    const float32x4_t sign_bit = vdupq_n_f32(-0.0f);
 
    unsigned int number = 0;
    const unsigned int quarterPoints = num_points / 4;
 
    for (; number < quarterPoints; number++) {
        float32x4_t x = vld1q_f32(aVector);
        aVector += 4;
 
        // n = round(x * 2/pi) using ARMv8 vrndnq_f32
        float32x4_t n_f = vrndnq_f32(vmulq_f32(x, two_over_pi));
        int32x4_t n = vcvtq_s32_f32(n_f);
 
        // r = x - n * (pi/2) using FMA for extended precision
        float32x4_t r = vfmsq_f32(x, n_f, pi_over_2_hi);
        r = vfmsq_f32(r, n_f, pi_over_2_lo);
 
        // Evaluate sin and cos polynomials using FMA
        float32x4_t sin_r = _vsin_poly_neonv8(r);
        float32x4_t cos_r = _vcos_poly_neonv8(r);
 
        // Quadrant-based reconstruction for cos:
        // n&1 == 0: use cos_r, n&1 == 1: use sin_r
        // (n+1)&2 == 0: positive, (n+1)&2 == 2: negative
        int32x4_t n_and_1 = vandq_s32(n, ones);
        int32x4_t n_plus_1_and_2 = vandq_s32(vaddq_s32(n, ones), twos);
 
        uint32x4_t swap_mask = vceqq_s32(n_and_1, ones);
        float32x4_t result = vbslq_f32(swap_mask, sin_r, cos_r);
 
        uint32x4_t neg_mask = vceqq_s32(n_plus_1_and_2, twos);
        result = vreinterpretq_f32_u32(
            veorq_u32(vreinterpretq_u32_f32(result),
                      vandq_u32(neg_mask, vreinterpretq_u32_f32(sign_bit))));
 
        vst1q_f32(bVector, result);
        bVector += 4;
    }
 
    for (number = quarterPoints * 4; number < num_points; number++) {
        *bVector++ = cosf(*aVector++);
    }
}
 
#endif /* LV_HAVE_NEONV8 */
 
#ifdef LV_HAVE_RVV
#include <riscv_vector.h>
 
static inline void
volk_32f_cos_32f_rvv(float* bVector, const float* aVector, unsigned int num_points)
{
    size_t vlmax = __riscv_vsetvlmax_e32m2();
 
    const vfloat32m2_t c4oPi = __riscv_vfmv_v_f_f32m2(1.2732395f, vlmax);
    const vfloat32m2_t cPio4a = __riscv_vfmv_v_f_f32m2(0.7853982f, vlmax);
    const vfloat32m2_t cPio4b = __riscv_vfmv_v_f_f32m2(7.946627e-09f, vlmax);
    const vfloat32m2_t cPio4c = __riscv_vfmv_v_f_f32m2(3.061617e-17f, vlmax);
 
    const vfloat32m2_t cf1 = __riscv_vfmv_v_f_f32m2(1.0f, vlmax);
    const vfloat32m2_t cf4 = __riscv_vfmv_v_f_f32m2(4.0f, vlmax);
 
    const vfloat32m2_t c2 = __riscv_vfmv_v_f_f32m2(0.0833333333f, vlmax);
    const vfloat32m2_t c3 = __riscv_vfmv_v_f_f32m2(0.0027777778f, vlmax);
    const vfloat32m2_t c4 = __riscv_vfmv_v_f_f32m2(4.9603175e-05f, vlmax);
    const vfloat32m2_t c5 = __riscv_vfmv_v_f_f32m2(5.5114638e-07f, vlmax);
 
    size_t n = num_points;
    for (size_t vl; n > 0; n -= vl, aVector += vl, bVector += vl) {
        vl = __riscv_vsetvl_e32m2(n);
        vfloat32m2_t v = __riscv_vle32_v_f32m2(aVector, vl);
        vfloat32m2_t s = __riscv_vfabs(v, vl);
        vint32m2_t q = __riscv_vfcvt_x(__riscv_vfmul(s, c4oPi, vl), vl);
        vfloat32m2_t r = __riscv_vfcvt_f(__riscv_vadd(q, __riscv_vand(q, 1, vl), vl), vl);
 
        s = __riscv_vfnmsac(s, cPio4a, r, vl);
        s = __riscv_vfnmsac(s, cPio4b, r, vl);
        s = __riscv_vfnmsac(s, cPio4c, r, vl);
 
        s = __riscv_vfmul(s, 1 / 8.0f, vl);
        s = __riscv_vfmul(s, s, vl);
        vfloat32m2_t t = s;
        s = __riscv_vfmsub(s, c5, c4, vl);
        s = __riscv_vfmadd(s, t, c3, vl);
        s = __riscv_vfmsub(s, t, c2, vl);
        s = __riscv_vfmadd(s, t, cf1, vl);
        s = __riscv_vfmul(s, t, vl);
        s = __riscv_vfmul(s, __riscv_vfsub(cf4, s, vl), vl);
        s = __riscv_vfmul(s, __riscv_vfsub(cf4, s, vl), vl);
        s = __riscv_vfmul(s, __riscv_vfsub(cf4, s, vl), vl);
        s = __riscv_vfmul(s, 1 / 2.0f, vl);
 
        vfloat32m2_t sine =
            __riscv_vfsqrt(__riscv_vfmul(__riscv_vfrsub(s, 2.0f, vl), s, vl), vl);
        vfloat32m2_t cosine = __riscv_vfsub(cf1, s, vl);
 
        vbool16_t m1 = __riscv_vmsne(__riscv_vand(__riscv_vadd(q, 1, vl), 2, vl), 0, vl);
        vbool16_t m2 = __riscv_vmsne(__riscv_vand(__riscv_vadd(q, 2, vl), 4, vl), 0, vl);
 
        cosine = __riscv_vmerge(cosine, sine, m1, vl);
        cosine = __riscv_vfneg_mu(m2, cosine, cosine, vl);
 
        __riscv_vse32(bVector, cosine, vl);
    }
}
#endif /*LV_HAVE_RVV*/
 
#endif /* INCLUDED_volk_32f_cos_32f_u_H */