import numpy as np
from matplotlib.pyplot import plot, figure, subplot, grid, show, legend
from components.plant.pot import Pot
from components.pid.temp_controller_smith import TempController


if __name__ == '__main__':
	ctrl_params = {
		"Outer": {
			"kp": 0.4,
			"ki": 0.0,
			"kd": 0.0,
			"kt": 0.0,
			"y_hold_min": -0.1
		},
		"Inner": {
			"Heat": {
				"kp": 0.08,
				"ki": 0.008,
				"kd": 0.0,
				"kt": 1.5
			},
			"Hold": {
				"kp": 0.08,
				"ki": 0.008,
				"kd": 0.0,
				"kt": 1.5,
				"yi_max": 0.3
			},
			"Cool": {
				"kp": 0.08,
				"ki": 0.008,
				"kd": 0.0,
				"kt": 1.5
			}
		}
	}

	plant_params = {
		"theta"		: 20,
		"C"			: 4190,
		"M"			: 20,
		"L"			: 0.2,
		"Td"		: 30,
		"kn"		: 0.2,
		"gain"		: 1.0
	}

	dt = 1.0
	theta_amb = 20
	k_noise = 0.001

	temp_ist = 0
	temp_soll = 20
	heatrate_soll = 1.25
	ctrl = TempController(dt)
	ctrl.set_params(ctrl_params)
	ctrl.set_model_plant_params(plant_params)
	ctrl.set_ambient_temperature(theta_amb)
	ctrl.set_enabled(True)
	plant = Pot(dt)
	plant.set_plant_params(plant_params)
	plant.set_ambient_temperature(theta_amb)
	_y = np.empty(0)
	_fb = np.empty(0)
	_t = np.empty(0)
	_temp_soll = np.empty(0)
	_heatrate_ist = np.empty(0)
	_temp_ist = np.empty(0)
	_temp_ist_model = np.empty(0)
	_temp_ist_model_delay = np.empty(0)
	a = 0.5
	fb = 0
	rho = 0.02
	temps = [{'Temp': 20, 'Duration': 1000}, {'Temp': 40, 'Duration': 1000}, {'Temp': 50, 'Duration': 1000}, {'Temp': 60, 'Duration': 1000}, {'Temp': 70, 'Duration': 1000}, {'Temp': 80, 'Duration': 1000}, {'Temp': 78, 'Duration': 1000}]

	t = 0
	for temp in temps:
		temp_soll = temp['Temp']
		hold_counter = temp['Duration']
		hold = False
		ctrl.set_theta_soll(temp_soll)
		ctrl.set_heatrate_soll(heatrate_soll)

		while True:
			if hold:
				if hold_counter == 0:
					break
				hold_counter -= 1
			plant.process()
			temp_ist = plant.get_temperature() + k_noise * np.random.randn()
			ctrl.set_theta_ist(temp_ist)
			ctrl.process()

			y = 3500*ctrl.get_power()
			power = max(0, y)
			plant.set_power(power)
			ctrl.set_model_power(power)
			fb = plant.get_power()
			if abs(temp_ist - temp_soll) < 0.1:
				hold = True

			temp_ist -= rho
			_temp_ist = np.append(_temp_ist, temp_ist)
			_temp_soll = np.append(_temp_soll, temp_soll)
			_y = np.append(_y, y)
			_fb = np.append(_fb, fb)
			_t = np.append(_t, t)
			_heatrate_ist = np.append(_heatrate_ist, max(-1, min(3, ctrl.heatrate_ist)))
			_temp_ist_model = np.append(_temp_ist_model, ctrl.theta_ist_model)
			_temp_ist_model_delay = np.append(_temp_ist_model_delay, ctrl.theta_ist_model_delay)
			t += 1

	figure(1)
	subplot(3, 1, 1)
	plot(_t, _temp_ist, _t, _temp_soll, 'r-', linewidth=1)
	legend(["ist", "soll"])
	grid(True)
	subplot(3, 1, 2)
	plot(_t, _y, '-b', _t, _fb, '-r', linewidth=1)
	legend(["y", "pot"])
	grid(True)
	subplot(3, 1, 3)
	plot(_t, _heatrate_ist, '-b', linewidth=1)
	legend(["heatrate"])
	grid(True)

	figure(2)
	plot(_t, _temp_ist, '-b', _t, _temp_ist_model, '-g', _t, _temp_ist_model_delay, '-r', linewidth=1)
	legend(["plant", "model (fast)", "model (delayed)"])
	grid(True)
	show()

	print("End of program")