#include "geo/geo.h"

#include <algorithm>
#include <cmath>


namespace geo {

// ── Distance (Haversine) ────────────────────────────────────────────────────

double distance_km(Coord a, Coord b) {
  double dLat = (b.lat - a.lat) * DEG2RAD;
  double dLon = (b.lon - a.lon) * DEG2RAD;
  double lat1 = a.lat * DEG2RAD;
  double lat2 = b.lat * DEG2RAD;

  double sinDLat = std::sin(dLat / 2.0);
  double sinDLon = std::sin(dLon / 2.0);
  double h = sinDLat * sinDLat + std::cos(lat1) * std::cos(lat2) * sinDLon * sinDLon;
  return 2.0 * EARTH_RADIUS_KM * std::asin(std::sqrt(h));
}

// ── Bounding box ────────────────────────────────────────────────────────────

BBox bbox(const std::vector<Coord>& ring) {
  if (ring.empty()) return {};
  BBox b{ring[0].lon, ring[0].lat, ring[0].lon, ring[0].lat};
  for (size_t i = 1; i < ring.size(); ++i) {
    b.minLon = std::min(b.minLon, ring[i].lon);
    b.minLat = std::min(b.minLat, ring[i].lat);
    b.maxLon = std::max(b.maxLon, ring[i].lon);
    b.maxLat = std::max(b.maxLat, ring[i].lat);
  }
  return b;
}

BBox bbox_union(const std::vector<BBox>& boxes) {
  if (boxes.empty()) return {};
  BBox u = boxes[0];
  for (size_t i = 1; i < boxes.size(); ++i) {
    u.minLon = std::min(u.minLon, boxes[i].minLon);
    u.minLat = std::min(u.minLat, boxes[i].minLat);
    u.maxLon = std::max(u.maxLon, boxes[i].maxLon);
    u.maxLat = std::max(u.maxLat, boxes[i].maxLat);
  }
  return u;
}

// ── Centroid ────────────────────────────────────────────────────────────────

Coord centroid(const std::vector<Coord>& ring) {
  if (ring.empty()) return {};
  double sumLon = 0, sumLat = 0;
  // Exclude last point if it's the same as first (closed ring)
  size_t n = ring.size();
  if (n > 1 && ring[0].lon == ring[n - 1].lon && ring[0].lat == ring[n - 1].lat) {
    n--;
  }
  for (size_t i = 0; i < n; ++i) {
    sumLon += ring[i].lon;
    sumLat += ring[i].lat;
  }
  return {sumLon / static_cast<double>(n), sumLat / static_cast<double>(n)};
}

// ── Area (Shoelace + latitude cosine correction) ────────────────────────────

double area_sq_m(const std::vector<Coord>& ring) {
  if (ring.size() < 3) return 0.0;

  // Shoelace formula in projected coordinates.
  // Each degree of longitude = cos(lat) * 111320 meters at that latitude.
  // Each degree of latitude = 110540 meters (approximate).
  double sum = 0.0;
  size_t n = ring.size();

  for (size_t i = 0; i < n; ++i) {
    size_t j = (i + 1) % n;
    // Convert coordinates to approximate meters using the average latitude
    double avgLat = (ring[i].lat + ring[j].lat) / 2.0;
    double cosLat = std::cos(avgLat * DEG2RAD);

    double x_i = ring[i].lon * cosLat * 111320.0;
    double y_i = ring[i].lat * 110540.0;
    double x_j = ring[j].lon * cosLat * 111320.0;
    double y_j = ring[j].lat * 110540.0;

    sum += x_i * y_j - x_j * y_i;
  }
  return std::abs(sum) / 2.0;
}

// ── Point-in-polygon (ray casting) ──────────────────────────────────────────

bool point_in_polygon(Coord pt, const std::vector<Coord>& ring) {
  bool inside = false;
  size_t n = ring.size();
  for (size_t i = 0, j = n - 1; i < n; j = i++) {
    double xi = ring[i].lon, yi = ring[i].lat;
    double xj = ring[j].lon, yj = ring[j].lat;

    if (((yi > pt.lat) != (yj > pt.lat)) &&
        (pt.lon < (xj - xi) * (pt.lat - yi) / (yj - yi) + xi)) {
      inside = !inside;
    }
  }
  return inside;
}

// ── Bearing ─────────────────────────────────────────────────────────────────

double bearing_deg(Coord from, Coord to) {
  double dLon = (to.lon - from.lon) * DEG2RAD;
  double lat1 = from.lat * DEG2RAD;
  double lat2 = to.lat * DEG2RAD;

  double y = std::sin(dLon) * std::cos(lat2);
  double x = std::cos(lat1) * std::sin(lat2) -
             std::sin(lat1) * std::cos(lat2) * std::cos(dLon);
  double brng = std::atan2(y, x) * RAD2DEG;
  return std::fmod(brng + 360.0, 360.0);
}

// ── Destination point ───────────────────────────────────────────────────────

Coord destination(Coord from, double brng_deg, double dist_km) {
  double brng = brng_deg * DEG2RAD;
  double lat1 = from.lat * DEG2RAD;
  double lon1 = from.lon * DEG2RAD;
  double d = dist_km / EARTH_RADIUS_KM;

  double lat2 = std::asin(std::sin(lat1) * std::cos(d) +
                           std::cos(lat1) * std::sin(d) * std::cos(brng));
  double lon2 = lon1 + std::atan2(
      std::sin(brng) * std::sin(d) * std::cos(lat1),
      std::cos(d) - std::sin(lat1) * std::sin(lat2));

  return {lon2 * RAD2DEG, lat2 * RAD2DEG};
}

// ── Square grid ─────────────────────────────────────────────────────────────

std::vector<Coord> square_grid(BBox extent, double cellSizeKm) {
  std::vector<Coord> centers;
  if (cellSizeKm <= 0) return centers;

  // Convert cell size to degrees at the center latitude
  double centerLat = (extent.minLat + extent.maxLat) / 2.0;
  double cosLat = std::cos(centerLat * DEG2RAD);
  if (cosLat < 1e-10) cosLat = 1e-10; // Avoid division by zero near poles

  double cellLatDeg = cellSizeKm / 110.574;          // ~110.574 km per degree lat
  double cellLonDeg = cellSizeKm / (111.320 * cosLat); // longitude correction

  for (double lat = extent.minLat + cellLatDeg / 2.0;
       lat < extent.maxLat; lat += cellLatDeg) {
    for (double lon = extent.minLon + cellLonDeg / 2.0;
         lon < extent.maxLon; lon += cellLonDeg) {
      centers.push_back({lon, lat});
    }
  }
  return centers;
}

// ── Hex grid ────────────────────────────────────────────────────────────────

std::vector<Coord> hex_grid(BBox extent, double cellSizeKm) {
  std::vector<Coord> centers;
  if (cellSizeKm <= 0) return centers;

  double centerLat = (extent.minLat + extent.maxLat) / 2.0;
  double cosLat = std::cos(centerLat * DEG2RAD);
  if (cosLat < 1e-10) cosLat = 1e-10;

  // Hex spacing: horizontal = cellSize, vertical = cellSize * sqrt(3)/2
  double cellLatDeg = cellSizeKm / 110.574;
  double cellLonDeg = cellSizeKm / (111.320 * cosLat);
  double rowHeight = cellLatDeg * std::sqrt(3.0) / 2.0;

  int row = 0;
  for (double lat = extent.minLat + rowHeight / 2.0;
       lat < extent.maxLat; lat += rowHeight) {
    // Offset every other row by half the cell width
    double lonOffset = (row % 2 == 1) ? cellLonDeg / 2.0 : 0.0;
    for (double lon = extent.minLon + cellLonDeg / 2.0 + lonOffset;
         lon < extent.maxLon; lon += cellLonDeg) {
      centers.push_back({lon, lat});
    }
    row++;
  }
  return centers;
}

// ── Viewport estimation ─────────────────────────────────────────────────────

double estimate_viewport_sq_km(double lat, int zoom, int widthPx, int heightPx) {
  double metersPerPx =
      (156543.03392 * std::cos(lat * DEG2RAD)) / std::pow(2.0, zoom);
  double widthKm = (widthPx * metersPerPx) / 1000.0;
  double heightKm = (heightPx * metersPerPx) / 1000.0;
  return widthKm * heightKm;
}

} // namespace geo