/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.1  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

#include "tools_defs.h"
#include "PHY/impl_defs_top.h"
#include "PHY/sse_intrin.h"

// Compute Energy of a complex signal vector, removing the DC component!
// input  : points to vector
// length : length of vector in complex samples

#define shift 4
//#define shift_DC 0
//#define SHRT_MIN -32768

//-----------------------------------------------------------------
// Average Power calculation with DC removing
//-----------------------------------------------------------------
int32_t signal_energy(int32_t *input,uint32_t length)
{
  uint32_t i;
  int32_t temp;
  simde__m128i in, in_clp, i16_min, coe1;
  simde__m128 num0, num1, num2, num3, recp1;

  //init
  num0 = simde_mm_setzero_ps();
  num1 = simde_mm_setzero_ps();
  i16_min = simde_mm_set1_epi16(SHRT_MIN);
  coe1 = simde_mm_set1_epi16(1);
  recp1 = simde_mm_rcp_ps(simde_mm_cvtepi32_ps(simde_mm_set1_epi32(length)));

  //Acc
  for (i = 0; i < (length >> 2); i++) {
    in = simde_mm_loadu_si128((simde__m128i *)input);
    in_clp = simde_mm_subs_epi16(in, simde_mm_cmpeq_epi16(in, i16_min));//if in=SHRT_MIN in+1, else in
    num0 = simde_mm_add_ps(num0, simde_mm_cvtepi32_ps(simde_mm_madd_epi16(in_clp, in_clp)));
    num1 = simde_mm_add_ps(num1, simde_mm_cvtepi32_ps(simde_mm_madd_epi16(in, coe1)));//DC
    input += 4;
  }
  //Ave
  num2 = simde_mm_dp_ps(num0, recp1, 0xFF);//AC power
  num3 = simde_mm_dp_ps(num1, recp1, 0xFF);//DC
  num3 = simde_mm_mul_ps(num3, num3);      //DC power
  //remove DC
  temp = simde_mm_cvtsi128_si32(simde_mm_cvttps_epi32(simde_mm_sub_ps(num2, num3)));

  return temp;
}

int32_t signal_energy_amp_shift(int32_t *input,uint32_t length)
{

  int32_t i;
  int32_t temp,temp2;
  register simde__m64 mm0,mm1,mm2,mm3;
  simde__m64 *in = (simde__m64 *)input;

  mm0 = simde_mm_setzero_si64();
  mm3 = simde_mm_setzero_si64();

  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
    mm2 = mm1;
    mm1 = simde_m_pmaddwd(mm1,mm1);
    mm1 = simde_m_psradi(mm1,AMP_SHIFT);// shift any 32 bits blocs of the word by the value shift_p9
    mm0 = simde_m_paddd(mm0,mm1);// add the two 64 bits words 4 bytes by 4 bytes
    mm3 = simde_m_paddw(mm3,mm2);// add the two 64 bits words 2 bytes by 2 bytes
  }

  mm1 = mm0;
  mm0 = simde_m_psrlqi(mm0,32);
  mm0 = simde_m_paddd(mm0,mm1);
  temp = simde_m_to_int(mm0);
  temp/=length; // this is the average of x^2


  // now remove the DC component


  mm2 = simde_m_psrlqi(mm3,32);
  mm2 = simde_m_paddw(mm2,mm3);
  mm2 = simde_m_pmaddwd(mm2,mm2);
  mm2 = simde_m_psradi(mm2,AMP_SHIFT); // fixed point representation of elements
  temp2 = simde_m_to_int(mm2);
  temp2/=(length*length);

  temp -= temp2;

  simde_mm_empty();
  simde_m_empty();

  return((temp>0)?temp:1);
}

int32_t signal_energy_nodc(int32_t *input,uint32_t length)
{
  int32_t i;
  int32_t temp;

  simde__m128i in;
  simde__m128  mm0;

//init
  mm0 = simde_mm_setzero_ps();
//Acc
  for (i=0; i<(length>>2); i++) {
    in = simde_mm_loadu_si128((simde__m128i *)input);
    mm0 = simde_mm_add_ps(mm0,simde_mm_cvtepi32_ps(simde_mm_madd_epi16(in,in)));
    input += 4;
  }
  //Ave
  temp = (int)((((float*)&mm0)[0] +
                 ((float*)&mm0)[1] +
                 ((float*)&mm0)[2] +
                 ((float*)&mm0)[3])/(float)length);

  return temp;
}
double signal_energy_fp(double *s_re[2],double *s_im[2],uint32_t nb_antennas,uint32_t length,uint32_t offset)
{

  int32_t aa,i;
  double V=0.0;

  for (i=0; i<length; i++) {
    for (aa=0; aa<nb_antennas; aa++) {
     V= V + (s_re[aa][i+offset]*s_re[aa][i+offset]) + (s_im[aa][i+offset]*s_im[aa][i+offset]);
    }
  }

  return(V/length/nb_antennas);
}

double signal_energy_fp2(struct complexd *s,uint32_t length)
{

  int32_t i;
  double V=0.0;

  for (i=0; i<length; i++) {
		          //    printf("signal_energy_fp2 : %f,%f => %f\n",s[i].x,s[i].y,V);
		  //        //      V= V + (s[i].y*s[i].x) + (s[i].y*s[i].x);
    V= V + (s[i].r*s[i].r) + (s[i].i*s[i].i);
  }
  return(V/length);
}
			  //
#ifdef MAIN
#define LENGTH 256
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
main(int argc,char **argv)
{

  int input[LENGTH];
  int energy=0,dc_r=0,dc_i=0;
  int16_t s=1,i;
  int amp;

  amp = atoi(argv[1]);// arguments to integer

  if (argc>1)
    printf("Amp = %d\n",amp);

  for (i=0; i<LENGTH; i++) {
    s = -s;
    ((int16_t*)input)[2*i]     = 31 + (int16_t)(amp*sin(2*M_PI*i/LENGTH));
    ((int16_t*)input)[1+(2*i)] = 30 + (int16_t)(amp*cos(2*M_PI*i/LENGTH));
    energy += (((int16_t*)input)[2*i]*((int16_t*)input)[2*i]) + (((int16_t*)input)[1+(2*i)]*((int16_t*)input)[1+(2*i)]);
    dc_r += ((int16_t*)input)[2*i];
    dc_i += ((int16_t*)input)[1+(2*i)];


  }

  energy/=LENGTH;
  dc_r/=LENGTH;
  dc_i/=LENGTH;

  printf("signal_energy = %d dB(%d,%d,%d,%d)\n",dB_fixed(signal_energy(input,LENGTH)),signal_energy(input,LENGTH),energy-(dc_r*dc_r+dc_i*dc_i),energy,(dc_r*dc_r+dc_i*dc_i));
  printf("dc = (%d,%d)\n",dc_r,dc_i);
}
#endif

int32_t signal_power(int32_t *input, uint32_t length)
{

  uint32_t i;
  int32_t temp;

  simde__m128i in, in_clp, i16_min;
  simde__m128  num0, num1;
  simde__m128  recp1;

  //init
  num0 = simde_mm_setzero_ps();
  i16_min = simde_mm_set1_epi16(SHRT_MIN);
  recp1 = simde_mm_rcp_ps(simde_mm_cvtepi32_ps(simde_mm_set1_epi32(length)));
  //Acc
  for (i = 0; i < (length >> 2); i++) {
    in = simde_mm_loadu_si128((simde__m128i *)input);
    in_clp = simde_mm_subs_epi16(in, simde_mm_cmpeq_epi16(in, i16_min));//if in=SHRT_MIN in+1, else in
    num0 = simde_mm_add_ps(num0, simde_mm_cvtepi32_ps(simde_mm_madd_epi16(in_clp, in_clp)));
    input += 4;
  }
  //Ave
  num1 = simde_mm_dp_ps(num0, recp1, 0xFF);
  temp = simde_mm_cvtsi128_si32(simde_mm_cvttps_epi32(num1));

  return temp;
}

int32_t interference_power(int32_t *input, uint32_t length)
{

  uint32_t i;
  int32_t temp;

  simde__m128i in, in_clp, i16_min;
  simde__m128i num0, num1, num2, num3;
  simde__m128  num4, num5, num6;
  simde__m128  recp1;

  //init
  i16_min = simde_mm_set1_epi16(SHRT_MIN);
  num5 = simde_mm_setzero_ps();
  recp1 = simde_mm_rcp_ps(simde_mm_cvtepi32_ps(simde_mm_set1_epi32(length>>2)));// 1/n, n= length/4
  //Acc
  for (i = 0; i < (length >> 2); i++) {
    in = simde_mm_loadu_si128((simde__m128i *)input);
    in_clp = simde_mm_subs_epi16(in, simde_mm_cmpeq_epi16(in, i16_min));           //if in=SHRT_MIN, in+1, else in
    num0 = simde_mm_cvtepi16_epi32(in_clp);                                   //lower 2 complex [0], [1]
    num1 = simde_mm_cvtepi16_epi32(simde_mm_shuffle_epi32(in_clp, 0x4E));          //upper 2 complex [2], [3]
    num2 = simde_mm_srai_epi32(simde_mm_add_epi32(num0, num1), 0x01);              //average A=complex( [0] + [2] ) / 2, B=complex( [1] + [3] ) / 2 
    num3 = simde_mm_sub_epi32(num2, simde_mm_shuffle_epi32(num2, 0x4E));           //complexA-complexB, B-A
    num4 = simde_mm_dp_ps(simde_mm_cvtepi32_ps(num3), simde_mm_cvtepi32_ps(num3), 0x3F);//C = num3 lower complex power, C, C, C
    num5 = simde_mm_add_ps(num5, num4);                                       //Acc Cn, Cn, Cn, Cn, 
    input += 4;
  }
  //Interference ve
  num6 = simde_mm_mul_ps(num5, recp1); //Cn / n
  temp = simde_mm_cvtsi128_si32(simde_mm_cvttps_epi32(num6));

  return temp;
}

