#define IR_PIN    0
#define LED_PIN   1
#define MOTOR_PIN 2

#define MIN_COMMAND 20
#define MIN_PWM     128

uint16_t lastData = 0;
unsigned long lastTime = 0;

// Stop hysteresis (optional – keep it to be safe)
uint8_t stopPending = 0;
const uint8_t STOP_THRESHOLD = 2;

uint16_t readSony12() {
  unsigned long hdr = pulseIn(IR_PIN, LOW, 6000);
  if (hdr == 0 || hdr < 1600 || hdr > 3400) return 0xFFFF;
  uint16_t data = 0;
  for (byte i = 0; i < 12; i++) {
    unsigned long dur = pulseIn(IR_PIN, LOW, 3000);
    if (dur == 0) return 0xFFFF;
    if (dur > 800) data |= ((uint16_t)1 << i);
  }
  return data;
}

void setup() {
  pinMode(IR_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(MOTOR_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  analogWrite(MOTOR_PIN, 0);
}

void loop() {
  uint16_t data = readSony12();

  if (data != 0xFFFF) {
    // Any valid packet resets the timeout timer
    lastTime = millis();

    byte command = data & 0x7F;
    byte address = data >> 7;

    // ---------- FWD ----------
    if (address == 1) {
      stopPending = 0;   // reset stop counter

      int pwm = 0;
      if (command > MIN_COMMAND) {
        pwm = map(command, MIN_COMMAND, 127, MIN_PWM, 255);
        if (pwm > 255) pwm = 255;
      }

      //  Write directly – with a stable cap, this is perfect.
      analogWrite(MOTOR_PIN, pwm);
      digitalWrite(LED_PIN, (pwm > 0) ? HIGH : LOW);
    }

    // ---------- REV ----------
    else if (address == 2) {
      stopPending++;
      if (stopPending >= STOP_THRESHOLD) {
        // Stop motor
        analogWrite(MOTOR_PIN, 0);
        digitalWrite(LED_PIN, LOW);
        stopPending = 0;
      }
    }
    // other addresses – ignored
  }
  else {
    //  Timeout reduced to 300ms (safe now, thanks to the cap!)
    if (millis() - lastTime > 200) {
      analogWrite(MOTOR_PIN, 0);
      digitalWrite(LED_PIN, LOW);
    }
  }
  delay(5);
}