#include <SPI.h>
#include <MFRC522.h>
#include <DHT.h>
#include <LedControl.h>
#include <LiquidCrystal.h>
#include <Servo.h>
#include <Stepper.h>

#define SS_PIN 53
#define RST_PIN 9
#define DHTPIN 2
#define DHTTYPE DHT22
#define RELAY_PIN 6
#define SERVO_PIN 10
#define STEP_PIN 11
#define DIR_PIN 12
#define STEPPER_PIN3 13
#define STEPPER_PIN4 14
#define ULTRASONIC_TRIG 22
#define ULTRASONIC_ECHO 23
#define LED_DATA_IN 3
#define LED_CLK 5
#define LED_CS 4
#define LCD_RS 42
#define LCD_EN 43
#define LCD_D4 44
#define LCD_D5 45
#define LCD_D6 46
#define LCD_D7 47
#define LED_RED 7
#define STEPS_PER_REV 2048
#define ULTRASONIC_THRESHOLD 25.0
const int segmentPins[8] = {30, 31, 32, 33, 34, 35, 36, 37}; // A, B, C, D, E, F, G, DP
const int digitPins[4] = {38, 39, 40, 41}; // Digit 1, 2, 3, 4

byte numbers[10][8] = {
  {1,1,1,1,1,1,0,0}, // 0
  {0,1,1,0,0,0,0,0}, // 1
  {1,1,0,1,1,0,1,0}, // 2
  {1,1,1,1,0,0,1,0}, // 3
  {0,1,1,0,0,1,1,0}, // 4
  {1,0,1,1,0,1,1,0}, // 5
  {1,0,1,1,1,1,1,0}, // 6
  {1,1,1,0,0,0,0,0}, // 7
  {1,1,1,1,1,1,1,0}, // 8
  {1,1,1,1,0,1,1,0}  // 9
};

MFRC522 mfrc522(SS_PIN, RST_PIN);
DHT dht(DHTPIN, DHTTYPE);
LedControl lc(LED_DATA_IN, LED_CLK, LED_CS, 1);
LiquidCrystal lcd(LCD_RS, LCD_EN, LCD_D4, LCD_D5, LCD_D6, LCD_D7);
Servo gateServo;
//Stepper stepper(STEPS_PER_REV, STEPPER_PIN1, STEPPER_PIN2, STEPPER_PIN3, STEPPER_PIN4);

bool rfidScanned = false;
bool temperatureChecked = false;
bool enterSuccess = false;
bool resetPosition = false;

unsigned long openTime = 0;

void resetSystem();
float getUltrasonicDistance();
void waitForUserToStepForward();
void scanRFID();
void checkTemperature();
void displayTemperature(float tempC);
void rotateStepperMotor(int steps, bool direction);
void grantAccess();
void displayCheckMark();
void displayRedX();
void lcdDisplayMessage(String message);
void displayDigit(int digit, int number, bool decimalPoint = false);
void displayNumber(int number);
void clearDigits();

unsigned long tempCheckTime = 0; // Timestamp of when the temperature was successfully checked
bool isEntryPending = false; // Indicates if an entry process is pending (temperature checked but waiting to rotate stepper)

void setup() {
  Serial.begin(9600);
  SPI.begin();
  mfrc522.PCD_Init();
  dht.begin();
  lcd.begin(16, 2);
  gateServo.attach(SERVO_PIN);
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  lc.shutdown(0, false);
  lc.setIntensity(0, 8);
  lc.clearDisplay(0);
  pinMode(ULTRASONIC_TRIG, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(STEP_PIN, LOW);
  digitalWrite(DIR_PIN, LOW);

  // Set segment pins as output
  for (int i = 0; i < 8; i++) {
    pinMode(segmentPins[i], OUTPUT);
  }

  // Set digit pins as output
  for (int i = 0; i < 4; i++) {
    pinMode(digitPins[i], OUTPUT);
  }
}

void loop() {
  static bool led2State = false; // Track LED2 (yellow) flashing state
  static unsigned long previousMillis = 0;
  const long interval = 500; // Interval at which to blink (milliseconds)
  unsigned long currentMillis = millis();
  if (getUltrasonicDistance() <= ULTRASONIC_THRESHOLD) {
    rotateStepperMotor(1024, HIGH); // Rotate 180 degrees
    // Wait for a moment or until a condition is met to rotate back
  }
  if (isEntryPending && millis() - tempCheckTime >= 5000) {
    // Rotate the stepper motor to grant access
    rotateStepperMotor(1024, HIGH); // Adjust direction as needed
    isEntryPending = false; // Reset the entry pending flag
    enterSuccess = true; // Mark the entry as successful
  }
  if (resetPosition) {
    resetSystem();
    digitalWrite(RELAY_PIN, HIGH); // Ensure LED1 (green) remains off
    led2State = false; // Reset LED2 state for consistency
    lcdDisplayMessage("READY: STEP FORWARD");
    digitalWrite(LED_RED, HIGH); // LED2 (yellow) solid on when system is ready
    gateServo.write(90); // Reset servo position
    resetPosition = false; // Clear system reset flag
  } else if (!rfidScanned) {
    digitalWrite(LED_RED, HIGH); // LED2 (yellow) remains solid on
    if (getUltrasonicDistance() > ULTRASONIC_THRESHOLD) {
      waitForUserToStepForward();
    }
    scanRFID();
  } else if (!temperatureChecked) {
    checkTemperature();
  } else {
    if (!enterSuccess) {
      displayCheckMark();
      grantAccess();
    } else {
      if (currentMillis - openTime < 5000) {
        // Flash LED2 (yellow) while the servo is opening
        if (currentMillis - previousMillis >= interval) {
          previousMillis = currentMillis;
          led2State = !led2State;
          digitalWrite(LED_RED, led2State ? HIGH : LOW); // Toggle LED2 (yellow)
        }
      } else {
        // After servo operation, make sure LED2 (yellow) turns solid again
        digitalWrite(LED_RED, HIGH); // Ensure LED2 (yellow) is solid on
        resetPosition = true; // Flag to reset the system for the next user
      }
    }
  }
}

void resetSystem() {
  rfidScanned = false;
  temperatureChecked = false;
  enterSuccess = false;
  resetPosition = false;
}

float getUltrasonicDistance() {
  digitalWrite(ULTRASONIC_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  long duration = pulseIn(ULTRASONIC_ECHO, HIGH);
  return duration * 0.034 / 2;
}

void waitForUserToStepForward() {
  lcdDisplayMessage("READY: STEP FORWARD");
  while (getUltrasonicDistance() > ULTRASONIC_THRESHOLD) {
    delay(100);
  }
  rotateStepperMotor(1024, HIGH);
}

void scanRFID() {
  lcdDisplayMessage("SCAN RFID");
  delay(2000);
  rfidScanned = false;
  displayRedX();
  rfidScanned=true;
  displayCheckMark();
  lcdDisplayMessage("Scan wrist for temperature check. Hold until Check");
}

void checkTemperature() {
  float tempC = dht.readTemperature();
  Serial.print("Temp: "); Serial.println(tempC);

  if (!isnan(tempC)) {
    if (tempC > 34.0) { // Change from >= 34.0 to > 34.0 if you want strictly above 34.0
      displayTemperature(tempC); // This will now only display if the condition is true
      temperatureChecked = true;
      tempCheckTime = millis();
      isEntryPending = true;
    } else {
      // You can handle the case for temperature <= 34.0 here if needed
      lcdDisplayMessage("Temp below threshold.");
      delay(4000); // Show message for a period
      resetSystem();
    }
  } else {
    lcdDisplayMessage("Temp Read Error. Rescan Wrist.");
  }
}


void displayNumber(int number) {
  for (int digit = 0; digit < 4; digit++) {
    int digitValue = number % 10;
    number /= 10;
    displayDigit(digit, digitValue);
    delay(5);
  }
}

void displayDigit(int digit, int number, bool decimalPoint = false) {
    for (int i = 0; i < 4; i++) {
        digitalWrite(digitPins[i], HIGH);
    }

    for (int segment = 0; segment < 7; segment++) {
        digitalWrite(segmentPins[segment], numbers[number][segment] ? LOW : HIGH);
    }

    digitalWrite(segmentPins[7], decimalPoint ? LOW : HIGH);

    digitalWrite(digitPins[digit], LOW);

    delay(1);
}

void displayTemperature(float tempC) {
  int tempToShow = static_cast<int>(tempC * 10);
  int digits[4] = {0};

  digits[0] = (tempToShow / 1000) % 10;
  digits[1] = (tempToShow / 100) % 10;
  digits[2] = (tempToShow / 10) % 10;
  digits[3] = tempToShow % 10;

  unsigned long endTime = millis() + 3500;
  while (millis() < endTime) {
    for (int i = 1; i <= 2; i++) {
      for (int j = 0; j < 4; j++) {
        digitalWrite(digitPins[j], HIGH);
      }
      displayDigit(i, digits[i], i == 2);
      delay(5);
    }
  }
}

void clearDigits() {
    for (int i = 0; i < 4; i++) {
        digitalWrite(digitPins[i], HIGH);
    }
    for (int segment = 0; segment < 8; segment++) {
        digitalWrite(segmentPins[segment], HIGH);
    }
}

void grantAccess() {
  digitalWrite(RELAY_PIN, LOW); // Assume LOW to turn LED ON for flashing; adjust as per your circuit

  // Simulate engaging relay to control access
  lcdDisplayMessage("Access Granted");
  displayCheckMark();

  unsigned long startTime = millis();
  unsigned long flashDuration = 5000; // 5 seconds duration for servo operation
  bool ledState = false;

  while(millis() - startTime < flashDuration) {
    ledState = !ledState;
    digitalWrite(RELAY_PIN, ledState ? HIGH : LOW); // Toggle LED state for flashing
    delay(250); // Flashing interval; adjust as needed
  }

  digitalWrite(RELAY_PIN, HIGH); // Turn LED off after operation

  gateServo.write(180); // Open gate (0-180 degrees, adjust as needed)
  enterSuccess = true;
  openTime = millis();

  // Keep the servo in the open state for a certain duration if needed
  delay(5000); // Example: Hold the servo open for an additional 5 seconds
  // After operation, proceed with any reset or next steps as required
}

void displayCheckMark() {
  lc.clearDisplay(0);

  byte checkMarkPattern[8] = {
    B00000010,
    B00000100,
    B00001000,
    B00010000,
    B00100000,
    B01000000,
    B01000000,
    B00100000
  };

  for (int i = 0; i < 8; i++) {
    lc.setRow(0, i, checkMarkPattern[i]);
  }
  delay(4000);
  lc.clearDisplay(0);
}

void displayRedX() {
  byte xPattern[8] = {
    B10000001,
    B01000010,
    B00100100,
    B00011000,
    B00011000,
    B00100100,
    B01000010,
    B10000001
  };

  for (int i = 0; i < 8; i++) {
    lc.setRow(0, i, xPattern[i]);
  }
  delay(4000);
  lc.clearDisplay(0);
}

void lcdDisplayMessage(String message) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(message);
}

void rotateStepperMotor(int steps, bool direction) {
  digitalWrite(DIR_PIN, direction);

  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(800);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(800);
  }
}