"""
Enhanced Stepper Motor Control with A4988 Driver and Raspberry Pi Pico
https://wokwi.com/projects/334512724199342674
https://how2electronics.com/control-stepper-motor-with-a4988-raspberry-pi-pico/
https://iot.keicode.com/arduino/arduino-stepper-motor-a4988.php
https://lastminuteengineers.com/a4988-stepper-motor-driver-arduino-tutorial/
https://smile-dental-clinic.info/wordpress/?p=11521 5Vで動くA4988探すか、 TMC2208/2209が良いかも？

Features:
- Dynamic microstepping control via MS1, MS2, MS3 pins
- Acceleration and deceleration profiles for smooth motion
- Limit switch integration for safety
- Adjustable steps per revolution based on microstepping mode
"""

from machine import Pin
import utime

# Microstepping resolution settings
MICROSTEP_RESOLUTION = {
    'FULL': (0, 0, 0, 1),
    'HALF': (1, 0, 0, 2),
    'QUARTER': (0, 1, 0, 4),
    'EIGHTH': (1, 1, 0, 8),
    'SIXTEENTH': (1, 1, 1, 16)
}

class Stepper:
    def __init__(self, step_pin, dir_pin, ms_pins, steps_per_rev=200):
        self.step_pin = Pin(step_pin, Pin.OUT)
        self.dir_pin = Pin(dir_pin, Pin.OUT)
        self.ms_pins = [Pin(pin, Pin.OUT) for pin in ms_pins]
        self.steps_per_rev = steps_per_rev
        self.delay = 0.005  # Default delay
        self.accel = 0.0  # Acceleration in steps/s^2
        self.decel = 0.0  # Deceleration in steps/s^2

    def set_microstep(self, mode):
        ms_settings = MICROSTEP_RESOLUTION.get(mode, MICROSTEP_RESOLUTION['HALF'])
        for pin, value in zip(self.ms_pins, ms_settings[:-1]):
            pin.value(value)
        self.microstep = ms_settings[-1]
        self.steps_per_rev = 200 * self.microstep  # Adjust steps per revolution

    def set_speed(self, rpm):
        self.delay = 60 / (self.steps_per_rev * rpm)

    def set_direction(self, direction):
        self.dir_pin.value(direction)

    def set_acceleration(self, accel, decel):
        self.accel = accel  # In steps/s^2
        self.decel = decel  # In steps/s^2

    def step(self):
        self.step_pin.value(1)
        utime.sleep_us(2)
        self.step_pin.value(0)

    def move(self, direction, steps):
        self.set_direction(direction)
        current_delay = self.delay
        accel_steps = int((self.steps_per_rev * (self.accel / 60)) / current_delay)
        decel_steps = int((self.steps_per_rev * (self.decel / 60)) / current_delay)
        steady_steps = steps - accel_steps - decel_steps

        # Acceleration phase
        for _ in range(accel_steps):
            self.step()
            utime.sleep(current_delay)
            current_delay = max(current_delay - self.accel, self.delay / 10)

        # Steady phase
        for _ in range(steady_steps):
            self.step()
            utime.sleep(self.delay)

        # Deceleration phase
        for _ in range(decel_steps):
            self.step()
            utime.sleep(current_delay)
            current_delay = min(current_delay + self.decel, self.delay)

    def stop(self):
        # Implement immediate stop if required
        pass

# Define the pins
dir_pin = 2    # GPIO number where DIR pin is connected
step_pin = 3   # GPIO number where STEP pin is connected
ms_pins = [4, 5, 6]  # GPIO numbers for MS1, MS2, MS3

# Initialize stepper
stepper = Stepper(step_pin, dir_pin, ms_pins)

def loop():
    while True:

        # Set microstepping mode
        stepper.set_microstep('FULL')  # Options: 'FULL', 'HALF', 'QUARTER', 'EIGHTH', 'SIXTEENTH'

        # Set speed and acceleration
        stepper.set_speed(120)  # Set speed in RPM
        stepper.set_acceleration(accel=0.001, decel=0.001)  # Acceleration and deceleration rates

        # Move forward 2 revolutions
        stepper.move(direction=1, steps=stepper.steps_per_rev * 2)
        utime.sleep(1)

        # Move backward 1 revolution
        stepper.move(direction=0, steps=stepper.steps_per_rev)
        utime.sleep(1)

if __name__ == '__main__':
    loop()