import numpy as np
from matplotlib.pyplot import plot, figure, subplot, grid, show
from components.pid.kalman import Kalman


if __name__ == '__main__':
	dt = 1.0

	params = {
		'var_P'	: 1,
		'var_Q'	: 0.0001,
		'var_R'	: 0.5
	}
	k = Kalman(dt, params)

	_x1 = np.empty(0)
	_y1 = np.empty(0)
	_x2 = np.empty(0)
	_y2 = np.empty(0)

	N = int(1000/dt)
	seqn = range(0, N)

	_seqn = range(0, 2*N)
	X = np.array([dt, 0])
	for n in seqn:
		Z = k.process_measurement(X, 0.5)
		# Kalman.print("Z:", Z)
		Xp = k.process(Z)

		_x1 = np.append(_x1, Z[0])
		_x2 = np.append(_x2, Z[1])
		_y1 = np.append(_y1, Xp[0])
		_y2 = np.append(_y2, Xp[1])

	for n in seqn:
		X[0] = X[0] + dt
		Z = k.process_measurement(X, 0.5)
		# Kalman.print("Z:", Z)
		Xp = k.process(Z)

		_x1 = np.append(_x1, Z[0])
		_x2 = np.append(_x2, Z[1])
		_y1 = np.append(_y1, Xp[0])
		_y2 = np.append(_y2, Xp[1])

	figure(1)
	subplot(2, 1, 1)
	plot(_seqn, _x1, 'bx', _seqn, _y1, '-r', linewidth=1)
	grid(True)
	subplot(2, 1, 2)
	plot(_seqn, _x2, 'bx', _seqn, _y2, '-r', linewidth=1)
	grid(True)
	show()

	print("End of program")