File content as of revision 30:c0bc92d009fe:

#include "mbed.h"
#include "WattBob_TextLCD.h"
#include "TCS3472_I2C.h"
#include "MCP23017.h"
#include <string>
#include <time.h>
//#include <future>
#include "globals.h"
#include "commander.h"
#include "fpga.h"

#define BACKLIGHT_ON(INTERFACE) INTERFACE->write_bit(1, 4);
#define BACKLIGHT_OFF(INTERFACE) INTERFACE->write_bit(0, 4);

#define LCDFL() lcd->locate(0,0);
#define LCDSL() lcd->locate(1,0);
#define D_LEDS_OFF() i2cport->write_bit(0, 12); i2cport->write_bit(0, 13); i2cport->write_bit(0, 14); i2cport->write_bit(0, 15);
#define U_LEDS_OFF() myLED1 = 0; myLED2 = 0; myLED3 = 0; myLED4 = 0;

DigitalOut myLED1(LED1);
DigitalOut myLED2(LED2);
DigitalOut myLED3(LED3);
DigitalOut myLED4(LED4);

MCP23017 *i2cport;
WattBob_TextLCD *lcd;
TCS3472_I2C rgbSensor(p28, p27);
Serial      pc(USBTX, USBRX);
uint8_t     rxBuffer[kSmallBufferSize + 1];
int 		rxIndex = 0;
float 		percentageError[3];
float 		adjustedValues[3];
int 		blockCount = 0;
bool 		lastBlockHaz = false;
bool 		recievingResponse = false;
bool 		isBetweenHazValues[3] = { false, false, false };

Commander 	_commander = Commander();
Commander 	*commander = &_commander;
FPGA 		_fpga = FPGA();
extern FPGA		*fpga = &_fpga;

Block defualtHazBlock = Block();
extern Block _HazBlock;
Block *HazBlock = &_HazBlock;

extern PCModes currentMode;

void initInternal();
void initPort(int baudRate=kDefaultBaudRate);
void printPCDetectedText();
void Rx_interrupt();
bool checkColour(int colourValues[]);
void runInServoTestMode();
void displayWaitingLine();
void displayPCStatus();
bool waitForBlock();
void sortBlock();
void runInBreakBeamTestMode();
void turnOffTopLEDs();
void turnOffBottomLEDs();
void runInColourSensorTestMode();
void newHazBlockMode();

int main()
{
    initInternal();
    initPort();

    for (;;) {
        if (connectedToPC == false) {
            pc.printf("DEBUG: PC did not responded.\n");
            lcd->cls();
            i2cport->write_bit(1, 12);
            lcd->locate(0,0);
            lcd->printf("1: Start sorting.");
            lcd->locate(1,0);
            i2cport->write_bit(1,13);
            lcd->printf("2: Connect to PC");
        }

//		wait(0.03);

        int selection = 0;
        do {
            myLED4 = selection;
            selection = readSwitches();
        } while (selection != 1 && selection != 2 && connectedToPC == false);
//		lcd->cls();
        D_LEDS_OFF();
        if (selection == 1) {
            // User selected op 1: Start sorting autonomously.
            i2cport->write_bit(1, 12);
            lcd->cls();
            LCDFL();
            lcd->printf("Starting sorting");
            wait(0.5);
            D_LEDS_OFF();

            for(;;) {
                if (blockCount > 0) {
                    lcd->cls();
                    displayWaitingLine();
                    lcd->locate(1,0);
                    if (lastBlockHaz == true)
                        lcd->printf("Hazardous");
                    else if (lastBlockHaz == false)
                        lcd->printf("Non-Hazardous");
                } else {
                    displayWaitingLine();
                }

                i2cport->write_bit(1, 15);

                // Break and return to main menu i.e. Start Op, or Connect to PC.
                if (waitForBlock() == false) {
                    D_LEDS_OFF();
                    break;
                } else {
                    sortBlock();
                }
            }
        }

        if (selection == 2 || connectedToPC == true) {
            wait(0.1);
            for (;;) {
                displayPCStatus();

                i2cport->write_bit(1, 15);
                int abortOperation = false;
                if (connectedToPC == false)
                    pc.printf(":<pc>connect;");
                while (connectedToPC == false && abortOperation == false) {
                    abortOperation = readSwitches() == 4;
                }

                if (abortOperation == true) {
                    D_LEDS_OFF();
                    break;
                }

                displayPCStatus();

                while (abortOperation == false && connectedToPC == true) {
                    if (currentMode == Maintanence) {
                        displayPCStatus();
                        while (currentMode == Maintanence) {
                            if (runServoTest == true)
                                runInServoTestMode();
                            else if (runBreakBeamTest == true)
                                runInBreakBeamTestMode();
                            else if (runColourSensorTest == true)
                                runInColourSensorTestMode();
                            else if (setNewHazBlock == true)
                            	newHazBlockMode();
                            if (i2cport->read_bit(11) == 1) {
                                if (displayAbortDialog() == true) {
                                    currentMode = None;
                                    pc.printf(":<mbed>mode=none;");
                                } else {
                                    displayPCStatus();
                                }
                            }
                        }
                    } else if (currentMode == Normal) {
                        displayPCStatus();
                        while (currentMode == Normal) {
                            if (currentState == Pause) {
                                lcd->cls();
                                lcd->locate(0,0);
                                lcd->printf("Sorting Paused.");
                                lcd->locate(1,0);
                                lcd->printf("1: Start");
                                int button = 0;
                                i2cport->write_bit(1, 12);
                                i2cport->write_bit(1, 15);
                                while (currentState == Pause && currentMode == Normal) {
                                    button = readSwitches();
                                    if (button == 1) {
                                        pc.printf(":<mbed>sort=start;");
                                        currentState = Start;
                                    } else if (button == 4) {
                                        currentMode = None;
                                        pc.printf(":<mbed>mode=none;");
                                        //                                        goto setModeNone;
                                        break;
                                    }
                                }
                            }
                            if (currentState == Start) {
                                lcd->cls();
                                lcd->locate(0,0);
                                lcd->printf("Sorting mode...");
                                while (currentState == Start && currentMode == Normal) {
                                    if (waitForBlock() == false) {
                                        if (connectedToPC == true && currentState != Pause && currentMode == Normal) {
                                            // TODO: Tell PC to update UI if aborted from MBED.
                                            pc.printf(":<mbed>sort=pause;");
                                            currentState = Pause;
                                            continue;
                                        }else if (connectedToPC == true && currentMode == None) {
                                        	 goto setModeNone;
                                        }else if (connectedToPC == false) {
                                            currentMode = None;
                                            currentState = Pause;
                                            abortOperation = true;
                                        }
                                    } else {
                                        sortBlock();
                                    }
                                }
                            }
                        }
                    } else if (currentMode == None) {
setModeNone:
                        D_LEDS_OFF();
                        i2cport->write_bit(1,15);
                        displayPCStatus();
                        while (currentMode == None && abortOperation == false && connectedToPC == true) {
                            if (i2cport->read_bit(11)) {
                                abortOperation = displayAbortDialog();
                                // Cancel the Abort
                                if (abortOperation == false) {
                                    displayPCStatus();
                                    i2cport->write_bit(1, 15);
                                }
                            }
                        }
                    }
                }

                if (abortOperation == true ) {
                    connectedToPC = false;
                    D_LEDS_OFF();
                    break;
                }
            }
        }

    }
}

// Waits until detects block.
// true if block detected, false if cancelled/connected to PC.
bool waitForBlock()
{
    myLED4 = 0;
    myLED1 = 1;
    if (connectedToPC == false) {
        bool abortOperation = false;
        int blockInserted = 0;
        // Wait until a block is breaking the beam, or button 4 is pressed to abort.
        do {
            blockInserted = fpga->getBeamValue(Top);
            myLED4 = blockInserted;
            if (i2cport->read_bit(11)) {
                abortOperation = displayAbortDialog();
                // Cancel the Abort
                if (abortOperation == false) {
                    displayWaitingLine();
                }
            }
        } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);

        if (abortOperation == true || connectedToPC == true)
            return false;
        else
            return true;
    } else if (connectedToPC == true) {
        bool abortOperation = false;
        int blockInserted = 0;
        // Wait until a block is breaking the beam, or button 4 is pressed to abort.
        pc.printf("INFO: Waiting for block.\n");
        do {
            blockInserted = fpga->getBeamValue(Top);
            myLED4 = blockInserted;
            if (readSwitches() == 4) {
                abortOperation = displayAbortDialog();
                // Cancel the Abort
                if (abortOperation == false) {
                    displayWaitingLine();
                    lcd->printf("for block");
                    i2cport->write_bit(1, 15);
                }
            }
        } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);

        if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode != Normal)
            return false;
        else
            return true;
    }
    return false;
}

void sortBlock()
{
    pc.printf("BLOCK: Sorting block");
    myLED1 = 0;
    myLED2 = 1;
    // Cannot Abort any longer. Block is inserted.
    // Detach rx interrupt until block processed.
    NVIC_DisableIRQ(UART1_IRQn);
    fpga->moveSortingServo(Haz);
    fpga->moveStoppingServo(Go);

    int colourValues[4];
    int averageColourValues[4] = {0, 0, 0, 0};
    int numberOfReadings = 3;
    for (int i = 0; i < 4; i++) {
        rgbSensor.getAllColors(colourValues);
        for (int j = 0; j < 4; j++) {
            averageColourValues[j] += colourValues[j];
        }
    }
    for (int i = 0; i < 4; i++) {
        averageColourValues[i] = averageColourValues[i] / numberOfReadings;
    }

    lastBlockHaz = false;
    lastBlockHaz = checkColour(averageColourValues);
//	lastBlockHaz = checkColour(colourValues);

    if (!lastBlockHaz) {
        fpga->moveSortingServo(NonHaz);
    }

    myLED3 = 1;
    int blockSize;
    while (fpga->checkForBlock() == 0) {  }
    blockSize = fpga->checkForSize();
    if (blockSize == HazBlock->size && lastBlockHaz) {
        //        fpga->moveSortingServo(Haz);
        //        fpga->moveStoppingServo(Go);
        //        blockSize = HazBlock->size;
        while(fpga->getBeamValue(Bottom) == 1) {}
        wait(kServoWait);
        fpga->moveStoppingServo(Stop);
    } else {
        lastBlockHaz = false;
    }
    fpga->moveSortingServo(NonHaz);
    while(fpga->checkForSize()) {}

    if (connectedToPC) {
        for (int i = 0; i < 3; i++) {
            pc.printf("DEBUG:Percentage Error: %.5f. Passed?: %i\n", percentageError[i], isBetweenHazValues[i]);
//            if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinRedError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxRedError[i] * errorMultiplier))
//                pc.printf("DEBUG:%i Pass.\n", i);
        }
        pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], lastBlockHaz, percentageError[0], percentageError[1], percentageError[2]);
        //        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], lastBlockHaz);
    }

    // Re-Attach rx interrupt
    NVIC_EnableIRQ(UART1_IRQn);

    blockCount++;
    myLED3 = 0;
    myLED4 = 1;
    return;
}

/// Called every-time it receives an char from PC.
void Rx_interrupt()
{
    recievingResponse = true;
    char interruptChar = pc.getc();
    // Uncomment to Echo to USB serial to watch data flow
    //    pc.putc(interruptChar);

    NVIC_DisableIRQ(UART1_IRQn);

    if (interruptChar == CommandTypeValue[Query]) {
        commander->decodeCommand(Query);
    } else if (interruptChar == CommandTypeValue[Set]) {
        commander->decodeCommand(Set);
    } else if (interruptChar== CommandTypeValue[Reply]) {
        commander->decodeCommand(Reply);
    }

    NVIC_EnableIRQ(UART1_IRQn);
    recievingResponse = false;
}

void initInternal()
{
    myLED1 = 1;
    i2cport = new MCP23017(p9, p10, 0x40);
    lcd = new WattBob_TextLCD(i2cport);
    BACKLIGHT_ON(i2cport);
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Initilizing...");
//    DefaultHazBlock();
    rgbSensor.enablePowerAndRGBC();
    rgbSensor.setIntegrationTime(gIntegrationTime);
    srand((unsigned)time(NULL));
    myLED2 = 1;
    fpga->moveStoppingServo(Stop);
    fpga->moveSortingServo(NonHaz);
    return;
}

void initPort(int baudRate)
{
    myLED3 = 1;
    pc.baud(baudRate);
    pc.format(8, SerialBase::None, gStopBits);
    pc.attach(&Rx_interrupt, Serial::RxIrq);
	myLED4 = 1;
    pc.printf(":<pc>connect;");
//    wait (0.3);
//    while(recievingResponse == true) {}
    turnOffTopLEDs();
    return;
}

bool checkColour(int colourValues[])
{
    for (int i = 0; i < 3; i++){
    	isBetweenHazValues[i] = false;
    }
    memset(adjustedValues, 0, sizeof(adjustedValues));
    memset(percentageError, 0, sizeof(percentageError));

    /* Harcoded working
    	for (int i = 0; i < 3; i++) {
    		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
    		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];

    		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
    			isBetweenHazValues[i] = true;
    		}
    	}
    */
    /*
    for (int i = 0; i < 3; i++) {
        adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
        percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
		// Don't forget to call DefaultHazBlock in init otherwise currentMin/MaxErr won't be set.
        if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
            isHazColour[i] = true;
        }
    }
    */
    for (int i = 0; i < 3; i++) {
            adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
            percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i];

            if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[HazBlock->colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[HazBlock->colour][i] * errorMultiplier)) {
                isBetweenHazValues[i] = true;
            }
        }

    bool isHazBlock = false;

    if (isBetweenHazValues[0] && isBetweenHazValues[1] && isBetweenHazValues[2]) {
        isHazBlock = true;
    } else {
        isHazBlock = false;
    }
    myLED2 = 0;
    return isHazBlock;
}

void newHazBlockMode(){
	trySetHazBlockAgain:
		pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
        fpga->moveSortingServo(Haz);
//        pc.printf("INFO:Setting new haz block.\n");
        int lowerBeam = 0;
        int higherBeam = 0;
        int colourValues[6][4];
        for (int i = 0; i < 6; i++){
        	memset(colourValues[i], 0, sizeof(colourValues[6]));
        }
        int readingCount = 0;

        lcd->cls();
        lcd->locate(0,0);
        lcd->printf("New haz block");
        
        do {
            higherBeam = fpga->getBeamValue(Top);
            if (readSwitches() == 4) {
                if (displayAbortDialog()) {
                    //TODO: tell pc
                    pc.printf(":<mbed>haz-block=pause;");
                    pc.printf("INFO: Operation aborted form MBED.\n");
                    fpga->moveSortingServo(NonHaz);
                    displayPCStatus();
                    return;
                } else {
                    lcd->cls();
                    lcd->locate(0,0);
                    lcd->printf("New haz block");
                }
            }
        } while (higherBeam != 1 && setNewHazBlock == true);

        if (setNewHazBlock == false) {
        	displayPCStatus();
        	return;
        }
        
        do {
        	rgbSensor.getAllColors(colourValues[readingCount]);
        	readingCount++;
        } while (readingCount < 6 && fpga->getBeamValue(Top) == 1 && fpga->getBeamValue(Bottom) == 0);

        lowerBeam = fpga->getBeamValue(Bottom);
        higherBeam = fpga->getBeamValue(Top);
        if (lowerBeam != 1){
        	lowerBeam = fpga->getBeamValue(Top);
        	while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); }
        	higherBeam = fpga->getBeamValue(Bottom);
        }
        int blockSize = higherBeam;

        int totalComponents[4];
        memset(totalComponents, 0, sizeof(totalComponents));
        for (int k = 0; k < readingCount; k++){
        	for (int i = 0; i < 4; i++){
        		totalComponents[i] += colourValues[k][i];
        	}
        }
        float averageComponents[4];
        memset(averageComponents, 0, sizeof(averageComponents));
        for (int i = 0; i < 4; i++){
        	averageComponents[i] = ((float)totalComponents[i] / (float)readingCount);
        }
		float adjustedValues[3];
		memset(adjustedValues, 0, sizeof(adjustedValues));
        for (int i = 0; i < 3; i++){
        	adjustedValues[i] = averageComponents[i] / averageComponents[3];
        }

        Block::BlockColour detectedColour = Block::Wrong;
        bool matchesColour[3] = {false, false, false};
		
		float percentageError[3];
		memset(percentageError, 0, sizeof(percentageError));
        for (int k = 0; k < 7; k++){
        	for (int i = 0; i < 3; i++) {
        		percentageError[i] = (adjustedValues[i] - kAverageValues[_HazBlock.colour][i]) / kAverageValues[_HazBlock.colour][i];
				pc.printf("DEBUG: Percenage error: %.5f\n", percentageError[i]);
				
        		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[_HazBlock.colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[_HazBlock.colour][i] * errorMultiplier)) {
        			pc.printf("DEBUG: Pass\n");
        			matchesColour[i] = true;
        		}
        		if (matchesColour[0] && matchesColour[1] && matchesColour[2]){
        			detectedColour = static_cast<Block::BlockColour>(k);
        			break;
        		}
        	}
        }

        if (detectedColour != Block::Wrong){
        	pc.printf("ERROR: Could not detect colour.\n");
        	lcd->cls();
        	lcd->printf("1: Try again");
        	lcd->locate(1,0);
        	lcd->printf("2: Revert to last");
        	int button = 0;
        	do {
        		button = readSwitches();
        		if (button == 1){
        			goto trySetHazBlockAgain;
        		}
        	} while (button != 2);
        }

        // Point and literal might not sync...
        _HazBlock.size = static_cast<Block::Size>(blockSize);
        _HazBlock.colour = detectedColour;

        pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);


        pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", _HazBlock.size, kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][2], kMaxError[_HazBlock.colour][0], kMaxError[_HazBlock.colour][1], kMaxError[_HazBlock.colour][2]);
        pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[_HazBlock.colour][0], kAverageValues[_HazBlock.colour][1], kAverageValues[_HazBlock.colour][2], kAverageValues[_HazBlock.colour][3]);
        fpga->moveSortingServo(NonHaz);
}

void printPCDetectedText()
{
    lcd->cls();
    LCDFL();
    lcd->printf("Detected PC.");
    LCDSL();
    lcd->printf("Connecting");
    initPort();
}

bool displayAbortDialog()
{
    while (i2cport->read_bit(11) == 1) {}
    i2cport->write_bit(1, 12);

    lcd->cls();
    LCDFL();
    lcd->printf("Abort?");
    LCDSL();
    lcd->printf("1:Yes, 2,3,4:No");
    int reply = 0;
    do {
        reply = readSwitches();
    } while(reply == 0);

    D_LEDS_OFF();
    if (reply == 1) {
        //		while (i2cport->read_bit(8)) {  }
        return true;
    } else {
        //		while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) {  }
        return false;
    }
}

void printServoInfoOnLCD()
{
    lcd->cls();
    lcd->locate(0,0);
    if (fpga->stoppingServoPosition == Stop)
        lcd->printf("1:Top: Go");
    else if (fpga->stoppingServoPosition == Go)
        lcd->printf("1:Top: Stop");

    lcd->locate(1,0);
    if (fpga->sortingServoPosition == NonHaz)
        lcd->printf("2:Bottom: Haz");
    else if (fpga->sortingServoPosition == Haz)
        lcd->printf("2:Bottom: NonHaz");
}

void printServoInfoOnPC()
{
    if (fpga->stoppingServoPosition == Stop)
        pc.printf(":<servos>1=Stop;");
    else if (fpga->stoppingServoPosition == Go)
        pc.printf(":<servos>1=Go;");

    if (fpga->sortingServoPosition == NonHaz)
        pc.printf(":<servos>2=NonHaz;");
    else if (fpga->sortingServoPosition == Haz)
        pc.printf(":<servos>2=Haz;");
}

void runInServoTestMode()
{
    pc.printf("VALUES:Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i\n:VALUES", Stop, Go, Haz, NonHaz);
    printServoInfoOnPC();
    printServoInfoOnLCD();

    i2cport->write_bit(1, 12);
    i2cport->write_bit(1, 13);
    i2cport->write_bit(1, 15);
    int button = 0;
    bool finished = false;
    do {
        button = readSwitches();

        // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
        if (gToggleServoNumber == 1) {
            button = 1;
            gToggleServoNumber = 0;
        } else if (gToggleServoNumber == 2) {
            button = 2;
            gToggleServoNumber = 0;
        }

        if (button == 1) {
            fpga->toggleStoppingServo();
            printServoInfoOnLCD();
            printServoInfoOnPC();
            wait(kServoWait);
        } else if (button == 2) {
            fpga->toggleSortingServo();
            printServoInfoOnLCD();
            printServoInfoOnPC();
            wait(kServoWait);
        }

        finished = button == 4;
        button = 0;
    } while (finished == false && runServoTest == true);

    D_LEDS_OFF();
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Done servo test");
    fpga->moveSortingServo(NonHaz);
    if (runServoTest == true) {
        pc.printf(":<servos>test=pause;");
        runServoTest = false;
    }
    wait(0.5);
    lcd->cls();
    return;
}

void printBeamInfoOnLCD()
{
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Top:L1 Btm: L2");
    lcd->locate(1,0);
    lcd->printf("On:High, Off:Low");
}

void printBeamInfoOnPC(int topBeam, int bottomBeam)
{
    pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
}

void runInBreakBeamTestMode()
{
    turnOffTopLEDs();
    i2cport->write_bit(1, 15);

    int topBeamValue = fpga->getBeamValue(1);
    int bottomBeamValue = fpga->getBeamValue(2);
    printBeamInfoOnPC(topBeamValue, bottomBeamValue);
    printBeamInfoOnLCD();

    int button = 0;
    bool finished = false;
    do {
        button = readSwitches();
        int currentTopBeamValue = fpga->getBeamValue(1);
        int currentBottomBeamValue = fpga->getBeamValue(2);

        // For debugging, hold down both 1 and 3 or 2 and 3
        if (i2cport->read_bit(10) == 1) {
            currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10);
            currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10);
        }
        myLED1 = currentTopBeamValue;
        myLED2 = currentBottomBeamValue;

        if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ) {
            topBeamValue = currentTopBeamValue;
            bottomBeamValue = currentBottomBeamValue;
            printBeamInfoOnPC(topBeamValue, bottomBeamValue);
        }
        finished = button == 4;
    } while (runBreakBeamTest == true && finished == false);

    turnOffBottomLEDs();
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Finished test");
    if (runBreakBeamTest == true) {
        pc.printf(":<break_beam>test=pause;");
        runBreakBeamTest = false;
    }
    wait(0.5);
    return;
}

void printColourSensorInfoOnLCD(int colourValues[])
{
    float weightedValues[4];

    for (int i = 0; i < 3; i++) {
        weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
    }
    //TODO: Print values on LCD
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Colour sensor");
    lcd->locate(1,0);
    lcd->printf("Test mode");
}

void printColourSensorInfoOnPC(int colourValues[])
{
    pc.printf(":<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
}

void runInColourSensorTestMode()
{
    turnOffTopLEDs();
    i2cport->write_bit(1, 15);
    i2cport->write_bit(1, 12);
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Colour Sensor");
    lcd->locate(1,0);
    lcd->printf("Test Mode");
    pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);

    int button = 0;
    bool finished = false;

    do {
        button = readSwitches();

        if (getColourSensorValue == true) {
            int colourValues[4];
            rgbSensor.getAllColors(colourValues);
            printColourSensorInfoOnPC(colourValues);
            printColourSensorInfoOnLCD(colourValues);
            getColourSensorValue = false;
        } else if (getBlockColourValue == true) {
            lcd->cls();
            lcd->locate(0,0);
            lcd->printf("Drop block");


            turnOffTopLEDs();

            while(fpga->getBeamValue(Top) == 0) {
                myLED1 = fpga->getBeamValue(Top);
                myLED2 = fpga->getBeamValue(Bottom);
            }

            myLED3 = 1;
            int colourValues[3][4];
            for (int i = 0; i < 3; i++) {
                memset(colourValues[i], 0, sizeof(colourValues));
            }
            int valueCount = 0;

            do {
                rgbSensor.getAllColors(colourValues[valueCount]);
                valueCount++;
            } while (fpga->getBeamValue(Top) == 1 && valueCount < 3);

            turnOffTopLEDs();
            myLED4 = 1;

            int averageValues[4] = {0, 0, 0, 0};
            for (int i = 0; i < 3; i++) {
                averageValues[0] += colourValues[i][0];
                averageValues[1] += colourValues[i][1];
                averageValues[2] += colourValues[i][2];
                averageValues[3] += colourValues[i][3];
            }
            averageValues[0] = averageValues[0] / valueCount;
            averageValues[1] = averageValues[1] / valueCount;
            averageValues[2] = averageValues[2] / valueCount;
            averageValues[3] = averageValues[3] / valueCount;
            myLED4 = 0;

            printColourSensorInfoOnPC(averageValues);
            printColourSensorInfoOnLCD(averageValues);

            getBlockColourValue = false;
        } else if (button == 1) {
            getColourSensorValue = true;
            button = 0;
        }

        finished = button == 4;
//		button = 0;
    } while (finished == false && runColourSensorTest == true);

    turnOffBottomLEDs();
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Finished test");
    if (runColourSensorTest == true) {
        pc.printf(":<colour_sensor>test=pause;");
        runColourSensorTest = false;
    }
    wait(0.5);
    return;
}

void displayWaitingLine()
{
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Waiting...");
    lcd->locate(1,0);
}

void displayPCStatus()
{
    lcd->cls();
    lcd->locate(0,0);
    if (connectedToPC == true) {
        if (currentMode == Normal)
            lcd->printf("Normal mode.");
        else if (currentMode == Maintanence)
            lcd->printf("Maintanence.");
        else if (currentMode == None)
            lcd->printf("Connected to PC");
    } else
        lcd->printf("Waiting for PC..");

    lcd->locate(1,0);
    lcd->printf("4:Disconnect");
    D_LEDS_OFF();
    i2cport->write_bit(1,15);
}

void turnOffTopLEDs()
{
    myLED1 = 0;
    myLED2 = 0;
    myLED3 = 0;
    myLED4 = 0;
}

void turnOffBottomLEDs()
{
    i2cport->write_bit(0, 12);
    i2cport->write_bit(0, 13);
    i2cport->write_bit(0, 14);
    i2cport->write_bit(0, 15);
}