// Stepper Motor Control Pins
const int stepPin = 7;
const int dirPin = 6;

// Proximity Switch Pin
const int proxSwitchPin = A0;

// Remote Switch Pin
const int remoteSwitchPin = A1;

// Stepper Motor Parameters
const int stepsPerRevolution = 200;
const float targetRPM = 10.0; // Target speed

// Desired Angles and Steps
const int ninetyDegreesSteps = (90.0 / 360.0) * stepsPerRevolution; // 50
const int ninetyFiveDegreesSteps = (95.0 / 360.0) * stepsPerRevolution; // ~52.78, use 53

// State Variable
bool isMoving = false;
bool stage2 = 0;

void setup() {
  // Initialize Output Pins
  Serial.begin(115200);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  // Initialize Input Pins
  pinMode(proxSwitchPin, INPUT_PULLUP); // Enable internal pull-up for proximity switch
  pinMode(remoteSwitchPin, INPUT_PULLUP); // Enable internal pull-up for remote switch

  // Set initial direction (optional - adjust as needed)
  digitalWrite(dirPin, HIGH);
  Serial.println("System Ready");

}

void loop() {
  
  // Check for Remote Switch Press (Active Low) and Motor Not Moving
  if (digitalRead(remoteSwitchPin) == LOW && !isMoving) {
    isMoving = true; // Set the moving flag
    Serial.println("Stage 1: 3*90 at 10 RPM");
    // Perform Three 90-Degree Turns
    for (int i = 0; i < 3; i++) {
      moveStepper(ninetyDegreesSteps, targetRPM);
      delay(1000); // Small delay between turns
      Serial.print("90 degree ");Serial.println(i);
    }
    Serial.println("Moving to 95 degree");
    // Perform 95-Degree Turn (Stop at 90 with Proximity Switch)
    digitalWrite(dirPin, HIGH); // Or LOW, depending on your setup
    for (int i = 0; i < ninetyFiveDegreesSteps; i++) {
      if(stage2){
        for(int j = 0;j<3;j++){
          digitalWrite(stepPin, HIGH);
          delayMicroseconds(calculateStepPulseWidth(targetRPM));
          digitalWrite(stepPin, LOW);
          delayMicroseconds(calculateStepPulseWidth(targetRPM));
        }
        Serial.println("System Ready");
        break;
      }
      else{
        // Generate one step pulse
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(calculateStepPulseWidth(targetRPM));
        digitalWrite(stepPin, LOW);
        delayMicroseconds(calculateStepPulseWidth(targetRPM));
      }
      // Check Proximity Switch
      if (digitalRead(proxSwitchPin) == LOW) { // Proximity switch is active
        stage2 = 1;
        Serial.println("Induction switch detected");
        Serial.println("Moving another 5 degree");
        delay(1000);
      }
    }
    stage2 = 0;
    // Delay before allowing next sequence
    delay(3000);
    isMoving = false; // Reset the moving flag
  }
}

// Function to Move Stepper Motor
void moveStepper(int steps, float rpm) {
  digitalWrite(dirPin, HIGH); // Or LOW, depending on desired direction
  for (int i = 0; i < steps; i++) {
    // Generate one step pulse
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(calculateStepPulseWidth(rpm));
    digitalWrite(stepPin, LOW);
    delayMicroseconds(calculateStepPulseWidth(rpm));
  }
}

// Function to Calculate Step Pulse Width (in microseconds)
unsigned long calculateStepPulseWidth(float rpm) {
  return (unsigned long)(60000000.0 / (stepsPerRevolution * rpm));
}