from AGV import *
from CBD.simulator import Simulator
from CBD.realtime.plotting import PlotManager, LinePlot
import matplotlib.pyplot as plt
from itertools import cycle
import pandas as pd

DELTA_T = 0.2
END = 650.0

if __name__ == '__main__':
	fig = plt.figure(figsize=(5, 5), dpi=100)
	ax = fig.add_subplot(111)
	ax.set_xlim((-.45, .45))
	ax.set_ylim((-.35, .55))

	lines = ["-","--","-.",":"]
	linecycler = cycle(lines)

	model = AGVVirtual("AGV", path_file="paths/falcon.csv", Kp=0.0, Ki=0.0, Kd=-0.02, v=0.001, T=35,
	                   wheel_radius=0.016512, wheel_axis=0.1944, initial=[0, -.3, 0])

	traj = model.findBlock("environment")[0].path
	ax.plot([x for x, _ in traj], [y for _, y in traj], c='salmon', label="path")

	sets = {
		"012": -0.02,
		"072": -0.12,
		"084": -0.14,
		"102": -0.17
	}
	for d, Kd in sets.items():
		data = pd.read_csv(f"tuning6/path-00{d}.csv")
		ax.plot(data["x"], data["y"], next(linecycler), lw=2, label="Kd = " + str(Kd))
	# data012 = pd.read_csv("tuning6/path-00012.csv")
	# data072 = pd.read_csv("tuning6/path-00072.csv")
	# data082 = pd.read_csv("tuning6/path-00084.csv")
	# data102 = pd.read_csv("tuning6/path-00102.csv")

	# print(data012)

# manager = PlotManager()
# manager.register("plot", model.findBlock("plot")[0], (fig, ax), LinePlot())
#
# sim = Simulator(model)
# sim.setDeltaT(DELTA_T)
# # sim.setProgressBar()
# # sim.setRealTimePlatformGameLoop()
# sim.setRealTime(scale=.01)
# sim.run(END)
#
	fig.tight_layout()
	ax.legend(loc="upper left")
	plt.show()