#include <Wire.h>
#include "MS5837.h"

MS5837 sensor;

const int pwmPin = 9; // PWM output pin

void setup() {
  Serial.begin(9600);
  Wire.begin();

  sensor.setModel(MS5837::MS5837_02BA); // Change to MS5837_02BA
  sensor.init();
  while (!sensor.init()) {
    Serial.println("Init failed!");
    Serial.println("Are SDA/SCL connected correctly?");
    Serial.println("Blue Robotics Bar02: White=SDA, Green=SCL");
    delay(5000);
  }

  sensor.setFluidDensity(997); // kg/m^3 (freshwater, 1029 for seawater)

  pinMode(pwmPin, OUTPUT); // Set the PWM pin as an output
}

void loop() {
  // Update pressure and temperature readings
  sensor.read();

  // Get depth in meters
  float depth = sensor.depth();

  Serial.print("Depth: ");
  Serial.print(depth);
  Serial.println(" m");

  // Convert depth to a delay in microseconds
  int delayMicros = depth * 1000.0; // 10 us/cm is 1000 us/m
  
  // Manual digital write-based PWM
  digitalWrite(pwmPin, HIGH);
  delayMicroseconds(delayMicros);
  digitalWrite(pwmPin, LOW);

  delay(200); // Delay to achieve 5 Hz update rate
}