#include <osmium/io/any_input.hpp>
#include <osmium/visitor.hpp>
#include <osmium/handler/node_locations_for_ways.hpp>
#include <osmium/index/map/sparse_file_array.hpp>
#include <osmium/osm/way.hpp>

#include <cstdio>
#include <cstdint>
#include <cstring>
#include <cmath>
#include <string>
#include <vector>
#include <algorithm>
#include <filesystem>
#include <fcntl.h>
#include <unistd.h>

namespace fs = std::filesystem;

using index_type = osmium::index::map::SparseFileArray<osmium::unsigned_object_id_type, osmium::Location>;
using location_handler_type = osmium::handler::NodeLocationsForWays<index_type>;

static constexpr double CELL = 0.5;
static constexpr int SUB = 32;
static constexpr double LON_MIN = -180.0;
static constexpr double LAT_MIN = -60.0;
static constexpr double LON_MAX = 180.0;
static constexpr double LAT_MAX = 85.0;

struct RawPoint {
    int32_t col;
    int32_t row;
    uint16_t lonq;
    uint16_t latq;
    uint8_t speed;
}; // 11 bytes

static inline uint16_t quantize_lon(double lon) {
    return static_cast<uint16_t>((lon - LON_MIN) / (LON_MAX - LON_MIN) * 65535.0);
}

static inline uint16_t quantize_lat(double lat) {
    return static_cast<uint16_t>((lat - LAT_MIN) / (LAT_MAX - LAT_MIN) * 65535.0);
}

static inline uint8_t parse_speed_val(const char* s) {
    if (!s || !*s) return 0;
    char* end = nullptr;
    double v = strtod(s, &end);
    if (end != s && v > 0 && v <= 255) return static_cast<uint8_t>(v);
    if (strstr(s, "mph")) {
        v = strtod(s, &end);
        if (end != s) {
            v *= 1.609;
            if (v > 0 && v <= 255) return static_cast<uint8_t>(v);
        }
    }
    return 0;
}

struct Point64 {
    uint16_t lonq;
    uint16_t latq;
    uint8_t speed;
};

class SpeedHandler : public osmium::handler::Handler {
public:
    FILE* tmpfile;
    uint64_t way_count = 0;
    uint64_t point_count = 0;

    explicit SpeedHandler(FILE* f) : tmpfile(f) {}

    void way(const osmium::Way& way) {
        const char* highway = way.tags().get_value_by_key("highway");
        if (!highway) return;

        const char* ms = way.tags().get_value_by_key("maxspeed");
        uint8_t speed = ms ? parse_speed_val(ms) : 0;
        if (speed == 0) return;

        way_count++;

        for (const auto& wn : way.nodes()) {
            if (!wn.location().valid()) continue;

            double lon = wn.location().lon();
            double lat = wn.location().lat();

            if (lon < LON_MIN || lon >= LON_MAX || lat < LAT_MIN || lat >= LAT_MAX) continue;

            int32_t col = static_cast<int32_t>((lon - LON_MIN) / CELL);
            int32_t row = static_cast<int32_t>((lat - LAT_MIN) / CELL);

            RawPoint rp;
            rp.col = col;
            rp.row = row;
            rp.lonq = quantize_lon(lon);
            rp.latq = quantize_lat(lat);
            rp.speed = speed;

            fwrite(&rp, sizeof(RawPoint), 1, tmpfile);
            point_count++;

            if (point_count % 10000000 == 0) {
                fprintf(stderr, "  %lu points (%lu ways)...\n", point_count, way_count);
            }
        }
    }
};

static bool compare_by_cell(const RawPoint& a, const RawPoint& b) {
    if (a.col != b.col) return a.col < b.col;
    return a.row < b.row;
}

static void split_and_write(const char* tmppath, const std::string& outdir) {
    fprintf(stderr, "Reading temp file into memory for sort...\n");

    FILE* f = fopen(tmppath, "rb");
    fseek(f, 0, SEEK_END);
    long fsize = ftell(f);
    fseek(f, 0, SEEK_SET);

    uint64_t npoints = fsize / sizeof(RawPoint);
    fprintf(stderr, "Sorting %lu points (%.1f MB)...\n", npoints, fsize / 1048576.0);

    std::vector<RawPoint> points(npoints);
    fread(points.data(), sizeof(RawPoint), npoints, f);
    fclose(f);

    std::sort(points.begin(), points.end(), compare_by_cell);
    fprintf(stderr, "Sort done. Writing cells...\n");

    fs::create_directories(outdir);

    std::string index_path = outdir + "/index.bin";
    FILE* index = fopen(index_path.c_str(), "wb");
    uint32_t magic = 0x5A4D4150;
    fwrite(&magic, 4, 1, index);
    uint16_t version = 2;
    fwrite(&version, 2, 1, index);
    uint16_t sub = SUB;
    fwrite(&sub, 2, 1, index);
    fwrite(&CELL, 8, 1, index);
    fwrite(&LON_MIN, 8, 1, index);
    fwrite(&LAT_MIN, 8, 1, index);
    fwrite(&LON_MAX, 8, 1, index);
    fwrite(&LAT_MAX, 8, 1, index);

    uint32_t num_cells = 0;
    fseek(index, 36, SEEK_SET);
    fwrite(&num_cells, 4, 1, index);
    fseek(index, 0, SEEK_END);

    int32_t cur_col = -1, cur_row = -1;
    FILE* cur_file = nullptr;
    uint32_t cell_points = 0;
    std::string cur_key;

    for (uint64_t i = 0; i <= npoints; i++) {
        bool new_cell = (i == npoints || points[i].col != cur_col || points[i].row != cur_row);

        if (new_cell && cur_file) {
            fclose(cur_file);

            uint32_t klen = static_cast<uint32_t>(cur_key.size());
            fwrite(&klen, 4, 1, index);
            fwrite(cur_key.data(), 1, klen, index);
            fwrite(&cell_points, 4, 1, index);
            num_cells++;

            if (num_cells % 1000 == 0) {
                fprintf(stderr, "  %u cells written...\n", num_cells);
            }
        }

        if (i == npoints) break;

        if (new_cell) {
            cur_col = points[i].col;
            cur_row = points[i].row;
            cur_key = std::to_string(cur_col) + "_" + std::to_string(cur_row);
            std::string cellfile = outdir + "/cell_" + cur_key + ".bin";
            cur_file = fopen(cellfile.c_str(), "wb");
            cell_points = 0;
        }

        Point64 p;
        p.lonq = points[i].lonq;
        p.latq = points[i].latq;
        p.speed = points[i].speed;
        fwrite(&p, sizeof(Point64), 1, cur_file);
        cell_points++;
    }

    fseek(index, 36, SEEK_SET);
    fwrite(&num_cells, 4, 1, index);
    fclose(index);

    fprintf(stderr, "Total cells: %u\n", num_cells);
}

int main(int argc, char* argv[]) {
    if (argc < 3) {
        fprintf(stderr, "Usage: %s <input.pbf> <output_dir>\n", argv[0]);
        return 1;
    }

    const char* input = argv[1];
    const char* outdir = argv[2];

    std::string tmppath = std::string(outdir) + "/_points.tmp";
    fs::create_directories(outdir);

    fprintf(stderr, "Step 1: Reading PBF, writing raw points to temp file...\n");
    FILE* tmpf = fopen(tmppath.c_str(), "w+b");
    if (!tmpf) { fprintf(stderr, "Cannot create temp file\n"); return 1; }

    std::string idx_path = std::string(outdir) + "/_node_index.tmp";
    int idx_fd = ::open(idx_path.c_str(), O_CREAT | O_RDWR | O_TRUNC, 0644);
    if (idx_fd == -1) { fprintf(stderr, "Cannot create index temp file\n"); return 1; }

    index_type index(idx_fd);
    location_handler_type location_handler(index);
    SpeedHandler handler(tmpf);

    osmium::io::Reader reader(input);
    osmium::apply(reader, location_handler, handler);
    reader.close();
    fclose(tmpf);

    fprintf(stderr, "\nWays with maxspeed: %lu\n", handler.way_count);
    fprintf(stderr, "Total points: %lu\n", handler.point_count);

    fprintf(stderr, "Step 2: Sort + write grid cells...\n");
    split_and_write(tmppath.c_str(), outdir);

    ::close(idx_fd);
    unlink(idx_path.c_str());
    unlink(tmppath.c_str());
    fprintf(stderr, "Done! Grid written to %s\n", outdir);
    return 0;
}