"""The class implementing high-level striker behavior."""

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):
        """For an example initialization see `__main__.py`

        Parameters
        ----------
        nao_ip : string
            Address of the NAO robot.
        nao_port : int
            TCP port on which the AIProxy listens.
        res : int
            Nao camera resolution. 0, 1, 2 or 3, see `imagereaders.py` for
            res description.
        ball_hsv : list
            Ball HSV parameters in format [[LH, LS, LV], [HH, HS, HV]]
        goal_hsv : list
            Goal HSV parameters in format [[LH, LS, LV], [HH, HS, HV]]
        field_hsv : list
            Field HSV parameters in format [[LH, LS, LV], [HH, HS, HV]]
        ball_min_radius : float
            Minimal radius of the ball, as a fraction of the frame height
            (for example 0.01 is 1% of the frame height)
        do_capture : bool
            Record the POV video or not (don't use, it's gonna crash the
            robot)

        """

        # 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):
        """Finish everyting and rest."""
        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):
        """Internal method: don't use."""
        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):
        """Don't use."""
        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):
        """Say stuff (useful for debugging)."""
        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.

        Parameters
        ----------
        cam : NaoImageReader
            A camera object form which to read
        mask : bool
            Mask out the field or not

        Returns
        -------
        tuple or None
            (x, y) coordinates in camera angles in radians or None if ball
            not found

        """
        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):
        """Get the goal left post, center and right post.

        Parameters
        ----------
        cam : NaoImageReader
            A camera object from which to get frames.
        mask : bool
            Mask out the field or not.

        Returns
        -------
        tuple or None
            (left, right, center) camera angles of goal in radians or None
            if not found

        """
        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):
        """Measure the distance to the ball in meters.

        Raises
        ------
        ValueError
        The ball could not be seen :(

        """
        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):
        """Turn the robot to the ball, find the ball if cannot see it.

        Parameters
        ----------
        soll : float
            X-Angle in radians, under which the ball should appear in the
            robot's camera (maybe we want the robot not to look at the ball
            directly).
        tol : float
            Allowable deviation in radians from soll-angle.

        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):
        """Fancy name for GO FORWARD."""
        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, assuming the ball is visible.

        This method is mainly used by `turn_to_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.

        Parameters
        ----------
        soll : float
            X-Angle in radians, under which the ball should appear in the
            robot's camera (maybe we want the robot not to look at the ball
            directly).
        tol : float
            Allowable deviation in radians from soll-angle.

        """

        # 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):
        """Perform one step of aligning behind the ball for the kick.

        Call this until this returns True.

        Returns
        -------
        bool
            True if nicely aligned else False.
        """
        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):
        """Perform one step of aligning ball and goal.

        Call this until this returns True.

        Returns
        -------
        bool
            True if nicely aligned else False.
        """
        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):
        """Find the goal. Return the angles in the robot frame.

        Returns
        -------
        The same as the `get_goal_angles_from_camera`.

        """
        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