#include "Altivec.h"

#include <cmath>
using namespace std;

float Altivec::vsum(const float* in1, unsigned int size)
{
  unsigned int vectorCount  = size/4;
  unsigned int vectorRemain = size%4;

  float result = 0;
  if (vectorCount)
  {
    vector float sum = (vector float) vec_splat_u32(0);
    vector unsigned char align = vec_lvsr( -((long) in1), (int*) 0L );
    vector float load2 = vec_ld( 0, in1 );

    if( (long) in1 & 15 )
    in1 += 16 / vec_step( vector float );

    vector float load1, alignedFloats;
    while( vectorCount-- )
    {
      load1 = load2;
      load2 = vec_ld( 0, in1 );
      alignedFloats = vec_perm( load1, load2, align);
      sum = vec_add( sum, alignedFloats );
      in1 += 16 / vec_step( vector float );
    }
    sum = vec_add( sum, vec_sld( sum, sum, 8 ) );
    sum = vec_add( sum, vec_sld( sum, sum, 4 ) );

    vec_ste( sum, 0, &result );
  }
  
  while(vectorRemain--)
  {
    result += *(in1++);
  }

  return result;
}
//end vsum()

float Altivec::vabssum(const float* in1, unsigned int size)
{
  unsigned int vectorCount  = size/4;
  unsigned int vectorRemain = size%4;

  float result = 0;
  if (vectorCount)
  {
    vector float sum = (vector float) vec_splat_u32(0);
    vector unsigned char align = vec_lvsr( -((long) in1), (int*) 0L );
    vector float load2 = vec_ld( 0, in1 );

    if( (long) in1 & 15 )
    in1 += 16 / vec_step( vector float );

    vector float load1, alignedFloats;
    while( vectorCount-- )
    {
      load1 = load2;
      load2 = vec_ld( 0, in1 );
      alignedFloats = vec_abs(vec_perm( load1, load2, align));
      sum = vec_add( sum, alignedFloats );
      in1 += 16 / vec_step( vector float );
    }
    sum = vec_add( sum, vec_sld( sum, sum, 8 ) );
    sum = vec_add( sum, vec_sld( sum, sum, 4 ) );

    vec_ste( sum, 0, &result );
  }
  
  while(vectorRemain--)
  {
    result += *(in1++);
  }

  return result;
}
//end vabssum()

pair<float,float> Altivec::meanAndStandardDeviation(const float* input, unsigned int size)
{
  float mu=0;
  float sigma=0;
  float temp=0;

  unsigned int vectorCount  = size/4;
  unsigned int vectorRemain = size%4;

  if (vectorCount)
  {
    vector float zero   = (vector float) vec_splat_u32(0);
    vector float temp   = (vector float) vec_splat_u32(0);
    vector float vmu    = (vector float) vec_splat_u32(0);
    vector float vsigma = (vector float) vec_splat_u32(0);
    vector unsigned char align = vec_lvsr( -((long) input), (int*) 0L );
    vector float load2 = vec_ld( 0, input );

    if( (long) input & 15 )
    input += 16 / vec_step( vector float );

    vector float load1, alignedFloats;
    while( vectorCount-- )
    {
      load1 = load2;
      load2 = vec_ld( 0, input );
      alignedFloats = vec_perm( load1, load2, align);

      temp = vec_madd(alignedFloats, alignedFloats, zero);
      vsigma = vec_add(vsigma, temp);
      vmu    = vec_add(vmu , vec_madd(temp,vec_rsqrte(temp),zero));
      input += 16 / vec_step( vector float );
    }
    vmu = vec_add( vmu, vec_sld( vmu, vmu, 8 ) );
    vmu = vec_add( vmu, vec_sld( vmu, vmu, 4 ) );
    vsigma = vec_add( vsigma, vec_sld( vsigma, vsigma, 8 ) );
    vsigma = vec_add( vsigma, vec_sld( vsigma, vsigma, 4 ) );
    
    vec_ste( vmu, 0, &mu);
    vec_ste( vsigma, 0, &sigma);
  }
  
  
  while(vectorRemain--)
  {
    temp = Altivec::sqr(*input++)+Altivec::sqr(*input++);
    sigma += temp;
    mu    += std::sqrt(temp);
  }

  mu /= size;
  sigma /=size;
  sigma -= mu*mu;
  sigma = std::sqrt(sigma);
  
  return make_pair(mu,sigma);
}
//end meanAndStandardDeviation()

pair<float,float> Altivec::vzMeanAndStandardDeviation(const float* in_re,
                                                      const float* in_im,
                                                      unsigned int size)
{
  float mu=0;
  float sigma=0;
  float temp=0;

  unsigned int vectorCount  = size/4;
  unsigned int vectorRemain = size%4;

  if (vectorCount)
  {
    vector float zero   = (vector float) vec_splat_u32(0);
    vector float temp   = (vector float) vec_splat_u32(0);
    vector float vmu    = (vector float) vec_splat_u32(0);
    vector float vsigma = (vector float) vec_splat_u32(0);
    vector unsigned char align_re = vec_lvsr( -((long) in_re), (int*) 0L );
    vector float load2_re = vec_ld( 0, in_re );
    vector unsigned char align_im = vec_lvsr( -((long) in_im), (int*) 0L );
    vector float load2_im = vec_ld( 0, in_im );

    if( (long) in_re & 15 )
    in_re += 16 / vec_step( vector float );
    if( (long) in_im & 15 )
    in_im += 16 / vec_step( vector float );

    vector float load1_re, alignedFloats_re;
    vector float load1_im, alignedFloats_im;
    while( vectorCount-- )
    {
      load1_re = load2_re;
      load1_im = load2_im;
      load2_re = vec_ld( 0, in_re );
      load2_im = vec_ld( 0, in_im );
      alignedFloats_re = vec_perm( load1_re, load2_re, align_re);
      alignedFloats_im = vec_perm( load1_im, load2_im, align_im);

      temp = vec_madd(alignedFloats_re,
                      alignedFloats_re,
                      vec_madd(alignedFloats_im,alignedFloats_im,zero)
                     );
      vsigma = vec_add(vsigma, temp);
      vmu    = vec_add(vmu , vec_madd(temp,vec_rsqrte(temp),zero));
      in_re += 16 / vec_step( vector float );
      in_im += 16 / vec_step( vector float );
    }
    vmu = vec_add( vmu, vec_sld( vmu, vmu, 8 ) );
    vmu = vec_add( vmu, vec_sld( vmu, vmu, 4 ) );
    vsigma = vec_add( vsigma, vec_sld( vsigma, vsigma, 8 ) );
    vsigma = vec_add( vsigma, vec_sld( vsigma, vsigma, 4 ) );
    
    vec_ste( vmu, 0, &mu);
    vec_ste( vsigma, 0, &sigma);
  }
  
  
  while(vectorRemain--)
  {
    temp = sqr(*in_re++)+sqr(*in_im++);
    sigma += temp;
    mu    += std::sqrt(temp);
  }

  mu /= size;
  sigma /=size;
  sigma -= mu*mu;
  sigma = std::sqrt(sigma);
  
  return make_pair(mu,sigma);
}
//end vzMeanAndStandardDeviation()

void Altivec::vzmul(const float* in1_re, const float* in1_im,
                    const float* in2_re, const float* in2_im,
                    float* out_re, float* out_im,
                    unsigned int size)
{
   DSPSplitComplex input1;
   input1.realp = const_cast<float*>(in1_re);
   input1.imagp = const_cast<float*>(in1_im);
   DSPSplitComplex input2;
   input2.realp = const_cast<float*>(in2_re);
   input2.imagp = const_cast<float*>(in2_im);
   DSPSplitComplex output;
   output.realp = out_re;
   output.imagp = out_im;
   ::zvmul(&input1,1,&input2,1,&output,1,size,1);
}
//end vzmul()