import asyncio
from time import sleep
import abc
from ws.server.ws_server_multi_user import WsServerMultiUser
from components.sensor.tempSensorSim import TempSensorSim, ATemperatureSensor
from components.plant.pot import Pot, APlant
from components.actor.heater import Heater, AHeater
from components.pid import kalman, temp_controller
from ws.message import MsgIo, MessageDispatcher
from utils.value import Value, ValueChanged


class ATask:
	def __init__(self, interval):
		self.interval = interval

	@abc.abstractmethod
	def on_process(self):
		pass


class TempSensorTask(ATask):
	def __init__(self, sensor: ATemperatureSensor, interval, msg_handler: MsgIo, kalman: kalman.Kalman):
		ATask.__init__(self, interval)

		self.msg_handler = msg_handler
		msg_handler.set_recv_handler(self.recv)
		self.sensor = sensor
		self.kalman = kalman

	def on_temp_changed(self, value):
		print("Sensor temp {}".format(value))

	def on_temp_changed_kalman(self, value):
		print("Kalman temp {}".format(value))
		asyncio.create_task(self.send({'Temp': value}))

	async def recv(self, data):
		print(data)

	async def send(self, data):
		await self.msg_handler.send(data)

	async def on_process(self):
		print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
		self.kalman.initial((self.sensor.temperature(), 0))
		temp = ValueChanged(self.on_temp_changed, prec=1)
		temp_kalman = ValueChanged(self.on_temp_changed_kalman, prec=1)
		self.sensor.set_on_changed("temp", temp.set)
		while True:
			self.sensor.process()
			Z = self.kalman.process_measurement((self.sensor.temperature(), 0), 0)
			xp = self.kalman.process(Z)
			theta_ist = xp[0, 0]
			heatrate_ist = xp[1, 0] * 60
			temp_kalman.set(theta_ist)
			await asyncio.sleep(self.interval)


class HeaterTask(ATask):
	def __init__(self, heater: AHeater, interval, msg_handler: MsgIo):
		ATask.__init__(self, interval)

		self.msg_handler = msg_handler
		msg_handler.set_recv_handler(self.recv)
		self.heater = heater
		self.y = 0
		self.power_soll = 0

	def actor(self, y):
		self.y = y

	async def recv(self, data):
		print(data)
		for pair in data.items():
			if 'Power' in pair[0]:
				self.heater.setPower(pair[1])
			if 'Activate' in pair[0]:
				self.heater.activate(bool(pair[1]))

	async def send(self, data):
		await self.msg_handler.send(data)

	async def on_process(self):
		print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
		await self.send({'Capabilities': {'Power': {'Min': self.heater.get_power_min(), 'Max': self.heater.get_power_max()}}})

		power = Value()
		while True:
			self.power_soll = 200 + self.heater.get_power_max()*self.y
			self.heater.setPower(self.power_soll)
			self.heater.process()
			power.set(round(self.heater.getPower()))
			if power.is_changed():
				await self.send({'Power': power.get(), 'Activate': int(self.heater.is_activated() != 0)})
			await asyncio.sleep(self.interval)


class PotTask(ATask):
	def __init__(self, pot: APlant, interval, msg_handler: MsgIo):
		ATask.__init__(self, interval)

		self.msg_handler = msg_handler
		msg_handler.set_recv_handler(self.recv)
		self.plant = pot

	async def recv(self, data):
		print(data)
		for key in data:
			print(key)

	async def send(self, data):
		await self.msg_handler.send(data)

	async def on_process(self):
		print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
		plant_temp = Value(-1)
		plant_power = Value()
		while True:
			self.plant.process()
			plant_power.set(round(self.plant.getPower()))
			plant_temp.set(round(self.plant.getTemperature(), 1))

			if plant_power.is_changed():
				print("Plant Power {}".format(plant_power.get()))
				await self.send({'Power': plant_power.get()})

			if plant_temp.is_changed():
				print("Plant Temp  {}".format(plant_temp.get()))
				await self.send({'Temp': plant_temp.get()})

			await asyncio.sleep(self.interval)


class TcTask(ATask):
	def __init__(self, tc: temp_controller.TempController, interval, msg_handler: MsgIo):
		ATask.__init__(self, interval)

		self.msg_handler = msg_handler
		msg_handler.set_recv_handler(self.recv)
		self.tc = tc

	async def recv(self, data):
		print(data)
		for pair in data.items():
			if 'Soll' in pair[0]:
				self.tc.set_theta_soll(pair[1])

	async def send(self, data):
		await self.msg_handler.send(data)

	async def on_process(self):
		print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
		self.tc.set_theta_soll(20)
		while True:
			self.tc.process()
			await asyncio.sleep(self.interval)


class TaskManager:
	def __init__(self):
		self.tasks = []

	def add(self, task: ATask):
		self.tasks.append(task)

	def start(self):
		funcs = []
		for task in self.tasks:
			funcs.append(task.on_process())

		return asyncio.gather(*funcs)


async def asleep(time):
	await asyncio.sleep(time)

if __name__ == '__main__':
	if 0:
		taskmgr = PeriodicUpdaterThreaded()
		taskmgr.start("localhost", 8765)
		bg_thread = Thread(target=taskmgr.loop.run_forever)
		bg_thread.setDaemon(True)
		bg_thread.start()
	else:
		dispatcher = MessageDispatcher()
		server = WsServerMultiUser(listener=dispatcher)
		taskmgr = TaskManager()

		# Kalman Filter
		kalman_params = {
			"dt"				: 1.0,
			"var_P"				: 0.1,
			"var_Q"				: 0.001,
			"var_R"				: 0.1
		}
		kalman = kalman.Kalman(kalman_params)

		# Sensor
		sensor = TempSensorSim()
		taskmgr.add(TempSensorTask(sensor, 1.0, dispatcher.msgio_get("Sensor"), kalman))

		# Plant
		pot_params = {
			"dt"			: 1.0,
			"theta_amb"		: 15,
			"C"				: 4190,
			"M"				: 20,
			"L"				: 0.1,
			"Td"			: 12,
			"kn"			: 0.2
		}
		pot = Pot(pot_params)
		pot.set_on_changed("temp", ValueChanged(sensor.set_fake_temp, prec=1).set)
		taskmgr.add(PotTask(pot, 1.0, dispatcher.msgio_get("Pot")))

		# Heater
		heater = Heater()
		heater.set_on_changed("power_eff", ValueChanged(pot.setPower, prec=0).set)
		heater_task = HeaterTask(heater, 1.0, dispatcher.msgio_get("Heater"))
		taskmgr.add(heater_task)

		# Temperature Controller
		tc_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
				}
			}
		}
		tc = temp_controller.TempController(tc_params)
		tc.set_on_changed("y_hold", heater_task.actor)
		tc_task = TcTask(tc, 1.0, dispatcher.msgio_get("Tc"))
		taskmgr.add(tc_task)
		sensor.set_on_changed("temp", ValueChanged(tc.set_theta_ist, prec=1).set)

		# Message dispatcher
		h_dispatcher = taskmgr.start()
		h_server = server.listen("localhost", 8765)
		asyncio.gather(h_dispatcher, h_server)
		server.run_forever()

	while True:
		print("Hallo")
		sleep(1)