#include <AccelStepper.h>
#include <Servo.h>
#include <Toggle.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>

#define SCREEN_I2C_ADDR 0x3C // or 0x3C
#define SCREEN_WIDTH 128     // OLED display width, in pixels
#define SCREEN_HEIGHT 64     // OLED display height, in pixels
#define OLED_RST_PIN -1      // Reset pin (-1 if not available)
#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64 // OLED height, in pixels

const byte limitSwitch1Pin = A3;
const byte limitSwitch2Pin = 9;
const byte limitSwitch3Pin = 10;
const byte limitSwitch4Pin = 11;

Toggle limitSwitch1;
Toggle limitSwitch2;
Toggle limitSwitch3;
Toggle limitSwitch4;

AccelStepper stepper1(AccelStepper::DRIVER, 12, 13);
AccelStepper stepper2(AccelStepper::DRIVER, 4, 7);
AccelStepper stepper3(AccelStepper::DRIVER, 3, 6);
AccelStepper stepper4(AccelStepper::DRIVER, 2, 5);


// create an OLED display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// OLED Animation: walk
// Code auto-generated by https://wokwi.com/animator, graphics by icons8.com

void setup() 
{


  // initialize OLED display with I2C address 0x3C
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
  {
    oled.print(F("failed to start SSD1306 OLED"));
    while (1);
  }

  Serial.begin(9600);
  limitSwitch1.begin(limitSwitch1Pin);
  limitSwitch2.begin(limitSwitch2Pin);
  limitSwitch3.begin(limitSwitch3Pin);
  limitSwitch4.begin(limitSwitch4Pin);

  stepper4.setMaxSpeed(1000);
  stepper4.setAcceleration(200);
  stepper3.setMaxSpeed(1000);
  stepper3.setAcceleration(200);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(200);
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(200);
  homing();
}



//Homing [Z]
void homing () {
  oled.print("Homing [Z]");
  stepper1.setSpeed(-1000);
  while (true) { // run CW at constant speed until we hit the button
    stepper1.runSpeed();
    limitSwitch1.poll();
    if (limitSwitch1.onPress()) break;
  }
  stepper1.setCurrentPosition(0);
  Serial.println("moving off limit switch");
  
  stepper1.moveTo(3800);
  while (stepper1.currentPosition() != 3800) {
    stepper1.run();
  }
  Serial.println(stepper1.currentPosition());
  delay (1000);
  Serial.println("Limit switch free. Setting position to 0");
  stepper1.setCurrentPosition(0);
  Serial.println(stepper1.currentPosition());

//Homing [A]
  
  Serial.println("Homing [A]");
  stepper2.setSpeed(-8);
  while (true) { // run CW at constant speed until we hit the button
    stepper2.runSpeed();
    limitSwitch2.poll();
    if (limitSwitch2.onPress()) break;
  }
  stepper2.setCurrentPosition(0);
  Serial.println("moving off limit switch");
  
  stepper2.moveTo(300);
  while (stepper2.currentPosition() != 300) {
    stepper2.run();
  }
  Serial.println(stepper3.currentPosition());
  delay (1000);
  Serial.println("Limit switch free. Setting position to 0");
  stepper2.setCurrentPosition(0);
  Serial.println(stepper2.currentPosition());

//Homing [X]
  
  Serial.println("Homing [X]");
  stepper3.setSpeed(-800);
  while (true) { // run CW at constant speed until we hit the button
    stepper3.runSpeed();
    limitSwitch3.poll();
    if (limitSwitch3.onPress()) break;
  }
  stepper3.setCurrentPosition(0);
  Serial.println("moving off limit switch");
  
  stepper3.moveTo(2000);
  while (stepper3.currentPosition() != 2000) {
    stepper3.run();
  }
  Serial.println(stepper3.currentPosition());
  delay (1000);
  Serial.println("Limit switch free. Setting position to 0");
  stepper3.setCurrentPosition(0);
  Serial.println(stepper3.currentPosition());
}


void loop() {}