#! /usr/bin/env python

from pygame import *
import pygame
import libardrone
import threading

from time import sleep
import Queue
import signal
import sys

class DroneController(threading.Thread):
	"""Control class (to control the drone)"""

	is_flying = False
	speed = 0.1
	event_queue = Queue.Queue()

	previous_cmds = []

	prev_cmd_len = 50

	def __init__(self, nom = ''):
		threading.Thread.__init__(self)
		self._stopevent = threading.Event( )

	def stop(self):
		self._stopevent.set( )

	def run(self):
		"""We call pygame (to use controler)"""
		pygame.init()
		clock = pygame.time.Clock()
		screen = pygame.display.set_mode((640, 400))

		"""Launch drone class"""
		self.drone = libardrone.ARDrone(True)

		print "running"

		move_forward = 0
		move_backward = 0
		move_left = 0
		move_right = 0
		turn = 0

		running = True
		while running:
			try:
				event, params = self.event_queue.get(True, 0.1)
				event(*params)
			except Queue.Empty:
				self.drone.hover()
				sleep(0.001)

			clock.tick(1000)

		print "Shutting down...",
		drone.reset()
		drone.halt()
		print "Ok."
		quit()

	def add_move(self, left=0, right=0, forward=0, back=0, up=0, down=0, t_left=0, t_right=0):

		if len(previous_cmds) == self.prev_cmd_len:
			previous_cmds.pop(0)
		previous_cmds.append({'left':left,'right':right,'forward':forward,'back':back,'up':up,'down':down,'t_left':t_left,'t_right':t_right})

		lr = 0
		lr_cnt = 0
		fb = 0
		fb_cnt = 0
		vv = 0
		vv_cnt = 0
		va = 0
		va_cnt = 0
		for cmd in previous_cmds:
			if cmd.right or cmd.left:
				lr = cmd.right - cmd.left
				lr_cnt += 1
			if cmd.back or cmd.forward:
				fb = cmd.back - cmd.forward
				fb_cnt += 1
			if cmd.up or cmd.down:
				vv = cmd.up - cmd.down
				vv_cnt += 1
			if cmd.t_right or cmd.t_left:
				va = cmd.t_right - cmd.t_left
				va_cnt += 1

		lr = lr / lr_cnt
		fb = fb / fb_cnt
		vv = vv / vv_cnt
		va = va / va_cnt

		self.event_queue.put((self.drone.move_4d, (lr,fb,vv,va)))


	def do_command(self, command_string):
		"""
		makes the drone do something
		command_string is the string to execute
		such as turn_left, move_forward, possibly followed by a nubmer 0..1
		"""
		if any(s in command_string for s in ['kill','reset']):
			self.event_queue.put((self.drone.reset,()))
			self.is_flying = False
		elif any(s in command_string for s in ['halt']):
			self.event_queue.put((self.drone.halt,()))
			self.is_flying = False
		elif any(s in command_string for s in ['takeoff','tkoff']) and not self.is_flying:
			#takeoff
			self.event_queue.put((self.drone.takeoff, ()))
			self.is_flying = True
		elif any(s in command_string for s in ['land','lnd']):
			#land
			self.event_queue.put((self.drone.land, ()))
			self.is_flying = False

		if not self.is_flying:
			#cant do stuff when not flying
			return False

		if len(command_string.split(' ')) == 4 and 'lr' in command_string and 'fb' in command_string:
			splits = command_string.split(' ')
			lr = 0
			fb = 0
			for num, item in enumerate(splits):
				if item == 'lr':
					lr = splits[num + 1]
				elif item == 'fb':
					fb = splits[num + 1]
			self.add_move(forward = fb, left = lr)
			return True
		if len(command_string.split(' ')) > 3:
			print "Too many words in the command string!"
			return False

		number = self.speed
		if len(command_string.split(' ')) > 1:
			try:
				number = float(command_string.split(' ')[1])
			except Exception:
				print "You really fucked up the command, {} is not a number".format(command_string.split(' ')[1])
				return False


		if any(s in command_string for s in ['forward','fwrd']):
			#move forward
			self.add_move(forward = number)
		elif any(s in command_string for s in ['backward','bckwd','back']):
			#move back
			self.add_move(back = number)
		elif 'turn' in command_string:
			#turn commands
			if any(s in command_string for s in ['left']):
				#move left
				self.add_move(t_left = number)
			elif any(s in command_string for s in ['right']):
				#move right
				self.add_move(t_right = number)
			else:
				print "Invalid Turn command"
				return False
		elif any(s in command_string for s in ['left']):
			#move left
			self.add_move(left = number)
		elif any(s in command_string for s in ['right']):
			#move right
			self.add_move(right = number)
		elif any(s in command_string for s in ['up']):
			#up
			self.add_move(up = number)
		elif any(s in command_string for s in ['down','dn']):
			#down
			self.add_move(down = number)
		elif any(s in command_string for s in ['speed']):
			#speed
			self.speed = number
		else:
			print "Bad drone command {}".format(command_string)
			return False
		return True

@@ -21,43 +21,8 @@
 on%0A%0A
-from pygame import *%0Aimport pygame%0A
 impo
@@ -472,144 +472,8 @@
 f):%0A
-%09%09%22%22%22We call pygame (to use controler)%22%22%22%0A%09%09pygame.init()%0A%09%09clock = pygame.time.Clock()%0A%09%09screen = pygame.display.set_mode((640, 400))%0A%0A
 %09%09%22%22

Remove pygame from drone control