#include "I2CMotor.h"

I2CMotor::I2CMotor() {}

I2CMotor::I2CMotor(uint8_t sda, uint8_t scl, uint32_t freq)
    : sda_(sda), scl_(scl), freq_(freq), slave_addr_(0) {}

void I2CMotor::begin() {
    Wire.begin(sda_, scl_, freq_);
    slave_addr_ = scanForMotorController();
}

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

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

void I2CMotor::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();
}

void I2CMotor::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();
}

void I2CMotor::setMotorDirection(uint8_t motor, uint8_t dir) {
    Wire.beginTransmission(slave_addr_);
    Wire.write(0x03);
    Wire.write(motor);
    Wire.write(dir);
    Wire.endTransmission();
}

void I2CMotor::setMotorSpeed(uint8_t motor, uint8_t speed_percent) {
    Wire.beginTransmission(slave_addr_);
    Wire.write(0x02);
    Wire.write(motor);
    Wire.write(speed_percent);
    Wire.endTransmission();
}

void I2CMotor::emergencyStop() {
    Wire.beginTransmission(slave_addr_);
    Wire.write(0x05);
    Wire.endTransmission();
}

uint8_t I2CMotor::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 I2CMotor::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;
}