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.4,
				"ki": 0.00004,
				"kd": 0.0,
				"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)
	ctrl.set_theta_ist(temp_ist)
	ctrl.set_theta_soll(temp_soll)
	t = range(0, 100)
	_temp_ist = np.empty(0)
	_temp_soll = np.empty(0)
	for i in t:
		_temp_ist = np.append(_temp_ist, temp_ist)
		_temp_soll = np.append(_temp_soll, temp_soll)
		ctrl.process()
		y = ctrl.get_power_hold()
		temp_ist += round(y, 2)
		ctrl.set_theta_ist(temp_ist)

	figure(1)
	plot(t, _temp_ist - _temp_soll, 'bx', linewidth=1)
	grid(True)
	show()

	print("End of program")