const int I1_PIN = 6;
const int I2_PIN = 7;
const int SPEED_PIN = 3; // PWM output
const int HALL_A_PIN = 2;
const int HALL_B_PIN = 4;

volatile int curPos = 0;

volatile int prevA = LOW;
volatile int prevB = LOW;
int prevPos = 0;

volatile boolean stateChanged = false;

void setup() {
  Serial.begin(9600);
  pinMode(I1_PIN, OUTPUT);
  pinMode(I2_PIN, OUTPUT);
  pinMode(SPEED_PIN, OUTPUT);

  delay(100);
  attachInterrupt(0, hallChange, CHANGE); // interrupt on pin 2
  // Move clockwise
  digitalWrite(I1_PIN, LOW);
  digitalWrite(I2_PIN, HIGH);
  analogWrite(SPEED_PIN, 100);
}

void loop() {
  Serial.println(digitalRead(HALL_A_PIN));
  if (curPos != prevPos) {
    Serial.println(curPos);
    prevPos = curPos;
  }
  /*
  if (stateChanged) {
    Serial.println("Changed");
    stateChanged=false;
  }*/
}

void hallChange() {
  stateChanged = true;
  int a = digitalRead(HALL_A_PIN);
  int b = digitalRead(HALL_B_PIN);
  if (a == HIGH && b == LOW) {
    if (prevA == LOW && prevB == LOW) {
      curPos = 1;
    }
  }
  else if (a == HIGH && b == HIGH) {
    if (prevA == HIGH && prevB == LOW) {
      curPos = 2;
    }
  }
  else if (a == LOW && b == HIGH) {
    if (prevA == HIGH && prevB == HIGH) {
      curPos = 3;
    }
  }
  else if (a == LOW && b == LOW) {
    if (prevA == LOW && prevB == HIGH) {
      curPos = 4;
    }
  }

  prevA = a;
  prevB = b;
}