#!/usr/bin/python3
# This file was automatically generated from drawio2cbd with the command:
#   ../../drawio2cbd.py Harmonic.xml CBDA.py -e ErrorA -t 1000 -T 0.01

from pyCBD.Core import CBD
from pyCBD.simulator import Simulator
from pyCBD.lib.std import *
import matplotlib.pyplot as plt


def plot_signals(block, signals, title):
	times = []
	outputs = []

	for signal in signals:
		tvpl = block.getSignalHistory(signal)
		times = [t for t, _ in tvpl]
		outputs.append([v for _, v in tvpl])

	# Plot
	plt.figure()
	plt.title(title)
	plt.xlabel('time')
	plt.ylabel('N')
	for i in range(len(signals)):
		plt.plot(times, outputs[i], label=signals[i])
	plt.legend()
	plt.show()


class SinBlock(CBD):
	def __init__(self, block_name):
		CBD.__init__(self, block_name, input_ports=[], output_ports=['OUT1'])

		# Create the blocks
		self.addBlock(TimeBlock(block_name='time'))
		self.addBlock(GenericBlock(block_name='sin', block_operator=("sin")))

		# Connect the blocks
		self.addConnection('time', 'sin')
		self.addConnection('sin', 'OUT1')


class HarmonicA(CBD):
	def __init__(self, block_name):
		CBD.__init__(self, block_name, output_ports=['x', 'v'])

		# Create the blocks
		self.addBlock(IntegratorBlock(block_name='int2'))
		self.addBlock(IntegratorBlock(block_name='int1'))
		self.addBlock(ConstantBlock(block_name='zero', value=(0)))
		self.addBlock(ConstantBlock(block_name='one', value=(1)))
		self.addBlock(NegatorBlock(block_name='neg'))

		# Connect the blocks
		self.addConnection('zero', 'int2', input_port_name='IC')
		self.addConnection('one', 'int1', input_port_name='IC')
		self.addConnection('int2', 'x')
		self.addConnection('int2', 'neg')
		self.addConnection('neg', 'int1')
		self.addConnection('int1', 'int2')
		self.addConnection('int1', 'v')


class ErrorA(CBD):
	def __init__(self, block_name):
		CBD.__init__(self, block_name, input_ports=[], output_ports=['e', 'real', 'out'])

		# Create the blocks
		self.addBlock(SinBlock(block_name='sin'))
		self.addBlock(NegatorBlock(block_name='neg'))
		self.addBlock(AdderBlock(block_name='sum'))
		self.addBlock(AbsBlock(block_name='abs'))
		self.addBlock(IntegratorBlock(block_name='int'))
		self.addBlock(ConstantBlock(block_name='zero', value=(0)))
		self.addBlock(HarmonicA(block_name='harmonic'))

		# Connect the blocks
		self.addConnection('sin', 'sum')
		self.addConnection('neg', 'sum')
		self.addConnection('sum', 'abs')
		self.addConnection('zero', 'int', input_port_name='IC')
		self.addConnection('abs', 'int')
		self.addConnection('int', 'e')
		self.addConnection('harmonic', 'neg', output_port_name='x')
		self.addConnection('sin', 'real')
		self.addConnection('harmonic', 'out', output_port_name='x')


class HarmonicB(CBD):
	def __init__(self, block_name):
		CBD.__init__(self, block_name, input_ports=[], output_ports=['x'])
		
		# Create the blocks
		self.addBlock(DerivatorBlock(block_name='deriv1'))
		self.addBlock(DerivatorBlock(block_name='deriv2'))
		self.addBlock(ConstantBlock(block_name='one', value=(1)))
		self.addBlock(ConstantBlock(block_name='zero', value=(0)))
		self.addBlock(NegatorBlock(block_name='neg'))
		
		# Connect the blocks
		self.addConnection('one', 'deriv1', input_port_name='IC')
		self.addConnection('zero', 'deriv2', input_port_name='IC')
		self.addConnection('deriv1', 'deriv2')
		self.addConnection('deriv2', 'neg')
		self.addConnection('neg', 'deriv1')
		self.addConnection('neg', 'x')


class ErrorB(CBD):
	def __init__(self, block_name):
		CBD.__init__(self, block_name, input_ports=[], output_ports=['e', 'out', 'real'])
		
		# Create the blocks
		self.addBlock(SinBlock(block_name='sin'))
		self.addBlock(NegatorBlock(block_name='neg'))
		self.addBlock(AdderBlock(block_name='sum'))
		self.addBlock(AbsBlock(block_name='abs'))
		self.addBlock(IntegratorBlock(block_name='int'))
		self.addBlock(ConstantBlock(block_name='zero', value=(0)))
		self.addBlock(HarmonicB(block_name='harmonic'))
		
		# Connect the blocks
		self.addConnection('sin', 'sum')
		self.addConnection('neg', 'sum')
		self.addConnection('sum', 'abs')
		self.addConnection('zero', 'int', input_port_name='IC')
		self.addConnection('abs', 'int')
		self.addConnection('int', 'e')
		self.addConnection('harmonic', 'neg', output_port_name='x')
		self.addConnection('harmonic', 'out', output_port_name='x')
		self.addConnection('sin', 'real')


if __name__ == '__main__':
	from pyCBD.converters.latexify import CBD2Latex
	import numpy as np

	errors = []
	signals = []
	dts = reversed([0.5, 0.1, 0.01])
	TIME = 8.5
	shapes = ['.', 'x', '+']
	print(" starting plotter")
	plt.figure(figsize=(5, 5))
	plt.xlabel('x')
	plt.ylabel('dx/dt')
	plt.xlim((-4, 4))
	plt.ylim((-4, 4))
	for i, dt in enumerate(dts):
		print("DT:", dt)
		signals.append(str(dt))

		outputs = []
		for cbd in [HarmonicA("DTHO")]:
			cbd2latex = CBD2Latex(cbd, show_steps=True, render_latex=True, delta_t="", time_variable='j')
			cbd2latex.simplify()
			raise 4

			# Run the simulation
			sim = Simulator(cbd)
			sim.setDeltaT(dt)
			sim.run(TIME)
			# errors.append(cbd.getSignalHistory("e"))
			outputs.append(cbd.getSignalHistory("x"))
			outputs.append(cbd.getSignalHistory("v"))
			# real = cbd.getSignalHistory('real')


		# plt.title(f"Integrator vs Derivator (delta = {dt})")

		A, B = outputs
		plt.plot([x for _, x in A], [x for _, x in B], shapes[i], label=f"{dt}")
		# plt.plot([t for t, _ in B], [x for _, x in B], label="derivators")

	# ts = np.arange(0, 10, 0.01)
	# plt.plot(ts, np.sin(ts), label="sine")

	times = np.arange(0, TIME, 0.01)
	plt.plot(np.sin(times), np.cos(times), label="analytical", c='r', lw=1)

	plt.tight_layout()
	plt.legend(loc='upper right')
	plt.show()

	# plt.figure()
	# plt.title("Errors")
	# plt.xlabel('time')
	# plt.ylabel('N')
	# for i in range(len(dts)):
	# 	for j in range(len(dts)):
	# 		time = [x for x, _ in errors[(i*2)+j]]
	# 		value = [x for _, x in errors[(i*2)+j]]
	# 		plt.plot(time, value, label=("delta = " + signals[i] + "; " + "AB"[j]))
	# plt.yscale("log")
	# plt.legend()
	# plt.show()

