#ifndef I2C_MOTOR_MASTER_H
#define I2C_MOTOR_MASTER_H

#include <Wire.h>

class I2CMotorMaster {
public:
    I2CMotorMaster(uint8_t sda = 8, uint8_t scl = 9, uint32_t freq = 100000)
        : sda_(sda), scl_(scl), freq_(freq), slave_addr_(0) {}

    void begin() {
        Wire.begin(sda_, scl_, freq_);
        // delay(1000);
        slave_addr_ = scanForMotorController();
        // delay(1000);
    }

    bool found() { return slave_addr_ != 0; }
    uint8_t address() { return slave_addr_; }

    String identify() {
        return queryIdentification(slave_addr_);
    }

    void setDeviceName(const char* newname) {
        uint8_t len = strlen(newname);
        Wire.beginTransmission(slave_addr_);
        Wire.write(0x06);
        Wire.write(len);
        Wire.write((const uint8_t*)newname, len);
        Wire.endTransmission();
        // delay(100);
    }

    void setMinMax(uint8_t min_pwm, uint8_t max_pwm) {
        Wire.beginTransmission(slave_addr_);
        Wire.write(0x07);
        Wire.write(min_pwm);
        Wire.write(max_pwm);
        Wire.endTransmission();
        // delay(100);
    }

    void setMotorDirection(uint8_t motor, uint8_t dir) {
        Wire.beginTransmission(slave_addr_);
        Wire.write(0x03);
        Wire.write(motor);
        Wire.write(dir); // 1=forward, 2=backward
        Wire.endTransmission();
        // delay(500);
    }

    void setMotorSpeed(uint8_t motor, uint8_t speed_percent) {
        Wire.beginTransmission(slave_addr_);
        Wire.write(0x02);
        Wire.write(motor);
        Wire.write(speed_percent);
        Wire.endTransmission();
        // delay(1000);
    }

    void emergencyStop() {
        Wire.beginTransmission(slave_addr_);
        Wire.write(0x05);
        Wire.endTransmission();
        // delay(1000);
    }

private:
    uint8_t sda_, scl_;
    uint32_t freq_;
    uint8_t slave_addr_;

    uint8_t scanForMotorController() {
        for (uint8_t addr = 1; addr < 127; addr++) {
            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("dc motor") >= 0) {
                    Serial.print("Found motor controller at 0x");
                    Serial.print(addr, HEX);
                    Serial.print(": ");
                    Serial.println(id);
                    return addr;
                }
            }
            delay(5);
        }
        Serial.println("Motor controller not found!");
        return 0;
    }

    String queryIdentification(uint8_t addr) {
        Wire.beginTransmission(addr);
        Wire.write('i');
        Wire.endTransmission();
        delay(10);
        Wire.requestFrom(addr, 48);
        String response;
        while (Wire.available()) {
            char c = Wire.read();
            if (c == '\0') break;
            response += c;
        }
        Serial.print("Identification at 0x");
        Serial.print(addr, HEX);
        Serial.print(": ");
        Serial.println(response);
        return response;
    }
};

#endif