#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

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

#define TCS_pin 10
#define RST_pin 9
#define DC_pin 8

int led_430 = 3;
int led_680 = 2;
int detector = A0;

double P_nw_430 = 1.0;  //change value to be correct
double P_nw_680 = 1.0;
double responsivity_430 = 0.221;
double responsivity_680 = 0.448;
double R_load = 50;

unsigned long timeSoFar;
unsigned long timeNow;
unsigned long test_time = 5000;
int buttonPin = 4;


Adafruit_ILI9341 tft_display = Adafruit_ILI9341(TCS_pin, DC_pin, RST_pin);

void setup() {
  tft_display.begin();
  tft_display.setRotation(1);

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

  tft_display.setCursor(20, 160);
  tft_display.setTextColor(ILI9341_GREEN);
  tft_display.setTextSize(2);
  tft_display.println("I can has colors?");
  Serial.begin(9600);

  // set LED to be an output pin
  pinMode(led_430, OUTPUT);
  pinMode(led_680, OUTPUT);
  pinMode(detector, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);

}

void loop() {
  
timeSoFar = millis();
  if (digitalRead(buttonPin) == LOW) {
    tft_display.fillScreen(WHITE);
    tft_display.setCursor(0, 0);
    tft_display.setTextColor(BLUE);
    tft_display.setTextWrap(true);
    tft_display.setTextSize(3);
    tft_display.print("Starting test!");
    triggerLED(led_430, test_time);
    triggerLED(led_680, test_time);
    tft_display.fillScreen(WHITE);
    tft_display.setCursor(0, 0);
    tft_display.setTextColor(BLUE, WHITE);
    tft_display.setTextWrap(true);
    tft_display.setTextSize(3);
    tft_display.print("Test is complete!");
  }

  //tft_display.fillScreen(BLACK);
  //tft_display.draw_text(voltage, BLUE, 0, 0);
}
  

void triggerLED(int led_pin, unsigned long time) {
  timeNow = timeSoFar;
  while (timeSoFar - timeNow <= time) {
    timeSoFar = millis();
    digitalWrite(led_pin, HIGH);
    double V_sample = analogRead(detector) * 5.0/1023.0;
    double I_sample = V_sample / R_load;
    double P_sample = I_sample / responsivity_430;
    double absorbance = log10(P_sample / P_nw_430);
    //tft_display.fillScreen(WHITE);
    tft_display.setCursor(tft_display.width()/2, tft_display.height()/2);
    tft_display.setTextColor(BLUE, WHITE);
    tft_display.setTextWrap(true);
    tft_display.setTextSize(3);
    tft_display.print(V_sample);
  }
  digitalWrite(led_pin, LOW);
}