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

   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 "window.h" 
#include "parse.h" 
#include "commnd.h"
#include "errors.h" 
 
#define PI 3.14159265358979

extern void display_signal_plot(VENSTER *vp);
extern VENSTER* vp[MAXSIG];
extern unsigned short MAX_LENGTH;
extern unsigned short SAMPLE_RATE;
extern int Find_Signal_Venster(char *signalname);    /* command.c */
extern int Create_Venster(char *signalname,unsigned int Alloc_Size);         /* command.c */
extern int Window_functions(SAMPLE *real_data_in,SAMPLE *imag_data_in,SAMPLE *real_data_out,SAMPLE *imag_data_out,int lengte,int windowing);
extern int Realloc_Venster(VENSTER *vp, unsigned int alloc_size);
extern BOOL don;

/**********************************************************************
 * 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;
 SAMPLE *real_data_out;
 SAMPLE *imag_data_out;
 SAMPLE *real_in1,*imag_in1,*real_in2,*imag_in2;
 SAMPLE *real_hulp1,*imag_hulp1,*real_hulp2,*imag_hulp2; /* Hulp pointers */
 SAMPLE *hulp1_a,*hulp1_b,*hulp2_a,*hulp2_b; /* Hulp pointers */
 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;
 
   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 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));
    imag_hulp1 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));
    real_hulp2 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));
    imag_hulp2 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));



    if ( (imag_hulp1 == NULL) || (real_hulp1 == NULL) || (imag_hulp2 == NULL) || (real_hulp2 == NULL) )
    werr(1,"Error in ConvolutionCi: heap_alloc failed\r");

    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];
      }

heap_free(imag_hulp2);
heap_free(real_hulp2);
heap_free(imag_hulp1);
heap_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 */

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

Window_Diag(vp[out_im]).data=NULL;

init_plot_draw(vp[out_im]);
if (don)
 {
  gen_signal_background(vp[out_im]); 
  gen_signal_plot(vp[out_im]);
  display_signal_plot(vp[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;
 SAMPLE *real_data_out;
 SAMPLE *imag_data_out;
 SAMPLE *real_in1,*imag_in1,*real_in2,*imag_in2;
 SAMPLE *real_hulp1,*imag_hulp1,*real_hulp2,*imag_hulp2; /* Hulp pointers */
 SAMPLE *hulp1_a,*hulp1_b,*hulp2_a,*hulp2_b; /* Hulp pointers */
 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;
 
   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 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));
    imag_hulp1 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));
    real_hulp2 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));
    imag_hulp2 = (SAMPLE*) heap_alloc((unsigned int)alloc_size*sizeof(SAMPLE));



    if ( (imag_hulp1 == NULL) || (real_hulp1 == NULL) || (imag_hulp2 == NULL) || (real_hulp2 == NULL) )
    werr(1,"Error in ConvolutionCi: heap_alloc failed\r");

    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];
      }

heap_free(imag_hulp2);
heap_free(real_hulp2);
heap_free(imag_hulp1);
heap_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 */

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

Window_Diag(vp[out_im]).data=NULL;

init_plot_draw(vp[out_im]);
if (don)
 {
  gen_signal_background(vp[out_im]); 
  gen_signal_plot(vp[out_im]);
  display_signal_plot(vp[out_im]);
  Window_InUse(vp[out_im])= TRUE;
 }
else
 Window_InUse(vp[out_im]) = FALSE;
return 0;           /* No Error */

}

