from components.pid import Kalman
from components.pid.temp_controller_base import TempControllerBase


class TempController(TempControllerBase):
	def __init__(self, dt, params):
		TempControllerBase.__init__(self, dt, params)
		self.kalman = Kalman(dt, params['Kalman'])

	def init_kalman(self, value):
		self.kalman.initial((value, 0))

	def process(self):
		# Process Kalman
		if self.use_kalman:
			Z = self.kalman.process_measurement((self.theta_ist_set, 0), 0.0)
			xp = self.kalman.process(Z)
			self.theta_ist = xp[0, 0]
			self.heatrate_ist = xp[1, 0] * 60
		else:
			self.theta_ist = self.theta_ist_set
			self.heatrate_ist = self.heatrate_ist_set

		# Compensate for max heat rate to reduce overshoot
		if self.heatrate_soll_set > 0:
			self.pid_hold.scale(1.0/self.heatrate_soll_set)

		self.heatrate_soll = self.heatrate_soll_set * self.pid_hold.get_y()
		theta_err = self.theta_soll_set - self.theta_ist
		heatrate_err = self.heatrate_soll - self.heatrate_ist

		diff = self.theta_soll_set - self.theta_ist
		self.process_fsm(diff)
		self.process_pid(theta_err, heatrate_err)