




/*
    Based on VOLK project.
    Modified by Bernard Gingold on:
    Date: 13-12-2020 2:20PM +00200
    contact: beniekg@gmail.com
    Few modification were added to original
    implementation (ICC pragmas, alignment directives and code layout rescheduled,
    unrolling completely 2-iteration for-loops)
    
*/

/*
 * Copyright 2018 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include <immintrin.h>
#include "GMS_32f_acos_32f.h"
#include "GMS_cephes.h"


         void dsp_32f_acos_32f_u_avx_looped(float * __restrict b,
	                                    float * __restrict a,
				            const int32_t npoints) {
              DSP_32F_ACOS_32F_BLOCK
#if defined __ICC || defined __INTEL_COMPILER
#pragma code_align(32)
#endif
              for(; idx != len; ++idx) {
		    _mm_prefetch((const char*)&a+32,_MM_HINT_T0);
                  aVal = _mm256_loadu_ps(a);
		  d    = aVal;
		  aVal = _mm256_div_ps(_mm256_sqrt_ps(_mm256_mul_ps(_mm256_add_ps(fones, aVal),
                                                          _mm256_sub_ps(fones, aVal))), aVal);
                  z = aVal;
		  condition =  _mm256_cmp_ps(z, fzeroes, _CMP_LT_OQ);
		  z = _mm256_sub_ps(z, _mm256_and_ps(_mm256_mul_ps(z, ftwos), condition));
                  condition = _mm256_cmp_ps(z, fones, _CMP_LT_OS);
                  x = _mm256_add_ps(
                        z, _mm256_and_ps(_mm256_sub_ps(_mm256_div_ps(fones, z), z), condition));
		  /* for (i = 0; i < 2; i++) <-- Possibly branch mispredict and loop overhead */ 
		  x = _mm256_add_ps(x, _mm256_sqrt_ps(_mm256_fmadd_ps(x, x, fones)));
		  x = _mm256_add_ps(x, _mm256_sqrt_ps(_mm256_fmadd_ps(x, x, fones))); // THis line Compiler may eliminate
		  x = _mm256_div_ps(fones, x);
                  y = fzeroes;
		  // loop unrolled
		  y = _mm256_fmadd_ps(
                         y, _mm256_mul_ps(x, x),n_third); // removing call to pow
		  y = _mm256_fmadd_ps(
                         y, _mm256_mul_ps(x, x), p_third);  // removed call to pow
		  y = _mm256_mul_ps(y, _mm256_mul_ps(x, ffours));
                  condition = _mm256_cmp_ps(z, fones, _CMP_GT_OS);
                  y = _mm256_add_ps(y, _mm256_and_ps(_mm256_fnmadd_ps(y, ftwos, pio2), condition));
                  arccosine = y;
                  condition = _mm256_cmp_ps(aVal, fzeroes, _CMP_LT_OQ);
                  arccosine = _mm256_sub_ps(
                              arccosine, _mm256_and_ps(_mm256_mul_ps(arccosine, ftwos), condition));
                  condition = _mm256_cmp_ps(d, fzeroes, _CMP_LT_OS);
                  arccosine = _mm256_add_ps(arccosine, _mm256_and_ps(pi, condition));
                  _mm256_storeu_ps(b, arccosine);
                  a += 8;
                  b += 8;
	      }
	      idx = len*8;
#if defined __ICC || defined __INTEL_COMPILER
#pragma loop_count min(1),avg(4),max(7)
#endif
              for(; idx != npoints; ++idx) {
                  b[i] = ceph_acosf(a[i]);
	      }
	 }

	 
        
         void dsp_32f_acos_32f_a_avx_looped(float * __restrict __ATTR_ALIGN__(32) b,
	                                    float * __restrict __ATTR_ALIGN__(32) a,
				            const int32_t npoints) {
              DSP_32F_ACOS_S32F_BLOCK
#if defined __ICC || defined __INTEL_COMPILER
              __assume_aligned(b,32);
	      __assume_aligned(a,32);
#elif defined __GNUC__ && !defined __INTEL_COMPILER
              b = (float*)__builtin_assume_aligned(b,32);
	      a = (float*)__builtin_assume_aligned(a,32);
#endif
#if defined __ICC || defined __INTEL_COMPILER
#pragma code_align(32)
#endif
                for(; idx != len; ++idx) {
		    _mm_prefetch((const char*)&a+32,_MM_HINT_T0);
                  aVal = _mm256_load_ps(a);
		  d    = aVal;
		  aVal = _mm256_div_ps(_mm256_sqrt_ps(_mm256_mul_ps(_mm256_add_ps(fones, aVal),
                                                          _mm256_sub_ps(fones, aVal))), aVal);
                  z = aVal;
		  condition =  _mm256_cmp_ps(z, fzeroes, _CMP_LT_OQ);
		  z = _mm256_sub_ps(z, _mm256_and_ps(_mm256_mul_ps(z, ftwos), condition));
                  condition = _mm256_cmp_ps(z, fones, _CMP_LT_OS);
                  x = _mm256_add_ps(
                        z, _mm256_and_ps(_mm256_sub_ps(_mm256_div_ps(fones, z), z), condition));
		  /* for (i = 0; i < 2; i++) <-- Possibly branch mispredict and loop overhead */ 
		  x = _mm256_add_ps(x, _mm256_sqrt_ps(_mm256_fmadd_ps(x, x, fones)));
		  x = _mm256_add_ps(x, _mm256_sqrt_ps(_mm256_fmadd_ps(x, x, fones))); // THis line Compiler may eliminate
		  x = _mm256_div_ps(fones, x);
                  y = fzeroes;
		  // loop unrolled
		  y = _mm256_fmadd_ps(
                         y, _mm256_mul_ps(x, x),n_third); // removing call to pow
		  y = _mm256_fmadd_ps(
                         y, _mm256_mul_ps(x, x), p_third);  // removed call to pow
		  y = _mm256_mul_ps(y, _mm256_mul_ps(x, ffours));
                  condition = _mm256_cmp_ps(z, fones, _CMP_GT_OS);
                  y = _mm256_add_ps(y, _mm256_and_ps(_mm256_fnmadd_ps(y, ftwos, pio2), condition));
                  arccosine = y;
                  condition = _mm256_cmp_ps(aVal, fzeroes, _CMP_LT_OQ);
                  arccosine = _mm256_sub_ps(
                              arccosine, _mm256_and_ps(_mm256_mul_ps(arccosine, ftwos), condition));
                  condition = _mm256_cmp_ps(d, fzeroes, _CMP_LT_OS);
                  arccosine = _mm256_add_ps(arccosine, _mm256_and_ps(pi, condition));
                  _mm256_store_ps(b, arccosine);
                  a += 8;
                  b += 8;
	      }
	       idx = len*8;
#if defined __ICC || defined __INTEL_COMPILER
#pragma loop_count min(1),avg(4),max(7)
#endif
              for(; idx != npoints; ++idx) {
                  b[i] = ceph_acosf(a[i]);
	      }
	 }



       
	 __m256 dsp_32f_acos_32f_avx(const __m256 v) {

	         register __m256 aVal, d, pi, pio2, x, y, z, arccosine;    
                 register __m256 n_third,p_third,condition,fzeroes,fones;	 
                 register __m256 ftwos,ffours;                              
                 pi = _mm256_set1_ps(3.14159265358979323846f);             
                 pio2 = _mm256_set1_ps(1.5707963267948966192f);            
                 fzeroes = _mm256_setzero_ps();                            
                 fones = _mm256_set1_ps(1.0f);                             
                 ftwos = _mm256_set1_ps(2.0f);                             
                 ffours  = _mm256_set1_ps(4.0f);                           
                 n_third = _mm256_set1_ps(-0.3333333333333333333333333f);  
                 p_third = _mm256_set1_ps(0.3333333333333333333333333f);
		 arccosine = _mm256_setzero_ps();
		 aVal = v;
		 d    = aVal;
		 aVal = _mm256_div_ps(_mm256_sqrt_ps(_mm256_mul_ps(_mm256_add_ps(fones, aVal),
                                                          _mm256_sub_ps(fones, aVal))), aVal);
                 z = aVal;
		 condition =  _mm256_cmp_ps(z, fzeroes, _CMP_LT_OQ);
		 z = _mm256_sub_ps(z, _mm256_and_ps(_mm256_mul_ps(z, ftwos), condition));
                 condition = _mm256_cmp_ps(z, fones, _CMP_LT_OS);
                 x = _mm256_add_ps(
                        z, _mm256_and_ps(_mm256_sub_ps(_mm256_div_ps(fones, z), z), condition));
		  /* for (i = 0; i < 2; i++) <-- Possibly branch mispredict and loop overhead */ 
		 x = _mm256_add_ps(x, _mm256_sqrt_ps(_mm256_fmadd_ps(x, x, fones)));
		 x = _mm256_add_ps(x, _mm256_sqrt_ps(_mm256_fmadd_ps(x, x, fones))); // THis line Compiler may eliminate
		 x = _mm256_div_ps(fones, x);
                 y = fzeroes;
		  // loop unrolled
		 y = _mm256_fmadd_ps(
                         y, _mm256_mul_ps(x, x),n_third); // removing call to pow
		 y = _mm256_fmadd_ps(
                         y, _mm256_mul_ps(x, x), p_third);  // removed call to pow
		 y = _mm256_mul_ps(y, _mm256_mul_ps(x, ffours));
                 condition = _mm256_cmp_ps(z, fones, _CMP_GT_OS);
                 y = _mm256_add_ps(y, _mm256_and_ps(_mm256_fnmadd_ps(y, ftwos, pio2), condition));
                 arccosine = y;
                 condition = _mm256_cmp_ps(aVal, fzeroes, _CMP_LT_OQ);
                 arccosine = _mm256_sub_ps(
                              arccosine, _mm256_and_ps(_mm256_mul_ps(arccosine, ftwos), condition));
                 condition = _mm256_cmp_ps(d, fzeroes, _CMP_LT_OS);
                 arccosine = _mm256_add_ps(arccosine, _mm256_and_ps(pi, condition));
		 return (arccosine);
	 }
	 
       


