#include <iostream>
#include <stdio.h>
#include <time.h>
#include <math.h>

using namespace std;

#define muls_arr(at,bt,ct)    \
     __asm__ __volatile__   \
            (     \
                    "movups (%[a]), %%xmm0 \n\t"    \
                    "movups (%[b]), %%xmm1 \n\t"    \
                    "mulps  %%xmm1, %%xmm0 \n\t"    \
                    "movups %%xmm0, %[c] \n\t"    \
                    : [c] "=m" (*ct)  \
                    : [a] "r" (at), [b] "r" (bt)    \
     )     \

#define copy_vec(at,bt)    \
     __asm__ __volatile__   \
            (     \
                    "movups (%[a]), %%xmm0 \n\t"    \
                    "movups %%xmm0, %[c]   \n\t"    \
                    : [c] "=m" (*bt)  \
                    : [a] "r" (at)                  \
     )     \

#define one_vec(at)     \
     __asm__ __volatile__   \
            (     \
                    "movups (%[a]),%%xmm0  \n\t"    \
                    "divps %%xmm0,%%xmm0  \n\t"     \
      "movups %%xmm0, %[b]   \n\t"    \
                    : [b] "=m" (*at)  \
                    : [a] "r"  (at)                 \
     )     \

#define norm_vec_old(v,n)     \
     __asm__ __volatile__   \
            (     \
                    "movaps (%[a]),%%xmm7     \n\t"    \
          \
      "shufps $0,%%xmm7,%%xmm0     \n\t" \
          \
      "shufps $1,%%xmm7,%%xmm1 \n\t" \
          \
      "movaps %%xmm7,%%xmm2     \n\t" \
      "shufps $2,%%xmm2,%%xmm2 \n\t" \
          \
      "mulss  %%xmm0,%%xmm0    \n\t" /* a^2 */ \
      "mulss  %%xmm1,%%xmm1    \n\t" /* b^2 */ \
      "mulss  %%xmm2,%%xmm2    \n\t" /* c^2 */ \
                       \
      "addss    %%xmm1,%%xmm0  \n\t"  \
      "addss    %%xmm2,%%xmm0  \n\t" /* a^2+b^2+c^2*/ \
          \
      "sqrtss %%xmm0,%%xmm0    \n\t"  \
          \
      "movss  (%[n]),%%xmm3   \n\t"\
      "divss  %%xmm0,%%xmm3  \n\t" /*xmm3=1.0/L*/ \
          \
      /*"shufps $0,%%xmm3,%%xmm3 \n\t" */\
          \
      /*"mulps    %%xmm3,%%xmm7     \n\t" */\
          \
      "movups %%xmm1, %[b]      \n\t"    \
                    : [b] "=m" (v)   \
                    : [a] "r"  (v) , [n] "r" (&n)  /*(0x3f800000) =1.0*/ \
     )     \

#define norm_vec(at,bt,n)          \
     __asm__ __volatile__   \
            (     \
                    "movups (%[a]), %%xmm7 \n\t"    \
          \
      "movaps  %%xmm7,%%xmm0     \n\t"\
      "shufps $0,%%xmm0,%%xmm0   \n\t"\
          \
      "movaps  %%xmm7,%%xmm1     \n\t"\
      "shufps $85,%%xmm1,%%xmm1   \n\t"\
          \
      "movaps  %%xmm7,%%xmm2     \n\t"\
      "shufps $170,%%xmm2,%%xmm2   \n\t"\
          \
          \
      "mulss  %%xmm0,%%xmm0    \n\t" /* a^2 */ \
      "mulss  %%xmm1,%%xmm1    \n\t" /* b^2 */ \
      "mulss  %%xmm2,%%xmm2    \n\t" /* c^2 */ \
                 \
      "addss    %%xmm1,%%xmm0  \n\t"  \
      "addss    %%xmm2,%%xmm0  \n\t" /* a^2+b^2+c^2*/ \
          \
      "sqrtss %%xmm0,%%xmm0    \n\t"  \
          \
      "movss  (%[n]),%%xmm3   \n\t"   \
      "divss  %%xmm0,%%xmm3  \n\t" /*xmm3=1.0/L*/ \
          \
      "shufps $0,%%xmm3,%%xmm3 \n\t"  \
          \
      "mulps    %%xmm3,%%xmm7     \n\t" \
      "movups %%xmm7, %[c]   \n\t"    \
          \
                    : [c] "=m" (*bt)  \
                    : [a] "r" (at), [n] "r" (&n)    \
     )     \

void norm_v(float *v,float *v1)
        {
        float q1=v[0];
        float q2=v[1];
        float q3=v[2];
        float l=sqrtf(q1*q1+q2*q2+q3*q3);
        float l1=1.0f/l;
          
            
        v1[0]=q1*l1;
        v1[1]=q2*l1;
        v1[2]=q3*l1;
        v1[3]=v[0];
            
        }     

int main() {
            float* aa = new float[4];
            float* bb = new float[4];
            float* cc = new float[4];
     float a[4] __attribute__ (( aligned(16)));
     float  n = 1.0;
            for(int i = 0; i<4; i++)
            {
                    aa[i] = 1.1111111f+(0.11111111f)*(float)i;
                    a[i] = 2.11111111f+(0.111111111f)*(float)i;

                    bb[i] = 2.5f;
                    cc[i] = 0.0f;
            }

            
        time_t start,end;
         start = clock();

     for(int i = 0; i<30000000; i++)
            {

                
     //muls_arr(aa,bb,cc);
     //copy_vec(a,aa);
            //one_vec(aa);
     norm_vec(aa,a,n);
     //norm_v(aa,a);
     }
     cout << aa[0] << " " << aa[1] << " " << aa[2] << " " << aa[3] << endl;
     cout << a[0] << " " << a[1] << " " << a[2] << " " << a[3] << endl;

            cout << bb[0] << " " << bb[1] << " " << bb[2] << " " << bb[3] << endl;
            cout << cc[0] << " " << cc[1] << " " << cc[2] << " " << cc[3] << endl;
         
      end=clock();     
     printf("time=%d\n",end-start);     
            return 0;     
     }

        //asm(".intel_syntax noprefix\n"); // turn ON sse2 comands movupd and other
        //asm(".att_syntax prefix");