#include <BitBang_I2C.h>
#include <ArduinoJson.h>

#define SDA_PIN 22
#define SCL_PIN 23

BBI2C bbi2c;

void setup() {
  Serial.begin(115200);
  memset(&bbi2c, 0, sizeof(bbi2c));
  bbi2c.bWire = false;
  bbi2c.iSDA = SDA_PIN;
  bbi2c.iSCL = SCL_PIN;
  I2CInit(&bbi2c, 100000L);
  delay(100);

  // Perform I2C scan automatically
  performI2CScan();
  Serial.println("Waiting for AI Serial message...");
}

void loop() {
  if (Serial.available() > 0) {
    StaticJsonDocument<256> doc;
    deserializeJson(doc, Serial);
    
    const char* action = doc["action"];
    if (strcmp(action, "scan") == 0) {
      performI2CScan();
    } else if (strcmp(action, "read") == 0) {
      int deviceAddress = doc["deviceAddress"];
      uint8_t registerAddress = doc["registerAddress"];
      uint8_t data;
      
      Serial.println("Starting I2C read...");
      if (I2CRead(&bbi2c, deviceAddress, &registerAddress, 1)) {
        Serial.println("I2C read completed.");
        StaticJsonDocument<128> response;
        response["status"] = "success";
        response["data"] = data;
        serializeJson(response, Serial);
      } else {
        Serial.println("I2C read failed.");
        StaticJsonDocument<128> response;
        response["status"] = "error";
        response["message"] = "Failed to read from device";
        serializeJson(response, Serial);
      }
    } else if (strcmp(action, "write") == 0) {
      int deviceAddress = doc["deviceAddress"];
      uint8_t registerAddress = doc["registerAddress"];
      uint8_t value = doc["value"];
      
      Serial.println("Starting I2C write...");
      if (I2CWrite(&bbi2c, deviceAddress, &registerAddress, 1)) {
        Serial.println("I2C write completed.");
        StaticJsonDocument<128> response;
        response["status"] = "success";
        serializeJson(response, Serial);
      } else {
        Serial.println("I2C write failed.");
        StaticJsonDocument<128> response;
        response["status"] = "error";
        response["message"] = "Failed to write to device";
        serializeJson(response, Serial);
      }
    } else {
      StaticJsonDocument<128> response;
      response["status"] = "error";
      response["message"] = "Invalid action";
      serializeJson(response, Serial);
    }
    Serial.println("Waiting for AI Serial message...");
  }
}

void performI2CScan() {
  Serial.println("Starting I2C scan...");
  StaticJsonDocument<1024> response;
  response["status"] = "success";
  JsonArray devices = response.createNestedArray("devices");
  
  uint8_t map[16];
  I2CScan(&bbi2c, map);
  Serial.println("I2C scan completed.");
  for (int i = 1; i < 128; i++) {
    if (map[i>>3] & (1 << (i & 7))) {
      int deviceAddress = i;
      char deviceType[32];
      uint32_t capabilityBits;
      int deviceTypeIndex = I2CDiscoverDevice(&bbi2c, deviceAddress, &capabilityBits);
      I2CGetDeviceName(deviceTypeIndex, deviceType);
      JsonObject device = devices.createNestedObject();
      device["address"] = deviceAddress;
      device["type"] = deviceType;
      device["capabilities"] = capabilityBits;
    }
  }
  serializeJson(response, Serial);
}