// Raspberry Pi Pico + Stepper Motor Example

#define DIR_PIN_M1 2
#define STEP_PIN_M1 3

#define DIR_PIN_M2 5
#define STEP_PIN_M2 6

// global
int current_pos = 0;
int target = 10;
int STEPS_ONE_LENGTH = 200;

void move_to_new_position() {
  Serial.print("TARGET: ");
  Serial.println(target);

  // target from 0 to 100 %
  // berechnen zu welchem STEP das dann führt
  float des_steps = (target / 100.0) * STEPS_ONE_LENGTH;
  // calculate the delta between current position and desired position
  float diff = des_steps - current_pos;

  Serial.print("DIFF: ");
  Serial.println(diff);

  // check if target is current position
  if (diff == 0) {
    Serial.println("No Position change made. Current position is the target position!");
    return;
  }
  // calculate the direction in which the Motor should move
  // target > current means it should move downwards 
  else if (diff > 0) {
    digitalWrite(DIR_PIN_M1, LOW);
    Serial.println("MOVING DOWN!");
  }
  else { // target < current means it should move upwards
    digitalWrite(DIR_PIN_M1, HIGH);
    Serial.println("MOVING UP!");
    diff = -diff; // Make diff positive for the for loop
  }

  // go to new position with 1rpm (200 Steps/s)
  for (int i = 0; i < (int)diff; i++) {
    digitalWrite(STEP_PIN_M1, HIGH);
    delay(5); // 5 ms * 200 = 1 second
    digitalWrite(STEP_PIN_M1, LOW);
    delay(5); // Adding delay here to ensure a proper step pulse
  }

  current_pos = des_steps; // Update the current position to the new position
  Serial.print("New Position: ");
  Serial.println(current_pos);
}

void setup() {
  Serial.begin(9600);

  pinMode(STEP_PIN_M1, OUTPUT);
  pinMode(DIR_PIN_M1, OUTPUT);

  pinMode(DIR_PIN_M2, OUTPUT);
  pinMode(STEP_PIN_M2, OUTPUT);

  digitalWrite(STEP_PIN_M1, LOW);
  digitalWrite(STEP_PIN_M2, LOW);
}

void loop() {
  target = 100;
  move_to_new_position();
  delay(500);
  target = 20;
  move_to_new_position();
  delay(500);
  target = 70;
  move_to_new_position();
  delay(500);
  target = 50;
  move_to_new_position();
  delay(500);
}