import rospy
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):
        print("Initializing...")
        self.robot = PiBot()
        self.start_timestamp = rospy.get_time()
        self.problem_solved = False

    def sense(self):
        l_straight = self.robot.get_rear_left_straight_ir()
        left = self.robot.get_rear_left_side_ir()
        right = self.robot.get_rear_right_side_ir()
        return (l_straight, left, right)

    def plan(self):
        print("Planning...")

    def act(self):
        print("Acting...")
        rospy.sleep(0.5)
        while True:
            self.sense()
            if self.robot.get_rear_left_straight_ir() < 0.15:
                self.robot.set_wheels_speed(0)
                rospy.sleep(0.2)
                print("Should start turning")
                #self.turn()
            if self.robot.get_rear_left_side_ir() > 0.9 and self.robot.get_rear_right_side_ir() > 0.9:
                self.problem_solved = True
                self.robot.set_wheels_speed(0)
                self.main()
            while self.robot.get_rear_left_side_ir() >= 0.1 and self.robot.get_rear_right_side_ir() <= 0.2:
                print('Moving near the wall.')
                print(self.robot.get_front_left_ir())
                self.robot.set_wheels_speed(-15)
            # vaata et vasakusse või paremasse seina ei sõidaks
            while self.robot.get_rear_left_side_ir() < 0.14:
                self.robot.set_left_wheel_speed(-14)
                self.robot.set_right_wheel_speed(-16)
            while self.robot.get_rear_left_side_ir() > 0.16:
                self.robot.set_left_wheel_speed(-16)
                self.robot.set_right_wheel_speed(-14)

    # def turn_left(self):
    #     left_side = self.robot.get_front_left_ir()
    #     right_side = self.robot.get_front_right_ir()
    #     left_enc = self.robot.get_left_wheel_encoder()
    #     if left_side > right_side:
    #         print("left turn")
    #         while self.robot.get_left_wheel_encoder() < left_enc + 505:
    #             self.robot.set_left_wheel_speed(-15)
    #             self.robot.set_right_wheel_speed(15)
    #     print("finished turning")
    #     self.robot.set_wheels_speed(0)
    #     rospy.sleep(2)
    #     self.main()
    #
    # def turn_right(self):
    #     left_side = self.robot.get_front_left_ir()
    #     right_side = self.robot.get_front_right_ir()
    #     left_enc = self.robot.get_left_wheel_encoder()
    #     if left_side < right_side:
    #         print("right turn")
    #         while self.robot.get_left_wheel_encoder() < left_enc + 505:
    #             self.robot.set_left_wheel_speed(15)
    #             self.robot.set_right_wheel_speed(-15)
    #     print("finished turning")
    #     self.robot.set_wheels_speed(0)
    #     rospy.sleep(2)
    #     self.main()

    def main(self):
        while self.problem_solved is False:
            self.sense()
            self.plan()
            self.act()
            rospy.sleep(0.05)  # 20 Hz
        if self.problem_solved:
            print("Solved! Good job, robot!")
            exit()
        else:
            print("Unable to solve! :(")


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