from components.pid.pid import Pid
from components.plant.pot import Pot
from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show
import numpy as np
from utils.value import AttributeChange


class TempController(AttributeChange):
	def __init__(self, params):
		AttributeChange.__init__(self)

		self.dt = params['dt']
		self.pid_hold = Pid()
		self.pid_rate = Pid()
		self.theta_ist = 0
		self.theta_soll = 0
		self.heatrate_ist = 0
		self.heatrate_soll = 0
		self.params = params
		self.y_hold = 0
		self.y_heat = 0

	def set_theta_ist(self, value):
		self.theta_ist = value

	def set_heatrate_ist(self, value):
		self.heatrate_ist = value

	def set_theta_soll(self, value):
		self.theta_soll = value

	def set_heatrate_soll(self, value):
		self.heatrate_soll = value

	def process(self):
		theta_err = self.theta_soll - self.theta_ist
		heatrate_err = self.heatrate_soll - self.heatrate_ist

		self.pid_hold.process(self.dt, self.params['Hold']['Pid'], theta_err)
		self.pid_rate.process(self.dt, self.params['Heat']['Pid'], heatrate_err)

		self.y_hold = self.pid_hold.get_y()
		self.y_heat = self.pid_rate.get_y()

	def get_power_hold(self):
		return self.y_hold

	def get_power_heat(self):
		return self.y_heat


if __name__ == '__main__':
	params = {
		"dt": 1.0,
		"Hold": {
			"Pid": {
				"kp": 0.8,
				"ki": 0.008,
				"kd": 0.0,
				"rho": 1.0
			}
		},
		"Heat": {
			"Pid": {
				"kp": 0.002,
				"ki": 0.0002,
				"kd": 0.0,
				"rho": 1.0
			}
		}
	}

	pot_params = {
		"dt"		: 1.0,
		"theta"			: 20,
		"C"				: 4190,
		"M"				: 20,
		"L"				: 0.05,
		"Td"			: 12,
		"kn"			: 0.2
	}

	temp_ist = 0
	temp_soll = 20
	ctrl = TempController(params)
	plant = Pot(pot_params)
	_temp_ist = np.empty(0)
	_temp_soll = np.empty(0)
	_y = np.empty(0)
	_fb = np.empty(0)
	_t = 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}]

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

		while True:
			if hold:
				if hold_counter == 0:
					break
				hold_counter -= 1
			ctrl.process()
			plant.process()
			y = max(0, 3500*ctrl.get_power_hold())
			plant.setPower(y)
			temp_ist = round(plant.getTemperature(), 1)
			fb = plant.getPower()
			ctrl.set_theta_ist(temp_ist)
			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)
			t += 1

	figure(1)
	subplot(2, 1, 1)
	plot(_t, _temp_ist, _t, _temp_soll, 'r-', linewidth=1)
	grid(True)
	subplot(2, 1, 2)
	plot(_t, _y, 'bx', _t, _fb, '-r', linewidth=1)
	grid(True)
	show()

	print("End of program")