#include <AccelStepper.h>
#include <Servo.h>

const int irSensorPin = A0;
const int servoPinX = 9;
Servo servoX;
const int stepPinY = 8;
const int dirPinY = 7;
AccelStepper stepperY(AccelStepper::FULL4WIRE, stepPinY, dirPinY);
float distance_to_center = 8;

void setup() {
  servoX.attach(servoPinX);
  servoX.write(0);
  stepperY.setMaxSpeed(1000);
  stepperY.setAcceleration(500);
  
  Serial.begin(9600);
}

void loop() {
  for (int angle = 0; angle <= 90; angle++) {
    servoX.write(angle);
    delay(10);
    getDistance();
  }
  
  // Move stepper 144 steps
  stepperY.move(144);
  stepperY.runToPosition();
  
  // Adjust Y position by 5 degrees
  int newYPosition = stepperY.currentPosition() - 139;
  stepperY.moveTo(newYPosition);
  stepperY.runToPosition();
  
  getDistance();
}

void getDistance() {
  int irValue = analogRead(irSensorPin);
  float voltage = irValue * (3.3 / 1023.0);
  float distance = -5.40274 * pow(voltage, 3) + 28.4823 * pow(voltage, 2) - 49.7115 * voltage + 31.3444;
  distance = distance_to_center - distance;
  
  Serial.print("X: ");
  Serial.print(servoX.read());
  Serial.print(" Y: ");
  Serial.print(stepperY.currentPosition());
  Serial.print(" Z: ");
  Serial.println(distance);
  
  delay(50);
}