#include <SPI.h>
#include <MFRC522.h>
#include <Servo.h>
#include <Stepper.h>
#include <avr/wdt.h> // Include watchdog timer library for software reset

#define SS_PIN 9
#define RST_PIN 8
MFRC522 mfrc522(SS_PIN, RST_PIN); // Create MFRC522 instance.

#ifdef ESP8266
  #include <ESP8266WiFi.h>
#elif defined(ESP32)
  #include <WiFi.h>
#endif

byte authorizedUID[] = {0xF6, 0x18, 0x9B, 0xF7};
byte deniedUID[] = {0x21, 0xA9, 0x0D, 0x1E};
bool test = false;

Servo servo1;
Servo servo2;

const int dirPin = 2;           // Stepper motor direction pin
const int stepPin = 3;          // Stepper motor step pin
const int stepsPerRevolution = 200;

const int dcMotorPin1 = 4;      // DC motor pin 1
const int dcMotorPin2 = 5;      // DC motor pin 2

bool motorMoved = false;
bool firstServoMoved = false;
bool secondServoMoved = false;
bool finishedMove = false;
bool final = false;

bool down = false;
const int stepsPerRevolutionWax = 200;
Stepper waxStepper(stepsPerRevolutionWax, 30, 32, 34, 36);
Stepper waxStepper2(stepsPerRevolutionWax, 38, 40, 42, 44);

bool motorRotatedCounterclockwise = false;
bool motorRotatedClockwise = false;

bool motorcount2 = false;
bool motor2 = false;

bool blue = false;
bool pink = false;
bool purple = false;
bool start = false;

// Constants
const unsigned long time_1 = 40000; 
const unsigned long time_2 = 20000;  
const unsigned long time_3 = 20000; 

// Pin assignments
const int heater = 46; // Green
const int motor = 47;  // Blue
const int relay = 48;

bool chick = false;
unsigned long startMillis = 0; // Variable to store the starting time

void setup() 
{
  Serial.begin(9600); // Initiate a serial communication
  SPI.begin(); // Initiate SPI bus
  mfrc522.PCD_Init(); // Initiate MFRC522
  delay(100); // Optional delay to ensure the RFID module initializes properly
  Serial.println("Put your card to the reader...");

  delay(2000);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(heater, OUTPUT);
  pinMode(motor, OUTPUT);
  pinMode(relay, OUTPUT);
  pinMode(dcMotorPin1, OUTPUT);
  pinMode(dcMotorPin2, OUTPUT);
  digitalWrite(relay, HIGH);
  digitalWrite(motor, HIGH);
  digitalWrite(heater, HIGH);
  servo2.attach(7); 
  servo1.attach(6); 

  servo2.write(160);
  servo1.write(30);
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  digitalWrite(A0, HIGH);
  digitalWrite(A1, HIGH);
  digitalWrite(A2, HIGH);
}

void loop() 
{
  Serial.println("Hello from Arduino!");
  String message = "";

  if (Serial.available() > 0) {
    message = Serial.readStringUntil('\n'); // Read the message
    message.trim(); // Remove any leading or trailing whitespace
    Serial.print("Received from ESP32: ");
    Serial.println(message); 

    if (message.equals("BLUE")) {
      blue = true;
      pink = false;
      start = true;
      Serial.println("blue choice");
    }

    else if (message.equals("PINK")) {
      pink = true;
      blue = false;
      start = true;
      Serial.println("pink choice");
    }
     else if (message.equals("PURPLE")) {
      pink = false;
      blue = false;
      start = true;
      purple=true;
      Serial.println("purple choice");
    }
     else {
      Serial.println("Unknown command");
      start = false;
    }
  }

  if (!chick && start) {
    if (!mfrc522.PICC_IsNewCardPresent()) {
      return;
    }

    if (!mfrc522.PICC_ReadCardSerial()) {
      return;
    }

    Serial.print("Card UID: ");
    for (byte i = 0; i < mfrc522.uid.size; i++) {
      Serial.print(mfrc522.uid.uidByte[i] < 0x10 ? " 0" : " ");
      Serial.print(mfrc522.uid.uidByte[i], HEX);
    }
    Serial.println();

    if (compareUID(mfrc522.uid.uidByte, mfrc522.uid.size, authorizedUID, sizeof(authorizedUID))) {
      Serial.println("Authorized access");
      test = true;
      startMillis = millis(); // Reset and start the timer
    } else {
      Serial.println("Unknown card");
      test = false;
    }
    chick = true;
  }

  if (test) {
    unsigned long currentMillis = millis() - startMillis; // Calculate the elapsed time

    delay(2000);

    if (blue) {
      if (!motorRotatedCounterclockwise) {
        Serial.println(".");
        rotateCounterclockwiseWax(40);
        Serial.println("Stepper blue ");
        motorRotatedCounterclockwise = true;
        digitalWrite(A0, LOW);
        digitalWrite(A1, HIGH);
        digitalWrite(A2, HIGH);
      }

      if (motorRotatedCounterclockwise && !motorRotatedClockwise) {
        rotateClockwiseWax(40);
        motorRotatedClockwise = true;
        Serial.println("Stepper blue back");
      }
    }

    if (pink) {
      if (!motorRotatedCounterclockwise) {
        Serial.println(".");
        rotateCounter2(40);
        Serial.println("Stepper pink");
        motorRotatedCounterclockwise = true;
digitalWrite(A0, LOW);
        digitalWrite(A1, HIGH);
        digitalWrite(A2, HIGH);      }

      if (motorRotatedCounterclockwise && !motorRotatedClockwise) {
        rotate2(40);
        motorRotatedClockwise = true;
        Serial.println("Stepper pink back");
      }
    }

    if (purple) {
      if (!motorRotatedCounterclockwise) {
        Serial.println(".");
        rotateCounter2(40);
        rotateCounterclockwiseWax(40);
        Serial.println("Stepper purple");
        motorRotatedCounterclockwise = true;
digitalWrite(A0, LOW);
        digitalWrite(A1, HIGH);
        digitalWrite(A2, HIGH);
              }

      if (motorRotatedCounterclockwise && !motorRotatedClockwise) {
        rotate2(40);
        
        motorRotatedClockwise = true;
        Serial.println("Stepper purple back");
      }
    }
    

    if (motorRotatedCounterclockwise && motorRotatedClockwise) {
      Serial.println("1");
      digitalWrite(A0, LOW);
      digitalWrite(A1, HIGH);
      digitalWrite(A2, HIGH);

      if (currentMillis <= time_1) {
        Serial.println("2");
        digitalWrite(heater, LOW); 
        digitalWrite(motor, HIGH);
        digitalWrite(relay, HIGH);  
        if (!firstServoMoved) {
          Serial.println("3");
          servo1.write(30);
          firstServoMoved = true;
        }
        if (firstServoMoved && !secondServoMoved) {
          servo2.write(160);
          secondServoMoved = true;
        }  
      } 
      else if (currentMillis <= time_1 + time_2) {
        Serial.println("4");
        digitalWrite(dcMotorPin1, HIGH);
        digitalWrite(dcMotorPin2, LOW);
        digitalWrite(heater, LOW); 
        digitalWrite(motor, LOW);  
        digitalWrite(relay, LOW);  
        digitalWrite(A0, LOW);
        digitalWrite(A1, LOW);
        digitalWrite(A2, HIGH);

        if (firstServoMoved && secondServoMoved && !finishedMove) {
          stepperMotorFunction(25);
          Serial.println("5");
          finishedMove = true;
        }
      } 
      else if (currentMillis <= time_1 + time_2 + time_3) {
        Serial.println("6");
        digitalWrite(heater, HIGH); 
        digitalWrite(motor, LOW); 
        digitalWrite(relay, LOW);
        digitalWrite(A0, LOW);
        digitalWrite(A1, LOW);
        digitalWrite(A2, HIGH);

        servo2.write(165);
        servo1.write(28);
        delay(1000);
        servo2.write(170);
        delay(2000);
        servo2.write(175);
        delay(2000);
        servo2.write(180);
        delay(2000);
        servo2.write(170);
        delay(2000);
      }
      else if (!final) {
        digitalWrite(heater, HIGH); 
        digitalWrite(motor, HIGH); 
        digitalWrite(relay, HIGH);  
        digitalWrite(dcMotorPin1, LOW);
        digitalWrite(dcMotorPin2, LOW);
        digitalWrite(A0, LOW);
        digitalWrite(A1, LOW);
        digitalWrite(A2, LOW);
        stepperMotorFunctionR(1);
        delay(2000);
        stepperMotorFunctionR(24);

        final = true;
        chick = false;
        start = false;
        blue = false;
        pink = false;
        test = false;
        
        // Trigger a software reset
        wdt_enable(WDTO_15MS); // Enable watchdog timer to reset the microcontroller
        while (1) {} // Wait for the reset to occur
      }
    }
  } else {
    digitalWrite(A1, LOW); // Turn off relay for unknown card
    digitalWrite(A0, HIGH); // Turn off relay for unknown card
    digitalWrite(A1, HIGH); // Turn off relay for unknown card
    chick = false;
  }
  delay(1000); // Add a delay to avoid spamming the serial monitor
}

bool compareUID(byte *uid, byte uidSize, byte *compareTo, byte compareSize) 
{
  if (uidSize != compareSize) 
  {
    return false;
  }
  for (byte i = 0; i < uidSize; i++) 
  {
    if (uid[i] != compareTo[i]) 
    {
      return false;
    }
  }
  return true;
}

void stepperMotorFunction(int x) {
  int desiredAngle = 600; 
  int stepsToDesiredAngle = abs((stepsPerRevolution * desiredAngle) / 360);
  stepsToDesiredAngle = stepsToDesiredAngle * x;
  Serial.println(stepsToDesiredAngle); 

  digitalWrite(dirPin, LOW);
  for (int i = 0; i < stepsToDesiredAngle; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(2000);
  }

  delay(1000);
  digitalWrite(stepPin, LOW);
}

void stepperMotorFunctionR(int x) {
  int desiredAngle = 600; 
  int stepsToDesiredAngle = abs((stepsPerRevolution * desiredAngle) / 360);
  stepsToDesiredAngle = stepsToDesiredAngle * x;
  Serial.println(stepsToDesiredAngle); 

  digitalWrite(dirPin, HIGH);
  for (int i = 0; i < stepsToDesiredAngle; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(2000);
  }

  delay(1000);
  digitalWrite(stepPin, LOW);
}

// Stepper 1
void rotateClockwiseWax(int degrees) {
  int motorSpeedWax = 50;
  waxStepper.setSpeed(motorSpeedWax);
  waxStepper.step(stepsPerRevolutionWax * degrees / 360);
}

void rotateCounterclockwiseWax(int degrees) {
  int motorSpeedWax = 50;
  waxStepper.setSpeed(motorSpeedWax);
  waxStepper.step(-stepsPerRevolutionWax * degrees / 360);
}

// Stepper 2
void rotate2(int degrees) {
  int motorSpeedWax = 50;
  waxStepper2.setSpeed(motorSpeedWax);
  waxStepper2.step(stepsPerRevolutionWax * degrees / 360);
}

void rotateCounter2(int degrees) {
  int motorSpeedWax = 50;
  waxStepper2.setSpeed(motorSpeedWax);
  waxStepper2.step(-stepsPerRevolutionWax * degrees / 360);