"""
Micropython	library for
PE_G1 	(Motor*5*2 & Servo*4)
MDB 	(Motor*4*2 & Servo*6)
===============================
@dahanzimin From the Mixly Team
"""
from time import sleep_ms
from micropython import const

_PE_GX_ADDRESS			= const(0x25)
_PE_GX_ID   			= const(0x00)
_PE_GX_VBAT   			= const(0x01)
_PE_GX_MOTOR			= const(0x03)

class PE_G1:
	def __init__(self, i2c_bus, addr=_PE_GX_ADDRESS):
		self._i2c = i2c_bus
		self._addr = addr
		sleep_ms(500)
		if self._rreg(_PE_GX_ID) == 0x25:
			self._PE_GX_SERVO = 0x0D
			self._type = 1 #PE_G1
		elif self._rreg(_PE_GX_ID) == 0x26:
			self._PE_GX_SERVO = 0x0B
			self._type = 2 #MDB
		else:
			raise AttributeError("Cannot find a PE_G1 or MDB")
		self.reset()

	def _wreg(self, reg, val):
		'''Write memory address'''
		try:
			self._i2c.writeto_mem(self._addr, reg, val.to_bytes(1, 'little'))
		except:
			return 0 

	def _rreg(self, reg, nbytes=1):
		'''Read memory address'''
		try:
			self._i2c.writeto(self._addr, reg.to_bytes(1, 'little'))
			return  self._i2c.readfrom(self._addr, nbytes)[0] if nbytes <= 1 else self._i2c.readfrom(self._addr, nbytes)
		except:
			return 0

	def reset(self):
		"""Reset all registers to default state"""
		if self._type == 1:
			self._i2c.writeto_mem(self._addr, _PE_GX_MOTOR, bytes(18))
		if self._type == 2:
			self._i2c.writeto_mem(self._addr, _PE_GX_MOTOR, bytes(20))

	def read_bat(self, ratio=1):
		'''Read battery power'''
		vbat = self._rreg(_PE_GX_VBAT) << 2 | self._rreg(_PE_GX_VBAT+1) >> 6
		if self._type == 1:
			return round(vbat * ratio * 0.00968, 2)
		if self._type == 2:
			return round(vbat * ratio * 0.01398, 2)

	def m_pwm(self, index, duty=None):
		"""Motor PWM duty cycle data register"""
		if self._type == 1: # Motor*5*2 
			if not 0 <= index <= 9:
				raise ValueError("Motor port must be a number in the range: 0~4")
		if self._type == 2: # Motor*4*2 
			if not 0 <= index <= 7:
				raise ValueError("Motor port must be a number in the range: 0~3")
		if duty is None:
			return self._rreg(_PE_GX_MOTOR + index)
		else:
			if not 0 <= duty <= 255:
				raise ValueError("Duty must be a number in the range: 0~255")
			self._wreg(_PE_GX_MOTOR + index, duty)

	def s_pwm(self, index, duty=None):
		"""Servo PWM duty cycle data register"""
		if self._type == 1: # Servo*4 
			if not 0 <= index <= 3:
				raise ValueError("Servo port must be a number in the range: 0~3")
		if self._type == 2: # Servo*6 
			if not 0 <= index <= 5:
				raise ValueError("Servo port must be a number in the range: 0~5")
		if duty is None:
			return self._rreg(self._PE_GX_SERVO + index * 2) << 8 | self._rreg(self._PE_GX_SERVO + index * 2 + 1)
		else:
			if not 0 <= duty <= 4095:
				raise ValueError("Duty must be a number in the range: 0~4095")
			self._wreg(self._PE_GX_SERVO + index * 2,duty >> 8)
			self._wreg(self._PE_GX_SERVO + index * 2 + 1,duty & 0xff)

	def motor(self, index, action="NC", speed=0):
		if not 0 <= speed <= 100:
			raise ValueError("Speed parameters must be a number in the range: 0~100")
		if action == "N":
			self.m_pwm(index * 2, 0)
			self.m_pwm(index * 2 + 1, 0)
		elif action == "P":
			self.m_pwm(index * 2, 255)
			self.m_pwm(index * 2 + 1, 255)
		elif action == "CW":
			self.m_pwm(index * 2, 0)
			self.m_pwm(index * 2 + 1, speed * 255 // 100)
		elif action == "CCW":
			self.m_pwm(index * 2, speed * 255 // 100)
			self.m_pwm(index * 2 + 1, 0)
		elif action == "NC":
			return round(self.m_pwm(index * 2) * 100 / 255), round(self.m_pwm(index * 2 + 1) * 100 / 255)
		else:
			raise ValueError('Invalid input, valid are "N","P","CW","CCW"')

	def servo180(self, index, angle=None):
		if angle is None:
			return  round((self.s_pwm(index) - 102.375) * 180 / 409.5)
		else:
			if not 0 <= angle <= 180:
				raise ValueError("Servo(180) angle must be a number in the range: 0~180")
			self.s_pwm(index,round(102.375 + 409.5 / 180 * angle))

	def servo360(self, index, speed=None):
		if speed is None:
			return  round((self.s_pwm(index) - 102.375) * 200 / 409.5 - 100)
		else:
			if not -100 <= speed <= 100:
				raise ValueError("Servo(360) speed must be a number in the range: -100~100")
			self.s_pwm(index,round(102.375 + 409.5 / 200 * (speed + 100)))