#include <I2CMotor.h>

// Define I2C pins and slave addresses
#define SDA_PIN 8
#define SCL_PIN 3
#define INITIAL_SLAVE_ADDR 0x10
#define NEW_SLAVE_ADDR     0x12

I2CMotor motor(SDA_PIN, SCL_PIN);

// Scan I2C bus for motor controllers and print their info
void scanI2CBusForMotors() {
    Serial.println("Scanning I2C bus for motor controllers...");
    for (uint8_t addr = 1; addr < 127; addr++) {
        Wire.beginTransmission(addr);
        uint8_t error = Wire.endTransmission();
        if (error == 0) {
            // If device ACKs, try to identify it
            Wire.beginTransmission(addr);
            Wire.write('i');
            if (Wire.endTransmission() == 0) {
                Wire.requestFrom(addr, 48);
                String id;
                while (Wire.available()) {
                    char c = Wire.read();
                    if (c == '\0') break;
                    id += c;
                }
                if (id.length() && id.indexOf("GENERIC") >= 0) {
                    Serial.print("Found motor controller at 0x");
                    Serial.print(addr, HEX);
                    Serial.print(": ");
                    Serial.println(id);
                }
            }
        }
        delay(5);
    }
    Serial.println("Scan complete.");
}

// Send the command to change the slave's I2C address
void setSlaveAddress(uint8_t currentAddr, uint8_t newAddr) {
    Wire.beginTransmission(currentAddr);
    Wire.write(0x01);      // CMD_SET_ADDR
    Wire.write(newAddr);   // New address
    Wire.endTransmission();
    Serial.print("Sent address change command to 0x");
    Serial.print(currentAddr, HEX);
    Serial.print(". New address should be 0x");
    Serial.println(newAddr, HEX);
}

void setup() {
    Serial.begin(115200);
    delay(2000); // Wait for serial and slave to boot

    // Initialize I2C and motor library
    Wire.begin(SDA_PIN, SCL_PIN);
    motor.begin();
    motor.setAddress(INITIAL_SLAVE_ADDR);

    // 1. Set device name
    motor.setDeviceName("TestMotor");
    delay(200);

    // 2. Identify device
    String id = motor.identify();
    Serial.print("Identify: "); Serial.println(id);
    delay(200);

    // 3. Control Motor 1: 80% speed, forward
    motor.motorControl(1, 80, 0);
    delay(2000);

    // 4. Control Motor 2: 60% speed, reverse
    motor.motorControl(2, 60, 1);
    delay(2000);

    // 5. Control both motors: A 100% forward, B 100% reverse
    motor.bothMotorsControl(100, 0, 100, 1);
    delay(2000);

    // 6. Stop all motors
    motor.stopAllMotors();
    delay(1000);

    // 7. All stop (emergency stop)
    motor.allStop();
    delay(1000);

    // 8. Scan I2C bus for motor controllers
    scanI2CBusForMotors();

    // 9. Change slave address
    setSlaveAddress(INITIAL_SLAVE_ADDR, NEW_SLAVE_ADDR);
    Serial.println("Waiting for slave to reboot with new address...");
    delay(2000); // Give time for slave to reboot

    // 10. Use the new address for all further commands
    motor.setAddress(NEW_SLAVE_ADDR);

    // 11. Identify device at new address
    String id2 = motor.identify();
    Serial.print("Identify at new address: "); Serial.println(id2);

    // 12. Run a test at new address (Motor 1, 50% forward)
    motor.motorControl(1, 50, 0);
    delay(2000);

    // 13. Stop all motors at new address
    motor.stopAllMotors();
    delay(1000);
}

void loop() {
    // Nothing here
}