#define ENVELOPE 0xAA
#define MOTOR1_PWM_PIN 16
#define MOTOR2_PWM_PIN 17
#define PWM_CHANNEL_1 0
#define PWM_CHANNEL_2 1
#define PWM_FREQ 1000
#define PWM_RESOLUTION 8 // 8 bits: 0-255

unsigned long lastReceiveTime = 0;
uint8_t prevMotor1 = 0, prevMotor2 = 0;

void setup() {
  Serial.begin(115200);
  ledcSetup(PWM_CHANNEL_1, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(PWM_CHANNEL_2, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(MOTOR1_PWM_PIN, PWM_CHANNEL_1);
  ledcAttachPin(MOTOR2_PWM_PIN, PWM_CHANNEL_2);
}

void loop() {
  static enum { WAIT_HEADER, WAIT_M1, WAIT_M2 } state = WAIT_HEADER;
  static uint8_t m1_val, m2_val;
  
  while (Serial.available()) {
    uint8_t b = Serial.read();
    if (state == WAIT_HEADER) {
      if (b == ENVELOPE) state = WAIT_M1;
    } else if (state == WAIT_M1) {
      m1_val = b;
      state = WAIT_M2;
    } else if (state == WAIT_M2) {
      m2_val = b;
      // Only update if changed
      if (m1_val != prevMotor1) {
        ledcWrite(PWM_CHANNEL_1, m1_val);
        prevMotor1 = m1_val;
      }
      if (m2_val != prevMotor2) {
        ledcWrite(PWM_CHANNEL_2, m2_val);
        prevMotor2 = m2_val;
      }
      lastReceiveTime = millis();
      state = WAIT_HEADER;
    }
  }

  // Motor safety: Stop if no update in 200 ms
  if (millis() - lastReceiveTime > 200) {
    if (prevMotor1 != 0) {
      ledcWrite(PWM_CHANNEL_1, 0);
      prevMotor1 = 0;
    }
    if (prevMotor2 != 0) {
      ledcWrite(PWM_CHANNEL_2, 0);
      prevMotor2 = 0;
    }
  }
}