/*
* DSP Library for ESIM Project 2009 (Software Defined Radio)
* students Work with help of LESIM Group.
* 
* Students:
* Delli Veneri Luca, Espresso Giovanni, Facchiano Giambattista, Gramazio Paolo, Varricchio Giuseppe 
*/

#include <math.h>
//#include <DSPF_sp_cfftr2_dit.h>
//#include <DSPF_sp_icfftr2_dif.h>
#define M_PI	3.14159265358979323846
#define N 512
#define LB 106
#define B 1000
#define NN (N-2*LB)
#define STD_AM 0.3
#define STD_FM 0.04


//COMPLEX DEFINITION

struct complex {
	float re[N];
	float img[N];
};


//FUNCTION PROTOTYPES

struct complex hilbert(float p[], int dim);
void unwrap(float p[],float u[],int dim);
float std(float x[],int dim);
void diff(float p[],float u[],int dim);
void rmvq(float p[],float u[],int dim, int lb);
void angle(struct complex c,float u[],int dim);
void module(struct complex c,float u[],int dim);
int clas(float stdAM,float stdFM);
void norm_by_std(float p[],float u[], int dim);

/* How to use: FFT
gen_w_r2(w, N); // Generate coefficient table
bit_rev(w, N>>1); // Bit-reverse coefficient table
DSPF_sp_cfftr2_dit(x, w, N); //input in normal order, output in order bit-reversed coefficient table in bit-reversed order
bit_rev(x, N); // input in bit-reversed, output in normal order.
*/
void DSPF_sp_cfftr2_dit(float* x, float* w, short n);
void gen_w_r2(float* w, int n);
void bit_rev(float* x, int n);

/* How to use: IFFT
gen_w_r2(w, N); // Generate coefficient table
bit_rev(w, N>>1); // Bit-reverse coefficient table
bit_rev(x,N); // bit-reverse output
DSPF_sp_icfftr2_dif(x, w, N); // Inverse radix 2 FFT input in bit-reversed order, order output in normal coefficient table in bit-reverse order
divide(x, N); // scale inverse FFT output result is the same as original input
*/
void DSPF_sp_icfftr2_dif(float* x, float* w, short n);
void divide(float* x, int n);


//FUNCTION IMPLEMENTATIONS

/* Hilbert Transform - using FFT and IFFT by TI dsplib for DSP TMS320C6711 MathWorks method Algorithm */
struct complex hilbert(float p[],int dim) {
	struct complex c;
	float t_x1[2*N],h[2*N];
	float w[N];
	int i;
	for(i=0; i<dim; i++) {
		t_x1[2*i]=p[i];
		t_x1[2*i+1]=0.0;
	}
//1. Hilbert calculates the FFT of the input sequence, storing the result in a vector x.
	gen_w_r2(w, dim);
	bit_rev(w, dim>>1);
	DSPF_sp_cfftr2_dit(t_x1, w, dim);
	bit_rev(t_x1, dim);
//2. It creates a vector h(i) for maskig.
	h[0]=1;
	h[dim]=1;
	for(i=1; i<dim; i++) h[i]=2;
	for(i=(dim+1); i<2*dim; i++) h[i]=0;
//3. It calculates the element-wise product of x and h.
	for(i=0; i<2*dim; i++) t_x1[i] *= h[i];
//4. It calculates the inverse FFT of the sequence obtained in step 3 and returns the first n elements of the result.
	gen_w_r2(w, dim);
	bit_rev(w, dim>>1);
	bit_rev(t_x1, dim);
	DSPF_sp_icfftr2_dif(t_x1, w, dim);
	divide(t_x1, dim);
//5. create the Complex Struct type.
	for(i=0; i<dim; i++) {
		c.re[i]=t_x1[2*i];
		c.img[i]=t_x1[2*i+1];
	}
	return c;
}

/* Unwrap - Phase Angle using Mathworks algorithm method */
void unwrap(float p[],float u[],int dim) {
	int i, k=0;
	for (i=0;i<dim;i++) {
		if(fabs(p[i+1]-p[i])<= M_PI) u[i]=p[i]+2*M_PI*k;
		else {
			u[i]=p[i]+2*M_PI*k;
			if(p[i+1]<p[i]) k++;
			else if(p[i+1]>p[i]) k--;
			else ;
		}
	}
}

/* STD - Standard Deviation - the Statistic inference type */
float std(float x[],int dim) {
	int i;
	float avg=0, std=0;
	for(i=0; i<dim; i++) avg+=x[i];
	avg/=dim;
	for(i=0;i<dim;i++) std+=pow((x[i]-avg),2);
	std/=(dim-1);
	std=sqrt(std);
	return std;
}

/* Diff function like MATLAB type (Mathworks) */
void diff(float p[],float u[],int dim) {
	int i;
	for (i=0;i<dim;i++) u[i]=p[i+1]-p[i];
}

/* Remove Queues - function for reduce and eliminate 2 queues from a Vector */
void rmvq(float p[],float u[],int dim, int lb) {
	int i;
	for(i=0;i<(dim-2*lb);i++) u[i]=p[lb+i];
}

/* Angle - Phase Angle calculated by ATAN2 function with complex (Vector) */
void angle(struct complex c,float u[],int dim) {
	int i;
	for(i=0; i<dim;i++) u[i] = atan2(c.img[i],c.re[i]);
}

/* Module - also ABS for Absolute Value of complex (Vector) */
void module(struct complex c,float u[],int dim) {
	int i;
	for(i=0; i<dim;i++) u[i] = sqrt(pow(c.re[i],2)+ pow(c.img[i],2));
}

/* Signal Classification - AM  or PM/FM, or No Modulation */
int clas(float stdAM,float stdFM) {
	//if (stdAM >= STD_AM && stdFM >=  STD_AM) return 0; //Unknown modulation
	if(stdAM >= STD_AM) return 1; //AM --> && stdFM < STD_MIN
	else if(stdFM >= STD_FM) return 2; //FM or PM --> && stdAM<STD_MIN
	else return 3; //No Modulation or error
}
    
/* Signal normalization by his STD (Standard Deviation) */
void norm_by_std(float p[],float u[], int dim) {
	int i;
	float t_std = std(p,dim);
	for(i=0;i<dim;i++) u[i]=p[i]/t_std;
}

/* Single-precision cache optimized radix-2 forward FFT with complex input  */
void DSPF_sp_cfftr2_dit(float* x, float* w, short n) {
	short n2, ie, ia, i, j, k, m;
	float rtemp, itemp, c, s;
	n2 = n;
	ie = 1;
	for(k=n; k > 1; k >>= 1) {
		n2 >>= 1;
		ia = 0;
		for(j=0; j < ie; j++) {
			c = w[2*j];
			s = w[2*j+1];
			for(i=0; i < n2; i++) {
				m = ia + n2;
				rtemp = c * x[2*m] + s * x[2*m+1];
				itemp = c * x[2*m+1] - s * x[2*m];
				x[2*m] = x[2*ia] - rtemp;
				x[2*m+1] = x[2*ia+1] - itemp;
				x[2*ia] = x[2*ia] + rtemp;
				x[2*ia+1] = x[2*ia+1] + itemp;
				ia++;
			}
			ia += n2;
		}
		ie <<= 1;
	}
}

/* generate real and imaginary twiddle table of size n/2 complex numbers */
void gen_w_r2(float* w, int n) {
	int i;
	float pi = 4.0*atan(1.0);
	float e = pi*2.0/n;
	for(i=0; i < ( n>>1 ); i++) {
		w[2*i] = cos(i*e);
		w[2*i+1] = sin(i*e);
	}
}

/* bit-reverse the output */
void bit_rev(float* x, int n) {
	int i, j, k;
	float rtemp, itemp;
	j = 0;
	for(i=1; i < (n-1); i++) {
		k = n >> 1;
		while(k <= j) {
			j -= k;
			k >>= 1;
		}
		j += k;
		if(i < j) {
			rtemp = x[j*2];
			x[j*2] = x[i*2];
			x[i*2] = rtemp;
			itemp = x[j*2+1];
			x[j*2+1] = x[i*2+1];
			x[i*2+1] = itemp;
		}
	}
}

/* Single-precision cache optimized radix-2 Inverse FFT with complex input  */
void DSPF_sp_icfftr2_dif(float* x, float* w, short n) {
	short n2, ie, ia, i, j, k, m;
	float rtemp, itemp, c, s;
	n2 = 1;
	ie = n;
	for(k=n; k > 1; k >>= 1) {
		ie >>= 1;
		ia = 0;
		for(j=0; j < ie; j++) {
			c = w[2*j];
			s = w[2*j+1];
			for(i=0; i < n2; i++) {
				m = ia + n2;
				rtemp = x[2*ia] - x[2*m];
				x[2*ia] = x[2*ia] + x[2*m];
				itemp = x[2*ia+1] - x[2*m+1];
				x[2*ia+1] = x[2*ia+1] + x[2*m+1];
				x[2*m] = c*rtemp - s*itemp;
				x[2*m+1] = c*itemp + s*rtemp;
				ia++;
			}
			ia += n2;
		}
		n2 <<= 1;
	}
}

/* divide each element of x by n */
void divide(float* x, int n) {
	int i;
	float inv = 1.0 / n;
	for(i=0; i < n; i++) {
		x[2*i] = inv * x[2*i];
		x[2*i+1] = inv * x[2*i+1];
	}
}

