#include "peg_pic.h"

void calfw(float *);

/***************************************************************/
/****** Calculation of current and potential oscillation
        frequences and phases *********************************/

void freqanalysis() {
	int i;


	for(i=0; i< nfft; i++) {
		cur_fft[i]= cur_hist[i];
		phi_fft[i]= phi_hist[0][i];
		mphi_fft[i]= phi_hist[1][i];
	}

	realft(phi_fft-1, freq_hi, 1);
	realft(mphi_fft-1,freq_hi, 1);
	realft(cur_fft-1, freq_hi, 1);

	/******************************************************/
	/**** Computing mag and phase of different signals  ***/

	calfw(cur_fft);                    /* Current */
	calfw(phi_fft);                    /* LHS potential */
	calfw(mphi_fft);                   /* RHS potential */

} /* END of freqanalysis() */


/*********************************************************/
/**** Computing magnitude and phase of the signal ********/
/**** "temp" is the array [0,2*freq_hi-1] corresponding **/
/**** to the fft transfom of the original array. *********/
/**** "calfw" calculates the amplitude of each harmonic **/
/**** (saves in "temp" [0, freq_hi-1]) and the corresp. **/
/**** phase (saves in "temp" [freq_hi, 2*freq_hi-1]) *****/

void calfw(float *temp) {
	int i, j;
	float *temp1, *temp2;

	temp1= (float *)malloc((nfft/2)*sizeof(float));
	temp2= (float *)malloc((nfft/2)*sizeof(float));

	temp1[0]= fabs(temp[0])/freq_hi/2;
	temp2[0]= (temp[0] > 0.0) ? 0.0 : 180.0;

	for(i=1, j=2; i< freq_hi; i++, j+=2) {
		temp1[i]= sqrt(temp[j]*temp[j]+ temp[j+1]*temp[j+1])/freq_hi;

		if((fabs(temp[j+1]) < 1e-30) && (fabs(temp[j]) < 1e-30)) {
			temp2[i]= 0.0;
		} else {
			temp2[i]= (180./3.141592653)*atan2(temp[j], temp[j+1]);
		}
	}

	for(i=0; i< freq_hi; i++) {
		temp[i]= temp1[i];
		temp[freq_hi+i]= temp2[i];
	}

	free(temp1);
	free(temp2);

} /* End of calfw() */

/****************************************************************/
