#include <iostream>
#include <queue>
#include <vector>
#include <cmath> // for std::isnan

using namespace std;

// Structure to represent bounding boxes
struct DetectionData {
    struct Rect {
        double x1, y1, x2, y2;
    } rect;
    int class_id;
    int tracking_id;
    int frame_id;
    double confidence_score;
};


// Function to apply Kalman filter prediction
DetectionData applyKalmanFilter(const DetectionData& detection, KalmanFilter& kalmanFilter) {
    // Use Kalman filter to predict next state
    VectorXd measurement(4);
    measurement << detection.rect.x1, detection.rect.y1, detection.rect.x2, detection.rect.y2;
    kalmanFilter.predict();
    kalmanFilter.correct(measurement);

    // Update the detection with predicted values
    DetectionData predictedDetection = detection;
    VectorXd state = kalmanFilter.getState();
    predictedDetection.rect.x1 = state(0);
    predictedDetection.rect.y1 = state(1);
    predictedDetection.rect.x2 = state(2);
    predictedDetection.rect.y2 = state(3);

    return predictedDetection;
}

// Function to interpolate missing bounding box outputs
queue<DetectionData> interpolateMissingData(queue<DetectionData> detectionQueue) {
    // Extract data from the queue
    vector<DetectionData> data(detectionQueue.begin(), detectionQueue.end());

    // Check if there's any missing data (no data except for frame_id)
    bool hasMissingData = false;
    for (const auto& detection : data) {
        if (detection.rect.x1 == -1 && detection.rect.y1 == -1 && detection.rect.x2 == -1 && detection.rect.y2 == -1 &&
            detection.class_id == -1 && detection.tracking_id == -1 && detection.confidence_score == -1) {
            hasMissingData = true;
            break;
        }
    }

    // If there's missing data, interpolate it
    if (hasMissingData) {
        // Find the previous and next available detections
        int prevIndex = -1;
        int nextIndex = -1;
        for (int i = 0; i < data.size(); ++i) {
            if (data[i].frame_id != -1) {
                if (prevIndex == -1) {
                    prevIndex = i;
                } else {
                    nextIndex = i;
                    break;
                }
            }
        }

        // Interpolate missing data using Kalman filter
        KalmanFilter kalmanFilter(4, 4); // Kalman filter with 4 states and 4 measurements
        kalmanFilter.init(); // Initialize Kalman filter
        for (int i = prevIndex; i < nextIndex; ++i) {
            DetectionData& detection = data[i];
            if (detection.rect.x1 == -1 && detection.rect.y1 == -1 && detection.rect.x2 == -1 && detection.rect.y2 == -1 &&
                detection.class_id == -1 && detection.tracking_id == -1 && detection.confidence_score == -1) {
                // Apply Kalman filter prediction for missing data
                detection = applyKalmanFilter(data[i - 1], kalmanFilter);
            }
        }

        // Update the detection queue with interpolated data
        detectionQueue = queue<DetectionData>(data.begin(), data.end());
    }

    // If there's no missing data, return the original queue
    return detectionQueue;
}



// Function to interpolate missing bounding box outputs
queue<DetectionData> interpolateMissingData(queue<DetectionData> detectionQueue) {
    // Extract data from the queue
    vector<DetectionData> data(detectionQueue.begin(), detectionQueue.end());

    // Check if there's any missing data (no data except for frame_id)
    bool hasMissingData = false;
    for (const auto& detection : data) {
        if (detection.rect.x1 == -1 && detection.rect.y1 == -1 && detection.rect.x2 == -1 && detection.rect.y2 == -1 &&
            detection.class_id == -1 && detection.tracking_id == -1 && detection.confidence_score == -1) {
            hasMissingData = true;
            break;
        }
    }

    // If there's missing data, interpolate it
    if (hasMissingData) {
        // Find the previous and next available detections
        int prevIndex = -1;
        int nextIndex = -1;
        for (int i = 0; i < data.size(); ++i) {
            if (data[i].frame_id != -1) {
                if (prevIndex == -1) {
                    prevIndex = i;
                } else {
                    nextIndex = i;
                    break;
                }
            }
        }

        // Interpolate missing data
        DetectionData interpolatedData = data[prevIndex];
        interpolatedData.frame_id = -1; // Set frame_id to -1 to indicate that it's interpolated
        for (int i = prevIndex + 1; i < nextIndex; ++i) {
            // Linear interpolation for rect and confidence_score
            double t = (double)(data[i].frame_id - data[prevIndex].frame_id) / (data[nextIndex].frame_id - data[prevIndex].frame_id);
            interpolatedData.rect.x1 = data[prevIndex].rect.x1 + t * (data[nextIndex].rect.x1 - data[prevIndex].rect.x1);
            interpolatedData.rect.y1 = data[prevIndex].rect.y1 + t * (data[nextIndex].rect.y1 - data[prevIndex].rect.y1);
            interpolatedData.rect.x2 = data[prevIndex].rect.x2 + t * (data[nextIndex].rect.x2 - data[prevIndex].rect.x2);
            interpolatedData.rect.y2 = data[prevIndex].rect.y2 + t * (data[nextIndex].rect.y2 - data[prevIndex].rect.y2);
            interpolatedData.confidence_score = data[prevIndex].confidence_score + t * (data[nextIndex].confidence_score - data[prevIndex].confidence_score);
            detectionQueue.push(interpolatedData);
        }
    }

    // If there's no missing data, return the original queue
    return detectionQueue;
}

int main() {
    // FIFO queue to store detection data
    queue<DetectionData> detectionQueue;

    // Example data (to simulate continuous addition of elements)
    for (int i = 0; i < 25; ++i) {
        DetectionData detection;
        detection.rect.x1 = i;
        detection.rect.y1 = i + 1;
        detection.rect.x2 = i + 2;
        detection.rect.y2 = i + 3;
        detection.class_id = 1;
        detection.tracking_id = 1;
        detection.frame_id = i;
        detection.confidence_score = 0.6 + (i % 3) * 0.1; // Varying confidence scores for demonstration
        detectionQueue.push(detection);

        // Maintain the queue size at 20 elements
        if (detectionQueue.size() > 20) {
            detectionQueue.pop();
        }

        // Analyze and interpolate missing data if present
        detectionQueue = interpolateMissingData(detectionQueue);
    }

    // Output the completed detection data queue
    cout << "Completed detection data queue:" << endl;
    while (!detectionQueue.empty()) {
        DetectionData detection = detectionQueue.front();
        detectionQueue.pop();
        cout << "Rect: (" << detection.rect.x1 << ", " << detection.rect.y1 << ", " << detection.rect.x2 << ", " << detection.rect.y2 << ")" << endl;
        cout << "Class ID: " << detection.class_id << endl;
        cout << "Tracking ID: " << detection.tracking_id << endl;
        cout << "Frame ID: " << detection.frame_id << endl;
        cout << "Confidence Score: " << detection.confidence_score << endl;
        cout << endl;
    }

    return 0;
}
