/* This program demonstrates the ability of a color sensor made by
 * Atlas Scientific through a shiftbrite LED chasing game.
 * Bi Ge bge6@gatech.edu
 */

#include "mbed.h"
DigitalOut latch(p15);
PwmOut enable(p23); // use PWM instead of DigitalOut to dim the LED
Serial color_sensor(p9, p10);

DigitalOut out_latch(p22);
PwmOut out_enable(p21);
SPI spi(p11, p12, p13); // the random shiftbrite
SPI out_spi(p5, p6, p7); // this shiftbrite is attached to the color sensor

// http://mbed.org/users/4180_1/notebook/shiftbrite1/
void RGB_LED(int red, int green, int blue)
{
    unsigned int low_color=0;
    unsigned int high_color=0;
    high_color=(blue<<4)|((red&0x3C0)>>6);
    low_color=(((red&0x3F)<<10)|(green));
    spi.write(high_color);
    spi.write(low_color);
    latch=1;
    latch=0;
}
/* This function is exactly the same as the one above, the only
 * difference is now it outputs value to the second shiftbrite
 */
void out_RGB_LED(int red, int green, int blue)
{
    unsigned int low_color=0;
    unsigned int high_color=0;
    high_color=(blue<<4)|((red&0x3C0)>>6);
    low_color=(((red&0x3F)<<10)|(green));
    out_spi.write(high_color);
    out_spi.write(low_color);
    out_latch=1;
    out_latch=0;
}
void split_RGB(char* color_str)
{
    char *save_ptr; // this ptr is used by strtok_r to save
    // the current parsing state
    char *p = strtok_r(color_str, ",", &save_ptr); // parse with ","
    int red = strtol(p, NULL, 10);
    p = strtok_r(NULL, ",", &save_ptr);
    int green = strtol(p, NULL, 10);
    p = strtok_r(NULL, ",", &save_ptr);
    int blue = strtol(p, NULL, 10);
    out_RGB_LED(red, green, blue);
    return;
}
void get_color()
{
    if(color_sensor.readable()) {
        char str[11];
        int i = 0;
        char c;
        /*
         * The color sensor sends its output in form
         * "rrr,ggg,bbb\r" in char. It can also output the
         * light intensity. See product document.
         */

        while(c=color_sensor.getc()) {
            // wait for the carriage return
            if(c!='\r') {
                str[i] = c;
                i++;
            } else {
                break;
            }
        }
        split_RGB(str);
    }
    return;
}
int main()
{
    int red=0;
    int green=0;
    int blue=0;
    spi.format(16,0);
    spi.frequency(500000);
    out_spi.format(16,0);
    out_spi.frequency(500000);
    enable=0.5; // set PWM to half, full on is too bright
    latch=0;
    out_enable=0.5;
    out_latch=0;
    color_sensor.baud(38400);
    wait(2);
    while(1) {
        red = rand() % 50;
        green = rand() % 50;
        blue = rand() % 50;
        RGB_LED( red, green, blue);
        /* IMPORTANT: The color sensor can only take a measurement every 620ms
         * Have to wait for the instruction to complete.
         */
        get_color();
        wait(1);
    }


}