#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include <Wire.h>      // this is needed for FT6206
#include <Adafruit_FT6206.h>


#define TFT_CS 22
#define TFT_DC 26
#define TFT_MOSI 28
#define TFT_CLK 30
#define TFT_RST 24
#define TFT_MISO 34

#define TS_MINX 150
#define TS_MINY 130
#define TS_MAXX 3800
#define TS_MAXY 4000

// Adafruit_FT6206 ctp = Adafruit_FT6206();

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_CLK, TFT_RST, TFT_MISO);

const int sw = 42;
const int x = A1;
const int y = A0;

int state = 0;
bool delta = false;
int speed=0;
double curr_input = 0;
double prev_input = 0;
int y_state = 0;



void setup() {
  while (!Serial);
  Serial.begin(115200);
  pinMode(x, INPUT_PULLUP);
  pinMode(y, INPUT_PULLUP);
  pinMode(sw, INPUT);
  digitalWrite(sw, HIGH);
  tft.begin();

  // if (! ctp.begin(40)) {  // pass in 'sensitivity' coefficient
  //   Serial.println("Couldn't start FT6206 touchscreen controller");
  //   while (1);
  // }
  
  tftMenu(state);


  while (state!=-1) {
    // TS_Point p = ctp.getPoint();
    int x_read = analogRead(x);
    int y_read = analogRead(y);
    int sw_read = digitalRead(sw);
    // Serial.print("x = ");
    // Serial.println(x_read);
    // Serial.print("y = ");
    // Serial.println(y_read);
    // Serial.print("sw = ");
    // Serial.println(sw_read);
    curr_input = millis();
    // lcdMenu(state, speed);
    tftMenu(state);
    // p.x = map(p.x, 0, 240, 240, 0);
    // p.y = map(p.y, 0, 320, 320, 0);

    

    if (curr_input-prev_input > 200) {
      if (x_read > 768 ) {
        tft.fillScreen(ILI9341_BLACK);
        state = (state+1) % 3;
        tftMenu(state);
        prev_input = curr_input;
        Serial.print("state= ");
        Serial.println(state);
      }
      else if (x_read < 256) {
        tft.fillScreen(ILI9341_BLACK);
        state = (state-1) % 3;
        if (state<0) {state+=3;}
        tftMenu(state);
        prev_input = curr_input;
        Serial.print("state= ");
        Serial.println(state);
      }
      switch (state) {
        case -1:
          Serial.println("run");
          delay(1000);
          state = 0;
        case 0:
          if (sw_read==0) {
            tft.fillScreen(ILI9341_BLACK);

            switch (y_state) {
              tftMenu(state);
              case 0: //low torque
                speed = 1200;
                break;
              case 1: //medium torque
                speed = 1000;
                break;
              case 2: //high torque
                speed = 700;
                break;
            }
            setup();
            state = -1;
          }
          break;
        case 1: //settings
          if (y_read > 768) {
            y_state = (y_state+1) % 3;
            prev_input = curr_input;
          }
          else if (y_read < 256) {
            y_state = (y_state-1) % 3;
            if (y_state<0) {y_state=y_state+3;}
            prev_input = curr_input;
          }
          break;
        case 2: //reset
          // resetMotors();
          tftMenu(state);
      }
    }
    // tftMenu(state);
  }
}
String list[3] = {"Menu", "Settings", "Reset"};
String settings[3] = {"low", "medium", "high"};

void tftMenu(int state) {
  switch (state) {
    case 0: // home
      tft.setCursor(26, 120);
      tft.setTextColor(ILI9341_RED);
      tft.drawFastHLine(0, 130, 2, ILI9341_BLUE);
      tft.setTextSize(3);
      tft.println(list[state]);

      tft.setCursor(20, 160);
      tft.setTextColor(ILI9341_GREEN);
      tft.setTextSize(2);
      tft.println("input");
    case 1: // settings
      tft.setCursor(26, 120);
      tft.setTextColor(ILI9341_RED);
      tft.setTextSize(3);
      tft.println(list[state]);

      tft.setCursor(20, 160);
      tft.setTextColor(ILI9341_GREEN);
      tft.setTextSize(2);
      tft.println(settings[state]);
    case 2: // reset
      tft.setCursor(26, 120);
      tft.setTextColor(ILI9341_RED);
      tft.setTextSize(3);
      tft.println(list[state]);

      tft.setCursor(20, 160);
      tft.setTextColor(ILI9341_GREEN);
      tft.setTextSize(2);
      tft.println("input");
  }
}

void loop() { }