#include <Adafruit_ILI9341.h>
#include <Adafruit_GFX.h>
#include <Fonts/FreeMono12pt7b.h>
#include <Fonts/FreeMonoBold12pt7b.h>
#include <Fonts/FreeSans12pt7b.h>
#include <Fonts/FreeSans18pt7b.h>
#include <Fonts/FreeSansBold12pt7b.h>
#include <Fonts/FreeSansBold18pt7b.h>
#include <Fonts/FreeSansBold24pt7b.h>

Adafruit_ILI9341 tft(/*CS*/ 5, /*DC*/ 22);

#define BLACK       0x0000
#define GREY        tft.color565(143,143,143)
#define LIGHT_GREY  tft.color565(200,200,200)
#define BLUE        0x001F
#define RED         0xF800
#define DARK_RED    tft.color565(176, 0, 0)
#define GREEN       0x07E0
#define CYAN        0x07FF
#define MAGENTA     0xF81F
#define YELLOW      0xFFE0 
#define WHITE       0xFFFF
#define LIME        tft.color565(214, 236, 25)

int button1 = LOW;
int button2 = LOW;
int button3 = LOW;

int corners = 7;

float testForce = 0;
float testTravel = 0;
bool newTest = false;


struct Sizes {
  String size;
};

struct Magnet {
  String style;
  Sizes options[4];
};

struct Brands {
  String brand;
  String name;
  Magnet models[4];
  int models_num;
  int brand_pos;
};

Brands brands[4] = {
  {
    .brand = "Rogue Magnetics",
    .name = "rogue",
    {
      {
        .style = "Clamp",
        {
          {
            .size = "Old 70x60"
          },
          {
            .size = "New 70x60"
          },
          {
            .size = "Old 70x30"
          },
          {
            .size = "New 70x30"
          }
        }
      }
    },
    .models_num = 4,
    .brand_pos = 0
  },
  {
    .brand = "Magnetar",
    .name = "magnetar",
    {
      {
        .style = "360",
        {
          {
            .size = "90x50"
          },
          {
            .size = "70x60"
          }
        }
      }
    },
    .models_num = 2,
    .brand_pos = 1
  },
  {
    .brand = "Magnet Store",
    .name = "magnet_store",
    {
      {
        .style = "Clamp",
        {
          {
            .size = "70x60"
          },
          {
            .size = "70x29"
          }
        }
      },
      {
        .style = "360",
        {
          {
            .size = "65x20"
          }
        }
      }
    },
    .models_num = 3,
    .brand_pos = 2
  },
  {
    .brand = "Kratos",
    .name = "kratos",
    {
      {
        .style = "Clamp",
        {
          {
            .size = "70x60"
          },
          {
            .size = "70x30"
          }
        }
      }
    },
    .models_num = 2,
    .brand_pos = 3
  }
};

struct Selection {
  String set;
  int loop_length = 4;
  String brand = "";
  int brand_pos = 100;
  String style = "";
  int style_pos = 100;
  String size = "";
  int size_pos = 100;
};

Selection selection;

void setup() {
  Serial.begin(115200);
  tft.begin();
  tft.fillScreen(BLACK);
  tft.setRotation(3);

  pinMode(21, INPUT);
  pinMode(32, INPUT);
  pinMode(33, INPUT);

  clearSelection();

  HOME_SCREEN();
}

int current_screen = 0;

struct Screens {
  int HOME = 0;
  int SETTINGS = 1;
  int INFO = 2;
  int BRAND = 3;
  int STYLE = 4;
  int SIZE = 5;
  int START = 6;
  int FINISH = 7;
};

Screens screens;

void (*page[])(void) = {
HOME_SCREEN,
SETTINGS_SCREEN,
INFO_SCREEN,
BRAND_SCREEN,
STYLE_SCREEN,
SIZE_SCREEN,
START_SCREEN,
FINISH_SCREEN
};

unsigned long current_millis;

class DataUpdater {
  int updateInterval;
  long value;
  unsigned long previous_millis;
  String dataset;
  int halfScreen;

  public:
  DataUpdater(int interval, String data) {
    updateInterval = interval;
    value = 0;
    previous_millis = 0;
    dataset = data;
    halfScreen = tft.width()/2+42;
  };
  
  void Update() {
    if(current_millis - previous_millis > updateInterval) {
      previous_millis = current_millis;
      if(dataset == "force") {
        clearForce();
        updateForce(value);
      } else if(dataset == "travel") {
        clearTravel();
        updateTravel(value);
      }
      value++;
    }
  };

  void updateForce(unsigned long value) {
    tft.setCursor(halfScreen+10, 120);
    tft.setFont(&FreeSansBold24pt7b);
    tft.setTextSize(1);
    tft.setTextColor(WHITE, BLACK);
    tft.print(value);
    if(value < testForce) {
      current_screen = screens.FINISH;
      (*page[current_screen])();
    } else {
      testForce = value;
    }
  }

  void clearForce() {
    tft.fillRoundRect(halfScreen+10, 75, tft.width()-halfScreen-20, 50, corners-2, BLACK);
  };

  void updateTravel(unsigned long value) {
    tft.setCursor(halfScreen+10, 216);
    tft.setFont(&FreeSansBold24pt7b);
    tft.setTextSize(1);
    tft.setTextColor(WHITE, BLACK);
    tft.print(value);
    testTravel = value;
  };

  void clearTravel() {
    tft.fillRoundRect(halfScreen+10, 175, tft.width()-halfScreen-20, 50, corners-2, BLACK);
  };

  void clearValue() {
    value = 0;
  }
};

DataUpdater dataForce(1000, "force");
DataUpdater dataTravel(1500, "travel");

void loop() {
  button1 = digitalRead(21);
  button2 = digitalRead(32);
  button3 = digitalRead(33);

  //**************************
  // HOME SCREEN

  if(current_screen == screens.HOME) {
    if(button1 == HIGH) {
      button1 = LOW;

      // HOME SCREEN -> INFO SCREEN

      current_screen = screens.INFO;
      (*page[current_screen])();
    } else if(button2 == HIGH) {
      button2 = LOW;

      // HOME SCREEN -> SETTINGS SCREEN

      current_screen = screens.SETTINGS;
      (*page[current_screen])();
    } else if(button3 == HIGH) {
      button3 = LOW;

      // HOME SCREEN -> START SCREEN

      newTest = true;
      clearSelection();
      current_screen = screens.BRAND;
      (*page[current_screen])();
    }
  }

  //**************************
  // INFO SCREEN

  if(current_screen == screens.INFO) {
    if(button1 == HIGH) {
      button1 = LOW;

      // INFO SCREEN -> HOME SCREEN

      current_screen = screens.HOME;
      (*page[current_screen])();
    }
  }

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

  if(current_screen == screens.SETTINGS) {
    if(button2 == HIGH) {
      button2 = LOW;

      // SETTINGS SCREEN -> HOME SCREEN

      current_screen = screens.HOME;
      (*page[current_screen])();
    } else if(button1 == HIGH && selection.brand == "") {
      button1 = LOW;

      // SETTINGS SCREEN -> BRAND SCREEN

      current_screen = screens.BRAND;
      (*page[current_screen])();
    } else if(button1 == HIGH && selection.brand != "" && selection.style == "") {
      button1 = LOW;

      // SETTINGS SCREEN -> STYLE SCREEN

      current_screen = screens.STYLE;
      (*page[current_screen])();
    } else if(button1 == HIGH && selection.style != "" && selection.size == "") {
      button1 = LOW;

      // SETTINGS SCREEN -> SIZE SCREEN

      current_screen = screens.SIZE;
      (*page[current_screen])();
    } else if(button1 == HIGH && selection.brand != "" && selection.style != "" && selection.size != "") {
      button1 = LOW;

      // SETTINGS SCREEN -> START SCREEN
      
      newTest = true;
      current_screen = screens.START;
      (*page[current_screen])();
    } else if(button3 == HIGH) {
      button3 = LOW;

      // SETTINGS SCREEN <-> INSERT INFO

      if(selection.size == "") {
        selection.brand_pos = 0;
        selection.brand = brands[0].brand;
        selection.style_pos = 1;
        selection.style = brands[0].models[0].style;
        selection.size_pos = 0;
        selection.size = brands[0].models[0].options[0].size;
        (*page[current_screen])();
      } else if(selection.size != "") {
        clearSelection();
        (*page[current_screen])();
      }
    }
  }

  //**************************
  // BRAND SCREEN

  if(current_screen == screens.BRAND) {
    if(button2 == HIGH) {
      button2 = LOW;

      // BRAND SCREEN -> SELECT BRAND -> SETTINGS SCREEN

      clearSelection();

      selection.brand_pos = 2;
      selection.set = brands[selection.brand_pos].name;
      selection.brand = brands[selection.brand_pos].brand;
      selection.loop_length = brands[selection.brand_pos].models_num;

      if(newTest) {
        current_screen = screens.STYLE;
        (*page[current_screen])();
      } else if(!newTest) {
        current_screen = screens.SETTINGS;
        (*page[current_screen])();
      }
    } else if(button1 == HIGH) {
      button1 = LOW;

      // BRAND SCREEN -> SETTINGS SCREEN

      if(newTest) {
        newTest = false;
        clearSelection();
        current_screen = screens.HOME;
        (*page[current_screen])();
      } else if(!newTest) {
        current_screen = screens.SETTINGS;
        (*page[current_screen])();
      }
    } else if(button3 == HIGH) {
      button3 = LOW;

      // BRAND SCREEN -> CLEAR SELECTION

      clearSelection();
      (*page[current_screen])();
    }
  }

  //**************************
  // STYLE SCREEN

  if(current_screen == screens.STYLE) {
    if(button2 == HIGH) {
      button2 = LOW;

      // STYLE SCREEN -> SELECT SIZE -> SETTINGS SCREEN

      clearStyle();
      clearSize();

      selection.style_pos = 0;
      selection.style = brands[selection.brand_pos].models[selection.style_pos].style;

      if(newTest) {
        current_screen = screens.SIZE;
        (*page[current_screen])();
      } else if(!newTest) {
        current_screen = screens.SETTINGS;
        (*page[current_screen])();
      }
    } else if(button1 == HIGH) {
      button1 = LOW;

      // STYLE SCREEN -> SETTINGS SCREEN

      if(newTest) {
        current_screen = screens.BRAND;
        (*page[current_screen])();
      } else if(!newTest) {
        current_screen = screens.SETTINGS;
        (*page[current_screen])();
      }
    } else if (button3 == HIGH) {
      button3 = LOW;

      // STYLE SCREEN -> CLEAR SELECTION

      clearStyle();
      clearSize();
      (*page[current_screen])();
    }
  }

  //**************************
  // SIZE SCREEN

  if(current_screen == screens.SIZE) {
    if(button2 == HIGH) {
      button2 = LOW;

      // SIZE SCREEN -> SELECT SIZE -> SETTINGS SCREEN

      clearSize();

      selection.size_pos = 0;
      selection.size = brands[selection.brand_pos].models[selection.style_pos].options[selection.size_pos].size;

      // if(newTest) {
      //   current_screen = screens.START;
      //   (*page[current_screen])();
      // } else if(!newTest) {
        current_screen = screens.SETTINGS;
        (*page[current_screen])();
      // }
    } else if(button1 == HIGH) {
      button1 = LOW;

      // SIZE SCREEN -> SETTINGS SCREEN

      if(newTest) {
        current_screen = screens.STYLE;
        (*page[current_screen])();
      } else if(!newTest) {
        current_screen = screens.SETTINGS;
        (*page[current_screen])();
      }
    } else if(button3 == HIGH) {
      button3 = LOW;

      // SIZE SCREEN -> CLEAR SELECTION

      clearSize();
      (*page[current_screen])();
    }
  }

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

  if(current_screen == screens.START) {
    current_millis = millis();

    dataTravel.Update();
    dataForce.Update();

    if(button2 == HIGH) {
      button2 = LOW;

      // START SCREEN -> HOME SCREEN

      current_screen = screens.FINISH;
      (*page[current_screen])();
    }
  }

  //**************************
  // FINISH SCREEN

  if(current_screen == screens.FINISH) {
    if(button1 == HIGH) {

      clearSelection();
      dataForce.clearValue();
      dataTravel.clearValue();

      testForce = 0;
      testTravel = 0;

      current_screen = screens.HOME;
      (*page[current_screen])();
    }
  }
}

//**************************
// CLEAR SELECTION

void clearSelection() {
  selection.set;
  selection.brand = "";
  selection.brand_pos = 100;
  selection.size = "";
  selection.size_pos = 100;
  selection.style = "";
  selection.style_pos = 100;
}

//**************************
// CLEAR BRAND

void clearBrand() {
  selection.brand = "";
  selection.brand_pos = 100;
}

//**************************
// CLEAR SIZE

void clearSize() {
  selection.size = "";
  selection.size_pos = 100;
}

//**************************
// CLEAR STYLE

void clearStyle() {
  selection.style = "";
  selection.style_pos = 100;
}

//**************************
// TOP BANNER

void topBanner() {
  int topBannerX = 0;
  int topBannerY = 0;
  tft.fillRect(topBannerX, topBannerY, 320, 12, GREY);
  tft.setCursor(topBannerX+115, topBannerY+2);
  tft.setFont();
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("Rogue Magnetics");
}

//**************************
// BACK BUTTON

void backButton() {
  int backButtonX = 0;
  int backButtonY = 202;
  tft.fillRoundRect(backButtonX, backButtonY, 100, 38, corners, GREY);
  tft.drawRoundRect(backButtonX, backButtonY, 100, 38, corners, LIGHT_GREY);
  tft.setCursor(backButtonX+18, backButtonY+21);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("BACK");
}

//**************************
// HOME BUTTON

void homeButton(uint16_t bg_color=GREY) {
  int homeButtonX = 0;
  int homeButtonY = 202;
  tft.fillRoundRect(homeButtonX, homeButtonY, 100, 38, corners, bg_color);
  if(bg_color == LIME) {
    tft.drawRoundRect(homeButtonX, homeButtonY, 100, 38, corners, WHITE);
    tft.setTextColor(BLACK);
  } else {
    tft.drawRoundRect(homeButtonX, homeButtonY, 100, 38, corners, LIGHT_GREY);
    tft.setTextColor(WHITE);
  }
  tft.setCursor(homeButtonX+14, homeButtonY+21);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.print("HOME");
}

//**************************
// NEXT BUTTON

void nextButton() {
  int nextButtonX = tft.width()-100;
  int nextButtonY = 202;
  tft.fillRoundRect(nextButtonX, nextButtonY, 100, 38, corners, LIME);
  tft.drawRoundRect(nextButtonX, nextButtonY, 100, 38, corners, WHITE);
  tft.setCursor(nextButtonX+18, nextButtonY+27);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("NEXT");
}

//**************************
// START BUTTON

void startButton() {
  int startButtonX = tft.width()-100;
  int startButtonY = 202;
  tft.fillRoundRect(startButtonX, startButtonY, 100, 38, corners, LIME);
  tft.drawRoundRect(startButtonX, startButtonY, 100, 38, corners, WHITE);
  tft.setCursor(startButtonX+12, startButtonY+27);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("START");
}

//**************************
// CLEAR BUTTON

void clearButton() {
  int clearButtonX = tft.width()-100;
  int clearButtonY = 202;
  tft.fillRoundRect(clearButtonX, clearButtonY, 100, 38, corners, GREY);
  tft.drawRoundRect(clearButtonX, clearButtonY, 100, 38, corners, LIGHT_GREY);
  tft.setCursor(clearButtonX+10, clearButtonY+27);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("CLEAR");
}

//**************************
// HOME SCREEN

void HOME_SCREEN() {
  tft.fillScreen(BLACK);
  topBanner();

  int newTestX = 0;
  int newTestY = 16;
  tft.fillRoundRect(newTestX, newTestY, 320, 151, corners, LIME);
  tft.drawRoundRect(newTestX, newTestY, 320, 151, corners, WHITE);
  tft.setCursor(newTestX+32, newTestY+80);
  tft.setFont(&FreeSansBold18pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("Start New Test");

  int settingsX = 0;
  int settingsY = 171;
  tft.fillRoundRect(settingsX, settingsY, 158, 69, corners, GREY);
  tft.drawRoundRect(settingsX, settingsY, 158, 69, corners, LIGHT_GREY);
  tft.setCursor(settingsX+38, settingsY+40);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("Settings");

  int controlsX = 162;
  int controlsY = settingsY;
  tft.fillRoundRect(controlsX, controlsY, 158, 69, corners, GREY);
  tft.drawRoundRect(controlsX, controlsY, 158, 69, corners, LIGHT_GREY);
  tft.setCursor(controlsX+60, controlsY+40);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("Info");
}

//**************************
// BRAND SCREEN

void BRAND_SCREEN() {
  tft.fillScreen(BLACK);
  topBanner();
  backButton();
  clearButton();

  int titleX = 0;
  int titleY = 16;
  tft.fillRoundRect(titleX, titleY, tft.width(), 38, corners, LIME);
  tft.drawRoundRect(titleX, titleY, tft.width(), 38, corners, WHITE);
  tft.setCursor(titleX+5, titleY+27);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("BRAND");

  int grid[4][4] = {{0,58},{0,130},{162,58},{162,130}};
  int box_width = 158;
  int box_height = 68;

  int ind1;
  String splitBrand;

  for(int i=0; i < 4; i++) {
    if(selection.brand_pos == i) {
      tft.fillRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, LIME);
      tft.drawRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, WHITE);
      tft.setTextColor(BLACK);
    } else {
      tft.fillRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, GREY);
      tft.drawRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, LIGHT_GREY);
      tft.setTextColor(WHITE);
    }
    tft.setFont(&FreeSans12pt7b);
    tft.setTextSize(1);
    ind1 = brands[i].brand.indexOf(' ');
    if(ind1 > 0) {
      tft.setCursor(grid[i][0]+5, grid[i][1]+29);
      splitBrand = brands[i].brand.substring(0, ind1);
      tft.print(brands[i].brand.substring(0, ind1));
      tft.setCursor(grid[i][0]+5, grid[i][1]+54);
      tft.print(brands[i].brand.substring(ind1+1,brands[i].brand.length()));
    } else {
      tft.setCursor(grid[i][0]+5, grid[i][1]+41);
      tft.print(brands[i].brand);
    }
  }
}

//**************************
// STYLE SCREEN

void STYLE_SCREEN() {
  tft.fillScreen(BLACK);
  topBanner();
  backButton();
  clearButton();

  int titleX = 0;
  int titleY = 16;
  tft.fillRoundRect(titleX, titleY, tft.width(), 38, corners, LIME);
  tft.drawRoundRect(titleX, titleY, tft.width(), 38, corners, WHITE);
  tft.setCursor(titleX+5, titleY+27);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("STYLE");

  int grid[4][4] = {{0,58},{0,130},{162,58},{162,130}};
  int box_width = 158;
  int box_height = 68;

  for(int i=0; i < selection.loop_length; i++) {
    if(brands[selection.brand_pos].models[i].style != "") {
      if(selection.style_pos == i) {
        tft.fillRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, LIME);
        tft.drawRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, WHITE);
        tft.setTextColor(BLACK);
      } else {
        tft.fillRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, GREY);
        tft.drawRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, LIGHT_GREY);
        tft.setTextColor(WHITE);
      }
      tft.setCursor(grid[i][0]+5, grid[i][1]+41);
      tft.setFont(&FreeSans12pt7b);
      tft.setTextSize(1);
      tft.print(brands[selection.brand_pos].models[i].style);
    }
  }
}

//**************************
// SIZE SCREEN

void SIZE_SCREEN() {
  tft.fillScreen(BLACK);
  topBanner();
  backButton();
  clearButton();

  int titleX = 0;
  int titleY = 16;
  tft.fillRoundRect(titleX, titleY, tft.width(), 38, corners, LIME);
  tft.drawRoundRect(titleX, titleY, tft.width(), 38, corners, WHITE);
  tft.setCursor(titleX+5, titleY+27);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("SIZE");

  int grid[4][4] = {{0,58},{0,130},{162,58},{162,130}};
  int box_width = 158;
  int box_height = 68;

  for(int i=0; i < selection.loop_length; i++) {
    if(brands[selection.brand_pos].models[selection.style_pos].options[i].size != "") {
      if(selection.size_pos == i) {
        tft.fillRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, LIME);
        tft.drawRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, WHITE);
        tft.setTextColor(BLACK);
      } else {
        tft.fillRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, GREY);
        tft.drawRoundRect(grid[i][0], grid[i][1], box_width, box_height, corners, LIGHT_GREY);
        tft.setTextColor(WHITE);
      }
      tft.setCursor(grid[i][0]+5, grid[i][1]+41);
      tft.setFont(&FreeSans12pt7b);
      tft.setTextSize(1);
      tft.print(brands[selection.brand_pos].models[selection.style_pos].options[i].size);
    }
  }
}

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

void START_SCREEN() {
  tft.fillScreen(BLACK);
  topBanner();

  int halfScreen = (tft.width()/2);

  int stopX = 0;
  int stopY = 16;
  tft.fillRoundRect(stopX, stopY, halfScreen-2, tft.height()-stopY, corners, RED);
  tft.drawRoundRect(stopX, stopY, halfScreen-2, tft.height()-stopY, corners, DARK_RED);
  tft.drawRoundRect(stopX+1, stopY+1, halfScreen-4, tft.height()-18, corners, DARK_RED);
  tft.setCursor(stopX+30, ((tft.height()-stopY)/2)+23);
  tft.setFont(&FreeSansBold18pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("STOP");

  int currentX = halfScreen+2;
  int currentY = stopY;
  tft.fillRoundRect(currentX, currentY, tft.width()-halfScreen-2, tft.height()-currentY, corners, GREY);
  tft.drawRoundRect(currentX, currentY, tft.width()-halfScreen-2, tft.height()-currentY, corners, LIGHT_GREY);
  tft.setCursor(currentX+4, currentY+24);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("STATS:");

  int forceX = currentX+4;
  int forceY = currentY+31;
  tft.fillRoundRect(forceX, forceY, tft.width()-halfScreen-10, 92, corners-2, BLACK);
  tft.drawRoundRect(forceX, forceY, tft.width()-halfScreen-10, 92, corners-2, LIGHT_GREY);
  tft.setCursor(forceX+4,forceY+24);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("FORCE:");
  // tft.setCursor(forceX+4, forceY+73);
  // tft.setFont(&FreeSansBold24pt7b);
  // tft.setTextSize(1);
  // tft.setTextColor(WHITE);
  // tft.print("200");

  int travelX = currentX+4;
  int travelY = forceY+96;
  tft.fillRoundRect(travelX, travelY, tft.width()-halfScreen-10, 92, corners-2, BLACK);
  tft.drawRoundRect(travelX, travelY, tft.width()-halfScreen-10, 92, corners-2, LIGHT_GREY);
  tft.setCursor(travelX+4,travelY+24);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("TRAVEL:");
  // tft.setCursor(travelX+4, travelY+73);
  // tft.setFont(&FreeSansBold24pt7b);
  // tft.setTextSize(1);
  // tft.setTextColor(WHITE);
  // tft.print("150");
}

//**************************
// FINISH SCREEN

void FINISH_SCREEN() {
  newTest = false;
  tft.fillScreen(BLACK);
  topBanner();
  homeButton(LIME);

  int maxX = 0;
  int maxY = 16;
  int maxW = 119;
  int maxH = 182;
  tft.fillRoundRect(maxX, maxY, maxW, maxH, corners, GREY);
  tft.drawRoundRect(maxX, maxY, maxW, maxH, corners, LIGHT_GREY);
  tft.setCursor(maxX+4, maxY+24);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("MAX:");

  int maxbgX = maxX+4;
  int maxbgY = maxY+31;
  int maxbgW = maxW-(2*(maxbgX-maxX));
  int maxbgH = maxH-(maxbgY-maxY)-4;
  tft.fillRoundRect(maxbgX, maxbgY, maxbgW, maxbgH, corners-2, BLACK);
  tft.drawRoundRect(maxbgX, maxbgY, maxbgW, maxbgH, corners-2, LIGHT_GREY);
  tft.setCursor(maxbgX+4, maxbgY+27);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("FORCE:");
  tft.setCursor(maxbgX+4, maxbgY+57);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextColor(WHITE);
  tft.print(testForce,1);

  tft.setCursor(maxbgX+4, maxbgY+(maxbgH/2)+17);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextColor(LIME);
  tft.print("TRAVEL:");
  tft.setCursor(maxbgX+4, maxbgY+(maxbgH/2)+47);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextColor(WHITE);
  tft.print(testTravel,1);

  int brandX = maxW+4;
  int brandY = 16;
  tft.fillRoundRect(brandX, brandY, tft.width()-brandX, 58, corners, GREY);
  tft.drawRoundRect(brandX, brandY, tft.width()-brandX, 58, corners, LIGHT_GREY);
  tft.setCursor(brandX+6, brandY+21);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("BRAND");
  tft.setCursor(brandX+6, brandY+48);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print(selection.brand);

  int sizeX = maxW+4;
  int sizeY = brandY+62;
  tft.fillRoundRect(sizeX, sizeY, tft.width()-sizeX, 58, corners, GREY);
  tft.drawRoundRect(sizeX, sizeY, tft.width()-sizeX, 58, corners, LIGHT_GREY);
  tft.setCursor(sizeX+6, sizeY+21);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("STYLE");
  tft.setCursor(sizeX+6, sizeY+48);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print(selection.style);

  int styleX = maxW+4;
  int styleY = sizeY+62;
  tft.fillRoundRect(styleX, styleY, tft.width()-styleX, 58, corners, GREY);
  tft.drawRoundRect(styleX, styleY, tft.width()-styleX, 58, corners, LIGHT_GREY);
  tft.setCursor(styleX+6, styleY+21);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("SIZE");
  tft.setCursor(styleX+6, styleY+48);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print(selection.size);
}

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

void SETTINGS_SCREEN() {
  newTest = false;
  tft.fillScreen(BLACK);
  topBanner();
  homeButton();
  startButton();

  int upX = 0;
  int upY = 16;
  int size = 89;
  tft.fillRoundRect(upX, upY, size, size, corners, LIME);
  tft.drawRoundRect(upX, upY, size, size, corners, WHITE);
  tft.setCursor(upX+28, upY+50);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("UP");

  int downX = 0;
  int downY = upY+size+4;
  tft.fillRoundRect(downX, downY, size, size, corners, LIME);
  tft.drawRoundRect(downX, downY, size, size, corners, WHITE);
  tft.setCursor(downX+5, downY+52);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("DOWN");

  int brandX = size+4;
  int brandY = 16;
  tft.fillRoundRect(brandX, brandY, tft.width()-brandX, 58, corners, GREY);
  tft.drawRoundRect(brandX, brandY, tft.width()-brandX, 58, corners, LIGHT_GREY);
  tft.setCursor(brandX+6, brandY+21);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("BRAND");
  tft.setCursor(brandX+6, brandY+48);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print(selection.brand);

  int sizeX = size+4;
  int sizeY = brandY+62;
  tft.fillRoundRect(sizeX, sizeY, tft.width()-sizeX, 58, corners, GREY);
  tft.drawRoundRect(sizeX, sizeY, tft.width()-sizeX, 58, corners, LIGHT_GREY);
  tft.setCursor(sizeX+6, sizeY+21);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  if(selection.brand != "") {
    tft.setTextColor(LIME);
  } else {
    tft.setTextColor(LIGHT_GREY);
  }
  tft.print("STYLE");
  tft.setCursor(sizeX+6, sizeY+48);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print(selection.style);

  int styleX = size+4;
  int styleY = sizeY+62;
  tft.fillRoundRect(styleX, styleY, tft.width()-styleX, 58, corners, GREY);
  tft.drawRoundRect(styleX, styleY, tft.width()-styleX, 58, corners, LIGHT_GREY);
  tft.setCursor(styleX+6, styleY+21);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  if(selection.size != "") {
    tft.setTextColor(LIME);
  } else {
    tft.setTextColor(LIGHT_GREY);
  }
  tft.print("SIZE");
  tft.setCursor(styleX+6, styleY+48);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print(selection.size);
}

//**************************
// INFO SCREEN

void INFO_SCREEN() {
  tft.fillScreen(BLACK);
  topBanner();
  backButton();

  // TITLE BLOCK
  int line1X = 4;
  int line1Y = 16;
  tft.fillRoundRect(0, line1Y, tft.width(), 56, corners, LIME);
  tft.drawRoundRect(0, line1Y, tft.width(), 56, corners, WHITE);
  tft.setCursor(line1X, line1Y+20);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(BLACK);
  tft.print("Rogue Magnetics");
  tft.setCursor(line1X, line1Y+48);
  tft.print("Force Tester");

  // GREY BACKGROUND BOX
  tft.fillRoundRect(0, 76, tft.width(), 122, corners, GREY);
  tft.drawRoundRect(0, 76, tft.width(), 122, corners, LIGHT_GREY);

  // DEVICE LIMITS
  int line2X = 4;
  int line2Y = line1Y+71;
  tft.setCursor(line2X, line2Y+12);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(WHITE);
  tft.print("DEVICE LIMITS:");

  // FORCE LIMITS
  int box1X = 4;
  int box1Y = line2Y+18;
  tft.fillRoundRect(box1X, box1Y, 154, 89, corners-2, BLACK);
  tft.drawRoundRect(box1X, box1Y, 154, 89, corners-2, LIGHT_GREY);
  tft.setCursor(box1X+6, box1Y+22);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("Force (kg):");
  tft.setCursor(box1X+6, box1Y+52);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextColor(WHITE);
  tft.print("Min: 0");
  tft.setCursor(box1X+6, box1Y+80);
  tft.print("Max: 2000");

  // TRAVEL LIMITS
  int box2X = 162;
  int box2Y = box1Y;
  tft.fillRoundRect(box2X, box2Y, 154, 89, corners-2, BLACK);
  tft.drawRoundRect(box2X, box2Y, 154, 89, corners-2, LIGHT_GREY);
  tft.setCursor(box2X+6, box2Y+22);
  tft.setFont(&FreeSansBold12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(LIME);
  tft.print("Travel (mm):");
  tft.setCursor(box2X+6, box2Y+52);
  tft.setFont(&FreeSans12pt7b);
  tft.setTextColor(WHITE);
  tft.print("Min: 0");
  tft.setCursor(box2X+6, box2Y+80);
  tft.print("Max: 350");
}