#! /usr/bin/python
__author__ = "J Dean Brock"

from smbus import SMBus
import time
import json

# Bus number on the Pi
RPI_I2C_BUS = 1

# I2C addresses
LSM303DLHC_ADDR_ACC = 0x19
LSM303DLHC_ADDR_MAG = 0x1E

# Register addresses for compass
LSM303DLHC_CRA_REG_M =          0x00
LSM303DLHC_CRB_REG_M =          0x01
LSM303DLHC_MR_REG_M =           0x02
LSM303DLHC_OUT_X_H_M =          0x03
LSM303DLHC_OUT_X_L_M =          0x04
LSM303DLHC_OUT_Y_H_M =          0x05
LSM303DLHC_OUT_Y_L_M =          0x06
LSM303DLHC_OUT_Z_H_M =          0x07
LSM303DLHC_OUT_Z_L_M =          0x08
LSM303DLHC_SR_REG_M =           0x09
LSM303DLHC_IRA_REG_M =          0x0A
LSM303DLHC_IRB_REG_M =          0x0B
LSM303DLHC_IRC_REG_M =          0x0C
LSM303DLHC_TEMP_OUT_H_M =       0x31
LSM303DLHC_TEMP_OUT_L_M =       0x32

# Register addresses for accelerometer
LSM303DLHC_CTRL_REG1_A =        0x20
LSM303DLHC_CTRL_REG2_A =        0x21
LSM303DLHC_CTRL_REG3_A =        0x22
LSM303DLHC_CTRL_REG4_A =        0x23
LSM303DLHC_CTRL_REG5_A =        0x24
LSM303DLHC_CTRL_REG6_A =        0x25
LSM303DLHC_REFERENCE_A =        0x26
LSM303DLHC_STATUS_REG_A =       0x27
LSM303DLHC_OUT_X_L_A =          0x28
LSM303DLHC_OUT_X_H_A =          0x29
LSM303DLHC_OUT_Y_L_A =          0x2A
LSM303DLHC_OUT_Y_H_A =          0x2B
LSM303DLHC_OUT_Z_L_A =          0x2C
LSM303DLHC_OUT_Z_H_A =          0x2D
LSM303DLHC_FIFO_CTRL_REG_A =    0x2E
LSM303DLHC_SRC_CTRL_REG_A =     0x2F
LSM303DLHC_INT1_CFG_A =         0x30
LSM303DLHC_INT1_SRC_A =         0x31
LSM303DLHC_INT1_THS_A =         0x32
LSM303DLHC_INT1_DURATION_A =    0x33
LSM303DLHC_INT2_CFG_A =         0x34
LSM303DLHC_INT2_SRC_A =         0x35
LSM303DLHC_INT2_THS_A =         0x36
LSM303DLHC_INT2_DURATION_A =    0x37
LSM303DLHC_CLICK_CFG_A =        0x38
LSM303DLHC_CLICK_SRC_A =        0x39
LSM303DLHC_CLICK_THS_A =        0x3A
LSM303DLHC_TIME_LIMIT_A =       0x3B
LSM303DLHC_TIME_LATENCY_A =     0x3C
LSM303DLHC_TIME_WINDOW_A =      0x3D

# Device returns 12 bits in our application
def hilo2raw(hi, lo):
    # only in Python
    return ( ( ( hi if hi < 128 else hi-256 ) << 8 ) | lo ) >> 4

def readLSM303DLHCacc():
    lsm303bus = SMBus(RPI_I2C_BUS)

    lsm303bus.write_byte_data(LSM303DLHC_ADDR_ACC,
                              LSM303DLHC_CTRL_REG1_A, 0x57)   # was 0x27
    time.sleep(0.1)
    lsm303bus.write_byte_data(LSM303DLHC_ADDR_ACC,
                              LSM303DLHC_CTRL_REG4_A, 0x80)

    ## x
    time.sleep(0.1)
    xha = lsm303bus.read_byte_data(LSM303DLHC_ADDR_ACC,
                                   LSM303DLHC_OUT_X_H_A)
    xla = lsm303bus.read_byte_data(LSM303DLHC_ADDR_ACC,
                                   LSM303DLHC_OUT_X_L_A)
    xraw = hilo2raw(xha, xla)
    print 'X -- high {0:02X}; low {1:02X}; 12bits {2:03X}; val {3:5d}'.format(
                 xha, xla, xraw & 0xFFF, xraw)
    x = xraw/1024.0

    ## y
    yha = lsm303bus.read_byte_data(LSM303DLHC_ADDR_ACC,
                                   LSM303DLHC_OUT_Y_H_A)
    yla = lsm303bus.read_byte_data(LSM303DLHC_ADDR_ACC,
                                   LSM303DLHC_OUT_Y_L_A)
    yraw = hilo2raw(yha, yla)
    print 'Y -- high {0:02X}; low {1:02X}; 12bits {2:03X}; val {3:5d}'.format(
                 yha, yla, yraw & 0xFFF, yraw)
    y = yraw/1024.0

    ## z
    zha = lsm303bus.read_byte_data(LSM303DLHC_ADDR_ACC,
                                   LSM303DLHC_OUT_Z_H_A)
    zla = lsm303bus.read_byte_data(LSM303DLHC_ADDR_ACC,
                                   LSM303DLHC_OUT_Z_L_A)
    zraw = hilo2raw(zha, zla)
    print 'Z -- high {0:02X}; low {1:02X}; 12bits {2:03X}; val {3:5d}'.format(
                 zha, zla, zraw & 0xFFF, zraw)
    z = zraw/1024.0
    return (x, y, z)

def main():
    (x, y, z) = readLSM303DLHCacc()
    jsonret = json.dumps({'x': x, 'y': y, 'z': z})
    print jsonret
    return

if __name__ == "__main__":
    main()