#!/usr/bin/env python

import roslib
import rospy
import numpy as np
import cv2
import datetime
import sys, termios, tty, os
import re

from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from nav_msgs.msg import Odometry #position (souscrire)
from geometry_msgs.msg import Twist #position (publier)
from std_msgs.msg import Empty #takeoff et land
from time import sleep
from math import *
import direction

#Tempo (autre facon)
#rate = rospy.Rate(10)
#for i in range(10):
#rate.sleep()

global v, d

bridge = CvBridge()

def image_callback(msg):
	global orient
	try:
        # Convert your ROS Image message to OpenCV2
		cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
	except CvBridgeError as e:
		print(e)
	else:
        # Save your OpenCV2 image as a jpeg
		date = datetime.datetime.now()
		ms = date.microsecond
		msec = int(ms/10000)
		if msec%4==0 :
			orient = direction.Direction(cv2_img)
			#print("Received an image!")
	 		#cv2.imwrite(''+str(date)+'.jpeg', cv2_img)
	
		

def takeoff():
	pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=1,latch=True)	
	pub.publish(Empty())
	sleep(6.)
		
def land():
	pub = rospy.Publisher('/bebop/land', Empty, queue_size=1, latch=True)	
	pub.publish(Empty())
	sleep(4.)

def fleche_gauche(): 
	rate = rospy.Rate(10)
	for i in range(90): #temps necessaire pour realiser un angle droit
		pubgauche = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
		gauche=Twist()
		gauche.angular.x=0
		gauche.angular.y=0
		gauche.angular.z=0.1 #tour a gauche
		gauche.linear.x=0
		gauche.linear.y=0
		gauche.linear.z=0
		pubgauche.publish(gauche)
		rate.sleep()
	if v != 0 : #choix vitesse
		for i in range(20): 
			pubgauche2 = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			gauche2=Twist()
			gauche2.angular.x=0
			gauche2.angular.y=0
			gauche2.angular.z=0
			gauche2.linear.x= v/9.0 #avance
			gauche2.linear.y=0
			gauche2.linear.z=0
			pubgauche2.publish(gauche2)
			rate.sleep()
	else : #choix distance
		for i in range(30): 
			pubgauche2 = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			gauche2=Twist()
			gauche2.angular.x=0
			gauche2.angular.y=0
			gauche2.angular.z=0
			gauche2.linear.x= d/13.0 #avance
			gauche2.linear.y=0
			gauche2.linear.z=0
			pubgauche2.publish(gauche2)
			rate.sleep()

def fleche_droite():
	rate = rospy.Rate(10)
	for i in range(90): #90 : faire un bel angle droit
		pubdroite = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
		droite=Twist()
		droite.angular.x=0
		droite.angular.y=0
		droite.angular.z=-0.1 #tour a droite
		droite.linear.x=0
		droite.linear.y=0
		droite.linear.z=0
		pubdroite.publish(droite)
		rate.sleep()
	if v != 0 : #vitesse
		for i in range(20): 
			pubdroite2 = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			droite2=Twist()
			droite2.angular.x=0
			droite2.angular.y=0
			droite2.angular.z=0
			droite2.linear.x= v/9.0 #avance
			droite2.linear.y=0
			droite2.linear.z=0
			pubdroite2.publish(droite2)
			rate.sleep()
	else : #distance
		for i in range(30): 
			pubdroite2 = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			droite2=Twist()
			droite2.angular.x=0
			droite2.angular.y=0
			droite2.angular.z=0
			droite2.linear.x= d/13.0 #avance
			droite2.linear.y=0
			droite2.linear.z=0
			pubdroite2.publish(droite2)
			rate.sleep()

def fleche_haut():
	if v != 0 : 
		for i in range(20): 
			pubhaut = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			haut=Twist()
			haut.angular.x=0
			haut.angular.y=0
			haut.angular.z=0
			haut.linear.x=0
			haut.linear.y=0
			haut.linear.z= v/9.0 #monte
			pubhaut.publish(haut)
			rate.sleep()
	else :
		for i in range(30): 
			pubhaut = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			haut=Twist()
			haut.angular.x=0
			haut.angular.y=0
			haut.angular.z=0
			haut.linear.x=0
			haut.linear.y=0
			haut.linear.z= d/9.0 #monte
			pubhaut.publish(haut)
			rate.sleep()

def fleche_bas():
	if v != 0 :
		for i in range (20): 
			pubbas = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			bas=Twist()
			bas.angular.x=0
			bas.angular.y=0
			bas.angular.z=0
			bas.linear.x=0
			bas.linear.y=0
			bas.linear.z= -v/9.0 #descend
			pubbas.publish(bas)
	else : 
		for i in range (30): 
			pubbas = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
			bas=Twist()
			bas.angular.x=0
			bas.angular.y=0
			bas.angular.z=0
			bas.linear.x=0
			bas.linear.y=0
			bas.linear.z= -d/9.0 #descend
			pubbas.publish(bas)

def stop():
	pubstop = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
	stop =Twist()
	stop.angular.x=0
	stop.angular.y=0
	stop.angular.z=0
	stop.linear.x=0
	stop.linear.y=0
	stop.linear.z=0
	pubstop.publish(stop)
	

def getch():
	fd = sys.stdin.fileno()
	old_settings = termios.tcgetattr(fd)
	try :
		tty.setraw(sys.stdin.fileno())
		ch = sys.stdin.read(1)
	finally :
		termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
	return ch

def odometry_callback(msg): #recup valeurs positions
	pos_x=msg.twist.twist.linear.x
	pos_y=msg.twist.twist.linear.y
	pos_z=msg.twist.twist.linear.z
	rospy.loginfo('La position lin est : [%f, %f, %f]', pos_x, pos_y, pos_z)
	ang_x=msg.twist.twist.angular.x
	ang_y=msg.twist.twist.angular.y
	ang_z=msg.twist.twist.angular.z
	rospy.loginfo('La position ang est : [%f, %f, %f]', ang_x, ang_y, ang_z)

def listener():
	rospy.init_node('test', anonymous=True)
	rospy.Subscriber('/bebop/odom', Odometry , odometry_callback)
	rospy.spin()


if __name__ == '__main__':
	global orient
	rospy.init_node('takeoffland', anonymous=True)
	rate = rospy.Rate(10)
	z = True
	while z :
		#orient = input("Donnez une direction (1 : droite, 2 : gauche, 3 : haut, 4 : bas) \n")
		b = input("Souhaitez vous controler le drone en vitesse ou en distance ? (vitesse : 1, distance : 2) \n")
		if (b == 1) :
			v = input("A quelle vitesse souhaitez vous voler ? (en m/s, entier entre 0 et 9) \n")
			print 'Vitesse :',v,'m/s'
		elif (b == 2) :
			d = input("Quelle distance souhaitez vous parcourir ? (en m, entier entre 0 et 9) \n")
			v = 0
			print 'Distance :',d,'m'
		else :
			print("Choix non valide")
			print("Fin")
			stop()
			z = False
		print ("En vol")	
		takeoff()
		a = True
		while a :
			sub = rospy.Subscriber('/bebop/image_raw', Image, image_callback)
			sleep(1.)
			#print 'La valeur est :',orient
			if (orient == 1) :
				print ("Va a droite")
				fleche_droite()
				sub.unregister()
				a = False
			elif (orient == 2) : 
				print ("Va a gauche")
				fleche_gauche()
				sub.unregister()
				a = False
			elif (orient == 3) :
				print ("Monte")
				fleche_haut()
				sub.unregister()
				a = False
			elif (orient == 4) : 
				print ("Descend")
				fleche_bas()
				sub.unregister()
				a = False
		print ("Si vous voulez arreter, appuyez sur e. Pour continuer, appuyez sur n'importe quelle autre touche")
		but = getch()
		if (but == "e") :
			print ("Fin")
			stop()
			z= False
	sleep(1.)
	land()
	print ("Atterrissage")
	sleep(3.)
