/*

2.4 kbps MELP Proposed Federal Standard speech coder

Fixed-point C code, version 1.0

Copyright (c) 1998, Texas Instruments, Inc.  

Texas Instruments has intellectual property rights on the MELP
algorithm.  The Texas Instruments contact for licensing issues for
commercial and non-government use is William Gordon, Director,
Government Contracts, Texas Instruments Incorporated, Semiconductor
Group (phone 972 480 7442).

The fixed-point version of the voice codec Mixed Excitation Linear
Prediction (MELP) is based on specifications on the C-language software
simulation contained in GSM 06.06 which is protected by copyright and
is the property of the European Telecommunications Standards Institute
(ETSI). This standard is available from the ETSI publication office
tel. +33 (0)4 92 94 42 58. ETSI has granted a license to United States
Department of Defense to use the C-language software simulation contained
in GSM 06.06 for the purposes of the development of a fixed-point
version of the voice codec Mixed Excitation Linear Prediction (MELP).
Requests for authorization to make other use of the GSM 06.06 or
otherwise distribute or modify them need to be addressed to the ETSI
Secretariat fax: +33 493 65 47 16.

*/
/*

  fs_lib.c: Fourier series subroutines 

*/

/*  compiler include files  */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "spbstd.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "mat.h"
#include "math_lib.h"
#include "fs.h"
#include "constant.h"

/*  compiler constants */
#define PRINT 1

/*								*/
/*	Subroutine FIND_HARM: find Fourier coefficients using	*/
/*	FFT of input signal divided into pitch dependent bins.	*/
/*								*/
/*  Q values:
    input - Q0
    fsmag - Q13
    pitch - Q7 */

#define	FFTLENGTH	512

/* Memory definition		*/
static Shortword find_hbuf[2*FFTLENGTH];
static Longword mag[FFTLENGTH];
static Shortword wr_array[FFTLENGTH/2+1];
static Shortword wi_array[FFTLENGTH/2+1];

void main(void)
{
	int i;
	short length;
	short input[400];
	length = 200;
	v_zap(find_hbuf,2*FFTLENGTH);
    for (i = 0; i < 2*length; i+=2) 
	{
      find_hbuf[i] = input[i/2];  
    }
    fft(find_hbuf,FFTLENGTH,MONE_Q15);
}

/*	Subroutine FFT: Fast Fourier Transform 		*/
/**************************************************************
* Replaces data by its DFT, if isign is 1, or replaces data   *
* by inverse DFT times nn if isign is -1.  data is a complex  *
* array of length nn, input as a real array of length 2*nn.   *
* nn MUST be an integer power of two.  This is not checked    *
* The real part of the number should be in the zeroeth        *
* of data , and the imaginary part should be in the next      *
* element.  Hence all the real parts should have even indeces *
* and the imaginary parts, odd indeces.			      *

* Data is passed in an array starting in position 0, but the  *
* code is copied from Fortran so uses an internal pointer     *
* which accesses position 0 as position 1, etc.		      *

* This code uses e+jwt sign convention, so isign should be    *
* reversed for e-jwt.                                         *
***************************************************************/
/* Q values:
   datam1 - Q14
   isign - Q15 */

#define	SWAP(a,b) temp1 = (a);(a) = (b); (b) = temp1

void fft(Shortword *datam1,Shortword nn,Shortword isign)
{
	Shortword n,mmax,m,j,istep,i;
	Shortword wr,wi,temp1;
	Longword register L_tempr,L_tempi;
	Shortword *data;
	Longword L_temp1,L_temp2;
	Shortword index,index_step;

	data = &datam1[-1];

	n = shl(nn,1);
	j = 1;
	for( i = 1; i < n; i+=2 ) 
	{
		if ( j > i) 
		{
			SWAP(data[j],data[i]);    
			SWAP(data[j+1],data[i+1]);    
		}
		m = nn;
		while ( m >= 2 && j > m ) 
		{
			j = sub(j,m);
			m = shr(m,1);
		}
		j = add(j,m);
	}
	mmax = 2;
	index_step = nn;
	while ( n > mmax) 
	{
		istep = shl(mmax,1);  

		index = 0;
		index_step = shr(index_step,1);

		wr = ONE_Q15;
		wi = 0;
		for ( m = 1; m < mmax;m+=2) 
		{
			for ( i = m; i <= n; i += istep) 
			{
				j = i + mmax;

				//tempr = wr * data[j] - wi * data[j+1]
				L_temp1 = L_shr(L_mult(wr,data[j]),1);
				L_temp2 = L_shr(L_mult(wi,data[j+1]),1);
				L_tempr = L_sub(L_temp1,L_temp2);

				//tempi = wr * data[j+1] + wi * data[j]
				L_temp1 = L_shr(L_mult(wr,data[j+1]),1);
				L_temp2 = L_shr(L_mult(wi,data[j]),1);
				L_tempi = L_add(L_temp1,L_temp2);

				//data[j] = data[i] - tempr
				L_temp1 = L_shr(L_deposit_h(data[i]),1);
				data[j] = extract_h(L_sub(L_temp1,L_tempr));

				//data[i] += tempr
				data[i] = extract_h(L_add(L_temp1,L_tempr));

				//data[j+1] = data[i+1] - tempi
				L_temp1 = L_shr(L_deposit_h(data[i+1]),1);
				data[j+1] = extract_h(L_sub(L_temp1,L_tempi));

				//data[i+1] += tempi
				data[i+1] = extract_h(L_add(L_temp1,L_tempi));
			}
			index = add(index,index_step);
			wr = wr_array[index];
			if (isign < 0)
			    wi = negate(wi_array[index]);
			else
			    wi = wi_array[index];
		}
		mmax = istep;
	}
} /* fft */






/* Initialization of wr_array and wi_array */
void fs_init(void)
{
	Shortword i;
	Shortword fft_len2,shift,step,theta;

	fft_len2 = shr(FFTLENGTH,1);
	shift = norm_s(fft_len2);
	step = shl(2,shift);
	theta = 0;

	for (i = 0; i <= fft_len2; i++) 
	{
		wr_array[i] = cos_fxp(theta);    
		wi_array[i] = sin_fxp(theta);    
		if (i >= (fft_len2-1))
			theta = ONE_Q15;
		else
			theta = add(theta,step);
	}
}