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

#define SHT41_ADDR_44 0x44          // Address specified in W10 schematic
#define SHT41_ADDR_45 0x45          // Alternative address (factory variation)
#define I2C_SDA_PIN 8              // SDA=8 (Shared with IMU, bus confirmed idle)
#define I2C_SCL_PIN 7              // SCL=7
#define I2C_CLOCK_SPEED 10000       // Stable 10kHz clock speed
#define POWER_UP_DELAY 5000         // 5-second power-up delay (ensure stable power supply)
#define COMM_RETRY_COUNT 3          // 3 communication retries
#define MEASURE_DELAY 200           // Extended measurement wait time

// SHT41 Commands 
#define SHT41_CMD_MEASURE_HIGH 0xFD // High precision measurement
#define SHT41_CMD_SOFT_RESET 0x94   // Soft reset (restore default state)

// Data Storage 
struct SHT41Data {
  float temperature;
  float humidity;
  bool isValid;
  uint8_t detectedAddr; // Detected I2C address
} sht41Data;

//  I2C Address Scan (Critical! Verify devices on I2C bus) 
void scanI2CAddresses() {
  sht41Data.detectedAddr = 0;
  Serial.println("\nScanning I2C bus for SHT41...");
  
  for (uint8_t addr = 0x40; addr < 0x48; addr++) { // Focus on SHT41 possible address range
    Wire.beginTransmission(addr);
    uint8_t ret = Wire.endTransmission();
    if (ret == 0) {
      Serial.printf("I2C device found at address 0x%02X\n", addr);
      sht41Data.detectedAddr = addr;
    }
  }
  
  if (sht41Data.detectedAddr == 0) {
    Serial.println("No SHT41-like device found on I2C bus!");
  } else {
    Serial.printf("Detected SHT41 address: 0x%02X\n", sht41Data.detectedAddr);
  }
}

// I2C Initialization (Enhanced pull-up + stable configuration)
void initI2C() {
  // Force enable internal pull-up (assist external pull-up, enhance signal)
  pinMode(I2C_SDA_PIN, INPUT_PULLUP);
  pinMode(I2C_SCL_PIN, INPUT_PULLUP);
  delay(200);

  Wire.end();
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  Wire.setClock(I2C_CLOCK_SPEED);
  Wire.setTimeout(1000000); // 1-second timeout, adapt to low-speed communication
  delay(300);
  Serial.printf("I2C initialized (Clock: %dkHz)\n", I2C_CLOCK_SPEED / 1000);
  
  // Scan I2C addresses
  scanI2CAddresses();
}

//Sensor Reset (Restore default state) 
bool resetSHT41(uint8_t addr) {
  Wire.beginTransmission(addr);
  Wire.write(SHT41_CMD_SOFT_RESET);
  uint8_t ret = Wire.endTransmission();
  if (ret == 0) {
    Serial.println("SHT41 reset successful");
    delay(500); // Extended wait time after reset
    return true;
  } else {
    Serial.printf("SHT41 reset failed (NACK: %d)\n", ret);
    return false;
  }
}

//Read SHT41 (Dual-address compatible + retry mechanism)
void readSHT41() {
  uint8_t dataBuffer[6];
  sht41Data.isValid = false;
  uint8_t targetAddr = (sht41Data.detectedAddr != 0) ? sht41Data.detectedAddr : SHT41_ADDR_44;

  //Use scanned address first, try dual addresses if none detected
  if (sht41Data.detectedAddr == 0) {
    Serial.println("\nTrying address 0x44...");
    if (!resetSHT41(SHT41_ADDR_44)) {
      Serial.println("Trying address 0x45...");
      targetAddr = SHT41_ADDR_45;
      if (!resetSHT41(targetAddr)) {
        Serial.println("Failed to reset SHT41 at both addresses");
        return;
      }
    }
  }

  //Send measurement command
  bool cmdSent = false;
  for (int retry = 0; retry < COMM_RETRY_COUNT; retry++) {
    Wire.beginTransmission(targetAddr);
    Wire.write(SHT41_CMD_MEASURE_HIGH);
    uint8_t ret = Wire.endTransmission();
    
    if (ret == 0) {
      Serial.printf("Measure command sent (Addr: 0x%02X, Retry: %d)\n", targetAddr, retry);
      cmdSent = true;
      break;
    } else {
      Serial.printf("Retry %d: Command failed (Addr: 0x%02X, NACK: %d)\n", retry + 1, targetAddr, ret);
      delay(100);
    }
  }
  if (!cmdSent) {
    return;
  }

  //Wait for measurement completion
  delay(MEASURE_DELAY);

  //Read data
  int readCount = Wire.requestFrom(targetAddr, 6);
  if (readCount != 6) {
    Serial.printf("Incomplete data (Received: %d/6 bytes)\n", readCount);
    return;
  }

  //Read buffer
  for (int i = 0; i < 6; i++) {
    dataBuffer[i] = Wire.read();
  }

  //CRC check
  uint8_t tempCrc = crc8(dataBuffer, 2);
  uint8_t humCrc = crc8(dataBuffer + 3, 2);
  if (tempCrc != dataBuffer[2] || humCrc != dataBuffer[5]) {
    Serial.println("CRC check failed");
    return;
  }

  //Calculate results
  uint16_t tempRaw = (dataBuffer[0] << 8) | dataBuffer[1];
  uint16_t humRaw = (dataBuffer[3] << 8) | dataBuffer[4];
  sht41Data.temperature = -45.0 + 175.0 * (float)tempRaw / 65535.0;
  sht41Data.humidity = 0.0 + 100.0 * (float)humRaw / 65535.0;
  sht41Data.isValid = true;
}

//CRC8 Calculation
uint8_t crc8(const uint8_t* data, uint8_t len) {
  const uint8_t polynomial = 0x31;
  uint8_t crc = 0xFF;
  for (uint8_t i = 0; i < len; i++) {
    crc ^= data[i];
    for (uint8_t j = 0; j < 8; j++) {
      crc = (crc & 0x80) ? (crc << 1) ^ polynomial : (crc << 1);
    }
  }
  return crc;
}

//Print Data
void printSHT41Data() {
  Serial.println("\n==================== SHT41 Data ====================");
  if (sht41Data.isValid) {
    Serial.printf("Detected Address: 0x%02X\n", sht41Data.detectedAddr);
    Serial.print("Temperature: "); Serial.print(sht41Data.temperature, 2); Serial.println(" °C");
    Serial.print("Humidity:    "); Serial.print(sht41Data.humidity, 2); Serial.println(" %RH");
  } else {
    Serial.println("Status: No Valid Data (SHT41 not responding)");
  }
  Serial.println("==================================================\n");
}

//Setup
void setup() {
  Serial.begin(115200);
  delay(10);
  Serial.println("=== W10 SHT41 Address Scan & Response Test ===");

  // Extra-long delay for power stabilization (resolve hidden power supply issues)
  Serial.printf("Waiting %dms for power stabilization...\n", POWER_UP_DELAY);
  delay(POWER_UP_DELAY);

  // Initialize I2C (including address scan)
  initI2C();

  // Read SHT41
  readSHT41();
  printSHT41Data();
}

//Loop
void loop() {
  readSHT41();
  printSHT41Data();
  delay(3000);
}