#include <AccelStepper.h>

#define dir_val   22   // Direção do motor_val
#define step_val  23   // Passo do motor_val
#define pos_asp   36   // Sensor posição aspiração
#define pos_inj   34  // Sensor posição injeção

#define dir_ser   2  // Direção do motor_ser
#define step_ser  0  // Passo do motor_ser
#define encoder   32  // Encoder de um canal
#define pos_home  33  // Sensor posição home

#define pump_on   25  // Botão de ligar/desligar
#define trigger   26  // Botão de ciclo


AccelStepper motor_val(AccelStepper::DRIVER, step_val, dir_val);
AccelStepper motor_ser(AccelStepper::DRIVER, step_ser, dir_ser);

volatile int encoder_count = 0;
bool bomba_ligada = false;

void IRAM_ATTR encoderISR() {
  encoder_count++;
}

void setup() {
  Serial.begin(115200);
  
  pinMode(pos_asp, INPUT_PULLUP);
  pinMode(pos_inj, INPUT_PULLUP);
  pinMode(pos_home, INPUT_PULLUP);
  pinMode(pump_on, INPUT_PULLUP);
  pinMode(trigger, INPUT_PULLUP);
  pinMode(encoder, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoder), encoderISR, RISING);

  motor_val.setMaxSpeed(1000);
  motor_val.setAcceleration(500);
  motor_ser.setMaxSpeed(1000);
  motor_ser.setAcceleration(500);

  Serial.println("Sistema iniciado...");
}

void loop() {
  if (digitalRead(pump_on) == LOW) {
    bomba_ligada = !bomba_ligada;
    Serial.print("Bomba ");
    Serial.println(bomba_ligada ? "LIGADA" : "DESLIGADA");
    delay(500);
  }

  if (bomba_ligada) {
    posicaoHome();
  }
}

void moverAteSensor(AccelStepper &motor, int sensor, int sentido, const char* nome) {
  Serial.print("Movendo ");
  Serial.print(nome);
  Serial.println(" até acionar sensor...");
  
  motor.setSpeed(sentido * 500);
  while (digitalRead(sensor) == HIGH) {
    motor.runSpeed();
  }
  motor.stop();
  Serial.print(nome);
  Serial.println(" atingiu o sensor!");
}

void moverEncoder(AccelStepper &motor, int passos, int sentido) {
  encoder_count = 0;
  Serial.print("Movendo motor_ser até ");
  Serial.print(passos);
  Serial.println(" pulsos do encoder...");

  motor.setSpeed(sentido * 500);
  while (encoder_count < passos) {
    motor.runSpeed();
  }
  motor.stop();
  
  Serial.println("Motor_ser atingiu 5 pulsos do encoder!");
}

void posicaoHome() {
  Serial.println("Iniciando posição Home...");

  moverAteSensor(motor_val, pos_inj, 1, "motor_val");
  moverAteSensor(motor_ser, pos_home, 1, "motor_ser");
  moverAteSensor(motor_val, pos_asp, -1, "motor_val");
  moverEncoder(motor_ser, 5, -1);
  moverAteSensor(motor_val, pos_inj, 1, "motor_val");

  Serial.println("Ciclo Home finalizado!");
}