#define MotorX_dirPin  22
#define MotorX_stepPin 23
#define MotorY_dirPin  19
#define MotorY_stepPin 21
#define MotorZ_dirPin  5
#define MotorZ_stepPin 18
#define gearRatio 1.8
#define fimCurso_X 17
#define fimCurso_Y 16
#define fimCurso_Z 4

#define btnReset 13

void funcao_ISR_Reset();
void funcao_ISR_Pause();
// Define the pins connected to the A4988 driver
//const int MotorZ_dirPin = 22;  // Direction pin
//const int MotorZ_stepPin = 23; // Step pin

// Define motor parameters
const int stepsPerRevolution = 200; // Steps per revolution of your motor
const int atraso = 100; // Adjust for desired speed
const int numRevolutions = 2; // Number of revolutions

float angulo_X,angulo_Y, angulo_Z;
uint16_t passoX,passoY,passoZ;

void setup() {
  config();
  attachInterrupt(btnReset, funcao_ISR_Reset, FALLING);
  IRAM_ATTR funcao_ISR_Reset();
  attachInterrupt(fimCurso_X, funcao_ISR_Pause, LOW);
  //attachInterrupt(fimCurso_Y, funcao_ISR, RISING);
  //attachInterrupt(fimCurso_Z, funcao_ISR, RISING);
  Serial.begin(115200); // Initialize serial communication for debugging
  angulo_X = 90.0;
  angulo_Y = 60.0;
  angulo_Z = 90.0;
  passoX = int( angulo_X * stepsPerRevolution * 16 / 360.0 );
  passoY = int( angulo_Y * stepsPerRevolution * 16 / 360.0 );
  passoZ = int( angulo_Z * stepsPerRevolution * 16 / 360.0 );
}

void loop() {
  //passoX = 800;
  Serial.print("PassoX: ");
  Serial.println(passoX);
  // Rotate the motor clockwise
  digitalWrite(MotorX_dirPin, HIGH); // Set direction to clockwise
  Serial.println("Rotating clockwise...");
  rotateStepMotor(MotorX_stepPin, passoX);  
  //rotateMotor(numRevolutions);
  delay(1000); // Delay after rotation
  // Rotate the motor counterclockwise
  digitalWrite(MotorX_dirPin, LOW);  // Set direction to counterclockwise
  Serial.println("Rotating counterclockwise...");
  rotateStepMotor(MotorX_stepPin, passoX);
  //rotateMotor(numRevolutions);
  delay(1000); // Delay after rotation
}

/* Função ISR (chamada quando há geração da interrupção) */
void IRAM_ATTR funcao_ISR_Reset() {
  angulo_X =0;
  angulo_Y =0;
  angulo_Z =0;
}

/* Função ISR (chamada quando há geração da interrupção) */
void IRAM_ATTR funcao_ISR_Pause() {
  while ( !digitalRead(fimCurso_X) || !digitalRead(fimCurso_Y) || !digitalRead(fimCurso_Z)) {
    delayMicroseconds(10);
  }
}

void config(){
  // Set the stepper motor pins as outputs
  pinMode(MotorX_stepPin, OUTPUT);
  pinMode(MotorX_dirPin, OUTPUT);
  pinMode(MotorY_stepPin, OUTPUT);
  pinMode(MotorY_dirPin, OUTPUT);
  pinMode(MotorZ_stepPin, OUTPUT);
  pinMode(MotorZ_dirPin, OUTPUT);
  pinMode(fimCurso_X, INPUT_PULLUP);
  pinMode(fimCurso_Y, INPUT_PULLUP);
  pinMode(fimCurso_Z, INPUT_PULLUP);
  pinMode( btnReset, INPUT_PULLUP);
}

// Function to rotate the motor for a specified number of revolutions
void rotateStepMotor(uint8_t pin,int numRotations ) {
    for (uint16_t i = 0; i < numRotations; i++) {
        digitalWrite(pin, HIGH);
        delayMicroseconds(atraso);
        digitalWrite(pin, LOW);
        delayMicroseconds(atraso);
    }
}

// Function to rotate the motor for a specified number of revolutions
void rotateMotor(int numRotations) {
    for (int i = 0; i < numRotations * stepsPerRevolution; i++) {
        digitalWrite(MotorZ_stepPin, HIGH);
        delayMicroseconds(atraso);
        digitalWrite(MotorZ_stepPin, LOW);
        delayMicroseconds(atraso);
    }
}