from components.plant.pot import Pot
from components.pid import Kalman
from components.pid.temp_controller_base import TempControllerBase
from components.pid.tc_constants import *


class TempController(TempControllerBase):
	def __init__(self, dt, params, model_params):
		TempControllerBase.__init__(self, dt, params)
		self.kalman_model = Kalman(dt, params['Kalman'])
		self.kalman_model_delay = Kalman(dt, params['Kalman'])
		self.kalman_plant = Kalman(dt, params['Kalman'])
		self.model = Pot(dt, model_params)

		self.theta_ist_plant = 0
		self.dtheta_ist_plant = 0
		self.theta_ist_model = 0
		self.dtheta_ist_model = 0
		self.theta_ist_model_delay = 0
		self.dtheta_ist_model_delay = 0

	def set_model_power(self, power):
		self.model.set_power(power)

	def init_kalman(self, value):
		self.kalman_model.initial((value, 0))
		self.kalman_model_delay.initial((value, 0))
		self.kalman_plant.initial((value, 0))

	def on_state_entered(self, state):
		if state == States.HEAT:
			self.model.initial(self.theta_ist)
			self.kalman_model.initial((self.theta_ist, 0))
			self.kalman_model_delay.initial((self.theta_ist, 0))

	def post_pid(self):
		self.model.process()

	def process(self):
		# Process Kalman of Plant
		Z_plant = self.kalman_plant.process_measurement((self.theta_ist_set, 0), 0.0)
		xp_plant = self.kalman_plant.process(Z_plant)
		theta_ist_plant = xp_plant[0, 0]
		heatrate_ist_plant = xp_plant[1, 0] * 60

		# Process Kalman of Model
		k_model = self.kalman_model.process_measurement((self.model.get_temperature_intermediate(), 0), 0.0)
		xp_model = self.kalman_model.process(k_model)
		theta_ist_model = xp_model[0, 0]
		heatrate_ist_model = xp_model[1, 0] * 60

		# Process Kalman of delayed Model
		k_model_delay = self.kalman_model_delay.process_measurement((self.model.get_temperature(), 0), 0.0)
		xp_model_delay = self.kalman_model_delay.process(k_model_delay)
		theta_ist_model_delay = xp_model_delay[0, 0]
		dtheta_ist_model_delay = xp_model_delay[1, 0] * 60

		self.theta_ist_plant = theta_ist_plant
		self.dtheta_ist_plant = heatrate_ist_plant
		self.theta_ist_model = theta_ist_model
		self.dtheta_ist_model = heatrate_ist_model
		self.theta_ist_model_delay = theta_ist_model_delay
		self.dtheta_ist_model_delay = dtheta_ist_model_delay
		self.theta_ist = theta_ist_plant
		self.heatrate_ist = heatrate_ist_plant

		# 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()

		if 1:
			theta_err = self.theta_soll_set - (theta_ist_plant - theta_ist_model_delay + theta_ist_model)
		else:
			theta_err = self.theta_soll_set - theta_ist_plant

		heatrate_err = self.heatrate_soll - (heatrate_ist_plant - dtheta_ist_model_delay + heatrate_ist_model)
		diff = self.theta_soll_set - self.theta_ist

		self.process_fsm(diff)
		self.process_pid(theta_err, heatrate_err)