//____________________________________________________________________________________________________________________________
//LIBRARIES AND DEFINITIONS---------------------------------------------------------------------------------------------------
#include <Adafruit_GFX.h>    // Core graphics library
#include <Adafruit_ST7789.h> // Hardware-specific library for ST7789
#include <SPI.h>
#include <Adafruit_NeoPixel.h>

#define TFT_CS        D10
#define TFT_RST        -1 // Or set to -1 and connect to Arduino RESET pin
#define TFT_DC         D9
#define TFT_COPI D11  // Data out
#define TFT_SCLK D13  // Clock out

#define BLACK    0x0000
#define BLUE     0x001F
#define RED      0xF800
#define GREEN    0x07E0
#define CYAN     0x07FF
#define MAGENTA  0xF81F
#define YELLOW   0xFFE0 
#define WHITE    0xFFFF




//____________________________________________________________________________________________________________________________
//GLOBAL VARIABLES------------------------------------------------------------------------------------------------------------
Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_COPI, TFT_SCLK, TFT_RST);
float p = 3.1415926;
uint16_t screenWidth = 320;
uint16_t screenHeight = 172;
uint16_t cx = screenWidth/2;
uint16_t cy = screenHeight/2;

//Eye Constants
uint16_t numLines = 10; //Number of vertical lines that make up eye
uint16_t space = 5; //Distance between vertical lines that make up eye
uint16_t eyeWidth = (numLines-1)*space; //Height of vertical lines that make up eye
uint16_t eyeHeight = 80; //Height of vertical lines that make up eye
uint16_t eyeColor = WHITE;
uint16_t eyex = cx;
uint16_t eyey = cy;
int screenLimit = 6; //Controls when eye has reached edge of screen
bool eyesWereClosed = false; //Keeps track of last time eyes were closed

//Ultrasonic Sensor Constants
const int echoPin1 = 8;
const int trigPin1 = 7;
const int echoPin2 = 6;
const int trigPin2 = 5;
const int echoPin3 = 4;
const int trigPin3 = 3;

//LED
#define LED_PIN    2
#define LED_COUNT 12
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);


//Other Stuff
unsigned long inViewTime = 0; //keeps track of how long someone has been in view
bool inViewTimerStarted = false; //keeps track of whether or not a timer has already been started
unsigned long inViewTimerStartTime = millis(); //beginning time for inViewTimer
bool tweet = false; //Tells us whether or not we should tweet
bool hasPrintedTweet = false; //Tells us whether or not a tweet has already been made for the person in view.




//____________________________________________________________________________________________________________________________
//INITIALIZATION--------------------------------------------------------------------------------------------------------------
void setup(void) {

  //SPI.begin();  // init SPI

  Serial.begin(9600);
  Serial.print(F("Hello! ST77xx TFT Test"));

  // OR use this initializer (uncomment) if using a 1.47" 172x320 TFT:
  tft.init(172, 320);
  tft.setSPISpeed(32000000);
  tft.setRotation(3);
  //tft.setSPIFreqency(1000000); // set 1 MHz
  //tft.initR(); // init @ 1 MHz.
  Serial.println(F("Initialized"));

  //Screen initialization
  tft.fillScreen(BLACK);
  for (int i=0;i<numLines;i++) {
    tft.drawFastVLine((eyex-eyeWidth/2)+(i*space), eyey-(eyeHeight/2), eyeHeight, WHITE);
  }

  //Ultrasonic sensor initialization
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);

  //LED
  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
  strip.show();            // Turn OFF all pixels ASAP
  strip.setBrightness(255); // Set BRIGHTNESS to about 1/5 (max = 255)
  for (int i=0; i < 12; i++) {
    strip.setPixelColor(i, 0, 0, 255);
  }
}




//____________________________________________________________________________________________________________________________
//LOOP------------------------------------------------------------------------------------------------------------------------
void loop() {
  //Ultrasonic sensors
  int dist1 = getDist(trigPin3, echoPin3); //Right
  int dist2 = getDist(trigPin2, echoPin2); //Mid
  int dist3 = getDist(trigPin1, echoPin1); //Left
  //Serial.print("Left: ");
  //Serial.print(dist3);
  //Serial.print("cm           ");
  //Serial.print("Mid: ");
  //Serial.print(dist2);
  //Serial.print("cm           ");
  //Serial.print("Right: ");
  //Serial.print(dist1);
  //Serial.print("cm           ");
  //Serial.print("Time in View (s): ");
  //Serial.print(inViewTime);
  //Serial.print("s            ");
  //Serial.print("Print tweet?   ");
  //Serial.println(tweet);

  //Moving eye
  if (dist1 < 60 || dist2 < 60 || dist3 < 60) {
    startInViewTimer();
    strip.fill(0x0000FF);
    strip.show();
    if (eyesWereClosed == true) openEyes(tft, eyex, eyey);
    if (abs(dist3 - dist2) <= 8 && abs(dist2 - dist1) <= 8) moveTowardCenter(tft, eyex, eyey);
    else {
      if (dist3 < dist2 && dist3 < dist1 && abs(dist3 - dist2) >= 8) moveLeft(tft, eyex, eyey);
      else if (dist1 < dist2 && dist1 < dist3 && abs(dist2 - dist1) >= 8) moveRight(tft, eyex, eyey);
      else {
        if (abs(dist3 - dist2) <= 8) moveTowardCenterLeft(tft, eyex, eyey);
        else if (abs(dist2 - dist1) <= 8) moveTowardCenterRight(tft, eyex, eyey);
        else {
          moveTowardCenter(tft, eyex, eyey);
        }
      }
    }
  }
  else {
    strip.clear();
    strip.show();
    closeEyes(tft, eyex, eyey);
    endInViewTimer();
  }

  //Decides whether or not a tweet should be made
  printTweet();
  if (tweet) Serial.println("Generate tweet");
}






//____________________________________________________________________________________________________________________________
//HELPER FUNCTIONS------------------------------------------------------------------------------------------------------------

//Decides whether or not a tweet should be made
void printTweet() {
  if (inViewTime >= 4 && hasPrintedTweet == false) {
    tweet = true;
    hasPrintedTweet = true;
  }
  else {
    tweet = false;
  }
}

//Begins a timer of how long somebody has been in view
void startInViewTimer() {
  if (inViewTimerStarted == false) {
    inViewTimerStartTime = millis();
    inViewTimerStarted = true;
  }
  inViewTime = abs(millis() - inViewTimerStartTime)/1000;
}

//Ends the timer for how long somebody has been in view
void endInViewTimer() {
  inViewTime = 0;
  inViewTimerStarted = false;
  hasPrintedTweet = false;
}

//Eye close animation
void closeEyes(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  uint16_t x = xmid - (eyeWidth/2);
  uint16_t y = ymid - (eyeHeight/2);
  for (int i=0; i < numLines; i++) {
    screen.drawFastVLine(x + (i*space), y, eyeHeight - 10, BLACK);
  }
  eyesWereClosed = true;
}

//Eye open animation
void openEyes(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  uint16_t x = xmid - (eyeWidth/2);
  uint16_t y = ymid - (eyeHeight/2);
  for (int i=0; i < numLines; i++) {
    screen.drawFastVLine(x + (i*space), y, eyeHeight - 10, WHITE);
  }
  eyesWereClosed = false;
}

//Takes center position of eye and moves it toward the center of the screen
void moveTowardCenter(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  if (xmid < cx) moveRight(screen, xmid, ymid);
  if (xmid > cx) moveLeft(screen, xmid, ymid);
}

//Takes center position of eye and moves it toward center left part of screen
void moveTowardCenterLeft(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  int xCenterLeft = screenWidth/3;
  if (xmid < xCenterLeft) moveRight(screen, xmid, ymid);
  if (xmid > xCenterLeft) moveLeft(screen, xmid, ymid);
}

//Takes center position of eye and moves it toward center right part of screen
void moveTowardCenterRight(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  int xCenterRight = (screenWidth*2)/3;
  if (xmid < xCenterRight) moveRight(screen, xmid, ymid);
  if (xmid > xCenterRight) moveLeft(screen, xmid, ymid);
}

//Takes center position of eye and moves it to the right
void moveRight(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  uint16_t x = xmid - (eyeWidth/2);
  uint16_t y = ymid - (eyeHeight/2);
  if(x + eyeWidth + space <= screenWidth - screenLimit) {
    screen.drawFastVLine(x, y, eyeHeight, BLACK);
    screen.drawFastVLine(x+eyeWidth+space, y, eyeHeight, WHITE);
    eyex+=space;
  }
}

//Takes center position of eye and moves it to the left
void moveLeft(Adafruit_ST7789 screen, uint16_t xmid, uint16_t ymid) {
  uint16_t x = xmid - (eyeWidth)/2;
  uint16_t y = ymid - (eyeHeight/2);
  if(x - space >= screenLimit) {
    screen.drawFastVLine(x - space, y, eyeHeight, WHITE);
    screen.drawFastVLine(x + eyeWidth, y, eyeHeight, BLACK);
    eyex-=space;
  }
}

//Returns distance detected by ultrasonic sensor
int getDist(int trigPin, int echoPin) {
  long duration, cm;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  return cm;
}

//For Ultrasonic Sensor From https://www.tutorialspoint.com/arduino/arduino_ultrasonic_sensor.htm
long microsecondsToInches(long microseconds) {
   return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds) {
   return microseconds / 29 / 2;
}