#ifndef ESP32MOTORI2C_H
#define ESP32MOTORI2C_H

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

#define DEFAULT_ADDR 0x08
#define EEPROM_SIZE 64
#define DEVICENAME_MAXLEN 24
#define EEPROM_MIN_PWM 32
#define EEPROM_MAX_PWM 33
#define PWM_RESOLUTION_BITS 8
#define PWM_RESOLUTION_MAX ((1 << PWM_RESOLUTION_BITS) - 1)

class ESP32MotorI2C {

public:
    
    ESP32MotorI2C(uint8_t en1, uint8_t in1, uint8_t in2, uint8_t in3, uint8_t in4, uint8_t en2)
        : en1_(en1), in1_(in1), in2_(in2), in3_(in3), in4_(in4), en2_(en2), id_requested_(false)
    {
        memset(device_name_, 0, sizeof(device_name_));
        min_pwm_ = 0;
        max_pwm_ = PWM_RESOLUTION_MAX;
    }

    void begin(uint8_t sda = 8, uint8_t scl = 9, uint32_t freq = 100000) {
        Serial.begin(115200);
        Serial.println("[SLAVE] Booting...");

        EEPROM.begin(EEPROM_SIZE);

        i2c_address_ = EEPROM.read(0);
        if(i2c_address_ < 0x08 || i2c_address_ > 0x77) {
            i2c_address_ = DEFAULT_ADDR;
            EEPROM.write(0, i2c_address_);
            EEPROM.commit();
        }
        Serial.print("[SLAVE] I2C address: 0x"); Serial.println(i2c_address_, HEX);

        loadDeviceName();
        Serial.print("[SLAVE] Device name: "); Serial.println(device_name_);

        min_pwm_ = EEPROM.read(EEPROM_MIN_PWM);
        max_pwm_ = EEPROM.read(EEPROM_MAX_PWM);
        enforcePwmBounds();
        Serial.print("[SLAVE] Min PWM: "); Serial.println(min_pwm_);
        Serial.print("[SLAVE] Max PWM: "); Serial.println(max_pwm_);

        pinMode(in1_, OUTPUT); pinMode(in2_, OUTPUT);
        pinMode(in3_, OUTPUT); pinMode(in4_, OUTPUT);

        analogWriteResolution(en1_, PWM_RESOLUTION_BITS);
        analogWriteResolution(en2_, PWM_RESOLUTION_BITS);
        analogWriteFrequency(en1_, 5000);
        analogWriteFrequency(en2_, 5000);

        Wire.onReceive(receiveEventStatic);
        Wire.onRequest(requestEventStatic);
        Wire.begin(i2c_address_, sda, scl, freq);

        instance_ = this;

        Serial.println("[SLAVE] Ready for I2C commands.");
    }

    void setMotorSpeed(uint8_t motor, uint8_t percent) {
        percent = constrain(percent, 0, 100);
        uint16_t duty = map(percent, 0, 100, min_pwm_, max_pwm_);
        if (motor == 1) analogWrite(en1_, duty);
        else if (motor == 2) analogWrite(en2_, duty);
        Serial.print("[SLAVE] Motor "); Serial.print(motor); Serial.print(" speed set to "); Serial.print(percent); Serial.print("% (PWM="); Serial.print(duty); Serial.println(")");
    }

    void setMotorDirection(uint8_t motor, uint8_t dir) {
        if (motor == 1) {
            digitalWrite(in1_, dir & 0x01);
            digitalWrite(in2_, (dir >> 1) & 0x01);
        } else if (motor == 2) {
            digitalWrite(in3_, dir & 0x01);
            digitalWrite(in4_, (dir >> 1) & 0x01);
        }
        Serial.print("[SLAVE] Motor "); Serial.print(motor); Serial.print(" direction set to "); Serial.println(dir, BIN);
    }

    void emergencyStop() {
        analogWrite(en1_, 0);
        analogWrite(en2_, 0);
        digitalWrite(in1_, LOW); digitalWrite(in2_, LOW);
        digitalWrite(in3_, LOW); digitalWrite(in4_, LOW);
        Serial.println("[SLAVE] Emergency stop!");
    }

private:

    uint8_t en1_, in1_, in2_, in3_, in4_, en2_;
    uint8_t i2c_address_;
    volatile bool id_requested_;
    char id_response_[64];
    char device_name_[DEVICENAME_MAXLEN + 1];
    uint16_t min_pwm_, max_pwm_;

    static ESP32MotorI2C* instance_;

    void enforcePwmBounds() {
        if (min_pwm_ > PWM_RESOLUTION_MAX) min_pwm_ = 0;
        if (max_pwm_ > PWM_RESOLUTION_MAX) max_pwm_ = PWM_RESOLUTION_MAX;
        if (min_pwm_ > max_pwm_) min_pwm_ = max_pwm_;
    }

    void loadDeviceName() {
        uint8_t len = EEPROM.read(1);
        if (len > DEVICENAME_MAXLEN) len = DEVICENAME_MAXLEN;
        for (uint8_t i = 0; i < len; ++i)
            device_name_[i] = EEPROM.read(2 + i);
        device_name_[len] = '\0';
        if (len == 0 || device_name_[0] == 0xFF || device_name_[0] == 0x00)
            strncpy(device_name_, "dc motor", DEVICENAME_MAXLEN);
    }

    void saveDeviceName(const char* newname, uint8_t len) {
        if (len > DEVICENAME_MAXLEN) len = DEVICENAME_MAXLEN;
        EEPROM.write(1, len);
        for (uint8_t i = 0; i < len; ++i)
            EEPROM.write(2 + i, newname[i]);
        EEPROM.commit();
        strncpy(device_name_, newname, len);
        device_name_[len] = '\0';
        Serial.print("[SLAVE] Device name changed to: "); Serial.println(device_name_);
    }

    void saveMinMaxPWM(uint16_t minv, uint16_t maxv) {
        if (minv > PWM_RESOLUTION_MAX) minv = 0;
        if (maxv > PWM_RESOLUTION_MAX) maxv = PWM_RESOLUTION_MAX;
        if (minv > maxv) minv = maxv;
        EEPROM.write(EEPROM_MIN_PWM, minv);
        EEPROM.write(EEPROM_MAX_PWM, maxv);
        EEPROM.commit();
        min_pwm_ = minv;
        max_pwm_ = maxv;
        Serial.print("[SLAVE] Min PWM set to: "); Serial.println(min_pwm_);
        Serial.print("[SLAVE] Max PWM set to: "); Serial.println(max_pwm_);
    }

    void receiveEvent(int numBytes) {
        if (numBytes < 1) return;
        uint8_t cmd = Wire.read();
        if (cmd == 'i') {
            memset(id_response_, 0, sizeof(id_response_));
            snprintf(id_response_, sizeof(id_response_), "%02X,dc motor,%s,%u,%u", i2c_address_, device_name_, min_pwm_, max_pwm_);
            id_requested_ = true;
            Serial.println("[SLAVE] Identification requested.");
            return;
        }
        switch (cmd) {
            case 0x01: // Change address
                if (Wire.available()) {
                    uint8_t new_addr = Wire.read();
                    EEPROM.write(0, new_addr);
                    EEPROM.commit();
                    Serial.print("[SLAVE] Address changed to 0x"); Serial.println(new_addr, HEX);
                    delay(100);
                    ESP.restart();
                }
                break;
            case 0x02: // Set speed
                if (Wire.available() >= 2) {
                    uint8_t motor = Wire.read();
                    uint8_t percent = Wire.read();
                    setMotorSpeed(motor, percent);
                }
                break;
            case 0x03: // Set direction
                if (Wire.available() >= 2) {
                    uint8_t motor = Wire.read();
                    uint8_t dir = Wire.read();
                    setMotorDirection(motor, dir);
                }
                break;
            case 0x05: // Emergency stop
                emergencyStop();
                break;
            case 0x06: // Set device name (variable length)
                if (Wire.available() >= 1) {
                    uint8_t len = Wire.read();
                    char newname[DEVICENAME_MAXLEN + 1];
                    uint8_t i = 0;
                    while (Wire.available() && i < len && i < DEVICENAME_MAXLEN) {
                        newname[i++] = Wire.read();
                    }
                    newname[i] = '\0';
                    saveDeviceName(newname, i);
                }
                break;
            case 0x07: // Set min/max PWM
                if (Wire.available() >= 2) {
                    uint16_t minv = Wire.read();
                    uint16_t maxv = Wire.read();
                    saveMinMaxPWM(minv, maxv);
                }
                break;
        }
    }

    void requestEvent() {
        if (id_requested_) {
            size_t len = strnlen(id_response_, sizeof(id_response_));
            Wire.write((uint8_t*)id_response_, len);
            id_requested_ = false;
        }
    }

    static void receiveEventStatic(int numBytes) {
        if (instance_) instance_->receiveEvent(numBytes);
    }

    static void requestEventStatic() {
        if (instance_) instance_->requestEvent();
    }

};

ESP32MotorI2C* ESP32MotorI2C::instance_ = nullptr;

#endif
