# nav.py
# Andrew Davison, ad@coe.psu.ac.th, July 2025
'''
  Navigation functions using latitude and longitude's of
  cities (obtained from CITIES_CSV) and the generation
  of the great circle and rhumb line linking two cities.

  Drawing is done by using Cartopy
  (https://scitools.org.uk/cartopy/docs/latest/),
  but that is separate from these functions.

'''

import math, re
import csv

EARTH_RADIUS = 6371.0  # in kilometers
COORD_EPS = 0.1
CITIES_CSV = "world_cities.csv"


# ------------ city CSV ----------------------


def loadCityData():
  cityList = []
  with open(CITIES_CSV, newline='', encoding='utf-8') as csvfile:
    reader = csv.DictReader(csvfile)
    for row in reader:
      cityEntry = {
        'city': row['city_ascii'].strip().lower(),
        'country': row['country'].strip().lower(),
        'lat': float(row['lat']),
        'lon': float(row['lng'])
      }
      cityList.append(cityEntry)
  return cityList


def readCoord(cityList, cityName, countryName=None):
  print(f"Searching for {cityName}...")
  cityName = cityName.strip().lower()
  matches = []

  for entry in cityList:
    if entry['city'] == cityName:
      if countryName:
        if entry['country'] == countryName.strip().lower():
          return entry['lat'], entry['lon']
      else:
        matches.append(entry)

  if not matches:
    print("No matches")
    raise ValueError("No match for "+ cityName)

  if len(matches) > 1:
    print("Multiple matches:")
    for m in matches:
      print("  ", list(m.values()))
    print("Returning first")

  return matches[0]['lat'],  matches[0]['lon']



def limitExtent(lat1, lon1, lat2, lon2, offset=20):
  # calculate a bounded box for the two coordinates
  lat1 = parseCoord(lat1)
  lon1 = parseCoord(lon1)
  lat2 = parseCoord(lat2)
  lon2 = parseCoord(lon2)
  leftLon = min(lon1, lon2) - offset
  rightLon = max(lon1, lon2) + offset

  lowerLat = min(lat1, lat2) - offset
  if lowerLat < -85:
    lowerLat = -85
  upperLat = max(lat1, lat2) + offset
  if upperLat > 85:
    upperLat = 85
  return [leftLon, rightLon, lowerLat, upperLat]



# --------  coordinate functions ----------------

def usesDateline(lon1, lon2):
  # are these two longitudes closer when linked across
  # the dataline?
  lon1 = parseCoord(lon1)
  lon2 = parseCoord(lon2)
  if lon1*lon2 < 0:  # a positive and negative coord
    dist1 = -lon1 if lon1 < 0 else lon1  # from prime meridian
    dist2 = -lon2 if lon2 < 0 else lon2
    return (dist1 + dist2 > 180)
  else:
    return False


def parseCoord(coord):
  # convert a coordinate to decimal format
  if isinstance(coord, (int, float)):
    return float(coord)
  elif isinstance(coord, str):
    degs, mins, secs, dir = parseDmsString(coord)
    return dmsToDecimal(degs, mins, secs, dir)
  elif isinstance(coord, (tuple, list)) and len(coord) == 4:
    degs, mins, secs, dir = coord
    return dmsToDecimal(degs, mins, secs, dir)
  else:
    raise ValueError("Coord must be a float, int, string or a (degs, mins, secs, dir) tuple.")


def parseDmsString(dmsStr):
  """
  Parse a DMS string like "51°28'40.12\"N" or "51 28 40.12 N"
  Returns (degrees, minutes, seconds, direction)
  """
  # Remove degree, minute, second symbols and normalize whitespace
  dmsStr = dmsStr.strip().replace("°", " ").replace("º", " ")
  dmsStr = dmsStr.replace("'", " ").replace("’", " ").replace("′", " ")
  dmsStr = dmsStr.replace('"', " ").replace("″", " ")
  dmsStr = re.sub(r'\s+', ' ', dmsStr)  # collapse multiple spaces

  match = re.match(r'^(-?\d+)\s+(\d+)\s+([\d.]+)\s*([NSEW])$', 
                          dmsStr, re.IGNORECASE)
  if not match:
    raise ValueError(f"Invalid DMS string format: {dmsStr}")

  degs = int(match.group(1))
  mins = int(match.group(2))
  secs = float(match.group(3))
  dir = match.group(4).upper()
  return degs, mins, secs, dir


def dmsToDecimal(degs, mins, secs, dir):
  """ Convert degs, mins, secs to decimal degs.
  dir: 'N', 'S', 'E', or 'W'
  """
  decimal = abs(degs) + mins/60 + secs/3600
  if dir.upper() in ['S', 'W']:
    decimal *= -1
  return decimal


def decimalToDms(decimal, isLat=True):
  """ Convert decimal degs to (degs, mins, secs, dir).
  isLatitude: True for latitude, False for longitude
  """
  dir = ''
  if isLat:
    dir = 'N' if decimal >= 0 else 'S'
  else:
    dir = 'E' if decimal >= 0 else 'W'

  absDecimal = abs(decimal)
  degs = int(absDecimal)
  minsFloat = (absDecimal - degs) * 60
  mins = int(minsFloat)
  secs = round((minsFloat - mins) * 60, 2)
  return degs, mins, secs, dir



def printCoord(lat, lon):
  latStr = format2Dms(lat, isLat=True)
  lonStr = format2Dms(lon, isLat=False)
  print(f"Lat: {latStr}; Lon: {lonStr}")


def format2Dms(coord, isLat=True):
  deg = parseCoord(coord)
  d, m, s, dir = decimalToDms(deg, isLat)
  return f"{d}°{m}'{s:.2f}\"{dir}"



# ----------- Great-circle functions --------------------
# https://en.wikipedia.org/wiki/Great_circle
''' Python versions of some of the Javascript code at
    https://www.movable-type.co.uk/scripts/latlong.html

   The functions here are assumed to be passed coordinates
   expressed as decimal degrees values.
'''


def distTo(lat1, lon1, lat2, lon2):
  phi1 = math.radians(lat1)
  phi2 = math.radians(lat2)
  dLambda = math.radians(lon2 - lon1)

  # Spherical law of cosines formula
  angle = math.acos(
    math.sin(phi1) * math.sin(phi2) + \
    math.cos(phi1) * math.cos(phi2) * math.cos(dLambda)
  )
  return EARTH_RADIUS * angle



def havDistTo(lat1, lon1, lat2, lon2):
  # using the haversine formula
  phi1 = math.radians(lat1)
  phi2 = math.radians(lat2)
  deltaPhi = math.radians(lat2 - lat1)
  dLambda = math.radians(lon2 - lon1)

  a = math.sin(deltaPhi/2)**2 + \
      math.cos(phi1) * math.cos(phi2) * \
      math.sin(dLambda/2)**2
  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
  return EARTH_RADIUS * c



def bearingTo(lat1, lon1, lat2, lon2):
  # return degree bearing from first to second coord
  phi1 = math.radians(lat1)
  phi2 = math.radians(lat2)
  dLambda = math.radians(lon2 - lon1)

  y = math.sin(dLambda) * math.cos(phi2)
  x = math.cos(phi1) * math.sin(phi2) - \
      math.sin(phi1) * math.cos(phi2) * math.cos(dLambda)
  theta = math.atan2(y, x)
  return (math.degrees(theta) + 360) % 360


def destCoord(lat1, lon1, dist, bearing):
  ''' Where does the GC using this coord, bearing 
      and length finish?  '''
  theta = math.radians(bearing)
  phi1 = math.radians(lat1)
  lambda1 = math.radians(lon1)
  delta = dist/EARTH_RADIUS

  phi2 = math.asin(math.sin(phi1) * math.cos(delta) + \
             math.cos(phi1) * math.sin(delta) * \
             math.cos(theta)  )

  lambda2 = lambda1 + \
       math.atan2(math.sin(theta) * math.sin(delta) * \
                  math.cos(phi1),
                  math.cos(delta) - math.sin(phi1) *  \
                  math.sin(phi2)  )

  lambda2 = (lambda2 + math.pi) % (2 * math.pi) - math.pi
  return math.degrees(phi2), math.degrees(lambda2)  # lat, lon



def greatCircleCoords(lat1, lon1, lat2, lon2, num):
  """ Generate a list of coordinates (and bearings)
      on the GC between these two coordinates
  """
  lat1 = parseCoord(lat1)
  lon1 = parseCoord(lon1)
  lat2 = parseCoord(lat2)
  lon2 = parseCoord(lon2)

  dist = distTo(lat1, lon1, lat2, lon2)
  distStep = dist/num

  bearing = bearingTo(lat1, lon1, lat2, lon2)
  nSteps = 1
  coords = [(lat1, lon1, bearing)]
  currLat, currLon = lat1, lon1
  while (nSteps < num) or \
        (not areClose(currLat, currLon, lat2, lon2)):
    destLat, destLon =  \
       destCoord(currLat, currLon, distStep, bearing)
    bearing = bearingTo(destLat, destLon, lat2, lon2)
    nSteps += 1
    coords.append((destLat, destLon, bearing))
    currLat, currLon = destLat, destLon
  return coords


def areClose(lat1, lon1, lat2, lon2,
                     eps = COORD_EPS):
   # are these two coordinates close?
   if lon1 < 0:
     lon1 = lon1 + 360
   if lon2 < 0:
     lon2 = lon2 + 360
   return (abs(lat1-lat2) < eps) and \
          (abs(lon1-lon2) < eps)



# --- Rhumb line functions ---
# https://en.wikipedia.org/wiki/Rhumb_line
# https://en.wikipedia.org/wiki/Mercator_projection
''' Python versions of some of the Javascript code at
    https://www.movable-type.co.uk/scripts/latlong.html

   The functions here are assumed to be passed coordinates
   expressed as decimal degrees values.
'''


def rhumbDistTo(lat1, lon1, lat2, lon2):
  phi1 = math.radians(lat1)
  phi2 = math.radians(lat2)
  deltaPhi = phi2 - phi1
  dLambda = math.radians(lon2 - lon1)

  deltaY = math.log(math.tan(math.pi/4 + phi2/2) / \
                      math.tan(math.pi/4 + phi1/2))
  q = deltaPhi/deltaY if abs(deltaY) > 1e-12 \
                        else math.cos(phi1)
  ''' If on a constant latitude course (travelling east-west), 
      use cos(phi)
  '''
  if abs(dLambda) > math.pi:
    # use shorter rhumb line across the date line
    dLambda = dLambda - 2*math.pi if dLambda > 0 \
                                      else dLambda + 2*math.pi
  return math.sqrt(deltaPhi**2 + (q * dLambda)**2) * EARTH_RADIUS



def rhumbBearingTo(lat1, lon1, lat2, lon2):
  # return degree bearing from first to second coord
  phi1 = math.radians(lat1)
  phi2 = math.radians(lat2)
  dLambda = math.radians(lon2 - lon1)

  deltaY = math.log(math.tan(math.pi/4 + phi2/2)/ \
                      math.tan(math.pi/4 + phi1/2))

  if abs(dLambda) > math.pi:
    # use shorter rhumb line across the date line
    dLambda = dLambda - 2*math.pi if dLambda > 0  \
                                  else dLambda + 2*math.pi

  psi = math.atan2(dLambda, deltaY)
  return (math.degrees(psi) + 360) % 360


def rhumbDestCoord(lat1, lon1, dist, bearing):
  ''' Where does the Rhumb line using this coord, 
      bearing and length finish?  '''
  phi1 = math.radians(lat1)
  lambda1 = math.radians(lon1)
  psi = math.radians(bearing)
  delta = dist/EARTH_RADIUS

  deltaPhi = delta * math.cos(psi)
  phi2 = phi1 + deltaPhi

  # normalize latitude if going past the pole
  if phi2 > math.pi/2:
    phi2 = math.pi - phi2
  elif phi2 < -math.pi/2:
    phi2 = -math.pi - phi2

  deltaY = math.log(math.tan(math.pi/4 + phi2/2)/ \
                      math.tan(math.pi/4 + phi1/2))
  q = deltaPhi/deltaY if abs(deltaY) > 1e-12 \
                        else math.cos(phi1)

  dLambda = delta * math.sin(psi)/q
  lambda2 = lambda1 + dLambda

  deglambda = ((math.degrees(lambda2) + 540) % 360) - 180
  return math.degrees(phi2), deglambda  # lat, lon



def rhumbCoords(lat1, lon1, lat2, lon2, num):
  """ Generate a list of coordinates (and bearings)
      on the Rhumb line between these two coordinates
  """
  lat1 = parseCoord(lat1)
  lon1 = parseCoord(lon1)
  lat2 = parseCoord(lat2)
  lon2 = parseCoord(lon2)

  dist = rhumbDistTo(lat1, lon1, lat2, lon2)
  distStep = dist/num

  bearing = rhumbBearingTo(lat1, lon1, lat2, lon2)
  nSteps = 1
  coords = [(lat1, lon1, bearing)]
  currLat, currLon = lat1, lon1
  while (nSteps < num) or \
        (not areClose(currLat, currLon, lat2, lon2)):
    destLat, destLon =  \
       rhumbDestCoord(currLat, currLon, distStep, bearing)
    bearing = rhumbBearingTo(destLat, destLon, lat2, lon2)
    nSteps += 1
    coords.append((destLat, destLon, bearing))
    currLat, currLon = destLat, destLon
  return coords


def mercatorY(lat):
  # http://en.wikipedia.org/wiki/Mercator_projection
  phi = math.radians(lat)
  return math.log(math.tan(math.pi/4 + phi/2))


def invMercatorY(h, radius=1):
  lat = 2*math.atan(math.exp(h/radius)) - math.pi/2
  return math.degrees(lat)


# ---------------- (x,y,z) coords -----------------

def cartesian(lat, lon):
  # Convert lat and lon to 3D Cartesian coords in km
  lat = parseCoord(lat)
  lon = parseCoord(lon)
  x = EARTH_RADIUS * math.cos(lat) * math.cos(lon)
  y = EARTH_RADIUS * math.cos(lat) * math.sin(lon)
  z = EARTH_RADIUS * math.sin(lat)
  return x, y, z


def chordDistTo(lat1, lon1, lat2, lon2):
  # straight-line (chord) distance between coords
  x1, y1, z1 = cartesian(lat1, lon1)
  x2, y2, z2 = cartesian(lat2, lon2)
  dx = x2 - x1
  dy = y2 - y1
  dz = z2 - z1
  return math.sqrt(dx*dx + dy*dy + dz*dz)



# ----------- GC and Rhumb line tests ---------------


def greatCircleTrip(lat1, lon1, lat2, lon2):
  """
  Given starting and ending coordinates in decimal degs,
  print GC distance, bearing, and check the destination.
  """
  lat1 = parseCoord(lat1)
  lon1 = parseCoord(lon1)
  lat2 = parseCoord(lat2)
  lon2 = parseCoord(lon2)

  dist = distTo(lat1, lon1, lat2, lon2)
  bearing = bearingTo(lat1, lon1, lat2, lon2)
  print(f"Great-circle dist: {dist:.2f} km")
  print(f"Bearing: {bearing:.2f}°")

  destLat, destLon = destCoord(lat1, lon1, dist, bearing)
  print(f"Dest coord from {dist:.2f} km @{bearing:.2f}°:({destLat:.2f}; {destLon:.2f})")



def rhumbLineTrip(lat1, lon1, lat2, lon2):
  """
  Given starting and ending coordinates in decimal degs,
  print RL distance, bearing, and check the destination.
  """
  lat1 = parseCoord(lat1)
  lon1 = parseCoord(lon1)
  lat2 = parseCoord(lat2)
  lon2 = parseCoord(lon2)

  dist = rhumbDistTo(lat1, lon1, lat2, lon2)
  bearing = rhumbBearingTo(lat1, lon1, lat2, lon2)
  print(f"Rhumb line dist: {dist:.2f} km")
  print(f"Constant bearing: {bearing:.2f}°")

  destLat, destLon = rhumbDestCoord(lat1, lon1, dist, bearing)
  print(f"Dest coord from {dist:.2f} km @{bearing:.2f}°:({destLat:.2f}; {destLon:.2f})")



# ------------------------------------

if __name__ == '__main__':
  cityData = loadCityData()
  print("Locating London, UK")
  cityName = 'London'
  countryName = 'United Kingdom'
  lat, lon = readCoord(cityData, cityName, countryName)
  printCoord(lat, lon)

  print("Locating NYC")
  cityName = 'New York'
  lat, lon = readCoord(cityData, cityName, None)
  printCoord(lat, lon)

  print()
  print("Greenwich Observatory to JFK Airport, NYC")
  # 51.4778, -0.0015  # Greenwich
  # 40.6413, -73.7781 # JFK
  lat1 = "51°28'40.08\"N"  #  (51, 28, 40.08, 'N')
  lon1 = (0, 0, 5.4, 'W')
  lat2 = (40, 38, 28.68, 'N')
  lon2 = -73.7781          # (73, 46, 41.16, 'W')
  printCoord(lat1, lon1)
  printCoord(lat2, lon2)
  print()
  greatCircleTrip(lat1, lon1, lat2, lon2)
  print()
  rhumbLineTrip(lat1, lon1, lat2, lon2)

  print("\nGreat Circle (lat,lon)s and deg Bearing:")
  coords = greatCircleCoords(lat1, lon1, lat2, lon2, 10)
  for coord in coords:
    cLat, cLon, cBear = coord
    print(f"  ({cLat:.2f}, {cLon:.2f}); {cBear:.2f}")

  print("\nRhumb Line (lat,lon)s and deg Bearing:")
  coords = rhumbCoords(lat1, lon1, lat2, lon2, 10)
  for coord in coords:
    cLat, cLon, cBear = coord
    print(f"  ({cLat:.2f}, {cLon:.2f}); {cBear:.2f}")