#include "HardwareInfo.c"
#include "JMLib.c"
#include <SetWaitForTime.h>
#include <SetLCDBack.h>
#include <SetDisplayString.h>
#include <SetLCDRectangle.h>
#include <GetLeftButton.h>
#include <SetColorSensor.h>
#include <GetRightButton.h>
#include <SetInBeep.h>
#include <GetColorSensor.h>
#include <SetDisplayVar.h>
#include <SetLCDFilledRectangle.h>

int main(void)
{
    EV5_Init();
    unsigned int color = 0;
    unsigned int rcolor = 0;
    unsigned long rgb888 = 0;
    unsigned int result = 0;
    int red = 0;
    int green = 0;
    int blue = 0;
    int btn1 = 0;
    int btn2 = 0;
    unsigned int r = 0;
    unsigned int g = 0;
    unsigned int b = 0;
    SetWaitForTime(0.2);
    SetLCDBack(ON);
    SetDisplayString(1, "P1", YELLOW, BLACK);
    SetDisplayString(2, "Color Sensor", YELLOW, BLACK);
    SetDisplayString(3, "Red", YELLOW, BLACK);
    SetDisplayString(5, "Green", YELLOW, BLACK);
    SetDisplayString(6, "Blue", YELLOW, BLACK);
    SetDisplayString(9, "Color", YELLOW, BLACK);
    SetLCDRectangle(99, 119, 141, 161, 1, YELLOW);
    SetDisplayString(13, "Result", YELLOW, BLACK);
    SetLCDRectangle(99, 187, 141, 229, 1, YELLOW);
    SetDisplayString(3, "------------------------------", YELLOW, BLACK);
    SetDisplayString(7, "------------------------------", YELLOW, BLACK);
    while (1)
    {
        btn1 = GetLeftButton();
        if ( btn1==1 )
        {
            SetColorSensor(_P1_, 31, 0);
            SetDisplayString(19, "White Balance                ", YELLOW, BLACK);
        }
        btn2 = GetRightButton();
        if ( btn2==1 )
        {
            SetColorSensor(_P1_, 32, 0);
            SetDisplayString(19, "Reset White Balance", YELLOW, BLACK);
        }
        if ( btn1==1||btn2==1 )
        {
            SetInBeep(ON);
            SetWaitForTime(0.2);
            SetInBeep(OFF);
        }
        red = GetColorSensor(_P1_, 1);
        green = GetColorSensor(_P1_, 2);
        blue = GetColorSensor(_P1_, 3);
        SetDisplayVar(4, red, YELLOW, BLACK);
        SetDisplayVar(5, green, YELLOW, BLACK);
        SetDisplayVar(6, blue, YELLOW, BLACK);
        rgb888 = GetColorSensor(_P1_, 5);
        if ( rgb888!=999 )
        {
            r=rgb888/65536;;
            g=(rgb888-r*65536)/256;;
            b=rgb888-r*65536-g*256;;
            color=(r/8)*2048+(g/4)*32+b/8;;
        }
        else
        {
            color=BLACK;
        }
        SetLCDFilledRectangle(100, 120, 140, 160, color);
        result = GetColorSensor(_P1_, 4);
        if ( result==1 )
        {
            rcolor=RED;
        }
        else
        {
            if ( result==2 )
            {
                rcolor=GREEN;
            }
            else
            {
                if ( result==3 )
                {
                    rcolor=BLUE;
                }
                else
                {
                    if ( result==4 )
                    {
                        rcolor=YELLOW;
                    }
                    else
                    {
                        if ( result==5 )
                        {
                            rcolor=BLACK;
                        }
                        else
                        {
                            if ( result==6 )
                            {
                                rcolor=WHITE;
                            }
                            else
                            {
                                rcolor=PURPLE;
                            }
                        }
                    }
                }
            }
        }
        SetLCDFilledRectangle(100, 188, 140, 228, rcolor);
        SetDisplayVar(13, result, YELLOW, BLACK);
    }
    while(1);
}

