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

// Protocol constants
#define START_SEQUENCE 0xAA  // same for L and H bit
#define END_SEQUENCE 0x55    // same for L and H bit

// Message IDs
#define MSG_TARGET_INFO 0x070C

// Initializing the static variables
double NRA15::distance = 0.0;
Stream* NRA15::serial = nullptr;

#ifndef NRA15_USE_HARDWARE_SERIAL
    SoftwareSerial* NRA15::softSerial = nullptr;
#endif

// Destructor
NRA15::~NRA15() {
#ifndef NRA15_USE_HARDWARE_SERIAL
    if (softSerial) {
        delete softSerial;
        softSerial = nullptr;
    }
#endif
}

void NRA15::NRA15RxTask(void *pvParameters)
{
    log_i("NRA15 Task Started"); // Logging task start
    
    // Initialize Serial (Hardware or Software)
#ifdef NRA15_USE_HARDWARE_SERIAL
    // Using HardwareSerial (Serial0)
    Serial.begin(115200, SERIAL_8N1, NRA15_RX_PIN, NRA15_TX_PIN);
    serial = &Serial;
    log_i("NRA15: Using HardwareSerial @ 115200 on GPIO%d/%d", NRA15_RX_PIN, NRA15_TX_PIN);
#else
    // Using SoftwareSerial
    softSerial = new SoftwareSerial(NRA15_RX_PIN, NRA15_TX_PIN);
    softSerial->begin(115200);
    serial = softSerial;
    log_i("NRA15: Using SoftwareSerial @ 115200 on GPIO%d/%d", NRA15_RX_PIN, NRA15_TX_PIN);
#endif
    
    // Getting TaskCreator object
    TaskCreator *mCreator = (TaskCreator *)pvParameters;
    NRA15 *obj = &(mCreator->mNRA15); // Getting NRA15 object from TaskCreator
    MavlinkTransmitter *transmitter = &(mCreator->mTransmitter);
    
    vTaskDelay(50); // Delay to allow initialization
    
    uint32_t lastMavlinkTime = xTaskGetTickCount();
    uint32_t lastValidDataTime = xTaskGetTickCount();
    
    // Local variables for packet assembly
    uint8_t buffer[14]; // Setting buffer size for UART communication
    uint8_t bufferIndex = 0;
    bool isValid = false;
    
    for (;;)
    {
        // Read data from Serial 
        while (serial->available()) {
            uint8_t byte = serial->read();
            
            if (bufferIndex < 2) {
                if (byte == START_SEQUENCE) {
                    buffer[bufferIndex++] = byte;
                } else {
                    bufferIndex = 0; // Reset
                }
            }
            else if (bufferIndex >= 2 && bufferIndex < 14) {
                buffer[bufferIndex++] = byte;
                
                // Complete packet received
                if (bufferIndex == 14) {
                    // Validate end sequence
                    if (buffer[12] == END_SEQUENCE && buffer[13] == END_SEQUENCE) {
                        // Get Message ID
                        uint16_t msgId = buffer[2] | (buffer[3] << 8);
                        
                        if (msgId == MSG_TARGET_INFO) {
                            // Parse target information
                            uint8_t rangeH = buffer[6];
                            uint8_t rangeL = buffer[7];
                            
                            // Calculate distance in meters
                            obj->distance = (rangeH * 256 + rangeL) * 0.01;
                            isValid = true;
                            lastValidDataTime = xTaskGetTickCount();
                            log_v("NRA15: %.2fm", obj->distance);
                        }
                    } else {
                        log_v("NRA15: Invalid end sequence");
                    }
                    
                    bufferIndex = 0; // Reset for next packet
                }
            }
        }
        
        
        uint32_t currentTime = xTaskGetTickCount();

        //Timeout check
        if ((currentTime - lastMavlinkTime) > pdMS_TO_TICKS(500)) {
            if (isValid) {
                isValid = false;
                log_w("NRA15: no target (timeout)");
            }
        }
        
        // Send MAVLink distance sensor message every 100ms
        if ((currentTime - lastMavlinkTime) >= pdMS_TO_TICKS(100)) {
            transmitter->SendDistanceSensor(
                obj->distance,
                isValid, 
                (float)RANGEFINDER_NRA15_MAX_DISTANCE
            );
            lastMavlinkTime = currentTime;
        }
        
        vTaskDelay(pdMS_TO_TICKS(10)); // 10ms delay
    }
}