import numpy as np
import math
import time

class PIDController:
    def __init__(self, event_listener):
        self.t = time.time()

        self.controlling = False

        self.P_angle = 0.4
        self.I_angle = 0.35
        self.D_angle = 0.1

        self.P_pos = 0.50
        self.I_pos = 0.3
        self.D_pos = 0.1

        self.mode = None

        self.event_listener = event_listener

    def control_multiple_robots(self, robots):
        robot_target_positions = {}
        for robot in robots:
            robot_target_positions[robot.id] = None

        running = True
        while running:
            pass


    def interactive_control(self, robots):
        controlled_robot_number = 0
        robot = robots[controlled_robot_number]

        ts = []
        angles = []

        target_pos = np.array([0.0, 0.0, 0.0])
        e_angle_old = 0.0
        e_pos_old = 0.0

        i = 0.0
        i_angle = 0.0
        i_pos = 0.0

        t0 = time.time()

        running = True
        while running:
            # handle events from opencv window
            while not self.event_listener.event_queue.empty():
                event = self.event_listener.event_queue.get()
                print("event: ", event)
                if event[0] == 'click':
                    target = event[1]
                    target_pos = np.array([target['x'], target['y'], target['angle']])
                    i_angle = 0
                    i_pos = 0
                    e_pos_old = 0
                    e_angle_old = 0
                    self.mode = 'combined'
                elif event[0] == 'key':
                    key = event[1]

                    if key == 32:  # arrow up
                        self.controlling = not self.controlling
                        if not self.controlling:
                            print("disable control")
                            robot.send_cmd()  # stop robot
                            self.mode = None    # reset control mode
                        else:
                            print("enable control")
                            i = 0
                            self.t = time.time()
                    elif key == 48:  # 0
                        target_pos = np.array([0.0, 0.0, 0.0])  # TODO: use center of board for target pos
                    elif key == 97:  # a
                        self.mode = 'angle'
                        e_angle_old = 0
                        i = 0
                        self.t = time.time()
                    elif key == 99:  # c
                        self.mode = 'combined'
                        e_angle_old = 0
                        e_pos_old = 0
                        i_angle = 0
                        i_pos = 0
                        self.t = time.time()
                    elif key == 112:  # p
                        self.mode = 'position'
                        e_pos_old = 0
                        i = 0
                        self.t = time.time()
                    elif key == 43: # +
                        self.P_pos += 0.1
                        print("P = ", self.P_angle)
                    elif key == 45: # -
                        self.P_pos -= 0.1
                        print("P = ", self.P_angle)
                    elif key == 9:  # TAB
                        # switch controlled robot
                        robot.send_cmd()  # stop current robot
                        controlled_robot_number = (controlled_robot_number + 1) % len(robots)
                        robot = robots[controlled_robot_number]
                        print(f"controlled robot: {robot.id}")
                    elif key == 113 or key == 27:  # q or ESCAPE
                        print("quit!")
                        self.controlling = False
                        robot.send_cmd()
                        return

            if self.controlling:
                # measure state
                t, x, y, angle = robot.get_measurement()
                x_pred = np.array([x, y, angle])
                dt = self.t - time.time()

                #print(f"x_pred = {x_pred}\ntarget = {target_pos}\nerror = {np.linalg.norm(target_pos - x_pred)}\n")
                if self.mode == 'angle':
                    # compute angle such that robot faces to target point
                    target_angle = target_pos[2]

                    ts.append(time.time() - t0)
                    angles.append(x_pred[2])

                    angles_unwrapped = np.unwrap([x_pred[2], target_angle])  # unwrap angle to avoid jump in data

                    e_angle = angles_unwrapped[0] - angles_unwrapped[1]   # angle difference
                    p = self.P_angle * e_angle
                    # i += self.I * dt * e     # right Riemann sum
                    i += self.I_angle * dt * (e_angle + e_angle_old)/2.0  # trapezoidal rule
                    d = self.D_angle * (e_angle - e_angle_old)/dt

                    u1 = p - i - d
                    u2 = - u1

                    e_angle_old = e_angle

                elif self.mode == 'combined':
                    # compute angle such that robot faces to target point
                    v = target_pos[0:2] - x_pred[0:2]
                    target_angle = math.atan2(v[1], v[0])

                    angles_unwrapped = np.unwrap([x_pred[2], target_angle])  # unwrap angle to avoid jump in data

                    e_angle = angles_unwrapped[0] - angles_unwrapped[1]  # angle difference
                    e_pos = np.linalg.norm(v)

                    if e_pos < 0.05:
                        self.mode = 'angle'
                        e_angle_old = 0
                        e_pos_old = 0
                        i_angle = 0
                        i_pos = 0
                        u1 = 0
                        u2 = 0
                    else:
                        forward = abs(e_angle) < np.pi/2.0

                        if not forward:
                            if e_angle > np.pi/2.0:
                                e_angle -= np.pi
                            else:
                                e_angle += np.pi

                        p_angle = self.P_angle * e_angle
                        i_angle += self.I_angle * dt * (e_angle + e_angle_old) / 2.0  # trapezoidal rule
                        d_angle = self.D_angle * (e_angle - e_angle_old) / dt

                        p_pos = self.P_pos * e_pos
                        i_pos += self.I_pos * dt * (e_pos + e_pos_old) / 2.0  # trapezoidal rule
                        d_pos = self.D_pos * (e_pos - e_pos_old) / dt

                        if forward:
                            u1 = p_angle + p_pos - i_angle - i_pos - d_angle - d_pos
                            u2 = - p_angle + p_pos + i_angle - i_pos + d_angle - d_pos
                        else:
                            u1 = p_angle - p_pos - i_angle + i_pos - d_angle + d_pos
                            u2 = - p_angle - p_pos + i_angle + i_pos + d_angle + d_pos

                        e_pos_old = e_pos
                        e_angle_old = e_angle

                else:
                    u1 = 0.0
                    u2 = 0.0
                #print(f"u = ({u1}, {u2})")
                robot.send_cmd(u1, u2)
                self.t = time.time()  # save time when the most recent control was applied
                time.sleep(0.05)