"""Pronks ja hõbe labürint."""
from PiBot import PiBot


class Robot:
    """
    The three primitives robotic paradigm: sense, plan, act [1].

    Sense - gather information using the sensors
    Plan - create a world model using all the information and plan the next move
    Act - carry out the next step of the plan using actuators
    [1] https://en.wikipedia.org/wiki/Robotic_paradigm
    """

    def __init__(self):
        """Creating the variables needed."""
        print("Initializing...")
        # rear left: 94
        # rear right: 108
        # diagonal: 59
        self.robot = PiBot()
        self.problem_solved = False
        self.state = "working"
        self.wall_distance = self.robot.get_rear_left_straight_ir()
        self.wall_distance_2 = self.robot.get_rear_right_straight_ir()
        self.start_right = 45
        self.right_laser_diagonal = self.robot.get_rear_right_diagonal_ir()
        self.right_ir = self.robot.get_rear_right_side_ir()
        self.front_ir = self.robot.get_rear_left_straight_ir()
        self.front_ir_right = self.robot.get_rear_right_straight_ir()
        self.left_laser = self.robot.get_rear_left_diagonal_ir()
        self.rotation = self.robot.get_rotation()
        self.start_rotation = self.robot.get_rotation()
        self.stop = 0
        self.speed = -12
        self.sleep = 0.05

    def moving_straight(self):
        """Move straight."""
        print("moving straight")
        if self.right_laser_diagonal == self.start_right:
            print(f'vorduvad: {self.right_laser_diagonal}  {self.start_right}')
            self.robot.set_wheels_speed(self.speed)
        elif self.right_laser_diagonal > self.start_right:
            print(f'kaugemal seinale: {self.right_laser_diagonal} > {self.start_right}')
            self.robot.set_left_wheel_speed(self.speed + 2)
            self.robot.set_right_wheel_speed(self.speed)
        elif self.right_laser_diagonal < self.start_right:
            print(f'lahemal seinale: {self.right_laser_diagonal} < {self.start_right}')
            self.robot.set_left_wheel_speed(self.speed)
            self.robot.set_right_wheel_speed(self.speed + 2)

    def turn_left(self):
        """Turn left."""
        print("turning left")
        self.robot.set_wheels_speed(0)
        self.robot.sleep(3)
        start_rotation = self.rotation
        print('------------------------------------------')
        print(f"start {start_rotation}")
        rotation = self.rotation
        while start_rotation - 90 < rotation:
            print(f"current {rotation}")
            rotation = self.robot.get_rotation()
            self.robot.set_left_wheel_speed(-self.speed)
            self.robot.set_right_wheel_speed(self.speed)
            self.robot.sleep(self.sleep)
        print('------------------------------------------')

    def turn_right(self):
        """Turn right."""
        print("Going away from the wall.")
        self.robot.set_left_wheel_speed(self.speed)
        self.robot.set_right_wheel_speed(-self.speed)
        self.robot.sleep(self.sleep)

    def u_curve(self):
        """U curve."""
        print("U curve.")
        self.robot.set_left_wheel_speed(self.speed)
        self.robot.set_right_wheel_speed(self.speed)
        self.robot.sleep(0.1)

    def too_far_from_wall(self):
        """Too far from the wall."""
        print("Going closer to the wall.")
        self.robot.set_left_wheel_speed(-self.speed)
        self.robot.set_right_wheel_speed(self.speed)
        self.robot.sleep(self.sleep)
        self.robot.set_wheels_speed(self.speed)

    def stop_robot(self):
        """Stop."""
        print("stopped")
        self.robot.set_left_wheel_speed(0)
        self.robot.set_right_wheel_speed(0)

    def sense(self):
        """Update all the sensors."""
        print("Sensing...")
        self.right_laser_diagonal = self.robot.get_rear_right_diagonal_ir()
        print(f' parem: {self.right_laser_diagonal}')
        self.front_ir = self.robot.get_rear_left_straight_ir()
        self.front_ir_right = self.robot.get_rear_right_straight_ir()
        print(f' otse: {self.front_ir}')
        print(f' otse parem: {self.front_ir_right}')
        self.rotation = self.robot.get_rotation()

    def plan(self):
        """Make sure, what I need to do as my next action."""
        print("Sensing...")
        if self.right_laser_diagonal < 40:
            self.state = 'U curve'
        if self.front_ir >= self.wall_distance or self.front_ir_right >= self.wall_distance_2:
            self.state = "turn left"
        elif self.front_ir < self.wall_distance or self.front_ir < self.wall_distance_2:
            self.state = "following wall"
        if self.front_ir < 10 and self.front_ir_right < 10 and self.right_laser_diagonal < 40:
            self.state = "stop"

    def act(self):
        """All differesnt actions possible."""
        print("Acting...")
        if self.state == "following wall":
            self.moving_straight()
        elif self.state == "turn left":
            self.turn_left()
        elif self.state == "turn right":
            self.turn_right()
        elif self.state == 'U curve':
            self.u_curve()
        elif self.state == "too far from the wall":
            self.too_far_from_wall()
        elif self.state == "stop":
            print("stopped")
            self.stop_robot()

    def main(self):
        """Call on all the steps."""
        self.robot.sleep(2)
        start_rotation = self.rotation
        print('------------------------------------------')
        print(f"start {start_rotation}")
        print(f'sein 1: {self.wall_distance}')
        print(f'sein 2: {self.wall_distance_2}')
        rotation = self.rotation
        while start_rotation + 175 > rotation:
            print(f"current {rotation}")
            rotation = self.robot.get_rotation()
            self.robot.set_left_wheel_speed(self.speed)
            self.robot.set_right_wheel_speed(-self.speed + 1)
            self.robot.sleep(self.sleep)
        print('------------------------------------------')
        while not self.problem_solved:
            self.sense()
            self.plan()
            self.act()
            self.robot.sleep(self.sleep)  # 20 Hz
        if self.problem_solved:
            print("Solved! Good job, robot!")
        else:
            print("Unable to solve! :(")


if __name__ == "__main__":
    robot = Robot()
    robot.main()