#include <Arduino.h>

// ============================
// PINES
// ============================

// Eje X Motor 1 Driver 8825
#define X_STEP 3
#define X_DIR  2
#define X_EN   4
#define X_ENDSTOP 13

// Eje Y Motor 2 Driver 8825
#define Y_STEP 1
#define Y_DIR  0
#define Y_EN   15
#define Y_ENDSTOP 14

// Eje Z Motor 3 Driver 8825
#define Z_STEP 9
#define Z_DIR  8
#define Z_EN   10
#define Z_ENDSTOP 16

// ============================
// PARÁMETROS
// ============================

#define PASOS_MM 800.0 //800 pasos son equivalente a 1mm de recorrido de la herramienta, es decir 
                      //si hay 1600 pasos la herramienta o el motor del eje X se movera 2mm de distancia


// ============================
// SOFT LIMITS (mm)
// ============================

/*Establcemos los limite de movimientos seguro de la maquina
estos valores dependeran del tamaño fisico de la maquina que estan fabricando mis compañeros*/
#define X_MIN 0 
#define X_MAX 200 

#define Y_MIN 0
#define Y_MAX 150

#define Z_MIN 0
#define Z_MAX 50

// ============================
// ESTADO GLOBAL
// ============================

volatile long posX = 0; //posicion actual del eje x en pasos
volatile long posY = 0; //posicion actual del eje y en pasos
volatile long posZ = 0; //posicion actual del eje z en pasos

volatile long dx, dy, dz; //posicion a la que se movera la herramienta en la maquina
volatile long stepsTotal; //duraccion total del movimiento

volatile long errX, errY, errZ;
volatile long stepsDone = 0;

volatile bool running = false;
volatile bool softLimitHit = false; //verificca los limites

volatile int velMinUs = 2000;
volatile int velMaxUs = 400;

repeating_timer_t timer;

// ============================
// STEP
// ============================

inline void stepPulse(int pin) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(2);
  digitalWrite(pin, LOW);
}

// ============================
// VALIDAR LIMITES
// ============================

bool dentroLimites(float x, float y, float z) {

  if (x < X_MIN || x > X_MAX) return false;
  if (y < Y_MIN || y > Y_MAX) return false;
  if (z < Z_MIN || z > Z_MAX) return false;

  return true;
}

// ============================
// ISR
// ============================

bool stepperISR(repeating_timer_t *t) {

  if (!running) return true;

  if (stepsDone >= stepsTotal) {
    running = false;
    return true;
  }

  float progress = (float)stepsDone / stepsTotal;
  float factor = progress * progress * (3 - 2 * progress);
  int vel = velMinUs - (velMinUs - velMaxUs) * factor;

  errX += dx;
  errY += dy;
  errZ += dz;

  // X
  if (errX >= stepsTotal) {
    stepPulse(X_STEP);
    posX += digitalRead(X_DIR) ? 1 : -1;
    errX -= stepsTotal;
  }

  // Y
  if (errY >= stepsTotal) {
    stepPulse(Y_STEP);
    posY += digitalRead(Y_DIR) ? 1 : -1;
    errY -= stepsTotal;
  }

  // Z
  if (errZ >= stepsTotal) {
    stepPulse(Z_STEP);
    posZ += digitalRead(Z_DIR) ? 1 : -1;
    errZ -= stepsTotal;
  }

  // ============================
  // SOFT LIMITS EN TIEMPO REAL
  // ============================

  float x = posX / PASOS_MM;
  float y = posY / PASOS_MM;
  float z = posZ / PASOS_MM;

  if (x < X_MIN || x > X_MAX ||
      y < Y_MIN || y > Y_MAX ||
      z < Z_MIN || z > Z_MAX) {

    running = false;
    softLimitHit = true;
    return true;
  }

  stepsDone++;

  cancel_repeating_timer(&timer);
  add_repeating_timer_us(-vel, stepperISR, NULL, &timer);

  return true;
}

// ============================
// MOVER XYZ (pasos)
// ============================

void moverXYZ(long x, long y, long z) {

  dx = abs(x);
  dy = abs(y);
  dz = abs(z);

  digitalWrite(X_DIR, (x > 0));
  digitalWrite(Y_DIR, (y > 0));
  digitalWrite(Z_DIR, (z > 0));

  delayMicroseconds(200);

  stepsTotal = max(dx, max(dy, dz));
//acumlaciones de errores
  errX = errY = errZ = 0;
  stepsDone = 0;

  running = true;

  add_repeating_timer_us(-velMinUs, stepperISR, NULL, &timer);
}

// ============================
// MOVER EN mm (con límites)
// ============================

void moverXYZmm(float x, float y, float z) {

  float actualX = posX / PASOS_MM;
  float actualY = posY / PASOS_MM;
  float actualZ = posZ / PASOS_MM;

  float newX = actualX + x;
  float newY = actualY + y;
  float newZ = actualZ + z;

  if (!dentroLimites(newX, newY, newZ)) {
    Serial.println("ERROR: Movimiento fuera de limites");
    return;
  }

  moverXYZ(x * PASOS_MM, y * PASOS_MM, z * PASOS_MM);
}

// ============================
// POSICIÓN
// ============================

void imprimirPosicion() {
  Serial.print("X: ");
  Serial.print(posX / PASOS_MM);
  Serial.print("  Y: ");
  Serial.print(posY / PASOS_MM);
  Serial.print("  Z: ");
  Serial.println(posZ / PASOS_MM);
}

// ============================
// HOMING
// ============================

void homingEje(int stepPin, int dirPin, int endstopPin, volatile long &pos) {

  pinMode(endstopPin, INPUT_PULLUP);

  digitalWrite(dirPin, LOW);
  delayMicroseconds(200);

  while (digitalRead(endstopPin) == HIGH) {
    stepPulse(stepPin);
    delayMicroseconds(1500);
  }

  pos = 0;
}

void homingXYZ() {

  Serial.println("Homing X...");
  homingEje(X_STEP, X_DIR, X_ENDSTOP, posX);

  Serial.println("Homing Y...");
  homingEje(Y_STEP, Y_DIR, Y_ENDSTOP, posY);

  Serial.println("Homing Z...");
  homingEje(Z_STEP, Z_DIR, Z_ENDSTOP, posZ);

  Serial.println("Homing completo");
}

// ============================
// SETUP
// ============================

void setup() {

  pinMode(X_STEP, OUTPUT);
  pinMode(X_DIR, OUTPUT);
  pinMode(X_EN, OUTPUT);

  pinMode(Y_STEP, OUTPUT);
  pinMode(Y_DIR, OUTPUT);
  pinMode(Y_EN, OUTPUT);

  pinMode(Z_STEP, OUTPUT);
  pinMode(Z_DIR, OUTPUT);
  pinMode(Z_EN, OUTPUT);

  digitalWrite(X_EN, LOW);
  digitalWrite(Y_EN, LOW);
  digitalWrite(Z_EN, LOW);

  Serial.begin(115200);

  homingXYZ();  //busca el origen de la maquina o sea x=0, y=0, z=0
}

// ============================
// LOOP
// ============================

void loop() {

  // Aviso de error fuera del ISR
  if (softLimitHit) { 
    Serial.println("ERROR: Soft limit alcanzado");
    softLimitHit = false;
  }

  if (!running) {

    Serial.println("Movimiento");

    moverXYZmm(10, 5, 2); // la maquina debe moverse a este punto

    while(running);

    imprimirPosicion();

    delay(3000);
  }
}