#include "mbed.h"
#include "LSM9DS0.h"
#include "rtos.h"
#include "uLCD_4DGL.h"

// SDO_XM and SDO_G are pulled up, so our addresses are:
#define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW

//Accelerometer over I2C
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);

//Shiftbrite over SPI and control pins
SPI shiftbrite(p5, p6, p7);
Mutex color_lock;
DigitalOut latch(p11);
DigitalOut enable(p8);

//Potentiometer
AnalogIn dial(p15);

//LCD over serial, and mutex for cross-thread accesses
uLCD_4DGL lcd(p28, p27, p30);
Mutex lcd_lock;

//5-way switch (ActiveLow)
DigitalIn right(p16);
DigitalIn down(p17);
DigitalIn left(p18);
DigitalIn center(p19);
DigitalIn up(p20);

//Color selection output vars
int selector = 0;
DigitalOut red_sel(LED1);
DigitalOut green_sel(LED2);
DigitalOut blue_sel(LED3);

//current location on lcd   NOTE: the 'pixel' drawn is actually 4 pixels (on a 64x64 grid)
int pixel_x = 63;
int pixel_y = 63;
//Color value, locked via mutex for cross-thread access
int color_value = 0x8A2BE2;

//Thread checking for shaking (and clearing LCD if occuring)
void erase_thread(void const*args)
{
    while(true) {
        imu.readAccel();
        printf("%.2f %.2f %.2f\n", imu.ax, imu.ay, imu.az);
        if (imu.ax > .20 || imu.ay > .20 || imu.az > 1.5) { //threshold values for accelerometer 'shaking', raise to lower sensitivity
            lcd_lock.lock();
            lcd.cls();
            lcd_lock.unlock();
        }
        Thread::wait(500);
    }
}

//Thread updating/changing the color
void color_thread(void const*args)
{
    red_sel=1;
    int red=0;
    int green=0;
    int blue=0;
    unsigned int high=0;
    unsigned int low=0;
    while(true) {
        //Figure out which color selector is being used
        if (!center) {
            selector = (selector==2)? 0: selector + 1;
            //Display selector
            red_sel = (selector==0);
            green_sel = (selector==1);
            blue_sel = (selector==2);
        }
        //set the presently selected color value
        if (selector == 0)
            red = dial*0xFF;
        else if (selector == 1)
            green = dial*0xFF;
        else if (selector == 2)
            blue = dial*0xFF;
        //put color values into shiftbrite format
        high = (blue<<4) | ((red&0x3C0)>>6);
        low = (((red&0x3F)<<10)|(green));
        //write to the shiftbrite and latch values
        shiftbrite.write(high);
        shiftbrite.write(low);
        latch=1;
        latch=0;
        //update the color value (for use on the LCD)
        color_lock.lock();
        color_value = (red<<16)| (green<<8) | blue;
        color_lock.unlock();
        Thread::wait(500);
    }

}

//Main thread reads in tactile switch, draws to LCD
int main()
{
    //Initializations and set-up
    imu.begin();
    shiftbrite.format(16,0);
    shiftbrite.frequency(500000);
    enable=0;
    latch=0;
    wait(2);
    //launch threads
    Thread eraser(erase_thread);
    Thread color(color_thread);
    //change in pixel location indicator - reduces time LCD is locked unecessarily
    bool no_movement = false;
    //setup LCD screen
    lcd_lock.lock();
    lcd.background_color(0x606060);
    lcd.cls();
    lcd_lock.unlock();
    while(1) {
        no_movement = false; //flag indicating the joystick has been triggered
        if (!down && pixel_y != 0)
            pixel_y--;
        else if (!up && pixel_y!=63)
            pixel_y++;
        else if (!right && pixel_x!=0)
            pixel_x--;
        else if (!left && pixel_x!=63)
            pixel_x++;
        else
            no_movement = true;
        if (!no_movement) { // update the lcd screen
            lcd_lock.lock();
            color_lock.lock();
            lcd.filled_rectangle(pixel_x<<1, pixel_y<<1, (pixel_x<<1)+1, (pixel_y<<1)+1, color_value);
            color_lock.unlock();
            lcd_lock.unlock();
        }
        Thread::wait(100);
    }
}