x,y = cone

if x < FRONT_DISTANCE:

return (abs(y) < HALF_CAR_WIDTH)

else:

# TODO Check if one cone has 2 correspondences

#sorted_blue = sort_cones(blue)

#sorted_yellow = sort_cones(yellow)

middle = []

for b in blue:

min_dist = inf

yellow_selected = None

for y in yellow:

d = dist(b, y)

if MIN_TRACK_WIDTH <= d <= MAX_TRACK_WIDTH:

if d < min_dist:

min_dist = d

yellow_selected = y

if yellow_selected is not None:

middle.append(np.average([b, yellow_selected], axis=0))

if len(blue)>0:

angle = STEER_MIN

elif len(yellow)>0:

angle = STEER_MAX

mid = []

cones = sort_cones(cones)

angle = 0

size = len(cones)

for i in range(size):

#a = angle_to_point([cones[i+1][0]-cones[i][0], cones[i+1][0]-cones[i][1]])

a = angle_to_point(cones[i])

print("angle:", a)

angle = angle+a

angle = angle/size

print("final angle:", angle)

for i in range(size-1):

m = (cones[i]+cones[i+1])/2

if color == "blue":

of = [0, -HALF_TRACK_WIDTH]

else:

of = [0, HALF_TRACK_WIDTH]

of = rotate(of, math.radians(angle))

print("offset", of)

mid.append(m+of)

if len(blue)>0:

angle = STEER_MIN

mid = get_fake_mid(blue, "blue")

elif len(yellow)>0:

angle = STEER_MAX

mid = get_fake_mid(yellow, "yellow")

print("mid:", mid)

print_info = ""

mid = get_middle(blue, yellow)

angle = DEFAULT_ANGLE

i = closest_point([0,0], mid)

closest = mid[i]

look_ahead = dist([0,0], closest)

mid.insert(0, mid.pop(i))

angle = angle_to_point(closest)

for b in blue:

if gonna_hit(b):

angle = STEER_MIN

print_info += "GOING TO HIT BLUE CONE, IGNORING MIDDLE PATH \n"

break

for y in yellow:

if gonna_hit(y):

angle = STEER_MAX

print_info += "GOING TO HIT YELLOW CONE, IGNORING MIDDLE PATH \n"

break

time_start = time()

blue_size = blue.shape[0]

yellow_size = yellow.shape[0]

total_size = blue_size + yellow_size

print_info = f"Controller:\n Input:\n Blue size: {blue_size:>5}\n Yellow size: {yellow_size:>3}\n "

if total_size == 0: # no cones

print_info += "Control Mode: no cones \n"

angle = last_steer

mid = []

elif total_size == 1: # one cone only

print_info += "Control Mode: one cone only \n"

angle, mid = one_cone(blue, yellow)

last_steer = angle

mid = []

elif blue_size == 0 or yellow_size == 0: # cones on only one side

print_info += "Control Mode: one side only \n"

angle, mid = one_side(blue, yellow)

last_steer = angle

else: # one or more cones on both sides

print_info += "Control Mode: lots of cones \n"

angle, mid, print_i = lots_of_cones(blue, yellow)

print_info += print_i

last_steer = angle

result = run(blue,yellow)

print(f"{test_name} : \nresult: {result} Expected {expected}")

print("Succeed test" if result == expected else "\n\nFailed test\n\n")