/***************************************************************************
*                                                                          *
*         Optech Laser Range Finder (G-150) Communications Program         *
*                                                                          *
*    Establishes serial communications with the Laser Range Finder (LRF).  *
* Continuously obtains readings from the LRF, looks up and sends the       *
* appropriate samples representing a cosine wave to the audio port.        *
* A reading containing more than 12.5% "drop-outs" (as indicated by a ')'),* 
* is treated as a valid reading.                                           *
* When an "out-of-range reading is encountered (as indicated by '))))'),   *
* the tone corresponding to the last valid reading is output.              *
*                                                                          *
*    The presence of a Depth Discontinuity (a change in distance greater   *
* than a pre-defined amount between the current and last valid (reading),  *
* causes the output of a different signal (non-tone).                      *
*                                                                          *
* The samples for each cos wave are stored in a table.  When a reading is  *
* obtained, it is mapped to the corresponding samples in the table         *
* and output. There is a slight delay (~8 sec.) between the time of program*
* execution and the time the first reading is obtained, to allow for the   *
* initialization of the table.                                             *
*                                                                          *
*    Prior to program execution, the LRF must be turned ON and in the      *
* "READY" mode.  Should the LRF be turned OFF, the program must be         *
* re-executed.  "CNTRL-C" to stop program execution.                       *
*                                                                          *
* Written By:   Bill Kapralos                                              *
* Date:         Monday, June 9 1997                                        *
*                                                                          *
* York University                                                          *
* Department of Computer Science                                           *
*                                                                          *
*                                                                          *
****************************************************************************/

#include "lrf_functions.h"
#include "lrf_audio.h"

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

#define DEPTH_DISCONTINUITY 1.5     /* Depth discontinuity mark (DC).      */
#define DEPTH_BUFFER_SIZE   100     /* Number of samples for depth signal. */

/* 1 varies the amplitude with distance, 0 does not vary the amplitude. */
#define VARY_MAGNITUDE      1      

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

main(){

   double crntDistance = 0.0, prevDistance = 0.0, distanceDif, tableDist,
          *prev, avg1 = 0, avg2 = 0, avgTotal;
   
   unsigned char *rangeReading;  /* Input data from LRF. */
   int index, tableIndex = 0, frequency, prevIndex = 0; 
   short *depthBuffer, *lookUpTable[NUM_OF_WAVES], *signalBuffer;

/**************************************************************************
   Perform all initializations and memory allocations required
****************************************************************************/

   if(!serialPortInit(TTY_DEVICE))
      error("Could not open serial port.");

   if(!audioInit(AUDIO_PORT))
      error("Could not open audio port.");
 
   if(!(rangeReading = (unsigned char *)calloc(LR_READINGLEN,
                        sizeof(unsigned char))))
      error("Could not allocate space for 'rangeReading' in main()");

   signalBuffer = (short *) calloc(NUM_OF_SAMPLES, sizeof(short *));
   prev = (double *) calloc(6, sizeof(double *));

   /* Constant signal - create it once. */         
   depthBuffer = makeDepthSignal(DEPTH_BUFFER_SIZE);

   /*tableInit(lookUpTable, VARY_MAGNITUDE);*/  /* Initialize cos look-up table. */
   
   lrfSetUp();  /* Send all initialization/operation commands to lrf */

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

   /* Obtain reading from LRF, determine whether this reading represents a 
      depth discontinuity (if so, output the corresponding signal), if not, 
      calculate corresponding index to look-up table and output the samples
      representing the appropriate cosine wave.
      Output the last valid tone when "out-of-range reading is encountered. */      
   for(;;){
   
       if(getReading(rangeReading)){   

	 /* Must have a valid distance reading to proceed. */
	 if((crntDistance = processReading(rangeReading)) > 0){
      
            prev[prevIndex] = crntDistance;
            prevIndex++;
            prevIndex = prevIndex % 6;
            
            avg1 = (prev[0] + prev[1] + prev[2]) / 3.0;            
            avg2 = (prev[3] + prev[4] + prev[5]) / 3.0;
     
            /* Determine the difference between current and previous readings.*/
            if(avg1 > avg2)
               distanceDif = avg1 - avg2;
            else
               distanceDif = avg2 - avg1;
            
            if(distanceDif < 0.07)
               distanceDif = 0.07;
                    
            frequency = FREQ_CONST * exp(-(distanceDif) * ADJUSTMENT); 
            
            frequency = 165 * ((1/pow(distanceDif, 1.2)) + 1); 
            signalBuffer = makeWave(frequency, 1);
            
            printf("%.2f\t%d\n", distanceDif, frequency);            

            sendAudio(signalBuffer, NUM_OF_SAMPLES);           
               

       
         }   
         else
         
            /* Invalid (out-of-range reading), send previous tone. */
            sendAudio(signalBuffer, NUM_OF_SAMPLES);  
      }         
         
   }
   
   close(serialFd); 
   close(audioFd);
     
   free(depthBuffer);  
   free(rangeReading);
   
   for(index = 0; index < NUM_OF_WAVES - tableOffset; index++)                                                                
       free(lookUpTable[index]);
}

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