"""
Example for BNO080 integrated IMU.
Available from Sparkfun.
"""

import sys
import serial
import time
import struct
import random
import math

from i2cdriver import I2CDriver
def hexdump(s):
    def toprint(c):
        if 32 <= c < 127:
            return chr(c)
        else:
            return "."
    def hexline(s):
        return (" ".join(["%02x" % c for c in s]).ljust(49) +
                "|" +
                "".join([toprint(c) for c in s]).ljust(16) +
                "|")
    return "\n".join([hexline(s[i:i+16]) for i in range(0, len(s), 16)])

CHANNEL_COMMAND = 0
CHANNEL_EXECUTABLE = 1
CHANNEL_CONTROL = 2
CHANNEL_REPORTS = 3
CHANNEL_WAKE_REPORTS = 4
CHANNEL_GYRO = 5

# All the ways we can configure or talk to the BNO080, figure 34, page 36 reference manual
# These are used for low level communication with the sensor, on channel 2
SHTP_REPORT_COMMAND_RESPONSE = 0xF1
SHTP_REPORT_COMMAND_REQUEST = 0xF2
SHTP_REPORT_FRS_READ_RESPONSE = 0xF3
SHTP_REPORT_FRS_READ_REQUEST = 0xF4
SHTP_REPORT_PRODUCT_ID_RESPONSE = 0xF8
SHTP_REPORT_PRODUCT_ID_REQUEST = 0xF9
SHTP_REPORT_BASE_TIMESTAMP = 0xFB
SHTP_REPORT_SET_FEATURE_COMMAND = 0xFD

# All the different sensors and features we can get reports from
# These are used when enabling a given sensor
SENSOR_REPORTID_ACCELEROMETER = 0x01
SENSOR_REPORTID_GYROSCOPE = 0x02
SENSOR_REPORTID_MAGNETIC_FIELD = 0x03
SENSOR_REPORTID_LINEAR_ACCELERATION = 0x04
SENSOR_REPORTID_ROTATION_VECTOR = 0x05
SENSOR_REPORTID_GRAVITY = 0x06
SENSOR_REPORTID_GAME_ROTATION_VECTOR = 0x08
SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR = 0x09
SENSOR_REPORTID_TAP_DETECTOR = 0x10
SENSOR_REPORTID_STEP_COUNTER = 0x11
SENSOR_REPORTID_STABILITY_CLASSIFIER = 0x13
SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER = 0x1E

# Record IDs from figure 29, page 29 reference manual
# These are used to read the metadata for each sensor type
FRS_RECORDID_ACCELEROMETER = 0xE302
FRS_RECORDID_GYROSCOPE_CALIBRATED = 0xE306
FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED = 0xE309
FRS_RECORDID_ROTATION_VECTOR = 0xE30B

# Command IDs from section 6.4, page 42
# These are used to calibrate, initialize, set orientation, tare etc the sensor
COMMAND_ERRORS = 1
COMMAND_COUNTER = 2
COMMAND_TARE = 3
COMMAND_INITIALIZE = 4
COMMAND_DCD = 6
COMMAND_ME_CALIBRATE = 7
COMMAND_DCD_PERIOD_SAVE = 9
COMMAND_OSCILLATOR = 10
COMMAND_CLEAR_DCD = 11

CALIBRATE_ACCEL = 0
CALIBRATE_GYRO = 1
CALIBRATE_MAG = 2
CALIBRATE_PLANAR_ACCEL = 3
CALIBRATE_ACCEL_GYRO_MAG = 4
CALIBRATE_STOP = 5

def normalize(v, tolerance=0.00001):
    mag2 = sum(n * n for n in v)
    if abs(mag2 - 1.0) > tolerance:
        mag = math.sqrt(mag2)
        v = tuple(n / mag for n in v)
    return v

class BNO080:
    def __init__(self, i2, a = 0x4b):
        self.i2 = i2
        self.a = a
        self.seqno = [0] * 8

        if 1:
            self.sendPacket(CHANNEL_EXECUTABLE, [1])
            time.sleep(0.150)
            while True:
                if not self.receivePacket():
                    break

        self.setFeature(SENSOR_REPORTID_ROTATION_VECTOR, 50000)

    def read_quaternion(self):
        while True:
            r = self.receivePacket()
            while r:
                tag = r[0]
                r = r[1:]
                if tag == 0xfb:
                    # print("Time %d" % struct.unpack("I", r[:4]))
                    r = r[4:]
                elif tag == SHTP_REPORT_COMMAND_RESPONSE:
                    r = r[16:]
                elif tag == 0xfc:
                    # print("Get Feature Response")
                    r = r[17:]
                elif tag == SENSOR_REPORTID_ACCELEROMETER:
                    r = r[10:]
                elif tag == SENSOR_REPORTID_ROTATION_VECTOR:
                    (_,_,_,i,j,k,w,_) = struct.unpack("<BBBhhhhh", r[:14])
                    Q14 = 2 ** -14
                    # print((i * Q14, j * Q14, k * Q14, w * Q14))
                    return (w * Q14, i * Q14, j * Q14, k * Q14)
                    r = r[14:]
                else:
                    assert 0, "Bad tag %#x" % tag

    def setFeature(self, reportID, timeBetweenReports, specificConfig = 0):
        p = struct.pack("<BBBHIII",
            SHTP_REPORT_SET_FEATURE_COMMAND,
            reportID, 
            0, 0,
            timeBetweenReports,
            0, specificConfig)
        # print(hexdump(p))

        self.sendPacket(CHANNEL_CONTROL, p)

    def sendPacket(self, channel, data):
        # print('send on', channel, 'seq', self.seqno[channel])
        self.i2.start(self.a, 0)
        self.i2.write(struct.pack("<HBB", 4 + len(data), channel, self.seqno[channel]))
        self.i2.write(data)
        self.i2.stop()
        self.seqno[channel] += 1

    def receivePacket(self):
        if not self.i2.start(self.a, 1):
            self.i2.stop()
            return None
        hdr = self.i2.read(4)
        self.i2.stop()
        length, channel, sequence = (struct.unpack("<HBB", hdr))
        length &= 0x7fff
        # print()
        # print('length', length)
        # print('channel', channel)
        # print('sequence', sequence)
        if length == 0 or channel not in range(8):
            return None

        # self.seqno[channel] = sequence + 1

        self.i2.start(self.a, 1)
        data = self.i2.read(length)
        self.i2.stop()
        # print(len(data), repr(data))
        # print(hexdump(data[4:]))
        return data[4:]
    
    def showpacket(self, data):
        tag = data[0]
        print('tag', tag)
        r = data[1:]
        if tag == 0:
            self.showpacket_00(r)
        else:
            assert False, "Cannot show packet %02x" % tag

    def showpacket_00(self, ad):
        while ad:
            (T, L) = struct.unpack("BB", ad[:2])
            V = ad[2:2+L]
            decoder = {
            1:    lambda: ("GUID", struct.unpack("I", V)),
            2:    lambda: ("MaxCargoPlusHeaderWrite", struct.unpack("H", V)),
            3:    lambda: ("MaxCargoPlusHeaderRead", struct.unpack("H", V)),
            4:    lambda: ("MaxTransferWrite", struct.unpack("H", V)),
            5:    lambda: ("MaxTransferRead", struct.unpack("H", V)),
            6:    lambda: ("NormalChannel", struct.unpack("B", V)),
            7:    lambda: ("WakeChannel", struct.unpack("B", V)),
            8:    lambda: ("AppName", (V[:-1].decode(), )),
            9:    lambda: ("ChannelName", (V[:-1].decode(), )),
            0x80: lambda: ("SHTP Version", (V[:-1].decode(), )),
            }.get(T, lambda: (str(T), (V,)))
            (nm,f) = decoder()
            print("%5d %-26s: %r" % ((len(ad), nm, ) + f))
            ad = ad[2+L:]

if __name__ == '__main__':
    i2 = I2CDriver(sys.argv[1])

    d = BNO080(i2)
    while True:
        print('quaternion:', d.read_quaternion())