#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Mechanical parameters
#define SCREW_PITCH_MM          3.0     // Screw pitch in mm per revolution
#define STEPS_PER_REVOLUTION    100     // Motor pulses per one full revolution
#define STEP_PULSE_US           800     // STEP pulse duration in microseconds

// Pins
#define STEP_PIN                2
#define DIR_PIN                 3
#define BUTTON_PIN              4
#define TRIGGER_INPUT_PIN       5
#define TRIGGER_OUTPUT_PIN      6

// LCD
#define LCD_ADDRESS             0x27
#define LCD_COLUMNS             20
#define LCD_ROWS                4

// Available movement distances in mm
const float distanceTable[] =
{
    3.0,
    6.0,
    9.0,
    12.0,
    15.0
};

const uint8_t DISTANCE_COUNT = sizeof(distanceTable) / sizeof(distanceTable[0]);

LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

uint8_t selectedIndex = 0;
bool lastButtonState = HIGH;
bool lastTriggerState = LOW;

long calculateSteps(float distanceMm)
{
    float revolutions = distanceMm / SCREW_PITCH_MM;
    return (long)(revolutions * STEPS_PER_REVOLUTION);
}

void generateSteps(long steps, bool direction)
{
    digitalWrite(DIR_PIN, direction);

    for (long i = 0; i < steps; i++)
    {
        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(STEP_PULSE_US);
        digitalWrite(STEP_PIN, LOW);
        delayMicroseconds(STEP_PULSE_US);
    }
}

void updateDisplay(bool isWorking)
{
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("  Wybrany dystans:");

    lcd.setCursor(0, 1);
    lcd.print("       ");
    lcd.print(distanceTable[selectedIndex], 1);
    lcd.print(" mm");

    if (isWorking)
    {
        lcd.setCursor(0, 3);
        lcd.print("      (PRACA)");
    }
}

void setup()
{
    pinMode(STEP_PIN, OUTPUT);
    pinMode(DIR_PIN, OUTPUT);
    pinMode(BUTTON_PIN, INPUT_PULLUP);
    pinMode(TRIGGER_INPUT_PIN, INPUT);
    pinMode(TRIGGER_OUTPUT_PIN, OUTPUT);

    digitalWrite(TRIGGER_OUTPUT_PIN, LOW);

    lcd.init();
    lcd.backlight();

    updateDisplay(false);
}

void loop()
{
    bool currentButtonState = digitalRead(BUTTON_PIN);

    if (lastButtonState == HIGH && currentButtonState == LOW)
    {
        selectedIndex++;
        if (selectedIndex >= DISTANCE_COUNT)
        {
            selectedIndex = 0;
        }
        updateDisplay(false);

        delay(10);
        while(!digitalRead(BUTTON_PIN)) {}
        delay(10);
    }

    lastButtonState = currentButtonState;


    bool currentTriggerState = digitalRead(TRIGGER_INPUT_PIN);

    if (lastTriggerState == LOW && currentTriggerState == HIGH)
    {
        float distance = distanceTable[selectedIndex];
        long steps = calculateSteps(distance);

        updateDisplay(true);

        // Move forward
        generateSteps(steps, HIGH);

        // Move backward
        generateSteps(steps, LOW);

        // Output 50 ms pulse
        digitalWrite(TRIGGER_OUTPUT_PIN, HIGH);
        delay(50);
        digitalWrite(TRIGGER_OUTPUT_PIN, LOW);

        updateDisplay(false);
    }

    lastTriggerState = currentTriggerState;
}