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

RTC_DS1307 rtc;

#define TFT_CS    8   // Double check this
#define TFT_DC    9

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);


// relay pins
const int gnssRelay = 1;
const int lidarRelay = 2;
const int sstRelay = 3;
const int metRelay = 4;
const int centralRelay = 5;


const int voltagePin = A0;

// resistor for voltage divider
const float R1 = 100000.0;
const float R2 = 10000.0;

const float voltageReference = 5.0;
const float lowVoltageThreshold = 14; 



void setup() {
  Serial.begin(9600);

  Serial.println("Initializing TFT...");
  tft.begin();

  // relays
  pinMode(gnssRelay, OUTPUT);
  pinMode(lidarRelay, OUTPUT);
  pinMode(sstRelay, OUTPUT);
  pinMode(metRelay, OUTPUT);
  pinMode(centralRelay, OUTPUT);
  
  if (! rtc.begin()) {
    Serial.println("Couldn't find RTC");
    Serial.flush();    
    tft.println("Couldn't find RTC");
    // Serial.flush();
    abort();
  }

  tft.setRotation(1);           // Landscape
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setCursor(50, 100);
  tft.setTextColor(ILI9341_WHITE);
  tft.println("* RTC Setup Done");  
  Serial.println("* RTC Setup Done");

  delay(3000);

}

void loop() {

  DateTime now = rtc.now();

  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLUE);
  tft.setCursor(10, 10);
  tft.setTextColor(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.println("Power Management System");

  tft.setCursor(5, 200);
  tft.print(now.year(), DEC);
  tft.print('/');
  tft.print(now.month(), DEC);
  tft.print('/');
  tft.println(now.day(), DEC);

  tft.setCursor(200, 200);
  tft.print(now.hour(), DEC);
  tft.print(':');
  tft.print(now.minute(), DEC);
  tft.print(':');
  tft.print(now.second(), DEC);
  tft.println();
  // delay(10);
  voltageChecker();
}




void voltageChecker() {
  float analogValue = analogRead(voltagePin);
  float vOut = (analogValue / 1023.0) * voltageReference;
  float actualVoltage = vOut / (R2 / (R1 + R2));

  Serial.print("Voltage: ");
  Serial.println(actualVoltage);

 
 for (int i = 14; i<=12; i--){
  Serial.println()("I = ", String(i));
  
  if (i < lowVoltageThreshold) {
    digitalWrite(lidarRelay, LOW);
    }
  else if (i < lowVoltageThreshold - 1) {
    digitalWrite(centralRelay, LOW);
 
  }   else if (i < lowVoltageThreshold - 2) {
    digitalWrite(metRelay, LOW);
 
  }   else if (i < lowVoltageThreshold - 3) {
    digitalWrite(sstRelay, LOW);
 
  }   else if (i < lowVoltageThreshold - 4) {
    digitalWrite(gnssRelay, LOW);
 
  } 
  else {
    digitalWrite(lidarRelay, HIGH);
    digitalWrite(centralRelay, HIGH);
    digitalWrite(metRelay, HIGH);
    digitalWrite(sstRelay, HIGH);
    digitalWrite(gnssRelay, HIGH);
  }
  delay(1000);
 }

}