---

// ============================================================
//  ТЕСТ: едем вперёд, смотрим тики энкодеров
//  Драйвер:  A(6=PWM, 7=DIR)   B(10=PWM, 11=DIR)
//  Энкодеры: A(A=3, B=4)       B(A=12, B=13)
//  Без библиотек, только регистры.
// ============================================================

// --- Пины моторов ---
const uint8_t MA_PWM = 6;
const uint8_t MA_DIR = 7;
const uint8_t MB_PWM = 10;
const uint8_t MB_DIR = 11;

// --- Счётчики (меняются в прерывании!) ---
volatile long    countA = 0;
volatile long    countB = 0;
volatile uint8_t stateA = 0;   // предыдущее состояние (A,B) энкодера A
volatile uint8_t stateB = 0;   // предыдущее состояние (A,B) энкодера B

// --- Таблица квадратурного декодера (16 значений) ---
const int8_t QDEC[16] = {
   0, -1,  1,  0,
   1,  0,  0, -1,
  -1,  0,  0,  1,
   0,  1, -1,  0
};

// ------------------------------------------------------------
void setup() {
  Serial.begin(115200);

  // Моторы
  pinMode(MA_PWM, OUTPUT);
  pinMode(MA_DIR, OUTPUT);
  pinMode(MB_PWM, OUTPUT);
  pinMode(MB_DIR, OUTPUT);
  analogWrite(MA_PWM, 0);
  analogWrite(MB_PWM, 0);

  // Энкодеры — входы с подтяжкой
  pinMode(3,  INPUT_PULLUP);
  pinMode(4,  INPUT_PULLUP);
  pinMode(12, INPUT_PULLUP);
  pinMode(13, INPUT_PULLUP);

  // Стартовое состояние читаем напрямую из PIN-регистров
  stateA = (((PIND >> PD3) & 1) << 1) | ((PIND >> PD4) & 1);
  stateB = (((PINB >> PB4) & 1) << 1) | ((PINB >> PB5) & 1);

  // --- Включаем Pin Change Interrupt ---
  // Порт D: пины 3 (PD3/PCINT19) и 4 (PD4/PCINT20) → энкодер A
  PCMSK2 |= (1 << PCINT19) | (1 << PCINT20);
  PCICR  |= (1 << PCIE2);

  // Порт B: пины 12 (PB4/PCINT4) и 13 (PB5/PCINT5) → энкодер B
  PCMSK0 |= (1 << PCINT4) | (1 << PCINT5);
  PCICR  |= (1 << PCIE0);

  Serial.println("=== Старт теста ===");
  delay(2000);
}

// ------------------------------------------------------------
// ISR на изменение любого пина порта D (пины 3 и 4 → энкодер A)
ISR(PCINT2_vect) {
  uint8_t p = PIND;
  uint8_t curr = (((p >> PD3) & 1) << 1) | ((p >> PD4) & 1);
  countA += QDEC[(stateA << 2) | curr];
  stateA = curr;
}

// ISR на изменение любого пина порта B (пины 12 и 13 → энкодер B)
ISR(PCINT0_vect) {
  uint8_t p = PINB;
  uint8_t curr = (((p >> PB4) & 1) << 1) | ((p >> PB5) & 1);
  countB += QDEC[(stateB << 2) | curr];
  stateB = curr;
}

// ------------------------------------------------------------
void loop() {
  Serial.println("Едем вперёд 2 секунды...");

  // Оба мотора — вперёд
  digitalWrite(MA_DIR, HIGH);
  digitalWrite(MB_DIR, HIGH);
  analogWrite(MA_PWM, 150);
  analogWrite(MB_PWM, 150);

  // 2 секунды печатаем счётчики
  unsigned long t0 = millis();
  while (millis() - t0 < 2000) {
    long a, b;
    noInterrupts();       // читаем атомарно
    a = countA; b = countB;
    interrupts();

    Serial.print("A="); Serial.print(a);
    Serial.print("\tB="); Serial.println(b);
    delay(200);
  }

  // Стоп
  analogWrite(MA_PWM, 0);
  analogWrite(MB_PWM, 0);

  // Итог
  long a, b;
  noInterrupts();
  a = countA; b = countB;
  interrupts();

  Serial.print("=== ИТОГО за 2 с: A=");
  Serial.print(a);
  Serial.print("  B=");
  Serial.println(b);
  Serial.println("Если A или B ~0 — проверь энкодер / питание.");
  Serial.println("Если A и B с РАЗНЫМИ знаками — колёса крутятся в разные стороны.");
  Serial.println("Дальше — стоп. Перезагрузи плату, чтобы повторить.\n");

  while (true) { /* стоп */ }
}