#include "mbed.h"
#include "NokiaLCD.h"
#include "ID12RFID.h"
#include "SRF08.h"

ID12RFID rfid(p10);             // UART RX
NokiaLCD lcd(p5, p7, p8, p9);   // mosi, sclk, cs, rst, type
SRF08 srf08(p28, p27, 0xE0);    // SDA, SCL pin and I2C address
I2C i2c (p28,p27);              // Setup the I2C interface: sda, scl
Serial pc (USBTX,USBRX);

AnalogIn pot1(p15);     // Pot1
AnalogIn pot2(p16);     // Pot2
AnalogIn pot3(p20);     // Pot3

int main() {

    int i = 0;
    int id = 0;
    char stop_led[1] = {'o'};   // BlinkM LED stop command
    char led_data[4] = {'c', 0x00, 0x00, 0x00};

    // set up the LCD, and draw some bands of colour
    lcd.foreground(0x000000);
    lcd.background(0xFFFFFF);
    lcd.cls();
    lcd.fill(2, 11, 128, 10, 0xFF0000);
    lcd.fill(2, 41, 128, 10, 0x00FF00);
    lcd.fill(2, 61, 128, 10, 0x0000FF);

    // A friendly welcome message
    lcd.locate(0,3);
    lcd.printf("ESC Brazil 2012");
    pc.printf("Welcome to ESC Brazil 2012\n\r");
    
    i2c.write(0x00, stop_led, 1);  // Stop previous BlinkM script

    // forever....
    while (1) {

        // Plot the three potentiometers
        lcd.pixel(i, 80 + int(pot1.read() * 50), 0xFF0000);
        lcd.pixel(i, 80 + int(pot2.read() * 50), 0x00FF00);
        lcd.pixel(i, 80 + int(pot3.read() * 50), 0x0000FF);
        
        //Update LED
        led_data[1] = char(pot1.read()*255);
        led_data[2] = char(pot2.read()*255);
        led_data[3] = char(pot3.read()*255);   
        i2c.write(0x00, led_data, 4);
   
        // Blank the canvas when we wrap
        if (i==130) {
            lcd.fill(0, 80, 130, 50, 0xFFFFFF);
            i=0;
        } else {
            i++;
        }

        // Check for ID tag, and Process
        if (rfid.readable()) {
        
            id = rfid.read();
            lcd.locate(0,0);
            lcd.printf("RFID:%d", id );
            pc.printf("Tag ID = %d\n\r",id);                                                                
        }

        // Take and RFIF Reading
        lcd.locate(0,4);
        lcd.printf("SRF:%dcm",int(srf08.read()));

        // give the loop a slower tick
        wait(0.05);
    }
}