#include <Wire.h>

#define AL4_I2C_ADDRESS 0x68

#define MEASUREMENT_COMMAND 0x30A2
#define READ_DATA_COMMAND 0x3000

#define ALPHA_FOR_10KPA (7373.0 / 10.0)
#define BETA 8192.0
#define TEMP_OFFSET -50.0
#define TEMP_MULTIPLIER (200.0 / 2047.0)

struct SensorData {
  int16_t pressure_counts;
  int16_t temperature_counts;
  float pressure_kPa;
  float temperature_C;
};

void readSensorData(SensorData* data);
bool al4_begin(uint8_t address);

void setup() {
  Serial.begin(115200);
  Serial.println("Fujikura AL4 Sensor Code (Optimized)");
  Wire.begin();

  if (!al4_begin(AL4_I2C_ADDRESS)) {
    Serial.println("AL4 sensor not detected. Check wiring and address. Freezing...");
    while (1);
  }
}

void loop() {
  SensorData sensorData;

  Wire.beginTransmission(AL4_I2C_ADDRESS);
  Wire.write((MEASUREMENT_COMMAND >> 8) & 0xFF);
  Wire.write(MEASUREMENT_COMMAND & 0xFF);
  Wire.endTransmission();

  delay(1);

  readSensorData(&sensorData);

  Serial.print("Raw Pressure Counts: ");
  Serial.print(sensorData.pressure_counts);
  Serial.print("\tRaw Temperature Counts: ");
  Serial.println(sensorData.temperature_counts);

  Serial.print("Pressure: ");
  Serial.print(sensorData.pressure_kPa, 4);
  Serial.print(" kPa \tTemperature: ");
  Serial.print(sensorData.temperature_C, 2);
  Serial.println(" °C");

  delay(1000);
}

bool al4_begin(uint8_t address) {
  Wire.beginTransmission(address);
  return Wire.endTransmission() == 0;
}

void readSensorData(SensorData* data) {
  Wire.requestFrom(AL4_I2C_ADDRESS, 4);

  if (Wire.available() >= 4) {
    uint8_t pressure_high_byte = Wire.read();
    uint8_t pressure_low_byte = Wire.read();
    uint8_t temperature_high_byte = Wire.read();
    uint8_t temperature_low_byte = Wire.read();

    data->pressure_counts = (int16_t)((pressure_high_byte << 8) | pressure_low_byte);
    data->temperature_counts = (int16_t)((temperature_high_byte << 8) | temperature_low_byte);
    
    data->pressure_kPa = ((float)data->pressure_counts - BETA) / ALPHA_FOR_10KPA;

    data->temperature_C = ((float)data->temperature_counts * TEMP_MULTIPLIER) + TEMP_OFFSET;
    
  } else {
    data->pressure_counts = -1;
    data->temperature_counts = -1;
    data->pressure_kPa = 0.0;
    data->temperature_C = 0.0;
    Serial.println("Error: Not enough data received from sensor.");
  }
}