from machine import Pin, SoftI2C

class MPU():

    def __init__(self):
        self.iic = SoftI2C(scl=Pin(22), sda=Pin(23))
        self.addr = 0x68
        self.iic.start()
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.stop()

    def get_raw_values(self):
        self.iic.start()
        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
        self.iic.stop()
        return a

    def get_ints(self):
        b = self.get_raw_values()
        c = []
        for i in b:
            c.append(i)
        return c

    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

    def read(self):
        try:
            raw_ints = self.get_raw_values()
        except Exception as e:
            print("Error: " + str(e))
            return None
        vals = {}
        vals['AcX'] = self.bytes_toint(raw_ints[0], raw_ints[1])
        vals['AcY'] = self.bytes_toint(raw_ints[2], raw_ints[3])
        vals['AcZ'] = self.bytes_toint(raw_ints[4], raw_ints[5])
        vals['Tmp'] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
        vals['GyX'] = self.bytes_toint(raw_ints[8], raw_ints[9])
        vals['GyY'] = self.bytes_toint(raw_ints[10], raw_ints[11])
        vals['GyZ'] = self.bytes_toint(raw_ints[12], raw_ints[13])
        return vals  # returned in range of Int16
        # -32768 to 32767
