#!/usr/bin/python3
# This file was automatically generated from drawio2cbd with the command:
#   /home/red/git/DrawioConvert/__main__.py -F CBD -e AGVVirtual -E delta=0.1 -aSgrvs -t 100 AGV.drawio

from ev3 import _PI as PI
from ev3 import *
from env import PathBlock
from AGV import *
from CBD.simulator import Simulator
import matplotlib.pyplot as plt
from CBD.realtime.plotting import PlotManager, LinePlot, Arrow, follow
import numpy as np

DELTA_T = 0.2
END = 600.0

# fig, ax1 = plt.subplots(1, 1)
# fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 7))
# ax1.set_xlim((-0.45, 0.45))
# ax1.set_ylim((-0.35, 0.55))
# ax1.set_xlim((0, 100))
# ax1.set_ylim((-0.07, 0.1))
# ax2.set_xlim((0, END))
# ax2.set_ylim((-3, -2.7))

# ax1.set_title("Trajectory")
# ax2.set_title("Accumulated Offset")

file = open('tuning9.log', 'a')
i = 300
for Ki in np.arange(0.0, -0.06, -0.01):
	for Kd in np.arange(0.0, -0.06, -0.01):
		for Kp in np.arange(0.0, -0.06, -0.01):
			# cbd = AGVVirtual("AGVVirtual", initial=[0, -0.3, PI], Kp=0.04, Ki=0.0, Kd=-0.04)
			cbd = AGVVirtual("AGVVirtual", initial=[0, -0.3, 0], Kp=Kp, Ki=Ki, Kd=Kd, v=0.01, T=35,
			                 wheel_radius=0.028, wheel_axis=0.12)
			# cbd = Test("Test")

			# env = cbd.findBlock("environment")[0]
			# path = env.path
			# ax1.plot([e[0] for e in path], [e[1] for e in path], color='lightcoral', linewidth=3, label="environment")
			#
			# plot = cbd.findBlock("plot")[0]
			# manager = PlotManager()
			# manager.register("plot", plot, (fig, ax1), LinePlot())
			# heading = Arrow(0.05, 0.02, color="blue")
			# manager.register("heading", cbd.findBlock("headingPlot")[0], (fig, ax1), heading)
			# manager.register("error", cbd.findBlock("errorPlot")[0], (fig, ax2), LinePlot(label="AGV"))
			# # manager.connect('plot', 'update', lambda d, axis=ax1: axis.set_xlim((min(d[0]), max(d[0]))))
			# # manager.connect('plot', 'update', lambda d, axis=ax1: axis.set_ylim((min(d[1]), max(d[1]))))
			# # manager.connect('heading', 'update', lambda d, axis=ax2: axis.set_xlim(follow(d[0], 10.0, lower_bound=0.0)))
			# manager.connect('error', 'update', lambda d, axis=ax2: axis.set_ylim((min(d[1]), max(d[1]))))
			#
			# def update_arrow(_):
			# 	if len(plot.data) > 0:
			# 		heading.position = plot.data[-1]
			#
			# manager.connect("heading", 'preupdate', update_arrow)

			# Run the Simulation
			sim = Simulator(cbd)
			# sim.setRealTime(scale=0.005)
			# sim.setProgressBar()
			sim.setDeltaT(DELTA_T)
			# sim.connect("finished", lambda: manager.stop())
			sim.run(END)

			# fig.tight_layout()
			# ax1.legend(loc='best')
			# plt.show()

			dist = cbd.findBlock("plot")[0].distance_from_start()
			if dist > 0:
				error = cbd.findBlock("errorPlot")[0].data_xy[1][-1] / dist
			else:
				error = float('inf')

			# print("error:", error)

			txt = "Kp = {:.2f}, Ki = {:.2f}, Kd = {:.2f}; error = {:.6f}".format(Kp, Ki, Kd, error)

			file.write("{}\n".format(txt))
			file.flush()
			# print(cbd.findBlock("Controller")[0].getSignal("velocity"))
			# print(cbd.findBlock("plot")[0].data_xy)


			fig, ax1 = plt.subplots(1, 1)
			ax1.set_xlim((-0.45, 0.45))
			ax1.set_ylim((-0.35, 0.55))

			env = cbd.findBlock("environment")[0]
			path = env.path
			traj = cbd.findBlock("plot")[0]
			ax1.plot([e[0] for e in path], [e[1] for e in path], color='red', linewidth=3)
			ax1.plot(*traj.data_xy, color='blue')
			ax1.set_title(txt)

			plt.savefig("tuning9/image-{:05d}".format(i))
			plt.close()

			print("{}:\t {}".format(i, txt))

			i += 1

			# # plt.show(block=False)
			# # plt.pause(1)
			# # plt.close()
