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.scan_speed = 14
        self.right_speed = 0
        self.speed = 14
        self.mid = self.robot.get_front_middle_ir()

    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 buffer(self, speed, turn):  # scans for object, returns dict of left coder and seen distance
        count = 1
        results = {}
        buffer = [0, 0, 0]
        last_ok_value = 0
        turning = True
        print(turn)
        left_encoder_before_looking = self.robot.get_left_wheel_encoder()
        right_encoder_before_looking = self.robot.get_right_wheel_encoder()
        # print(left_encoder_before_looking)
        # print(right_encoder_before_looking)
        rospy.sleep(1)
        while turning is True:
            # print('left: ' + str(self.robot.get_left_wheel_encoder()))
            # print(left_encoder_before_looking + turn)
            # print(right_encoder_before_looking + turn)
            print(self.robot.get_right_wheel_encoder())
            print(self.robot.get_left_wheel_encoder())
            self.robot.set_left_wheel_speed(speed)
            self.robot.set_right_wheel_speed(-speed)
            encoder = self.robot.get_left_wheel_encoder()
            distance = self.robot.get_front_middle_ir()
            if count == 1:
                buffer = [distance] * 3
                last_ok_value = distance
            count += 1
            buffer = buffer[1:]
            buffer.append(distance)
            # print(buffer)
            if self.robot.get_left_wheel_encoder() == turn + left_encoder_before_looking:
                turning = False
            if abs(distance - round(sum(buffer) / 3, 3)) <= 0.3:
                results[encoder] = distance
                last_ok_value = distance
            else:
                results[encoder] = last_ok_value
        self.robot.set_wheels_speed(0)
        # print("results:")
        # print(results)
        # print()
        print(results)
        rospy.sleep(2)
        return results

    def find_object(self, dict):
        a = min(dict, key=lambda x: dict.get(x))
        turning1 = True
        while turning1 is True:
            self.robot.get_front_middle_ir()
            self.robot.get_right_wheel_encoder()
            self.robot.set_right_wheel_speed(self.right_speed)
            self.robot.set_left_wheel_speed(-self.left_speed)
            print(self.robot.get_front_middle_ir())
            if self.robot.get_right_wheel_encoder() >= a + 1895:
                turning1 = False
                self.robot.set_wheels_speed(0)
        rospy.sleep(1)

    def sense(self):
        self.mid = self.robot.get_front_middle_ir()
        return self.mid

    def scan(self, scan_speed):  # full 360 deg scan
        # print("Scanning...")
        # print("*bass-boosted ♫ Deja Vu♫ plays in the background*")
        return self.buffer(scan_speed, 2016)


    def main(self):
        self.sense()
        dict = self.scan(scan_speed=14)
        self.find_object(dict)
        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()