# !/usr/bin/python
#JMD February 2015
# Ultrasonic SFR04

import time
import pygame
import RPi.GPIO as GPIO
from RPIO import PWM
import threading

pygame.init()
size = [360,460]
screen = pygame.display.set_mode (size)
pygame.display.set_caption("Radar")

BLACK = (  0,  0,  0)
GREEN = (  0,255,  0)
GREEN_Y = (  120,255,  0)
WHITE = (255, 255, 255)
BLUE = (  0,    0, 255)

screen.fill (WHITE)

GPIO.setmode(GPIO.BCM)

GPIO_SWDOWN  = 23
GPIO_SWUP = 18
GPIO_SERVO =25
GPIO_TRIGGER = 22
GPIO_ECHO = 17

GPIO.setup(GPIO_SERVO,GPIO.OUT)         # Servo
GPIO.setup(GPIO_SWUP,GPIO.IN)           #Switch Up
GPIO.setup(GPIO_SWDOWN,GPIO.IN)         # Switch Down
GPIO.setup(GPIO_TRIGGER,GPIO.OUT)      	# Trigger
GPIO.setup(GPIO_ECHO,GPIO.IN)           # Echo

PWM.setup()
PWM.init_channel(0)

C= 80
C_OLD = C
I= 10
STOP =0
distance_old=0

done= False
clock = pygame.time.Clock()
pygame.draw.rect(screen, BLUE, [50, 0, 260, 440], 3)
pygame.draw.rect(screen, BLACK, [52, 0, 257, 438])

GPIO.output(GPIO_TRIGGER, False)
time.sleep(0.5)

class ServoThread(threading.Thread):
    def run(self) :
        global C
        global I
        global STOP
        while True :
                #Polling switch Down and Up
                if (GPIO.input(GPIO_SWUP) == False ):
                        I = I+1
                        time.sleep(0.1)
                        if (I >10):
                                I = 10
                if(GPIO.input(GPIO_SWDOWN) ==False):
                       I=I - 1
                       time.sleep(0.1)
                       if (I<1):
                                I = 1
                                
                #Pulse 20 ms
                for T in range (0, I):
                        PWM.add_channel_pulse(0, GPIO_SERVO, 0, C)
                        time.sleep(0.03)                                                          
                C=C+1
                if C > 181 :
                        C= 80
                        STOP = 1

myServo= ServoThread()
myServo.start()
        
while not done:
        clock.tick(10)
        for event in pygame.event.get(): # User did something
                if event.type == pygame.QUIT:
                        GPIO.cleanup()
                        pygame.quit()
                        done = True                      

        # Send 10us pulse to trigger
        GPIO.output(GPIO_TRIGGER, True)
        time.sleep(0.00001)
        GPIO.output(GPIO_TRIGGER, False)
        start = time.time()
        while GPIO.input(GPIO_ECHO)==0:
                start = time.time()

        while GPIO.input(GPIO_ECHO)==1:
                stop = time.time()

        # Calculate pulse length
        elapsed = stop-start

        # Distance pulse travelled in that time is time
        # multiplied by the speed of sound (cm/s)
        distance = elapsed * 34000

        # That was the distance there and back so halve the value
        distance = distance / 2

        print "Distance : %.1f" % distance , " Cm"

        # Draw on display
        pygame.draw.line(screen, GREEN, [180,0], [C_OLD*2-90,distance_old],1)
        pygame.draw.line(screen, GREEN_Y, [C_OLD*2-90,distance_old], [C*2-90,distance_old],3)
        C_OLD = C        
        pygame.draw.line(screen, GREEN_Y, [C*2-90,distance_old], [C*2-90,distance],3)
        distance_old = distance
        
        if (STOP == 1):
                pygame.draw.rect(screen, BLACK, [52, 0, 257, 438])
                STOP=0
        pygame.display.flip()