"""Set of simple class files for Digital Twin.

These can (and should) be expanded for a complete library.
"""
from CBD.Core import BaseBlock
from pybricks.ev3devices import Motor, ColorSensor
from pybricks.parameters import Port

_PI = 3.14159265359
def _rad2deg(radians):
	return radians * 180 / _PI


class MotorActuatorBlock(BaseBlock):
	def __init__(self, block_name, port_name):
		super().__init__(block_name, input_ports=["IN1"], output_ports=[])
		self.port = getattr(Port, port_name)
		self.motor = Motor(self.port)

	def compute(self, curIteration):
		self.motor.run(_rad2deg(self.getInputSignal(curIteration, "IN1").value))


class ReflectionSensorBlock(BaseBlock):
	def __init__(self, block_name, port_name):
		super().__init__(block_name, input_ports=[], output_ports=["OUT1"])
		self.port = getattr(Port, "S" + port_name)
		self.sensor = ColorSensor(self.port)

	def compute(self, curIteration):
		ref = self.sensor.reflection()
		if ref is None:
			ref = 0.0
		self.appendToSignal(ref, "OUT1")

