/***************************************************************/ 
/*                                                             */ 
/*   Fast Fourier Transformatie                                */ 
/*                                                             */ 
/*                                                             */ 
/*                                                             */ 
/***************************************************************/ 
 
#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

#define MIN_LENGTH 512

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 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];
 SAMPLE *real_data_out;
 SAMPLE *imag_data_out;
 SAMPLE *real_data_in;
 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]));  


   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;

   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);

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

      }
     }


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

if (don)
  {
   gen_bode_background(vp[out_im]); 

   gen_bode_plot(vp[out_im],mid_type);
   display_signal_plot(vp[out_im]);
  
   Window_InUse(vp[out_im]) = TRUE;
  }
else
   Window_InUse(vp[out_im]) = FALSE;
return 0;
}

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

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];
 SAMPLE *real_data_out;
 SAMPLE *imag_data_out;
 SAMPLE *real_data_in;
 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;

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


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

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

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];
  SAMPLE *real_data_out;
  SAMPLE *real_data_in;
  SAMPLE *imag_data_out;
  SAMPLE *imag_data_in;
  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;
       
  DataType(vp[out_im])=(short)REAL;

 
  temp = (SAMPLE *) calloc(DataLength(vp[out_im]),sizeof(SAMPLE));
  if (temp == NULL)
  werr (1,"Error in magnitude: calloc failed\r");
  
  cum1 = (SAMPLE *) calloc(DataLength(vp[out_im]),sizeof(SAMPLE));
  if (cum1 == NULL)
  werr (1,"Error in magnitude: calloc failed\r");
  
  cum2 = (SAMPLE *) calloc(DataLength(vp[out_im]),sizeof(SAMPLE));
  if (cum2 == NULL)
  werr (1,"Error in magnitude: calloc failed\r");   

  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);
 

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

if (don)
 {
  gen_magnitude_background(vp[out_im]);
  gen_magnitude_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 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];
  SAMPLE *real_data_out;
  SAMPLE *real_data_in;
  SAMPLE *imag_data_out;
  SAMPLE *imag_data_in;
  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;
       
  DataType(vp[out_im])=(short)REAL;

 
  temp = (SAMPLE *) calloc(DataLength(vp[in_im]),sizeof(SAMPLE));
  if (temp == NULL)
  werr (1,"Error in PhaseCi: calloc failed\r");
  
  cum1 = (SAMPLE *) calloc(DataLength(vp[in_im]),sizeof(SAMPLE));
  if (cum1 == NULL)
  werr (1,"Error in PhaseCi: calloc failed\r");
  
  cum2 = (SAMPLE *) calloc(DataLength(vp[in_im]),sizeof(SAMPLE));
  if (cum2 == NULL)
  werr (1,"Error in PhaseCi: calloc failed\r");   

  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);
 

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

if (don)
  {
  gen_phase_background(vp[out_im]);
  gen_phase_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;
}

