#!/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.01
END = 600.0

fig, ax1 = plt.subplots(1, 1)
# fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 7))
ax1.set_xlim((-.45, .45))
ax1.set_ylim((-.35, .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")

cbd = AGVVirtual("AGVVirtual", path_file="paths/falcon.csv", initial=[0, -.3, 0], gamma=-0.06, v=0.05)
# cbd = AGVVirtual("AGVVirtual", initial=[0, -0.3, PI], Kp=Kp, Ki=Ki, Kd=Kd)
# 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', lw=5, label="environment")

plot = cbd.findBlock("plot")[0]
manager = PlotManager()
manager.register("plot", plot, (fig, ax1), LinePlot())
heading = Arrow(0.03, 0.02, color="blue")
manager.register("heading", cbd.findBlock("headingPlot")[0], (fig, ax1), heading)
# manager.register("error", cbd.findBlock("errorPlot")[0], (fig, ax2), LinePlot())
# 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=.01)
#sim.setProgressBar()
sim.setDeltaT(DELTA_T)
sim.connect("finished", lambda: manager.stop())
sim.run(END)

# fig.tight_layout()
# ax1.legend(loc='best')
ax1.set_title("AGV (PID); Kp = 0.02, Ki = 0.0, Kd = 0.01")
plt.show()

