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

#define EEPROM_SIZE        64
#define EEPROM_ADDR_LOC    0
#define EEPROM_NAME_LOC    1
#define DEVICE_NAME_LEN    24
#define DEFAULT_I2C_ADDR   0x08

// Motor pins
#define EN1 0
#define IN1 1
#define IN2 2
#define IN3 4
#define IN4 5
#define EN2 6

char deviceName[DEVICE_NAME_LEN] = "Motor";
uint8_t i2cAddress = DEFAULT_I2C_ADDR;

// I2C command codes
enum Command : uint8_t {
  CMD_SET_ADDR    = 0x01,
  CMD_SET_NAME    = 0x02,
  CMD_MOTOR_CTRL  = 0x10,
  CMD_BOTH_CTRL   = 0x11,
};

// Store address to EEPROM
void storeAddressToEEPROM(uint8_t addr) {
  EEPROM.write(EEPROM_ADDR_LOC, addr);
  EEPROM.commit();
}

// Read address from EEPROM (with fallback to default)
uint8_t readAddressFromEEPROM() {
  uint8_t addr = EEPROM.read(EEPROM_ADDR_LOC);
  if (addr == 0xFF || addr == 0x00) { // Uninitialized or invalid
    addr = DEFAULT_I2C_ADDR;
    storeAddressToEEPROM(addr);
  }
  return addr;
}

void storeNameToEEPROM(const char* name) {
  for (int i = 0; i < DEVICE_NAME_LEN; i++) {
    EEPROM.write(EEPROM_NAME_LOC + i, name[i]);
    if (name[i] == '\0') break;
  }
  EEPROM.commit();
}

void readNameFromEEPROM(char* name) {
  for (int i = 0; i < DEVICE_NAME_LEN; i++) {
    name[i] = EEPROM.read(EEPROM_NAME_LOC + i);
    if (name[i] == '\0') break;
  }
  name[DEVICE_NAME_LEN - 1] = '\0';
}

// Helper: Set motor speed and direction
void setMotor(uint8_t enPin, uint8_t in1, uint8_t in2, uint8_t speed, uint8_t dir) {
  analogWrite(enPin, speed);
  if (dir == 0) {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
  } else {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
  }
}

// Stop motor
void stopMotor(uint8_t enPin, uint8_t in1, uint8_t in2) {
  analogWrite(enPin, 0);
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
}

// I2C receive handler
void onReceive(int len) {
  if (len < 1) return;
  uint8_t cmd = Wire.read();
  switch (cmd) {
    case CMD_SET_ADDR: 
      if (Wire.available()) {
        uint8_t newAddr = Wire.read();
        if (newAddr > 0x00 && newAddr < 0x78) { // Valid 7-bit address
          storeAddressToEEPROM(newAddr);
          ESP.restart(); // Or NVIC_SystemReset() on STM32, or just reset the MCU as needed
        }
      }
      break;
    case CMD_SET_NAME: {
      int i = 0;
      while (Wire.available() && i < DEVICE_NAME_LEN - 1) {
        deviceName[i++] = Wire.read();
      }
      deviceName[i] = '\0';
      storeNameToEEPROM(deviceName);
      break;
    }
    case CMD_MOTOR_CTRL: {
      if (Wire.available() >= 3) {
        uint8_t motor_id = Wire.read(); // 1: MotorA, 2: MotorB
        uint8_t speed_pct = Wire.read(); // 0-100
        uint8_t dir = Wire.read(); // 0: FWD, 1: REV
        uint8_t pwm = map(speed_pct, 0, 100, 0, 255);

        if (motor_id == 1) { // MotorA
          setMotor(EN1, IN1, IN2, pwm, dir);
        } else if (motor_id == 2) { // MotorB
          setMotor(EN2, IN3, IN4, pwm, dir);
        }
      }
      break;
    }
    case CMD_BOTH_CTRL: {
      if (Wire.available() >= 4) {
        uint8_t speedA = Wire.read();
        uint8_t dirA = Wire.read();
        uint8_t speedB = Wire.read();
        uint8_t dirB = Wire.read();
        uint8_t pwmA = map(speedA, 0, 100, 0, 255);
        uint8_t pwmB = map(speedB, 0, 100, 0, 255);

        setMotor(EN1, IN1, IN2, pwmA, dirA);
        setMotor(EN2, IN3, IN4, pwmB, dirB);
      }
      break;
    }
    default: break;
  }
}

// I2C request handler
void onRequest() {
  String response = String(i2cAddress, HEX) + ",GENERIC," + deviceName;
  Wire.write((const uint8_t*)response.c_str(), response.length());
}

void setup() {
  Serial.begin(115200);
  EEPROM.begin(EEPROM_SIZE);

  i2cAddress = readAddressFromEEPROM();
  readNameFromEEPROM(deviceName);

  // Motor pins setup
  pinMode(EN1, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  Wire.begin(i2cAddress);
  Wire.onReceive(onReceive);
  Wire.onRequest(onRequest);

  Serial.print("I2C Slave started at address 0x");
  Serial.println(i2cAddress, HEX);
  Serial.print("Device name: ");
  Serial.println(deviceName);
}

void loop() {
  // Nothing here
}