/** 
 * The UltrasoundDetector class can be used to help analyze an 
 * ultrasound signal.  UltrasoundDetector creates three Goertzel objects
 * in order to determine whether or not the desired frequency is present
 * in a set of data.  One Goertzel object performs the Goertzel
 * algorithm with the target frequency, obtaining a relative magnitude
 * for this frequency, while the other two Goertzel objects perform the
 * Goertzel algorithm with frequencies slightly above and slightly below
 * the target frequency.  The relative magnitude returned by the target
 * frequency is compared to the relative magnitudes returned by the other
 * two frequencies.  If the magnitude of the target frequency is more
 * than twice the average of the other two magnitudes, the
 * UltrasoundDetector class decides that ultrasound has been detected.
 * <p>
 * After it is constructed with the desired parameters, an
 * UltrasoundDetector must be initialized with the init method.  After
 * initialization, the analyze method can be called to determine whether
 * the target frequency is present in a block of N data samples.
 * <p>
 * This class is based on a C program implemented by Kevin Banks of 
 * Embedded Systems Programming.
 *
 * @author Chris Palistrant
 * @author Tony Offer
 * @version 0.0, June 2004
 */

public class UltrasoundDetector implements Constants {

    protected boolean debug = false;
    
    // Declare three instances of the Goertzel algorithm: one for
    // detecting the ultrasound frequency, one for detecting a frequency
    // slightly above the ultrasound frequency, and one for detecting a
    // frequency slightly below the ultrasound frequency.
    protected Goertzel g, gless, gmore;
    
    /**
     * Default Constructor
     */
    public UltrasoundDetector(){
	this(SAMPLING_RATE, TARGET_FREQUENCY, N, false);
    }
    

    /**
     * Constructor.
     * @param sampleRate is the sampling rate of the signal to be analyzed
     * @param targetFreq is the frequency that Goertzel will look for.
     * @param inN is the block size to use with Goertzel
     * @param inDebug indicates whether or not to turn debugging info on.
     */
    public UltrasoundDetector(float sampleRate, float targetFreq, int inN,
		              boolean inDebug){
	// Calculate the frequencies that will be slightly above and
	// below the target frequency.  These frequencies will be looked
	// for in the signal to make sure they don't exist.
	int difference = ((int)sampleRate)*19/inN;
	float targetFreqless = targetFreq - difference;
	float targetFreqmore = targetFreq + difference;
	    
	// Instantiate a Goertzel algorithm for each of the frequencies.
	g = new Goertzel(sampleRate, targetFreq, inN, inDebug);	
	gless = new Goertzel(sampleRate, targetFreqless, inN, inDebug);
	gmore = new Goertzel(sampleRate, targetFreqmore, inN, inDebug);
	
	debug = inDebug;
/*
	// Not tested; but intended to eliminate n from param list by 
	// computing n based on N

        float temp_n;
	float a = targetFreq/sampleRate;

	for(int n = 200; n <= 600; n++){
	    temp_n = a*n;
            //if temp_n is accurate to 0.0XXX of an int, then its okay
	    if( ( (int)(temp_n*10) - ((int)temp_n)*10 ) == 0  )
		N = temp_n;
	}
	if(n==0){
	    System.out.println("Block size error");
	    System.exit(0);
	}    
*/
    }

    /**
     * Method to initialize the Goertzel algorithm.  If this method is
     * not called, the Goertzel algorithm will be initialized using the
     * default values defined in Goertzel.java.
     */
    public void init() {
        g.initGoertzel();
	gless.initGoertzel();
	gmore.initGoertzel();
    }
    
    /**
     * Provide means to access the value of N that is used by the
     * Goertzel algorithm to detect the presence of a specified
     * frequency.  N should be the number of samples that are given to
     * this class to process since the Goertzel algorithm returns a
     * result every N samples.
     *
     * @return    The value of N used by the Goertzel algorithm.
     */
    public int getN() {
        return N;
    }
    
    /**
     * This is the main method of the UltrasoundDetector class and it is
     * used for testing N samples for the presence of the frequency
     * specified by TARGET_FREQUENCY.  An array of size N and type
     * double is passed into this method and tested to see whether or
     * not the desired frequency is present in the N samples.
     *
     * @param block   An array of type double containing the N samples.
     * @return        Boolean indicating whether or not the frequency
     *                was present.
     */
    public boolean analyze(double [] block) {
        double mag, magless, magmore;

	// Check to make sure that the size of 'block' is exactly N.
	// The UltrasoundDetector class is designed to process exactly N
	// samples at a time.
	if (block.length != N) {
	    System.out.println("UltrasoundDetector: Block size is not " +
			       "equal to N where N is " + N);
	    System.exit(1);
	}

	// Process each of the N samples using the Goertzel object.
	for (int i = 0; i < N; i++) {
	    g.processSample(block[i]);
	    gless.processSample(block[i]);
	    gmore.processSample(block[i]);
	}

	// N samples have been processed.  The Goertzel algorithm is now
	// ready to get the magnitude, which will enable a decision as
	// to the presence of the desired frequency.  The method called
	// here employs the optimized Goertzel algorithm without phase
	// information.
	mag = java.lang.Math.sqrt(g.getMagnitudeSquared());
	magless = java.lang.Math.sqrt(gless.getMagnitudeSquared());
	magmore = java.lang.Math.sqrt(gmore.getMagnitudeSquared());

	if(debug)
	    System.out.println("Magnitude: " + (int)mag + "\t" +
			       (int)magless + "\t" + (int)magmore);
		
	// Finished processing N samples.  Reset the Goertzel algorithm.
	g.resetGoertzel();
	gless.resetGoertzel();
	gmore.resetGoertzel();
    
	// If the calculated magnitude is high enough relative to the
	// magnitudes returned by the other two frequencies, return 'true'
	// indicating the desired frequency was present in this block of
	// N samples.
	if ( (mag > ((magless + magmore)/2)*2) )
	    return true;
	else return false;
    }
}