#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 STEPPER_PIN1 11
#define STEPPER_PIN2 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 displayTemperature(float tempC);
void displayDigit(int digit, int number, bool showDecimal = false);


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);
  stepper.setSpeed(15);
  lc.shutdown(0, false);
  lc.setIntensity(0, 8);
  lc.clearDisplay(0);
  pinMode(ULTRASONIC_TRIG, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);

  // 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 (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) {
      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();
}

void scanRFID() {
  lcdDisplayMessage("SCAN RFID");
  delay(2000); // Simulate scan time for demonstration
  // Simulate successful RFID scan
  rfidScanned = true;
  displayCheckMark();
  lcdDisplayMessage("Scan wrist for temperature check. Hold until Check");
}

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

  if (!isnan(tempC)) {
    if (tempC >= 34.0 && tempC < 38.0) { // Valid temperature range
      displayTemperature(tempC); // Display temperature
      temperatureChecked = true; // Indicate temperature has been checked
    } else if (tempC >= 38.0) { // Temperature too high
      lcdDisplayMessage("Temp too high. Go home!");
      displayRedX();
      delay(4000); // Display message for 4 seconds before resetting
      resetSystem();
    } else { // If temperature is below 34.0, consider it as checked but don't display
      temperatureChecked = true; // Still set this to true to allow for flow continuation
    }
  }
}




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

// Function to display a number on a specific digit with optional decimal point
void displayDigit(int digit, int number, bool decimalPoint = false) {
    // Assuming digitPins and segmentPins are correctly defined and set up
    // Turn off all digits
    for (int i = 0; i < 4; i++) {
        digitalWrite(digitPins[i], HIGH);
    }

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

    // Handle the decimal point
    digitalWrite(segmentPins[7], decimalPoint ? LOW : HIGH);

    // Activate the current digit
    digitalWrite(digitPins[digit], LOW);

    delay(1); // A minimal delay to ensure the digit is lit
}


void displayTemperature(float tempC) {
  int tempToShow = static_cast<int>(tempC * 10); // Convert to an integer representation (e.g., 34.7°C becomes 347)
  int digits[4] = {0}; // Array to hold each digit for display

  // Extract individual digits for display
  digits[0] = (tempToShow / 1000) % 10; // Thousands place, should be 0 for our range
  digits[1] = (tempToShow / 100) % 10; // Hundreds place, the tens in our temperature
  digits[2] = (tempToShow / 10) % 10; // Tens place, the ones in our temperature
  digits[3] = tempToShow % 10; // Ones place, the decimal fraction in our temperature

  unsigned long endTime = millis() + 3500; // Set end time for displaying temperature
  while (millis() < endTime) {
    for (int i = 1; i <= 2; i++) { // Loop through tens and ones place for temperature
      for (int j = 0; j < 4; j++) { // Ensure all digits are off by default
        digitalWrite(digitPins[j], HIGH); // Turn off digit (for common cathode)
      }
      displayDigit(i, digits[i], i == 2); // Enable decimal point for the ones place
      delay(5); // Delay to ensure digit is visible
    }
  }
}


void clearDigits() {
    // Assuming common cathode; for common anode, use LOW instead of HIGH
    for (int i = 0; i < 4; i++) {
        digitalWrite(digitPins[i], HIGH); // Turns off the digit
    }
    for (int segment = 0; segment < 8; segment++) { // Includes decimal point
        digitalWrite(segmentPins[segment], HIGH); // Turns off all segments
    }
}


void grantAccess() {
  digitalWrite(RELAY_PIN, HIGH); // Simulate engaging relay to control access
  lcdDisplayMessage("Success! Enter");
  gateServo.write(-90); // Simulate opening gate
  enterSuccess = true;
  openTime = millis();
}

void displayCheckMark() {
  // Placeholder for checkmark display logic
}

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); // Display for 2 seconds
  lc.clearDisplay(0);
}

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

void rotateStepperMotor() {
  stepper.step(2048); // Rotate 180 degrees
}