import RPi.GPIO as GPIO
import time

# GPIO pins for motor control
IN1_PIN = 17
IN2_PIN = 23
IN3_PIN = 27
IN4_PIN = 22

# Motor parameters
STEPS_PER_REVOLUTION = 512
DEGREES_PER_STEP = 360 / STEPS_PER_REVOLUTION

# Setup GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(IN1_PIN, GPIO.OUT)
GPIO.setup(IN2_PIN, GPIO.OUT)
GPIO.setup(IN3_PIN, GPIO.OUT)
GPIO.setup(IN4_PIN, GPIO.OUT)

def set_step_sequence(step, direction):
    sequence = [
        [GPIO.HIGH, GPIO.LOW, GPIO.LOW, GPIO.LOW],
        [GPIO.HIGH, GPIO.HIGH, GPIO.LOW, GPIO.LOW],
        [GPIO.LOW, GPIO.HIGH, GPIO.LOW, GPIO.LOW],
        [GPIO.LOW, GPIO.HIGH, GPIO.HIGH, GPIO.LOW],
        [GPIO.LOW, GPIO.LOW, GPIO.HIGH, GPIO.LOW],
        [GPIO.LOW, GPIO.LOW, GPIO.HIGH, GPIO.HIGH],
        [GPIO.LOW, GPIO.LOW, GPIO.LOW, GPIO.HIGH],
        [GPIO.HIGH, GPIO.LOW, GPIO.LOW, GPIO.HIGH]
    ]
    if direction == 'forward':
        GPIO.output(IN1_PIN, sequence[step][0])
        GPIO.output(IN2_PIN, sequence[step][1])
        GPIO.output(IN3_PIN, sequence[step][2])
        GPIO.output(IN4_PIN, sequence[step][3])
    elif direction == 'backward':
        # Reverse the step sequence for backward direction
        GPIO.output(IN1_PIN, sequence[7 - step][0])
        GPIO.output(IN2_PIN, sequence[7 - step][1])
        GPIO.output(IN3_PIN, sequence[7 - step][2])
        GPIO.output(IN4_PIN, sequence[7 - step][3])

def move_motor(degrees):
    # Calculate the number of steps to move based on the given degrees
    steps = int(degrees / DEGREES_PER_STEP)

    # Set the direction based on the sign of the steps
    if steps > 0:
        direction = 'forward'
    else:
        direction = 'backward'

    # Perform the specified number of steps
    for _ in range(abs(steps)):
        for step in range(8):
            set_step_sequence(step, direction)
            time.sleep(0.001)  # Adjust delay as needed for your motor

    # Calculate the actual position in degrees
    position = steps * DEGREES_PER_STEP
    return position

# Example usage
target_degrees = 100 # Desired rotation in degrees
final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = -60 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = 180 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = -180 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = 220 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = -220 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = 344 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
# target_degrees = -344 # Desired rotation in degrees
# final_position = move_motor(target_degrees)
# time.sleep(1)
print(f"Final position: {round(final_position)} degrees")

# Cleanup GPIO
GPIO.cleanup()