Rush Hour board game on MBED for 4180 final project

Dependencies:   4DGL-uLCD-SE FATFileSystem SDFileSystem mbed wave_player

main.cpp

Committer:
giavottop
Date:
2018-05-01
Revision:
0:22e251b14893

File content as of revision 0:22e251b14893:

#include "mbed.h"
#include "uLCD_4DGL.h"
#include "Vehicle.h"
#include "SDFileSystem.h"
#include "wave_player.h"



uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin;

AnalogIn vert(p17);
AnalogIn horz(p19);
AnalogIn sel(p20);

SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
AnalogOut DACout(p18);

wave_player waver(&DACout);

Serial pc(USBTX,USBRX);

// In current layout vert->0 = right, vert->1 = left, horz->0 = down, horz->1 = up
// 0=right 1=up 2=left 3=down
int selection = 0;

int stickXloc = 0;
int stickYloc = 0;
int selectedX = -1;
int selectedY = -1;
int selected = 0;

int mainCarX = 0;
int mainCarY = 0;







int rv[36][4] = {{7,1,24,18},{26,1,43,18},{45,1,62,18},{64,1,81,18},{83,1,100,18},{102,1,119,18},
                {7,20,24,37},{26,20,43,37},{45,20,62,37},{64,20,81,37},{83,20,100,37},{102,20,119,37},
                {7,39,24,56},{26,39,43,56},{45,39,62,56},{64,39,81,56},{83,39,100,56},{102,39,119,56},
                {7,58,24,75},{26,58,43,75},{45,58,62,75},{64,58,81,75},{83,58,100,75},{102,58,119,75},
                {7,77,24,94},{26,77,43,94},{45,77,62,94},{64,77,81,94},{83,77,100,94},{102,77,119,94},
                {7,96,24,113},{26,96,43,113},{45,96,62,113},{64,96,81,113},{83,96,100,113},{102,96,119,113}};
                
int spots[6][6] = {{0,1,2,3,4,5},
                  {6,7,8,9,10,11},
                  {12,13,14,15,16,17},
                  {18,19,20,21,22,23},
                  {24,25,26,27,28,29},
                  {30,31,32,33,34,35}};
                  
int open[6][6] =  {{0,0,0,0,0,0},
                  {0,0,0,0,0,0},
                  {0,0,0,0,0,0},
                  {0,0,0,0,0,0},
                  {0,0,0,0,0,0},
                  {0,0,0,0,0,0}};                
int getInput() {
    int direction = -1;
    
    while(direction < 0) {
        float v = vert.read();
        float h = horz.read();
        float s = sel.read();
        
        wait(0.01);
    
        if(v < .2)
            direction = 0;
        else if(v > .8)
            direction = 2;
        else if(h < .2)
            direction = 3;
        else if(h > .8)
            direction = 1;
        else if(s < .01)
            direction = 5;
    }
    
    return direction;
}

void setUpGrid(){
    for(int i = 0; i < 36; i++) {
        uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],BLUE);
    }
}

void updateStickLocation(int direction){
    if(direction == 2 && selected == 0){
        if(stickXloc != 0){
            int i = spots[stickYloc][stickXloc - 1];
            uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
            int j = spots[stickYloc][stickXloc];
            uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],BLUE);
            stickXloc--; 
        }
    }
    if(direction == 1 && selected == 0){
        if(stickYloc != 0){
            int i = spots[stickYloc - 1][stickXloc];
            uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
            int j = spots[stickYloc][stickXloc];
            uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],BLUE);
            stickYloc--; 
        }
    }
    if(direction == 0 && selected == 0){
        if(stickXloc != 5){
            int i = spots[stickYloc][stickXloc + 1];
            uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
            int j = spots[stickYloc][stickXloc];
            uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],BLUE); 
            stickXloc++;
        }
    }
    if(direction == 3 && selected == 0){
        if(stickYloc != 5){
            int i = spots[stickYloc + 1][stickXloc];
            uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
            int j = spots[stickYloc][stickXloc];
            uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],BLUE);
            stickYloc++; 
        }
        
    }
    if(direction == 5){
        if(selected == 0){
            selected = 1;
            selectedX = stickXloc;
            selectedY = stickYloc;
            pc.printf("%i \n\r",selectedX);
        }
        else if(selected == 1){
            selected = 0;
            selectedX = -1;
            selectedY = -1;
            pc.printf("done");
        }
    }
} 

void updateMainCar(int direction){
    int i = spots[mainCarY][mainCarX];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],RED);
    open[mainCarY][mainCarX] = 1;
    int j = spots[mainCarY][mainCarX + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],RED);
    open[mainCarY][mainCarX + 1] = 1;
    if(mainCarX > 0 && open[mainCarY][mainCarX - 1] == 0){
       int p = spots[mainCarY][mainCarX - 1];
       uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(mainCarX < 4 && open[mainCarY][mainCarX + 2] == 0){
       int p = spots[mainCarY][mainCarX + 2];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == mainCarX || selectedX == mainCarX + 1) && selectedY == mainCarY){
        if(direction == 2 && open[mainCarY][mainCarX - 1] == 0){
            if(mainCarX != 0){
                mainCarX--;
                selectedX--;
                stickXloc--; 
                open[mainCarY][mainCarX + 2] = 0; 
            }
        
        }
        if(direction == 0 && open[mainCarY][mainCarX + 2] == 0){
            if(mainCarX != 4){
                mainCarX++;
                selectedX++;
                stickXloc++;
                open[mainCarY][mainCarX - 1] = 0;
            }
        }
        
    } 
}

//void updateHorCar(int direction, Vehicle hold){
//    int i = spots[hold.getY()][hold.getX()];
//    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],GREEN);
//    int j = spots[hold.getY()][hold.getX() + 1];
//    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],GREEN);
//    if(hold.getX() > 0){
//       int p = spots[hold.getY()][hold.getX() - 1];
//        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
//    }
//    if(hold.getX() < 4){
//       int p = spots[hold.getY()][hold.getX() + 2];
//        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
//    }
//    
//    
//    
//    if((selectedX == hold.getX() || selectedX == hold.getX() + 1) && selectedY == hold.getY()){
//        if(direction == 2){
//            if(hold.getX() != 0){
//                hold.dimX();
//                selectedX--;
//                stickXloc--;  
//            }
//        
//        }
//        if(direction == 0){
//            if(hold.getX() != 4){
//                hold.incX();
//                uLCD.printf("%i",hold.getX());
//                selectedX++;
//                stickXloc++;
//            }
//        }
//        
//    } 
//    
//}
//
//void updateVertVehicle(int xPos, int yPos){
//    
//    
//}

void easyLevel(){
    FILE *wave_file1;
        
    wave_file1=fopen("/sd/wavfiles/horn.wav","r");
    waver.play(wave_file1); 
    fclose(wave_file1);
    
    mainCarX = 1;
    mainCarY = 2;
    
    Vehicle car1(0,0);
    Vehicle car2(4,4);
    Vehicle car3(4,0);
    Vehicle truck1(0,5);
    Vehicle truck2(1,0);
    Vehicle truck3(1,3);
    Vehicle truck4(5,2);
    
    uLCD.filled_rectangle(121,39,128,56,RED);
    
    while(1){
        int direction = getInput();
        updateStickLocation(direction);
        updateMainCar(direction);
        //updateHorCar(direction, car1);
        
        
    int i = spots[car1.getY()][car1.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],GREEN);
    open[car1.getY()][car1.getX()] = 1;
    int j = spots[car1.getY()][car1.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],GREEN);
    open[car1.getY()][car1.getX() + 1] = 1;
    if(car1.getX() > 0 && open[car1.getY()][car1.getX() - 1] == 0){
       int p = spots[car1.getY()][car1.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car1.getX() < 4 && open[car1.getY()][car1.getX() + 2] == 0){
       int p = spots[car1.getY()][car1.getX() + 2];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == car1.getX() || selectedX == car1.getX() + 1) && selectedY == car1.getY()){
        if(direction == 2 && open[car1.getY()][car1.getX() - 1] == 0){
            if(car1.getX() != 0){
                car1.dimX();
                selectedX--;
                stickXloc--;
                open[car1.getY()][car1.getX() + 2] = 0;  
            }
        
        }
        if(direction == 0 && open[car1.getY()][car1.getX() + 2] == 0){
            if(car1.getX() != 4){
                car1.incX();
                selectedX++;
                stickXloc++;
                open[car1.getY()][car1.getX() - 1] = 0;
            }
        }
        
    }
    
    i = spots[car2.getY()][car2.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],GREEN);
    open[car2.getY()][car2.getX()] = 1;
    j = spots[car2.getY()][car2.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],GREEN);
    open[car2.getY()][car2.getX() + 1] = 1;
    if(car2.getX() > 0 && open[car2.getY()][car2.getX() - 1] == 0){
       int p = spots[car2.getY()][car2.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car2.getX() < 4 && open[car2.getY()][car2.getX() + 2] == 0){
       int p = spots[car2.getY()][car2.getX() + 2];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == car2.getX() || selectedX == car2.getX() + 1) && selectedY == car2.getY()){
        if(direction == 2 && open[car2.getY()][car2.getX() - 1] == 0){
            if(car2.getX() != 0){
                car2.dimX();
                selectedX--;
                stickXloc--;
                open[car2.getY()][car2.getX() + 2] = 0;  
            }
        
        }
        if(direction == 0 && open[car2.getY()][car2.getX() + 2] == 0){
            if(car2.getX() != 4){
                car2.incX();
                selectedX++;
                stickXloc++;
                open[car2.getY()][car2.getX() - 1] = 0;
            }
        }
        
    }
    
    i = spots[car3.getY()][car3.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],GREEN);
    open[car3.getY()][car3.getX()] = 1;
    j = spots[car3.getY() + 1][car3.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],GREEN);
    open[car3.getY() + 1][car3.getX()] = 1;
    if(car3.getY() > 0 && open[car3.getY() - 1][car3.getX()] == 0){
       int p = spots[car3.getY() - 1][car3.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car3.getY() < 4 && open[car3.getY() + 2][car3.getX()] == 0){
       int p = spots[car3.getY() + 2][car3.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == car3.getY() || selectedY == car3.getY() + 1) && selectedX == car3.getX()){
        if(direction == 1 && open[car3.getY() - 1][car3.getX()] == 0){
            if(car3.getY() != 0){
                car3.dimY();
                selectedY--;
                stickYloc--;
                open[car3.getY() + 2][car3.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[car3.getY() + 2][car3.getX()] == 0){
            if(car3.getY() != 4){
                car3.incY();
                selectedY++;
                stickYloc++;
                open[car3.getY() - 1][car3.getX()] = 0;
            }
        }
        
    }
        
    i = spots[truck1.getY()][truck1.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck1.getY()][truck1.getX()] = 1;
    j = spots[truck1.getY() + 1][truck1.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck1.getY() + 1][truck1.getX()] = 1;
    j = spots[truck1.getY() + 2][truck1.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck1.getY() + 2][truck1.getX()] = 1;
    if(truck1.getY() > 0 && open[truck1.getY() - 1][truck1.getX()] == 0){
       int p = spots[truck1.getY() - 1][truck1.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck1.getY() < 3 && open[truck1.getY() + 3][truck1.getX()] == 0){
       int p = spots[truck1.getY() + 3][truck1.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck1.getY() || selectedY == truck1.getY() + 1 || selectedY == truck1.getY() + 2) && selectedX == truck1.getX()){
        if(direction == 1 && open[truck1.getY() - 1][truck1.getX()] == 0){
            if(truck1.getY() != 0){
                truck1.dimY();
                selectedY--;
                stickYloc--;
                open[truck1.getY() + 3][truck1.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck1.getY() + 3][truck1.getX()] == 0){
            if(truck1.getY() != 3){
                truck1.incY();
                selectedY++;
                stickYloc++;
                open[truck1.getY() - 1][truck1.getX()] = 0;
            }
        }
        
    }
    
    
    
    i = spots[truck2.getY()][truck2.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck2.getY()][truck2.getX()] = 1;
    j = spots[truck2.getY() + 1][truck2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck2.getY() + 1][truck2.getX()] = 1;
    j = spots[truck2.getY() + 2][truck2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck2.getY() + 2][truck2.getX()] = 1;
    if(truck2.getY() > 0 && open[truck2.getY() - 1][truck2.getX()] == 0){
       int p = spots[truck2.getY() - 1][truck2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck2.getY() < 3 && open[truck2.getY() + 3][truck2.getX()] == 0){
       int p = spots[truck2.getY() + 3][truck2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck2.getY() || selectedY == truck2.getY() + 1 || selectedY == truck2.getY() + 2) && selectedX == truck2.getX()){
        if(direction == 1 && open[truck2.getY() - 1][truck2.getX()] == 0){
            if(truck2.getY() != 0){
                truck2.dimY();
                selectedY--;
                stickYloc--;
                open[truck2.getY() + 3][truck2.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck2.getY() + 3][truck2.getX()] == 0){
            if(truck2.getY() != 3){
                truck2.incY();
                selectedY++;
                stickYloc++;
                open[truck2.getY() - 1][truck2.getX()] = 0;
            }
        }
        
    }
    
    i = spots[truck3.getY()][truck3.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck3.getY()][truck3.getX()] = 1;
    j = spots[truck3.getY() + 1][truck3.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck3.getY() + 1][truck3.getX()] = 1;
    j = spots[truck3.getY() + 2][truck3.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck3.getY() + 2][truck3.getX()] = 1;
    if(truck3.getY() > 0 && open[truck3.getY() - 1][truck3.getX()] == 0){
       int p = spots[truck3.getY() - 1][truck3.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck3.getY() < 3 && open[truck3.getY() + 3][truck3.getX()] == 0){
       int p = spots[truck3.getY() + 3][truck3.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck3.getY() || selectedY == truck3.getY() + 1 || selectedY == truck3.getY() + 2) && selectedX == truck3.getX()){
        if(direction == 1 && open[truck3.getY() - 1][truck3.getX()] == 0){
            if(truck3.getY() != 0){
                truck3.dimY();
                selectedY--;
                stickYloc--;
                open[truck3.getY() + 3][truck3.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck3.getY() + 3][truck3.getX()] == 0){
            if(truck3.getY() != 3){
                truck3.incY();
                selectedY++;
                stickYloc++;
                open[truck3.getY() - 1][truck3.getX()] = 0;
            }
        }
        
    }
    
    
    i = spots[truck4.getY()][truck4.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck4.getY()][truck4.getX()] = 1;
    j = spots[truck4.getY()][truck4.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck4.getY()][truck4.getX() + 1] = 1;
    j = spots[truck4.getY()][truck4.getX() + 2];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck4.getY()][truck4.getX() + 2] = 1;
    if(truck4.getX() > 0 && open[truck4.getY()][truck4.getX() - 1] == 0){
       int p = spots[truck4.getY()][truck4.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck4.getX() < 3 && open[truck4.getY()][truck4.getX() + 3] == 0){
       int p = spots[truck4.getY()][truck4.getX() + 3];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == truck4.getX() || selectedX == truck4.getX() + 1 || selectedX == truck4.getX() + 2) && selectedY == truck4.getY()){
        if(direction == 2 && open[truck4.getY()][truck4.getX() - 1] == 0){
            if(truck4.getX() != 0){
                truck4.dimX();
                selectedX--;
                stickXloc--;
                open[truck4.getY()][truck4.getX() + 3] = 0;  
            }
        
        }
        if(direction == 0 && open[truck4.getY()][truck4.getX() + 3] == 0){
            if(truck4.getX() != 3){
                truck4.incX();
                selectedX++;
                stickXloc++;
                open[truck4.getY()][truck4.getX() - 1] = 0;
            }
        }
        
    }
    
    if(mainCarX == 4){
            uLCD.text_width(2); //4X size text
            uLCD.text_height(2);
            uLCD.printf("\nYOU WIN\n");
            
            FILE *wave_file;
        
          for(int i=0; i<4; ++i) {
                wave_file=fopen("/sd/wavfiles/boo_yah.wav","r");
                waver.play(wave_file); 
                fclose(wave_file);
            }
            
            
            wait(2.0);
            return; 
        }
        
        
    }
    
}

void mediumLevel(){
    FILE *wave_file1;
        
    wave_file1=fopen("/sd/wavfiles/horn.wav","r");
    waver.play(wave_file1); 
    fclose(wave_file1);
    
    mainCarX = 0;
    mainCarY = 2;
    
    Vehicle car1(0,1);
    Vehicle car2(0,0);
    Vehicle car3(4,4);
    Vehicle truck1(1,2);
    Vehicle truck2(0,5);
    Vehicle truck3(3,3);
    Vehicle truck4(5,0);
    
    uLCD.filled_rectangle(121,39,128,56,RED);
    
    while(1){
        int direction = getInput();
        updateStickLocation(direction);
        updateMainCar(direction);
        //updateHorCar(direction, car1);
        
        
    int i = spots[car1.getY()][car1.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],GREEN);
    open[car1.getY()][car1.getX()] = 1;
    int j = spots[car1.getY()][car1.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],GREEN);
    open[car1.getY()][car1.getX() + 1] = 1;
    if(car1.getX() > 0 && open[car1.getY()][car1.getX() - 1] == 0){
       int p = spots[car1.getY()][car1.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car1.getX() < 4 && open[car1.getY()][car1.getX() + 2] == 0){
       int p = spots[car1.getY()][car1.getX() + 2];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == car1.getX() || selectedX == car1.getX() + 1) && selectedY == car1.getY()){
        if(direction == 2 && open[car1.getY()][car1.getX() - 1] == 0){
            if(car1.getX() != 0){
                car1.dimX();
                selectedX--;
                stickXloc--;
                open[car1.getY()][car1.getX() + 2] = 0;  
            }
        
        }
        if(direction == 0 && open[car1.getY()][car1.getX() + 2] == 0){
            if(car1.getX() != 4){
                car1.incX();
                selectedX++;
                stickXloc++;
                open[car1.getY()][car1.getX() - 1] = 0;
            }
        }
        
    }
    
    i = spots[car2.getY()][car2.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],DGREY);
    open[car2.getY()][car2.getX()] = 1;
    j = spots[car2.getY() + 1][car2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],DGREY);
    open[car2.getY() + 1][car2.getX()] = 1;
    if(car2.getY() > 0 && open[car2.getY() - 1][car2.getX()] == 0){
       int p = spots[car2.getY() - 1][car2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car2.getY() < 4 && open[car2.getY() + 2][car2.getX()] == 0){
       int p = spots[car2.getY() + 2][car2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == car2.getY() || selectedY == car2.getY() + 1) && selectedX == car2.getX()){
        if(direction == 1 && open[car2.getY() - 1][car2.getX()] == 0){
            if(car2.getY() != 0){
                car2.dimY();
                selectedY--;
                stickYloc--;
                open[car2.getY() + 2][car2.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[car2.getY() + 2][car2.getX()] == 0){
            if(car2.getY() != 4){
                car2.incY();
                selectedY++;
                stickYloc++;
                open[car2.getY() - 1][car2.getX()] = 0;
            }
        }
        
    }
    
    i = spots[car3.getY()][car3.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],DGREY);
    open[car3.getY()][car3.getX()] = 1;
    j = spots[car3.getY() + 1][car3.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],DGREY);
    open[car3.getY() + 1][car3.getX()] = 1;
    if(car3.getY() > 0 && open[car3.getY() - 1][car3.getX()] == 0){
       int p = spots[car3.getY() - 1][car3.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car3.getY() < 4 && open[car3.getY() + 2][car3.getX()] == 0){
       int p = spots[car3.getY() + 2][car3.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == car3.getY() || selectedY == car3.getY() + 1) && selectedX == car3.getX()){
        if(direction == 1 && open[car3.getY() - 1][car3.getX()] == 0){
            if(car3.getY() != 0){
                car3.dimY();
                selectedY--;
                stickYloc--;
                open[car3.getY() + 2][car3.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[car3.getY() + 2][car3.getX()] == 0){
            if(car3.getY() != 4){
                car3.incY();
                selectedY++;
                stickYloc++;
                open[car3.getY() - 1][car3.getX()] = 0;
            }
        }
        
    }
        
    i = spots[truck1.getY()][truck1.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck1.getY()][truck1.getX()] = 1;
    j = spots[truck1.getY() + 1][truck1.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck1.getY() + 1][truck1.getX()] = 1;
    j = spots[truck1.getY() + 2][truck1.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck1.getY() + 2][truck1.getX()] = 1;
    if(truck1.getY() > 0 && open[truck1.getY() - 1][truck1.getX()] == 0){
       int p = spots[truck1.getY() - 1][truck1.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck1.getY() < 3 && open[truck1.getY() + 3][truck1.getX()] == 0){
       int p = spots[truck1.getY() + 3][truck1.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck1.getY() || selectedY == truck1.getY() + 1 || selectedY == truck1.getY() + 2) && selectedX == truck1.getX()){
        if(direction == 1 && open[truck1.getY() - 1][truck1.getX()] == 0){
            if(truck1.getY() != 0){
                truck1.dimY();
                selectedY--;
                stickYloc--;
                open[truck1.getY() + 3][truck1.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck1.getY() + 3][truck1.getX()] == 0){
            if(truck1.getY() != 3){
                truck1.incY();
                selectedY++;
                stickYloc++;
                open[truck1.getY() - 1][truck1.getX()] = 0;
            }
        }
        
    }
    
    
    
    i = spots[truck2.getY()][truck2.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck2.getY()][truck2.getX()] = 1;
    j = spots[truck2.getY() + 1][truck2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck2.getY() + 1][truck2.getX()] = 1;
    j = spots[truck2.getY() + 2][truck2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck2.getY() + 2][truck2.getX()] = 1;
    if(truck2.getY() > 0 && open[truck2.getY() - 1][truck2.getX()] == 0){
       int p = spots[truck2.getY() - 1][truck2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck2.getY() < 3 && open[truck2.getY() + 3][truck2.getX()] == 0){
       int p = spots[truck2.getY() + 3][truck2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck2.getY() || selectedY == truck2.getY() + 1 || selectedY == truck2.getY() + 2) && selectedX == truck2.getX()){
        if(direction == 1 && open[truck2.getY() - 1][truck2.getX()] == 0){
            if(truck2.getY() != 0){
                truck2.dimY();
                selectedY--;
                stickYloc--;
                open[truck2.getY() + 3][truck2.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck2.getY() + 3][truck2.getX()] == 0){
            if(truck2.getY() != 3){
                truck2.incY();
                selectedY++;
                stickYloc++;
                open[truck2.getY() - 1][truck2.getX()] = 0;
            }
        }
        
    }
    
    i = spots[truck3.getY()][truck3.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], GREEN);
    open[truck3.getY()][truck3.getX()] = 1;
    j = spots[truck3.getY()][truck3.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], GREEN);
    open[truck3.getY()][truck3.getX() + 1] = 1;
    j = spots[truck3.getY()][truck3.getX() + 2];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], GREEN);
    open[truck3.getY()][truck3.getX() + 2] = 1;
    if(truck3.getX() > 0 && open[truck3.getY()][truck3.getX() - 1] == 0){
       int p = spots[truck3.getY()][truck3.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck3.getX() < 3 && open[truck3.getY()][truck3.getX() + 3] == 0){
       int p = spots[truck3.getY()][truck3.getX() + 3];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == truck3.getX() || selectedX == truck3.getX() + 1 || selectedX == truck3.getX() + 2) && selectedY == truck3.getY()){
        if(direction == 2 && open[truck3.getY()][truck3.getX() - 1] == 0){
            if(truck3.getX() != 0){
                truck3.dimX();
                selectedX--;
                stickXloc--;
                open[truck3.getY()][truck3.getX() + 3] = 0;  
            }
        
        }
        if(direction == 0 && open[truck3.getY()][truck3.getX() + 3] == 0){
            if(truck3.getX() != 3){
                truck3.incX();
                selectedX++;
                stickXloc++;
                open[truck3.getY()][truck3.getX() - 1] = 0;
            }
        }
        
    }
    
    
    i = spots[truck4.getY()][truck4.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck4.getY()][truck4.getX()] = 1;
    j = spots[truck4.getY()][truck4.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck4.getY()][truck4.getX() + 1] = 1;
    j = spots[truck4.getY()][truck4.getX() + 2];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck4.getY()][truck4.getX() + 2] = 1;
    if(truck4.getX() > 0 && open[truck4.getY()][truck4.getX() - 1] == 0){
       int p = spots[truck4.getY()][truck4.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck4.getX() < 3 && open[truck4.getY()][truck4.getX() + 3] == 0){
       int p = spots[truck4.getY()][truck4.getX() + 3];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == truck4.getX() || selectedX == truck4.getX() + 1 || selectedX == truck4.getX() + 2) && selectedY == truck4.getY()){
        if(direction == 2 && open[truck4.getY()][truck4.getX() - 1] == 0){
            if(truck4.getX() != 0){
                truck4.dimX();
                selectedX--;
                stickXloc--;
                open[truck4.getY()][truck4.getX() + 3] = 0;  
            }
        
        }
        if(direction == 0 && open[truck4.getY()][truck4.getX() + 3] == 0){
            if(truck4.getX() != 3){
                truck4.incX();
                selectedX++;
                stickXloc++;
                open[truck4.getY()][truck4.getX() - 1] = 0;
            }
        }
        
    }
    
    if(mainCarX == 4){
            uLCD.text_width(2); //4X size text
            uLCD.text_height(2);
            uLCD.printf("\nYOU WIN\n");
            
           FILE *wave_file;
        
          for(int i=0; i<4; ++i) {
                wave_file=fopen("/sd/wavfiles/boo_yah.wav","r");
                waver.play(wave_file); 
                fclose(wave_file);
            }
            
            
            wait(2.0);
            return; 
        }
        
        
    }
    
}


void hardLevel(){
    FILE *wave_file1;
        
    wave_file1=fopen("/sd/wavfiles/horn.wav","r");
    waver.play(wave_file1); 
    fclose(wave_file1);
    
    
    mainCarX = 1;
    mainCarY = 2;
    
    Vehicle car1(0,0);
    Vehicle car2(0,2);
//    Vehicle car3(4,4);
    Vehicle truck1(1,0);
    Vehicle truck2(0,3);
    Vehicle truck3(3,1);
    Vehicle truck4(5,3);
    
    uLCD.filled_rectangle(121,39,128,56,RED);
    
    while(1){
        int direction = getInput();
        updateStickLocation(direction);
        updateMainCar(direction);
        //updateHorCar(direction, car1);
        
        
    int i = spots[car1.getY()][car1.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],GREEN);
    open[car1.getY()][car1.getX()] = 1;
    int j = spots[car1.getY()][car1.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],GREEN);
    open[car1.getY()][car1.getX() + 1] = 1;
    if(car1.getX() > 0 && open[car1.getY()][car1.getX() - 1] == 0){
       int p = spots[car1.getY()][car1.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car1.getX() < 4 && open[car1.getY()][car1.getX() + 2] == 0){
       int p = spots[car1.getY()][car1.getX() + 2];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == car1.getX() || selectedX == car1.getX() + 1) && selectedY == car1.getY()){
        if(direction == 2 && open[car1.getY()][car1.getX() - 1] == 0){
            if(car1.getX() != 0){
                car1.dimX();
                selectedX--;
                stickXloc--;
                open[car1.getY()][car1.getX() + 2] = 0;  
            }
        
        }
        if(direction == 0 && open[car1.getY()][car1.getX() + 2] == 0){
            if(car1.getX() != 4){
                car1.incX();
                selectedX++;
                stickXloc++;
                open[car1.getY()][car1.getX() - 1] = 0;
            }
        }
        
    }
    
    i = spots[car2.getY()][car2.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],DGREY);
    open[car2.getY()][car2.getX()] = 1;
    j = spots[car2.getY() + 1][car2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],DGREY);
    open[car2.getY() + 1][car2.getX()] = 1;
    if(car2.getY() > 0 && open[car2.getY() - 1][car2.getX()] == 0){
       int p = spots[car2.getY() - 1][car2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(car2.getY() < 4 && open[car2.getY() + 2][car2.getX()] == 0){
       int p = spots[car2.getY() + 2][car2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == car2.getY() || selectedY == car2.getY() + 1) && selectedX == car2.getX()){
        if(direction == 1 && open[car2.getY() - 1][car2.getX()] == 0){
            if(car2.getY() != 0){
                car2.dimY();
                selectedY--;
                stickYloc--;
                open[car2.getY() + 2][car2.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[car2.getY() + 2][car2.getX()] == 0){
            if(car2.getY() != 4){
                car2.incY();
                selectedY++;
                stickYloc++;
                open[car2.getY() - 1][car2.getX()] = 0;
            }
        }
        
    }
    
//    i = spots[car3.getY()][car3.getX()];
//    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],DGREY);
//    open[car3.getY()][car3.getX()] = 1;
//    j = spots[car3.getY() + 1][car3.getX()];
//    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3],DGREY);
//    open[car3.getY() + 1][car3.getX()] = 1;
//    if(car3.getY() > 0 && open[car3.getY() - 1][car3.getX()] == 0){
//       int p = spots[car3.getY() - 1][car3.getX()];
//        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
//    }
//    if(car3.getY() < 4 && open[car3.getY() + 2][car3.getX()] == 0){
//       int p = spots[car3.getY() + 2][car3.getX()];
//        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
//    }
//    
//    
//    
//    if((selectedY == car3.getY() || selectedY == car3.getY() + 1) && selectedX == car3.getX()){
//        if(direction == 1 && open[car3.getY() - 1][car3.getX()] == 0){
//            if(car3.getY() != 0){
//                car3.dimY();
//                selectedY--;
//                stickYloc--;
//                open[car3.getY() + 2][car3.getX()] = 0;  
//            }
//        
//        }
//        if(direction == 3 && open[car3.getY() + 2][car3.getX()] == 0){
//            if(car3.getY() != 4){
//                car3.incY();
//                selectedY++;
//                stickYloc++;
//                open[car3.getY() - 1][car3.getX()] = 0;
//            }
//        }
//        
//    }
        
    i = spots[truck1.getY()][truck1.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck1.getY()][truck1.getX()] = 1;
    j = spots[truck1.getY() + 1][truck1.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck1.getY() + 1][truck1.getX()] = 1;
    j = spots[truck1.getY() + 2][truck1.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck1.getY() + 2][truck1.getX()] = 1;
    if(truck1.getY() > 0 && open[truck1.getY() - 1][truck1.getX()] == 0){
       int p = spots[truck1.getY() - 1][truck1.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck1.getY() < 3 && open[truck1.getY() + 3][truck1.getX()] == 0){
       int p = spots[truck1.getY() + 3][truck1.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck1.getY() || selectedY == truck1.getY() + 1 || selectedY == truck1.getY() + 2) && selectedX == truck1.getX()){
        if(direction == 1 && open[truck1.getY() - 1][truck1.getX()] == 0){
            if(truck1.getY() != 0){
                truck1.dimY();
                selectedY--;
                stickYloc--;
                open[truck1.getY() + 3][truck1.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck1.getY() + 3][truck1.getX()] == 0){
            if(truck1.getY() != 3){
                truck1.incY();
                selectedY++;
                stickYloc++;
                open[truck1.getY() - 1][truck1.getX()] = 0;
            }
        }
        
    }
    
    
    
    i = spots[truck2.getY()][truck2.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], 0xFFFF00);
    open[truck2.getY()][truck2.getX()] = 1;
    j = spots[truck2.getY() + 1][truck2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck2.getY() + 1][truck2.getX()] = 1;
    j = spots[truck2.getY() + 2][truck2.getX()];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], 0xFFFF00);
    open[truck2.getY() + 2][truck2.getX()] = 1;
    if(truck2.getY() > 0 && open[truck2.getY() - 1][truck2.getX()] == 0){
       int p = spots[truck2.getY() - 1][truck2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck2.getY() < 3 && open[truck2.getY() + 3][truck2.getX()] == 0){
       int p = spots[truck2.getY() + 3][truck2.getX()];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedY == truck2.getY() || selectedY == truck2.getY() + 1 || selectedY == truck2.getY() + 2) && selectedX == truck2.getX()){
        if(direction == 1 && open[truck2.getY() - 1][truck2.getX()] == 0){
            if(truck2.getY() != 0){
                truck2.dimY();
                selectedY--;
                stickYloc--;
                open[truck2.getY() + 3][truck2.getX()] = 0;  
            }
        
        }
        if(direction == 3 && open[truck2.getY() + 3][truck2.getX()] == 0){
            if(truck2.getY() != 3){
                truck2.incY();
                selectedY++;
                stickYloc++;
                open[truck2.getY() - 1][truck2.getX()] = 0;
            }
        }
        
    }
    
    i = spots[truck3.getY()][truck3.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], GREEN);
    open[truck3.getY()][truck3.getX()] = 1;
    j = spots[truck3.getY()][truck3.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], GREEN);
    open[truck3.getY()][truck3.getX() + 1] = 1;
    j = spots[truck3.getY()][truck3.getX() + 2];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], GREEN);
    open[truck3.getY()][truck3.getX() + 2] = 1;
    if(truck3.getX() > 0 && open[truck3.getY()][truck3.getX() - 1] == 0){
       int p = spots[truck3.getY()][truck3.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck3.getX() < 3 && open[truck3.getY()][truck3.getX() + 3] == 0){
       int p = spots[truck3.getY()][truck3.getX() + 3];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == truck3.getX() || selectedX == truck3.getX() + 1 || selectedX == truck3.getX() + 2) && selectedY == truck3.getY()){
        if(direction == 2 && open[truck3.getY()][truck3.getX() - 1] == 0){
            if(truck3.getX() != 0){
                truck3.dimX();
                selectedX--;
                stickXloc--;
                open[truck3.getY()][truck3.getX() + 3] = 0;  
            }
        
        }
        if(direction == 0 && open[truck3.getY()][truck3.getX() + 3] == 0){
            if(truck3.getX() != 3){
                truck3.incX();
                selectedX++;
                stickXloc++;
                open[truck3.getY()][truck3.getX() - 1] = 0;
            }
        }
        
    }
    
    
    i = spots[truck4.getY()][truck4.getX()];
    uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3], GREEN);
    open[truck4.getY()][truck4.getX()] = 1;
    j = spots[truck4.getY()][truck4.getX() + 1];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], GREEN);
    open[truck4.getY()][truck4.getX() + 1] = 1;
    j = spots[truck4.getY()][truck4.getX() + 2];
    uLCD.filled_rectangle(rv[j][0],rv[j][1],rv[j][2],rv[j][3], GREEN);
    open[truck4.getY()][truck4.getX() + 2] = 1;
    if(truck4.getX() > 0 && open[truck4.getY()][truck4.getX() - 1] == 0){
       int p = spots[truck4.getY()][truck4.getX() - 1];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    if(truck4.getX() < 3 && open[truck4.getY()][truck4.getX() + 3] == 0){
       int p = spots[truck4.getY()][truck4.getX() + 3];
        uLCD.filled_rectangle(rv[p][0],rv[p][1],rv[p][2],rv[p][3],BLUE); 
    }
    
    
    
    if((selectedX == truck4.getX() || selectedX == truck4.getX() + 1 || selectedX == truck4.getX() + 2) && selectedY == truck4.getY()){
        if(direction == 2 && open[truck4.getY()][truck4.getX() - 1] == 0){
            if(truck4.getX() != 0){
                truck4.dimX();
                selectedX--;
                stickXloc--;
                open[truck4.getY()][truck4.getX() + 3] = 0;  
            }
        
        }
        if(direction == 0 && open[truck4.getY()][truck4.getX() + 3] == 0){
            if(truck4.getX() != 3){
                truck4.incX();
                selectedX++;
                stickXloc++;
                open[truck4.getY()][truck4.getX() - 1] = 0;
            }
        }
        
    }
    
    if(mainCarX == 4){
            uLCD.text_width(2); //4X size text
            uLCD.text_height(2);
            uLCD.printf("\nYOU WIN\n");
            
            FILE *wave_file;
        
          for(int i=0; i<4; ++i) {
                wave_file=fopen("/sd/wavfiles/boo_yah.wav","r");
                waver.play(wave_file); 
                fclose(wave_file);
            }
            
            
            wait(2.0);
            return; 
        }
        
        
    }
    
}




int main() {

 //   uLCD.baudrate(3000000);
    
    uLCD.textbackground_color(BLACK);
    uLCD.color(RED);
    
    uLCD.locate(7,4);
    uLCD.printf("Easy");
    uLCD.locate(6,7);
    uLCD.printf("Medium");
    uLCD.locate(7,11);
    uLCD.printf("Hard");
    
    uLCD.line(22, 42, 106, 42, WHITE);
    
        while(1) {
        
        int direction = getInput();
        
        while(direction == 0 || direction == 2)
            direction = getInput();
        
        // If clicked in
        if(direction == 5) {
            if(selection == 0) {
                uLCD.cls();
                wait(0.2);
                setUpGrid();
                int i = spots[stickYloc][stickXloc];
                uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
                easyLevel();
            }
            else if(selection == 1) {
                uLCD.cls();
                wait(0.2);
                setUpGrid();
                int i = spots[stickYloc][stickXloc];
                uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
                mediumLevel();
            }
            else if(selection == 2) {
                uLCD.cls();
                wait(0.2);
                setUpGrid();
                int i = spots[stickYloc][stickXloc];
                uLCD.filled_rectangle(rv[i][0],rv[i][1],rv[i][2],rv[i][3],WHITE);
                hardLevel();   
            }
            
            uLCD.cls();
            wait(0.2);
            uLCD.locate(7,4);
            uLCD.printf("Easy ");
            uLCD.locate(6,7);
            uLCD.printf("Medium");
            uLCD.locate(7,11);
            uLCD.printf("Hard");
            
            uLCD.line(22, 42, 106, 42, WHITE);
        }
        else if(direction == 1 && (selection == 1 || selection == 2)) {
            selection = selection-1;
            if(selection == 0) {  
                uLCD.line(22, 42, 106, 42, WHITE);
                uLCD.line(22, 68, 106, 68, BLACK);
                uLCD.line(22, 98, 106, 98, BLACK);
            }
            else {
                uLCD.line(22, 42, 106, 42, BLACK);
                uLCD.line(22, 68, 106, 68, WHITE);
                uLCD.line(22, 98, 106, 98, BLACK); 
            }
        }
        else if(direction == 3 && (selection == 0 || selection == 1)) {
            selection = selection+1;
            if(selection == 1) {
                uLCD.line(22, 42, 106, 42, BLACK);
                uLCD.line(22, 68, 106, 68, WHITE);
                uLCD.line(22, 98, 106, 98, BLACK);
            }
            else {
                uLCD.line(22, 42, 106, 42, BLACK);
                uLCD.line(22, 68, 106, 68, BLACK);
                uLCD.line(22, 98, 106, 98, WHITE);               
            }
        }
        
        //Wait to essentialially debounce (want to flick switch)
        wait(0.3);
           
    }
}