#include "fgc/sensors/Sht41Protocol.h"

namespace fgc {

uint8_t sht41Crc8(const uint8_t* data, size_t len) {
    uint8_t crc = 0xFF;
    for (size_t i = 0; i < len; ++i) {
        crc ^= data[i];
        for (int b = 0; b < 8; ++b)
            crc = (crc & 0x80) ? static_cast<uint8_t>((crc << 1) ^ 0x31)
                               : static_cast<uint8_t>(crc << 1);
    }
    return crc;
}

std::optional<Sht41Reading> decodeSht41Measurement(const std::vector<uint8_t>& rx) {
    if (rx.size() != kSht41ResponseBytes) return std::nullopt;
    if (sht41Crc8(rx.data(), 2) != rx[2] || sht41Crc8(rx.data() + 3, 2) != rx[5])
        return std::nullopt;

    const uint16_t traw  = static_cast<uint16_t>((rx[0] << 8) | rx[1]);
    const uint16_t rhraw = static_cast<uint16_t>((rx[3] << 8) | rx[4]);

    Sht41Reading out;
    out.temp_c       = -45.0f + 175.0f * (static_cast<float>(traw) / 65535.0f);
    out.humidity_pct = -6.0f + 125.0f * (static_cast<float>(rhraw) / 65535.0f);
    if (out.humidity_pct < 0.0f) out.humidity_pct = 0.0f;
    if (out.humidity_pct > 100.0f) out.humidity_pct = 100.0f;
    return out;
}

}  // namespace fgc