/***************************************************************/
/*                                                             */
/*   Fast Fourier Transformatie                                */
/*                                                             */
/*                                                             */
/*                                                             */
/***************************************************************/

#include <string.h>
#include <math.h>
#include <stdio.h>
#include <limits.h>
#include <math.h>
#include "allegro.h"  /* only for TRUE definition */
#include "asm.h"
#include "parse.h"
#include "commnd.h"
#include "image.h"
#include "errors.h"
#include "imagwind.h"
#include "plotutil.h"
#include "windows.h"
#include "fft.h"

#define PI M_PI
#define MIN_LENGTH 512

extern VENSTER* vp[MAXSIG];
extern unsigned short MAX_LENGTH;
extern unsigned short SAMPLE_RATE;
extern int current_signal;  /* The actual signal drawn to screen */
extern BOOL don;

/**************************************************************************
 *
 * int FFTCi(char *p)
 *
 **************************************************************************/

int FFTCi(char *p)
{
	int in_im,out_im,i,windowing;  /* Vensternummer */
	int lengte,l_max,mid_type,h;
	unsigned short fft_lengte,records_out;
	unsigned int alloc_size;
	char *in_signalname;
	char *out_signalname;
	char buffer1[128],buffer2[128];
	ASM_SAMPLE *real_data_out;
	ASM_SAMPLE *imag_data_out;
	ASM_SAMPLE *real_data_in;
	ASM_SAMPLE *imag_data_in;


	in_signalname = strarg(&p,NULL,"Input Signal","a",buffer1);
	out_signalname = strarg(&p,NULL, "Output Signal","b",buffer2);

	in_im = Find_Signal_Venster(in_signalname);
	if (in_im == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */


	l_max= log10(DataLength(vp[in_im]))/log10(2);
	lengte = intarg(&p,NULL,"Size (2^n)",MIN_N,l_max,l_max);
	windowing = intarg(&p,NULL,"Window-type",0,6,1);
	mid_type = intarg(&p,NULL,"Average-type",0,1,0);


	if (DataDomain(vp[in_im]) != TIME) return ERR_TRANSFORM;

	alloc_size = DataAllocSize(vp[in_im]); /* Geheugenruimte grootte nodig voor nieuw venster */

	out_im = Find_Signal_Venster(out_signalname);      /* Vind het juiste venster */
	if (out_im == -1)                                  /* Signaal bestond nog niet */
	{
		out_im = Create_Venster(out_signalname,alloc_size);
		if (out_im == -1) return ERR_WINDOWS;             /* Te veel displaywindows */
	}
	else if(in_im != out_im) Realloc_Venster(vp[out_im],alloc_size);




	real_data_out =  DataReal(vp[out_im]);             /* Bewaar de pointers voor de data */
	imag_data_out =  DataImag(vp[out_im]);
	real_data_in  =  DataReal(vp[in_im]);
	imag_data_in  =  DataImag(vp[in_im]);


	(*vp[out_im]).header = (*vp[in_im]).header;   /* Maak de headers aan elkaar gelijk   */



	DataReal(vp[out_im]) = real_data_out;             /* Zet de pointers van de data weer terug*/
	DataImag(vp[out_im]) = imag_data_out;

	fft_lengte = (short) pow(2,lengte);    /* lengte van de FFT  moet een macht van 2 zijn */
	if (lengte > DataLength(vp[in_im])) fft_lengte = DataLength(vp[in_im]);
	/* aantal FFT's per channel */
	records_out = (short) ((DataLength(vp[in_im])/fft_lengte)*DataRecords(vp[in_im]));

	asm_wprintf("records_out = %d",records_out);

	DataLength(vp[out_im])=fft_lengte;
	DataRecords(vp[out_im])=records_out;
	DataDomain(vp[out_im])=(short)FREQ;
	DataType(vp[out_im])=(short)COMP;
	strncpy(DataSignalName(vp[out_im]),out_signalname,sizeof(DataSignalName(vp[out_im])));
	Window_Mode(vp[out_im])= BODE_M;
	Window_Record(vp[out_im]) = 0;
	Window_Channel(vp[out_im]) = 0;
	Window_HScale(vp[out_im]) = Window_HScale(vp[in_im]);

	for (h=0; h < DataChannels(vp[out_im]); ++h)
	{

		for (i=0; i < records_out; ++i)
		{

			/* Vermenigvuldig de ingangsdata met een widow en zet die in out_im */
			Window_functions(real_data_in,imag_data_in,real_data_out,imag_data_out,fft_lengte,windowing);

			fft(real_data_out,imag_data_out,fft_lengte,1);

			/*   asm_wprintf("FFT %d done \n",i);  */

			/* Hoog de adressen fft_lengte op */
			real_data_out = real_data_out + fft_lengte;
			imag_data_out = imag_data_out + fft_lengte;
			real_data_in  = real_data_in  + fft_lengte;
			imag_data_in  = imag_data_in  + fft_lengte;

		}
	}


	if (don)
	{
		make_plot(vp[out_im]);
		current_signal = out_im;
		Window_InUse(vp[out_im]) = TRUE;
	}
	else
		Window_InUse(vp[out_im]) = FALSE;
	return 0;
}

/************************************************************************
 *
 * int IFFTCi(char *p)
 *
 ************************************************************************/

int IFFTCi(char *p)
{
	int in_im,out_im,i;  /* Vensternummer */
	unsigned int alloc_size;
	short lengte,aantal;
	char *in_signalname;
	char *out_signalname;
	char buffer1[128],buffer2[128];
	ASM_SAMPLE *real_data_out;
	ASM_SAMPLE *imag_data_out;
	ASM_SAMPLE *real_data_in;
	ASM_SAMPLE *imag_data_in;


	in_signalname = strarg(&p,NULL,"Input Signal","a",buffer1);
	out_signalname = strarg(&p,NULL, "Output Signal","b",buffer2);

	in_im = Find_Signal_Venster(in_signalname);
	if (in_im == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */


	if (DataDomain(vp[in_im]) != FREQ) return ERR_TRANSFORM;

	alloc_size = DataAllocSize(vp[in_im]); /* Geheugenruimte grootte nodig voor nieuw venster */

	out_im = Find_Signal_Venster(out_signalname);      /* Vind het juiste venster */
	if (out_im == -1)                                  /* Signaal bestond nog niet */
	{
		out_im = Create_Venster(out_signalname,alloc_size);
		if (out_im == -1) return ERR_WINDOWS;             /* Te veel displaywindows */
	}
	else if (in_im != out_im) Realloc_Venster(vp[out_im],alloc_size);


	real_data_out =  DataReal(vp[out_im]);             /* Bewaar de pointers voor de data */
	imag_data_out =  DataImag(vp[out_im]);
	real_data_in  =  DataReal(vp[in_im]);
	imag_data_in  =  DataImag(vp[in_im]);


	(*vp[out_im]).header = (*vp[in_im]).header;   /* Maak de headers aan elkaar gelijk   */



	DataReal(vp[out_im]) = real_data_out;             /* Zet de pointers van de data weer terug*/
	DataImag(vp[out_im]) = imag_data_out;


	DataDomain(vp[out_im])=(short)TIME;
	DataType(vp[out_im])=(short)COMP;
	strncpy(DataSignalName(vp[out_im]),out_signalname,sizeof(DataSignalName(vp[out_im])));
	Window_Mode(vp[out_im])= REAL_M;
	Window_Record(vp[out_im]) = 0;
	Window_Channel(vp[out_im]) = 0;
	Window_HScale(vp[out_im]) = Window_HScale(vp[in_im]);


	lengte = DataLength(vp[out_im]);         /* IFFT lengte gelijk aan record lengte */
	aantal = DataRecords(vp[out_im])*DataChannels(vp[out_im]);    /* aantal IFFT's */

	/* Copieer de inhoud van in_im naar out_im */

	for (i=0;i < DataLength(vp[out_im])*DataRecords(vp[out_im])*DataChannels(vp[out_im]);i++)
	{

		real_data_out[i] = real_data_in[i];
		imag_data_out[i] = imag_data_in[i];
	}

	for (i=0;i < aantal;i++)
	{
		/* Doe de inverse fourier transformatie */
		fft(real_data_out,imag_data_out,lengte,-1);

		/* Hoog de adressen met lengte op */

		real_data_out = real_data_out + lengte;
		imag_data_out = imag_data_out + lengte;
		real_data_in  = real_data_in + lengte;
		imag_data_in  = imag_data_in + lengte;
	}



	if (don)
	{
		make_plot(vp[out_im]);
		current_signal = out_im;
		Window_InUse(vp[out_im]) = TRUE;
	}
	else
		Window_InUse(vp[out_im]) = FALSE;

	return 0;
}

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

int MagnitudeCi (char *p)
{
	int in_im,out_im,mid_type,channel_nr;
	int h,i,j,log;
	unsigned int offs,data_nr;
	unsigned int alloc_size;
	char *in_signalname;
	char *out_signalname;
	char buffer1[128],buffer2[128];
	ASM_SAMPLE *real_data_out;
	ASM_SAMPLE *real_data_in;
	ASM_SAMPLE *imag_data_out;
	ASM_SAMPLE *imag_data_in;
	ASM_SAMPLE *temp,*cum1,*cum2;

	in_signalname  = strarg(&p,NULL,"Input Signal","a",buffer1);
	out_signalname = strarg(&p,NULL, "Output Signal",in_signalname,buffer2);

	in_im = Find_Signal_Venster(in_signalname);
	if (in_im == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */

	if (DataDomain(vp[in_im]) != FREQ) return ERR_TRANSFORM;

	channel_nr = (unsigned short) intarg(&p,NULL,"Channel",0,(DataChannels(vp[in_im])-1),0);
	mid_type   = intarg(&p,NULL,"Average-type",0,1,0);
	log        = intarg(&p,NULL,"Log magnitude",0,1,0);

	alloc_size = DataAllocSize(vp[in_im])/DataRecords(vp[in_im]); /* Geheugenruimte grootte nodig voor nieuw venster */

	out_im = Find_Signal_Venster(out_signalname);      /* Vind het juiste venster */
	if (out_im == -1)                                  /* Signaal bestond nog niet */
	{
		out_im = Create_Venster(out_signalname,alloc_size);
		if (out_im == -1) return ERR_WINDOWS;             /* Te veel displaywindows */
	}
	else if(in_im != out_im) Realloc_Venster(vp[out_im],alloc_size);


	real_data_out = DataReal(vp[out_im]);             /* Bewaar de pointers voor de data */
	imag_data_out = DataImag(vp[out_im]);
	real_data_in  = DataReal(vp[in_im]);
	imag_data_in  = DataImag(vp[in_im]);

	(*vp[out_im]).header = (*vp[in_im]).header;   /* Maak de headers aan elkaar gelijk   */

	DataReal(vp[out_im]) = real_data_out;             /* Zet de pointers van de data weer terug*/
	DataImag(vp[out_im]) = imag_data_out;

	strncpy(DataSignalName(vp[out_im]),out_signalname,sizeof(DataSignalName(vp[out_im])));
	DataDomain(vp[out_im]) = MAGN;
	DataRecords(vp[out_im])=1;
	Window_Log(vp[out_im])=log;
	Window_Mode(vp[out_im])= MAGN_M;
	Window_Channel(vp[out_im]) = channel_nr;
	Window_Record(vp[out_im]) = 0;
	Window_HScale(vp[out_im]) = Window_HScale(vp[in_im]);

	DataType(vp[out_im])=(short)REAL;


	temp = (ASM_SAMPLE *) calloc(DataLength(vp[out_im]),sizeof(ASM_SAMPLE));
	if (temp == NULL)
	{
		asm_wprintf("Error in magnitude: calloc failed (temp)");
		return 0;
	}

	cum1 = (ASM_SAMPLE *) calloc(DataLength(vp[out_im]),sizeof(ASM_SAMPLE));
	if (cum1 == NULL)
	{
		asm_wprintf("Error in magnitude: calloc failed (cum1)");
		free(temp);
		return 0;
	}

	cum2 = (ASM_SAMPLE *) calloc(DataLength(vp[out_im]),sizeof(ASM_SAMPLE));
	if (cum2 == NULL)
	{
		asm_wprintf("Error in magnitude: calloc failed (cum2)");
		free(cum1);
		free(temp);
		return 0;
	}

	if (mid_type == 0)
	{
		for(h=0; h < DataChannels(vp[in_im]);h++)       /* aantal channels */
		{
			/* offset van betreffende channel */
			offs = h*DataRecords(vp[in_im])*DataLength(vp[in_im]);

			for (j=0; j < DataRecords(vp[in_im]);j++)     /* aantal records  */

				for (i=0;i < DataLength(vp[in_im]);i++)   /* record-lengte  */
				{
					data_nr = offs+i+j*DataLength(vp[in_im]);
					/* bepaal de magnitude  */
					temp[i] = sqrt(pow(real_data_in[data_nr],2) + pow(imag_data_in[data_nr],2));
					/* Tel de magnitudes bij elkaar op */
					cum1[i] = cum1[i] + temp[i];
				}

			for (i=0;i < DataLength(vp[in_im]);i++)
			{
				/* Deel de cumulatieve magnitudes door het aantal records */
				cum1[i] = cum1[i]/(double)(DataRecords(vp[in_im]));

				/* In het magnitude domein bevatten de channels altijd maar 1 record */
				real_data_out[i+h*DataLength(vp[in_im])]=cum1[i];
				imag_data_out[i+h*DataLength(vp[in_im])]=0.0;

				cum1[i] = 0.0;    /* leeg maken voor volgende channel  */
			}
		}

	}

	if (mid_type == 1)
	{
		for(h=0; h < DataChannels(vp[in_im]);h++)
		{
			/* offset van betreffende channel */
			offs = h*DataRecords(vp[in_im])*DataLength(vp[in_im]);

			for (j=0; j < DataRecords(vp[in_im]);j++)
			{
				/* Tel de records bij van een channel elkaar op */
				for (i=0;i < DataLength(vp[in_im]);i++)
				{

					cum1[i] = cum1[i] + (real_data_in[offs+i+j*DataLength(vp[in_im])]);
					cum2[i] = cum2[i] + (imag_data_in[offs+i+j*DataLength(vp[in_im])]);
				}
			}
			for (i=0;i < DataLength(vp[in_im]);i++)
			{
				/* Deel door het aantal records */
				cum1[i] = cum1[i] / (double)(DataRecords(vp[in_im]));
				cum2[i] = cum2[i] / (double)(DataRecords(vp[in_im]));

				/* Bepaal de magnitude */
				/* In het magnitude domein bevatten de channels altijd maar 1 record */
				real_data_out[i+h*DataLength(vp[out_im])] = sqrt( pow(cum1[i],2) + pow(cum2[i],2) );
				imag_data_out[i+h*DataLength(vp[out_im])] = 0.0;


				cum1[i] = cum2[i] = 0.0;   /* leeg maken voor volgend channel */
			}
		}
	}

	free(cum2);
	free(cum1);
	free(temp);


	if (don)
	{
		make_plot(vp[out_im]);
		current_signal= out_im;
		Window_InUse(vp[out_im]) = TRUE;
	}
	else
		Window_InUse(vp[out_im]) = FALSE;

	return 0;   /* No Error */
}


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

int PhaseCi (char *p)
{
	int in_im,out_im,mid_type,channel_nr;
	unsigned int alloc_size;
	int h,i,j;
	char *in_signalname;
	char *out_signalname;
	char buffer1[128],buffer2[128];
	ASM_SAMPLE *real_data_out;
	ASM_SAMPLE *real_data_in;
	ASM_SAMPLE *imag_data_out;
	ASM_SAMPLE *imag_data_in;
	ASM_SAMPLE *temp,*cum1,*cum2;

	in_signalname  = strarg(&p,NULL,"Input Signal","a",buffer1);
	out_signalname = strarg(&p,NULL, "Output Signal",in_signalname,buffer2);


	in_im = Find_Signal_Venster(in_signalname);
	if (in_im == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */

	if (DataDomain(vp[in_im]) != FREQ) return ERR_TRANSFORM;

	channel_nr = (unsigned short) intarg(&p,NULL,"Channel",0,(DataChannels(vp[in_im])-1),0);
	mid_type   = intarg(&p,NULL,"Average-type",0,1,0);


	alloc_size = DataAllocSize(vp[in_im])/DataRecords(vp[in_im]); /* Geheugenruimte grootte nodig voor nieuw venster */

	out_im = Find_Signal_Venster(out_signalname);      /* Vind het juiste venster */
	if (out_im == -1)                                  /* Signaal bestond nog niet */
	{
		out_im = Create_Venster(out_signalname,alloc_size);
		if (out_im == -1) return ERR_WINDOWS;             /* Te veel displaywindows */
	}
	else if (in_im != out_im) Realloc_Venster(vp[out_im],alloc_size);



	real_data_out = DataReal(vp[out_im]);             /* Bewaar de pointers voor de data */
	imag_data_out = DataImag(vp[out_im]);
	real_data_in  = DataReal(vp[in_im]);
	imag_data_in  = DataImag(vp[in_im]);

	(*vp[out_im]).header = (*vp[in_im]).header;       /* Maak de headers aan elkaar gelijk   */

	DataReal(vp[out_im]) = real_data_out;             /* Zet de pointers van de data weer terug*/
	DataImag(vp[out_im]) = imag_data_out;

	strncpy(DataSignalName(vp[out_im]),out_signalname,sizeof(DataSignalName(vp[out_im])));
	DataDomain(vp[out_im]) = PHAS;
	DataRecords(vp[out_im])= 1;
	Window_Mode(vp[out_im])= PHAS_M;
	Window_Channel(vp[out_im]) = channel_nr;
	Window_Record(vp[out_im]) = 0;
	Window_HScale(vp[out_im]) = Window_HScale(vp[in_im]);

	DataType(vp[out_im])=(short)REAL;


	temp = (ASM_SAMPLE *) calloc(DataLength(vp[in_im]),sizeof(ASM_SAMPLE));
	if (temp == NULL)
	{
		asm_wprintf("Error in PhaseCi: calloc failed (temp)");
		return 0;
	}

	cum1 = (ASM_SAMPLE *) calloc(DataLength(vp[in_im]),sizeof(ASM_SAMPLE));
	if (cum1 == NULL)
	{
		asm_wprintf("Error in PhaseCi: calloc failed (cum1)");
		free(temp);
	}

	cum2 = (ASM_SAMPLE *) calloc(DataLength(vp[in_im]),sizeof(ASM_SAMPLE));
	if (cum2 == NULL)
	{
		asm_wprintf("Error in PhaseCi: calloc failed (cum2)");
		free(cum1);
		free(temp);
		return 0;
	}

	if (mid_type == 0)
	{
		for(h=0; h < DataChannels(vp[in_im]);h++)       /* aantal channels */
		{

			for (j=0; j < DataRecords(vp[in_im]);j++)     /* aantal records  */
			{


				/* Bereken het adres van record j in channel h */
				real_data_in = DataReal(vp[in_im]) + (h*DataRecords(vp[in_im]) + j)*DataLength(vp[in_im]);
				imag_data_in = DataImag(vp[in_im]) + (h*DataRecords(vp[in_im]) + j)*DataLength(vp[in_im]);


				for (i=0;i < DataLength(vp[in_im]);i++)   /* record-lengte  */
				{
					/* bepaal de phase  */
					temp[i] = (180/PI)*atan2((imag_data_in[i]),(real_data_in[i]));
					cum1[i] = cum1[i] + temp[i];   /* Cumulatief */
				}
			}

			/* Bereken het adres van channel h */
			/* In het fase domein bevat een channel altijd maar 1 record */
			real_data_out = DataReal(vp[out_im]) + h*DataRecords(vp[out_im])*DataLength(vp[out_im]);
			imag_data_out = DataImag(vp[out_im]) + h*DataRecords(vp[out_im])*DataLength(vp[out_im]);


			/* Deel de cumulatieve fase door het aantal records */

			for (i=0;i < DataLength(vp[out_im]);i++)
			{
				real_data_out[i]=cum1[i]/(double)(DataRecords(vp[in_im]));
				imag_data_out[i]=0.0;


				cum1[i] = 0.0;                      /* leeg maken voor volgende channel  */
			}
		}
	}


	if (mid_type == 1)
	{
		for(h=0; h < DataChannels(vp[in_im]);h++)
		{

			for (j=0; j < DataRecords(vp[in_im]);j++)
			{
				/* Bereken het adres van record j in channel h */
				real_data_in = DataReal(vp[in_im]) + (h*DataRecords(vp[in_im]) + j)*DataLength(vp[in_im]);
				imag_data_in = DataImag(vp[in_im]) + (h*DataRecords(vp[in_im]) + j)*DataLength(vp[in_im]);


				for (i=0;i < DataLength(vp[in_im]);i++)
				{
					/* Tel de records bij elkaar op */
					cum1[i] = cum1[i] + real_data_in[i];
					cum2[i] = cum2[i] + imag_data_in[i];
				}
			}

			/* Bereken het adres van channel h */
			/* In het fase domein bevat een channel altijd maar 1 record */

			real_data_out = DataReal(vp[out_im]) + h*DataRecords(vp[out_im])*DataLength(vp[out_im]);
			imag_data_out = DataImag(vp[out_im]) + h*DataRecords(vp[out_im])*DataLength(vp[out_im]);

			for (i=0;i < DataLength(vp[out_im]);i++)
			{
				/* deel door het aantal records */
				cum1[i] = cum1[i]/(double)(DataRecords(vp[in_im]));
				cum2[i] = cum2[i]/(double)(DataRecords(vp[in_im]));
				/* Bepaal de fase */
				real_data_out[i] = (180/PI)*atan2(cum2[i],cum1[i]);
				imag_data_out[i] = 0.0;


				cum1[i] = cum2[i] = 0.0;   /* leeg maken voor volgend channel */
			}
		}
	}
	free(cum2);
	free(cum1);
	free(temp);

	if (don)
	{
		make_plot(vp[out_im]);
		current_signal = out_im;
		Window_InUse(vp[out_im]) = TRUE;
	}
	else
		Window_InUse(vp[out_im]) = FALSE;

	return 0;
}
