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.left_speed = 0
        self.right_speed = 0
        self.speed = 14
        self.mid = self.robot.get_rear_left_straight_ir()
        self.left = self.robot.get_rear_left_side_ir()
        self.right = self.robot.get_rear_right_side_ir()
        self.left_encoder = self.robot.get_left_wheel_encoder()
        self.right_encoder = self.robot.get_right_wheel_encoder()
        self.drive = True
    # def calculate_real_speed(self):
    #     left_encoder_0 = self.robot.get_left_wheel_encoder()
    #     right_encoder_0 = self.robot.get_right_wheel_encoder()
    #     drive = True
    #     while drive:
    #         left_encoder_new = self.robot.get_left_wheel_encoder()
    #         right_encoder_new = self.robot.get_right_wheel_encoder()
    #         left_error = left_encoder_new - left_encoder_0
    #         right_error = right_encoder_new - right_encoder_0
    #         error = right_error - left_error
    #         left_encoder_0 = self.robot.get_left_wheel_encoder()
    #         right_encoder_0 = self.robot.get_right_wheel_encoder()
    #         self.left_speed = (int(self.speed + round((error / 2) / self.speed)))
    #         self.right_speed = (int(self.speed - round((error / 2) / self.speed)))
    #         return self.left_speed, self.right_speed

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

    def act(self):
        print("Acting...")
        rospy.sleep(0.5)
        left_encoder_start = self.robot.get_left_wheel_encoder()
        right_encoder_start = self.robot.get_right_wheel_encoder()
        self.drive = True
        while self.drive is True:
            print(self.sense())
            print('LEFT DIAGONAL: ' + str(self.robot.get_rear_left_diagonal_ir()))
            print('LEFT middle: ' + str(self.mid))
            print('RIGHT DIAGONAL: ' + str(self.robot.get_rear_right_diagonal_ir()))
            # left_encoder = self.robot.get_left_wheel_encoder()
            # right_encoder = self.robot.get_right_wheel_encoder()
            # left_encoder_change = left_encoder - left_encoder_start
            # right_encoder_change = right_encoder - right_encoder_start
            # problem = right_encoder_change - left_encoder_change
            # left_encoder_start = self.robot.get_left_wheel_encoder()
            # right_encoder_start = self.robot.get_right_wheel_encoder()
            # self.robot.set_left_wheel_speed(-int(self.speed + round((problem / 2) / self.speed)))
            # self.robot.set_right_wheel_speed(-int(self.speed + round((problem / 2) / self.speed)))
            if self.mid < 0.05 and self.robot.get_rear_right_straight_ir() < 0.05:
                self.robot.set_wheels_speed(0)
                rospy.sleep(0.2)
                self.drive = False
            if self.mid == 0.05 and self.robot.get_rear_left_diagonal_ir() == self.robot.get_rear_right_diagonal_ir():
                left_encoder = self.robot.get_left_wheel_encoder()
                right_encoder = self.robot.get_right_wheel_encoder()
                left_encoder_change = left_encoder - left_encoder_start
                right_encoder_change = right_encoder - right_encoder_start
                problem = right_encoder_change - left_encoder_change
                left_encoder_start = self.robot.get_left_wheel_encoder()
                right_encoder_start = self.robot.get_right_wheel_encoder()
                self.robot.set_left_wheel_speed(-int(self.speed + round((problem / 2) / self.speed)))
                self.robot.set_right_wheel_speed(-int(self.speed + round((problem / 2) / self.speed)))
            if self.robot.get_rear_left_diagonal_ir() != 0.05 or self.robot.get_rear_right_diagonal_ir() != 0.05:
                if self.left < self.right:
                    print('Turn right a little bit')
                    self.robot.set_left_wheel_speed(-18)
                    self.robot.set_right_wheel_speed(-self.right_speed)
                elif self.left > self.right:
                    print('Turn left a little bit')
                    self.robot.set_left_wheel_speed(-self.left_speed)
                    self.robot.set_right_wheel_speed(-18)

    # def turning(self):
    #     left_encoder_start = self.robot.get_left_wheel_encoder()
    #     right_encoder_start = self.robot.get_right_wheel_encoder()
    #     if self.robot.get_front_right_ir() > self.robot.get_front_left_ir():
    #         self.robot.set_left_wheel_speed(self.speed)
    #         self.robot.set_right_wheel_speed(self.speed)
    #         rospy.sleep(0.2)
    #         left_encoder = self.robot.get_left_wheel_encoder()
    #         right_encoder = self.robot.get_right_wheel_encoder()
    #         left_encoder_change = left_encoder - left_encoder_start
    #         right_encoder_change = right_encoder - right_encoder_start
    #         problem = right_encoder_change - left_encoder_change
    #         left_encoder_start = self.robot.get_left_wheel_encoder()
    #         right_encoder_start = self.robot.get_right_wheel_encoder()
    #         self.robot.set_left_wheel_speed(-int(self.speed + round((problem / 2) / self.speed)))
    #         self.robot.set_right_wheel_speed(int(self.speed - round((problem / 2) / self.speed)))
    #         rospy.sleep(1.2)
    #         self.drive = True
    #     if self.robot.get_front_right_ir() < self.robot.get_front_left_ir():
    #         self.robot.set_left_wheel_speed(self.speed)
    #         self.robot.set_right_wheel_speed(self.speed)
    #         rospy.sleep(0.2)
    #         left_encoder = self.robot.get_left_wheel_encoder()
    #         right_encoder = self.robot.get_right_wheel_encoder()
    #         left_encoder_change = left_encoder - left_encoder_start
    #         right_encoder_change = right_encoder - right_encoder_start
    #         problem = right_encoder_change - left_encoder_change
    #         left_encoder_start = self.robot.get_left_wheel_encoder()
    #         right_encoder_start = self.robot.get_right_wheel_encoder()
    #         self.robot.set_left_wheel_speed(int(self.speed - round((problem / 2) / self.speed)))
    #         self.robot.set_right_wheel_speed(-int(self.speed + round((problem / 2) / self.speed)))
    #         rospy.sleep(1.2)
    #         self.drive = True

    def main(self):
        self.robot.set_grabber_height(100)
        while not self.problem_solved:
            if self.drive is True:
                self.sense()
                self.act()
            # if self.drive is False:
            #     self.turning()
            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()