// This file can be used directly in Wokwi for developing the buoyancy chip
#include <Wire.h>

const uint32_t PROX_ADDRESS = 0x60;
const uint32_t DEPTH_ADDRESS = 0x76;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Wire.begin(45, 42);
  Wire1.begin(4, 5);
  Serial.println("ESP32-S2 Setup Complete");
}

void loop() {

  // Prox i2c
  Wire.beginTransmission(PROX_ADDRESS);
  Wire.write(0x08);

  if (Wire.endTransmission(false) != 0) //Send a restart command. Do not release bus.
  {
      Serial.println("prox: end transmission fail");
  }

  Wire.requestFrom(PROX_ADDRESS, 2);
  if (Wire.available())
  {
    uint8_t lsb = Wire.read();
    uint8_t msb = Wire.read();
    uint16_t response = ((uint16_t)msb << 8 | lsb);
    Serial.println("prox:");
    Serial.println(response);
  }

  // Depth i2c
  Wire1.beginTransmission(DEPTH_ADDRESS);
  Wire1.write(0x00);

  if (Wire1.endTransmission(false) != 0) //Send a restart command. Do not release bus.
  {
      Serial.println("depth: end transmission fail");
  }

  Wire1.requestFrom(DEPTH_ADDRESS, 4);
  if (Wire1.available())
  {
    uint8_t b1 = Wire1.read();
    uint8_t b2 = Wire1.read();
    uint8_t b3 = Wire1.read();
    uint8_t b4 = Wire1.read();

    uint8_t bytes[4] = { b1, b2, b3, b4 };

    float response;
    memcpy(&response, bytes, sizeof(response));

    Serial.println("depth:");
    Serial.println(response);
  }
  

  delay(500);
}