File content as of revision 7:2ce6e90f6d47:

#include "Graphics.h"

//***********
//  Sprites
//***********

int flag[8][8] = {
    {4,1,1,1,1,1,1,1},
    {4,1,1,0,1,0,1,1},
    {4,1,0,1,0,1,1,1},
    {4,1,1,0,1,0,1,1},
    {4,1,1,1,1,1,1,1},
    {4,4,4,4,4,4,1,1},
    {4,4,4,4,4,4,1,1},
    {4,4,4,4,4,4,1,1},
};

//CARS

int basic_car[8][8] = {
    {4,4,4,4,4,4,4,4},
    {4,4,4,4,4,4,4,4},
    {4,4,1,1,1,1,4,4},
    {4,1,0,0,0,0,1,4},
    {4,1,0,0,0,0,1,4},
    {1,0,0,0,0,0,0,1},
    {1,1,1,1,1,1,1,1},
    {1,1,4,4,4,4,1,1},

};

int offroad_car[8][8] = {
    {4,4,4,4,4,4,4,4},
    {4,1,1,1,1,1,1,4},
    {4,1,0,1,1,0,1,4},
    {4,1,1,0,0,1,1,4},
    {4,1,1,0,0,1,1,4},
    {1,1,0,1,1,0,1,1},
    {1,1,1,1,1,1,1,1},
    {1,1,4,4,4,4,1,1}
};

int drifter_car[8][8] = {
    {4,4,4,4,4,4,4,4},
    {1,1,4,4,4,4,1,1},
    {4,4,1,1,1,1,4,4},
    {4,1,0,0,0,0,1,4},
    {4,1,0,0,0,0,1,4},
    {1,4,1,1,1,1,4,1},
    {1,1,1,1,1,1,1,1},
    {1,4,4,4,4,4,4,1},

};

int sports_car[8][8] = {
    {4,4,4,4,4,4,4,4},
    {4,4,4,4,4,4,4,4},
    {4,4,4,4,4,4,4,4},
    {1,1,4,4,4,4,1,1},
    {4,4,1,1,1,1,4,4},
    {4,1,0,0,0,0,1,4},
    {1,1,1,1,1,1,1,1},
    {1,1,4,4,4,4,1,1},

};

int race_car[8][8] = {
    {4,4,4,4,4,4,4,4},
    {4,4,4,4,4,4,4,4},
    {4,1,1,1,1,1,1,4},
    {4,4,4,1,1,4,4,4},
    {4,4,4,1,1,4,4,4},
    {1,4,1,1,1,1,4,1},
    {1,1,1,0,0,1,1,1},
    {1,4,1,1,1,1,4,1},
};

int stupid_car[8][8] = {
    {4,4,4,4,4,4,4,4},
    {4,4,4,1,1,4,4,4},
    {4,4,1,0,0,1,4,4},
    {4,1,1,0,0,1,1,4},
    {1,0,1,1,1,1,0,1},
    {1,0,0,0,0,0,0,1},
    {4,1,1,1,1,1,1,4},
    {4,4,4,4,4,4,4,4}
};

int ghost[8][8] = {
    {0,0,4,4,4,4,0,0},
    {0,1,0,4,4,0,1,0},
    {4,0,1,0,0,1,0,4},
    {4,4,0,1,1,0,4,4},
    {4,4,0,1,1,0,4,4},
    {4,0,1,0,0,1,0,4},
    {0,1,0,4,4,0,1,0},
    {0,0,4,4,4,4,0,0},
};

int count_down_3[17][17] = {
    {4,4,4,4,0,0,0,0,0,0,0,4,4,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,0,4,4,4,4,4},
    {4,4,0,1,1,1,1,1,1,1,1,1,0,4,4,4,4},
    {4,0,1,1,1,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,0,1,1,1,1,0,0,0,0,1,1,1,1,0,4,4},
    {4,0,1,1,1,0,4,4,4,4,0,1,1,1,0,4,4},
    {4,0,1,1,1,0,4,4,0,0,1,1,1,1,0,4,4},
    {4,4,0,0,0,4,4,0,1,1,1,1,1,0,4,4,4},
    {4,4,4,4,4,4,4,0,1,1,1,1,0,4,4,4,4},
    {4,4,0,0,0,4,4,0,1,1,1,1,1,0,4,4,4},
    {4,0,1,1,1,0,4,4,0,0,1,1,1,1,0,4,4},
    {4,0,1,1,1,0,4,4,4,4,0,1,1,1,0,4,4},
    {4,0,1,1,1,1,0,0,0,0,1,1,1,1,0,4,4},
    {4,4,0,1,1,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,1,0,4,4,4,4},
    {4,4,4,4,0,1,1,1,1,1,1,0,4,4,4,4,4},
    {4,4,4,4,4,0,0,0,0,0,0,4,4,4,4,4,4}
};

int count_down_2[17][17] = {
    {4,4,4,4,4,0,0,0,0,0,0,4,4,4,4,4,4},
    {4,4,4,4,0,1,1,1,1,1,1,0,4,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,1,0,4,4,4,4},
    {4,4,0,1,1,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,0,1,1,1,1,0,0,0,0,1,1,1,1,0,4,4},
    {4,0,1,1,1,0,4,4,4,0,1,1,1,1,0,4,4},
    {4,0,1,1,1,0,4,4,0,1,1,1,1,1,0,4,4},
    {4,4,0,0,0,4,4,0,1,1,1,1,1,0,4,4,4},
    {4,4,4,4,4,4,0,1,1,1,1,1,0,4,4,4,4},
    {4,4,4,4,4,0,1,1,1,1,1,0,4,4,4,4,4},
    {4,4,4,4,0,1,1,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,0,4,4,4,4,4,4,4},
    {4,4,0,1,1,1,1,1,0,0,0,0,0,0,4,4,4},
    {4,0,1,1,1,1,1,1,1,1,1,1,1,1,0,4,4},
    {4,0,1,1,1,1,1,1,1,1,1,1,1,1,0,4,4},
    {4,0,1,1,1,1,1,1,1,1,1,1,1,1,0,4,4},
    {4,4,0,0,0,0,0,0,0,0,0,0,0,0,4,4,4}
};

int count_down_1[17][17] = {
    {4,4,4,4,4,4,4,0,0,0,4,4,4,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,0,1,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,0,1,1,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,0,0,0,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,0,0,1,1,1,1,1,0,0,4,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,4,4,4,0,0,0,0,0,0,0,0,0,4,4,4,4}
};

int count_down_0[18][18] = {
    {4,4,4,0,0,0,0,4,4,4,4,4,4,4,4,4,4,4},
    {4,4,0,1,1,1,1,0,4,4,4,4,4,4,4,4,4,4},
    {4,0,1,1,1,1,1,0,4,4,4,4,4,4,4,4,4,4},
    {0,1,1,1,0,0,0,4,4,4,4,4,4,4,4,4,4,4},
    {0,1,1,0,0,0,0,0,0,0,4,4,4,4,4,4,4,4},
    {0,1,1,0,0,1,1,1,1,0,4,4,4,4,4,4,4,4},
    {0,1,1,0,0,1,1,1,1,0,4,0,0,0,0,4,4,4},
    {0,1,1,0,0,0,0,1,1,0,0,1,1,1,1,0,4,4},
    {0,1,1,1,0,0,1,1,1,0,1,1,1,1,1,1,0,4},
    {4,0,1,1,1,1,1,1,0,1,1,1,0,0,1,1,1,0},
    {4,4,0,1,1,1,1,0,0,1,1,0,0,0,0,1,1,0},
    {4,4,4,0,0,0,0,4,0,1,1,0,0,0,0,1,1,0},
    {4,4,4,4,4,4,4,4,0,1,1,0,0,0,0,1,1,0},
    {4,4,4,4,4,4,4,4,0,1,1,0,0,0,0,1,1,0},
    {4,4,4,4,4,4,4,4,0,1,1,1,0,0,1,1,1,0},
    {4,4,4,4,4,4,4,4,4,0,1,1,1,1,1,1,0,4},
    {4,4,4,4,4,4,4,4,4,4,0,1,1,1,1,0,4,4},
    {4,4,4,4,4,4,4,4,4,4,4,0,0,0,0,4,4,4},
};

int cup[16][16] = {
    {4,4,4,4,4,0,0,0,0,0,0,4,4,4,4,4},
    {4,0,0,0,0,1,1,1,1,1,1,0,0,0,0,4},
    {0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0},
    {0,1,0,0,0,1,1,0,1,1,1,0,0,0,1,0},
    {0,1,0,0,0,1,1,0,0,1,1,0,0,0,1,0},
    {4,0,1,0,0,1,1,0,0,1,1,0,0,1,0,4},
    {4,0,1,0,0,1,1,0,0,1,1,0,0,1,0,4},
    {4,4,0,1,0,1,1,0,0,1,1,0,1,0,4,4},
    {4,4,0,1,0,1,1,1,0,1,1,0,1,0,4,4},
    {4,4,4,0,1,1,1,1,1,1,1,1,0,4,4,4},
    {4,4,4,4,0,0,1,1,1,1,0,0,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,4,0,1,1,0,4,4,4,4,4,4},
    {4,4,4,4,4,0,1,1,1,1,0,4,4,4,4,4},
    {4,4,4,4,0,1,1,1,1,1,1,0,4,4,4,4},
    {4,4,4,4,4,0,0,0,0,0,0,4,4,4,4,4},
};

//**************************
//  MAIN GRAPHICS FUNCTION
//**************************

void Graphics::Draw_Map(Point_2D translation, int angle, float squish, Line_2D *Track_Lines, Line_2D *Track_Dotted_Lines, Line_2D *Track_Walls, Point_2D *Track_Flags, Triangle_2D *Track_Boost_Plates, Map_Data map_info, int car_type, Point_2D ghost_position, N5110 &LCD)
{
    enum cars {Basic, Offroad, Drifter, Sportscar, Racecar, Stupid};
    Line_2D Track_Lines_Transformed[map_info.number_of_track_lines];
    Line_2D Track_Dotted_Lines_Transformed[map_info.number_of_dotted_lines];
    Point_2D Transformed_Flags[map_info.number_of_flags];
    Line_2D Transformed_Walls[map_info.number_of_walls];
    Point_2D Transformed_Ghost;

    for(int i = 0; i < map_info.number_of_track_lines; i++) {
        Track_Lines_Transformed[i] = Track_Lines[i];
        //Translation
        Track_Lines_Transformed[i].from = Translate_Point(Track_Lines_Transformed[i].from, Round(translation.x), Round(translation.y));
        Track_Lines_Transformed[i].to = Translate_Point(Track_Lines_Transformed[i].to, Round(translation.x), Round(translation.y));
        //Rotation
        Track_Lines_Transformed[i].from = Rotate_Point(Track_Lines_Transformed[i].from, angle);
        Track_Lines_Transformed[i].to = Rotate_Point(Track_Lines_Transformed[i].to, angle);
        //Squish
        Track_Lines_Transformed[i].from = Squish_Point(Track_Lines_Transformed[i].from, squish);
        Track_Lines_Transformed[i].to = Squish_Point(Track_Lines_Transformed[i].to, squish);
    }

    for(int i = 0; i < map_info.number_of_dotted_lines; i++) {
        Track_Dotted_Lines_Transformed[i] = Track_Dotted_Lines[i];
        //Translation
        Track_Dotted_Lines_Transformed[i].from = Translate_Point(Track_Dotted_Lines_Transformed[i].from, Round(translation.x), Round(translation.y));
        Track_Dotted_Lines_Transformed[i].to = Translate_Point(Track_Dotted_Lines_Transformed[i].to, Round(translation.x), Round(translation.y));
        //Rotation
        Track_Dotted_Lines_Transformed[i].from = Rotate_Point(Track_Dotted_Lines_Transformed[i].from, angle);
        Track_Dotted_Lines_Transformed[i].to = Rotate_Point(Track_Dotted_Lines_Transformed[i].to, angle);
        //Squish
        Track_Dotted_Lines_Transformed[i].from = Squish_Point(Track_Dotted_Lines_Transformed[i].from, squish);
        Track_Dotted_Lines_Transformed[i].to = Squish_Point(Track_Dotted_Lines_Transformed[i].to, squish);
    }

    for(int i = 0; i < map_info.number_of_flags; i++) {
        Transformed_Flags[i] = Track_Flags[i];
        //Translation
        Transformed_Flags[i] = Translate_Point(Transformed_Flags[i], Round(translation.x), Round(translation.y));
        //Rotation
        Transformed_Flags[i] = Rotate_Point(Transformed_Flags[i], angle);
        //Squish
        Transformed_Flags[i] = Squish_Point(Transformed_Flags[i], squish);
    }

    for(int i = 0; i < map_info.number_of_walls; i++) {
        Transformed_Walls[i] = Track_Walls[i];
        //Translation
        Transformed_Walls[i].from = Translate_Point(Transformed_Walls[i].from, Round(translation.x), Round(translation.y));
        Transformed_Walls[i].to = Translate_Point(Transformed_Walls[i].to, Round(translation.x), Round(translation.y));
        //Rotation
        Transformed_Walls[i].from = Rotate_Point(Transformed_Walls[i].from, angle);
        Transformed_Walls[i].to = Rotate_Point(Transformed_Walls[i].to, angle);
        //Squish
        Transformed_Walls[i].from = Squish_Point(Transformed_Walls[i].from, squish);
        Transformed_Walls[i].to = Squish_Point(Transformed_Walls[i].to, squish);
    }

    Transformed_Ghost = ghost_position;
    //Translation
    Transformed_Ghost = Translate_Point(Transformed_Ghost, Round(translation.x), Round(translation.y));
    //Rotation
    Transformed_Ghost = Rotate_Point(Transformed_Ghost, angle);
    //Squish
    Transformed_Ghost = Squish_Point(Transformed_Ghost, squish);

    for(int i = 0; i < map_info.number_of_track_lines; i++) {
        Graphics_Draw_Line(Track_Lines_Transformed[i].from, Track_Lines_Transformed[i].to, true, LCD);
    }

    for(int i = 0; i < map_info.number_of_dotted_lines; i++) {
        Graphics_Draw_Line(Track_Dotted_Lines_Transformed[i].from, Track_Dotted_Lines_Transformed[i].to,false, LCD);
    }

    for(int i = 0; i < map_info.number_of_boost_plates; i++) {
        Graphics_Draw_Boost_Plate(Track_Boost_Plates[i], translation, angle, squish, LCD);
    }

    for(int i = 0; i < map_info.number_of_flags; i++) {
        Graphics_Draw_Sprite(Transformed_Flags[i], 8, 8, (int *)flag, LCD);
    }

    Graphics_Draw_Sprite(Transformed_Ghost, 8, 8, (int *)ghost, LCD);

    for(int i = 0; i < map_info.number_of_walls; i++) {
        Graphics_Draw_Wall(Transformed_Walls[i].from, Transformed_Walls[i].to,4, LCD);
    }

    Point_2D car_position = {4,0};
    
    if(car_type == Stupid) {
        Graphics_Draw_Sprite(car_position,8,8,(int *)stupid_car, LCD);
    } else if(car_type == Racecar) {
        Graphics_Draw_Sprite(car_position,8,8,(int *)race_car, LCD);
    } else if(car_type == Sportscar) {
        Graphics_Draw_Sprite(car_position,8,8,(int *)sports_car, LCD);
    } else if(car_type == Drifter) {
        Graphics_Draw_Sprite(car_position,8,8,(int *)drifter_car, LCD);
    } else if(car_type == Offroad) {
        Graphics_Draw_Sprite(car_position,8,8,(int *)offroad_car, LCD);
    } else {
        Graphics_Draw_Sprite(car_position,8,8,(int *)basic_car, LCD);
    }
}

//*******************
//  SCREEN SETTINGS
//*******************

void Graphics::Change_Contrast(N5110 &LCD, Gamepad &Device)
{
    LCD.setContrast((0.35 + 0.2 * Device.read_pot1()));
}

//****************
//  START SCREEN
//****************

void Graphics::Start_Sequence(int state, N5110 &LCD)
{
    Point_2D count_down_position = {8,8};
    if(state == 3) {
        Graphics_Draw_Sprite(count_down_position, 17, 17, (int *) count_down_3, LCD);
    } else if(state == 2) {
        Graphics_Draw_Sprite(count_down_position, 17, 17, (int *) count_down_2, LCD);
    } else if(state == 1) {
        Graphics_Draw_Sprite(count_down_position, 17, 17, (int *) count_down_1, LCD);
    } else if(state == 0) {
        Graphics_Draw_Sprite(count_down_position, 18, 18, (int *) count_down_0, LCD);
    }

}

void Graphics::Finish(N5110 &LCD)
{
    Point_2D finish_position = {8,8};
    Graphics_Draw_Sprite(finish_position, 16, 16, (int *) cup, LCD);
}

//********************
//  DRAW LAP COUNTER
//********************

void Graphics::Draw_Laps(int laps, N5110 &LCD)
{
    if(laps == 1) {
        LCD.printString("1",0,5);
    } else if (laps == 2) {
        LCD.printString("2",0,5);
    } else if (laps == 3) {
        LCD.printString("3",0,5);
    } else if (laps == 4) {
        LCD.printString("4",0,5);
    } else if (laps == 5) {
        LCD.printString("5",0,5);
    }
}

void Graphics::Draw_Time(bool finished, Time time, N5110 &LCD)
{
    if(finished == false) {
        char min[1];
        sprintf(min, "%i", time.mins);
        LCD.printString(min,47,5);

        LCD.printString(":",52,5);

        if(time.secs < 10) {
            LCD.printString("0",57,5);
            char sec[1];
            sprintf(sec, "%i", time.secs);
            LCD.printString(sec,63,5);
        } else {
            char sec[2];
            sprintf(sec, "%i", time.secs);
            LCD.printString(sec,57,5);
        }

        LCD.printString(".",68,5);

        char mili[2];
        sprintf(mili, "%i", time.milis);
        LCD.printString(mili,73,5);
    } else {
        char min[1];
        sprintf(min, "%i", time.mins);
        LCD.printString(min,23,0);

        LCD.printString(":",28,0);

        if(time.secs < 10) {
            LCD.printString("0",33,0);
            char sec[1];
            sprintf(sec, "%i", time.secs);
            LCD.printString(sec,39,0);
        } else {
            char sec[2];
            sprintf(sec, "%i", time.secs);
            LCD.printString(sec,33,0);
        }

        LCD.printString(".",44,0);

        char mili[2];
        sprintf(mili, "%i", time.milis);
        LCD.printString(mili,49,0);
    }
}

//**************
//  TRANSFORM
//**************

//ROTATE
Point_2D Graphics::Rotate_Point(Point_2D point, float angle)
{
    angle = (angle * PI)/180;
    float x_rotated = point.x * cos(angle) - point.y * sin(angle);
    float y_rotated = point.y * cos(angle) + point.x * sin(angle);
    return {x_rotated, y_rotated};
}
//TRANSLATE
Point_2D Graphics::Translate_Point(Point_2D point, int translate_x, int translate_y)
{
    float x_translated = point.x - translate_x;
    float y_translated = point.y - translate_y;
    return {x_translated, y_translated};
}
//SQUISH
Point_2D Graphics::Squish_Point(Point_2D point, float squish)
{
    float x_squish = point.x;
    float y_squish = point.y * squish;
    return {x_squish, y_squish};
}

//********
//  MATH
//********

int Graphics::Round(float number)
{
    int number_int = (number * 10);
    int remainder = number_int % 10;
    if(remainder < 5) {
        return ((number_int - remainder) / 10);
    } else {
        return ((number_int + (10 - remainder)) / 10);
    }
}

float Graphics::Gradient(Point_2D from, Point_2D to)
{
    float change_in_y = to.y - from.y;
    float change_in_x = to.x - from.x;
    float gradient = change_in_y / change_in_x;
    if(gradient < 0.001 && gradient > -0.001) {
        return 0;
    } else {
        return gradient;
    }
}

bool Graphics::Gradient_Check_Infinate(Point_2D from, Point_2D to)
{
    float change_in_x = to.x - from.x;
    if(change_in_x < 0.001 && change_in_x > -0.001) {
        return true;
    } else {
        return false;
    }
}

//*************
//  DRAW LINE
//*************

void Graphics::Graphics_Draw_Line(Point_2D from, Point_2D to, bool solid, N5110 &LCD) //Draw a line between two points on only the portion of the screen
{

    if( Gradient_Check_Infinate(from, to)  == false ) { //Checking to see if the line is vertical
        Point_2D plot_x_from = {0,0};
        Point_2D plot_x_to = {0,0};
        Point_2D plot_y_from = {0,0};
        Point_2D plot_y_to = {0,0};

        float gradient = Gradient(from, to); // Calculating the gradient
        float y_intercept = (from.y - gradient * from.x); // Calulating the y intercept y - mx = c
        float x_intercept = (from.x - from.y / gradient); // Calculating the x intercept x - y/m = d

        if(gradient <= 1 && gradient >= -1) {
            //Reordering from and to so that the for loops below can use ++
            if(to.x < from.x) {
                plot_x_from = to;
                plot_x_to = from;
            } else {
                plot_x_from = from;
                plot_x_to = to;
            }
            if(solid == true) {
                for(int x = Round(plot_x_from.x); x <= Round(plot_x_to.x); x++) { //Iterating through the x points
                    int y = -(Round((gradient * x) + y_intercept)); //Calculating the value of y for each x point
                    Point_2D plot_x = {x+42, y+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                    if(plot_x.x <= 84 && plot_x.x >= 0 && plot_x.y <=48 && plot_x.y >= 0) {
                        LCD.setPixel(plot_x.x, plot_x.y, true); //Plotting the points
                    }
                }
            } else {
                for(int x = Round(plot_x_from.x); x <= Round(plot_x_to.x); x+=2) { //Iterating through the x points
                    int y = -(Round((gradient * x) + y_intercept)); //Calculating the value of y for each x point
                    Point_2D plot_x = {x+42, y+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                    if(plot_x.x <= 84 && plot_x.x >= 0 && plot_x.y <=48 && plot_x.y >= 0) {
                        LCD.setPixel(plot_x.x, plot_x.y, true); //Plotting the points
                    }
                }
            }
        } else {
            if(to.y < from.y) {
                plot_y_from = to;
                plot_y_to = from;
            } else {
                plot_y_from = from;
                plot_y_to = to;
            }
            if(solid == true) {
                for(int y = Round(plot_y_from.y); y <= Round(plot_y_to.y); y++) { //Iterating through the Y points
                    int x = Round((y / gradient) + x_intercept); //Calculating the value of x for every y point
                    Point_2D plot_y = {x+42, (-y)+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                    if(plot_y.x <= 84 && plot_y.x >= 0 && plot_y.y <=48 && plot_y.y >= 0) {
                        LCD.setPixel(plot_y.x, plot_y.y, true); //Plotting the points
                    }
                }
            } else {
                for(int y = Round(plot_y_from.y); y <= Round(plot_y_to.y); y+=2) { //Iterating through the Y points
                    int x = Round((y / gradient) + x_intercept); //Calculating the value of x for every y point
                    Point_2D plot_y = {x+42, (-y)+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                    if(plot_y.x <= 84 && plot_y.x >= 0 && plot_y.y <=48 && plot_y.y >= 0) {
                        LCD.setPixel(plot_y.x, plot_y.y, true); //Plotting the points
                    }
                }
            }
        }

    } else {
        Point_2D plot_y_from = {0,0};
        Point_2D plot_y_to = {0,0};
        //Reordering from and to so that the for loops below can use ++
        if(to.y < from.y) {
            plot_y_from = to;
            plot_y_to = from;
        } else {
            plot_y_from = from;
            plot_y_to = to;
        }
        float x_intercept = from.x; //Calculating the x_intercept
        if(solid == true) {
            for(int y = plot_y_from.y; y <= plot_y_to.y; y++) {
                Point_2D plot_y = {x_intercept+42, (-y)+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                LCD.setPixel(plot_y.x, plot_y.y, true); //Plotting the points
            }
        } else {
            for(int y = plot_y_from.y; y <= plot_y_to.y; y+=2) {
                Point_2D plot_y = {x_intercept+42, (-y)+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                LCD.setPixel(plot_y.x, plot_y.y, true); //Plotting the points
            }
        }
    }
}

//***************
//  DRAW SPRITE
//***************

void Graphics::Graphics_Draw_Sprite(Point_2D point, int x_size, int y_size, int *sprite, N5110 &LCD)
{
    Point_2D zeroed_point = {Round(point.x) + 42 - x_size, Round(-point.y) + 36 - y_size};
    if(zeroed_point.x <= 84 && zeroed_point.x >= 0 && zeroed_point.y <= 48 && zeroed_point.y >= 0) {
        for(int y = 0; y < y_size; y++) {
            for(int x = 0; x < x_size; x++) {
                if(*((sprite + y*y_size)+x) == 1) {
                    LCD.setPixel((zeroed_point.x + x), (zeroed_point.y + y), true);
                } else if(*((sprite + y*y_size)+x) == 0) {
                    LCD.setPixel((zeroed_point.x + x), (zeroed_point.y + y), false);
                }
            }
        }
    }
}

//***************
//  DRAW WALL
//***************

void Graphics::Graphics_Draw_Wall(Point_2D from, Point_2D to, int height, N5110 &LCD) //Draw a line between two points on only the portion of the screen
{
    if(Gradient_Check_Infinate(from, to) == 0) { //Checking to see if the line is vertical
        Point_2D plot_x_from = {0,0};
        Point_2D plot_x_to = {0,0};
        Point_2D plot_y_from = {0,0};
        Point_2D plot_y_to = {0,0};

        float gradient = Gradient(from, to); // Calculating the gradient
        float y_intercept = (from.y - gradient * from.x); // Calulating the y intercept y - mx = c
        float x_intercept = (from.x - from.y / gradient); // Calculating the x intercept x - y/m = d

        if(gradient <= 1 && gradient >= -1) {
            //Reordering from and to so that the for loops below can use ++
            if(to.x < from.x) {
                plot_x_from = to;
                plot_x_to = from;
            } else {
                plot_x_from = from;
                plot_x_to = to;
            }
            for(int x = Round(plot_x_from.x); x <= Round(plot_x_to.x); x++) { //Iterating through the x points
                int y = -(Round((gradient * x) + y_intercept)); //Calculating the value of y for each x point
                Point_2D plot_x = {x+42, y+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                if(plot_x.x <= 84 && plot_x.x >= 0 && plot_x.y <=48 && plot_x.y >= 0) {
                    for(int y_add = 0; y_add < height; y_add++) {
                        LCD.setPixel(plot_x.x, plot_x.y - y_add, true);
                    }
                }
            }
        } else {
            if(to.y < from.y) {
                plot_y_from = to;
                plot_y_to = from;
            } else {
                plot_y_from = from;
                plot_y_to = to;
            }
            for(int y = Round(plot_y_from.y); y <= Round(plot_y_to.y); y++) { //Iterating through the Y points
                int x = Round((y / gradient) + x_intercept); //Calculating the value of x for every y point
                Point_2D plot_y = {x+42, (-y)+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
                if(plot_y.x <= 84 && plot_y.x >= 0 && plot_y.y <=48 && plot_y.y >= 0) {
                    for(int y_add = 0; y_add < height; y_add++) {
                        LCD.setPixel(plot_y.x, plot_y.y - y_add, true);
                    }
                }
            }
        }

    } else {
        Point_2D plot_y_from = {0,0};
        Point_2D plot_y_to = {0,0};
        //Reordering from and to so that the for loops below can use ++
        if(to.y < from.y) {
            plot_y_from = to;
            plot_y_to = from;
        } else {
            plot_y_from = from;
            plot_y_to = to;
        }
        float x_intercept = from.x; //Calculating the x_intercept
        for(int y = plot_y_from.y; y <= plot_y_to.y; y++) {
            Point_2D plot_y = {x_intercept+42, (-y)+36}; //Assigning them to a Plot_2D variable and transforming so the centre of the screen is 0,0
            for(int y_add = 0; y_add < height; y_add++) {
                LCD.setPixel(plot_y.x, plot_y.y - y_add, true);
            }
        }

    }
}

//********************
//  DRAW BOOST PLATE
//********************

void Graphics::Graphics_Draw_Boost_Plate(Triangle_2D boost_plate, Point_2D translation, int angle, float squish, N5110 &LCD)
{
    Point_2D boost_plate_TL = boost_plate.TL;
    Point_2D boost_plate_TR = {boost_plate.BR.x, boost_plate.TL.y};
    Point_2D boost_plate_BL = {boost_plate.TL.x, boost_plate.BR.y};
    Point_2D boost_plate_BR = boost_plate.BR;
    Point_2D boost_plate_mid;
    if(boost_plate.Type == 1) {
        boost_plate_mid.x = boost_plate_BR.x;
        boost_plate_mid.y = (((boost_plate_TL.y - boost_plate_BR.y) / 2) + boost_plate_BR.y);
    } else if(boost_plate.Type == 2) {
        boost_plate_mid.x = (((boost_plate_BR.x - boost_plate_TL.x) / 2) + boost_plate_TL.x);
        boost_plate_mid.y = boost_plate_BR.y;
    } else if(boost_plate.Type == 3) {
        boost_plate_mid.x = boost_plate_TL.x;
        boost_plate_mid.y = (((boost_plate_TL.y - boost_plate_BR.y) / 2) + boost_plate_BR.y);
    } else if(boost_plate.Type == 4) {
        boost_plate_mid.x = (((boost_plate_BR.x - boost_plate_TL.x) / 2) + boost_plate_TL.x);
        boost_plate_mid.y = boost_plate_TL.y;
    }

    //Translation
    boost_plate_TL = Translate_Point(boost_plate_TL, Round(translation.x), Round(translation.y));
    boost_plate_TR = Translate_Point(boost_plate_TR, Round(translation.x), Round(translation.y));
    boost_plate_BL = Translate_Point(boost_plate_BL, Round(translation.x), Round(translation.y));
    boost_plate_BR = Translate_Point(boost_plate_BR, Round(translation.x), Round(translation.y));
    boost_plate_mid = Translate_Point(boost_plate_mid, Round(translation.x), Round(translation.y));

    //Rotation
    boost_plate_TL = Rotate_Point(boost_plate_TL, angle);
    boost_plate_TR = Rotate_Point(boost_plate_TR, angle);
    boost_plate_BL = Rotate_Point(boost_plate_BL, angle);
    boost_plate_BR = Rotate_Point(boost_plate_BR, angle);
    boost_plate_mid = Rotate_Point(boost_plate_mid, angle);

    //Squish
    boost_plate_TL = Squish_Point(boost_plate_TL, squish);
    boost_plate_TR = Squish_Point(boost_plate_TR, squish);
    boost_plate_BL = Squish_Point(boost_plate_BL, squish);
    boost_plate_BR = Squish_Point(boost_plate_BR, squish);
    boost_plate_mid = Squish_Point(boost_plate_mid, squish);

    Graphics_Draw_Line(boost_plate_TL, boost_plate_TR, true, LCD);
    Graphics_Draw_Line(boost_plate_TR, boost_plate_BR, true, LCD);
    Graphics_Draw_Line(boost_plate_BR, boost_plate_BL, true, LCD);
    Graphics_Draw_Line(boost_plate_BL, boost_plate_TL, true, LCD);

    if(boost_plate.Type == 1) {
        Graphics_Draw_Line(boost_plate_TL, boost_plate_mid, true, LCD);
        Graphics_Draw_Line(boost_plate_BL, boost_plate_mid, true, LCD);
    } else if(boost_plate.Type == 2) {
        Graphics_Draw_Line(boost_plate_TL, boost_plate_mid, true, LCD);
        Graphics_Draw_Line(boost_plate_BL, boost_plate_mid, true, LCD);
    } else if(boost_plate.Type == 3) {
        Graphics_Draw_Line(boost_plate_TR, boost_plate_mid, true, LCD);
        Graphics_Draw_Line(boost_plate_BR, boost_plate_mid, true, LCD);
    } else if(boost_plate.Type == 4) {
        Graphics_Draw_Line(boost_plate_TR, boost_plate_mid, true, LCD);
        Graphics_Draw_Line(boost_plate_BR, boost_plate_mid, true, LCD);
    }

}