#include "EncoderStepCounter.h"

#define ENCODER_PIN1 2
#define ENCODER_INT1 digitalPinToInterrupt(ENCODER_PIN1)
#define ENCODER_PIN2 3
#define ENCODER_INT2 digitalPinToInterrupt(ENCODER_PIN2)

// Motor:
// OFF:   S1=H S2=L S3=H S4=L
// RIGHT: S1=L S2=L S3=H S4=H
// LEFT:  S1=H S2=H S3=L S4=L

#define BRIDGE_PIN_S1_PMOS 41
#define BRIDGE_PIN_S2_NMOS 43
#define BRIDGE_PIN_S3_PMOS 45
#define BRIDGE_PIN_S4_NMOS 47
#define BTN_VOL_UP 51
#define BTN_VOL_DN 53

bool mainState = true;


// Create instance for one full step encoder
EncoderStepCounter encoder(ENCODER_PIN1, ENCODER_PIN2);
// Use the following for half step encoders
//EncoderStepCounter encoder(ENCODER_PIN1, ENCODER_PIN2, HALF_STEP);


void setup() {

  Serial.begin(9600);
  Serial.println("start");

  // Initialize encoder
  encoder.begin();
  // Initialize interrupts
  attachInterrupt(ENCODER_INT1, interrupt, CHANGE);
  attachInterrupt(ENCODER_INT2, interrupt, CHANGE);

  // put your setup code here, to run once:
  pinMode(BRIDGE_PIN_S1_PMOS, OUTPUT);
  pinMode(BRIDGE_PIN_S2_NMOS, OUTPUT);
  pinMode(BRIDGE_PIN_S3_PMOS, OUTPUT);
  pinMode(BRIDGE_PIN_S4_NMOS, OUTPUT);
  pinMode(BTN_VOL_UP, INPUT_PULLUP);
  pinMode(BTN_VOL_DN, INPUT_PULLUP);

  motor_off(30);

}


// Call tick on every change interrupt
void interrupt() {
  if (mainState == true) {
    encoder.tick();
  }
}


signed long position = 0;


void loop() {

  if (mainState == true) {
    if ((digitalRead(BTN_VOL_UP) == LOW) && (digitalRead(BTN_VOL_DN) == HIGH)) {
      motor_right(300);
    } else if ((digitalRead(BTN_VOL_UP) == HIGH) && (digitalRead(BTN_VOL_DN) == LOW)) {
      motor_left(300);
    } else {
      motor_off(30);
    }
  } else {
    motor_off(30);
  }

  if (mainState == true) {
    signed char pos = encoder.getPosition();
  //  Serial.println(pos);
    if (pos != 0) {
      if (pos > 0) {
      //  move_output(1);
        motor_right(300);
      }
      if (pos < 0) {
      //  move_output(-1);
        motor_left(300);
      }
      position += pos;
      encoder.reset();
      Serial.println(position);
    } else {
      motor_off(30);
    }
  } else {
    motor_off(30);
  }


}


// Motor OFF
void motor_off(int duration) {
  digitalWrite(BRIDGE_PIN_S1_PMOS, HIGH);
  digitalWrite(BRIDGE_PIN_S2_NMOS, LOW);
  digitalWrite(BRIDGE_PIN_S3_PMOS, HIGH);
  digitalWrite(BRIDGE_PIN_S4_NMOS, LOW);
  delay(duration);
}

// Motor RIGHT
void motor_right(int duration) {
  // first off pins
  digitalWrite(BRIDGE_PIN_S2_NMOS, LOW);
  digitalWrite(BRIDGE_PIN_S3_PMOS, HIGH);
  delay(30);
  // then on pins
  digitalWrite(BRIDGE_PIN_S1_PMOS, LOW);
  digitalWrite(BRIDGE_PIN_S4_NMOS, HIGH);
  delay(duration);
}

// Motor LEFT
void motor_left(int duration) {
  // first off pins
  digitalWrite(BRIDGE_PIN_S1_PMOS, HIGH);
  digitalWrite(BRIDGE_PIN_S4_NMOS, LOW);
  delay(30);
  // then on pins
  digitalWrite(BRIDGE_PIN_S2_NMOS, HIGH);
  digitalWrite(BRIDGE_PIN_S3_PMOS, LOW);
  delay(duration);
}