#include "ElevationReader.h"
#include <cmath>
#include <fstream>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <limits>

// The value used within HGT files to represent "no data" points (e.g., water bodies).
// This corresponds to the minimum possible value for a signed 16-bit integer.
constexpr int16_t HGT_NO_DATA_VALUE = std::numeric_limits<int16_t>::min(); // -32768

ElevationReader::ElevationReader(const std::string& hgt_directory)
    : hgt_dir_path_(hgt_directory) {}

double ElevationReader::get_elevation(double latitude, double longitude) {
    // Validate input coordinates to be within the standard WGS 84 range.
    if (latitude < -90.0 || latitude >= 90.0 || longitude < -180.0 || longitude >= 180.0) {
        return USER_NO_DATA_VALUE;
    }

    // Determine which tile file is needed for the given coordinates.
    std::string basename = get_tile_basename(latitude, longitude);

    // Load the tile from disk if it's not already in the cache.
    if (!ensure_tile_is_loaded(basename)) {
        // This occurs if the file doesn't exist or is invalid (e.g., wrong size).
        std::cerr << "Warning: Tile file for " << basename << ".hgt not found or is invalid." << std::endl;
        return USER_NO_DATA_VALUE;
    }

    // Retrieve the tile data from the cache. `at()` is used for safe access.
    const auto& data = tile_cache_.at(basename);

    // Calculate the fractional part of the coordinates to find the position within the 1x1 degree tile.
    double lat_fractional = latitude - floor(latitude);
    double lon_fractional = longitude - floor(longitude);

    // IMPORTANT: HGT data is stored in rows from North to South (top to bottom).
    // A higher latitude corresponds to a lower row index. So, we invert the latitude logic.
    // Example: For N45 tile (45-46 deg), lat 45.99 is near row 0; lat 45.01 is near row 3600.
    int row = static_cast<int>(round((1.0 - lat_fractional) * (HGT_GRID_DIMENSION - 1)));
    int col = static_cast<int>(round(lon_fractional * (HGT_GRID_DIMENSION - 1)));

    // Calculate the 1D index into the data vector.
    size_t index = static_cast<size_t>(row) * HGT_GRID_DIMENSION + col;

    // Safety check to prevent out-of-bounds access.
    if (index >= data.size()) {
        return USER_NO_DATA_VALUE;
    }

    int16_t elevation = data[index];

    // Check if the point corresponds to a "no data" value within the file.
    if (elevation == HGT_NO_DATA_VALUE) {
        return USER_NO_DATA_VALUE;
    }

    return static_cast<double>(elevation);
}

std::string ElevationReader::get_tile_basename(double latitude, double longitude) const {
    // The HGT filename convention is based on the integer coordinates of the bottom-left (SW) corner.
    int lat_int = static_cast<int>(floor(latitude));
    int lon_int = static_cast<int>(floor(longitude));

    char lat_char = (lat_int >= 0) ? 'N' : 'S';
    char lon_char = (lon_int >= 0) ? 'E' : 'W';

    // Use a stringstream for safe and clean string formatting.
    std::stringstream ss;
    ss << lat_char << std::setfill('0') << std::setw(2) << std::abs(lat_int)
       << lon_char << std::setfill('0') << std::setw(3) << std::abs(lon_int);
    
    return ss.str();
}

bool ElevationReader::ensure_tile_is_loaded(const std::string& tile_basename) {
    // 1. Check if the tile is already in our memory cache to avoid disk I/O.
    if (tile_cache_.count(tile_basename)) {
        return true;
    }

    // 2. Construct the full path to the .hgt file.
    std::string file_path = hgt_dir_path_ + "/" + tile_basename + ".hgt";

    std::ifstream file(file_path, std::ios::binary);
    if (!file) {
        return false; // File does not exist or cannot be opened.
    }

    // 3. Validate the file size to ensure it's a complete 1-arc-second tile.
    file.seekg(0, std::ios::end);
    std::streampos file_size = file.tellg();
    file.seekg(0, std::ios::beg);

    const std::streampos expected_size = HGT_GRID_DIMENSION * HGT_GRID_DIMENSION * sizeof(int16_t);
    if (file_size != expected_size) {
        std::cerr << "Warning: Invalid file size for " << file_path
                  << ". Expected " << expected_size << " bytes, but got " << file_size << " bytes." << std::endl;
        return false;
    }

    // 4. Read the entire file into a vector of 16-bit integers.
    std::vector<int16_t> tile_data(HGT_GRID_DIMENSION * HGT_GRID_DIMENSION);
    file.read(reinterpret_cast<char*>(tile_data.data()), expected_size);
    
    if (!file) {
        std::cerr << "Error: Failed to read all data from " << file_path << std::endl;
        return false;
    }

    // 5. CRITICAL STEP: Convert every value from Big-Endian to the host's native endianness.
    for (auto& val : tile_data) {
        val = byteswap(val);
    }
    
    // 6. Store the loaded and processed data in the cache using std::move for efficiency.
    tile_cache_[tile_basename] = std::move(tile_data);

    return true;
}

int16_t ElevationReader::byteswap(int16_t value) const {
    // Cast to unsigned to prevent issues with sign extension during bit shifts on negative numbers.
    uint16_t u_value = static_cast<uint16_t>(value);
    
    // Example: 0x1234 (Big Endian) becomes 0x3412 (Little Endian)
    // - (u_value & 0xFF00) >> 8  ->  (0x1200) >> 8  ->  0x0012
    // - (u_value & 0x00FF) << 8  ->  (0x0034) << 8  ->  0x3400
    // - 0x0012 | 0x3400 = 0x3412
    return static_cast<int16_t>(((u_value & 0xFF00) >> 8) | ((u_value & 0x00FF) << 8));
}