#include <Arduino.h>
#include <Wire.h>

const uint8_t SENSOR_ADDRESS = 0x10; // TFMini I2C Address
const uint8_t DISTANCE_DATA_LENGTH = 7; // Data length for distance data request
const uint8_t MAX_RETRY_COUNT = 3; // Maximum number of retries for reading data
const uint16_t UNRELIABLE_DATA = 0xFFFF; // Indicating unreliable data

struct SensorData {
  uint16_t distance;  // Measured distance
  uint16_t strength;  // Signal strength
  uint8_t rangeType;  // Range scale: 0 (short), 3 (intermediate), 7 (long)
  bool isValid;       // Flag to indicate if the data is valid
};

unsigned long readFrequencyMillis = 50; // Default read frequency: 50 ms

void setup() {
  Wire.begin();
  Serial.begin(115200);
  Serial.println("TFMini I2C Test");
}

void loop() {
  static unsigned long lastReadTime = 0; // Tracks the last read time
  unsigned long currentTime = millis(); // Current time

  if (currentTime - lastReadTime >= readFrequencyMillis) {
    SensorData data = tryReadSensorData(SENSOR_ADDRESS, MAX_RETRY_COUNT);

    if (data.isValid && data.distance != UNRELIABLE_DATA) {
      Serial.print("\tDist[");
      Serial.print(data.distance);
      Serial.print("]\tStrength[");
      Serial.print(data.strength);
      Serial.print("]\tMode[");
      Serial.print(data.rangeType);
      Serial.println("]");
    }

    lastReadTime = currentTime; // Update the last read time
  }
}

void setReadFrequency(unsigned long frequencyMillis) {
  readFrequencyMillis = frequencyMillis;
}

SensorData tryReadSensorData(uint8_t deviceAddress, uint8_t retries) {
  while (retries--) {
    SensorData data = readSensorData(deviceAddress);
    if (data.isValid) {
      return data; // Valid data received
    }
    delay(10); // Wait a bit before retrying
  }
  return SensorData{0, 0, 0, false}; // Return invalid data after exhausting retries
}

SensorData readSensorData(uint8_t deviceAddress) {
  SensorData data = {0, 0, 0, false};

  Wire.beginTransmission(deviceAddress);
  Wire.write(0x01); // MSB
  Wire.write(0x02); // LSB
  Wire.write(DISTANCE_DATA_LENGTH); // Request distance data
  if (Wire.endTransmission(false) != 0) {
    return data; // Sensor did not acknowledge
  }

  Wire.requestFrom(deviceAddress, DISTANCE_DATA_LENGTH);
  if (Wire.available() < DISTANCE_DATA_LENGTH) {
    return data; // Not enough data available
  }

  for (uint8_t i = 0; i < DISTANCE_DATA_LENGTH; i++) {
    uint8_t incoming = Wire.read();
    switch (i) {
      case 0:
        data.isValid = (incoming == 0x01);
        break;
      case 2:
        data.distance = incoming; // LSB of the distance
        break;
      case 3:
        data.distance |= incoming << 8; // MSB of the distance
        if (data.distance == UNRELIABLE_DATA) {
          data.isValid = false;
        }
        break;
      case 4:
        data.strength = incoming; // LSB of signal strength
        break;
      case 5:
        data.strength |= incoming << 8; // MSB of signal strength
        break;
      case 6:
        data.rangeType = incoming; // Range scale
        break;
    }
  }
  return data;
}