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

Servo servo1;
Servo servo2;

#define TFT_DC 9
#define TFT_CS 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

void setup() 
{
  Serial.begin(9600);
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT);

  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);

  servo1.attach(6);
  servo2.attach(5);

  ///////////


  tft.begin();

  //  tft.setCursor(26, 120);
  //  tft.setTextColor(ILI9341_RED);
  //  tft.setTextSize(3);
  //  tft.println("Hello, TFT!");

  //  tft.setCursor(20, 10);
  //  tft.setTextColor(ILI9341_GREEN);
  //  tft.setTextSize(1);
  //  tft.println("I can has colors?");


}

void loop() 
{
  int flip = digitalRead(4);

  int push1 = digitalRead(2);
  int push2 = digitalRead(3);
  int up1 = analogRead(A0);
  int up2 = analogRead(A1);
  int side1 = analogRead(A2);
  int side2 = analogRead(A3);

  int virt1 = map(up1, 0, 1023, 0, 180);
  int virt2 = map(up2, 0, 1023, 0, 180);
  int horz1 = map(side1, 0, 1023, 0, 180);
  int horz2 = map(side2, 0, 1023, 0, 180);

  servo1.write(virt2);
  servo2.write(horz2);


  tft.setCursor(20, 10);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print(up2);
  tft.print(" ");
  tft.print(side2);
  tft.print(" ");
  tft.print(push2);
  
  tft.setCursor(20, 30);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print(up1);
  tft.print(" ");
  tft.print(side1);
  tft.print(" ");
  tft.print(push1);


  if(flip == HIGH) //activate the serial readout
  {
    Serial.print(up1);
    Serial.print(" ");
    Serial.print(up2);
    Serial.print(" ");
    Serial.print(side1);
    Serial.print(" ");
    Serial.print(side2);
    Serial.print(" ");
    Serial.print(push1);
    Serial.print(push2);
    Serial.print('\n');


    delay(200);
  }
  
  if(up1 == 1023)
  {
    digitalWrite(7, HIGH);
  }
  if(up1 == 0)
  {
    digitalWrite(8, HIGH);
  }
  if(up1 == 512)
  {
    digitalWrite(7, LOW);
    digitalWrite(8, LOW);
  }
  delay(250);
  tft.setCursor(20, 10);
  tft.setTextColor(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.print(up2);
  tft.print(" ");
  tft.print(side2);
  tft.print(" ");
  tft.print(push2);
  
  tft.setCursor(20, 30);
  tft.setTextColor(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.print(up1);
  tft.print(" ");
  tft.print(side1);
  tft.print(" ");
  tft.print(push1);

}