final_paths_may3.py

from scipy.spatial.transform import Rotation
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import seaborn as sns

### ABC
### DEF

# ab = cb horizontal flip
# cd = fd horizontal flip

goal_a  = [1.0, -1.0]
goal_b  = [3.0, -1.0] 
goal_c  = [5.0, -1.0]

goal_d  = [1.0, -3.0]
goal_e  = [3.0, -3.0]
goal_f  = [5.0, -3.0]

state_dict = {}
state_dict['A'] = goal_a
state_dict['B'] = goal_b
state_dict['C'] = goal_c
state_dict['D'] = goal_d
state_dict['E'] = goal_e
state_dict['F'] = goal_f

goal_list 	= [goal_a, goal_b, goal_c, goal_d, goal_e, goal_f]
goal_colors = ['red', 'blue', 'purple', 'green', 'orange', 'pink']

# GENERATED PATHS
export_name = 'toptwo' #'null' #'toptwo'

inspection_save_path = "defense/"

def inspect_path_set():

	print("\nEarly dict")
	early_dict 	= setup_path_dict('early')
	print("\nLate dict")
	late_dict 	= setup_path_dict('late')
	# print("\nEven dict")
	even_dict	= setup_path_dict('even')

	print("Obstacle path")
	obstacle_dict = setup_path_dict('obs')

	export_path_dict('early', early_dict)
	export_path_dict('late', late_dict)
	export_path_dict('even', even_dict)
	export_path_dict('obs', obstacle_dict)

	##### Calculate the path lengths, also
	count_path_lengths(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict)

	##### draw them in groups by the path segment
	draw_paths_by_segment(inspection_save_path, early_dict, late_dict, even_dict)

	##### draw them in groups by the early/late/etc/group
	draw_paths_by_dict(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict)

	##### draw them in groups by the early/late/etc/group
	draw_paths_by_dict_defense(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict)

	##### draw them in groups by the early/late/etc/group
	draw_paths_by_dict_demo_defense(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict)

	##### special drawings for obstacle avoidance
	draw_obstacle_sets(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict)

def get_xy_from_path(path):
	if len(path) == 0:
		return [], []

	x_list, y_list = list(map(list, zip(*path)))

	return x_list, y_list

def draw_paths_by_segment(inspection_save_path, early_dict, late_dict, even_dict):

	for key in early_dict.keys():
		path_early 	= early_dict[key]
		path_late 	= late_dict[key]
		path_even 	= even_dict[key]

		early_x, early_y 	= get_xy_from_path(path_early)
		late_x, late_y 		= get_xy_from_path(path_late)
		even_x, even_y 		= get_xy_from_path(path_even)


		plt.figure(figsize=(5, 4))
		f, ax = plt.subplots()

		buffer = 2
		ax = plt.gca()
		ax.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
		ax.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
		ax.set_aspect('equal')

		plt.plot(early_x, 	early_y, 	 label = "early", color='red', lw=2)
		plt.plot(late_x, 	late_y, 	 label = "late", color='green', lw=2)
		# plt.plot(even_x, 	even_y, 	 label = "even", color='blue', lw=2)
		 
		plt.title('Path options for ' + key)
			 
		for j in range(len(goal_list)):
			goal 	= goal_list[j]
			color = goal_colors[j]
			circle = plt.Circle(goal, .1, color=color)
			ax.add_patch(circle)

		# show a legend on the plot
		plt.legend() #loc='upper center', bbox_to_anchor=(0.5, 1.05), ncol=3, fancybox=True, shadow=True)
		 
		# function to show the plot
		plt.savefig(inspection_save_path + key + '.png')
		plt.clf()
		plt.close()

	
	print("Exported images of all paths")		


def draw_paths_by_dict(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict):
	fig, axes = plt.subplot_mosaic("ABCD;IJKL", figsize=(8, 6), gridspec_kw={'width_ratios':[1, 1, 1, 1], 'height_ratios':[1, 1]})

	ax_mappings = {}
	ax_early 	= axes['A']
	# ax_even 	= axes['E']
	ax_late 	= axes['I']

	ax_early2 	= axes['B']
	# ax_even2 	= axes['F']
	ax_late2 	= axes['J']

	ax_early3 	= axes['C']
	# ax_even3 	= axes['G']
	ax_late3 	= axes['K']

	ax_early4 	= axes['D']
	# ax_even4 	= axes['H']
	ax_late4 	= axes['L']

	buffer = 1
	ax_early.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early.set_aspect('equal')

	# ax_even.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even.set_aspect('equal')

	ax_late.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late.set_aspect('equal')

	ax_early2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early2.set_aspect('equal')

	# ax_even2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even2.set_aspect('equal')

	ax_late2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late2.set_aspect('equal')

	ax_early3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early3.set_aspect('equal')

	# ax_even3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even3.set_aspect('equal')

	ax_late3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late3.set_aspect('equal')

	ax_early4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early4.set_aspect('equal')

	# ax_even4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even4.set_aspect('equal')

	ax_late4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late4.set_aspect('equal')

	### -AB -AC -AD -AE -AF -BA -BC -BD -BE -BF -CA -CB -CD -CE -CF -DA DB DC -DE -DF -EA -EB -EC -ED -EF -FA -FB -FC -FD -FE

	title1 = "Outwards / Down"
	group1 = ['BA', 'BC', 'BD', 'BF', 'EF', 'ED', "AD", 'CF']

	title2 = "Inwards / Up"
	group2 = ['AB', 'CB', 'DA', 'FC', 'EA', 'EC', 'FE', 'DE', 'BE']

	title3 = "Long / Diagonal"
	group3 = ['AF', 'AC', 'AE'] # FA, DC CD CA DF FD

	# title4 = "Short Diagonal"
	# group4 = ['AE', 'CE', 'DB', 'FB']

	title4 = "Obstacle Repulsion"
	group4 = ['AC-obs', 'AF-obs']

	### boringly flat
	# 'BE', EB

	ax_early.set_title("Early\n " + title1, fontweight="bold")
	# ax_even.set_title("Even\n " + title1, fontweight="bold")
	ax_late.set_title("Late\n " + title1, fontweight="bold")

	ax_early2.set_title("Early\n " + title2, fontweight="bold")
	# ax_even2.set_title("Even\n " + title2, fontweight="bold")
	ax_late2.set_title("Late\n " + title2, fontweight="bold")

	ax_early3.set_title("Early\n " + title3, fontweight="bold")
	# ax_even3.set_title("Even\n " + title3, fontweight="bold")
	ax_late3.set_title("Late\n " + title3, fontweight="bold")

	if False:
		ax_early4.set_title("Early\n " + title4, fontweight="bold")
		# ax_even4.set_title("Even\n " + title4, fontweight="bold")
		ax_late4.set_title("Late\n " + title4, fontweight="bold")

	for j in range(len(goal_list)):
		goal 	= goal_list[j]
		color = goal_colors[j]

		circle1 = plt.Circle(goal, .1, color=color)
		ax_early.add_patch(circle1)

		circle2 = plt.Circle(goal, .1, color=color)
		ax_late.add_patch(circle2)
		
		# circle3 = plt.Circle(goal, .1, color=color)
		# ax_even.add_patch(circle3)

		circle4 = plt.Circle(goal, .1, color=color)
		ax_early2.add_patch(circle4)

		circle5 = plt.Circle(goal, .1, color=color)
		ax_late2.add_patch(circle5)
		
		# circle6 = plt.Circle(goal, .1, color=color)
		# ax_even2.add_patch(circle6)

		circle7 = plt.Circle(goal, .1, color=color)
		ax_early3.add_patch(circle7)

		circle8 = plt.Circle(goal, .1, color=color)
		ax_late3.add_patch(circle8)
		
		# circle9 = plt.Circle(goal, .1, color=color)
		# ax_even3.add_patch(circle9)

		circle10 = plt.Circle(goal, .1, color=color)
		ax_early4.add_patch(circle10)

		circle11 = plt.Circle(goal, .1, color=color)
		ax_late4.add_patch(circle11)
		
		# circle12 = plt.Circle(goal, .1, color=color)
		# ax_even4.add_patch(circle12)


	for key in early_dict.keys():
		path_early 	= early_dict[key]
		path_late 	= late_dict[key]
		# path_even 	= even_dict[key]

		early_x, early_y 	= get_xy_from_path(path_early)
		late_x, late_y 		= get_xy_from_path(path_late)
		# even_x, even_y 		= get_xy_from_path(path_even)


		if key in group1:
			ax_early.plot(early_x, 	early_y, 	 label = "early", color='red', markersize=0, linestyle='solid')
			ax_late.plot(late_x, 	late_y, 	 label = "late", color='green', markersize=0, linestyle='solid')
			# ax_even.plot(even_x, 	even_y, 	label = "even", color='blue')

			# ax_early.scatter(early_x, 	early_y, c='red', s=8,zorder=10)
			# ax_late.scatter(late_x, 	late_y, c='blue', s=8,zorder=10)


		elif key in group2:
			ax_early2.plot(early_x, 	early_y, 	 label = "early", color='red', markersize=0, linestyle='solid')
			ax_late2.plot(late_x, 	late_y, 	 label = "late", color='green', markersize=0, linestyle='solid')
			# ax_even2.plot(even_x, 	even_y, 	label = "even", color='blue')

		elif key in group3:
			ax_early3.plot(early_x, 	early_y, 	 label = "early", color='red', markersize=0, linestyle='solid')
			ax_late3.plot(late_x, 	late_y, 	 label = "late", color='green', markersize=0, linestyle='solid')
			# ax_even3.plot(even_x, 	even_y, 	label = "even", color='blue')

		elif key in group4:
			ax_early4.plot(early_x, 	early_y, 	 label = "early", color='red', markersize=0, linestyle='solid')
			ax_late4.plot(late_x, 	late_y, 	 label = "late", color='green', markersize=0, linestyle='solid')
			# ax_even4.plot(even_x, 	even_y, 	label = "even", color='blue')
		 
		# plt.title('Path options for ' + key)
			 
	##### Add the obstacle paths
	AC_early 	= obstacle_dict['AC_OBS-early']
	AC_late 	= obstacle_dict['AC_OBS-late']
	# AC_even 	= obstacle_dict['AC_OBS-even']

	AF_early 	= obstacle_dict['AF_OBS-early']
	AF_late 	= obstacle_dict['AF_OBS-late']
	# AF_even 	= obstacle_dict['AF_OBS-even']


	AC_early_x, AC_early_y 		= get_xy_from_path(AC_early)
	AC_late_x, AC_late_y 		= get_xy_from_path(AC_late)
	# AC_even_x, AC_even_y 		= get_xy_from_path(AC_even)

	AF_early_x, AF_early_y 		= get_xy_from_path(AF_early)
	AF_late_x, AF_late_y 		= get_xy_from_path(AF_late)
	# AF_even_x, AF_even_y 		= get_xy_from_path(AF_even)


	ax_early4.plot(AC_early_x, 	AC_early_y, 	label = "early", color='red')
	ax_late4.plot(AC_late_x, 	AC_late_y, 	label = "late", color='green')
	# ax_even4.plot(AC_even_x, 	AC_even_y, 	label = "even", color='blue')


	ax_early4.plot(AF_early_x, 	AF_early_y, 	label = "early", color='red')
	ax_late4.plot(AF_late_x, 	AF_late_y, 	label = "late", color='green')
	# ax_even4.plot(AF_even_x, 	AF_even_y, 	label = "even", color='blue')


	# show a legend on the plot
	# plt.legend() #loc='upper center', bbox_to_anchor=(0.5, 1.05), ncol=3, fancybox=True, shadow=True)
	 
	# function to show the plot
	plt.tight_layout()
	plt.savefig(inspection_save_path + "overview-finally" + '.png')
	print(inspection_save_path + "overview-final" + '.png')
	plt.clf()
	plt.close()

	
	print("Exported images of all paths")


def draw_obstacle_sets(inspection_save_path, early_dict, late_dict, even_dict, obstacle_paths):
	pass

def count_path_lengths(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict):
	# Create a csv of the lengths for each path

	length_rows = []
	for key in early_dict.keys():
		early_length 	= get_path_length(early_dict[key])
		late_length 	= get_path_length(late_dict[key])
		even_length 	= "" #get_path_length(even_dict[key])


		percent_a = (late_length - early_length) / (late_length)
		percent_b = (early_length - late_length) / (early_length)

		row = [key, early_length, late_length, percent_a, percent_b]

		length_rows.append(row)

		# print("INSPECTION")
		if key in ['AB', 'AC', 'AD', 'AE', 'AF', 'BC', 'BE', 'BD']:
			# if np.abs(row[3]) > 0.03:
			print(row)


	obstacle_dict_pairs = obstacle_dict.keys()
	obstacle_dict_pairs = [od.replace('-early', "") for od in obstacle_dict_pairs]
	obstacle_dict_pairs = [od.replace('-late', "") for od in obstacle_dict_pairs]

	obstacle_paths = list(set(obstacle_dict_pairs))

	for key in obstacle_dict_pairs:
		early_length 	= get_path_length(obstacle_dict[key + "-early"])
		late_length 	= get_path_length(obstacle_dict[key + "-late"])

		percent_a = (late_length - early_length) / late_length
		percent_b = (early_length - late_length) / early_length

		row = [key, early_length, late_length, percent_a, percent_b]

		if key == 'AC_OBS': # np.abs(row[3]) > 0.03 and 
			print(row)

		length_rows.append(row)


	df = pd.DataFrame(length_rows, columns=['PATH', 'early', 'late', 'pct_late', 'pct_early'])
	df.to_csv(inspection_save_path + "lengths.csv")

	print()
	print()
	print()

	
def dist_between(x1, x2):
        # print(x1)
        # print(x2)
        # print(x1[0], x2[0], x1[1], x2[1])

        distance = np.sqrt((x1[0] - x2[0])**2 + (x1[1] - x2[1])**2)
        return distance

def get_path_length(path):
	total_length = 0
	for pi in range(1, len(path)):
		p0 = path[pi - 1]
		p1 = path[pi]

		total_length += dist_between(p0, p1)
	return total_length


ramp_a = [goal_a, [goal_a[0], goal_a[1] + .05], [goal_a[0], goal_a[1] + .1], [goal_a[0], goal_a[1] + .15], [goal_a[0], goal_a[1] + .2], [goal_a[0], goal_a[1] + .25], [goal_a[0], goal_a[1] + .3], [goal_a[0], goal_a[1] + .35]]
ramp_b = [goal_b, [goal_b[0], goal_b[1] + .05], [goal_b[0], goal_b[1] + .1], [goal_b[0], goal_b[1] + .15], [goal_b[0], goal_b[1] + .2], [goal_b[0], goal_b[1] + .25], [goal_b[0], goal_b[1] + .3], [goal_b[0], goal_b[1] + .35]]
ramp_c = [goal_c, [goal_c[0], goal_c[1] + .05], [goal_c[0], goal_c[1] + .1], [goal_c[0], goal_c[1] + .15], [goal_c[0], goal_c[1] + .2], [goal_c[0], goal_c[1] + .25], [goal_c[0], goal_c[1] + .3], [goal_c[0], goal_c[1] + .35]]
ramp_d = [goal_d, [goal_d[0], goal_d[1] - .05], [goal_d[0], goal_d[1] - .1], [goal_d[0], goal_d[1] - .15], [goal_d[0], goal_d[1] - .2], [goal_d[0], goal_d[1] - .25], [goal_d[0], goal_d[1] - .3], [goal_d[0], goal_d[1] - .35]]
ramp_e = [goal_e, [goal_e[0], goal_e[1] - .05], [goal_e[0], goal_e[1] - .1], [goal_e[0], goal_e[1] - .15], [goal_e[0], goal_e[1] - .2], [goal_e[0], goal_e[1] - .25], [goal_e[0], goal_e[1] - .3], [goal_e[0], goal_e[1] - .35]]
ramp_f = [goal_f, [goal_f[0], goal_f[1] - .05], [goal_f[0], goal_f[1] - .1], [goal_f[0], goal_f[1] - .15], [goal_f[0], goal_f[1] - .2], [goal_f[0], goal_f[1] - .25], [goal_f[0], goal_f[1] - .3], [goal_f[0], goal_f[1] - .35]]

ramps = {}
ramps['A'] = ramp_a
ramps['B'] = ramp_b
ramps['C'] = ramp_c
ramps['D'] = ramp_d
ramps['E'] = ramp_e
ramps['F'] = ramp_f

def add_offramps(path_dict):
	new_path_dict = {}

	for key in path_dict.keys():
		end_point = key[1]
		new_path_dict[key] = path_dict[key] + ramps[end_point]

		if len(path_dict[key]) > 0:
			dist = dist_between(path_dict[key][-1], ramps[end_point][0])
			if dist > .51:
				print(key)
				print(dist)


	return new_path_dict


def get_early_paths():
	tag = 'early'

	path_ab = [[1.0, -1.0], [1.2900086108419888, -0.7530968277028878], [1.5326131861312136, -0.563870378673592], [1.7331356014741433, -0.428053403454109], [1.8968782424338746, -0.34085685298818574], [2.0292799758346884, -0.2975697416824669], [2.132489843759339, -0.29596599897736564], [2.208601907197292, -0.33431086361277346], [2.287321793712234, -0.390016261958912], [2.3685235822791832, -0.4619300924512736], [2.4512191210544403, -0.5435280485663146], [2.534292001916787, -0.6275712992913405], [2.6174250901534433, -0.7120055118817148], [2.700566987150271, -0.796496612047891], [2.783710117425584, -0.8809957453939968], [2.8668534341455674, -0.9654960675552924], [2.9499967925365715, -1.0499966145808957]]
	# 3.8
	path_ab = [[1.0, -1.0], [1.2312687631725519, -0.8544874305225588], [1.43827061995188, -0.7382304913013441], [1.6226287665170918, -0.6502188157966381], [1.786227892478407, -0.5890213132499537], [1.9308652306937912, -0.553187032556764], [2.058087963941256, -0.5414572029871656], [2.169687476450365, -0.5526153955414693], [2.2668361964711177, -0.5854280221725405], [2.351450561977712, -0.6375098523349474], [2.4367780535117842, -0.6950957362376183], [2.5222865535922927, -0.7540448812862358], [2.607824131060768, -0.8132077694068731], [2.693365955329286, -0.87240114619535], [2.778908389507326, -0.9315988089241781], [2.8644509103027693, -0.9907970721338606], [2.9499934412868227, -1.0499954162813105]]
	# 3.3l another 3.3 also good
	# 3.8 winner
	path_ac = [[1.0, -1.0], [0.9787682286864174, -1.0799206363565257], [0.9704342944210872, -1.1485812221120248], [0.9746869786421921, -1.2080700717363928], [0.991144228560706, -1.2602179924638381], [1.019433764361216, -1.3064932085885252], [1.0592157780813878, -1.348005963059005], [1.1102044561389066, -1.3855014035796935], [1.1748597020285654, -1.4154418548823178], [1.2721077859536307, -1.438042585184314], [1.399031131559468, -1.4533252903368812], [1.5645566059804865, -1.461052522921165], [1.745900630042828, -1.460805290096055], [1.9207187450283525, -1.4588634145547335], [2.092013382410193, -1.4525612003505728], [2.2612245386247327, -1.440753124506868], [2.428471617214298, -1.4236499730213934], [2.5936319207393566, -1.4019901588857833], [2.75686893476847, -1.3779501113175028], [2.9184611093014343, -1.353599912858305], [3.0786416622100954, -1.3294465815513814], [3.237610080934422, -1.305532388936387], [3.395537584465804, -1.2818330642553006], [3.5525711967969245, -1.2583195280570971], [3.70883717362727, -1.234965708119023], [3.8644439385736726, -1.2117490679628116], [4.0194846019790775, -1.188650234937424], [4.174039121695308, -1.1656525700779363], [4.329001440943403, -1.1425700407771358], [4.484215907689373, -1.1194350387086172], [4.639446302612022, -1.096296721929374], [4.794700748030567, -1.073153400155551], [4.950003935797926, -1.0499999349233207]] #[[1.0, -1.0], [1.0936162783714765, -1.0188821086568032], [1.213104330202878, -1.0118923939255944], [1.3833893898769196, -0.9541056665504293], [1.5676623065533526, -0.8934756840584461], [1.7404053751494066, -0.8409646079856987], [1.9001810556590648, -0.7978770531666225], [2.0456872761291844, -0.7653906327405762], [2.1757837987872075, -0.7445300051771211], [2.289513242542139, -0.7361464879093549], [2.386117316681028, -0.7409026270842115], [2.4650491623411326, -0.7592599492466254], [2.525982000801285, -0.7914708113058755], [2.568815966312955, -0.8375723915344582], [2.5936853119557157, -0.8973807955376802], [2.6009862178673324, -0.9704704929825398], [2.6314231793254845, -1.0466024663645523], [2.6856382926875724, -1.1243905126982825], [2.836990198441508, -1.119862772375264], [2.9878615312554024, -1.1148817530421995], [3.138788657198287, -1.109892641893761], [3.2897219376697597, -1.1049019328477523], [3.4406558112249166, -1.0999108861021232], [3.5915897596805078, -1.0949197650846412], [3.7425237308631782, -1.0899286020591223], [3.893457695170999, -1.084937435418666], [4.044391696638339, -1.079946258220977], [4.195325658397063, -1.0749550957469607], [4.34625966113692, -1.069963916756095], [4.497193645024629, -1.0649727490016088], [4.6481275936199875, -1.059981598672735], [4.799061541675947, -1.0549904480113563], [4.949995488691247, -1.049999296638954]] #[[1.0, -1.0], [1.070137191477684, -1.0684129991438087], [1.1369962191274818, -1.1401041622240118], [1.204610681740745, -1.2110398905855024], [1.2753564523072172, -1.2788443108538867], [1.3503228872202475, -1.342428066380115], [1.4294904105662591, -1.4018107335556271], [1.5117367210625188, -1.4581146136832661], [1.5946808079079675, -1.5137207173339664], [1.6900584265619636, -1.556893288975543], [1.7999745132625755, -1.5874064980702483], [1.9240398650669253, -1.605647354999098], [2.0616083098812807, -1.6121640097039893], [2.2091469609910632, -1.6083846818271024], [2.3659587114556397, -1.5950117607585441], [2.5156887408796584, -1.5768001181851945], [2.658882874645599, -1.5538975901475165], [2.8020770089840252, -1.525814792828815], [2.945271143163374, -1.4948279917430007], [3.0884652773612786, -1.4631613344526346], [3.2316594118826174, -1.431394112520958], [3.374853546746325, -1.3996133425381931], [3.518047681974779, -1.3678307721749634], [3.661241817205157, -1.3360479632592304], [3.804435956809333, -1.304265121307404], [3.947630096796795, -1.2724822750774114], [4.090824236715288, -1.2406994283188169], [4.23401837657134, -1.2089165815060345], [4.377212516433794, -1.1771337346811062], [4.520406656322612, -1.1453508878407124], [4.663600796313688, -1.1135680409451805], [4.80679493631433, -1.08178519404448], [4.949989076311325, -1.0500023471457172]]
	path_ad = [[1.0, -1.0], [0.6973118526447455, -1.3464168007561812], [0.4852253558237619, -1.6178007108495702], [0.3272523358279763, -1.847197664715945], [0.21810217258401768, -2.0406473510601675], [0.1528503899240416, -2.2034443602805087], [0.1275063880881212, -2.339767032558422], [0.13912049356333622, -2.452696817138602], [0.1853857492328317, -2.544563560169027], [0.26210250763121656, -2.618513442481937], [0.35621117643595723, -2.6824831982596686], [0.4545590109115881, -2.7440707921481624], [0.5535642209783596, -2.8052964849679416], [0.6526625494574246, -2.866472015664823], [0.7517739680594719, -2.9276406730774], [0.8508871804551823, -2.9888084074939805], [0.9500006187117132, -3.0499760239299123]]
	path_ae = [[1.0, -1.0], [1.0993452322067239, -1.0744240400646206], [1.198331409415434, -1.149207118322848], [1.3074364557315141, -1.2138713713310931], [1.4390633251996632, -1.2560137547608121], [1.5589683289945415, -1.3098780028942403], [1.6666270398159844, -1.3759886009471625], [1.770976852025361, -1.4517260372961294], [1.87494454436189, -1.5285540576846062], [1.9782942104795576, -1.6067917513598153], [2.0652041726505637, -1.7015923857659998], [2.1361907224295535, -1.812433662660791], [2.2086378767760686, -1.9250374660908625], [2.2812360242579177, -2.038984897658427], [2.3554652102104936, -2.1516522439654553], [2.4297798144223965, -2.2639821506510778], [2.5041032016177516, -2.3762715019080867], [2.578419958808133, -2.4885581878919028], [2.652735763279363, -2.600844433304009], [2.7270514632212315, -2.7131306119216303], [2.8013671474856805, -2.8254167839878273], [2.87568283224588, -2.93770295264504], [2.9499985208589994, -3.049989117402151]] #[[1.0, -1.0], [1.1073518986088022, -1.0750852876432029], [1.2010904244059681, -1.1637839867937376], [1.2823621429751277, -1.264949430681716], [1.355092351328065, -1.3746564502155167], [1.4234192777871426, -1.4887665923309048], [1.4913212792680142, -1.603301732217868], [1.5617151065494643, -1.7153451595373859], [1.636540873576316, -1.822956646862734], [1.7169145262289522, -1.925020308109006], [1.8032262769220735, -2.021145768681018], [1.894622966125363, -2.1117381462195812], [1.989634848601455, -2.19828317545417], [2.085786555191, -2.283445444555702], [2.1819368156653507, -2.3685321433735718], [2.2780409750299393, -2.453658231111498], [2.374047955676469, -2.538841225097957], [2.4700416317403264, -2.624032462090378], [2.566033173607143, -2.709224879481786], [2.662024403451633, -2.7944172577370634], [2.758015628283966, -2.879609604011806], [2.854006845548093, -2.964801952788175], [2.9499980530841725, -3.0499943103779814]]
	path_af = [[1.0, -1.0], [1.1245329930202155, -1.094758120591664], [1.2319032974957411, -1.2151016670455976], [1.3343161968439536, -1.3384350211975897], [1.4386005307049214, -1.4584688786001987], [1.5592793457988854, -1.5678101901486865], [1.6995909163678986, -1.6642144648626338], [1.8587120489414042, -1.7487698961954], [2.009305734846439, -1.8314138090101941], [2.1665170100151223, -1.8983719431218267], [2.323360778741101, -1.9536415150669495], [2.479824572405001, -1.9968789344313518], [2.6358587264967133, -2.027876468814201], [2.7913681604333656, -2.073570825207726], [2.946241572298187, -2.131020800299172], [3.1005916625038, -2.199026865600126], [3.2547552332629635, -2.269536226913062], [3.408876891136628, -2.3404227662779418], [3.5629900989632852, -2.411372021992301], [3.7171019983221996, -2.4823293028676168], [3.8712136407176554, -2.553287649434818], [4.025325229499445, -2.6242461259171805], [4.179436813826897, -2.695204615722572], [4.33354840631328, -2.766163101549239], [4.487659998774542, -2.8371215873541273], [4.641771590322849, -2.9080800736573047], [4.7958831827307264, -2.979038559570849], [4.949994776867857, -3.0]]

	# # generate_vanilla_straight_line_paths_for_testing(goal_b, [goal_a, goal_c, goal_d, goal_e, goal_f])
	path_bc = [[3.0, -1.0], [3.4188633729998408, -0.7552786791450201], [3.716196211497597, -0.5692030123566075], [3.950181446046529, -0.4455162060842515], [4.145923719438314, -0.376730219202704], [4.283696911401175, -0.34892998377094636], [4.375328068907346, -0.35585801532953326], [4.454969539039409, -0.38396639502554397], [4.524172685938499, -0.43156877124391724], [4.583979919255225, -0.4968694825057302], [4.637876310394277, -0.5730268849159894], [4.690157052944371, -0.6520651766554977], [4.742162553474759, -0.7315861890580081], [4.794126450847634, -0.8111800404108194], [4.846084310302697, -0.8907845967916005], [4.898041307614097, -0.9703907039324433], [4.949998182278259, -1.049997033211917]]
	path_ba = horizontal_flip(path_bc)
	# vanilla straight 3.38
	path_be = [[3.0, -1.0], [2.9921212717346757, -1.3654741249591815], [2.9801218300702343, -1.6723880318440563], [2.9726188297283382, -1.9053958893959069], [2.975872474459037, -2.051908627599181], [2.9745439982725514, -2.168109851294602], [2.969879382479441, -2.259879647217641], [2.9707663668287383, -2.3419634215741634], [2.975915569784843, -2.419544116461221], [2.975861765365448, -2.495881758253198], [2.974884269041804, -2.5728713895613127], [2.9710671188808475, -2.652170181493826], [2.966883960176791, -2.73172669826686], [2.9626669897133677, -2.811287455685808], [2.9584457527247627, -2.8908485778163167], [2.9542240894897662, -2.9704097042518613], [2.950002354165734, -3.049970839294933]]
	
	# 3.33
	path_bf = [[3.0, -1.0], [3.0235022416801, -1.1695717008582793], [3.0628065009916803, -1.3163249155292334], [3.120629500159606, -1.4495432477383612], [3.1938406393987058, -1.5708990049605733], [3.2786391093758027, -1.6816948581464624], [3.361940258275606, -1.783200726879168], [3.4495049947585796, -1.8793532159429411], [3.543026046291918, -1.9687552923710236], [3.6434811948718693, -2.051074092272749], [3.752121379479021, -2.125595061775354], [3.8702492872484275, -2.191487966277852], [3.9976818862713057, -2.249363493096062], [4.107241647347651, -2.3151317011541983], [4.209582651689908, -2.3863818227983997], [4.308013393071312, -2.462092348462622], [4.403646338150244, -2.5413750407864946], [4.4969173418873645, -2.62369500947266], [4.587890438432844, -2.708849460988016], [4.677556612073671, -2.7951692207571384], [4.769575942057786, -2.878836526591472], [4.86072236910105, -2.963428839741053], [4.949995397159319, -3.0499800874495806]]
	
	#3.2
	path_bd = horizontal_flip(path_bf)

	# 4.0, 3.3071818981185417
	# path_ad = [[1.0, -1.0], [0.8450066460867036, -1.2420517814984886], [0.7248671590247899, -1.4550388649094668], [0.6319008128102347, -1.64629393526737], [0.5648453560834257, -1.817579592411624], [0.5223019542348142, -1.970681062580033], [0.5029445861729119, -2.1072311080843207], [0.5056058037074743, -2.228647166771597], [0.5290974598742898, -2.336256743883777], [0.5707628268036204, -2.4322234521505646], [0.622588227559914, -2.5218829803512506], [0.6768188022705147, -2.6100941994521336], [0.7314110525215415, -2.69809432679548], [0.7860529434717176, -2.7860663898245086], [0.8407015655661394, -2.874034788005744], [0.895351095294153, -2.9620027078137072], [0.9500007499705694, -3.049970564288138]]

	path_ae = [[1.0, -1.0], [1.1326597892153492, -1.0570282433726284], [1.2752959348737143, -1.1040801303668677], [1.4512651626877107, -1.1177989351377997], [1.6103621172822153, -1.1483900132779394], [1.7517617885960788, -1.1966783745046532], [1.874968512022933, -1.2631596835980547], [1.9796986419988494, -1.3481175860222057], [2.065883432510892, -1.451620828116438], [2.1336709781008607, -1.573521315439521], [2.183447418301862, -1.7134329081548456], [2.2392480822049983, -1.8499634004771675], [2.2980020444045657, -1.9843193524661005], [2.3635933637035476, -2.0944487976886945], [2.4289637047493438, -2.200847224002731], [2.4941220467629472, -2.307021637083482], [2.559249765282227, -2.4131641849781595], [2.624373362374383, -2.5193020451058583], [2.6894963690503464, -2.625439256835994], [2.7546192856094427, -2.7315763814553855], [2.81974218954189, -2.8377134939478883], [2.8848650923150365, -2.943850604511318], [2.949987994848944, -3.049987714847889]]
	path_ad = [[1.0, -1.0], [0.7534464647713802, -1.3082509953282107], [0.5806452401846156, -1.5549746486798932], [0.44947267361505855, -1.7689870394301384], [0.3572541679426934, -1.953772567839765], [0.30109821485868593, -2.1127655134601104], [0.2783430215967724, -2.248983583380833], [0.28675396971138334, -2.3648951510916976], [0.3243064277613185, -2.4625804384178047], [0.3871788114016662, -2.544968685322053], [0.4642579577129546, -2.619011497183923], [0.5447313726728165, -2.691108539750423], [0.625721090311913, -2.7629165610721906], [0.7067828115271431, -2.8346852897481547], [0.7878544458687176, -2.906448761223966], [0.8689274445686318, -2.9782115277667653], [0.9500006624707337, -3.04997418878327]]

	path_dict = {}
	path_dict['AB'] = path_ab
	path_dict['AC'] = path_ac
	path_dict['AD'] = path_ad
	path_dict['AE'] = path_ae
	path_dict['AF'] = path_af

	path_dict['BA'] = path_ba
	path_dict['BC'] = horizontal_flip(path_ba)
	path_dict['BD'] = path_bd
	path_dict['BE'] = path_be
	path_dict['BF'] = horizontal_flip(path_bd)

	path_dict = add_offramps(path_dict)

	# Return the name and the list
	return path_dict

def get_late_paths():
	### AC, AF, BA
	path_ab = [[1.0, -1.0], [1.139197370383164, -0.9428408708041127], [1.2783947022313513, -0.8856817894653627], [1.4175917456164873, -0.8285230667819355], [1.5567866613018349, -0.7713669404045002], [1.6959668170071747, -0.714229548878496], [1.8350456633021839, -0.6572250660278522], [1.9734428400164423, -0.6011425476707658], [2.1077042108598616, -0.5508464280089307], [2.2265142153963184, -0.5233214442259632], [2.3202308825573135, -0.5338167532386597], [2.4167220098063034, -0.5644084669114453], [2.5162582004317358, -0.6158136141965511], [2.61909123422008, -0.688797819227689], [2.7254797751049247, -0.7844365406250485], [2.835691151881214, -0.9041946888940767], [2.949995747817214, -1.0499943875598339]]
	path_ac = [[1.0, -1.0], [1.1498092136791775, -1.0374347459206497], [1.2996184273583482, -1.0748694918412998], [1.4494276410375384, -1.1123042377619372], [1.5992368547168245, -1.1497389836824583], [1.7490460683969757, -1.1871737296021352], [1.8988552820838853, -1.2246084755159778], [2.048664495818083, -1.2620432213892148], [2.1984737095523537, -1.2994779671151615], [2.3482829232866074, -1.3369127117346515], [2.4980921370209166, -1.3743474480477316], [2.6479013507552165, -1.4117821220527917], [2.7977105644893228, -1.4492163290335784], [2.947519778223643, -1.4866470388015944], [3.097328991958193, -1.5240516262424506], [3.2471382056925293, -1.5612638351156771], [3.396947419427171, -1.5971857627885964], [3.546756633161801, -1.6276262997662352], [3.6965658468963536, -1.6482331774076928], [3.8465748316691815, -1.6574746759830592], [3.9968084147221576, -1.6547385875082117], [4.147056665329427, -1.6396407333788232], [4.297317197903731, -1.611762613243981], [4.447586732527683, -1.5709588810033972], [4.597861040450443, -1.5175659161096533], [4.748134923206579, -1.4520179641887891], [4.853646122343416, -1.392412250530775], [4.935796699582601, -1.3333935699973531], [4.992858594098143, -1.275063752751158], [5.023446501501771, -1.2175145656417774], [5.026636513657088, -1.1608138106942496], [5.002067500207655, -1.1049883504747902], [4.950001995707094, -1.0500020874400975]]
	path_ad = [[1.0, -1.0], [0.9241794547078082, -1.1700139532178016], [0.8483591494275136, -1.340027654303158], [0.7725406856001354, -1.5100404986702611], [0.6967274730246248, -1.6800516080879513], [0.6209321333678215, -1.8500479538840535], [0.545255914026754, -2.0199522658552644], [0.47042588776931793, -2.189258719838303], [0.40073573087466596, -2.3550991019992424], [0.35081705645956013, -2.5082662702873924], [0.3331692321320707, -2.641800738374603], [0.34958076523656745, -2.75558903419184], [0.39964755897071297, -2.8507094710825096], [0.4834099021621462, -2.9278557039945983], [0.6017247923966582, -2.9871620536163266], [0.7563534337855453, -3.0282034166867193], [0.9500006187185951, -3.0499785242609296]]
	path_ae = [[1.0, -1.0], [1.1469479008314472, -1.0758316316721552], [1.2938958016849285, -1.1516632632819865], [1.4408437028414316, -1.2274948939340316], [1.5877916022701977, -1.3033265254647846], [1.7347394882830343, -1.3791581693451833], [1.8816872714627084, -1.4549899164810436], [2.028634271844581, -1.5308224480649173], [2.1755753399462927, -1.6066609106285843], [2.322471748962186, -1.6825440317435403], [2.469038347211909, -1.7587569648757506], [2.613399290186494, -1.8371755518114057], [2.7485790358783904, -1.9248813149895416], [2.867652048230138, -2.0288879101587565], [2.9681114960956814, -2.1499425874644262], [3.049789932849268, -2.288082965789721], [3.1134043018753665, -2.4424777951902152], [3.1407803877673706, -2.581433802654852], [3.1352248786628607, -2.7047369694689873], [3.1148975800875682, -2.81245915956221], [3.0803580076517374, -2.904488212298604], [3.0318243494834243, -2.981032612043894], [2.9500055448003293, -3.049995105114468]] #[[1.0, -1.0], [1.1219586482812436, -1.0403881106168258], [1.243915235019268, -1.0807785851795801], [1.3658677140751223, -1.1211737695017734], [1.4878140495000205, -1.1615759945266966], [1.609752216377997, -1.2019875790316419], [1.7316802153334878, -1.242410824659459], [1.8535967499971713, -1.2828477440363464], [1.9755066394078964, -1.3232937116535952], [2.097389023359379, -1.363769341607614], [2.2191723530669067, -1.4043452331678636], [2.3395949507856653, -1.4454244121917506], [2.4499273409762874, -1.4962477076704876], [2.5424699884375204, -1.5641550076209112], [2.6303712311369676, -1.636352687629858], [2.7135542082138833, -1.7128802873943787], [2.7919704188984253, -1.793808624910775], [2.8622654457898578, -1.9773456946434576], [2.9234770493310287, -2.2592071967846863], [2.9657425372151005, -2.5298586348312675], [2.976316763940371, -2.7813300188475134], [2.9773485720838644, -2.9323444161121173], [2.9500054352765552, -3.049984005775869]]
	path_af = [[1.0, -1.0], [1.1600563980958165, -1.07266841586868], [1.3201127961908234, -1.14533683175032], [1.4801691942872166, -1.2180052476258778], [1.6402255923919988, -1.2906736635473353], [1.8002819905312706, -1.363342079421549], [1.9603383889169943, -1.4360104950747503], [2.120394789046737, -1.5086789089655004], [2.2804511898126414, -1.581347313338729], [2.440507582429783, -1.6540156515435163], [2.600563822800718, -1.7266835201481001], [2.7606200631687527, -1.799347999073933], [2.920676303523513, -1.8719863158708248], [3.0807325438830078, -1.9444255852928767], [3.240788784239529, -2.0154864709399054], [3.4008450246095356, -2.0813160663091574], [3.5609012649762057, -2.1375195316661553], [3.72095750536284, -2.182741279164618], [3.8813983566305934, -2.2168654271008], [4.042027828341924, -2.240192413481585], [4.201783251578418, -2.302414213754077], [4.360437684410089, -2.404261591921397], [4.497270639487636, -2.526892265568702], [4.614313102408636, -2.635919632684226], [4.719119792067808, -2.7428887973699054], [4.810648259057721, -2.8476337337581468], [4.8879068182636995, -2.9500270267321747], [4.949996797723957, -3.0]]

	path_ae = [[1.0, -1.0], [1.1402313941228372, -1.1079709401050577], [1.2804627882355315, -1.2159418802356547], [1.4206941823589503, -1.3239128203808574], [1.5609255775994106, -1.4318837596934164], [1.7011569879731883, -1.5398546844701917], [1.841388559722496, -1.6478254485612867], [1.9816194361614161, -1.7557969076433508], [2.12184509955399, -1.8637735801868736], [2.26203174058364, -1.9717892741412428], [2.401930206333997, -2.0800931422668563], [2.539849283867812, -2.190338963427814], [2.669361631211483, -2.3088157355829186], [2.784016615610702, -2.4418127690966047], [2.8824075855184086, -2.5907395158827113], [2.962603551771712, -2.7139243642005697], [3.000050211555512, -2.8101449220237393], [3.0242881833290767, -2.890917818257237], [3.0348778753886507, -2.9561317552505715], [3.031430348901313, -3.0058736379448443], [3.0135987483115345, -3.040387373659397], [2.987744104574669, -3.0537890930430422], [2.950002563082231, -3.0500002369934367]]

	# path_ac = [[1.0, -1.0], [1.1498092136791775, -1.0374347459206497], [1.2996184273583482, -1.0748694918412998], [1.4494276410375384, -1.1123042377619372], [1.5992368547168245, -1.1497389836824583], [1.7490460683969757, -1.1871737296021352], [1.8988552820838853, -1.2246084755159778], [2.048664495818083, -1.2620432213892148], [2.1984737095523537, -1.2994779671151615], [2.3482829232866074, -1.3369127117346515], [2.4980921370209166, -1.3743474480477316], [2.6479013507552165, -1.4117821220527917], [2.7977105644893228, -1.4492163290335784], [2.947519778223643, -1.4866470388015944], [3.097328991958193, -1.5240516262424506], [3.2471382056925293, -1.5612638351156771], [3.396947419427171, -1.5971857627885964], [3.546756633161801, -1.6276262997662352], [3.6965658468963536, -1.6482331774076928], [3.8465748316691815, -1.6574746759830592], [3.9968084147221576, -1.6547385875082117], [4.147056665329427, -1.6396407333788232], [4.297317197903731, -1.611762613243981], [4.447586732527683, -1.5709588810033972], [4.597861040450443, -1.5175659161096533], [4.748134923206579, -1.4520179641887891], [4.853646122343416, -1.392412250530775], [4.935796699582601, -1.3333935699973531], [4.992858594098143, -1.275063752751158], [5.023446501501771, -1.2175145656417774], [5.026636513657088, -1.1608138106942496], [5.002067500207655, -1.1049883504747902], [4.950001995707094, -1.0500020874400975]]

	# # generate_vanilla_straight_line_paths_for_testing(goal_b, [goal_a, goal_c, goal_d, goal_e, goal_f])
	path_ba = [[3.0, -1.0], [2.84464712734914, -0.9437842454773084], [2.689294368681327, -0.8875685328710651], [2.533942494290066, -0.8313531798578918], [2.3785964105698034, -0.7751403078153123], [2.2232881212847135, -0.7189450624647438], [2.06821094945589, -0.6628676931850418], [1.9132805900558933, -0.6080192909034989], [1.7588930851844709, -0.5587924387634673], [1.6029339759062708, -0.5293139489783156], [1.465165324747436, -0.5264009658033867], [1.343821126251378, -0.5495129200748203], [1.2370609269874855, -0.5973628202794585], [1.1441171090451845, -0.6698407472933501], [1.06485140195943, -0.7678536483867996], [0.9997505720510083, -0.8934360265065577], [0.9500076227087499, -1.0499937361142258]]
	path_bc = None
	path_be = [[3.0, -1.0], [2.9804625367314106, -1.1395570227158582], [2.9609250766774684, -1.2791140087249966], [2.941387569460522, -1.4186706572791214], [2.9218495249697884, -1.5582248731137445], [2.9023058495623744, -1.697760966496838], [2.8827130200722078, -1.8371640553532822], [2.8627136124544825, -1.9756223478182353], [2.8475993892410987, -2.10889466912689], [2.8485456080471785, -2.2298587105554706], [2.874921457029713, -2.332206179954131], [2.927739546282291, -2.4159023474589607], [2.9567351809763047, -2.5241189267919895], [2.9695129194488765, -2.6492532941790334], [2.9696335217146643, -2.7802131099638814], [2.960393600308126, -2.915091975661581], [2.950006772248204, -3.049970839223085]]
	# 5.75 lambda
	path_bf = [[3.0, -1.0], [3.1241279668275084, -1.0804903831498067], [3.2482559351959504, -1.1609807652544835], [3.3723839065805605, -1.2414711478582938], [3.496511879457595, -1.3219615265248745], [3.6206398491608605, -1.4024519141216643], [3.744767806944641, -1.482942321290396], [3.8688955546723474, -1.563432930765803], [3.99302165561885, -1.64392518726675], [4.1171342730043605, -1.7244309143492211], [4.241140430234065, -1.8050431054807565], [4.364393825427494, -1.8864080571040973], [4.484343584209294, -1.9785556228419903], [4.598418958698763, -2.0853887179816013], [4.706162709982264, -2.2089308321904535], [4.8079450924514715, -2.3384343240875674], [4.892381089261572, -2.4669015066450966], [4.959076001243545, -2.5934485803722223], [4.998343629684881, -2.7092807629645765], [5.009228395158241, -2.8144016854993454], [5.004197033128171, -2.9063286494111926], [4.984146377574358, -2.9848611037218293], [4.950009513246287, -3.049987050347337]]
	path_bd = horizontal_flip(path_bf)


	path_dict = {}
	path_dict['AB'] = path_ab
	path_dict['AC'] = path_ac
	path_dict['AD'] = path_ad
	path_dict['AE'] = path_ae
	path_dict['AF'] = path_af

	path_dict['BA'] = path_ba
	path_dict['BC'] = horizontal_flip(path_ba)
	path_dict['BD'] = path_bd
	path_dict['BE'] = path_be
	path_dict['BF'] = horizontal_flip(path_bd)

	path_dict = add_offramps(path_dict)

	# Return the name and the list
	return path_dict

def get_even_paths():
	path_ab = [[1.0, -1.0], [1.139197370383164, -0.9428408708041127], [1.2783947022313513, -0.8856817894653627], [1.4175917456164873, -0.8285230667819355], [1.5567866613018349, -0.7713669404045002], [1.6959668170071747, -0.714229548878496], [1.8350456633021839, -0.6572250660278522], [1.9734428400164423, -0.6011425476707658], [2.1077042108598616, -0.5508464280089307], [2.2265142153963184, -0.5233214442259632], [2.3202308825573135, -0.5338167532386597], [2.4167220098063034, -0.5644084669114453], [2.5162582004317358, -0.6158136141965511], [2.61909123422008, -0.688797819227689], [2.7254797751049247, -0.7844365406250485], [2.835691151881214, -0.9041946888940767], [2.949995747817214, -1.0499943875598339]]
	path_ac = [[1.0, -1.0], [1.0748196861171284, -1.0769025069421758], [1.1510191007001955, -1.1524252947512483], [1.2359389198892856, -1.2192276770156572], [1.329518973053761, -1.2773698834949598], [1.4360123037692638, -1.3261923229775312], [1.555001624432835, -1.3661270545384543], [1.6775878220241693, -1.400629909769473], [1.8049413196711908, -1.4295335474562716], [1.936955736009406, -1.4529514800416965], [2.0733826083096747, -1.471047180605778], [2.2098094616428736, -1.4855025294443869], [2.346236295486091, -1.496266545532588], [2.482663075136035, -1.5032881485305505], [2.6190899072711202, -1.5065175876613757], [2.755516731232521, -1.5058981894154595], [2.8919435434227223, -1.5014016187100456], [3.0283703582566437, -1.493023045232766], [3.1647972035744414, -1.4807779288149454], [3.3012240495401173, -1.464692042106218], [3.437650874664959, -1.4448012421894467], [3.5771074842663806, -1.4203726608267624], [3.719600973078941, -1.3915044977848106], [3.8652058533344755, -1.3583311522253914], [4.008686401283216, -1.323628610803148], [4.147678078516748, -1.288993401909212], [4.281670038017043, -1.254440742855777], [4.410130289721114, -1.2199881525790282], [4.532507256490395, -1.1856556290954063], [4.648238726866099, -1.1514655789451307], [4.756762808644261, -1.1174423568559133], [4.857527026361652, -1.0836117595039896], [4.9499975326883074, -1.0500004036841404]]
	path_ad = [[1.0, -1.0], [0.8596585733744453, -1.2278157302601582], [0.7506099468369825, -1.429360149990184], [0.6610811352403071, -1.6152200795561678], [0.5905174725456657, -1.7863798919744107], [0.5381214866712465, -1.943992112552087], [0.502977246642973, -2.0892555682167457], [0.4841517031388481, -2.223321807981366], [0.48077102468440847, -2.347233610381561], [0.49207082387717216, -2.4618909852829196], [0.5174288086224761, -2.568038072828005], [0.5563800969767213, -2.666264424943885], [0.6086224516546559, -2.7570154883145355], [0.6740150527583179, -2.840607364672642], [0.7525738600524818, -2.917242802796254], [0.8444654108916714, -2.987026164741242], [0.9500006187288894, -3.04997619706652]]
	path_ae = [[1.0, -1.0], [1.0571786223499102, -1.1605751484788112], [1.1262653504600504, -1.3092421324379595], [1.194691061353501, -1.4585701615846007], [1.2626860431864937, -1.6083290353285369], [1.330482503621384, -1.758286448205167], [1.3983290498680163, -1.9081937651545207], [1.4664927400419028, -2.0577841672684545], [1.5352545320783861, -2.206776608698673], [1.604902913132665, -2.354882578137259], [1.695133642063655, -2.4959463187480875], [1.8067245028497423, -2.629669644297157], [1.9476313570384243, -2.7355540894967154], [2.1174468817641983, -2.8140489143500753], [2.314597117845594, -2.8666788197729653], [2.4816913096208406, -2.9067227867044974], [2.6106182013647325, -2.9413324213537453], [2.7138132109857276, -2.972681225319436], [2.794329274334708, -3.000270573127519], [2.856480887615702, -3.0223141295670453], [2.9024798175902697, -3.038072272533793], [2.9346061664500085, -3.0468671039283572], [2.9500011715796535, -3.0499961502334796]]
	path_af = [[1.0, -1.0], [1.100195483042804, -1.1209398879293098], [1.2082709502225033, -1.2339997852703726], [1.32425633510361, -1.3391497323413009], [1.4481374111656675, -1.43640384967054], [1.579854839188333, -1.5258215513023352], [1.7193026246053407, -1.6075088773035395], [1.868446180365881, -1.6812519962004642], [2.0269415034478606, -1.747320626718026], [2.1854368194545626, -1.809169001697067], [2.3439321800753725, -1.866625830070872], [2.5024275771726905, -1.9195289539994282], [2.660922991029325, -1.9677264730117252], [2.81941841649772, -2.0110780950978584], [2.9779138491922144, -2.049456347437308], [3.1364092524608864, -2.082747301181042], [3.294904627130814, -2.126881905953286], [3.4533999979143273, -2.181430242378275], [3.6118953661506152, -2.2460378925944893], [3.770390756816983, -2.320404266737441], [3.9291496177736605, -2.4049144488652914], [4.088240819277836, -2.498755344764352], [4.243661336375082, -2.5921760766000617], [4.3949850478729084, -2.6850999540996425], [4.5417607993813505, -2.7774414128073497], [4.683516017564791, -2.8691065073221598], [4.819761128303207, -2.9599938662559144], [4.949994776519084, -3.049996149308088]]

	path_bc = [[3.0, -1.0], [2.74095162876091, -1.0692602834295626], [2.5293729400404517, -1.1234896879575864], [2.337875237826563, -1.1825638906443194], [2.165470531754623, -1.2476943516307293], [1.9971431868115719, -1.2918163474180322], [1.8320376913607022, -1.311331318244287], [1.669309421821982, -1.30818305466289], [1.5082928083610854, -1.2893023370645662], [1.3486075769486117, -1.2700102935624709], [1.2730869617446985, -1.210446491983415], [1.200714670776524, -1.15075197527985], [1.1374074141858086, -1.1060213951124995], [1.0814900459276955, -1.0747521059167857], [1.0320670257694742, -1.0554212930088518], [0.9882333725503196, -1.047084435371563], [0.9500103137222297, -1.0499908879746018]]
	path_ba = horizontal_flip(path_bc)
	path_bd = []
	path_be = []
	path_bf = None

	path_dict = {}
	path_dict['AB'] = path_ab
	path_dict['AC'] = path_ac
	path_dict['AD'] = path_ad
	path_dict['AE'] = path_ae
	path_dict['AF'] = path_af

	path_dict['BA'] = path_ba
	path_dict['BC'] = horizontal_flip(path_ba)
	path_dict['BD'] = path_bd
	path_dict['BE'] = path_be
	path_dict['BF'] = horizontal_flip(path_bd)

	path_dict = add_offramps(path_dict)

	# Return the name and the list
	return path_dict

def get_obstacle_paths():
	# 0.5, lm4.5
	#path_ac_early 	= [[1.0, -1.0], [1.0581645405322175, -1.0975945539315506], [1.1246265234852584, -1.1868916653887254], [1.205499902064594, -1.2617773812615702], [1.2882742371865374, -1.334762140470666], [1.3728975755178954, -1.4058978964336988], [1.4593254870054981, -1.4752290792579683], [1.5475282472199112, -1.542785413822417], [1.6443507725705222, -1.6072990676147139], [1.7498068790131234, -1.6689121578061268], [1.887690248722391, -1.7037235721848312], [2.03636372546013, -1.7192804251994471], [2.1835996139275435, -1.7283124053787038], [2.3308355023595775, -1.7315539026669735], [2.5033761342987817, -1.722333496003813], [2.702480586768213, -1.7007021586726268], [2.890745917064809, -1.6735035766190463], [3.054609581632105, -1.6413981692492305], [3.1937149771119704, -1.6065639519293968], [3.3060026618765352, -1.5710935116097313], [3.432462421684142, -1.5310229035285408], [3.558922181496881, -1.4909397101514468], [3.685382784319793, -1.4508547065796837], [3.8118434738973406, -1.410769454441675], [3.9383041749605483, -1.3706841698077452], [4.064764873164852, -1.330598883507838], [4.191225570734051, -1.2905135971964885], [4.317686268221163, -1.2504283108697298], [4.4441469656927035, -1.2103430245432483], [4.57060766315843, -1.1702577382193797], [4.697068360628516, -1.130172451892094], [4.823529058091508, -1.090087165569946], [4.9499897555561905, -1.05000187924656]]
	#path_ac_early 	= [[1.0, -1.0], [1.0581645405322175, -1.0975945539315506], [1.1246265234852584, -1.1868916653887254], [1.205499902064594, -1.2617773812615702], [1.2882742371865374, -1.334762140470666], [1.3728975755178954, -1.4058978964336988], [1.4593254870054981, -1.4752290792579683], [1.5475282472199112, -1.542785413822417], [1.6443507725705222, -1.6072990676147139], [1.7498068790131234, -1.6689121578061268], [1.887690248722391, -1.7037235721848312], [2.03636372546013, -1.7192804251994471], [2.1835996139275435, -1.7283124053787038], [2.3308355023595775, -1.7315539026669735], [2.5033761342987817, -1.722333496003813], [2.702480586768213, -1.7007021586726268], [2.890745917064809, -1.6735035766190463], [3.054609581632105, -1.6413981692492305], [3.1937149771119704, -1.6065639519293968], [3.3060026618765352, -1.5710935116097313], [3.432462421684142, -1.5310229035285408], [3.558922181496881, -1.4909397101514468], [3.685382784319793, -1.4508547065796837], [3.8118434738973406, -1.410769454441675], [3.9383041749605483, -1.3706841698077452], [4.064764873164852, -1.330598883507838], [4.191225570734051, -1.2905135971964885], [4.317686268221163, -1.2504283108697298], [4.4441469656927035, -1.2103430245432483], [4.57060766315843, -1.1702577382193797], [4.697068360628516, -1.130172451892094], [4.823529058091508, -1.090087165569946], [4.9499897555561905, -1.05000187924656]]
	path_ac_early   = [[1.0, -1.0], [1.0581645405322175, -1.0975945539315506], [1.1246265234852584, -1.1868916653887254], [1.205499902064594, -1.2617773812615702], [1.2882742371865374, -1.334762140470666], [1.3728975755178954, -1.4058978964336988], [1.4593254870054981, -1.4752290792579683], [1.5475282472199112, -1.542785413822417], [1.6443507725705222, -1.6072990676147139], [1.7498068790131234, -1.6689121578061268], [1.887690248722391, -1.7037235721848312], [2.03636372546013, -1.7192804251994471], [2.1835996139275435, -1.7283124053787038], [2.3308355023595775, -1.7315539026669735], [2.5033761342987817, -1.722333496003813], [2.702480586768213, -1.7007021586726268], [2.890745917064809, -1.6735035766190463], [3.054609581632105, -1.6413981692492305], [3.1937149771119704, -1.6065639519293968], [3.3060026618765352, -1.5710935116097313], [3.432462421684142, -1.5310229035285408], [3.558922181496881, -1.4909397101514468], [3.685382784319793, -1.4508547065796837], [3.8118434738973406, -1.410769454441675], [3.9383041749605483, -1.3706841698077452], [4.064764873164852, -1.330598883507838], [4.191225570734051, -1.2905135971964885], [4.317686268221163, -1.2504283108697298], [4.4441469656927035, -1.2103430245432483], [4.57060766315843, -1.1702577382193797], [4.697068360628516, -1.130172451892094], [4.823529058091508, -1.090087165569946], [4.9499897555561905, -1.05000187924656]]
	path_ac_late 	= [[1.0, -1.0], [1.1046046075738576, -1.0407911975363127], [1.2092092151103224, -1.0815823950983694], [1.3138138229626413, -1.1223735926544067], [1.4184184306923555, -1.1631647901376114], [1.523023039103807, -1.2039559877282602], [1.6276276476902367, -1.2447471854259755], [1.7322322574540308, -1.2855383832455511], [1.8368368678020872, -1.3263295807603532], [1.9414414804893303, -1.3671207772037095], [2.0460460928727704, -1.4079119700587415], [2.1506507068224536, -1.4487031340436076], [2.2552553209208286, -1.4894940761620767], [2.3763538532619615, -1.5302833304045431], [2.5169820704991763, -1.5640423567118493], [2.6588986312589697, -1.590599694048955], [2.802157263554024, -1.6094890003961773], [2.946745980097297, -1.6188392333727741], [3.0926510404609737, -1.6168258471049157], [3.2399660177758185, -1.6031524604677643], [3.3890950758790965, -1.5778469405565188], [3.538224133912956, -1.5471948432545932], [3.6873531921562592, -1.5113045929184887], [3.838273647025577, -1.4696596104735302], [3.9911619615882983, -1.4224263924005378], [4.138356956264587, -1.3752862223280724], [4.279179144507105, -1.3282601627601158], [4.412919511142334, -1.2813722451015408], [4.538850239988183, -1.234649241114925], [4.656237295583855, -1.1881201922503186], [4.764354308794248, -1.1418156763259089], [4.862497074146294, -1.0957668182034042], [4.949997929468263, -1.0500040848154797]]
	# path_ac_even 	= []

	path_ac_late = [[1.0, -1.0], [1.1194269214124393, -1.0444691089750053], [1.2388538121907937, -1.0889382609049874], [1.358335075943671, -1.1333311698626165], [1.4776745216241616, -1.1779229391624515], [1.5968134795821893, -1.2227958366564216], [1.7157611222014633, -1.267937000601048], [1.8344794729026814, -1.3133996825849634], [1.9529178998239012, -1.359254879932503], [2.0710156690348978, -1.4055877553299156], [2.1887017167365066, -1.4524979424258928], [2.312780248661451, -1.4904443410479717], [2.432849589806713, -1.5340116024993344], [2.552577579989232, -1.5780500299001905], [2.6722114342460666, -1.6221721584600233], [2.7918490756855583, -1.6658286636005155], [2.9118687583341885, -1.7056875683650912], [3.031889006837823, -1.7283384705944524], [3.1678481539849837, -1.7144615842411128], [3.2970548907054384, -1.7083198140228684], [3.397388082063292, -1.7101603419356022], [3.5204142287106035, -1.7059171265743955], [3.669812951101358, -1.6910339444091693], [3.8192121809419044, -1.6839942877160445], [3.9770791358506075, -1.639623596546428], [4.143460059151906, -1.5650657009910374], [4.308454055449966, -1.4863994188548997], [4.452969286786023, -1.4112197732188634], [4.577454597533556, -1.3381388479894214], [4.6904793046355975, -1.2653515186626065], [4.790971755927245, -1.1929000170572408], [4.87796343850172, -1.121596830952051], [4.949994527926124, -1.050002305879155]]
	#3.4 too big
	# path_ac_late 	= [[1.0, -1.0], [1.086781556995945, -1.0597981063772544], [1.1735631119701266, -1.1195962138126607], [1.2603446576898247, -1.179394321504774], [1.3471262087731881, -1.2391924285105103], [1.4339077659063504, -1.2989905340368841], [1.5206893475080965, -1.3587886403559422], [1.6074709669820484, -1.418586747765364], [1.69425258790974, -1.478384863019949], [1.7810342058179303, -1.5381829837839895], [1.8678159342995235, -1.59798108829196], [1.9545978587760575, -1.6577790136630683], [2.04138130064509, -1.717575297713729], [2.128178180400818, -1.7773579819026168], [2.2149352638900703, -1.8371804271496657], [2.302438755326379, -1.8962552350754227], [2.393424689544049, -1.9518396280450114], [2.4824983599447847, -2.0028101266622214], [2.571572045835883, -2.045724171496415], [2.6606457255248883, -2.0794446570688554], [2.7672597653945217, -2.0972814773977104], [2.8974545879872178, -2.0896437103482417], [3.05654658352465, -2.0492395145744804], [3.2476397757215723, -1.9739264878556577], [3.4387329588293456, -1.8897474495069781], [3.641115388370548, -1.7938021222349403], [3.854383800247721, -1.6869781124653414], [4.061011945841415, -1.5802785840674904], [4.259872549603094, -1.4737307746824695], [4.449637780687244, -1.3673773549514272], [4.628892819283625, -1.2612678416690213], [4.7961705036671125, -1.1554573131949912], [4.949992213149034, -1.0500042724406358]] #[[1.0, -1.0], [1.1155610528044166, -1.0449419800694133], [1.2311221056088522, -1.089883960137855], [1.3466831584138101, -1.1348259402062784], [1.4622442112189942, -1.1797679202715134], [1.5778052640239693, -1.2247099003398991], [1.6933663168325874, -1.2696518804009294], [1.8089273696697086, -1.3145938604367515], [1.9244884227187136, -1.3595358402782047], [2.040049476711175, -1.404477818966083], [2.1556105307064435, -1.4494197922447756], [2.271171584700334, -1.4943617242671998], [2.392140724320988, -1.539303341828554], [2.5197058144727107, -1.5819400297569814], [2.6478476158078976, -1.622281393533219], [2.7787076746270714, -1.6576198507771223], [2.913089699143051, -1.6856639349909466], [3.0519122927660347, -1.7017209992580846], [3.1963017562500182, -1.701000189425513], [3.347797675435636, -1.68103591666814], [3.499293594638925, -1.6532585621683655], [3.6507895138553046, -1.6177900536298937], [3.8066450228469484, -1.5732182208759096], [3.9667295893490286, -1.5198848454401324], [4.119220680010847, -1.4666829451997199], [4.263162855176926, -1.413643334599462], [4.3975692676591835, -1.3608004359379442], [4.52144225939829, -1.3081914838203597], [4.633794049687735, -1.2558558625675258], [4.733671829268176, -1.203833447638358], [4.820184299761292, -1.1521622187416833], [4.892525290673444, -1.1008760042334644], [4.9499948329558805, -1.050002256277985]]

	path_ac_late = [[1.0, -1.0], [1.104557610366046, -1.045466081515962], [1.2091152171829789, -1.0909321622450465], [1.3136728256185843, -1.1363982435395539], [1.4182304236496792, -1.1818643255947423], [1.5227880216354095, -1.2273304050129867], [1.6273456372276731, -1.2727964820089661], [1.7319032734335922, -1.3182625605262615], [1.8364608868235872, -1.363728639993356], [1.9410185139688532, -1.409194713649154], [2.045576121661707, -1.454660778677534], [2.1501337935978184, -1.5001267738866795], [2.254691924803387, -1.5455922189029938], [2.3592535740600016, -1.5910537742900333], [2.4637979729622623, -1.6364965270087424], [2.56822887667904, -1.6817973360645797], [2.6722352601682924, -1.7260641509975407], [2.7762416770833918, -1.7660642894386736], [2.880248086309309, -1.7983744358032543], [2.9942067683966056, -1.8201513431326806], [3.1223081246171156, -1.8247594611361586], [3.268874513681005, -1.806289163689883], [3.4375242399583614, -1.760718601303196], [3.606173955906873, -1.7065349050471854], [3.782880373965034, -1.6416479341737191], [3.9675136882431645, -1.56661172216635], [4.141636308899243, -1.4920354827509783], [4.306572256423556, -1.4176416969292767], [4.46108960899179, -1.3434728392058737], [4.60392637233304, -1.269575615260555], [4.733823758451754, -1.195999264329687], [4.849561752438334, -1.1227930623903517], [4.949994461025674, -1.0500033378623543]]

	path_af_early 	= [] #[[1.0, -1.0], [1.1289989551532633, -1.0789910820553308], [1.2598852476617508, -1.1637746626107182], [1.3924402043360675, -1.2536548308372035], [1.5263993034477734, -1.3475768185551016], [1.661426437663688, -1.4440207603774926], [1.7970689951463892, -1.540902444515892], [1.9326879758613624, -1.635532286142856], [2.0674010842923605, -1.7247529100830445], [2.211119642643209, -1.8068015021945927], [2.361008341660447, -1.8822848506936167], [2.513786375148033, -1.9532476432455013], [2.6671369792566257, -2.0221177168767985], [2.8201829716228697, -2.090425478394405], [2.972828028351959, -2.158742656006296], [3.1251831262515406, -2.2271715309607827], [3.2773621363106704, -2.295678032598627], [3.429444343717742, -2.364219377875445], [3.581477825278265, -2.432771787935006], [3.733488645965968, -2.50132623686277], [3.8854895340655977, -2.569880388031892], [4.037486223262068, -2.638434010019115], [4.189481396416992, -2.706987276317515], [4.34147594034211, -2.7755403456757604], [4.493470217620229, -2.8440933212172252], [4.645464379134971, -2.9126462563217155], [4.79745848908949, -2.9811991761934817], [4.949452575517707, -3.0497520929267012]]
	path_af_late 	= []
	# path_af_even 	= []

	# AC_OBS-even
	obstacle_paths = {}
	obstacle_paths['AC_OBS-early'] 	= path_ac_early
	# obstacle_paths['AC_OBS-even']	= path_ac_even
	obstacle_paths['AC_OBS-late']	= path_ac_late
	obstacle_paths['AF_OBS-early'] 	= path_af_early
	# obstacle_paths['AF_OBS-even'] 	= path_af_even
	obstacle_paths['AF_OBS-late'] 	= path_af_late


	obstacle_paths['FD_OBS-early'] 	= horizontal_flip(vertical_flip(path_ac_early))
	# obstacle_paths['FD_OBS-even']	= horizontal_flip(vertical_flip(path_ac_even))
	obstacle_paths['FD_OBS-late']	= horizontal_flip(vertical_flip(path_ac_late))

	obstacle_paths['DF_OBS-early'] 	= vertical_flip(path_ac_early)
	# obstacle_paths['DF_OBS-even'] 	= vertical_flip(path_ac_even)
	obstacle_paths['DF_OBS-late'] 	= vertical_flip(path_ac_late)

	obstacle_paths['CA_OBS-early'] 	= horizontal_flip(path_ac_early)
	# obstacle_paths['CA_OBS-even']	= horizontal_flip(path_ac_even)
	obstacle_paths['CA_OBS-late']	= horizontal_flip(path_ac_late)

	obstacle_paths['CD_OBS-early'] 	= horizontal_flip(path_af_early)
	# obstacle_paths['CD_OBS-even'] 	= horizontal_flip(path_af_even)
	obstacle_paths['CD_OBS-late'] 	= horizontal_flip(path_af_late)

	obstacle_paths['FA_OBS-early'] 	= horizontal_flip(vertical_flip(path_af_early))
	# obstacle_paths['FA_OBS-even']	= horizontal_flip(vertical_flip(path_af_even))
	obstacle_paths['FA_OBS-late']	= horizontal_flip(vertical_flip(path_af_late))
	
	obstacle_paths['DC_OBS-early'] 	= vertical_flip(path_af_early)
	# obstacle_paths['DC_OBS-even'] 	= vertical_flip(path_af_even)
	obstacle_paths['DC_OBS-late'] 	= vertical_flip(path_af_late)


	obstacle_paths = add_offramps(obstacle_paths)
	print(obstacle_paths)


	# # generate_vanilla_straight_line_paths_for_testing(goal_b, [goal_a, goal_c, goal_d, goal_e, goal_f])
	# Return the name and the list
	return obstacle_paths

def get_straight_line_paths():
	# VANILLA PATHS
	# generate_vanilla_straight_line_paths_for_testing(goal_a, [goal_b, goal_c, goal_d, goal_e, goal_f])

	path_ab = [[1.0, -1.0], [1.25, -1.0], [1.5, -1.0], [1.75, -1.0], [2.0, -1.0], [2.25, -1.0], [2.5, -1.0], [2.75, -1.0], [3.0, -1.0]]
	path_ac = [[1.0, -1.0], [1.5, -1.0], [2.0, -1.0], [2.5, -1.0], [3.0, -1.0], [3.5, -1.0], [4.0, -1.0], [4.5, -1.0], [5.0, -1.0]]
	path_ad = [[1.0, -1.0], [1.0, -1.25], [1.0, -1.5], [1.0, -1.75], [1.0, -2.0], [1.0, -2.25], [1.0, -2.5], [1.0, -2.75], [1.0, -3.0]]
	path_ae = [[1.0, -1.0], [1.25, -1.25], [1.5, -1.5], [1.75, -1.75], [2.0, -2.0], [2.25, -2.25], [2.5, -2.5], [2.75, -2.75], [3.0, -3.0]]
	path_af = [[1.0, -1.0], [1.5, -1.25], [2.0, -1.5], [2.5, -1.75], [3.0, -2.0], [3.5, -2.25], [4.0, -2.5], [4.5, -2.75], [5.0, -3.0]]

	# # generate_vanilla_straight_line_paths_for_testing(goal_b, [goal_a, goal_c, goal_d, goal_e, goal_f])
	path_ba = [[3.0, -1.0], [2.75, -1.0], [2.5, -1.0], [2.25, -1.0], [2.0, -1.0], [1.75, -1.0], [1.5, -1.0], [1.25, -1.0], [1.0, -1.0]]
	path_bc = [[3.0, -1.0], [3.25, -1.0], [3.5, -1.0], [3.75, -1.0], [4.0, -1.0], [4.25, -1.0], [4.5, -1.0], [4.75, -1.0], [5.0, -1.0]]
	path_bd = [[3.0, -1.0], [2.75, -1.25], [2.5, -1.5], [2.25, -1.75], [2.0, -2.0], [1.75, -2.25], [1.5, -2.5], [1.25, -2.75], [1.0, -3.0]]
	path_be = [[3.0, -1.0], [3.0, -1.25], [3.0, -1.5], [3.0, -1.75], [3.0, -2.0], [3.0, -2.25], [3.0, -2.5], [3.0, -2.75], [3.0, -3.0]]
	path_bf = [[3.0, -1.0], [3.25, -1.25], [3.5, -1.5], [3.75, -1.75], [4.0, -2.0], [4.25, -2.25], [4.5, -2.5], [4.75, -2.75], [5.0, -3.0]]

	path_dict = {}
	path_dict['AB'] = path_ab
	path_dict['AC'] = path_ac
	path_dict['AD'] = path_ad
	path_dict['AE'] = path_ae
	path_dict['AF'] = path_af

	path_dict['BA'] = path_ba
	path_dict['BC'] = horizontal_flip(path_ba)
	path_dict['BD'] = path_bd
	path_dict['BE'] = path_be
	path_dict['BF'] = horizontal_flip(path_bd)


	path_dict = add_offramps(path_dict)

	# Return the name and the list
	return path_dict

def get_bigger_path_rectangle_1():
	path_ab = [[-1.0, -1.0], [-0.625, -1.0], [-0.25, -1.0], [0.125, -1.0], [0.5, -1.0], [0.875, -1.0], [1.25, -1.0], [1.625, -1.0], [2.0, -1.0]]
	path_ac = [[-1.0, -1.0], [-0.25, -1.0], [0.5, -1.0], [1.25, -1.0], [2.0, -1.0], [2.75, -1.0], [3.5, -1.0], [4.25, -1.0], [5.0, -1.0]]
	path_ad = [[-1.0, -1.0], [-1.0, -1.375], [-1.0, -1.75], [-1.0, -2.125], [-1.0, -2.5], [-1.0, -2.875], [-1.0, -3.25], [-1.0, -3.625], [-1.0, -4.0]]
	path_ae = [[-1.0, -1.0], [-0.625, -1.375], [-0.25, -1.75], [0.125, -2.125], [0.5, -2.5], [0.875, -2.875], [1.25, -3.25], [1.625, -3.625], [2.0, -4.0]]
	path_af = [[-1.0, -1.0], [-0.25, -1.375], [0.5, -1.75], [1.25, -2.125], [2.0, -2.5], [2.75, -2.875], [3.5, -3.25], [4.25, -3.625], [5.0, -4.0]]


	path_ba = [[-1.0, -1.0], [-0.625, -1.0], [-0.25, -1.0], [0.125, -1.0], [0.5, -1.0], [0.875, -1.0], [1.25, -1.0], [1.625, -1.0], [2.0, -1.0]]
	path_bc = [[-1.0, -1.0], [-0.25, -1.0], [0.5, -1.0], [1.25, -1.0], [2.0, -1.0], [2.75, -1.0], [3.5, -1.0], [4.25, -1.0], [5.0, -1.0]]
	path_bd = [[-1.0, -1.0], [-1.0, -1.375], [-1.0, -1.75], [-1.0, -2.125], [-1.0, -2.5], [-1.0, -2.875], [-1.0, -3.25], [-1.0, -3.625], [-1.0, -4.0]]
	path_be = [[-1.0, -1.0], [-0.625, -1.375], [-0.25, -1.75], [0.125, -2.125], [0.5, -2.5], [0.875, -2.875], [1.25, -3.25], [1.625, -3.625], [2.0, -4.0]]
	path_bf = [[-1.0, -1.0], [-0.25, -1.375], [0.5, -1.75], [1.25, -2.125], [2.0, -2.5], [2.75, -2.875], [3.5, -3.25], [4.25, -3.625], [5.0, -4.0]]

	path_dict = {}
	path_dict['AB'] = path_ab
	path_dict['AC'] = path_ab
	path_dict['AD'] = path_ab
	path_dict['AE'] = path_ab
	path_dict['AF'] = path_ab

	path_dict['BA'] = path_ab
	path_dict['BC'] = path_ab
	path_dict['BD'] = path_ab
	path_dict['BE'] = path_ab
	path_dict['BF'] = path_ab

	path_dict = add_offramps(path_dict)

	# Return the name and the list
	return path_dict


def get_curvey_line_paths_1():
	# # Curvier paths from my code!
	path_ab = [[ 1., -1.], [ 1.19634799, -0.9675439], [ 1.38696757, -0.9396737], [ 1.5707822,  -0.91637407], [ 1.74675563, -0.89764527], [ 1.91389779, -0.88350265], [ 2.07127019, -0.87397608], [ 2.21799127, -0.8691097], [ 2.35324126, -0.86896153], [ 2.47626681, -0.87360334], [ 2.58638522, -0.88312052], [ 2.68298828, -0.89761207], [ 2.76554573, -0.91719071], [ 2.83360823, -0.94198308], [ 2.88680999, -0.97212998], [ 2.92487083, -1.00778683], [ 2.94759786, -1.04912413], [3, -1]]
	path_ac = [[ 1., -1.], [ 1.11368137, -0.98140284], [ 1.22742351, -0.96473847 ], [ 1.34128619, -0.94994051 ], [ 1.45532829, -0.93694329 ], [ 1.56960782, -0.92568186 ], [ 1.68418199, -0.91609189 ], [ 1.79910729, -0.90810962 ], [ 1.91443952, -0.90167181 ], [ 2.03023386, -0.89671569 ], [ 2.14654492, -0.89317886 ], [ 2.26342682, -0.89099931 ], [ 2.38093322, -0.89011529 ], [ 2.4991174,  -0.8904653  ], [ 2.61803227, -0.89198803 ], [ 2.7377305,  -0.89462229 ], [ 2.8582645,  -0.89830697 ], [ 2.97968654, -0.90298098 ], [ 3.10204875, -0.9085832  ], [ 3.22540321, -0.91505241 ], [ 3.34980198, -0.92232728 ], [ 3.47529721, -0.93034626 ], [ 3.6019411,  -0.93904756 ], [ 3.72978607, -0.9483691  ], [ 3.8588847, -0.95824843 ], [ 3.98928989, -0.9686227 ], [ 4.12105485, -0.97942859], [ 4.25423316, -0.99060226], [ 4.38887889, -1.00207931], [ 4.52504655, -1.01379468], [ 4.66279127, -1.02568264], [ 4.80216877, -1.03767672], [ 4.94323543, -1.04970963], [ 5, -1.0]]
	path_ad = [[ 1., -1. ], [ 0.95770111, -1.19513019], [ 0.92008508, -1.38473945], [ 0.88723032, -1.56795431], [ 0.85922746, -1.74393511], [ 0.83617874, -1.91187987], [ 0.81819738, -2.07102781], [ 0.80540715, -2.2206629], [ 0.79794194, -2.36011707], [ 0.79594556, -2.48877333], [ 0.79957152, -2.60606861], [ 0.80898299, -2.71149643], [ 0.82435288, -2.80460929], [ 0.84586399, -2.88502087], [ 0.8737093,  -2.9524079 ], [ 0.90809241, -3.00651185], [ 0.94922806, -3.04714029], [1.0, -3.0]]
	path_ae = [[ 1., -1.], [ 1.09347342, -1.10510911], [ 1.18688026, -1.21006585], [ 1.28015412, -1.31471796], [ 1.37322898, -1.41891336], [ 1.46603937, -1.52250028], [ 1.55852057, -1.6253273], [ 1.65060881, -1.72724353], [ 1.74224141, -1.8280986], [ 1.83335703, -1.92774283], [ 1.92389582, -2.0260273], [ 2.0137996,  -2.12280391], [ 2.10301208, -2.21792547], [ 2.19147905, -2.31124581], [ 2.27914853, -2.40261982], [ 2.36597101, -2.49190354], [ 2.45189964, -2.57895424], [ 2.53689037, -2.66363043], [ 2.62090223, -2.74579199], [ 2.70389745, -2.82530018], [ 2.78584171, -2.90201769], [ 2.86670433, -2.97580869], [ 2.94645846, -3.04653886], [3.0, -3.0] ]
	path_af = [[ 1., -1.], [ 1.13715194, -1.07246145], [ 1.27471105, -1.14595233], [ 1.41271057, -1.22038189], [ 1.55118327, -1.29565933], [ 1.69016156, -1.37169368], [ 1.82967757, -1.44839377], [ 1.96976325, -1.52566822], [ 2.11045048, -1.60342532], [ 2.25177114, -1.68157301], [ 2.39375723, -1.76001884], [ 2.53644095, -1.83866991], [ 2.67985482, -1.91743279], [ 2.82403174, -1.9962135], [ 2.96900513, -2.07491745], [ 3.11480901, -2.15344937], [ 3.26147807, -2.23171329], [ 3.40904785, -2.30961243], [ 3.55755476, -2.38704919], [ 3.70703623, -2.46392508], [ 3.85753081, -2.54014065], [ 4.00907825, -2.61559544], [ 4.16171966, -2.69018793], [ 4.31549756, -2.76381544], [ 4.47045604, -2.83637411], [ 4.62664085, -2.90775882], [ 4.78409952, -2.9778631], [ 4.94288148, -3.0465791], [5.0, -3]]

	# Some paths from my code!
	path_ba = [[ 3.0, -1.], [ 2.78503369, -0.97566994], [ 2.57910723, -0.95427874], [ 2.38289537, -0.93598619], [ 2.1970388,  -0.92095047], [ 2.02214191, -0.90932796], [ 1.85877079, -0.90127304], [ 1.70745117, -0.89693786], [ 1.56866669, -0.89647229], [ 1.44285713, -0.90002378], [ 1.33041692, -0.90773734], [ 1.23169368, -0.91975548], [ 1.14698704, -0.9362183], [ 1.07654746, -0.95726354], [ 1.02057534, -0.98302671], [ 0.97922023, -1.01364127], [ 0.95258022, -1.04923881], [1.0, -1.0]]
	path_bc = [[ 3.0, -1.], [ 3.22435566, -0.97780816], [ 3.43920927, -0.9584987],  [ 3.64344306, -0.94217732], [ 3.83599603, -0.9289503],  [ 4.01586948, -0.918924],   [ 4.18213222, -0.91220442], [ 4.33392543, -0.9088968],  [ 4.47046715, -0.90910529], [ 4.59105635, -0.91293264], [ 4.69507665, -0.92047998], [ 4.78199952, -0.93184666], [ 4.85138711, -0.94713013], [ 4.90289453, -0.96642586], [ 4.93627171, -0.9898274], [ 4.95136477, -1.01742644], [ 4.94811684, -1.04931296], [5.0, -1.0]]
	path_bd = [[ 3.0, -1.], [ 2.88602503, -1.18844841], [ 2.77447803, -1.37347181], [ 2.6653771,  -1.55407742], [ 2.55873833, -1.72929519], [ 2.4545714,  -1.89818036], [ 2.35287536, -2.05981599], [ 2.25363443, -2.21331531], [ 2.15681376, -2.35782397], [ 2.06235533, -2.49252214], [ 1.97017368, -2.6166263], [ 1.88015175, -2.72939094], [ 1.79213653, -2.83010996], [ 1.70593474, -2.91811779], [ 1.62130831, -2.99279023], [ 1.53796972, -3.053545], [ 1.45557725, -3.09984193], [ 1.37372991, -3.13118278], [ 1.2919622,  -3.14711075], [ 1.20973854, -3.14720945], [ 1.12644735, -3.13110161], [ 1.04139482, -3.0984472], [ 0.95379814, -3.04894118], [1.0, -3.0]]
	path_be = [[ 3.0, -1.], [ 2.99576749, -1.21327589], [ 2.99156891, -1.41977882], [ 2.98743628, -1.61833005], [ 2.98339959, -1.80779615], [ 2.9794867,  -1.98709555], [ 2.97572318, -2.15520468], [ 2.97213222, -2.31116383], [ 2.96873454, -2.45408261], [ 2.9655482,  -2.58314505], [ 2.96258854, -2.69761425], [ 2.95986808, -2.79683658], [ 2.95739636, -2.88024544], [ 2.95517987, -2.94736445], [ 2.95322195, -2.99781021], [ 2.95152268, -3.03129447], [ 2.9500788,  -3.04762575], [3.0, -3.0]]
	path_bf = [[ 3.0, -1.], [ 3.05262313, -1.13295336], [ 3.10357956, -1.26325768], [ 3.15359442, -1.39067955], [ 3.20337847, -1.51499112], [ 3.25363395, -1.63597215], [ 3.30506041, -1.75341204], [ 3.35836053, -1.86711181], [ 3.41424601, -1.97688612], [ 3.47344346, -2.08256518], [ 3.53670065, -2.1839967], [ 3.60479279, -2.28104788], [ 3.67852922, -2.37360729], [ 3.7587604,  -2.46158692], [ 3.84638532, -2.54492408], [ 3.94235941, -2.6235835], [ 4.04770301, -2.69755936], [ 4.16351046, -2.76687743], [ 4.29095991, -2.83159729], [ 4.43132403, -2.89181456], [ 4.58598152, -2.94766335], [ 4.75642971, -2.99931873], [ 4.94429833, -3.04699939], [5.0, -3.0]]

	path_dict = {}
	path_dict['AB'] = path_ab
	path_dict['AC'] = path_ab
	path_dict['AD'] = path_ab
	path_dict['AE'] = path_ab
	path_dict['AF'] = path_ab

	path_dict['BA'] = path_ab
	path_dict['BC'] = path_ab
	path_dict['BD'] = path_ab
	path_dict['BE'] = path_ab
	path_dict['BF'] = path_ab

	path_dict = add_offramps(path_dict)

	# Return the name and the list
	return path_dict

def get_all_possible_path_names():
	all_paths = []
	targets = ['A', 'B', 'C', 'D', 'E', 'F']

	for i in targets:
		for j in targets:
			link = i + j

			if link not in all_paths and i != j:
				all_paths.append(link)

	return all_paths

def generate_vanilla_straight_line_paths_for_testing(start, goal_list):
	N = 8
	for end_state in goal_list:
		start_state = start
		crow_flies_vector = [end_state[0] - start_state[0], end_state[1] - start_state[1]]
		step_vector = [1.0 * crow_flies_vector[0] / N, 1.0 * crow_flies_vector[1] / N]

		path = [start_state]
		print("~~")
		prev_pt = path[0]
		for i in range(N):
			pt = [prev_pt[0] + step_vector[0], prev_pt[1] + step_vector[1]]
			path.append(pt)
			prev_pt = pt

		print(path)

def horizontal_flip(path):
	center_horiz = goal_b[0]

	new_path = []
	for p in path:
		offset = (center_horiz - p[0])
		new_x = center_horiz + (offset)

		new_p = [new_x, p[1]]
		new_path.append(new_p)


	return new_path

def vertical_flip(path):
	center_horiz = (goal_a[1] + goal_d[1]) / 2.0

	new_path = []
	for p in path:
		offset = (center_horiz - p[1])
		new_y = center_horiz + (offset)
		
		new_p = [p[0], new_y]
		new_path.append(new_p)


	return new_path


def quaternion_multiply(q0, q1):
    """
    Multiplies two quaternions.
    https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html

    Input
    :param q0: A 4 element array containing the first quaternion (q01, q11, q21, q31)
    :param q1: A 4 element array containing the second quaternion (q02, q12, q22, q32)

    Output
    :return: A 4 element array containing the final quaternion (q03,q13,q23,q33)

    """
    # Extract the values from q0
    w0 = q0[0]
    x0 = q0[1]
    y0 = q0[2]
    z0 = q0[3]

    # Extract the values from q1
    w1 = q1[0]
    x1 = q1[1]
    y1 = q1[2]
    z1 = q1[3]

    # Computer the product of the two quaternions, term by term
    q0q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
    q0q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
    q0q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
    q0q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1

    # Create a 4 element array containing the final quaternion
    final_quaternion = np.array([q0q1_w, q0q1_x, q0q1_y, q0q1_z])

    # Return a 4 element array containing the final quaternion (q02,q12,q22,q32)
    return final_quaternion

# generate_vanilla_straight_line_paths_for_testing(goal_a, [goal_b, goal_c, goal_d, goal_e, goal_f])
# generate_vanilla_straight_line_paths_for_testing(goal_b, [goal_a, goal_c, goal_d, goal_e, goal_f])

def setup_path_dict(path_title):
	all_paths = get_all_possible_path_names()
	# print(all_paths)
	# path_dict = {}
	# for name in all_paths:
	# 	path_dict[name] = None

	if path_title == 'null':
		path_dict = get_straight_line_paths()

	elif path_title == 'toptwo':
		path_dict = get_curvey_line_paths_1()

	elif path_title == 'bigger':
		path_dict = get_curvey_line_paths_1()

	elif path_title == 'early':
		path_dict = get_early_paths()

	elif path_title == 'late':
		path_dict = get_late_paths()

	elif path_title == 'even':
		path_dict = get_even_paths()
	
	elif path_title == 'obstacles_special':
		path_dict = get_obstacle_paths()

	elif path_title == 'obs':
		path_dict = get_obstacle_paths()
		return path_dict

	path_ab = path_dict['AB']
	path_ac = path_dict['AC']
	path_ad = path_dict['AD']
	path_ae = path_dict['AE']
	path_af = path_dict['AF']

	path_ba = path_dict['BA']
	path_bc = path_dict['BC']
	path_bd = path_dict['BD']
	path_be = path_dict['BE']
	path_bf = path_dict['BF']

	path_dict['CB'] = horizontal_flip(path_ab)
	path_dict['CA'] = horizontal_flip(path_ac)
	path_dict['EA'] = horizontal_flip(path_ae)
	path_dict['CD'] = horizontal_flip(path_af)
	path_dict['CE'] = horizontal_flip(path_ae)
	path_dict['CF'] = horizontal_flip(path_ad)

	path_dict['DA'] = vertical_flip(path_ad)
	path_dict['DB'] = vertical_flip(path_ae)
	path_dict['DC'] = vertical_flip(path_af)
	path_dict['DE'] = vertical_flip(path_ab)
	path_dict['DF'] = vertical_flip(path_ac)

	path_dict['EA'] = vertical_flip(path_bd)
	path_dict['EB'] = vertical_flip(path_be)
	path_dict['EC'] = vertical_flip(path_bf)
	path_dict['ED'] = vertical_flip(path_ba)
	path_dict['EF'] = vertical_flip(path_bc)

	path_dict['FA'] = horizontal_flip(vertical_flip(path_af))
	path_dict['FB'] = horizontal_flip(vertical_flip(path_ae))
	path_dict['FC'] = horizontal_flip(vertical_flip(path_ad))
	path_dict['FD'] = horizontal_flip(vertical_flip(path_ac))
	path_dict['FE'] = horizontal_flip(vertical_flip(path_ab))
	
	### VERIFY THAT ALL PATHS ARE COVERED
	todo = []
	for key in path_dict.keys():
		if path_dict[key] == None:
			todo.append(key)
	# print(todo)

	is_problem = False
	#### VERIFY ALL HAVE CORRECT START AND END
	for key in path_dict.keys():
		path = path_dict[key]

		start 	= state_dict[key[0]]
		end 	= state_dict[key[1]]

		if len(path) > 0:
			if path[0] != start:
				print("Broken in " + key + " bad start")
				is_problem = True

			if path[-1] != end:
				print("Broken in " + key + " bad end")
				is_problem = True
		else:
			print("No path yet for \n\t" + path_title + " -> " + key)

	if is_problem:
		print("Problem in path transformations")
	else:
		print("All paths added and checked for reasonableness!")

	return path_dict

def export_path_dict(export_name, path_dict):
	directory_name = "paths/"

	for key in path_dict.keys():
		path = path_dict[key]

		csv_content = ""
		# line format for ROS is 

		for i in range(1, len(path)):
			p0 = path[i - 1]
			p1 = path[i]

			prev_x, prev_y 	= p0[:2]
			curr_x, curr_y 	= p1[:2]
			prev_z, curr_z 	= 0, 	0

			q1_inv 	= [0, 0, 0, 0]
			q2 		= [0, 0, 0, 0]

			# Here's an example to get the relative rotation 
			# from the previous robot pose to the current robot pose: 
			# http://wiki.ros.org/tf2/Tutorials/Quaternions
			q1_inv[0] = prev_x
			q1_inv[1] = prev_y
			q1_inv[2] = 0 #prev_pose.pose.orientation.z
			q1_inv[3] = -1 #-prev_pose.pose.orientation.w # Negate for inverse

			q2[0] = curr_x
			q2[1] = curr_y
			q2[2] = 0 #current_pose.pose.orientation.z
			q2[3] = 1 #current_pose.pose.orientation.w
			
			qr = quaternion_multiply(q2, q1_inv)


			dX = curr_x - prev_x 
			dY = curr_y - prev_y
			dZ = 0 # since the robot doesn't float

			roll 	= 0
			yaw 	= np.arctan2(dY, dX)
			pitch 	= 0 #np.arctan2(np.sqrt(dZ * dZ + dX * dX), dY) + np.pi;

			# Create a rotation object from Euler angles specifying axes of rotation
			# (roll about an X-axis) / (subsequent pitch about the Y-axis) / (subsequent yaw about the Z-axis), 
			# rot = Rotation.from_euler('xyz', [0, 0, yaw], degrees=False)

			# # Convert to quaternions and print
			# rot_quat = rot.as_quat()

			x, y, z = prev_x, prev_y, 0
			qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
			qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
			qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
			qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
			# qx, qy, qz, qw = rot_quat

			csv_content += str(x) + ", " + str(y) + ", " + str(z) + ", " + str(qx) + ", " + str(qy) + ", " + str(qz) + ", " + str(qw) + "\n"

		x, y, z = curr_x, curr_y, 0
		csv_content += str(x) + ", " + str(y) + ", " + str(z) + ", " + str(qx) + ", " + str(qy) + ", " + str(qz) + ", " + str(qw) + "\n"

		filename = directory_name + key + "-" + export_name + ".csv"
		if export_name == 'obs':
			filename = directory_name + key + ".csv"

		f = open(filename, "w")
		f.write(csv_content)
		f.close()
		# print("wrote out " + filename)


def draw_paths_by_dict_demo_defense(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict):
	fig, axes = plt.subplot_mosaic("ABCD;IJKL", figsize=(12, 5), gridspec_kw={'width_ratios':[1, 1, 1, 1], 'height_ratios':[1, 1]})

	hues = sns.color_palette("Paired", 8)[:2] + sns.color_palette("Paired", 8)[6:]
	hues_distracted = sns.color_palette("Paired", 10)[8:]
	hues_early_late = [hues[1], hues[3]]

	color_early 	= hues_early_late[0]
	color_late 	= hues_early_late[1]

	ax_mappings = {}
	ax_early 	= axes['A']
	# ax_even 	= axes['E']
	ax_late 	= axes['I']

	ax_early2 	= axes['B']
	# ax_even2 	= axes['F']
	ax_late2 	= axes['J']

	ax_early3 	= axes['C']
	# ax_even3 	= axes['G']
	ax_late3 	= axes['K']

	ax_early4 	= axes['D']
	# ax_even4 	= axes['H']
	ax_late4 	= axes['L']

	buffer = 1
	ax_early.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early.set_aspect('equal')

	# ax_even.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even.set_aspect('equal')

	ax_late.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late.set_aspect('equal')

	ax_early2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early2.set_aspect('equal')

	# ax_even2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even2.set_aspect('equal')

	ax_late2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late2.set_aspect('equal')

	ax_early3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early3.set_aspect('equal')

	# ax_even3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even3.set_aspect('equal')

	ax_late3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late3.set_aspect('equal')

	ax_early4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early4.set_aspect('equal')

	# ax_even4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even4.set_aspect('equal')

	ax_late4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late4.set_aspect('equal')

	### -AB -AC -AD -AE -AF -BA -BC -BD -BE -BF -CA -CB -CD -CE -CF -DA DB DC -DE -DF -EA -EB -EC -ED -EF -FA -FB -FC -FD -FE

	title1 = "SHORT-OUTER / SHORT-SIDE"
	group1 = ['BA', 'BC', 'EF', 'ED', 'AD', 'FC'] # 'BD', 'BF', "AD", 'CF'

	title2 = "SHORT-INNER / SHORT-MID"
	group2 = ['AB', 'CB', 'FE', 'DE', 'BE'] # 'DA', 'FC', 'EA', 'EC', 

	title3 = "DIAG-INNER / DIAG-OUTER"
	group3 = ['AE', 'EC'] # FA, DC CD CA DF FD

	title4 = "LONG / LONG-DIAG"
	group4 = ['AF', 'AC']


	### boringly flat
	# 'BE', EB

	ax_early.set_title("Early\n " + title1, fontweight="bold")
	# ax_even.set_title("Even\n " + title1, fontweight="bold")
	ax_late.set_title("Late\n " + title1, fontweight="bold")

	ax_early2.set_title("Early\n " + title2, fontweight="bold")
	# ax_even2.set_title("Even\n " + title2, fontweight="bold")
	ax_late2.set_title("Late\n " + title2, fontweight="bold")

	ax_early3.set_title("Early\n " + title3, fontweight="bold")
	# ax_even3.set_title("Even\n " + title3, fontweight="bold")
	ax_late3.set_title("Late\n " + title3, fontweight="bold")

	ax_early4.set_title("Early\n " + title4, fontweight="bold")
	# ax_even4.set_title("Even\n " + title4, fontweight="bold")
	ax_late4.set_title("Late\n " + title4, fontweight="bold")

	for j in range(len(goal_list)):
		goal 	= goal_list[j]
		color = goal_colors[j]

		circle1 = plt.Circle(goal, .1, color=color)
		ax_early.add_patch(circle1)

		circle2 = plt.Circle(goal, .1, color=color)
		ax_late.add_patch(circle2)
		
		# circle3 = plt.Circle(goal, .1, color=color)
		# ax_even.add_patch(circle3)

		circle4 = plt.Circle(goal, .1, color=color)
		ax_early2.add_patch(circle4)

		circle5 = plt.Circle(goal, .1, color=color)
		ax_late2.add_patch(circle5)
		
		# circle6 = plt.Circle(goal, .1, color=color)
		# ax_even2.add_patch(circle6)

		circle7 = plt.Circle(goal, .1, color=color)
		ax_early3.add_patch(circle7)

		circle8 = plt.Circle(goal, .1, color=color)
		ax_late3.add_patch(circle8)
		
		# circle9 = plt.Circle(goal, .1, color=color)
		# ax_even3.add_patch(circle9)

		circle10 = plt.Circle(goal, .1, color=color)
		ax_early4.add_patch(circle10)

		circle11 = plt.Circle(goal, .1, color=color)
		ax_late4.add_patch(circle11)
		
		# circle12 = plt.Circle(goal, .1, color=color)
		# ax_even4.add_patch(circle12)


	dotsize = 2
	for key in early_dict.keys():
		path_early 	= early_dict[key]
		path_late 	= late_dict[key]
		# path_even 	= even_dict[key]

		early_x, early_y 	= get_xy_from_path(path_early)
		late_x, late_y 		= get_xy_from_path(path_late)
		# even_x, even_y 		= get_xy_from_path(path_even)


		if key in group1:
			ax_early.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late.plot(late_x, 	late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even.plot(even_x, 	even_y, 	label = "even", color='blue')

			ax_early.scatter(early_x, 	early_y, 	c=color_early, 	s=dotsize,	zorder=10)
			ax_late.scatter(late_x, 	late_y, 	c=color_late, 	s=dotsize,	zorder=10)


		if key in group2:
			ax_early2.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late2.plot(late_x, 		late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even2.plot(even_x, 	even_y, 	label = "even", color='blue')

			ax_early2.scatter(early_x, 	early_y, 	c=color_early, 	s=dotsize,	zorder=10)
			ax_late2.scatter(late_x, 	late_y, 	c=color_late, 	s=dotsize,	zorder=10)

		if key in group3:
			ax_early3.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late3.plot(late_x, 		late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even3.plot(even_x, 	even_y, 	label = "even", color='blue')

			ax_early3.scatter(early_x, 	early_y, 	c=color_early, 	s=dotsize,	zorder=10)
			ax_late3.scatter(late_x, 	late_y, 	c=color_late, 	s=dotsize,	zorder=10)

		if key in group4:
			ax_early4.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late4.plot(late_x, 		late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even4.plot(even_x, 	even_y, 	label = "even", color='blue')

			ax_early4.scatter(early_x, 	early_y, 	c=color_early, 	s=dotsize,	zorder=10)
			ax_late4.scatter(late_x, 	late_y, 	c=color_late, 	s=dotsize,	zorder=10)
		 

	# show a legend on the plot
	# plt.legend() #loc='upper center', bbox_to_anchor=(0.5, 1.05), ncol=3, fancybox=True, shadow=True)
	 
	# function to show the plot
	plt.tight_layout()
	plt.savefig(inspection_save_path + "overview-defense-all" + '.png')
	plt.clf()
	plt.close()
	
	print("Exported images of all paths")

def draw_paths_by_dict_defense(inspection_save_path, early_dict, late_dict, even_dict, obstacle_dict):
	fig, axes = plt.subplot_mosaic("ABC;IJK", figsize=(8, 6), gridspec_kw={'width_ratios':[1, 1, 1], 'height_ratios':[1, 1]})

	hues = sns.color_palette("Paired", 8)[:2] + sns.color_palette("Paired", 8)[6:]
	hues_distracted = sns.color_palette("Paired", 10)[8:]
	hues_early_late = [hues[1], hues[3]]

	color_early 	= hues_early_late[0]
	color_late 	= hues_early_late[1]

	ax_mappings = {}
	ax_early 	= axes['A']
	# ax_even 	= axes['E']
	ax_late 	= axes['I']

	ax_early2 	= axes['B']
	# ax_even2 	= axes['F']
	ax_late2 	= axes['J']

	ax_early3 	= axes['C']
	# ax_even3 	= axes['G']
	ax_late3 	= axes['K']

	if False:
		ax_early4 	= axes['D']
		# ax_even4 	= axes['H']
		ax_late4 	= axes['L']

	buffer = 1
	ax_early.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early.set_aspect('equal')

	# ax_even.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even.set_aspect('equal')

	ax_late.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late.set_aspect('equal')

	ax_early2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early2.set_aspect('equal')

	# ax_even2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even2.set_aspect('equal')

	ax_late2.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late2.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late2.set_aspect('equal')

	ax_early3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_early3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_early3.set_aspect('equal')

	# ax_even3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	# ax_even3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	# ax_even3.set_aspect('equal')

	ax_late3.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
	ax_late3.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
	ax_late3.set_aspect('equal')

	if False:
		ax_early4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
		ax_early4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
		ax_early4.set_aspect('equal')

		# ax_even4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
		# ax_even4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
		# ax_even4.set_aspect('equal')

		ax_late4.set_xlim([goal_a[0] - buffer, goal_c[0] + buffer])
		ax_late4.set_ylim([goal_d[1] - buffer, goal_a[1] + buffer])
		ax_late4.set_aspect('equal')

	### -AB -AC -AD -AE -AF -BA -BC -BD -BE -BF -CA -CB -CD -CE -CF -DA DB DC -DE -DF -EA -EB -EC -ED -EF -FA -FB -FC -FD -FE

	title1 = "Outwards / Down"
	group1 = ['BA', 'BC', 'BD', 'BF', 'EF', 'ED', "AD", 'CF']

	title2 = "Inwards / Up"
	group2 = ['AB', 'CB', 'DA', 'FC', 'EA', 'EC', 'FE', 'DE', 'BE']

	title3 = "Long / Diagonal"
	group3 = ['AF', 'AC', 'AE'] # FA, DC CD CA DF FD

	# title4 = "Short Diagonal"
	# group4 = ['AE', 'CE', 'DB', 'FB']

	if False:
		title4 = "Obstacle Repulsion"
		group4 = ['AC-obs', 'AF-obs']

	### boringly flat
	# 'BE', EB

	ax_early.set_title("Early\n " + title1, fontweight="bold")
	# ax_even.set_title("Even\n " + title1, fontweight="bold")
	ax_late.set_title("Late\n " + title1, fontweight="bold")

	ax_early2.set_title("Early\n " + title2, fontweight="bold")
	# ax_even2.set_title("Even\n " + title2, fontweight="bold")
	ax_late2.set_title("Late\n " + title2, fontweight="bold")

	ax_early3.set_title("Early\n " + title3, fontweight="bold")
	# ax_even3.set_title("Even\n " + title3, fontweight="bold")
	ax_late3.set_title("Late\n " + title3, fontweight="bold")

	if False:
		ax_early4.set_title("Early\n " + title4, fontweight="bold")
		# ax_even4.set_title("Even\n " + title4, fontweight="bold")
		ax_late4.set_title("Late\n " + title4, fontweight="bold")

	for j in range(len(goal_list)):
		goal 	= goal_list[j]
		color = goal_colors[j]

		circle1 = plt.Circle(goal, .1, color=color)
		ax_early.add_patch(circle1)

		circle2 = plt.Circle(goal, .1, color=color)
		ax_late.add_patch(circle2)
		
		# circle3 = plt.Circle(goal, .1, color=color)
		# ax_even.add_patch(circle3)

		circle4 = plt.Circle(goal, .1, color=color)
		ax_early2.add_patch(circle4)

		circle5 = plt.Circle(goal, .1, color=color)
		ax_late2.add_patch(circle5)
		
		# circle6 = plt.Circle(goal, .1, color=color)
		# ax_even2.add_patch(circle6)

		circle7 = plt.Circle(goal, .1, color=color)
		ax_early3.add_patch(circle7)

		circle8 = plt.Circle(goal, .1, color=color)
		ax_late3.add_patch(circle8)
		
		# circle9 = plt.Circle(goal, .1, color=color)
		# ax_even3.add_patch(circle9)

		if False:
			circle10 = plt.Circle(goal, .1, color=color)
			ax_early4.add_patch(circle10)

			circle11 = plt.Circle(goal, .1, color=color)
			ax_late4.add_patch(circle11)
			
			# circle12 = plt.Circle(goal, .1, color=color)
			# ax_even4.add_patch(circle12)


	for key in early_dict.keys():
		path_early 	= early_dict[key]
		path_late 	= late_dict[key]
		# path_even 	= even_dict[key]

		early_x, early_y 	= get_xy_from_path(path_early)
		late_x, late_y 		= get_xy_from_path(path_late)
		# even_x, even_y 		= get_xy_from_path(path_even)


		if key in group1:
			ax_early.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late.plot(late_x, 	late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even.plot(even_x, 	even_y, 	label = "even", color='blue')

			# ax_early.scatter(early_x, 	early_y, c='red', s=8,zorder=10)
			# ax_late.scatter(late_x, 	late_y, c='blue', s=8,zorder=10)


		elif key in group2:
			ax_early2.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late2.plot(late_x, 		late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even2.plot(even_x, 	even_y, 	label = "even", color='blue')

		elif key in group3:
			ax_early3.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late3.plot(late_x, 		late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even3.plot(even_x, 	even_y, 	label = "even", color='blue')

		elif False and key in group4:
			ax_early4.plot(early_x, 	early_y, 	 label = "early", color=color_early, markersize=0, linestyle='solid')
			ax_late4.plot(late_x, 		late_y, 	 label = "late", color=color_late, markersize=0, linestyle='solid')
			# ax_even4.plot(even_x, 	even_y, 	label = "even", color='blue')
		 
		# plt.title('Path options for ' + key)
			 
	if False:
		##### Add the obstacle paths
		AC_early 	= obstacle_dict['AC_OBS-early']
		AC_late 	= obstacle_dict['AC_OBS-late']
		# AC_even 	= obstacle_dict['AC_OBS-even']

		AF_early 	= obstacle_dict['AF_OBS-early']
		AF_late 	= obstacle_dict['AF_OBS-late']
		# AF_even 	= obstacle_dict['AF_OBS-even']


		AC_early_x, AC_early_y 		= get_xy_from_path(AC_early)
		AC_late_x, AC_late_y 		= get_xy_from_path(AC_late)
		# AC_even_x, AC_even_y 		= get_xy_from_path(AC_even)

		AF_early_x, AF_early_y 		= get_xy_from_path(AF_early)
		AF_late_x, AF_late_y 		= get_xy_from_path(AF_late)
		# AF_even_x, AF_even_y 		= get_xy_from_path(AF_even)


		ax_early4.plot(AC_early_x, 	AC_early_y, 	label = "early", color='red')
		ax_late4.plot(AC_late_x, 	AC_late_y, 	label = "late", color='green')
		# ax_even4.plot(AC_even_x, 	AC_even_y, 	label = "even", color='blue')


		ax_early4.plot(AF_early_x, 	AF_early_y, 	label = "early", color='red')
		ax_late4.plot(AF_late_x, 	AF_late_y, 	label = "late", color='green')
		# ax_even4.plot(AF_even_x, 	AF_even_y, 	label = "even", color='blue')


	# show a legend on the plot
	# plt.legend() #loc='upper center', bbox_to_anchor=(0.5, 1.05), ncol=3, fancybox=True, shadow=True)
	 
	# function to show the plot
	plt.tight_layout()
	plt.savefig(inspection_save_path + "overview-defense" + '.png')
	plt.clf()
	plt.close()
	
	print("Exported images of all paths")


path_dict = setup_path_dict("null")
export_path_dict('null', path_dict)

path_dict = setup_path_dict("obs")
export_path_dict('obs', path_dict)


# generate_vanilla_straight_line_paths_for_testing(goal_a, [goal_b, goal_c, goal_d, goal_e, goal_f])

# path_dict = setup_path_dict('bigger')
# export_name = 'bigger'

# path_dict1 = setup_path_dict('null')
# path_dict2 = setup_path_dict('toptwo')

if False:
	export_path_dict(export_name, path_dict)
	print("All exported to paths/")


inspect_path_set()