#include "AHT2x.h"

#define AHT_STATUS_BUSY 0x01
#define AHT_STATUS_CALIBRATED 0x10
#define AHT_CMD_INIT 0xBE
#define AHT_CMD_TRIGGER 0xAC
#define AHT_CMD_RESET 0xBA
#define AHT_CRC_POLYNOMIAL 0x31
#define AHT_CRC_MSB 0x80
#define AHT_CRC_INIT 0xFF


#ifndef DPRINTF
#define DPRINTF(...)
#endif

AHT2x aht25(Wire);
AHT2x aht10_0x39(Wire);

const uint8_t cmd_init[3] = { 0xBE, 0x08, 0x00 };

AHT2x::AHT2x(TwoWire &i2c) : _i2c(i2c) {};

bool AHT2x::setup(uint8_t address, bool crc) {
    m_nAddress = address;
    _active_crc = crc;
    m_nErrorCount = 0;
    m_nTemp = -9999;
    m_nRH = -9999;
    bRequested = false;
    m_bSensor = false;
    m_bScan = false;
    tickRequested = millis();
    delay(40);

    if (scan()) {
        while (!isCalibrated()) {
            calibrate();
        }

        requestMeasurement(tickRequested);
        delay(82);
        humid[3] = -9999;
        temp[3] = -9999;
        if (readSensor(tickRequested + 82)) {
            for (int i = 0; i < 3; i++) {
                humid[i] = humid[3];
                temp[i] = temp[3];
            }
        }
        return true;
    }
    return false;
}

bool AHT2x::scan() {
    //DPRINTF("AHTx0 - Scanning AHTx0 Device at %X\n", m_nAddress);
    int i;
    for (i = 0; i < 5; i++) {
        Wire.beginTransmission(m_nAddress);
        if (Wire.endTransmission() == 0) {
            DPRINTF("AHTx0 - FOUND I2C Device at %X\n", m_nAddress);
            bool ret = false;
            //sendCommand(m_nAddress, 0xBE, 0x08, 0x00)) { // AHT20 init command
            uint8_t cmd[3] = { 0xBE, 0x08, 0x00 };
            _i2c.beginTransmission(m_nAddress);
            _i2c.write((uint8_t *)cmd_init, 3);
            if (_i2c.endTransmission() == 0) {
                m_bSensor = true;
                DPRINTF("AHT2x - Found AHT2x at %X\n", m_nAddress);
                break;
            }
        }
        delay(2);
    }
    if (i == 5) {
        DPRINTF("AHTx0 - I2C Transmission Error at %X\n", m_nAddress);
    }
    return m_bSensor;
}

bool AHT2x::isReady() {
    if (status() & AHT_STATUS_BUSY) {
        return false;
    }
    return measure();
}

bool AHT2x::isCalibrated() {
    return status() & AHT_STATUS_CALIBRATED;
}

void AHT2x::reset() {
    uint8_t cmd = AHT_CMD_RESET;
    _i2c.beginTransmission(m_nAddress);
    _i2c.write(cmd);
    _i2c.endTransmission();
    delay(20);
    while (!isCalibrated()) {
        calibrate();
    }
}

uint8_t AHT2x::status() {
    _i2c.requestFrom(m_nAddress, (uint8_t)6);
    for (int i = 0; i < 6; i++) {
        _buf[i] = _i2c.read();
    }
    if (!_active_crc || (crc8() == _buf[6])) {
        return _buf[0];
    }
    return AHT_STATUS_BUSY;
}

void AHT2x::calibrate() {
    uint8_t cmd[] = {AHT_CMD_INIT, 0x08, 0x00};
    _i2c.beginTransmission(m_nAddress);
    _i2c.write(cmd, 3);
    _i2c.endTransmission();
    delay(10);
}

void AHT2x::requestMeasurement(unsigned long tick) {
    uint8_t cmd[] = {AHT_CMD_TRIGGER, 0x33, 0x00};
    _i2c.beginTransmission(m_nAddress);
    _i2c.write(cmd, 3);
    _i2c.endTransmission();
    tickRequested = tick;
    bRequested = true;
}

bool AHT2x::readSensor(unsigned long tick) {
    if (m_bScan) {
        // Re-Scan sensor for any hardware change or failure
        scan();
        m_bScan = false;
    }

    bool ret = false;
    if (bRequested) {
        if (tick - tickRequested > 80) {
            _i2c.requestFrom(m_nAddress, (uint8_t)6);
            ret = true;
            for (int i = 0; i < 6; i++) {
                if (Wire.available()) {
                    _buf[i] = _i2c.read();
                } else {
                    ret = false;  // If data is unavailable, return false
                    break;
                }
            }

            if (ret && (!_active_crc || (crc8() == _buf[6]))) {
                humid[0] = humid[1];
                humid[1] = humid[2];
                humid[2] = humid[3];
                uint32_t hum32 = (_buf[1] << 12) | (_buf[2] << 4) | (_buf[3] >> 4);
                humid[3]  = (int16_t) roundf(hum32 * 10000.0f / 0x100000);
                if (m_nRH > 2000 && m_nRH <= 10000) {
                    if (humid[3] - m_nRH < -1500 || humid[3] - m_nRH> 1500)
                        humid[3] = m_nRH;
                }
                m_nRH = (int16_t)((humid[0] + humid[1] + humid[2] + humid[3]) / 4); 

                temp[0] = temp[1];
                temp[1] = temp[2];
                temp[2] = temp[3];
                uint32_t temp32 = ((_buf[3] & 0xF) << 16) | (_buf[4] << 8) | _buf[5];
                temp[3] = (int16_t) roundf(temp32 * 20000.0f / 0x100000 - 5000);
                if (m_nTemp > 2000 && m_nTemp <= 3000) {
                    if (temp[3] - m_nTemp < -1000 || temp[3] - m_nTemp> 1000)
                        temp[3] = m_nTemp;
                } 
                m_nTemp = (int16_t)((temp[0] + temp[1] + temp[2] + temp[3]) / 4);
                m_nErrorCount = 0; 
                ret = true;
            } else {
                if (++m_nErrorCount > 4) {
                    m_nTemp = -9999;
                    m_nRH = -9999;
                    m_nErrorCount = 0;
                }
            }
        } else {
            // tick - tickRequested <= 80
            // do nothing;
            return false;
        }
    }
    requestMeasurement(tick);
    return ret;
}

uint8_t AHT2x::crc8() {
    uint8_t crc = AHT_CRC_INIT;
    for (int i = 0; i < 6; i++) {
        crc ^= _buf[i];
        for (int j = 0; j < 8; j++) {
            if (crc & AHT_CRC_MSB) {
                crc = (crc << 1) ^ AHT_CRC_POLYNOMIAL;
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}