#include "mbed.h"
#include "MMA8451Q.h"
#define MMA8451_I2C_ADDRESS (0x1d<<1)

PinName const SDA = PTE0;
PinName const SCL = PTE1;

Serial pc(USBTX, USBRX);

DigitalOut myled(LED1); 
AnalogIn a(PTB0);
DigitalOut myled1(LED2);

PwmOut m1(PTD4);
PwmOut m2(PTA12);
PwmOut m3(PTA4);
PwmOut m4(PTA5);

MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); // Create this once not every loop.

// float i; // not used.

void forward(float i) { 
  m1=0;
  m2=i;
  m3=0;
  m4=i;
}

void backward(float i) {
  m1=i;
  m2=0;
  m3=i;
  m4=0;
}

void right(float i) {
  m1=0;
  m2=i;
  m3=0;
  m4=0;
 }

void left(float i) {
  m1=0;
  m2=0;
  m3=0;
  m4=i;
 }

void stop() { m1=0; m2=0; m3=0; m4=0; }

// float t; // moved to being local to main

Ticker screenUpdate; // timer to only update the screen at a fixed rate.
volatile bool screenUpdateDue = false; // flag to indicate that the screen should be updated.

void onScreenUpdate(void) {  // function called when we need to update the screen
  screenUpdateDue = true;
}


int main() { 

  float range; // replacement for global t. No performance hit for variable names with more than one letter so make the code readable.

  printf("MMA8451 ID: %d\n", acc.getWhoAmI()); // only do this on startup not every loop

  screenUpdate.attach(&onScreenUpdate,1); // attach the update timer

  while(1) {  // main loop
    range=a;        // read the ADC
    if (screenUpdateDue) {  // if we are due to update the screen
      pc.printf("IR Sensor = %.3f\n\r",range); // limit to 3 decimal places.
      screenUpdateDue  = false;
    }

    if (range<0.3) {          // if too close more backwards
     myled=1; myled1=0; backward(1);
    } else {
     myled1=1; myled=0; forward(1); 
    }

    // optionaly add a small delay here 
  } // end while(1)
}