import time
import math
from compLib.Encoder import Encoder
from compLib.Robot import Robot

last_run = time.time()
last_enc_left = 0
last_enc_right = 0

pos_x = 0
pos_y = 0
orientation = 0


class Odometry():
    """DTO used for holding all odometry information. \n
    Coordinate system: \n
        X: + Forward; - Backwards \n
        Y: + Right; - Left \n
        Orientation: + Right; - Left
    """

    def __init__(self, x, y, orientation):
        self.x = x
        self.y = y
        self.orientation = orientation

    def get_x(self) -> float:
        """Returns distance driven on x-axis in meters"""
        return self.x

    def get_y(self) -> float:
        """Returns distance driven on y-axis in meters"""
        return self.y

    def get_orientation(self) -> float:
        """Returns degrees turned in radians"""
        return self.orientation

    def __str__(self):
        return f"X: {self.x} Y: {self.y} O: {self.orientation}"


class Odom(object):
    """Class used to track the movement of the robot in X, Y, Theta (Orientation)
    """

    @staticmethod
    def get_odom() -> Odometry:
        """
        :return: Current orientation of the robot
        """
        return Odometry(pos_x, pos_y, orientation)

    @staticmethod
    def clear() -> None:
        """
        Clears the current odometry information and start from X, Y, Orientation set to 0
        """
        global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation

        last_run = 0
        last_enc_left = 0
        last_enc_right = 0
        pos_x = 0
        pos_y = 0
        orientation = 0

    @staticmethod
    def update() -> None:
        """
        Updates the current odometry information of the robot, Should be called in a loop with at least 100HZ.
        Do not clear encoder positions between updates! Must be cleared, when clearing encoders!
        """
        global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation

        now = time.time()
        if last_run + 0.01 < now:
            last_run = now

            encoder_positions = Encoder.read_all()
            current_left = encoder_positions[Robot.LEFT_PORT - 1]
            current_right = -encoder_positions[Robot.RIGHT_PORT - 1]
            distance_left = (current_left - last_enc_left) / Robot.TICKS_PER_METER
            distance_right = (current_right - last_enc_right) / Robot.TICKS_PER_METER
            last_enc_left = current_left
            last_enc_right = current_right

            distance = (distance_left + distance_right) / 2.0
            theta = (distance_left - distance_right) / Robot.ARBOR_LENGTH_M

            if distance != 0:
                distance_x = math.cos(theta) * distance
                distance_y = -math.sin(theta) * distance

                pos_x = pos_x + (math.cos(orientation) * distance_x -
                                 math.sin(orientation) * distance_y)
                pos_y = pos_y + (math.sin(orientation) * distance_x +
                                 math.cos(orientation) * distance_y)
            if theta != 0:
                orientation += theta