from components.pid.pid import Pid
from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show
import numpy as np


class TempController:
	def __init__(self, params):
		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

	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)

	def get_power_hold(self):
		return self.pid_hold.get_y()

	def get_power_heat(self):
		return self.pid_rate.get_y()


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

	temp_ist = 0
	temp_soll = 20
	ctrl = TempController(params)
	_temp_ist = np.empty(0)
	_temp_soll = np.empty(0)
	_t = np.empty(0)
	a = 0.5
	fb = 0
	rho = 0.001
	temps = [{'Temp': 20, 'Duration': 100}, {'Temp': 40, 'Duration': 100}, {'Temp': 30, 'Duration': 100}]

	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()
			y = ctrl.get_power_hold()
			fb = (1-a)*fb + a*round(y, 2)
			temp_ist += fb
			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)
			_t = np.append(_t, t)
			t += 1

	figure(1)
	plot(_t, _temp_ist, _t, _temp_soll, 'r-', linewidth=1)
	grid(True)
	show()

	print("End of program")