# FRDM_MMA8451Q
# http://mbed.org/users/mbed_official/code/FRDM_MMA8451Q/
import mbed
import sys

def float(n):
    s = 1.0
    if n < 0:
        s = -1.0
        n *= -1
    f = 0.0
    a = 1.0
    while n:
        if n & 1:
            f += a
        n >>= 1
        a *= 2.0
    f *= s
    return f

def abs(a):
    if a < 0:
        return -a
    return a
    
class MMA8451Q:
    REG_WHO_AM_I=0x0D
    REG_CTRL_REG_1=0x2A
    REG_OUT_X_MSB=0x01
    REG_OUT_Y_MSB=0x03
    REG_OUT_Z_MSB=0x05
    UINT14_MAX=16383

    def __init__(self, sda, scl, addr):
        self._i2c = mbed.I2C(sda, scl)
        self._addr = addr
        data = chr(self.REG_CTRL_REG_1)+chr(0x01)
        self.writeRegs(data)

    def getAccX(self):
        return float(self.getAccAxis(self.REG_OUT_X_MSB)) / 4096.0

    def getAccY(self):
        return float(self.getAccAxis(self.REG_OUT_Y_MSB)) / 4096.0
        
    def getAccZ(self):
        return float(self.getAccAxis(self.REG_OUT_Z_MSB)) / 4096.0
        
    def getWhoAmI(self):
        who_am_i = chr(0)
        self.readRegs(self.REG_WHO_AM_I, who_am_i);
        return ord(who_am_i)
    
    def getAccAxis(self, addr):
        res = chr(0)+chr(0)
        self.readRegs(addr, res)
        acc = (ord(res[0]) << 6) | (ord(res[1]) >> 2)
        if acc > (self.UINT14_MAX/2):
            acc -= self.UINT14_MAX;
        return acc

    def readRegs(self, addr, data):
        self._i2c.write(self._addr, chr(addr), 1, 1)
        self._i2c.read(self._addr, data, len(data), 0)

    def writeRegs(self, data):
        self._i2c.write(self._addr, data, len(data), 0)

MMA8451_I2C_ADDRESS=(0x1d<<1)

acc = MMA8451Q('PTE25', 'PTE24', MMA8451_I2C_ADDRESS)
rled = mbed.PwmOut('LED_RED')
gled = mbed.PwmOut('LED_GREEN')
bled = mbed.PwmOut('LED_BLUE');
    
while 1:
    rled.write(1.0 - abs(acc.getAccX()))
    gled.write(1.0 - abs(acc.getAccY()))
    bled.write(1.0 - abs(acc.getAccZ()))
    sys.wait(100) # 100ms
    #print sys.heap()
    #sys.wait(1000)