#include <Wire.h>
#include <EEPROM.h>

// Stepper pin definitions (adjust as needed)
#define DIR_PIN  5
#define STEP_PIN 2
#define EN_PIN   8

#define ADDR_EEPROM_LOC 0
#define EEPROM_SIZE 64

#define CMD_CHANGE_ADDR 0xAA

volatile uint8_t i2c_addr = 0x55; // Default I2C address

// Reads the I2C address from EEPROM, returns default if not set
uint8_t getI2CAddress(uint8_t defaultAddr = 0x55) {
    uint8_t addr = EEPROM.read(ADDR_EEPROM_LOC);
    if (addr == 0xFF || addr == 0) addr = defaultAddr;
    return addr;
}

// Writes new I2C address to EEPROM and restarts the MCU
void setI2CAddress(uint8_t newAddr) {
    EEPROM.write(ADDR_EEPROM_LOC, newAddr);
    EEPROM.commit();
    Serial.print("Address changed to: 0x");
    Serial.println(newAddr, HEX);
    delay(100);
    ESP.restart(); // Use NVIC_SystemReset() for STM32, or asm volatile ("  jmp 0"); for AVR
}

// Stepper control function: move a fixed number of steps
void moveSteps(uint16_t steps, uint8_t direction, uint16_t speedDelay) {
    digitalWrite(DIR_PIN, direction);
    for (uint16_t i = 0; i < steps; i++) {
        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(speedDelay);
        digitalWrite(STEP_PIN, LOW);
        delayMicroseconds(speedDelay);
    }
}

// I2C receive event handler
void receiveEvent(int howMany) {
    if (howMany < 1) return;
    uint8_t cmd = Wire.read();

    if (cmd == CMD_CHANGE_ADDR) {
        if (Wire.available() >= 1) {
            uint8_t newAddr = Wire.read();
            setI2CAddress(newAddr);
            while (Wire.available()) Wire.read(); // Clear buffer
        }
        return;
    }

    // If not a command, treat as speed/direction
    uint8_t speed = cmd;
    if (Wire.available() < 1) return;
    uint8_t direction = Wire.read();

    // Map speed (0-100) to step delay (us): 100 = fast, 1 = slow
    uint16_t speedDelay = map(speed, 1, 100, 2000, 200); // Tune as needed

    // Move a fixed number of steps for demonstration
    moveSteps(200, direction, speedDelay); // 200 steps = 1 rev for 1.8° stepper
}

void setup() {
    pinMode(DIR_PIN, OUTPUT);
    pinMode(STEP_PIN, OUTPUT);
    pinMode(EN_PIN, OUTPUT);
    digitalWrite(EN_PIN, LOW); // Enable driver

    Serial.begin(115200);

    EEPROM.begin(EEPROM_SIZE);
    i2c_addr = getI2CAddress(0x55);

    Serial.print("Using I2C address: 0x");
    Serial.println(i2c_addr, HEX);

    Wire.begin(i2c_addr);
    Wire.onReceive(receiveEvent);
}

void loop() {
    // Nothing needed here
}