#include <LiquidCrystal.h>
#include <Servo.h>
#include <EEPROM.h>

//      Threshholds
//min light in lux
int minlux = 150;
//max distance in cm
int maxdist = 350;
//Max VOC reading (currently for temperature)
int maxVOC = 30;




// Pin configurations
const int TRIGGER_PIN = 4;
const int ECHO_PIN = 3;
const int SERVO_PIN = 2;
const int PHOTORES_PIN = A15;
const int TEMP = A14;
const int LED = 53;
const int Button = 51;
const int Servo_Pin = 45;

// LCD initialization
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
Servo Servo;

// Constants
const double SPEED_OF_SOUND = 343.0; // Local speed of sound in m/s

// Variables
bool screen_on = true;
bool danger = false;
bool danger_memory = false;
int refresh_rate = 1000;
double lux;
bool dismissed = false;


// Function prototypes
double measureDistance();
double measureLux();
double measureTemp();

void controlDoor(double currentDistance);
void moveServo(int angle);
void LCD_Update(double currentDistance);
bool Screen_On(double currentDistance, double lux);
bool Read_Button();
bool Screen_state(int lux, int currentDistance);

void setup() {
  Serial.begin(115200);
  lcd.begin(16, 2);
  Servo.attach(Servo_Pin);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(PHOTORES_PIN, INPUT);
  pinMode(TEMP, INPUT);
  pinMode(LED, OUTPUT);
  pinMode(Button, INPUT_PULLUP);
  moveServo(0);
}

void loop() {
  //All sensors are measured and stored in variables. Functions convert analog input to units
  double currentDistance = measureDistance();
  double temp = measureTemp();
  double lux = measureLux();
  double VOC = temp;
  
  

  // Checks if dismissed button is pushed, and will change dismissed to true or false
  bool mem_dismissed = Read_Button();
  if (mem_dismissed == true){
      dismissed = true;
  }

  //Checks if screen should be on according to brightness and the US sensor data
  screen_on = Screen_state(lux, currentDistance);
  
  //Checks if the VOC sensor is over the threshold, and then changes the "danger" accordingly
  //Right now is configured to work with a temperature sensor 
  if (VOC > maxVOC){
    danger = true;
  }
  else{
    danger = false;
  }

  //Code runs when becomes Dangerous, not inbetween
  if (danger == true && danger_memory == false){

    dismissed = false;
    digitalWrite(LED, HIGH);
    moveServo(180);

  }

  //Runs when No Longer Dangerous
  if (danger == false && danger_memory == true){

    digitalWrite(LED, LOW);
    dismissed = false;
    moveServo(0);

  }

  //User Dismisses with the button
  if (dismissed == true){

    digitalWrite(LED, LOW);
    moveServo(0);

  }


  //Prints all of the variables in the serial monitor
  Serial_Print(currentDistance, lux, temp, screen_on,danger,danger_memory,dismissed);

  danger_memory = danger;

  //Delays to save power and not spam the serial monitor
  delay(refresh_rate);
}

bool Screen_state(int lux, int currentDistance){

  if (lux < minlux || currentDistance > maxdist){
    return false;
  } 
  else{
    return true;
  }

}

bool Read_Button(){
  if (digitalRead(Button) == 0) {
    Serial.print("BUTTON PUSHED \n\n\n\n");
    // turn LED on:
    return true;
  }
  else{
    return false;
  }
  
}

double measureLux(){
  // These constants should match the photoresistor's "gamma" and "rl10" attributes
  const float GAMMA = 0.7;
  const float RL10 = 50;

  // Convert the analog value into lux value:
  int lux_analogValue = analogRead(PHOTORES_PIN);
  float voltage = lux_analogValue / 1024. * 5;
  float resistance = 2000 * voltage / (1 - voltage / 5);
  return pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));
}

double measureTemp(){
  double analogValue = analogRead(TEMP);
  const float BETA = 3950; // should match the Beta Coefficient of the thermistor
  return (1 / (log(1 / (1023. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15);

}

double measureDistance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  return (duration / 2.0) * (SPEED_OF_SOUND / 10000);
}

void Serial_Print(double currentDistance, double lux, double temp, bool screen_on, bool danger, bool danger_memory, bool dismissed) {
  //Serial.println("\033[2J\033[H");
  Serial.print("US Distance: ");
  Serial.println(currentDistance);
  Serial.print("Light: ");
  Serial.println(lux);
  Serial.print("Temp: ");
  Serial.println(temp);
  Serial.print("Screen On: ");
  Serial.println(screen_on ? "true" : "false");
  Serial.print("Danger: ");
  Serial.println(danger ? "true" : "false");
  Serial.print("Danger Memory: ");
  Serial.println(danger_memory? "true" : "false");
  Serial.print("Dismissed: ");
  Serial.println(dismissed? "true" : "false");

  Serial.println();
}

void moveServo(int angle) {
  Servo.write(angle);
  
}