#!/usr/bin/env python
# coding: latin-1
"""
This module is designed to communicate with the XLoBorg

busNumber               I²C bus on which the XLoBorg is attached (Rev 1 is bus 0, Rev 2 is bus 1)
bus                     the smbus object used to talk to the I²C bus
addressAccelerometer    The I²C address of the accelerometer chip
addressCompass          The I²C address of the compass chip
foundAccelerometer      True if the accelerometer chip can be seen, False otherwise
foundCompass            True if the compass chip can be seen, False otherwise
printFunction           Function reference to call when printing text, if None "print" is used
gPerCount               Number of G represented by the LSB of the accelerometer at the current sensitivity
tempOffest              The offset to add to the temperature reading in °C
"""

# Import the libraries we need
import sys
import smbus
import struct
import math

### MODULE DATA ###
# Shared values used by this module
global busNumber
global bus
global addressAccelerometer
global addressCompass
global foundAccelerometer
global foundCompass
global printFunction
global gPerCount
global tempOffest 

# Constant values
addressAccelerometer = 0x1C
addressCompass = 0x0E

# Check here for Rev 1 vs Rev 2 and select the correct bus
busNumber = 1

### MODULE FUNCTIONS ###
def Print(message):
    """
Print(message)

Wrapper used by the XLoBorg module to print messages, will call printFunction if set, print otherwise
    """
    global printFunction
    if printFunction == None:
        print message
    else:
        printFunction(message)

def NoPrint(message):
    """
NoPrint(message)

Does nothing, intended for disabling diagnostic printout by using:
XLoBorg.printFunction = XLoBorg.NoPrint
    """
    pass

def Init(tryOtherBus = True):
    """
Init([tryOtherBus])

Prepare the I2C driver for talking to the XLoBorg
If tryOtherBus is True or omitted, this function will attempt to use the other bus if none of the XLoBorg devices can be found on the current busNumber
    """
    global busNumber
    global bus
    global addressAccelerometer
    global addressCompass
    global foundAccelerometer
    global foundCompass
    global magxoffset, magyoffset, magzoffset

    # Read predetermined magnetometer offsets from file if present
    # File MAG3110offsets should contain 3 signed dicimal numbers separated by spaces.
    # Blank lines, lines starting with # and lines following a valid line are ignored.
    magxoffset = magyoffset = magzoffset = 0
    try:
        f = open('MAG3110offsets', 'r')
	line = "\n"
	while line == "\n" or line[0] == "#":
	    line = f.readline()
	data = line.split()
	magxoffset = int(data[0])
	magyoffset = int(data[1])
	magzoffset = int(data[2])
	print("magxoffset=%d magyoffset=%d magzoffset=%d" % (magxoffset, magyoffset, magzoffset))
    except:
        print('No MAG3110offsets file found: offsets set to zero')

    # Open the bus
    Print('Loading XLoBorg on bus %d' % (busNumber))
    bus = smbus.SMBus(busNumber)

    # Check for accelerometer
    try:
        byte = bus.read_byte_data(addressAccelerometer, 1)
        foundAccelerometer = True
        Print('Found accelerometer at %02X' % (addressAccelerometer))
    except:
        foundAccelerometer = False
        Print('Missing accelerometer at %02X' % (addressAccelerometer))

    # Check for compass
    try:
        byte = bus.read_byte_data(addressCompass, 1)
        foundCompass = True
        Print('Found compass at %02X' % (addressCompass))
    except:
        foundCompass = False
        Print('Missing compass at %02X' % (addressCompass))

    # See if we are missing chips
    if not (foundAccelerometer or foundCompass):
        Print('Both the compass and accelerometer were not found')
        if tryOtherBus:
            if busNumber == 1:
                busNumber = 0
            else:
                busNumber = 1
            Print('Trying bus %d instead' % (busNumber))
            Init(False)
        else:
            Print('Are you sure your XLoBorg is properly attached, and the I2C drivers are running?')
            bus = None
    else:
        Print('XLoBorg loaded on bus %d' % (busNumber))
        if foundAccelerometer:
            InitAccelerometer()
        if foundCompass:
            InitCompass()

def InitAccelerometer():
    """
InitAccelerometer()

Initialises the accelerometer on bus to default states
    """
    global bus
    global addressAccelerometer
    global gPerCount

    # Setup mode configuration
    register = 0x2A             # CTRL_REG1
    data =  (0 << 6)            # Sleep rate 50 Hz
    data |= (0 << 4)            # Data rate 800 Hz
    data |= (0 << 2)            # No reduced noise mode
    data |= (1 << 1)            # Normal read mode
    data |= (1 << 0)                # Active
    try:
        bus.write_byte_data(addressAccelerometer, register, data)
    except:
        Print('Failed sending CTRL_REG1!')

    # Setup range
    register = 0x0E             # XYZ_DATA_CFG
    data = 0x00                 # Range 2G, no high pass filtering
    try:
        bus.write_byte_data(addressAccelerometer, register, data)
    except:
        Print('Failed sending XYZ_DATA_CFG!')
    gPerCount = 2.0 / 128       # 2G over 128 counts

    # System state
    register = 0x0B             # SYSMOD
    data = 0x01                 # Awake mode
    try:
        bus.write_byte_data(addressAccelerometer, register, data)
    except:
        Print('Failed sending SYSMOD!')

    # Reset ready for reading
    register = 0x00             
    try:
        bus.write_byte(addressAccelerometer, register)
    except:
        Print('Failed sending final write!')
            
def InitCompass():
    """
InitCompass()

Initialises the compass on bus to default states
    """
    global bus
    global addressCompass

    # System operation
    register = 0x10             # CTRL_REG1
    # Set in standby mode before setting config
    data = 0
    try:
        bus.write_byte_data(addressCompass, register, data)
    except:
        Print('Failed sending CTRL_REG1!')
    data  = (0 << 5)            # Output data rate (10 Hz when paired with 128 oversample)
    data |= (3 << 3)            # Oversample of 128
    data |= (0 << 2)            # Disable fast read
    data |= (0 << 1)            # Continuous measurement
    data |= (1 << 0)            # Active mode
    try:
        bus.write_byte_data(addressCompass, register, data)
    except:
        Print('Failed sending CTRL_REG1!')

    # Acquisition mode
    register = 0x11             # CTRL_REG2
    data  = (1 << 7)            # Reset before each acquisition
    data |= (1 << 5)            # Raw mode, do not apply user offsets
    data |= (0 << 4)            # Disable reset cycle
    try:
        bus.write_byte_data(addressCompass, register, data)
    except:
        Print('Failed sending CTRL_REG2!')

def ReadAccelerometer():
    """
x, y, z = ReadAccelerometer()

Reads the X, Y and Z axis force, in terms of Gs
    """
    global bus
    global addressAccelerometer
    global gPerCount

    # Read the data from the accelerometer chip
    try:
        [status, x, y, z] = bus.read_i2c_block_data(addressAccelerometer, 0, 4)
    except:
        Print('Failed reading registers!')
        status = 0
        x = 0
        y = 0
        z = 0
    
    # Convert from unsigned to correctly signed values
    bytes = struct.pack('BBB', x, y, z)
    x, y, z = struct.unpack('bbb', bytes)

    # Convert to Gs
    x *= gPerCount
    y *= gPerCount
    z *= gPerCount

    return x, y, z


def ReadCompassRaw():
    """
x, y, z = ReadCompassRaw()

Reads the X, Y and Z axis raw magnetometer readings
    """
    global bus
    global addressCompass
    global magxoffset, magyoffset, magzoffset

    # Read the data from the compass chip
    try:
        bus.write_byte(addressCompass, 0x00)
        [status, xh, xl, yh, yl, zh, zl, who, sm, oxh, oxl, oyh, oyl, ozh, ozl, temp, c1, c2] = bus.read_i2c_block_data(addressCompass, 0, 18)
    except:
        Print('Failed reading registers!')
        status = 0
        xh = 0
        xl = 0
        yh = 0
        yl = 0
        zh = 0
        zl = 0
    
    # Convert from unsigned to correctly signed values
    bytes = struct.pack('BBBBBB', xl, xh, yl, yh, zl, zh)
    x, y, z = struct.unpack('hhh', bytes)

    return x - magxoffset, y - magyoffset, z - magzoffset

def ReadTemperature():
    """
temp = ReadTemperature()

Reads the die temperature of the compass in degrees Celsius
    """
    global bus
    global addressCompass
    global tempOffest 

    # Read the data from the compass chip
    try:
        bus.write_byte(addressCompass, 0x00)
        [status, xh, xl, yh, yl, zh, zl, who, sm, oxh, oxl, oyh, oyl, ozh, ozl, temp, c1, c2] = bus.read_i2c_block_data(addressCompass, 0, 18)
    except:
        Print('Failed reading registers!')
        temp = 0
    
    # Convert from unsigned to correctly signed values
    bytes = struct.pack('B', temp)
    temp = struct.unpack('b', bytes)[0]
    temp += tempOffset

    return temp

### STARTUP ROUTINES ###
# Default user settings
printFunction = None
tempOffset = 0

# Auto-run code if this script is loaded directly
if __name__ == '__main__':
    # Load additional libraries
    import time
    # Start the XLoBorg module (sets up devices)
    Init()
    try:
        # Loop indefinitely
        while True:
            # Read the 
            x, y, z = ReadAccelerometer()
            mx, my, mz = ReadCompassRaw()
	    mxf = float(mx)/10
	    myf = float(my)/10
	    mzf = float(mz)/10
	    hdg = math.atan2(myf, mxf)
	    hdg = hdg * 180/math.pi
	    if hdg < 0:
	        hdg = hdg + 360
            temp = ReadTemperature()
            print 'X = %+01.4f G, Y = %+01.4f G, Z = %+01.4f G, mX = %6.1f uT, mY = %6.1f uT, mZ = %6.1f uT, Hdg = %3d T = %+03d°C\r' % (x, y, z, mxf, myf, mzf, hdg, temp),
	    sys.stdout.flush()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # User aborted
        print ''