import analogio
import board
import busio
import digitalio
import math
import time
import ustruct


class Hardware:
    def __init__(self):
        self.i2c = busio.I2C(board.SCL, board.SDA, frequency=400000)
        self._i2c_buffer = bytearray(2)
        self._init_servo()
        self._init_eyes()
        self._init_sensor()
        #self._init_voice()

    def _i2c_reg(self, address, register, value=None):
        self.i2c.try_lock()
        try:
            self._i2c_buffer[0] = register
            if value is None:
                self.i2c.writeto(address, self._i2c_buffer, end=1, stop=False)
                self.i2c.readfrom_into(address, self._i2c_buffer, start=1)
                return self._i2c_buffer[1]
            self._i2c_buffer[1] = value
            self.i2c.writeto(address, self._i2c_buffer)
        finally:
            self.i2c.unlock()

    def _init_servo(self):
        self._i2c_reg(0x40, 0x00, 0x00)
        time.sleep(0.05)
        old_mode = self._i2c_reg(0x40, 0x00)
        self._i2c_reg(0x40, 0x00, old_mode & 0x7f | 0x10)
        self._i2c_reg(0x40, 0xfe, 123)  # ~50Hz
        self._i2c_reg(0x40, 0x00, old_mode)
        time.sleep(0.05)
        self._i2c_reg(0x40, 0x00, old_mode | 0xa1)

    def servo(self, index, radians=None):
        min_duty = 123
        max_duty = 491
        span = max_duty - min_duty
        if radians is None:
            duty = 4096
        else:
            duty = min(max_duty, max(min_duty,
                                     int(min_duty + span * radians / math.pi)))
        data = ustruct.pack('<BHH', 0x06 + 4 * index, 0, duty)
        self.i2c.try_lock()
        try:
            self.i2c.writeto(0x40, data)
        finally:
            self.i2c.unlock()

    def _init_sensor(self):
        self._i2c_reg(0x60, 0xee, 0x06)
        #self._i2c_reg(0x60, 0x3f, 0x00)
        #self._i2c_reg(0x60, 0xa8, 0x03)

    def sensor(self):
        self._i2c_buffer[0] = 0x5e
        self.i2c.try_lock()
        try:
            self.i2c.writeto(0x60, self._i2c_buffer, end=1, stop=False)
            self.i2c.readfrom_into(0x60, self._i2c_buffer)
        finally:
            self.i2c.unlock()
        if self._i2c_buffer[0] == 0xff:
            return 0
        return ((self._i2c_buffer[0] << 4) | self._i2c_buffer[1]) / 3.2

    def _init_eyes(self):
        self._eye_pins = (
            digitalio.DigitalInOut(board.D9),
            digitalio.DigitalInOut(board.D11),
            digitalio.DigitalInOut(board.D10),
        )
        for pin in self._eye_pins:
            pin.switch_to_output()

    def eye_color(self, color):
        for index, pin in enumerate(self._eye_pins):
            pin.value = color & (1 << index)

    def _init_voice(self):
        self._dac = analogio.AnalogOut(board.A0)

    def voice(self, period=1, duration=1000):
        for i in range(duration/period):
            self._dac.value = 65535
            time.sleep(period/1000)
            self._dac.value = 0