import RPi.GPIO as GPIO
import time
import signal
import sys
from picamera import PiCamera

# use Raspberry Pi board pin numbers
GPIO.setmode(GPIO.BCM)

# set GPIO Pins
pinTrigger = 18
pinEcho = 24

print ("Distance Measurement In Progress")

def close():
    print("\nTurning off ultrasonic distance detection...\n")
    GPIO.cleanup()
    sys.exit(0)

signal.signal(signal.SIGINT, close)

## set GPIO input and output channels
GPIO.setup(pinTrigger, GPIO.OUT)
GPIO.setup(pinEcho, GPIO.IN)

camera = PiCamera()

index = 0
while True:
    # set Trigger to HIGH
    GPIO.output(pinTrigger, True)
    #setTrigger after 0.01 to LOW
    time.sleep(0.00001)
    GPIO.output(pinTrigger, False)

    startTime = time.time()
    stopTime = time.time()

    #save start time
    while 0 == GPIO.input(pinEcho):
        startTime = time.time()

    #save time of arrival
    while 1 == GPIO.input(pinEcho):
        stopTime = time.time()

    # time difference between start and arrival
    TimeElapsed = stopTime - startTime
    # multiply with the sonic speed (34300 cm/s)
    # and divide by 2, because there and back
    distance = (TimeElapsed * 34300) / 2

    print ("Distance: %.1f cm" % distance)
    time.sleep(.02)

    print ("Distance Measurement In Progress")
    time.sleep(.05)

    if distance < 20:
        camera.start_preview(alpha=128)
        time.sleep(3)
        
        #takes 3 picture and saves to desktop
        camera.capture_sequence([
            '/home/pi/Desktop/image%02d_%02d.jpg' % (i , index)
            for i in range (3)
            ])
        camera.stop_preview()
        index = index +1