import ../types
import json
import options
import math

proc gpsDistance(fromLat: float, fromLon: float, toLat: float, toLon: float): float =
  proc toRad(x: float): float = x * PI / 180
  let dLat = toRad(toLat - fromLat)
  let dLon = toRad(toLon - fromLon)
  let fromLat = toRad(fromLat)
  let toLat = toRad(toLat)
  let a = pow(sin(dLat / 2), 2) + (pow(sin(dLon / 2), 2) * cos(fromLat) * cos(toLat))
  let c = 2 * arctan2(sqrt(a), sqrt(1 - a))
  return 6371 * c

proc parseIntegers(str: string): seq[int] =
  var byte = 0
  var current = 0
  var bits = 0
  for c in str:
    byte = int(c) - 63
    current = current or (( byte and 31 ) shl bits)
    bits += 5

    if byte < 32:
      if (current and 1) == 1:
        current = -current
      current = current shr 1

      result.add(current)

      current = 0
      bits = 0

proc mkParsePolyline*(points: seq[Point]): proc =
  proc parsePolyline(l: JsonNode): Polyline =
    let line = l.to(HafasPolyLine)
  
    result.type = "FeatureCollection"
  
    var lat = 0
    var lon = 0
  
    let ints = parseIntegers(line.crdEncYX)
    var i = 0
    while i < len(ints):
      lat += ints[i]
      lon += ints[i+1]
      result.features.add(Feature(
        type: "Feature",
        geometry: FeatureGeometry(
          type: "Point",
          coordinates: @[lon / 100000, lat / 100000],
        ),
      ))
      i += 2
  
    if line.ppLocRefL.isSome:
      for p in line.ppLocRefL.get:
        result.features[p.ppIdx].properties = points[p.locX].stop

      # sort out coordinates closer than 5m to their neighbours
      var j = 1
      while true:
        if j >= len(result.features): break
        let last = result.features[j-1].geometry.coordinates
        let current = result.features[j].geometry.coordinates
        if gpsDistance(last[1], last[0], current[1], current[0]) <= 0.005:
          result.features.delete(j)
          continue
        j += 1

  return parsePolyline