from __future__ import print_function
from __future__ import division

from threading import Thread
from math import pi, tan, asin, atan, radians
from time import sleep, time, strftime
from collections import deque

from .imagereaders import NaoImageReader
from .finders import BallFinder, GoalFinder, FieldFinder
from .movements import NaoMover
from naoqi import ALProxy


class Striker(object):

    def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv,
                 ball_min_radius, do_capture=False):

        # Maintenance
        self.run_id = strftime('%Y%m%d%H%M%S')
        self.is_over = False
        self.last_goal = 'right'
        self.doing_caputre = do_capture

        # Motion
        self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)

        # Sight
        self.upper_camera = NaoImageReader(
            nao_ip, port=nao_port, res=res, fps=10, cam_id=0,
        )
        self.lower_camera = NaoImageReader(
            nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
        )

        # POV
        if do_capture:
            self.upper_pov = NaoImageReader(
                nao_ip, port=nao_port, res=1, fps=10, cam_id=0,
                video_file='./cam0_' + self.run_id + '.avi'
            )

            self.lower_pov = NaoImageReader(
                nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
                video_file='./cam1_' + self.run_id + '.avi'
            )
            self.pov_thread = Thread(target=self._pov)
            self.pov_thread.start()

        # Recognition
        self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]),
                                      ball_min_radius)
        self.field_finder = FieldFinder(tuple(field_hsv[0]),
                                        tuple(field_hsv[1]))
        self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))

        # Talking
        self.player = ALProxy('ALAudioPlayer', bytes(nao_ip), nao_port)
        self.tts = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port)
        self.speach_queue = deque(maxlen=4)
        self.speach_history = []
        self.tts_thread = Thread(target=self._speaker)
        self.tts_thread.start()

        # Debugging
        self._timer_start = 0
        self._timer_stop = 0

    def close(self):
        self.is_over = True

        if self.tts_thread.isAlive():
            self.tts_thread.join()
        if self.doing_caputre and self.pov_thread.isAlive():
            self.pov_thread.join()
            self.upper_pov.close()
            self.lower_pov.close()

        self.upper_camera.close()
        self.lower_camera.close()
        self.mover.stop_moving()

    def _speaker(self):
        while not self.is_over:
            while self.speach_queue:
                text = self.speach_queue.pop()
                if text in ('hasta', 'tiger'):
                    file_id = self.player.loadFile(
                        '/home/nao/audio/' + text + '.mp3'
                    )
                    self.player.play(file_id)
                else:
                    self.tts.say(text)
            sleep(0.5)

    def _pov(self):
        while not self.is_over:
            try:
                self.upper_pov.get_frame()
                self.lower_pov.get_frame()
                sleep(0.1)
            except RuntimeError as e:
                print(e)
                continue

    def speak(self, text):
        if not self.speach_history or self.speach_history[-1] != text:
            self.speach_queue.appendleft(text)
            self.speach_history.append(text)

    def scan_rotation(self):
        """Intelligently rotate the robot to search for stuff."""
        self.mover.stop_moving()
        self.rotating = False
        yaw, pitch = self.mover.get_head_angles()

        # determine direction of head rotation
        sign = 1 if yaw >= 0 else -1

        # the robot starts to move arround his z-Axis in the direction where his
        # head is aligned when the head yaw angle has reached his maximum
        if yaw > pi/3:
            self.mover.set_head_angles(-pi / 8, pitch, 0.5)
            sleep(0.5)
        elif yaw < -pi/3:
            self.mover.move_to(0, 0, -pi / 4)
            self.mover.wait()
        # rotate head to the left, if head yaw angle is equally zero or larger
        # rotate head to the right, if head yaw angle is smaller than zero
        else:
            self.mover.change_head_angles(sign * pi / 8, 0, 0.5)
            sleep(0.3)

    def get_ball_angles_from_camera(self, cam, mask=True):
        """Detect the ball and return its angles in camera coordinates."""
        try:
            frame = cam.get_frame()
        except RuntimeError as e:  # Sometimes camera doesn't return an image
            print(e)
            return None

        if cam == self.lower_camera:
            mask = False

        if mask:
            field = self.field_finder.find(frame)
            frame = self.field_finder.mask_it(frame, field)
        ball = self.ball_finder.find(frame)

        if ball is None:
            return None

        (x, y), _ = ball
        x, y = cam.to_relative(x, y)
        x, y = cam.to_angles(x, y)
        return x, y

    def get_goal_angles_from_camera(self, cam, mask=True):
        try:
            frame = cam.get_frame()
        except RuntimeError as e:  # Sometimes camera doesn't return an image
            print(e)
            return None

        if mask:
            field = self.field_finder.find(frame)
            frame = self.field_finder.mask_it(frame, field, inverse=True)

        goal = self.goal_finder.find(frame)
        if goal is None:
            return None

        goal_l, goal_r = self.goal_finder.left_right_post(goal)
        goal_c = self.goal_finder.goal_center(goal)

        goal_l, _ = cam.to_relative(goal_l, 0)
        goal_l, _ = cam.to_angles(goal_l, 0)
        goal_r, _ = cam.to_relative(goal_r, 0)
        goal_r, _ = cam.to_angles(goal_r, 0)
        goal_c, _ = cam.to_relative(goal_c, 0)
        goal_c, _ = cam.to_angles(goal_c, 0)
        return goal_l, goal_r, goal_c

    def distance_to_ball(self):
        camera = 'upper'
        angles = self.get_ball_angles_from_camera(self.upper_camera)
        if angles == None:
            camera = 'lower'
            angles = self.get_ball_angles_from_camera(self.lower_camera)
            if angles == None:
                raise ValueError('No ball in sight')
        y_angle = angles[1]
        y_angle = pi/2 - y_angle - radians(15) - (radians(39)
                                                  if camera == 'lower'
                                                  else 0)
        print('Ball distance through', camera, 'camera')
        print('Angles', angles)
        return 0.5 * tan(y_angle)

    def walking_direction(self, lr, d, hypo):
        return (asin(0.40 / d) if hypo == 'bdist' else atan(0.2 / d)) * lr

    def ball_tracking(self, soll=0, tol=0.15):
        """Track the ball using the feed from top and bottom camera.

        Returns
        -------
        bool
            True if robot is nicely aligned to ball; else False.

        """

        ball_locked = False
        tried_step_back = False
        while not ball_locked:
            # visibility check
            for i in range(3):
                cams = [self.lower_camera, self.upper_camera]
                in_sight = False

                for cam in cams:
                    ball_angles = self.get_ball_angles_from_camera(cam)
                    if ball_angles is not None:
                        x, y = ball_angles
                        self._timer_start = time()
                        in_sight = True

                        break
                if in_sight:
                    break
            # stop visibility check

            if not in_sight:
                if not tried_step_back:
                    self.mover.move_to(-0.1, 0, 0)
                    self.mover.wait()
                    self.mover.stand_up()
                    tried_step_back = True
                else:
                    self.scan_rotation()
                continue

            ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
            print()

    def run_to_ball(self, d):
        self.mover.move_to(d, 0, 0)
        # self.mover.wait()

    def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0):
        """Align robot to the ball.

        If head is not centered at the ball (within tolerance), then
        turn head to ball. If after that the angle of head to body
        becomes too big, rotate the body by the head angle and
        simultaneously rotate head into 0 position to achieve alignment.
        """

        # only the x ball angle is relevant for alignment
        d_yaw, d_pitch = ball_x, 0
        print('ball yaw in camera:', d_yaw)

        # center head at the ball
        if (abs(d_yaw) > 0.01):
            self.mover.change_head_angles(d_yaw, d_pitch,
                                          min(1, abs(d_yaw) * 1.25))
            sleep(0.1)

        head_yaw, head_pitch = self.mover.get_head_angles()
        self._timer_stop = time()
        print('Ball to head', self._timer_stop - self._timer_start)
        print('Head yaw', head_yaw, end=' ')
        d_yaw = head_yaw - soll
        print('Head d_yaw', d_yaw)
        print('Allowed tolerance', tol)

        if abs(d_yaw) > tol:
            self.mover.stop_moving()
            print('Going to rotate by', d_yaw)
            self.mover.set_head_angles(soll, head_pitch, 0.3)
            self.mover.move_to(0, 0, d_yaw)
            self.mover.wait()
            return False
        else:
            print('Ball locked')
            return True

    def align_to_ball(self):
        ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
                                                       mask=False)
        if ball_angles is None:
            raise ValueError('No ball')
        x, y = ball_angles
        goal_x, goal_y = 0.092, 0.38
        dx, dy = goal_x - x, goal_y - y

        dx = -dx * 0.2 if abs(dx) > 0.03 else 0
        dy = dy * 0.3 if abs(dy) > 0.05 else 0
        if dx == 0  and dy == 0:
            return True
        print('Moving to dxdy', dx, dy)
        self.mover.move_to(dy, dx, 0)
        self.mover.wait()
        return False

    def align_to_goal(self):
        ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
                                                       mask=False)
        if ball_angles is None:
            self.mover.move_to(-0.1, 0, 0)
            self.mover.wait()
            ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
                                                           mask=False)
            if ball_angles is None:
                raise ValueError('No ball')
        x, y = ball_angles

        print(x, y)
        self.turn_to_ball(x, y, tol=0.15)
        sleep(0.2)

        goal = self.goal_search()
        if goal is None:
            return False

        gcl, gcr, gcc = goal
        print('Goal:', gcl, gcr, gcc)

        if gcl > 0.15 > -0.22 > gcr:
            return True

        if y > 0.38:
            self.mover.move_to(-0.05, 0, 0)
            self.mover.wait()
            # return False
        elif y < 0.28:
            self.mover.move_to(0.05, 0, 0)
            self.mover.wait()
            # return False

        sign = -1 if gcc > 0 else 1
        if sign == 1:
            self.speak('Goal is on the right')
        elif sign == -1:
            self.speak('Goal is on the left')

        for _ in range(2):
            self.mover.move_to(0, 0.05 * sign, 0)
            self.mover.wait()
        return False

    def goal_search(self):
        self.speak('Searching for goal')
        print('Last goal:', self.last_goal)
        goal_angles = None
        positions = [0, pi/6, pi/4, pi/3, pi/2]
        direction = 1 if self.last_goal == 'right' else -1
        angles = [-p for p in positions] + [p for p in positions][1:]
        angles = [a * direction for a in angles]

        for angle in angles:
            self.mover.set_head_angles(angle, -0.3)
            sleep(0.5)
            for i in range(5):
                print(i, goal_angles)
                goal_angles = self.get_goal_angles_from_camera(
                    self.upper_camera
                )
                if goal_angles is not None:
                    goal_angles = tuple(gc + angle for gc in goal_angles)
                    self.mover.set_head_angles(0, 0)
                    print('Goal found:', str(goal_angles))
                    self.last_goal = 'left' if goal_angles[2] > 0 else 'right'
                    return goal_angles
            print('Goal not found at ', str(angle))

        self.mover.set_head_angles(0, 0)
        return None
