// Calibration data is stored in SPIFFS so we need to include it
#include "FS.h"

#include <SPI.h>

#include <TFT_eSPI.h> // Hardware-specific library

TFT_eSPI tft = TFT_eSPI();       // Invoke custom library

// This is the file name used to store the touch coordinate
// calibration data. Change the name to start a new calibration.
#define CALIBRATION_FILE "/TouchCalData3"

// Set REPEAT_CAL to true instead of false to run calibration
// again, otherwise it will only be done once.
// Repeat calibration if you change the screen rotation.
#define REPEAT_CAL true

#define MenuWidth 240
#define MenuHeight 40

#define TFT_GREY 0x5AEB

// Pin definitions for Trims
#define Ail    32
#define Ele    33
#define Fla    25

#define MainMenuLevel  1
#define CalibrateMenuLevel  2
#define SettingsMenuLevel  3
#define TrimsMenuLevel  4
#define RestartMenuLevel  5


#define CALIBRATION_FILE "/TouchCalData3"

#define REPEAT_CAL false


//
int AilValue=0;
int EleValue=0;
int FlaValue=0;
int AilMin=0;
int EleMin=0;
int FlaMin=0;
int AilMax=0;
int EleMax=0;
int FlaMax=0;
int MenuTier=1;
int MenuYValue=0;
int AileronMin=0;
int AileronMax=0;

bool calAileron;
bool calElevator;
bool calFlaps;


// Menu definitions
const int MainMenuItems = 4;
const int CalibrateMenuItems = 4;
String mainMenu[MainMenuItems] = {"Calibrate", "Settings", "Trims", "Restart"};
String calibrateMenu[CalibrateMenuItems] = {"Aileron", "Elevator", "Flaps", "Back"};

void setup() {
  // put your setup code here, to run once:
  // Initialize Serial Monitor for debugging
  Serial.begin(115200);

  pinMode(Ail,INPUT_PULLUP);
  pinMode(Ele,INPUT_PULLUP);
  pinMode(Fla,INPUT_PULLUP);


  tft.init();
  // Set the rotation before we calibrate
  tft.setRotation(0);
  // call screen calibration
  touch_calibrate();
  // clear screen
  tft.fillScreen(TFT_BLUE);
  // tft.setTextSize(2);
  drawMainMenu();
  // drawBackground();

}

void loop() {
  // put your main code here, to run repeatedly:

  uint16_t x, y;

  if (! tft.getTouch(&x, &y)) {
    return;
  }

  // Print out the remapped (rotated) coordinates
  Serial.print("("); Serial.print(x);
  Serial.print(", "); Serial.print(y);
  Serial.println(")");

  MenuYValue = y;
  Serial.print("MenuYValue = "); Serial.println(MenuYValue);


  if (MenuTier == MainMenuLevel) {
    Serial.println("Decided it is a one!!!");
    MainMenuAction();
    } else if (MenuTier == CalibrateMenuLevel) {
      Serial.println("Decided it is a two!!!");
      CalibrateMenuAction();
      }


}


void MainMenuAction() {

  if (MenuYValue > 5) {
      if (MenuYValue < 40) {drawCalibrateMenu(); Serial.println("MainMenuAction");}
      else if (MenuYValue < 80) {drawSettingsMenu();} 
      else if (MenuYValue < 120) {drawBackground();} 
      else if (MenuYValue < 160) {}
  }
}

void CalibrateMenuAction() {

  if (MenuYValue > 5 && MenuYValue <=160) {
      if (MenuYValue > 120) {
        MenuTier = MainMenuLevel;
        drawMainMenu();
        return;
        }
      else if (MenuYValue > 80) {
        calibrateFlaps(); // Calibrate Flaps
        }
      else if (MenuYValue > 40) {
        calibrateElevator(); // Calibrate Elevator
        }
      else {calibrateAileron();}
       // Calibrate Aileron
      // Serial.println("CalibrateMenuAction");
    }
}

void drawCalibrateMenu() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_CYAN);
  tft.setTextSize(3);
  
  // Build a Menu outline = number of items defined
  for (int i = 0; i < CalibrateMenuItems; i++) {
      tft.drawRect(0, 10 + i * MenuHeight, MenuWidth, MenuHeight, TFT_YELLOW);
      tft.setCursor(10, 20 + i * MenuHeight);
      tft.print(calibrateMenu[i]);
  }

  // tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_YELLOW);
  MenuTier = CalibrateMenuLevel;
  
  delay (500);

}

void drawSettingsMenu(){

  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_CYAN);
  tft.setTextSize(3);

  tft.setCursor(5,50);
  tft.print("Reserved for future use");
  delay(3000);

  drawMainMenu();
  
}


void drawMainMenu() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_BLACK);
  tft.setTextSize(2);
  
  // Build a Menu outline = number of items defined
  for (int i = 0; i < MainMenuItems; i++) {
      tft.drawRect(0, 10 + i * MenuHeight, MenuWidth, MenuHeight, TFT_GREY);
      tft.fillRect(2, 10 + 2 + i * MenuHeight, MenuWidth - 2, MenuHeight - 2, TFT_WHITE);
      tft.drawCentreString(mainMenu[i],120, 17 + i * MenuHeight,2);
  }

  // tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  MenuTier = MainMenuLevel;
  delay (500);

}

void drawBackground() {
 
  tft.fillScreen(TFT_BLACK);
  delay(100);
  tft.setTextSize(1);
  
  // tft.setTextColor(TFT_GREEN, TFT_BLACK);
  tft.setTextColor(TFT_BLUE, TFT_BLACK);
     
  // Draw Flaps Meter
  tft.drawRect(35, 30, 40, 160, TFT_GREY);
  tft.fillRect(37, 50, 37, 160 - 20, TFT_WHITE);
  tft.fillRect(37 + 37, 50, 7, 70, TFT_GREEN);
  tft.fillRect(37 + 37, 50 + 70, 7, 70, TFT_YELLOW);
  tft.drawCentreString("FL",35 + 40 / 2, 30 + 2 , 2);
  // Draw Elevator Meter
  tft.drawRect(155, 30, 40, 179, TFT_GREY);
  tft.fillRect(157, 50, 37, 160 - 20, TFT_WHITE);
  tft.drawCentreString("DN",155 + 40 / 2, 30 + 2, 2);
  tft.drawCentreString("UP",155 + 40 / 2, 190 + 2, 2);
  // Draw Aileron Meter
  tft.drawRect(35, 240, 170, 34, TFT_GREY);
  tft.fillRect(35 + 20, 242, 170 - 40, 32, TFT_WHITE);
  tft.drawCentreString("L",44, 230 + 40 / 2, 2);
  tft.drawCentreString("R",196, 230 + 40 / 2, 2);
  
}


// Function to calibrate the Aileron
void calibrateAileron() {

  calAileron = false;

  while (! calAileron) {
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(5, 5);
  tft.setTextSize(2);
  tft.print("Set Aileron to minimum and press 'SET'");
  tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  tft.setCursor(90, 290);
  tft.setTextSize(3);
  tft.print("SET");

  waitforSet();

  AilMin = analogRead(Ail);
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(5, 5);
  tft.setTextSize(2);
  tft.print("Set Aileron to maximum and press 'SET'");
  tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  tft.setCursor(90, 290);
  tft.setTextSize(3);
  tft.print("SET");

  waitforSet();

  AilMax = analogRead(Ail);
  Serial.println("Aileron Min / Max");
  Serial.println(AilMin);
  Serial.println(AilMax);

  }

}


// Function to calibrate the Elevator
void calibrateElevator() {

  calElevator = false;

  while (! calElevator) {

 
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(5, 5);
  tft.setTextSize(2);
  tft.print("Set Elevator to minimum and press 'SET'");

  tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  tft.setCursor(90, 290);
  tft.setTextSize(3);
  tft.print("SET");

  waitforSet();

  EleMin = analogRead(Ele);

  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(5, 5);
  tft.setTextSize(2);
  tft.print("Set Elevator to maximum and press 'SET'");

  tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  tft.setCursor(90, 290);
  tft.setTextSize(3);
  tft.print("SET");

  waitforSet();

  EleMax = analogRead(Ele);

  Serial.println("Elevator Min / Max");
  Serial.println(EleMin);
  Serial.println(EleMax);

  }

}


// Function to calibrate the Flaps
void calibrateFlaps() {

  calFlaps = false;

  while (! calElevator) {

 
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(5, 5);
  tft.setTextSize(2);
  tft.print("Set Flaps to minimum and press 'SET'");

  tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  tft.setCursor(90, 290);
  tft.setTextSize(3);
  tft.print("SET");

  waitforSet();

  FlaMin = analogRead(Fla);

  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(5, 5);
  tft.setTextSize(2);
  tft.print("Set Flaps to maximum and press 'SET'");

  tft.drawRect(0, 280, MenuWidth, MenuHeight, TFT_GREEN);
  tft.setCursor(90, 290);
  tft.setTextSize(3);
  tft.print("SET");

  waitforSet();

  FlaMax = analogRead(Fla);

  Serial.println("Flaps Min / Max");
  Serial.println(FlaMin);
  Serial.println(FlaMax);

  }

}

void waitforSet() {

  // bool setPressed = false;

  // while (! setPressed) {
  //   while (! ctp.touched()) {
  //     }
  //   // Retrieve a point  
  //   TS_Point p = ctp.getPoint();
  //   // flip it around to match the screen.
  //   p.x = map(p.x, 0, 240, 240, 0);
  //   p.y = map(p.y, 0, 320, 320, 0);
  //   // Serial.println("This is in the Cal Function");
  //   Serial.print("("); Serial.print(p.x);
  //   Serial.print(", "); Serial.print(p.y);
  //   Serial.println(")");
  //   if (p.y > 280) {
  //     setPressed = true;
  //     Serial.println("Set was Pressed");
  //   }
  // }
}


void touch_calibrate()
{
  uint16_t calData[5];
  uint8_t calDataOK = 0;

  // check file system exists
  if (!SPIFFS.begin()) {
    Serial.println("Formatting file system");
    SPIFFS.format();
    SPIFFS.begin();
  }

  // check if calibration file exists and size is correct
  if (SPIFFS.exists(CALIBRATION_FILE)) {
    if (REPEAT_CAL)
    {
      // Delete if we want to re-calibrate
      SPIFFS.remove(CALIBRATION_FILE);
    }
    else
    {
      File f = SPIFFS.open(CALIBRATION_FILE, "r");
      if (f) {
        if (f.readBytes((char *)calData, 14) == 14)
          calDataOK = 1;
        f.close();
      }
    }
  }

  if (calDataOK && !REPEAT_CAL) {
    // calibration data valid
    tft.setTouch(calData);
  } else {
    // data not valid so recalibrate
    tft.fillScreen(TFT_BLACK);
    tft.setCursor(20, 0);
    tft.setTextFont(2);
    tft.setTextSize(1);
    tft.setTextColor(TFT_WHITE, TFT_BLACK);

    tft.println("Touch corners as indicated");

    tft.setTextFont(1);
    tft.println();

    if (REPEAT_CAL) {
      tft.setTextColor(TFT_RED, TFT_BLACK);
      tft.println("Set REPEAT_CAL to false to stop this running again!");
    }

    tft.calibrateTouch(calData, TFT_MAGENTA, TFT_BLACK, 15);

    tft.setTextColor(TFT_GREEN, TFT_BLACK);
    tft.println("Calibration complete!");

    // store data
    File f = SPIFFS.open(CALIBRATION_FILE, "w");
    if (f) {
      f.write((const unsigned char *)calData, 14);
      f.close();
    }
  }
}