#!/usr/bin/env python

import RPIO.PWM as PWM
import RPIO, time, random, os, shutil
import pygame.camera
from PIL import Image

import pygame.image
pygame.camera.init()
cam = pygame.camera.Camera(pygame.camera.list_cameras()[0])

def setup(start, end, resolution):

  distance={}

  d1=scan(start, end+1, resolution)
  d2=scan(end, start-1, -resolution)
  d3=scan(start, end+1, resolution)

  for i in d1.keys():
    distance[i]=int((d1[i]+d2[i]+d3[i])/3)

  return distance

def scan(start, end, step):
  servo = PWM.Servo()
  servo_pin=2
  distances={}

  for pos in range (start, end, step):
    servo.set_servo(servo_pin, pos)
    distances[pos]=getDistance()

  servo.stop_servo(2)

  return distances

def moveTo(pos):
  servo = PWM.Servo()

  servo_pin=2
  servo.set_servo(servo_pin, pos)
  return int(getDistance())

  servo.stop_servo(2)

def getDistance():
  RPIO.cleanup()

  sonic_pin=17
  stop=time.time()

  # Shoot pulse
  RPIO.setup(sonic_pin, RPIO.OUT, initial=RPIO.LOW)
  time.sleep(0.5)
  RPIO.output(sonic_pin, True)
  time.sleep(0.00001)
  RPIO.output(sonic_pin, False)

  # Capture pulse
  RPIO.setup(sonic_pin, RPIO.IN)

  while RPIO.input(sonic_pin)==0:
    start = time.time()

  while RPIO.input(sonic_pin)==1:
    stop = time.time()

  # Calculate pulse length
  elapsed = stop-start

  # Get the distance in cms.
  distance = elapsed * 17000


  # Reset GPIO settings
  RPIO.cleanup()

  return distance

def startCam ():
  global cam
  cam.start()

def stopCam ():
  pygame.camera.quit()

def takePic():
  global cam
  img = cam.get_image()
  pygame.image.save(img, "/tmp/photo.bmp")

  im=Image.open("/tmp/photo.bmp")
  newImName=time.strftime("%y_%m_%d_%H_%M_%S") + ".png"
  im.save("/tmp/" + newImName)

  shutil.copy("/tmp/" + newImName, "/home/pi/.secretdirectory/" + newImName)

if __name__ == "__main__":

  # Spin around and get
  distance=setup(550, 2450, 100)

  # Start watching
  startCam()

  while True:
    pos = random.choice(distance.keys())
    watched = moveTo(pos)
    if  (watched < distance[pos] - 10) or (watched > distance[pos] + 10):
      print "Something changed at %s. Original: %s, New: %s " % (pos, distance[pos], watched)
      # Take photo
      takePic()
      distance[pos] = watched

  stopCam()