#include "modules/my_altimeter/GRF500.h"
#include "modules/my_task_creator/TaskCreator.h"
#include "modules/my_mavlink_tx/MavlinkTransmitter.h"

uint8_t GRF500::address = 0x66;
double GRF500::distance = 0.0;

GRF500::GRF500(uint8_t addr)
    : registerModeActivated(false), lastReadMs(0)
{
    address = addr;
}

bool GRF500::begin()
{
    log_v("GRF500 begin");
    Wire.begin(SDA_PIN, SCL_PIN); // SDA, SCL

    log_v("Scanning I2C bus for devices...");
    bool deviceFound = false;
    for (uint8_t addr = 1; addr < 127; ++addr) {
        if (scan(addr)) {
            log_i("I2C device found at 0x%02X", addr);
            deviceFound = true;
        }
    }
    if (!deviceFound) {
        log_i("No I2C devices found");
        return false;
    }

    // Verify GRF500 at configured address
    log_i("Checking GRF500 at address 0x%02X", address);
    if (!scan(address)) {
        log_i("GRF500 not responding at 0x%02X", address);
        return false;
    }
    log_i("GRF500 responsive at 0x%02X", address);
    vTaskDelay(pdMS_TO_TICKS(50));

    enableRegisterMode();
    registerModeActivated = checkRegisterMode();
    log_i("Register mode %s", registerModeActivated ? "OK" : "FAIL");
    if (!registerModeActivated) return false;

    readDistance();
    return true;
}

void GRF500::Task(void *pvParameters)
{
    log_v("GRF500 Task started");
    TaskCreator *creator = static_cast<TaskCreator*>(pvParameters);
    GRF500 *sensor = &(creator->mGRF500);
    MavlinkTransmitter *transmitter = &(creator->mTransmitter);

    if (!sensor->begin()) {
        log_i("GRF500 initialization failed");
        vTaskDelete(NULL);
        return;
    }

    log_i("GRF500 initialized successfully");
    uint32_t lastTick = xTaskGetTickCount();
    for (;;) {
        sensor->readMeasurements();
        bool valid = (distance > 0);

        uint32_t now = xTaskGetTickCount();
        if (valid && (now - lastTick >= 100)) {
            transmitter->SendDistanceSensor(distance, valid, (float)RANGEFINDER_GRF500_MAX_DISTANCE);
            lastTick = now;
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

void GRF500::update()
{
    uint32_t now = millis();
    if (now - lastReadMs < 100) return;
    lastReadMs = now;
    if (!registerModeActivated) return;

    readMeasurements();
}

bool GRF500::scan(uint8_t addr)
{
    Wire.beginTransmission(addr);
    return (Wire.endTransmission() == 0);
}

void GRF500::enableRegisterMode()
{
    Wire.beginTransmission(address);
    Wire.write((uint8_t)120);
    Wire.write((uint8_t)0xAA);
    Wire.write((uint8_t)0xAA);
    if (Wire.endTransmission() != 0) {
        log_i("Register-mode write error");
    }
}

bool GRF500::checkRegisterMode()
{
    Wire.beginTransmission(address);
    Wire.write((uint8_t)120);
    if (Wire.endTransmission() != 0) return false;
    if (Wire.requestFrom(address, (uint8_t)2) != 2) return false;

    uint8_t a = Wire.read();
    uint8_t b = Wire.read();
    return (a == 0xCC && b == 0);
}

void GRF500::readDistance()
{
    Wire.beginTransmission(address);
    Wire.write((uint8_t)27);
    if (Wire.endTransmission() != 0) {
        log_i("Distance write error");
        return;
    }
    if (Wire.requestFrom(address, (uint8_t)4) == 4) {
        uint32_t raw = 0;
        raw |= (uint32_t)Wire.read();
        raw |= (uint32_t)Wire.read() << 8;
        raw |= (uint32_t)Wire.read() << 16;
        raw |= (uint32_t)Wire.read() << 24;
        distance = raw / 100.0;
        log_i("Distance: %.2f m", distance);
    }
}

void GRF500::readMeasurements()
{
    Wire.beginTransmission(address);
    Wire.write((uint8_t)44);
    if (Wire.endTransmission() != 0) {
        log_i("Measurements write error");
        return;
    }
    if (Wire.requestFrom(address, (uint8_t)8) == 8) {
        int16_t first = Wire.read() | (Wire.read() << 8);
        int16_t strength1 = Wire.read() | (Wire.read() << 8);
        int16_t last = Wire.read() | (Wire.read() << 8);
        int16_t strength2 = Wire.read() | (Wire.read() << 8);

        distance = first / 10.0;
        // log_i("First=%d Last=%d S1=%d S2=%d", first, last, strength1, strength2);
    }
}