#include "mbed.h"
#include "TextLCD.h"
#include "ADXL345_I2C.h"
#include "ITG3200.h"

I2C i2c(p9,p10);//p28,p27
Serial pc(USBTX, USBRX);
I2C i2c_lcd(p9,p10); // sda, scl
ADXL345_I2C accelerometer(p9, p10); //device i2c DevId 
ITG3200 gyro(p9, p10);
TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, 2004A Slaveaddress 0x4E, LCD Type

//TextLCD Textlcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4);

Timer t;

    
int main() {
   //Set highest bandwidth.
    gyro.setLpBandwidth(LPFBW_42HZ); //was gyro1.seLpBandwidth
    
    int readings[3] = {0, 0, 0};
     
    //pc.printf("Starting ADXL345 test...\n");
    lcd.printf("Starting ADXL 345 test...\n");
    wait(1);
    //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
    lcd.printf("Device ID is: 0x%02X\n", accelerometer.getDeviceID());
    wait(1);
    
     // These are here to test whether any of the initialization fails. It will print the failure
    if (accelerometer.setPowerControl(0x00)){
         //pc.printf("didn't intitialize power control\n");
         lcd.printf("didn't intitialize power control\n");
         return 0;  }
     //Full resolution, +/-16g, 4mg/LSB.
     wait(0.1);
     
     if(accelerometer.setDataFormatControl(0x0B)){
        //pc.printf("didn't set data format\n");
        lcd.printf("didn't set data format\n");
        return 0;  }
     wait(0.1);
     
     //3.2kHz data rate.
     if(accelerometer.setDataRate(ADXL345_3200HZ)){
        //pc.printf("didn't set data rate\n");
        lcd.printf("didn't set data rate\n");
        return 0;    }
     wait(0.1);
     
     //Measurement mode.
     
     if(accelerometer.setPowerControl(MeasurementMode)) {
        //pc.printf("didn't set the power control to measurement\n");
        lcd.printf("didn't set the power control to measurement\n");
        return 0;   } 
 
   lcd.cls();
   lcd.printf("I2CU! Searching for I2C devices...\n");
   lcd.setBacklight(TextLCD::LightOn);
   int count = 0;
    for (int address=0; address<256; address+=1) {
        if (!i2c.write(address, NULL, 0)) { // 0 returned is ok
           lcd.printf(" - I2C device found at address 0x%02X\n", address);
           //device 0x4E is lcd display, device ADXL345 or ITG3200 is 0xA6 or 0xD0 ????
            count++;
            wait(1);
            lcd.cls();
        }
    }
    lcd.printf("%d devices found\n", count); 
     wait(1);
    
    t.start();
    while (1){
     
        wait(0.1);
        accelerometer.getOutput(readings);
        //pc.printf("%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
              
        lcd.cls();
        lcd.printf("ITG3200 DevID 0x%02X\n",gyro.getWhoAmI()); //was gyro1.getWhoAmI()
        lcd.printf("ADXL DevID is: 0x%02x\n", accelerometer.getDeviceID());
        lcd.printf("Ax%i Ay%i Az%i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
        lcd.printf("Gx%i Gy%i Gz%i\n\r", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
        //lcd.printf("Temperature %2.1f \n",gyro.getTemperature());
        wait(1);
               
        }
}