/**
 * DRV8411A + 2x DC + Servo + Lectura de corriente por IPROPI (ESP32-S3).
 * Muestra en Serial Plotter: IA(A) \t IB(A) \t ITRIP(A) mientras ejecuta secuencias de ambos motores y un servo.
 * Fórmulas IPROPI (datasheet): IPROPI(µA) = I_LS_total(A) * 200 µA/A; V_IPROPI = IPROPI * R_IPROPI;
 *                              I_motor = V_IPROPI / (R_IPROPI * 200e-6); I_TRIP = VREF / (R_IPROPI * 200e-6).
 */
/* ───────── KODE | docs.kode.diy ───────── */

#include <Arduino.h>
#include <ESP32Servo.h>

/* Pines DRV8411A */
constexpr int PIN_AIN1   = 42;
constexpr int PIN_AIN2   = 41;
constexpr int PIN_BIN1   = 39;
constexpr int PIN_BIN2   = 40;
constexpr int PIN_nFAULT = 3;

/* IPROPI -> ADC (canal A y canal B) */
constexpr int PIN_AIPROPI = 2;   /* Entrada ADC para A-IPROPI */
constexpr int PIN_BIPROPI = 1;   /* Entrada ADC para B-IPROPI */

/* Servo */
constexpr int PIN_SERVO = 13;
Servo servo;

/* Configuración del servo (microsegundos y ángulos) */
constexpr int SERVO_MIN_US = 500;
constexpr int SERVO_MAX_US = 2500;
constexpr int SERVO_CENTER = 90;
constexpr int SERVO_LEFT   = 0;
constexpr int SERVO_RIGHT  = 180;

/* Funciones auxiliares DRV8411A */
inline void motorA_coast()  { digitalWrite(PIN_AIN1, LOW);  digitalWrite(PIN_AIN2, LOW);  } /* Alta impedancia en ambas entradas */
inline void motorA_brake()  { digitalWrite(PIN_AIN1, HIGH); digitalWrite(PIN_AIN2, HIGH); } /* Freno por decaimiento rápido */
inline void motorA_fwd()    { digitalWrite(PIN_AIN1, HIGH); digitalWrite(PIN_AIN2, LOW);  } /* Motor A adelante */
inline void motorA_rev()    { digitalWrite(PIN_AIN1, LOW);  digitalWrite(PIN_AIN2, HIGH); } /* Motor A atrás */

inline void motorB_coast()  { digitalWrite(PIN_BIN1, LOW);  digitalWrite(PIN_BIN2, LOW);  } /* Alta impedancia en ambas entradas */
inline void motorB_brake()  { digitalWrite(PIN_BIN1, HIGH); digitalWrite(PIN_BIN2, HIGH); } /* Freno por decaimiento rápido */
inline void motorB_fwd()    { digitalWrite(PIN_BIN1, HIGH); digitalWrite(PIN_BIN2, LOW);  } /* Motor B adelante */
inline void motorB_rev()    { digitalWrite(PIN_BIN1, LOW);  digitalWrite(PIN_BIN2, HIGH); } /* Motor B atrás */

/* Tiempos */
constexpr uint32_t T_MOTOR_DIR   = 2000; /* 2 s por sentido */
constexpr uint32_t T_SERVO_TRAMO = 1000; /* 1 s por tramo */

/* Parámetros IPROPI (ajustar R al valor real usado) */
constexpr float VREF_V         = 3.3f;       /* VREF (V) */
constexpr float RIPROPI_OHMS   = 23700.0f;   /* Cambiar a 10000.0f si se usa 10 kΩ */
constexpr float AIPROPI_GAIN   = 200e-6f;    /* 200 µA/A (ganancia IPROPI) */

/* Configuración del ADC */
constexpr int   ADC_BITS       = 12;         /* Resolución ADC */
constexpr float ADC_VFULL      = 3.3f;       /* Con atenuación 11 dB, ~3.3 V de escala completa */

/* Máquinas de estado concurrentes */
enum class Phase { FWD, REV, DONE };
struct MotorSeq {
  bool enabled=false; Phase phase=Phase::FWD; uint32_t tEnd=0;
  void start() { enabled=true; phase=Phase::FWD; tEnd=millis()+T_MOTOR_DIR; }
};
struct ServoSeq {
  bool enabled=false; int step=0; uint32_t tEnd=0;
  void start() { enabled=true; step=0; servo.write(SERVO_RIGHT); tEnd=millis()+T_SERVO_TRAMO; }
};
MotorSeq seqA, seqB; ServoSeq seqS;

/* ADC -> voltios */
float adcVolts(int pin) {
  uint16_t raw = analogRead(pin);
  return (raw * ADC_VFULL) / ((1 << ADC_BITS) - 1);
}

/* Voltios IPROPI -> corriente del motor (A) */
float ipropiToCurrentA(float v_ipropi) {
  return v_ipropi / (RIPROPI_OHMS * AIPROPI_GAIN);
}

/* Muestreo periódico para Serial Plotter: IA, IB e ITRIP */
void sampleAndPrintCurrents() {
  float vA = adcVolts(PIN_AIPROPI);
  float vB = adcVolts(PIN_BIPROPI);
  float iA = ipropiToCurrentA(vA);
  float iB = ipropiToCurrentA(vB);
  float iTRIP = VREF_V / (RIPROPI_OHMS * AIPROPI_GAIN); /* Umbral de sobrecorriente (A) */

  /* Columnas separadas por tabuladores para Arduino Serial Plotter */
  Serial.print(iA, 4); Serial.print('\t');
  Serial.print(iB, 4); Serial.print('\t');
  Serial.println(iTRIP, 4);
}

/* Espera activa con muestreo periódico (~cada 20 ms) */
void waitWithSampling(uint32_t ms) {
  uint32_t t0 = millis(), tNext = 0;
  while ((uint32_t)(millis() - t0) < ms) {
    uint32_t now = millis();
    if ((int32_t)(now - tNext) >= 0) {
      sampleAndPrintCurrents();
      tNext = now + 20;
    }
    delay(1); /* Breve espera para ceder CPU */
  }
}

/* Verifica si /nFAULT está en bajo durante la sección indicada */
void checkFault(const char* tag) {
  if (digitalRead(PIN_nFAULT) == LOW) {
    Serial.print("[nFAULT] Falla detectada durante ");
    Serial.println(tag);
  }
}

void setup() {
  Serial.begin(115200);
  delay(100);

  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_BIN1, OUTPUT);
  pinMode(PIN_BIN2, OUTPUT);
  pinMode(PIN_nFAULT, INPUT_PULLUP);

  motorA_coast();
  motorB_coast();

  /* ADC: 11 dB para abarcar hasta ~3.3 V y resolución de 12 bits */
  analogReadResolution(ADC_BITS);
  analogSetAttenuation(ADC_11db);

  /* Cabecera opcional para Serial Plotter */
  Serial.println("IA(A)\tIB(A)\tITRIP(A)");

  /* Configuración del servo (50 Hz, limitado a los pulsos definidos) */
  servo.setPeriodHertz(50);
  servo.attach(PIN_SERVO, SERVO_MIN_US, SERVO_MAX_US);
  servo.write(SERVO_CENTER);

  /* 1) Motor A adelante y atrás con muestreo */
  motorA_fwd();  waitWithSampling(T_MOTOR_DIR);
  motorA_rev();  waitWithSampling(T_MOTOR_DIR);
  motorA_brake(); delay(100); motorA_coast();
  checkFault("Secuencia 1 (Motor A)");

  /* 2) Motor B adelante y atrás con muestreo */
  motorB_fwd();  waitWithSampling(T_MOTOR_DIR);
  motorB_rev();  waitWithSampling(T_MOTOR_DIR);
  motorB_brake(); delay(100); motorB_coast();
  checkFault("Secuencia 2 (Motor B)");

  /* 3) Barrido del servo: +90°, -90° (1 s cada tramo) */
  servo.write(SERVO_RIGHT); waitWithSampling(T_SERVO_TRAMO);
  servo.write(SERVO_LEFT);  waitWithSampling(T_SERVO_TRAMO);
  servo.write(SERVO_CENTER);

  /* 4) Ejecución concurrente */
  seqA.start(); seqB.start(); seqS.start();
}

void loop() {
  /* Muestreo continuo para el Plotter (~50 Hz) */
  static uint32_t tNext = 0;
  uint32_t now = millis();
  if ((int32_t)(now - tNext) >= 0) {
    sampleAndPrintCurrents();
    tNext = now + 20;
  }

  /* Secuencia concurrente Motor A */
  if (seqA.enabled) {
    if (seqA.phase == Phase::FWD) {
      motorA_fwd();
      if ((int32_t)(now - seqA.tEnd) >= 0) {
        seqA.phase = Phase::REV;
        seqA.tEnd = now + T_MOTOR_DIR;
      }
    } else if (seqA.phase == Phase::REV) {
      motorA_rev();
      if ((int32_t)(now - seqA.tEnd) >= 0) {
        seqA.phase = Phase::DONE; motorA_brake(); delay(50); motorA_coast();
      }
    }
  }

  /* Secuencia concurrente Motor B */
  if (seqB.enabled) {
    if (seqB.phase == Phase::FWD) {
      motorB_fwd();
      if ((int32_t)(now - seqB.tEnd) >= 0) {
        seqB.phase = Phase::REV;
        seqB.tEnd = now + T_MOTOR_DIR;
      }
    } else if (seqB.phase == Phase::REV) {
      motorB_rev();
      if ((int32_t)(now - seqB.tEnd) >= 0) {
        seqB.phase = Phase::DONE; motorB_brake(); delay(50); motorB_coast();
      }
    }
  }

  /* Secuencia concurrente Servo (DERECHA -> IZQUIERDA -> CENTRO) */
  if (seqS.enabled) {
    if (seqS.step == 0) {
      if ((int32_t)(now - seqS.tEnd) >= 0) {
        servo.write(SERVO_LEFT);
        seqS.step = 1; seqS.tEnd = now + T_SERVO_TRAMO;
      }
    } else if (seqS.step == 1) {
      if ((int32_t)(now - seqS.tEnd) >= 0) {
        servo.write(SERVO_CENTER);
        seqS.step = 2;
      }
    }
  }

  /* Condición de fin: ambos motores terminados y servo centrado */
  if (seqA.phase == Phase::DONE && seqB.phase == Phase::DONE && seqS.step == 2) {
    checkFault("Secuencia 4 (concurrente)");
    seqA.enabled = seqB.enabled = false; seqS.enabled = false;
    motorA_coast(); motorB_coast();
    while (true) { delay(1000); } /* Mantener aquí tras finalizar */
  }
}