#include "NV7Rangefinder.h"
#include "modules/my_task_creator/TaskCreator.h"

#ifdef NV7RANGEFINDER_DISABLE_LOGS
  #define log_i(...)
  #define log_w(...)
  #define log_e(...)
#endif

NV7Rangefinder::NV7Rangefinder() {}

void NV7Rangefinder::singleMeasure(Stream& port) {
    const uint8_t cmd[] = {0xAE, 0xA7, 0x04, 0x00, 0x05, 0x09, 0xBC, 0xBE};
    port.write(cmd, sizeof(cmd));
    log_i("NV7Rangefinder: Single measure command sent");
}

void NV7Rangefinder::startContinuousMeasure(Stream& port) {
    const uint8_t cmd[] = {0xAE, 0xA7, 0x04, 0x00, 0x0E, 0x12, 0xBC, 0xBE};
    port.write(cmd, sizeof(cmd));
    log_i("NV7Rangefinder: Continuous measure command sent");
}

void NV7Rangefinder::stopContinuousMeasure(Stream& port) {
    const uint8_t cmd[] = {0xAE, 0xA7, 0x04, 0x00, 0x0F, 0x13, 0xBC, 0xBE};
    port.write(cmd, sizeof(cmd));
    log_i("NV7Rangefinder: Stop continuous measure command sent");
}

NV7Rangefinder::Measurement NV7Rangefinder::getLastMeasurement() {
    return lastMeasurement;
}

void NV7Rangefinder::TaskNV7Rangefinder(void* pvParameters) {
    TaskCreator* mCreator = (TaskCreator*)pvParameters;
    NV7Rangefinder* self = &(mCreator->mNV7);
    MavlinkTransmitter* transmitter = &(mCreator->mTransmitter);

    constexpr size_t PACKET_LEN = 27;
    uint8_t buffer[PACKET_LEN];
    size_t idx = 0;

    Stream* serial = nullptr;

#ifdef NV7RANGEFINDER_USE_SERIAL0
    Serial.begin(9600, SERIAL_8N1, 16, 17);
    serial = &Serial;
    log_i("NV7Rangefinder: Using Serial0 @ %lu", 9600);
#else
    SoftwareSerial softSerial;
    softSerial.begin(9600, SWSERIAL_8N1, 16, 17);
    serial = &softSerial;
    log_i("NV7Rangefinder: Using SoftwareSerial @ %lu", 9600);
#endif

    log_i("NV7Rangefinder: Read task started");
    self->startContinuousMeasure(*serial);
    vTaskDelay(10 / portTICK_PERIOD_MS);

    while (true) {
        while (serial->available()) {
            uint8_t b = serial->read();

            if (idx == 0 && b != 0xAE) continue;
            if (idx == 1 && b != 0xA7) { idx = 0; continue; }

            if (idx < PACKET_LEN) {
                buffer[idx++] = b;
            } else {
                log_w("Buffer overflow, reset idx");
                transmitter->SendDistanceSensor(0.0f, false, (float)RANGEFINDER_NV7_MAX_DISTANCE);
                idx = 0;
                continue;
            }

            if (idx == PACKET_LEN) {
                if (buffer[25] != 0xBC || buffer[26] != 0xBE) {
                    // log_w("Invalid footer bytes");
                    transmitter->SendDistanceSensor(0.0f, false,(float)RANGEFINDER_NV7_MAX_DISTANCE);
                    idx = 0;
                    continue;
                }

                uint8_t checksum = 0;
                for (int i = 2; i <= 23; i++) checksum += buffer[i];

                if (checksum != buffer[24]) {
                    log_w("Checksum failed: got %02X, expected %02X", buffer[24], checksum);
                    transmitter->SendDistanceSensor(0.0f, false,(float)RANGEFINDER_NV7_MAX_DISTANCE);
                    idx = 0;
                    continue;
                }

                const uint8_t* d = &buffer[5];
                self->lastMeasurement = {
                    .elevation       = (int16_t)(d[0] << 8 | d[1]),
                    .distance        = (d[2] << 8 | d[3]) / 10.0f,
                    .sineHeight      = (int16_t)(d[4] << 8 | d[5]),
                    .horizDistance   = (int16_t)(d[6] << 8 | d[7]),
                    .twoPointHeight  = (int16_t)(d[8] << 8 | d[9]),
                    .azimuth         = (int16_t)(d[10] << 8 | d[11]),
                    .horizAngle      = (int16_t)(d[12] << 8 | d[13]),
                    .span            = (int16_t)(d[14] << 8 | d[15]),
                    .speed           = (int16_t)(d[16] << 8 | d[17]),
                    .unit            = d[18],
                    .secondElevation = (int16_t)(d[19] << 8 | d[20]),
                    .isValid         = true
                };

                log_i("Parsed: dist=%.1f (unit=%d)", self->lastMeasurement.distance, self->lastMeasurement.unit);
                transmitter->SendDistanceSensor(self->lastMeasurement.distance, true, (float)RANGEFINDER_NV7_MAX_DISTANCE);
                idx = 0;
            }
        }

        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}