import numpy as np
import json
from enum import Enum
from pid import Pid
from utils import Smoother, Stable
import time
import threading
from kalman import Kalman
from timer import Timer, TimerManager, TimerListener
import queue

ovenStates = Enum('ovenStates', 'IDLE HEAT HOLD')
states = Enum('controllerStates', 'IDLE NORMAL PAUSE')
commands = Enum('controllerCommands', 'RUN PAUSE ABORT STIRR_OFF STIRR_ON')

class Controller(TimerListener):

	def __init__(self, params, plant, stirrer):
		print(json.dumps({'Controller': params}, indent=4, sort_keys=True))
		self.sim_warp_factor = params['sim_warp_factor']
		self.ovenState = ovenStates.IDLE
		self.plant = plant
		self.stirrer = stirrer
		self.dt = params['dt']
		self.params = params
		self.pid_hold = Pid()
		self.pid_rate = Pid()
		self.sensorTime_v = np.empty(0)
		self.theta_raw_v = np.empty(0)
		self.theta_v = np.empty(0)
		self.theta_k_v = np.empty(0)
		self.heatrate_v = np.empty(0)
		self.heatrate_k_v = np.empty(0)
		self.time_v = np.empty(0)
		self.power_soll = 0
		self.power_v = np.empty(0)
		self.power_heat_v = np.empty(0)
		self.power_hold_v = np.empty(0)
		self.error_v = np.empty(0)
		self.time = 0
		self.report_interval = 1.0
		self.thread = None
		self.receipe_sema = threading.Semaphore(0)
		self.receipe = None
		self.theta_sm = Smoother(1.0)
		self.heatrate_sm = Smoother(0.01)
		self.theta_err_sm = Smoother(1.0)
		self.heatrate_err_sm = Smoother(1.0)
		self.useKalman = params['useKalman']
		self.kalman = Kalman(params['kalman'])
		self.sensor_dt = params['kalman']['dt']
		self.timerMgr = TimerManager()
		self.processTimer = Timer("Process", self)
		self.rastTimer = Timer("Rast", self)
		self.sensorTimer = Timer("Sensor", self)
		self.plantTimer = Timer("Plant", self)
		self.reportTimer = Timer("Report", self)
		self.timerMgr.registerTimer(self.processTimer)
		self.timerMgr.registerTimer(self.rastTimer)
		self.timerMgr.registerTimer(self.sensorTimer)
		self.timerMgr.registerTimer(self.plantTimer)
		self.timerMgr.registerTimer(self.reportTimer)
		self.power_hold_offset = 200
		self.power_heat_offset = 1500
		self.queue = queue.Queue()
		self.thread = threading.Thread(target=self.main)
		self.rast_done = False
		self.receipe_done = False
		self.thread.start()

	def command(self, cmd):
		self.queue.put_nowait(cmd)

	def onTimer(self, timer):
		if timer == self.sensorTimer:
			self.sensorTime_v = np.append(self.sensorTime_v, timer.count*self.sensor_dt / 60)

			# Temperature
			self.theta_raw = self.plant.getTemperature()

			if timer.count == 0:
				# Kalman filter initial value
				self.kalman.initial((self.theta_raw, 0))
				self.theta_sm.initial(self.theta_raw)

			self.theta_raw_v = np.append(self.theta_raw_v, self.theta_raw)

			# Process Kalman
			Z = self.kalman.process_measurement((self.theta_raw, 0))
			xp = self.kalman.process(Z)
			self.theta_ist_k = xp[0, 0]
			self.heatrate_ist_k = xp[1, 0] * 60
			self.theta_k_v = np.append(self.theta_k_v, self.theta_ist_k)
			self.heatrate_k_v = np.append(self.heatrate_k_v, self.heatrate_ist_k)

		elif timer == self.plantTimer:
			# Process plant
			self.plant.process()

			# Process stirrer
			self.stirrer.process()

		elif timer == self.reportTimer:
			self.report()

		elif timer == self.processTimer:
			rast = self.rasten[self.rast_index]
			if rast is not None:
				self.ctrl_theta_soll = rast['temp']
				self.ctrl_heatrate_soll = rast['heatRate']
				self.rast_timer_soll = 60 * rast['time']
				isFinished = self.rast(rast)
				if isFinished:
					if not self.rastTimer.isActive and not self.rast_done:
						self.log ("Start rast timer with {} min.".format(self.rast_timer_soll/60))
						self.rastTimer.start(self.rast_timer_soll / self.sim_warp_factor, mode='oneshot')
					self.rast_done = True
			else:
				self.receipe_done = True

		elif timer == self.rastTimer:
			rast = self.rasten[self.rast_index]
			if not rast["waitForUser"]:
				self.rast_index += 1
				self.rast_done = False
				self.ovenState = ovenStates.IDLE

			self.log("Rast timer elapsed")

	def log(self, s):
		now = time.time()
		print("{:.2f}: {}".format(now, s))

	def report(self):
		self.log("Temp SOLL {} °C".format(self.ctrl_theta_soll))
		self.log("Temp IST = {:0.1f} °C".format(self.ctrl_theta))
		self.log("Heatrate SOLL = {:0.1f} °C/min.".format(self.ctrl_heatrate_soll))
		self.log("Heatrate IST = {:0.1f} °C/min.".format(self.ctrl_heatrate))
		self.log("Power = {:0.0f} W".format(self.power_soll))
		if self.rastTimer.isActive:
			self.log("Rast remaining : {:0.1f} min.".format(self.rastTimer.elapseDuration / 60))


	def setRecipe(self, recipe):
		print(json.dumps({recipe['Name'] : recipe}, indent=4, sort_keys=True))
		self.receipe = recipe
		self.rasten = recipe['Rasten']
		self.rasten.append(None)

		# Stirrer
		self.stirrSpeedHeat = recipe['stirrSpeedHeat']
		self.stirrSpeedRast = recipe['stirrSpeedRast']
		self.stirrDutyRast = recipe['stirrDutyRast']
		self.stirrCycleTime = recipe['stirrCycleTime']

	def wait_finished(self):
		self.receipe_sema.acquire()
		pass

	def main(self):
		self.sensorTimer.start(self.sensor_dt/self.sim_warp_factor)
		self.plantTimer.start(1/self.sim_warp_factor)

		run = True
		while(run and not self.receipe_done):
			self.timerMgr.process()
			wait = self.timerMgr.getMinTimeout(1.0/self.sim_warp_factor)
			try:
				msg = self.queue.get(block=True, timeout=wait)
				print ("Received: ", msg)
				if msg == commands.PAUSE:
					pass

				elif msg == commands.RUN:
					if not self.processTimer.isActive:
						self.time = 0
						self.rast_index = 0
						self.reportTimer.start(self.report_interval)
						self.processTimer.start(self.dt / self.sim_warp_factor)

						# Stirrer and plant
						self.stirrer.activate()
						self.plant.activate(True)
						self.stirrer.setCycleTime(self.stirrCycleTime)
						self.receipe_done = False
					else:
						self.rast_index += 1
						self.rast_done = False
						self.ovenState = ovenStates.IDLE

				elif msg == commands.ABORT:
					run = False

				elif msg == commands.STIRR_ON:
					self.stirrer.activate()

				elif msg == commands.STIRR_OFF:
					self.stirrer.deactivate()
			except:
				pass

		# Stirrer and plant
		self.stirrer.deactivate()
		self.plant.activate(False)
		self.receipe_sema.release()

	def rast(self, rast):

		# Temperature
		self.ctrl_theta = self.theta_ist_k
		self.ctrl_heatrate = self.heatrate_ist_k

		ovenStateNext = self.ovenState

		# -----------------------------------------
		ctrl_theta = self.ctrl_theta
		ctrl_heatrate = self.ctrl_heatrate

		ctrl_theta_err = self.ctrl_theta_soll - ctrl_theta
		ctrl_heatrate_err = self.ctrl_heatrate_soll - ctrl_heatrate

		if self.ovenState != ovenStates.IDLE:
			self.pid_hold.process(self.dt, self.params['Hold']['Pid'], ctrl_theta_err)
			self.pid_rate.process(self.dt, self.params['Heat']['Pid'], ctrl_heatrate_err)

		power_hold = self.power_hold_offset + self.params['P_max'] * self.pid_hold.get_y()
		power_heat = self.power_heat_offset + self.params['P_max'] * self.pid_rate.get_y()

		power_hold = max(self.params['P_min'], min(self.params['P_max'], power_hold))
		power_heat = max(self.params['P_min'], min(self.params['P_max'], power_heat))

		self.power_soll = power_hold
		if self.ovenState == ovenStates.HEAT:
			self.power_soll = power_heat

		self.plant.setPower(self.power_soll)

		self.time_v = np.append(self.time_v, self.time/60)
		self.theta_v = np.append(self.theta_v, ctrl_theta)
		self.heatrate_v = np.append(self.heatrate_v, ctrl_heatrate)
		self.power_v = np.append(self.power_v, self.power_soll)
		self.power_hold_v = np.append(self.power_hold_v, power_hold)
		self.power_heat_v = np.append(self.power_heat_v, power_heat)
		self.error_v = np.append(self.error_v, ctrl_theta_err)

		self.time += self.dt

		# -----------------------------------------
		rast_finished = False

		if self.ovenState == ovenStates.IDLE:
			if (ctrl_theta + 1.0) < self.ctrl_theta_soll:
				ovenStateNext = ovenStates.HEAT

		if self.ovenState == ovenStates.HEAT:
			if (ctrl_theta + 1.0) >= self.ctrl_theta_soll:
				ovenStateNext = ovenStates.HOLD

		if ovenStateNext != self.ovenState:
			self.log("{} -> {}".format(self.ovenState, ovenStateNext))
			if ovenStateNext == ovenStates.HEAT:
				self.pid_rate.reset()
				self.stirrer.setSpeed(self.stirrSpeedHeat)
				self.stirrer.setDutyCycle(1.0)
				if self.ovenState == ovenStates.HOLD:
					self.power_hold_offset = power_hold

			if ovenStateNext == ovenStates.HOLD:
				self.pid_hold.reset()
				self.stirrer.setSpeed(self.stirrSpeedRast)
				self.stirrer.setDutyCycle(self.stirrDutyRast)
				if self.ovenState == ovenStates.HEAT:
					self.power_heat_offset = power_heat

		# ------------------------------
		self.rast_running = True
		self.ovenState = ovenStateNext
		return self.ovenState == ovenStates.HOLD
