// sonar.c - Simple program to exercise the Ultrasound Range Finder board
// Version 1.0, Jerry Davis, 08/08/2015
//
// This program is written for the Raspberry Pi and uses the wiringPi
// library written by Gordon Henderson which can be found at the link
// below:
//
// http://www.wiringpi.com
//
// I've successfully compiled this program on Raspbian 1.4.1
// Feel free to update and improve at will.

#include <stdio.h>
#include <wiringPi.h>
#include <time.h>
#include <signal.h>

#define TE_PORT 0  // The wiringPi interface for the Range Finder TE pin
#define CD_PORT 1  // The wiringPi interface for the Range Finder CD pin
#define CARRIER_PULSE_WIDTH 1000  // Carrier pulse width in microseconds
#define SONAR_TIMEOUT 2           // Receiver timout in seconds
#define TX_DEBOUNCE 100000        // Transmit debounce time in nanoseconds
#define RX_DEBOUNCE 500000        // Reflection debounce time in nanoseconds
#define REPEAT_DELAY 1000         // Ranging cycle repeat time in milliseconds

int exit_flag = 0;  // Global for SIGINT handling


void exit_handler ()
{
   // Catch SIGINT and flag the main loop to exit

   exit_flag = 1;
}


int main (void)
{
   struct timespec sonar_start; // Storage for the start time of the sonar cycle
   struct timespec sonar_end;   // Storage for the first carrier detect pulse
   int debounce, timeout;       // Debounce and timeout flags
   long int cycle_time, debounce_time;  // Temporary storage for debounce and timeout
   float distance;                      // Storage for the calculated distance

   printf ("Sonar v1.0\n\n");

   // Set up signal handler

   if (signal (SIGINT, exit_handler) == SIG_ERR)
   {
      printf ("Error setting up SIGINT handler.\n");
      return (-1);
   }

   wiringPiSetup ();  // Initialize the wiringPi library

   pinMode (TE_PORT, OUTPUT);  // Set up the Transmit Enable (TE) pin
   pinMode (CD_PORT, INPUT);   // Set up the Carrier Detect (CD) pin

   // Execute the following until CTRL-C

   while (!exit_flag)
   {
      clock_gettime (CLOCK_MONOTONIC, &sonar_start);  // Get the start time for the sonar cycle

      digitalWrite (TE_PORT, HIGH);             // Put TE port active high
      delayMicroseconds (CARRIER_PULSE_WIDTH);  // Generate 40Khz carrier for 1ms
      digitalWrite (TE_PORT, LOW);              // Put TE port low

      debounce = 0;  // Initialize debounce and timeout flags
      timeout = 0;

      // The proximity of the receiver to the transmitter causes the CD pulse to go low
      // while transmitting.  The piezo crystal in transmitter rings slightly after the
      // transmitter is shut off.  The debounce routine below waits for the CD pin to
      // go high and stay high for a defined number of microseconds indicating that the
      // ultrasound wavefront has passed the receiver.

      while (!timeout && !debounce)
      {
         clock_gettime (CLOCK_MONOTONIC, &sonar_end);  // Get current time

         if (!digitalRead (CD_PORT))
         {
            // If the CD pin stays low longer than the timeout value, signal a timeout

            if ((sonar_end.tv_sec - sonar_start.tv_sec) > SONAR_TIMEOUT)
               ++timeout;
            else
               debounce_time = sonar_end.tv_nsec;  // Zero elapsed time counter
         }
         else
         {

            // Signal debounce complete if current time greater than debounce limit

            if ((sonar_end.tv_nsec - debounce_time) > TX_DEBOUNCE)
               ++debounce;
         }
      }

      // If no transmit timeout, wait for CD pin to go active low signaling 
      // receipt of ultrasound reflection.  Apply a receive pulse width limit
      // before signaling acquisition of a reflection pulse.

      debounce = 0;  // Initialize debounce and timeout flags

      while (!timeout && !debounce)
      {
         clock_gettime (CLOCK_MONOTONIC, &sonar_end);

         if (digitalRead (CD_PORT))
         {
            // If the CD pin stays high longer than the timeout value, signal a timeout

            if ((sonar_end.tv_sec - sonar_start.tv_sec) > SONAR_TIMEOUT)
               ++timeout;
            else
               debounce_time = sonar_end.tv_nsec;  // Zero elapsed time counter
         }
         else
         {
            // Signal debounce complete if current time greater than debounce limit

            if ((sonar_end.tv_nsec - debounce_time) > RX_DEBOUNCE)
               ++debounce;
         }
      }

      // Convert elapsed time to distance using propagation if sound in air
   
      if (timeout)
         printf ("Sonar Timeout!\n");
      else
      {
         // Adjust for nanosecond counter rollover and then calculate delta time

         if (sonar_end.tv_nsec < sonar_start.tv_nsec)
            cycle_time = ((999999999 - sonar_start.tv_nsec) + sonar_end.tv_nsec) - RX_DEBOUNCE;
         else 
            cycle_time = (sonar_end.tv_nsec - sonar_start.tv_nsec) - RX_DEBOUNCE;

         // Convert time to milliseconds and multiply by propagation velocity 34.320 cm/ms
         // Then divide by two to find the one-way distance to the target

         distance = ((cycle_time * .000001) * 34.320) / 2;

         // Print distance in cm and print converted to feet

         printf ("Target found at %.2f cm (%.2f ft)\n", distance, distance / 30.48);
      }

      // Wait one second before repeating the sonar cycle

      delay (REPEAT_DELAY);
   }

   // Exit via SIGINT

   printf ("\nExiting sonar.\n");
   pinMode (TE_PORT, INPUT);  // Change TE_PORT back to an input 
   return 0;
}