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

	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.heater.process()
			power.set(round(self.heater.getPower()))
			if power.is_changed():
				await self.send({'Power': power.get(), 'Activate': int(self.heater.getPower() != 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 TaskManager:
	def __init__(self):
		self.tasks: ATask = []

	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, 0.2, 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.connect_theta_changed(sensor.set_fake_temp)
		taskmgr.add(PotTask(pot, 0.1, dispatcher.msgio_get("Pot")))

		# Heater
		heater = Heater()
		heater.connect_power_changed(pot.setPower)
		taskmgr.add(HeaterTask(heater, 0.02, dispatcher.msgio_get("Heater")))

		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)