from __future__ import print_function
from pixy import *
from ctypes import *
import RPi.GPIO as GPIO
import time

from dronekit import connect, VehicleMode, LocationGlobalRelative
import os
import os.path

status = 0

class Blocks (Structure):
  _fields_ = [ ("type", c_uint),
               ("signature", c_uint),
               ("x", c_uint),
               ("y", c_uint),
               ("width", c_uint),
               ("height", c_uint),
               ("angle", c_uint) ]

#def search():
      #vehicle.mode = VehicleMode("Auto_Loiter")
      #vehicle.armed = True

#def stop():
      #vehicle.mode = VehicleMode("ALTCTL")

      #vehicle.armed = True

connection_string = '/dev/ttyACM0'

if not connection_string:
    import dronekit_sitl
    sitl = dronekit_sitl.start_default()
    connection_string = sitl.connection_string()


# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)

GPIO.setmode(GPIO.BOARD)
GPIO.setup(40, GPIO.OUT)

pixy_init()

blocks = BlockArray(100)
frame  = 0

GPIO.output(40, GPIO.HIGH)#Schalte Lampe ein
time.sleep(5)
GPIO.output(40, GPIO.LOW)#Schalte Lampe aus

# Wait for blocks #
while 1:

        if (os.path.exists("/tmp/Status") and status == 0):
              #search()
              status = 1
        elif (not os.path.exists("/tmp/Status") and status == 1 ) :
            #stop()
            status  = 0

	count = pixy_get_blocks(100, blocks)
	
        
	if count > 0: 
          #print ("Objekt gefunden")
          GPIO.output(40, GPIO.HIGH)#Schalte Lampe ein
          file = open('/home/pi/Drohne/Found','w+')
          try:
              os.remove('/tmp/Status')
          except:
              pass

        else:
          #print ("Objekt nicht gefunden")      
          GPIO.output(40, GPIO.LOW)#Schalte Lampe aus
          try:
            os.remove('/home/pi/Drohne/Found')
          except:
            KeyboardInterrupt
          time.sleep(1)