import math
import threading
import time

import cv2
from djitellopy import tello as drone

from .cytron_motor import Cytron_Motor



class Drone:
	is_flying = False

	def __init__(self, mock: bool):
		self.sdk = drone.Tello()
		self.drone_locator = DroneLocator()
		self.drone_locator.watch()
		self.mock = mock
		self.video_stream = self.start_stream()
		self.robot = Cytron_Motor(9600)

	def start(self):
		if self.mock:
			print("Starting mocked drone")
			return

		# self.sdk.connect()
		# self.sdk.streamon()

	def get_frame(self):
		return self.video_stream()

	def start_stream(self):
		if not self.mock:
			return lambda: self.sdk.get_frame_read().frame

		cap = cv2.VideoCapture(0)
		return lambda: cap.read()[1]

	def get_battery(self):
		if not self.mock:
			return self.sdk.get_battery()
		return 100

	def get_wifi_signal(self):
		if not self.mock:
			signal_noise_ratio = self.sdk.query_wifi_signal_noise_ratio()
			print(signal_noise_ratio)
			return signal_noise_ratio


	def takeoff(self):
		self.is_flying = True
		if not self.mock:
			self.sdk.takeoff()

	def land(self):
		self.is_flying = False
		if not self.mock and self.sdk.is_flying:
			self.sdk.land()

	def map_drone2robot(self, right_left, forward_back, up_down, rotation):
		speed = (-1) * up_down
		turn =  (-1) * rotation
		return speed, turn

	def update(self, right_left, forward_back, up_down, rotation):
		self.drone_locator.update_axis(right_left, forward_back, up_down, rotation)
		speed, turn = self.map_drone2robot(right_left, forward_back, up_down, rotation)
	
		if turn > 12: self.robot.turnLeft(turn +20) 
		elif turn < -12: self.robot.turnRight((-1) * turn +20)
		else: self.robot.setspeed(speed)

		if speed + turn ==0: self.robot.stop()
		#self.robot.steerMove(speed, turn)
		if not self.mock:
			self.sdk.send_rc_control(right_left, forward_back, up_down, rotation)
			return
		print("speed " + str(speed) , "turn " + str(turn) )


	def get_position_history(self):
		return self.drone_locator.points

	def clean_position_history(self):
		self.drone_locator.points = [(0, 0)]







# class Drone:
# 	is_flying = False
#
# 	def __init__(self, mock: bool):
# 		self.sdk = drone.Tello()
# 		self.drone_locator = DroneLocator()
# 		self.drone_locator.watch()
# 		self.mock = mock
# 		self.video_stream = self.start_stream()
#
# 	def start(self):
# 		if self.mock:
# 			print("Starting mocked drone")
# 			return
#
# 		self.sdk.connect()
# 		self.sdk.streamon()
#
# 	def get_frame(self):
# 		return self.video_stream()
#
# 	def start_stream(self):
# 		if not self.mock:
# 			return lambda: self.sdk.get_frame_read().frame
#
# 		cap = cv2.VideoCapture(0)
# 		return lambda: cap.read()[1]
#
# 	def get_battery(self):
# 		if not self.mock:
# 			return self.sdk.get_battery()
# 		return 100
#
# 	def get_wifi_signal(self):
# 		if not self.mock:
# 			signal_noise_ratio = self.sdk.query_wifi_signal_noise_ratio()
# 			print(signal_noise_ratio)
# 			return signal_noise_ratio
#
#
# 	def takeoff(self):
# 		self.is_flying = True
# 		if not self.mock:
# 			self.sdk.takeoff()
#
#
# 	def land(self):
# 		self.is_flying = False
# 		if not self.mock and self.sdk.is_flying:
# 			self.sdk.land()
#
# 	def update(self, right_left, forward_back, up_down, rotation):
# 		self.drone_locator.update_axis(right_left, forward_back, up_down, rotation)
# 		if not self.mock:
# 			self.sdk.send_rc_control(right_left, forward_back, up_down, rotation)
# 			return
# 		print(right_left, forward_back, up_down, rotation)
#
#
# 	def get_position_history(self):
# 		return self.drone_locator.points
#
# 	def clean_position_history(self):
# 		self.drone_locator.points = [(0, 0)]


class DroneLocator:
	interval = 0.25  # interval to draw the map
	move_speed = 25 * interval
	rotation_speed = 72 * interval
	x, y = 350, 350
	accumulated_angle = 0
	points = [(0, 0), (0, 0)]
	distance = 0
	angle_calc_disabled = False
	axis_speed = {"rotation": 0, "right-left": 0, "forward-back": 0, "up-down": 0}

	def watch(self):
		"""Watch for drone moves"""

		def draw_map(locator: DroneLocator, gambi):
			while True:
				locator.update()
				time.sleep(DroneLocator.interval)

		td = threading.Thread(target=draw_map, args=(self, ""))
		td.start()

	def update_axis(self, right_left, forward_back, up_down, rotation):
		self.axis_speed = {"rotation": rotation, "right-left": right_left, "forward-back": forward_back, "up-down": up_down}

	def update(self):
		self.calculate_current_position()
		if self.points[-1][0] != self.x or self.points[-1][1] != self.y:
			self.points.append((self.x, self.y))

	def calculate_current_position(self):
		distance = 0
		direction = 0

		# left
		if self.axis_speed["right-left"] < 0:
			distance = self.move_speed
			direction = -90

		# right
		if self.axis_speed["right-left"] > 0:
			distance = self.move_speed
			direction = 90

		# Forward
		if self.axis_speed["forward-back"] > 0:
			distance = self.move_speed
			direction = 0

		# Backward
		if self.axis_speed["forward-back"] < 0:
			distance = self.move_speed
			direction = 180

		if not self.angle_calc_disabled:
			# Rotation left
			if self.axis_speed["rotation"] < 0:
				self.accumulated_angle -= self.rotation_speed

			# Rotation right
			if self.axis_speed["rotation"] > 0:
				self.accumulated_angle += self.rotation_speed

		angle = direction + self.accumulated_angle

		self.x += int(distance * math.cos(math.radians(angle)))
		self.y += int(distance * math.sin(math.radians(angle)))

