#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include <Servo.h>

Servo myservo;

#define servoOut 7
//#define exR_Open 2
//#define exR_Closed 9
//#define exL_Open 4
//#define exL_Closed 5

#define but1 5
#define but2 8
#define but3 9

int valvePos = 0;
int AMode = 0;
int MMode = 0;
int SMode = 0;
int a = 0;
int b = 0;
int c = 0;
int d = 120;
int start = 0;
int x = 16;

int RPM = 0;

String open = "Open";
String closed = "Closed";

#define TFT_DC 4
#define TFT_CS 3
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
#define pushbutton 5


void setup() {

  // put your setup code here, to run once:
  Serial.begin(9600);
  myservo.attach(7);
  myservo.write(0);

  tft.begin();
  tft.setRotation(1);

  tft.drawCircle(d, 18, 12, ILI9341_WHITE);
  tft.drawCircle(d + x, 18, 12, ILI9341_WHITE);
  tft.drawCircle(d + x + x, 18, 12, ILI9341_WHITE);
  tft.drawCircle(d + x + x + x, 18, 12, ILI9341_WHITE);



  tft.setCursor(0, 40);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.println("Audi A6 Exhaust Controller");

  tft.setCursor(0, 60);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.println("Select Mode");

  tft.setCursor(23, 90);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.println("Current Mode:");

  tft.setCursor(0, 120);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.println("Valve position:");

  tft.fillRoundRect(185, 85, 120, 25, 8, ILI9341_WHITE);

  tft.fillRoundRect(185, 115, 120, 25, 8, ILI9341_WHITE);

  pinMode(but1, INPUT_PULLUP);
  pinMode(but2, INPUT_PULLUP);
  pinMode(but3, INPUT_PULLUP);
  start = 1;


}

void loop() {



  int button1 = digitalRead(but1);
  int button2 = digitalRead(but2);
  int button3 = digitalRead(but3);

  if (button1 == LOW or start == 1) AMode = 1, MMode = 0, SMode = 0, b = 0, c = 0, AutomaticMode();
  if (button2 == LOW) MMode = 1, SMode = 0, AMode = 0, a = 0, c = 0, start = 0, ManualMode();
  if (button3 == LOW) SMode = 1, AMode = 0, MMode = 0, a = 0, b = 0, start = 0, SportMode();

  if (AMode == 1) {
    RPM = analogRead(A1);
    Serial.print(RPM);
    valvePos = (RPM / 5.7);
    myservo.write(valvePos);
    Serial.print("  Valve Pos: ");
    Serial.println(valvePos);
  }
  //  Serial.print("AMode = "), Serial.print(AMode), Serial.print("  SMode = "), Serial.print(SMode),
  //  Serial.print("  MMode = "), Serial.print(MMode), Serial.print("  a = "), Serial.print(a),
  //  Serial.print("  b = "), Serial.print(b), Serial.print("  c = "), Serial.print(c), Serial.print(" Valve Position = "), Serial.println(valvePos);
}

void AutomaticMode() {
  if (a == 0) {
    tft.fillRoundRect(185, 85, 120, 25, 8, ILI9341_WHITE);
    tft.setCursor(190, 90);
    tft.setTextColor(ILI9341_BLACK);
    tft.setTextSize(2);
    tft.print("Automatic");
    //tft.setCursor(165, 85);
    //tft.setTextColor(ILI9341_BLACK);
    //tft.print(open);

    a = 1;
  }
}

void ManualMode() {
  while (MMode == 1) {
    if (b == 0) {
      tft.fillRoundRect(185, 85, 120, 25, 8, ILI9341_WHITE);
      tft.setCursor(190, 90);
      tft.setTextColor(ILI9341_BLACK);
      tft.setTextSize(2);
      tft.print("Manual");
      valvePos = 90;
      myservo.write(valvePos);
      b = 1;
    }
    break;
  }
}

void SportMode() {
  while (SMode == 1) {
    if (c == 0) {
      tft.fillRoundRect(185, 85, 120, 25, 8, ILI9341_WHITE);
      tft.setCursor(190, 90);
      tft.setTextColor(ILI9341_BLACK);
      tft.setTextSize(2);
      tft.print("Sport");
      valvePos = 180;
      myservo.write(valvePos);
      c = 1;
    }
    break;
  }
}

void tempControl() {

}