#include "mbed.h"
#include "MMA8451Q.h"

#define EEPROM_CONTROL_BYTE 0xA0
#define I2C_READ 0x01

#include "WakeUp.h"
#define OFF 1
#define ON 0
DigitalOut led2(LED_GREEN);


void wakeirq(void)
{
led2 = ON;
wait(0.2);
led2 = OFF;
}

#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
  PinName const SDA = PTE25;
  PinName const SCL = PTE24;

#else
  #error TARGET NOT DEFINED
#endif
 
#define MMA8451_I2C_ADDRESS (0x1d<<1)

I2C i2c(PTE0, PTE1); // sda, scl on KL25Z
Serial pc(USBTX, USBRX); // tx, rx 
 
 int n;
 char data;
 
char offset= 0;
char mac[6];
 
int main(void) {
    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
    PwmOut rled(LED1);
    PwmOut gled(LED2);
    PwmOut bled(LED3);
    
    printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
    WakeUp::calibrate();
    WakeUp::attach(wakeirq);
    WakeUp::set(10);
    led2 = OFF; // turn off the leds
    printf("\n\nStart\n");
 
    while (true) {
        float x, y, z;
        x = rled = 1.0 - abs(acc.getAccX());
        y = gled = 1.0 - abs(acc.getAccY());
        z = bled = 1.0 - abs(acc.getAccZ());
        wait(0.1);
        printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z);
        
        i2c.start();
        i2c.write(EEPROM_CONTROL_BYTE);
        i2c.write(0x12); // address to be written
        i2c.write(0xAA); // data to be written
        i2c.stop();
        wait(0.01);
        
        i2c.start();
        i2c.write(EEPROM_CONTROL_BYTE);
        i2c.write(0x12); // address to be read
        i2c.start(); // repeated start
        i2c.write(EEPROM_CONTROL_BYTE + I2C_READ); // control byte + Read
        data = i2c.read(0); // read one byte of data
        i2c.stop();
          
        
        
    }
}