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

   File: ConvCorr.c

   Inhoud: De funkties voor het bepalen van de convolutie en de correlatie
           tussen twee signalen.

           int ConvolutionCi(char *p), int CorrelationCi(char *p)

   Schrijvers: Edwin Zoer & Erwin Waterlander

   Datum: juni 1993

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

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


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

/**********************************************************************
 * int ConvolutionCi(char *p)

   Input : Twee signalen. Deze moeten in het tijd domein zitten en even
           groot zijn. (Evenveel elementen, records en channels)
   Output: Een signaal dat de convolutie van de twee ingangssignalen is.
           Het uitgangssignaal heeft een recordlengte die twee maal zo
           groot is als die van de ingangssignalen omdat de ingangs-
           signalen eerst gezeropad worden.
 **********************************************************************/

int ConvolutionCi(char *p)
{
	int in_im1,in_im2,out_im,i;  /* Vensternummer */
	int record,channel,windowing;
	unsigned int alloc_size,offset_in,offset_out;
	char *in_signalname1;
	char *in_signalname2;
	char *out_signalname;
	char buffer1[128],buffer2[128],buffer3[128];
	unsigned short fft_lengte, window_lengte, temp_length;
	ASM_SAMPLE *real_data_out;
	ASM_SAMPLE *imag_data_out;
	ASM_SAMPLE *real_in1,*imag_in1,*real_in2,*imag_in2;
	ASM_SAMPLE *real_hulp1,*imag_hulp1,*real_hulp2,*imag_hulp2; /* Hulp pointers */
	ASM_SAMPLE *hulp1_a,*hulp1_b,*hulp2_a,*hulp2_b; /* Hulp pointers */
	ASM_SAMPLE temp;  /* hulp variabele */


	in_signalname1  = strarg(&p,NULL,"Input Signal1","a",buffer1);
	in_signalname2  = strarg(&p,NULL,"Input Signal2","b",buffer2);
	out_signalname  = strarg(&p,NULL, "Output Signal",in_signalname2,buffer3);
	windowing = intarg(&p,NULL,"Window-type",0,6,0);

	in_im1 = Find_Signal_Venster(in_signalname1);
	if (in_im1 == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */

	in_im2 = Find_Signal_Venster(in_signalname2);
	if (in_im2 == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */

	if ((DataDomain(vp[in_im1]) != REAL) || (DataDomain(vp[in_im2]) != REAL))
		return ERR_DOMAIN;    /*verkeerde domein  */




	if ( (DataLength(vp[in_im1]) != DataLength(vp[in_im2])) || (DataRecords(vp[in_im1]) != DataRecords(vp[in_im2])) ||
	    (DataChannels(vp[in_im1]) != DataChannels(vp[in_im2]))  )
		return ERR_SIZE;

	if(DataLength(vp[in_im1]) > (pow(2,(MAX_N-1)))) return ERR_LONG;


	alloc_size = 2*DataAllocSize(vp[in_im1]); /* 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 ( !( (out_im == in_im1) || (out_im == in_im2) ) )
			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_in1 = DataReal(vp[in_im1]);
	imag_in1 = DataImag(vp[in_im1]);
	real_in2 = DataReal(vp[in_im2]);
	imag_in2 = DataImag(vp[in_im2]);



	(*vp[out_im]).header = (*vp[in_im1]).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])));

	DataType(vp[out_im])=COMP;

	Window_Mode(vp[out_im])= Window_Mode(vp[in_im1]);
	Window_Record(vp[out_im])=0;
	Window_Channel(vp[out_im])=0;
	Window_HScale(vp[out_im]) = Window_HScale(vp[in_im1]);

	temp_length = DataLength(vp[in_im1]);
	DataLength(vp[out_im]) = 2*DataLength(vp[in_im1]);  /* Vanwege zeropad */

	window_lengte = temp_length;
	fft_lengte = DataLength(vp[out_im]);

	real_hulp1=imag_hulp1=real_hulp2=imag_hulp2=NULL;

	/* Vraag geheugen aan voor de hulp pointers */
	real_hulp1 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));
	imag_hulp1 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));
	real_hulp2 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));
	imag_hulp2 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));



	if ( (imag_hulp1 == NULL) || (real_hulp1 == NULL) || (imag_hulp2 == NULL) || (real_hulp2 == NULL) )
	{
		asm_wprintf("Error in ConvolutionCi: malloc failed");
		return -1;
	}


	hulp1_a = real_hulp1;
	hulp1_b = imag_hulp1;
	hulp2_a = real_hulp2;
	hulp2_b = imag_hulp2;

	/* Copieer en Zeropad de ingangs signalen */

	for (channel = 0;channel < DataChannels(vp[in_im1]); channel++)
	{
		for (record = 0;record < DataRecords(vp[in_im1]); record++)
		{
			/* offset betreffende record */
			offset_in = (channel*DataRecords(vp[in_im1]) + record)*temp_length;
			offset_out = 2*offset_in;
			for (i=0;i < temp_length;i++)
			{
				real_hulp1[i+offset_out] = real_in1[i+offset_in];
				imag_hulp1[i+offset_out] = imag_in1[i+offset_in];
				real_hulp2[i+offset_out] = real_in2[i+offset_in];
				imag_hulp2[i+offset_out] = imag_in2[i+offset_in];
			}

			/* Records aanvullen met nullen */
			for (i=temp_length;i < 2*temp_length;i++)
			{
				real_hulp1[i+offset_out] = 0.0;
				imag_hulp1[i+offset_out] = 0.0;
				real_hulp2[i+offset_out] = 0.0;
				imag_hulp2[i+offset_out] = 0.0;
			}
		}
	}

	for (channel=0; channel < DataChannels(vp[in_im1]); channel++)
	{

		for (record=0; record < DataRecords(vp[in_im1]); record++)
		{

			/* Vermenigvuldig de ingangsdata met een window en doe de fft */

			Window_functions(real_hulp1,imag_hulp1,real_hulp1,imag_hulp1,window_lengte,windowing);
			Window_functions(real_hulp2,imag_hulp2,real_hulp2,imag_hulp2,window_lengte,windowing);

			fft(real_hulp1,imag_hulp1,fft_lengte,1);
			fft(real_hulp2,imag_hulp2,fft_lengte,1);

			/* Hoog de adressen fft_lengte op */

			real_hulp1 = real_hulp1 + fft_lengte;
			imag_hulp1 = imag_hulp1 + fft_lengte;
			real_hulp2 = real_hulp2 + fft_lengte;
			imag_hulp2 = imag_hulp2 + fft_lengte;

		}
	}

	real_hulp1 = hulp1_a;  /* Zet pointers terug */
	imag_hulp1 = hulp1_b;
	real_hulp2 = hulp2_a;
	imag_hulp2 = hulp2_b;

	/* Vermenigvuldig de resultaten met elkaar en zet dit in hulp1*/

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

		temp = real_hulp1[i];
		real_hulp1[i] = (real_hulp1[i]*real_hulp2[i] - imag_hulp1[i]*imag_hulp2[i]);
		imag_hulp1[i] = (temp*imag_hulp2[i] + imag_hulp1[i]*real_hulp2[i]);
	}

	/* Doe de inverse FFT van het resultaat */
	for (channel=0; channel < DataChannels(vp[in_im1]); channel++)
	{

		for (record=0; record < DataRecords(vp[in_im1]); record++)
		{

			/* Doe de ifft */


			fft(real_hulp1,imag_hulp1,fft_lengte,-1);

			/* Hoog de adressen fft_lengte op */

			real_hulp1 = real_hulp1 + fft_lengte;
			imag_hulp1 = imag_hulp1 + fft_lengte;

		}
	}

	real_hulp1 = hulp1_a;  /* Zet pointers terug */
	imag_hulp1 = hulp1_b;
	real_hulp2 = hulp2_a;
	imag_hulp2 = hulp2_b;


	if ( (out_im == in_im1) || (out_im == in_im2) )
	{
		Realloc_Venster(vp[out_im],alloc_size);
		real_data_out = DataReal(vp[out_im]);
		imag_data_out = DataImag(vp[out_im]);
	}

	/* Zet de data in out_im */

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

		real_data_out[i] = real_hulp1[i];
		imag_data_out[i] = imag_hulp1[i];
	}

	free(imag_hulp2);
	free(real_hulp2);
	free(imag_hulp1);
	free(real_hulp1);

	/* Geef het gereserveerde geheugen voor het (al bestaande) DrawObject vrij
   (heapfree), wis het DrawObject, reserveer nieuw geheugen (init_plot_draw)
   en maak een nieuw DrawObject */

	free(Window_Diag(vp[out_im]).data);

	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 CorrelationCi(char *p)

   Input : Twee signalen. Deze moeten in het tijd domein zitten en even
           groot zijn. (Evenveel elementen, records en channels)
   Output: Een signaal dat de convolutie van de twee ingangssignalen is.
           Het uitgangssignaal heeft een recordlengte die twee maal zo
           groot is als die van de ingangssignalen omdat de ingangs-
           signalen eerst gezeropad worden.
 **********************************************************************/

int CorrelationCi(char *p)
{
	int in_im1,in_im2,out_im,i;  /* Vensternummer */
	int record,channel,windowing;
	unsigned int alloc_size,offset_in,offset_out;
	char *in_signalname1;
	char *in_signalname2;
	char *out_signalname;
	char buffer1[128],buffer2[128],buffer3[128];
	unsigned short fft_lengte, window_lengte,temp_length;
	ASM_SAMPLE *real_data_out;
	ASM_SAMPLE *imag_data_out;
	ASM_SAMPLE *real_in1,*imag_in1,*real_in2,*imag_in2;
	ASM_SAMPLE *real_hulp1,*imag_hulp1,*real_hulp2,*imag_hulp2; /* Hulp pointers */
	ASM_SAMPLE *hulp1_a,*hulp1_b,*hulp2_a,*hulp2_b; /* Hulp pointers */
	ASM_SAMPLE temp;

	in_signalname1  = strarg(&p,NULL,"Input Signal1","a",buffer1);
	in_signalname2  = strarg(&p,NULL,"Input Signal2","b",buffer2);
	out_signalname  = strarg(&p,NULL, "Output Signal",in_signalname2,buffer3);
	windowing = intarg(&p,NULL,"Window-type",0,6,0);

	in_im1 = Find_Signal_Venster(in_signalname1);
	if (in_im1 == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */

	in_im2 = Find_Signal_Venster(in_signalname2);
	if (in_im2 == -1) return ERR_SIGNAL;       /* Signaal bestaat niet */

	if ((DataDomain(vp[in_im1]) != REAL) || (DataDomain(vp[in_im2]) != REAL))
		return ERR_DOMAIN;    /*verkeerde domein  */

	if ( (DataLength(vp[in_im1]) != DataLength(vp[in_im2])) || (DataRecords(vp[in_im1]) != DataRecords(vp[in_im2])) ||
	    (DataChannels(vp[in_im1]) != DataChannels(vp[in_im2]))  )
		return ERR_SIZE;

	if(DataLength(vp[in_im1]) > (pow(2,(MAX_N-1)))) return ERR_LONG;

	alloc_size = 2*DataAllocSize(vp[in_im1]); /* 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 ( !( (out_im == in_im1) || (out_im == in_im2) ) )
			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_in1 = DataReal(vp[in_im1]);
	imag_in1 = DataImag(vp[in_im1]);
	real_in2 = DataReal(vp[in_im2]);
	imag_in2 = DataImag(vp[in_im2]);



	(*vp[out_im]).header = (*vp[in_im1]).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])));

	DataType(vp[out_im])=COMP;

	Window_Mode(vp[out_im])= Window_Mode(vp[in_im1]);
	Window_Record(vp[out_im])=0;
	Window_Channel(vp[out_im])=0;
	Window_HScale(vp[out_im]) = Window_HScale(vp[in_im1]);

	temp_length = DataLength(vp[in_im1]);
	DataLength(vp[out_im]) = 2*DataLength(vp[in_im1]);  /* Vanwege zeropad */

	window_lengte = temp_length;
	fft_lengte =  DataLength(vp[out_im]);

	real_hulp1=imag_hulp1=real_hulp2=imag_hulp2=NULL;

	/* Vraag geheugen aan voor de hulp pointers */
	real_hulp1 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));
	imag_hulp1 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));
	real_hulp2 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));
	imag_hulp2 = (ASM_SAMPLE*) malloc((unsigned int)alloc_size*sizeof(ASM_SAMPLE));



	if ( (imag_hulp1 == NULL) || (real_hulp1 == NULL) || (imag_hulp2 == NULL) || (real_hulp2 == NULL) )
	{
		asm_wprintf("Error in ConvolutionCi: malloc failed");
		return -1;
	}

	hulp1_a = real_hulp1;
	hulp1_b = imag_hulp1;
	hulp2_a = real_hulp2;
	hulp2_b = imag_hulp2;

	/* Copieer en Zeropad de ingangs signalen */

	for (channel = 0;channel < DataChannels(vp[in_im1]); channel++)
	{
		for (record = 0;record < DataRecords(vp[in_im1]); record++)
		{
			/* offset betreffende record */
			offset_in = (channel*DataRecords(vp[in_im1]) + record)*temp_length;
			offset_out = 2*offset_in;
			for (i=0;i < temp_length;i++)
			{
				real_hulp1[i+offset_out] = real_in1[i+offset_in];
				imag_hulp1[i+offset_out] = imag_in1[i+offset_in];
				real_hulp2[i+offset_out] = real_in2[i+offset_in];
				imag_hulp2[i+offset_out] = imag_in2[i+offset_in];
			}

			/* Records aanvullen met nullen */
			for (i=temp_length;i < 2*temp_length;i++)
			{
				real_hulp1[i+offset_out] = 0.0;
				imag_hulp1[i+offset_out] = 0.0;
				real_hulp2[i+offset_out] = 0.0;
				imag_hulp2[i+offset_out] = 0.0;
			}
		}
	}

	for (channel=0; channel < DataChannels(vp[in_im1]); channel++)
	{

		for (record=0; record < DataRecords(vp[in_im1]); record++)
		{

			/* Vermenigvuldig de ingangsdata met een widow en doe de fft */

			Window_functions(real_hulp1,imag_hulp1,real_hulp1,imag_hulp1,window_lengte,windowing);
			Window_functions(real_hulp2,imag_hulp2,real_hulp2,imag_hulp2,window_lengte,windowing);

			fft(real_hulp1,imag_hulp1,fft_lengte,1);
			fft(real_hulp2,imag_hulp2,fft_lengte,1);

			/* Hoog de adressen fft_lengte op */

			real_hulp1 = real_hulp1 + fft_lengte;
			imag_hulp1 = imag_hulp1 + fft_lengte;
			real_hulp2 = real_hulp2 + fft_lengte;
			imag_hulp2 = imag_hulp2 + fft_lengte;

		}
	}

	real_hulp1 = hulp1_a;  /* Zet pointers terug */
	imag_hulp1 = hulp1_b;
	real_hulp2 = hulp2_a;
	imag_hulp2 = hulp2_b;

	/* Vermenigvuldig de resultaten met elkaar en zet dit in hulp1
     waarbij van hulp1 de complex toegevoegde wordt genomen */

	for (i=0;i < (DataChannels(vp[out_im])*DataRecords(vp[out_im])*DataLength(vp[out_im]));i++)
	{
		temp = real_hulp1[i];
		real_hulp1[i] = (real_hulp1[i]*real_hulp2[i] - (-imag_hulp1[i])*imag_hulp2[i]);
		imag_hulp1[i] = (temp*imag_hulp2[i] + (-imag_hulp1[i])*real_hulp2[i]);
	}

	/* Doe de inverse FFT van het resultaat */
	for (channel=0; channel < DataChannels(vp[in_im1]); channel++)
	{

		for (record=0; record < DataRecords(vp[in_im1]); record++)
		{

			/* Doe de ifft */


			fft(real_hulp1,imag_hulp1,fft_lengte,-1);

			/* Hoog de adressen fft_lengte op */

			real_hulp1 = real_hulp1 + fft_lengte;
			imag_hulp1 = imag_hulp1 + fft_lengte;

		}
	}

	real_hulp1 = hulp1_a;  /* Zet pointers terug */
	imag_hulp1 = hulp1_b;
	real_hulp2 = hulp2_a;
	imag_hulp2 = hulp2_b;


	if ( (out_im == in_im1) || (out_im == in_im2) )
	{
		Realloc_Venster(vp[out_im],alloc_size);
		real_data_out = DataReal(vp[out_im]);
		imag_data_out = DataImag(vp[out_im]);
	}

	/* Zet de data in out_im */

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

		real_data_out[i] = real_hulp1[i];
		imag_data_out[i] = imag_hulp1[i];
	}

	free(imag_hulp2);
	free(real_hulp2);
	free(imag_hulp1);
	free(real_hulp1);

	/* Geef het gereserveerde geheugen voor het (al bestaande) DrawObject vrij
   (heapfree), wis het DrawObject, reserveer nieuw geheugen (init_plot_draw)
   en maak een nieuw DrawObject */

	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 */

}
