package scalaz
package geo

sealed trait Coord {
  val latitude: Latitude
  val longitude: Longitude

  import Scalaz._
  import scala.math._
  import Geo._

  def |*|(e: Elevation) = position(this, e)

  private def square(d: Double) = d * d

  def direct(bear: Bearing, dist: Double, convergence: Double = 0.0000000000001D, ellipsoid: Ellipsoid = wgs84): Vector = {
    case class P(origSigma: Double, sigma: Double, prevSigma: Double) {
      def transition(d: Double) = P(origSigma, origSigma + d, sigma)

      lazy val sinSigma = sin(sigma)

      lazy val cosSigma = cos(sigma)

      def sigmaM2(d: Double) = 2D * d + sigma

      def cosSigmaM2(d: Double) = cos(sigmaM2(d))

      def cos2SigmaM2(d: Double) =
        square(cosSigmaM2(d))
    }

    def ps(d: Double) = P(d, d, d)

    val sMnr = ellipsoid.semiMinor
    val flat = ellipsoid.flattening
    val alpha = bear.toRadians
    val cosAlpha = cos(alpha)
    val sinAlpha = sin(alpha)
    val tanu1 = (1D - flat) * tan(latitude.toRadians)
    val cosu1 = 1D / sqrt(1D + square(tanu1))
    val sinu1 = tanu1 * cosu1
    val sigma1 = atan2(tanu1, cosAlpha)
    val csa = cosu1 * sinAlpha
    val sin2Alpha = csa * csa
    val cos2Alpha = 1 - sin2Alpha
    def ab(d: Double, f: Double, g: Double, h: Double, i: Double) = {
      val s = cos2Alpha * (square(ellipsoid.semiMajor / sMnr) - 1)
      (s / d) * ( f + s * (g + s * (h - i * s)))
    }
    val a = 1 + ab(16384, 4096, -768, 320, 175)
    val b = ab(1024, 256, -128, 74, 47)
    val end = {
      val begin = ps(dist / sMnr / a)
      def iter(p: P) = {
        def tf(d: Double) = -3 + 4 * d
        val cosSigmaM2 = p.cosSigmaM2(sigma1)
        val cos2SigmaM2 = p.cos2SigmaM2(sigma1)
        p.transition(b * p.sinSigma * (cosSigmaM2 + b / 4D * (p.cosSigma * (-1 + 2 * cos2SigmaM2) - (b / 6D) * cosSigmaM2 * tf(square(p.sinSigma)) * tf(cos2SigmaM2))))
      }
      def pred(p: P) = (p.sigma - p.prevSigma).abs >= convergence
      begin.doWhile(iter(_), pred(_))
    }
    val c = flat / 16 * cos2Alpha * (4 + flat * (4 - 3 * cos2Alpha))
    val cc = cosu1 * end.cosSigma
    val ccca = cc * cosAlpha
    val sss = sinu1 * end.sinSigma
    val lat = atan2(sinu1 * end.cosSigma + cosu1 * end.sinSigma * cosAlpha, (1D - flat) * sqrt(sin2Alpha + square(sss - ccca))).fromRadians[Latitude].value
    val lon = longitude.value + (atan2(end.sinSigma * sinAlpha, cc - sss * cosAlpha) - (1 - c) * flat * csa * (end.sigma + c * end.sinSigma * (end.cosSigmaM2(sigma1) + c * end.cosSigma * (-1 + 2 * end.cos2SigmaM2(sigma1))))).fromRadians[Longitude].value
    vector(lat |-| lon, (atan2(csa, ccca - sss)).fromRadians[Bearing])
  }

  def inverse(end: Coord, convergence: Double = 0.0000000000001D, ellipsoid: Ellipsoid = wgs84): GeodeticCurve = {
    sealed trait InverseResult
    case object Continue extends InverseResult
    case object Limit extends InverseResult
    case object Converge extends InverseResult

    case class Q(count: Int, result: InverseResult, lambda: Double, a: Double, sigma: Double, deltasigma: Double)

    val b = ellipsoid.semiMinor
    val f = ellipsoid.flattening
    val (phi1, phi2) = {
      def rl(k: Coord) = k.latitude.toRadians
      (rl(this), rl(end))
    }
    val a2b2b2 = square(ellipsoid.semiMajor) / square(b) - 1
    val omega = {
      def rl(k: Coord) = k.longitude.toRadians
      rl(end) - rl(this)
    }
    val (u1, u2) = {
      def at(d: Double) = atan ((1 - f) * (tan(d)))
      (at(phi1), at(phi2))
    }
    val sinu1 = sin(u1)
    val cosu1 = cos(u1)
    val sinu2 = sin(u2)
    val cosu2 = cos(u2)
    val sinu1sinu2 = sinu1 * sinu2
    val cosu1sinu2 = cosu1 * sinu2
    val sinu1cosu2 = sinu1 * cosu2
    val cosu1cosu2 = cosu1 * cosu2
    val begin = Q(0, Continue, omega, 0, 0, 0)
    def iter(q: Q): Q = {
      val sinlambda = sin(q.lambda)
      val coslambda = cos(q.lambda)
      val sin2sigma = square(cosu2) * square(sinlambda)+ square(cosu1sinu2 - sinu1cosu2 * coslambda)
      val sinsigma = sqrt(sin2sigma)
      val cossigma = sinu1sinu2 + cosu1cosu2 * coslambda
      val sigma = atan2(sinsigma, cossigma)
      val sinalpha = if(sin2sigma == 0D) 0D else cosu1cosu2 * sinlambda / sinsigma
      val alpha = asin(sinalpha)
      val cos2alpha = square(cos(alpha))
      val cos2sigmam = if(cos2alpha == 0D) 0D else cossigma - 2 * sinu1sinu2 / cos2alpha
      val u2 = cos2alpha * a2b2b2
      val cos2sigmam2 = square(cos2sigmam)
      val a = 1D + u2 / 16384 * (4096 + u2 * (u2 * (320 - 175 * u2) - 768))
      val b = u2 / 1024 * (256 + u2 * (u2 * (74 - 47 * u2) - 128))
      val deltasigma = b * sinsigma * (cos2sigmam + b / 4 * (cossigma * (2 * cos2sigmam2 - 1) - b / 6 * cos2sigmam * (4 * sin2sigma - 3) * (cos2sigmam2 * 4 - 3)))
      val c  = f / 16 * cos2alpha * (4 + f * (4 - 3 * cos2alpha))
      val l  = omega + (1 - c) * f * sinalpha * (sigma + c * sinsigma * (cos2sigmam + c * cossigma * (2 * cos2sigmam2 - 1)))
      val r = if(q.count == 20) Limit
              else if(q.count > 1 && cos(alpha) < convergence) Converge
              else Continue
      Q(q.count + 1, r, l, a, sigma, deltasigma)
    }
    def pred(q: Q) = q.result == Continue
    val ed = begin.whileDo(iter(_), pred(_))
    def ifi[A](p: A => Boolean, t: A => A, a: A): A = if(p(a)) t(a) else a

    def normalise(d: Double) = if(d >= 360) d - 360 else d
    val (a1, a2) = if(ed.result == Converge) (0D, 0D)
                           else if(phi1 gt phi2) (180D, 0D)
                           else if(phi1 lt phi2) (0D, 180D)
                           else (Double.NaN, Double.NaN)
    val (alpha1, alpha2) = (normalise(a1), normalise(a2))
    curve(b * ed.a * (ed.sigma - ed.deltasigma), azimuth(alpha1), azimuth(alpha2))
  }
}

trait Coords {
  def coord(lat: Latitude, lon: Longitude) = new Coord {
    val latitude = lat
    val longitude = lon
  }
}

object Coord {
  import Scalaz._
  import Geo._

  implicit def CoordShow: Show[Coord] = shows((c: Coord) => "[" + c.latitude.shows + " " + c.longitude.shows + "]")

  implicit def CoordEqual: Equal[Coord] = equalBy(((_: Coord).latitude) &&& ((_: Coord).longitude))

  implicit def CoordOrder: Order[Coord] = orderBy(((_: Coord).latitude) &&& ((_: Coord).longitude))
}