# 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
#         # self.turning = True
#
#     def sense(self):
#         print('Sensing...')
#         middle_list = []
#         print('left_front: ' + str(self.robot.get_front_left_ir()))
#         print('middle_front: ' + str(self.robot.get_front_middle_ir()))
#         print('right_front: ' + str(self.robot.get_front_right_ir()))
#         print('left_back_side: ' + str(self.robot.get_rear_left_side_ir()))
#         print('right_back_side: ' + str(self.robot.get_rear_right_side_ir()))
#         print('left_encoder: ' + str(self.robot.get_left_wheel_encoder()))
#         print('right_encoder: ' + str(self.robot.get_right_wheel_encoder()))
#         rospy.sleep(2)
#         middle_list.append(self.robot.get_front_middle_ir())
#         return middle_list
#
#     def plan(self, middle_dict):
#         turn = True
#         print("Planning...")
#         if len(middle_dict) > 0:
#             print('Have some data in list.')
#             if middle_dict[0] == 1.0:
#                 print('Think that i should turn.')
#                 while turn is True:
#                     print('Turning...')
#                     self.robot.set_left_wheel_speed(15)
#                     self.robot.set_right_wheel_speed(-15)
#                     if self.robot.get_left_wheel_encoder() >= 1010:
#                         turn = False
#                         print('Should stop here.')
#                         self.robot.set_left_wheel_speed(0)
#                         self.robot.set_right_wheel_speed(0)
#                         rospy.sleep(2)
#                 return turn
#
#     def act(self, turn):
#         print("Acting...")
#         rospy.sleep(1)
#         while turn is False:
#             print('left_front: ' + str(self.robot.get_front_left_ir()))
#             print('middle_front: ' + str(self.robot.get_front_middle_ir()))
#             print('right_front: ' + str(self.robot.get_front_right_ir()))
#             rospy.sleep(2)
#             while self.robot.get_front_left_ir() >= 0.1 and self.robot.get_front_left_ir() <= 0.2:
#                 print('Moving near the wall.')
#                 print(self.robot.get_front_left_ir())
#                 self.robot.set_wheels_speed(15)
#             while self.robot.get_front_right_ir() < self.robot.get_front_left_ir():
#                 print('Lost the wall, trying to correct that.')
#                 self.robot.set_left_wheel_speed(-15)
#                 self.robot.set_right_wheel_speed(15)
#
#             """Should vote(maybe left one) the wall and moving near this wall. So its like LineTracker."""
#             # while self.robot.get_front_left_ir() == self.robot.get_front_right_ir():
#             #     self.robot.set_wheels_speed(15)
#             #     print('Start moving forward.')
#
#         # self.turning = True
#         # while self.turning is True:
#         #     left_encoder = self.robot.get_left_wheel_encoder()
#         #     self.robot.set_left_wheel_speed(20)
#         #     self.robot.set_right_wheel_speed(-20)
#         #     sensor = self.robot.get_front_middle_ir()
#         #     print(left_encoder)
#         #     if left_encoder == 1010:  # instead of 2020 should be perimeter of circle, but i do not know it.
#         #         self.robot.set_left_wheel_speed(0)
#         #         self.robot.set_right_wheel_speed(0)
#         #         print('HAVE TURNED')
#         #         self.turning = False
#         # move = True
#         # while move is True:
#         #     self.robot.set_wheels_speed(15)
#         #     print('left_front: ' + str(self.robot.get_front_left_ir()))
#         #     print('middle_front: ' + str(self.robot.get_front_middle_ir()))
#         #     print('right_front: ' + str(self.robot.get_front_right_ir()))
#         #     print('left_back: ' + str(self.robot.get_rear_left_side_ir()))
#         #     print('right_back: ' + str(self.robot.get_rear_right_side_ir()))
#         #     rospy.sleep(0.05)
#         #     distance_from_object = self.robot.get_front_middle_ir()
#         #     while distance_from_object > 0.3:
#         #         distance_from_object = self.robot.get_front_middle_ir()
#         #         rospy.sleep(0.05)
#         #         move = False
#         # turning = True
#         # while turning is True:
#         #     self.robot.set_left_wheel_speed(-15)
#         #     self.robot.set_right_wheel_speed(15)
#         #     if self.robot.get_left_wheel_encoder() == 1008:
#         #         self.robot.set_left_wheel_speed(0)
#         #         self.robot.set_right_wheel_speed(0)
#         #         print('left_front: ' + str(self.robot.get_front_left_ir()))
#         #         print('middle_front: ' + str(self.robot.get_front_middle_ir()))
#         #         print('right_front: ' + str(self.robot.get_front_right_ir()))
#         #         print('left_back: ' + str(self.robot.get_rear_left_side_ir()))
#         #         print('right_back: ' + str(self.robot.get_rear_right_side_ir()))
#         #         turning = False
#         #         rospy.sleep(0.5)
#
#         # while self.robot.get_rear_right_side_ir() > self.robot.get_rear_left_side_ir():
#         #     """Should track the left wall."""
#         #     self.robot.set_wheels_speed(-15)
#         #     pass
#         # while self.robot.get_rear_right_side_ir() < self.robot.get_rear_left_side_ir():
#         #     """Should track the right wall."""
#         #     self.robot.set_wheels_speed(-15)
#         #     pass
#
#     def main(self):
#         middle_list = self.sense()
#         turn = self.plan(middle_list)
#         self.act(turn)
#         rospy.sleep(0.05)  # 20 Hz
#         if self.problem_solved:
#             print("Solved! Good job, robot!")
#         else:
#             print("Unable to solve! :(")
#
#
# if __name__ == "__main__":
#     robot = Robot()
#     robot.main()