autopilot/misc/map_tool/find_nearest_tiles/main.cpp

#include <QCoreApplication>

#include <QtCore>
#include <cmath>
#include <iostream>

// Structure to store tile information
struct Tile {
    int row;
    int column;
    double topLeftLat;
    double topLeftLon;
    double bottomRightLat;
    double bottomRightLon;
    double centerLat;
    double centerLon;

    // Constructor
    Tile(int r, int c, double tlLat, double tlLon, double brLat, double brLon)
        : row(r), column(c), topLeftLat(tlLat), topLeftLon(tlLon),
        bottomRightLat(brLat), bottomRightLon(brLon) {
        centerLat = (topLeftLat + bottomRightLat) / 2.0;
        centerLon = (topLeftLon + bottomRightLon) / 2.0;
    }
};

// Function to calculate Haversine distance between two geographic points
double haversine(double lat1, double lon1, double lat2, double lon2) {
    constexpr double R = 6371.0; // Earth radius in kilometers
    double dLat = qDegreesToRadians(lat2 - lat1);
    double dLon = qDegreesToRadians(lon2 - lon1);
    double a = std::sin(dLat / 2) * std::sin(dLat / 2) +
               std::cos(qDegreesToRadians(lat1)) * std::cos(qDegreesToRadians(lat2)) *
                   std::sin(dLon / 2) * std::sin(dLon / 2);
    double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
    return R * c;
}

// Function to parse filenames and extract Tile information
QVector<Tile> parseTiles(const QStringList &filenames) {
    QVector<Tile> tiles;
    static const QRegularExpression regex(R"(map_(\d+)_(\d+)_tl_([-\d\.]+)_([-\d\.]+)_br_([-\d\.]+)_([-\d\.]+)\.)");

    for (const QString &filename : filenames) {
        QRegularExpressionMatch match = regex.match(filename);
        if (match.hasMatch()) {
            int row = match.captured(1).toInt();
            int column = match.captured(2).toInt();
            double topLeftLat = match.captured(3).toDouble();
            double topLeftLon = match.captured(4).toDouble();
            double bottomRightLat = match.captured(5).toDouble();
            double bottomRightLon = match.captured(6).toDouble();

            tiles.append(Tile(row, column, topLeftLat, topLeftLon, bottomRightLat, bottomRightLon));
        }
    }
    return tiles;
}

// Main function to find the nearest tiles
QVector<Tile> findNearestTiles(const QString &directoryPath, double currentLat, double currentLon) {
    QDir directory(directoryPath);
    QStringList filenames = directory.entryList(QStringList() << "*.jpg" << "*.png", QDir::Files);
    QVector<Tile> tiles = parseTiles(filenames);

    // Sort tiles based on Haversine distance
    std::sort(tiles.begin(), tiles.end(), [currentLat, currentLon](const Tile &a, const Tile &b) {
        return haversine(currentLat, currentLon, a.centerLat, a.centerLon) <
               haversine(currentLat, currentLon, b.centerLat, b.centerLon);
    });

    return tiles;
}

int main(int argc, char *argv[]) {
    QCoreApplication app(argc, argv);

    if (argc != 4) {
        std::cerr << "Usage: " << argv[0] << " <directory> <latitude> <longitude>" << std::endl;
        return -1;
    }

    QString directoryPath = argv[1];
    double currentLat = QString(argv[2]).toDouble();
    double currentLon = QString(argv[3]).toDouble();

    qInfo().noquote().nospace() << "Directory:  " << directoryPath;
    qInfo().noquote().nospace() << "CurrentLat: " << currentLat;
    qInfo().noquote().nospace() << "CurrentLon: " << currentLon;

    QVector<Tile> sortedTiles = findNearestTiles(directoryPath, currentLat, currentLon);

    for (const Tile &tile : sortedTiles) {
        std::cout << "Row: " << tile.row << ", Column: " << tile.column
                  << ", Center: (" << tile.centerLat << ", " << tile.centerLon << ")\n";
    }

    return 0;
}