"""Objects bronze."""
import math

import rospy
from PiBot import PiBot

robot = PiBot()

ROBOT_DIAMETER = 0.14
WHEEL_DIAMETER = 0.025

TURNING_CONSTANT = 1


def turn(degrees):
    """Turn left [degrees] degrees on spot."""
    # degrees %= 360
    wheel_rotation = ROBOT_DIAMETER * math.pi / (WHEEL_DIAMETER * math.pi) * TURNING_CONSTANT * degrees
    left_enc = robot.get_left_wheel_encoder() + wheel_rotation
    right_enc = robot.get_right_wheel_encoder() - wheel_rotation
    robot.set_left_wheel_speed(20)
    robot.set_right_wheel_speed(-20)
    while robot.get_left_wheel_encoder() < left_enc or robot.get_right_wheel_encoder() > right_enc:
        rospy.sleep(0.01)
    robot.set_wheels_speed(0)


def go_meters_before_object(distance, front_or_back):
    """Go straight until you're [distance] meters away from object."""
    distance_with_front = robot.get_front_middle_ir()
    distance_with_rear = robot.get_rear_right_straight_ir()

    robot.set_wheels_speed(25)
    if front_or_back == 'front':
        while distance_with_front > distance:
            distance_with_front = robot.get_front_middle_ir()
            print(distance_with_front)
            rospy.sleep(0.01)
    if front_or_back == 'back':
        while distance_with_rear > distance:
            distance_with_rear = robot.get_rear_right_straight_ir()
            print(distance_with_rear)
            rospy.sleep(0.01)

    robot.set_wheels_speed(0)


def mdea():
    """
    340 deg/s with speed 15
    3.4 deg/0.01s
    """



def scan():
    objects = []
    a = robot.get_front_middle_ir()
    while turn(360):
        objects.append(a)
        rospy.sleep(0.01)
        print(objects)


if __name__ == '__main__':
    mdea()
    print("finished")