#define EN 0  // PA4
#define PUL 1 // PA5
#define DIR 2 // PA6

#define LIMIT_SW 8 // PA3
#define GO_SW 9 //PA2

#define REV 800 // steps/rev => 38mm
#define STEP_DIST 38 // mm / rev

#define MAX_DIST 550 // 550mm

// Global variables
bool is_zeroed = false;
float curr_dist = 0;
float delta_dist = (STEP_DIST * 1.0) / REV;

// Note: define CCW as negative distance

void rotate_CW(float distance) {
  Serial.print("Rotate  CW by "); Serial.println(distance);
  int steps = (distance / STEP_DIST) * REV;
  digitalWrite(EN, HIGH);
  digitalWrite(DIR, LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(PUL, LOW);
    delay(1);
    digitalWrite(PUL, HIGH);
    curr_dist += delta_dist;
    if (curr_dist + delta_dist > MAX_DIST) break;
  }
  digitalWrite(EN, LOW);
  Serial.println("Done CW");
}

void rotate_CCW(float distance) {
  Serial.print("Rotate CCW by "); Serial.println(distance);
  int steps = (distance / STEP_DIST) * REV;
  digitalWrite(EN, HIGH);
  digitalWrite(DIR, HIGH);
  for (int i = 0; i < steps; i++) {
    digitalWrite(PUL, LOW);
    delay(1);
    digitalWrite(PUL, HIGH);
    curr_dist -= delta_dist;
    if (curr_dist - delta_dist < 0) break;
  }
  digitalWrite(EN, LOW);
  Serial.println("Done CCW");
}

void rotate(float distance) {
  if (distance > 0) {
    rotate_CW(distance);
  } else {
    rotate_CCW(-distance);
  }
}

void zeroPan() {
  digitalWrite(EN, HIGH);
  digitalWrite(DIR, HIGH);
  while (!is_zeroed) {
    digitalWrite(PUL, LOW);
    delay(1);
    digitalWrite(PUL, HIGH);
    is_zeroed = !digitalRead(LIMIT_SW);
  }
  digitalWrite(EN, LOW);
  rotate_CW(10);
  is_zeroed = false;
  Serial.println("Done zero!");
  curr_dist = 0;
}

void runPan() {
  rotate_CW(MAX_DIST);
}

void setup() {
  // put your setup code here, to run once:
  pinMode(EN, OUTPUT);
  pinMode(PUL, OUTPUT);
  pinMode(DIR, OUTPUT);

  pinMode(LIMIT_SW, INPUT);
  pinMode(GO_SW, INPUT);

  // initialize stepper
  //  digitalWrite(EN, HIGH);
  digitalWrite(PUL, HIGH);
  Serial.begin(9600);

  zeroPan();
}

void loop() {
  // put your main code here, to run repeatedly:
  if (Serial.available()) {
    float input_distance = Serial.parseFloat();
    if (input_distance > 0 || input_distance < 0) {
      rotate(input_distance);
    }
    delay(1000);
  }
  bool go = !digitalRead(GO_SW);
  if (go) {
    runPan();
    delay(2000);
    zeroPan();
  }
}