/*
 *  This function fills an array with  'amplitude' and 'frequency' (0 < f < Fn=0.5, Nyquist normalized freq) values 
 *  of the peaks (and following valleys) under f_max and above f_min frequency by finding local maxima 
 *  with a bisection equivalent algorithm.
 *  It returns the number of calculated peaks 
 */
int peaksVector (
   double& peaks[],     // this vector must be dimensioned to 4*degree
   double f_max,        // the frequency under which to search peaks (f_max < Fn)
   double f_min,        // the frequency above which to search peaks (f_max > 0 )
   double& aa[],        // autoregression coefficients
   int degree)          // order of autoregression
{  
      double frequency, Qn, delta_f, delta_f2;
      double s1, s2;
      double f1, f2, fm, d1, d2, dm;
      double tolerance=0.01;                 // zero (maximum) finding tolerance (we don't need a very high precision
      double fine_stepping = 1.0 / 10.0;     // 2nd level stepping for local minimum/maximum search
      int count=0, i;
      bool growing=true;
      
      // check for Nyquist
      if (f_max > 0.5) f_max=0.5;
      
      // First evaluate Qn to set proper resolution delta_f... (see (II-70) formula in Burg's PhD thesis)
      /*
      Qn=1.0;
      for (i=0; i<degree; i++)
      {
         Qn=Qn*(1.0+MathAbs(aa[i]))/(1.0-MathAbs(aa[i]));
      }*/
      Qn=50; // above formula gives too high Qn, to be verified
      delta_f=1.0/(degree*Qn);    
      
      // ...then starts to find peaks (and valleys)
      s1=spectrumValue(f_min+delta_f,aa,degree);
      peaks[0]= s1;              // include first point ...
      peaks[1]= f_min+delta_f;   // ... and frequency
      i=2;
      d1=spectrumValue(f_min+delta_f*(1.0+fine_stepping),aa,degree) - s1 ;
      if (d1 < 0) 
         growing=false;    // set initial slope direcion
      
      for (frequency=f_min+2*delta_f; frequency < f_max || (frequency >= f_max && !growing); frequency+=delta_f)   
      {    
         s2=spectrumValue (frequency,aa,degree);
         if (s2>s1 && growing)                     
         { 
            s1=s2;                                 // updates new maximum   
         } 
         else if (s2<=s1 && growing)               // found an interval in which there is a local maximum??  
         {
            // search for a local maximum in the interval [ frequency-delta_f,frequency + delta_f ] 
            f1=frequency-2.0*delta_f;
            f2=frequency;
            delta_f2=delta_f*fine_stepping; // delta_f2 (adaptive) is used to evaluate funcion's slope   
            d1=spectrumValue(f1+delta_f2,aa,degree) - spectrumValue(f1,aa,degree);
            d2=spectrumValue(f2+delta_f2,aa,degree) - spectrumValue(f2,aa,degree);
            while (true)
            {
               // try to find maximum value by evaluating central point's slope
               fm = (f1+f2)/2.0;
               dm=spectrumValue(fm+delta_f2,aa,degree) - spectrumValue(fm,aa,degree);
               if ( MathAbs(dm) < tolerance) 
                  break;
               if (dm < 0.0) f2=fm;
               else f1=fm;
               
               delta_f2=(f2-f1)*fine_stepping;        // adapt delta_f2
               d1=spectrumValue(f1+delta_f2,aa,degree) - spectrumValue(f1,aa,degree);
               d2=spectrumValue(f2+delta_f2,aa,degree) - spectrumValue(f2,aa,degree);   
            } 
            peaks[i]= spectrumValue (fm,aa,degree);  
            peaks[i+1]=fm;
            i+=2; 
            count++;          // increments number of peaks
            growing=false;    // after a peak there must be a valley, so the funcion starts to decrease
         }        
         else if (s2<s1 && !growing)   
         {
            s1=s2;                                    // updates new minimum  
         } 
         else if (s2>=s1 && !growing)                 // found an interval in which there is a local minimum??  
         {
            // search for a local maximum in the interval [ frequency-delta_f,frequency + delta_f ] 
            f1=frequency-2.0*delta_f;
            f2=frequency;
            delta_f2=delta_f*fine_stepping; // delta_f2 (adaptive) is used to evaluate funcion's slope   
            d1=spectrumValue(f1+delta_f2,aa,degree) - spectrumValue(f1,aa,degree);
            d2=spectrumValue(f2+delta_f2,aa,degree) - spectrumValue(f2,aa,degree);
            while (true)
            {
               // try to find maximum value by evaluating central point's slope
               fm = (f1+f2)/2.0;
               dm=spectrumValue(fm+delta_f2,aa,degree) - spectrumValue(fm,aa,degree);  
               if ( MathAbs(dm) < tolerance) 
                  break;
               if (dm > 0.0) f2=fm;
               else f1=fm;      
                  
               delta_f2=(f2-f1)*fine_stepping;        // adapt delta_f2   
               d1=spectrumValue(f1+delta_f2,aa,degree) - spectrumValue(f1,aa,degree);
               d2=spectrumValue(f2+delta_f2,aa,degree) - spectrumValue(f2,aa,degree);
            
            } 
            peaks[i]= spectrumValue (fm,aa,degree);  
            peaks[i+1]=fm;
            i+=2;
            growing=true;    
         }
      }

      return(count);
}