#include "IQmathLib.h"         // Include header for IQmath library

#include "smooth.h"
#include "math_pi.h"
#include "math_pi.h"


#define SIZE_SMOOTH_INPUT	180


#pragma CODE_SECTION(my_mean,".fast_run");
_iq23 my_mean(int cnt, SMOOTH *v)
{
  _iq23 summ = 0;
  int start;


  start = v->current_pos_buf_input;
  if (start==0)
   start = (MAX_SIZE_SMOOTH_INPUT-1);

  while(cnt>0)
  {
    cnt--;
	start--;
	summ += v->buf_input[start];
	if (start==0)
	 start = (MAX_SIZE_SMOOTH_INPUT-1);
  }
  return summ;

}



void smooth_init(SMOOTH *v)
{
 int i=0;

 v->c = 1;
 v->av = 0;
 v->w = _IQ23(WINDOW_START);

 for (i=0;i<MAX_SIZE_SMOOTH_INPUT;i++)
 {
	v->buf_input[i] = 0;
 }
// current_pos_buf_input = 0;
 v->current_pos_buf_input = 0;

}

#pragma CODE_SECTION(smooth_add,".fast_run");
void smooth_add(SMOOTH *v)
{
 volatile int i;

 i = v->current_pos_buf_input;
 v->buf_input[i] = v->input;

 v->current_pos_buf_input++;
 if (v->current_pos_buf_input>=MAX_SIZE_SMOOTH_INPUT)
   v->current_pos_buf_input = 0;

}


#pragma CODE_SECTION(smooth_calc,".fast_run");
void smooth_calc(SMOOTH *v)
{
 _iq23 e=0;
 _iq23 summ=0;
 _iq23 w_new;
 long w_int;

    w_int = _IQ23int(v->w);

	if (v->c <= (WINDOW_START*2))
	{
		summ = my_mean(v->c,v);
		v->av = _IQ23div(summ,_IQ23(v->c));
		e = 0;
	}
	else
	{
        e = _IQ23div(CONST_IQ_2PI,v->av) - v->w;   //(2*pi*fs/av) - w
		v->ee = v->ee + _IQ23mpy(v->kp,(e - v->e0)) + _IQ23mpy(v->ki, e);
		w_new = v->w + v->ee;

		if (w_new>_IQ23(SIZE_SMOOTH_INPUT))
		   w_new = _IQ23(SIZE_SMOOTH_INPUT);

		w_int = _IQ23int(w_new);
		summ = my_mean(w_int,v);
		v->w  = _IQ23(w_int);
		v->av = _IQ23div(summ, v->w);

	}

	if (v->c<SIZE_SMOOTH_INPUT)
	  v->c++;

	v->e0 = e;
	v->w_int = w_int;
}


#pragma CODE_SECTION(smooth_simple_calc,".fast_run");
void smooth_simple_calc(SMOOTH *v)
{
 volatile _iq23 summ=0;

	if (v->c <= v->w_int_simple )
	{
		summ = my_mean(v->c, v);
		v->summ = summ;
		v->av = _IQ23div(summ,_IQ23(v->c));
	}
	else
	{
		summ = my_mean(v->w_int_simple, v);
		v->summ = summ;
		v->av = _IQ23div(summ, _IQ23(v->w_int_simple));
	}

	if (v->c<MAX_SIZE_SMOOTH_INPUT)
	  v->c++;


}




void iq_smooth (_iq23 *input, _iq23 *output, int n, int window)
{
   int i,j,z,k1,k2,hw;
   int fm1,fm2;

   _iq23 tmp;


   fm1 = window/2;
   fm2 = fm1*2;
   if ((window-fm2)==0) 
     window++;

   hw = (window-1)/2;
   output[0] = input[0];

   for (i=1;i<n;i++)
   {
       tmp=0;
       if(i<hw)
       {
           k1=0;
           k2=2*i;
           z=k2+1;
       }
       else if((i+hw)>(n-1))
       {
           k1=i-n+i+1;
           k2=n-1;
           z=k2-k1+1;
       }
       else
       {
           k1=i-hw;
           k2=i+hw;
           z=window;
       }

       for (j=k1;j<=k2;j++)
       {
           tmp=tmp + input[j];
       }
       output[i] = _IQ23div(tmp,_IQ23(z));
	}

}