#define squareOut   8       // Sortie Carrée

#define N           128     // Données N=2^n, TFD Nf=2^(n-1)
                            // Agit sur la Résolution fréquentielle!!!!!!!! 
                            // df= Fs/N
                             
#define Ts_us       500     // Ts=1/Fs: Approchée (voir le programme) 
                            // Agit sur la fréquence MAX du spéctre!!!!!!!! 
                            // FMXA = Fs/2

bool State=false; 
unsigned long T1=0, T2=0; 
float Ts=0.0, Fs=0.0; 
float Data[N], Nfft=N>>1; 
float dF=0.0; 
float RealFFT[N>>1], ImagFFT[N>>1], ModFFT[N>>1];
float MaxIdMax[2]={0.0,0.0}; 
float Am, ReIm[2]={0.0,0.0};
float sMoy=0.0; 
int n=0; 



const double q=1.0/4096.0; 


void setup()
{
  // Square Out - PWM (Due) - Non utilisée 
  /* 
  pinMode(squareOut, OUTPUT); 
  analogWriteResolution(12);     // Ajustable <= 12 (4096 Niveaux) 
  analogWrite(squareOut, 2048);  // Pins 2-13, 1000 Hz !!
  */
  
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  
  // Affichage 
  Serial.begin(250000); 
}

void loop()
{ 
  // 1. Lecture des données
  float A0_i=0.0,A1_i=0.0;
  Ts=(float)Ts_us;
  for(int i=0; i<N; i++)
  {
    T1=micros();
    A0_i=(float)analogRead(A0)+(0.5*q*rand10()); 
    A1_i=(float)analogRead(A1)+(0.5*q*rand10()); 
    Data[i]=(A0_i-A1_i);
    Data[i]*=3.3/4096.0; 
    delayMicroseconds(Ts_us);
    Ts+=(float)micros()-(float)T1; 
    Ts=Ts/2.0;
    //Serial.println(1000.0*Data[i]); 
  }
  Fs=1E6/Ts; 
  //Serial.println(Fs); return; 

  // 2. Calcul du spectre 
  dF=DFT(Data, N, RealFFT, ImagFFT, Fs); 

  // Calcul du Module de la DFT
  getModul(RealFFT, ImagFFT, ModFFT, N>>1); 

  // Extraction de la valeur Max
  ModFFT[0]=0.0;getMax(ModFFT, N>>1, MaxIdMax);

  // Affichage (f0) 
  //Serial.print(MaxIdMax[0]); Serial.print(" \t");
  //Serial.print(MaxIdMax[1]); Serial.print(" \t");
  Serial.println(MaxIdMax[1]*dF);
  return; 

  // 3. Visualisation du spectre  
  static bool state=false;
  if(state==false)
  {
    Serial.println("");
    for (int i=0;i<(N>>1)-1;i++)
    {
      if (i!=0) 
      {
        Serial.print(dF*(float)i); Serial.print("\t");
        Serial.print(2.0*1000.0*ModFFT[i]/(float)N); Serial.print("\t");
        Serial.println(1000*Data[2*i]); 
      }
      else 
      {
        Serial.print(dF*(float)i); Serial.print("\t");
        Serial.print(1000.0*ModFFT[i]/(float)N); Serial.print("\t"); 
        Serial.println(1000*Data[2*i]);
      }
    }
    Serial.println("");
    state=!state;
  }
}

float DFT(float *data, int Ndata, float *real, float *imag, float fs)
{
  int nfft=floor((float)Ndata/2.0); 
  float df=(fs/2.0)/(float)nfft; 
  float somme_r=0.0, somme_i=0.0;  
  float wt=0.0; 
  
  for (int i=0; i<nfft; i++)
  {
      for (int j=0; j<Ndata; j++)
      {
        wt=-2.0*PI*(float)i*(float)j/Ndata;
        somme_r+=data[j]*cos(wt); 
        somme_i+=data[j]*sin(wt); 
      }      
      real[i]= somme_r; somme_r=0.0;
      imag[i]= somme_i; somme_i=0.0;
  }
  return df; 
}

float DFTn(float *data, int Ndata, float *realImag, int n, float fs)
{
  int nfft=floor((float)Ndata/2.0); 
  float df=(fs/2.0)/(float)nfft; 
  float somme_r=0.0, somme_i=0.0;  
  float wt=0.0; 

  if (n>=nfft) return 0.0; 
  
  int i=n;
  for (int j=0; j<Ndata; j++)
  {
    wt=-2.0*PI*(float)i*(float)j/(float)Ndata;
    somme_r+=data[j]*cos(wt); 
    somme_i+=data[j]*sin(wt); 
  }      
  realImag[0]= somme_r;
  realImag[1]= somme_i; 
  return df; 
}

 void getMax(float *datain, int n, float *maxid)
 {
    float maxe=datain[0];
    float id=0; 

    for (int i=1;i<n; i++)
    {
      if (datain[i]>=maxe)
      {
        maxe=datain[i]; 
        id=i; 
      }
    }
    maxid[0]=maxe; 
    maxid[1]=id; 
 }

void getModul(float *real, float *imag, float *modul, int n)
{
  for (int i=0;i<n; i++)
    modul[i]=sqrt((imag[i]*imag[i]) + (real[i]*real[i])); 
}

double rand10(void)
{
  double rn1=2.0*((random(1e10)/1e10)-0.5); 
  double rn2=2.0*((random(1e10)/1e10)-0.5); 
  double rn=tanh((rn1-rn2)); 

  return rn; 
}