from contextlib import contextmanager
from components import AStirrer
from components.actor.pololu1376 import *
import time
import serial


class StirrerPololu1376(AStirrer):
	def name(self):
		return "Pololu1376"

	def __init__(self, dt, params):
		AStirrer.__init__(self, dt, simulated=False)

		# Deferred-open pyserial idiom - see Pololu1376's own constructor
		# comment: connect() is what actually opens params['port'] (and
		# sets output flow control, which itself requires an open port).
		ser = serial.Serial()
		ser.port = params['port']
		ser.baudrate = params['speed']
		self.drv = Pololu1376(ser)

	def connect(self):
		ok = self.drv.connect()
		self.connected = ok
		return ok

	def disconnect(self):
		self.drv.disconnect()
		self.connected = False

	@contextmanager
	def remote_open(self):
		try:
			self.activate(True)
			yield None
		finally:
			self.activate(False)

	def get_speed(self):
		if self.isOn and self.is_activated():
			return self.speed
		return 0

	def activate(self, enable):
		if not self.connected:
			return
		try:
			self.log.debug("activate {}".format(enable))
			if enable:
				self.drv.go()
			else:
				self.drv.stop()
		except Exception as e:
			# See HeaterHendi.activate()'s own comment: a device that was
			# connected can still fail a live command (cable pulled
			# mid-run, ...) - this is called both from StirrerTask's tick
			# loop (with self.device.open(): activate(True)/(False)) and
			# from a client's Connect/Disconnect command, neither of which
			# expect activate() itself to ever raise.
			self.log.error(f"comm error in activate({enable}): {e}")
			self.disconnect()

	def _on_set_speed(self, speed):
		if not self.connected:
			return
		try:
			self.drv.motor_forward(speed)
			self.log.debug("Set speed to {} %".format(speed))
		except Exception as e:
			self.log.error(f"comm error in _on_set_speed: {e}")
			self.disconnect()

	def _on_process(self):
		if not self.connected:
			return
		try:
			errors = self.drv.get_status_errors()
			if errors:
				self.log.warning(f"motor controller reports errors: {errors!r}")
				if StatusError.SAFE_START_VIOLATION in errors:
					self.log.warning("Recover after safe start violation!")
					self.drv.go()
		except Exception as e:
			self.log.error(f"comm error in _on_process: {e}")
			self.disconnect()
		

if __name__ == '__main__':
	s = StirrerPololu1376(dt=1.0, params={'port': "/dev/ttyACM0", 'speed': 115200})
	s.connect()

	with s.remote_open():

		print("Set some speeds")
		s.set_speed(50)
		time.sleep(2)
		s.set_speed(20)
		time.sleep(2)
		s.set_speed(50)
		time.sleep(2)

		print("Pulsed operation, duty cycle = 50%")
		s.set_duty_cycle(0.5)
		s.set_cycle_time(10)
		for i in range(1, 30):
			s.process()
			time.sleep(1)

		print("Pulsed operation, duty cycle = 20%")
		s.set_duty_cycle(0.2)
		s.set_cycle_time(10)
		for i in range(1, 30):
			s.process()
			time.sleep(1)

		print("Continious operation")
		s.set_duty_cycle(1.0)
		s.set_speed(50)
		time.sleep(3.0)

		print("Continious operation increasing speed")
		for speed in range (25, 75, 5):
			s.set_duty_cycle(1.0)
			s.set_speed(speed)
			for d in range(0,3,1):
				s.process()
				time.sleep(1.0)

		print("Leaving context")