#include "modules/my_altimeter/Altimeter.h"
#include "modules/my_task_creator/TaskCreator.h"

#define INVALID_FRAME 0x61A8 

uint8_t Altimiter::address = 0x66; // Initializing the default I2C address
double Altimiter::distance = 0.0; // Initializing the distance variable

void Altimiter::AltimiterRxTask(void *pvParameters)
{
    log_v("Task Started"); // Logging task start
    Wire.begin(SDA_PIN, SCL_PIN); // Initializing Wire library
    Wire.setBufferSize(32); // Setting buffer size for Wire communication
    TaskCreator *mCreator = (TaskCreator *)pvParameters; // Getting TaskCreator object
    Altimiter *obj = &(mCreator->mAltimiter); // Getting Altimiter object from TaskCreator
    MavlinkTransmitter* transmitter = &(mCreator->mTransmitter);

    if(!obj->scan(obj->address))
    {
        vTaskDelete(NULL);
    }

    vTaskDelay(50); // Delay to allow initialization
    uint32_t mCurrentTick=xTaskGetTickCount();

    for (;;)
    {
        bool isValid = false;

        Wire.requestFrom(static_cast<uint16_t>(obj->address), static_cast<size_t>(2), true);

        while (Wire.available() >= 2) {
            byte byteH = Wire.read();
            byte byteL = Wire.read();

            uint16_t raw = (byteH << 8) | byteL;

            if (raw == INVALID_FRAME) {
                isValid = false;
                continue;
            }

            obj->distance = raw / 100.0f;
            isValid = true;
        }

        uint32_t t = xTaskGetTickCount();
        if (isValid && (t - mCurrentTick >= 100)) {
            transmitter->SendDistanceSensor(obj->distance, isValid, (float)RANGEFINDER_SF30_MAX_DISTANCE);
            mCurrentTick = t;
        }

        vTaskDelay(10);
    }
}

bool Altimiter::scan(byte adress)
{
    Wire.beginTransmission(adress);
    byte error = Wire.endTransmission();

    if (error == 0)
    {
        log_i("Altimiter found");
        return true;
    }
    else
    {
        log_i("Altimiter not found");
        return false;
    }
}