/*
   Copyright 2010 Aaron J. Radke

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
package cc.drx

object Ned{
   def apply(n:Length, e:Length, d:Length=Length(0)):Ned = new Ned(Vec(n.m, e.m, d.m)) //meters
   def apply(range:Length, bearing:Angle):Ned = new Ned(Vec.polar(range.m, bearing)) //meters
   def apply(ned:Vec):Ned = new Ned(ned) //meters
   def zero:Ned = Ned(Vec.zero) //meters
   //def apply(n:Double, e:Double, d:Double=0):Ned = new Ned(n, e, d) //meters
}
class Ned(val vec:Vec) extends AnyVal{ //meters
   def n = Length(vec.x)
   def e = Length(vec.y)
   def d = Length(vec.z)
   /**convenience function for -d to support ENU from NED*/
   def u = Length(-vec.z)
   def dist:Length = Length(vec.norm)
   def bearing:Angle = vec.heading
   def unary_- :Ned = Ned(-vec)
   def +(that:Ned):Ned = Ned(this.vec + that.vec)
   def -(that:Ned):Ned = Ned(this.vec - that.vec)
   override def toString = s"Ned($n,$e,$d)"
}
object Ecef{
   /**semi-major (equatorial) axis*/
   val a:Length = 6378137.m
   /**semi-minor (polar) axis*/
   val b:Length = 6356752.3142.m
   /**flattening*/
   val f:Double = 1 - b.m/a.m
   /**eccentricity*/
   val e:Double = 2*f - f*f
   /**average radius of the earth*/
   val r:Length = 6371E3.m

   def apply(loc:Loc):Ecef = {
      val sinLat:Double = loc.latR.sin
      val cosLat:Double = loc.latR.cos
      val N:Length = a * math.sqrt(1d - e*e * sinLat*sinLat).inv
      // val N:Length = a / math.sqrt(1d - e*e * sinLat*sinLat) //TODO failure in dotty-0.10.0 is it fixed yet?
      val X:Length = (N           + loc.alt) * cosLat * loc.lonR.cos
      val Y:Length = (N           + loc.alt) * cosLat * loc.lonR.sin
      val Z:Length = (N*(1 - e*e) + loc.alt) * sinLat
      Ecef(X,Y,Z) //meters
   }
   def apply(X:Length, Y:Length, Z:Length):Ecef = new Ecef(Vec(X.m, Y.m, Z.m)) //meters
   def apply(vec:Vec):Ecef = new Ecef(vec) //meters

}
class Ecef(val vec:Vec) extends AnyVal{ //meters
   def X = Length(vec.x)
   def Y = Length(vec.y)
   def Z = Length(vec.z)
   //def -(that:Ecef):Ecef = Ecef(this.vec - that.vec)
   //def +(that:Ecef):Ecef = Ecef(this.vec + that.vec)
   override def toString = s"Ecef($X,$Y,$Z)"

   def loc:Loc = Loc(this)

   /** spherical interpolation (great circle)  wikipedia://Slerp*/
   def slerp(that:Ecef)(t:Double):Ecef = Ecef((this.vec slerp that.vec)(t))
   /** linear interpolation*/
   def lerp(that:Ecef)(t:Double):Ecef = Ecef((this.vec lerp that.vec)(t))

   /**nudge by ned*/
   def -(ned:Ned):Ecef = this + (-ned)
   def +(ned:Ned):Ecef = {
      //init enu coordinates
      val n = ned.vec.x; val e = ned.vec.y; val d = ned.vec.z // val Vec(n,e,d) = ned.vec
      val u = -d  //to support enu from ned
      val loc = this.loc
      val latR = loc.latR
      val lonR = loc.lonR
      //calc
      val X = -lonR.sin*e  - latR.sin*lonR.cos*n   + latR.cos*lonR.cos*u;
      val Y =  lonR.cos*e  - latR.sin*lonR.sin*n   + latR.cos*lonR.sin*u;
      val Z =                latR.cos         *n   + latR.sin         *u;
      Ecef(vec + Vec(X,Y,Z))
   }
}

object Loc{
   //val  NYC = ???
   //val  LON = ???
   def apply(lat:Double, lon:Double, alt:Length=Length(0)):Loc = new Loc(lat,lon, alt) //alt in Meters
   /** wikipedia://Geographic_coordinate_conversion using Bowrings irrational geodedic-lattitude equation with Newton-Raphson*/
   def apply(ecef:Ecef):Loc = {
      //constants
      val e = Ecef.e
      val a = Ecef.a.m
      val k0 = 1/(1 - e*e)
      //init
      val Vec(x,y,z) = ecef.vec
      val p = math.sqrt(x*x + y*y)

      def kNext(k:Double) = {
         val c = ((p*p + (1-e*e)*z*z*k*k)**1.5)/(a*e*e)
         (c + (1-e*e)*(z*z)*(k*k*k))/(c - p*p)
      }

      val k = kNext(kNext(kNext(k0))) //TODO make automatic number of iterations instead of fixed 3

      //val k = p/z*lat.tan
      val lat = (k*z/p).atan
      val lon = math.atan2(y,x)
      val alt = 1/(e*e)*(1/k - 1/k0)*math.sqrt(p*p + z*z*k*k)
      Loc(lat*rad2deg,lon*rad2deg,Length(alt))
   }
}
class Loc(val lat:Double, val lon:Double, val alt:Length){  //alt in Meters

   lazy val latR = lat*deg2rad
   lazy val lonR = lon*deg2rad
   lazy val ecef = Ecef(this)
   lazy val utm  = Utm(this)

   def slerp(that:Loc)(t:Double):Loc = (this.ecef slerp that.ecef)(t).loc
   def lerp(that:Loc)(t:Double):Loc = (this.ecef lerp that.ecef)(t).loc

   def +(ned:Ned):Loc = (ecef + ned).loc
   def -(ned:Ned):Loc = (ecef - ned).loc

   /** great circle bearing
     * http://www.movable-type.co.uk/scripts/latlong.html
     */
   def greatBearingTo(that:Loc):Angle = {
      val dLon = that.lonR - this.lonR
      val y = dLon.sin * that.latR.cos
      val x = this.latR.cos*that.latR.sin  -  this.latR.sin*that.latR.cos*dLon.cos
      Angle(y atan2 x)
   }

   /** great circle dist
     * http://www.movable-type.co.uk/scripts/latlong.html
     */
   def greatDist(that:Loc):Length = {
      val dLat = that.latR - this.latR
      val dLon = that.lonR - this.lonR

      val sinHalfdLat = (dLat/2).sin
      val sinHalfdLon = (dLon/2).sin
      val a = sinHalfdLat*sinHalfdLat + this.latR.cos*that.latR.cos*sinHalfdLon*sinHalfdLon
      val c = 2*(a.sqrt atan2 (1-a).sqrt)

      Ecef.r*c  //radius of the earth
   }

   /**accurate but slower version of `~>` (using test:runMain LocSpeedTest shows about 4x slower) 
      This uses an intermediate ECEF coordinate transformation
     */
   def nedTo(that:Loc):Ned = {
      val delta = that.ecef.vec - this.ecef.vec
      val e = -         lonR.sin*delta.x  +           lonR.cos*delta.y
      val n = -latR.sin*lonR.cos*delta.x  -  latR.sin*lonR.sin*delta.y   +  latR.cos*delta.z
      val u =  latR.cos*lonR.cos*delta.x  +  latR.cos*lonR.sin*delta.y   +  latR.sin*delta.z
      val d = -u
      Ned(Vec(n,e,d))
   }
   2017-07-30("use nedTo(Loc) instead to reserve the drx ~> semantic meanings for something else","v0.1.21c")
   def ~>(that:Loc):Ned = nedToApproximate(that)
   /**approximate but faster version of `~>` (using test:runMain LocSpeedTest shows about 4x faster)
     * This function uses the average radius of the earth and assumes small angles
     */
   def nedToApproximate(that:Loc):Ned = {
      val R = Ecef.r + (this.alt + that.alt)/2    //radius of the earth + avg altitude
      val n = R*(that.latR - this.latR)
      val e = R*(that.lonR - this.lonR)*math.cos((this.latR + that.latR)/2)
      val d = that.alt - this.alt
      Ned(n, e, d)
   }

   override def toString = s"Loc(${lat}deg, ${lon}deg, ${alt.m}m)"
}

/** https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system */
object Utm{

   sealed trait Hemisphere{ val N0:Double; val symbol:String }
   case object North extends Hemisphere{ val N0 = 0.0 ;    val symbol="N"}
   case object South extends Hemisphere{ val N0 = 10000E3; val symbol="S" }

   import math._
   val a = 6378137.0
   val f_inv = 298.257223563 //f = 1.0/298.257223563
   private val n  = 1.0/(2.0*f_inv-1.0)
   private val n2 = n*n
   private val n3 = n*n*n
   private val A = a/(1.0+n)*(1.0 + n2/4 + n3*n/64 /*+ ...*/)
   private val alpha = Array(0, n/2 - n2*2/3 + n3*5/16,   n2*13/48 + n3*3/5,    n3*61/240)
   private val beta  = Array(0, n/2 - n2*2/3 + n3*37/96,  n2/48    + n3/15,     n3*17/480)
   private val delta = Array(0, n*2 - n2*2/3 - n3*2,      n2*7/3   - n3*8/5,    n3*56/15)

   private val k0 = 0.9996
   private val k0A = k0*A
   private val E0 = 500E3

   private val nrootJib = n.sqrt*2/(n+1)

   private def trigSum(coef:Int => Double, termA:Double=>Double, paramA:Double, termB:Double=>Double, paramB:Double):Double =
      (1 to 3).foldLeft(0.0){case (a,j) => a + coef(j)*termA(2*j*paramA)*termB(2*j*paramB)}

   def toLoc(utm:Utm):Loc = {//wikipedia implementation
      val zeta   = (utm.n.m - utm.hemisphere.N0)/k0A
      val mu     = (utm.e.m - E0)/k0A
      val zetaP  = zeta - trigSum(j => beta(j),     sin, zeta, cosh, mu)
      val muP    = mu   - trigSum(j => beta(j),     cos, zeta, sinh, mu)
      val chi    = (zetaP.sin / muP.cosh).asin
      val lat    = chi  + trigSum(j => delta(j),    sin, chi,  _=>1, 1)  //radians
      val lon    = lon0(utm.zone) + ( muP.sinh / zetaP.cos ).atand //degrees
      Loc(lat*rad2deg,lon,utm.alt)
   }

   def zone(loc:Loc):Int = ((loc.lon + 180)/6).toInt % 60 + 1 //TODO need to add special zone mods for Svalbard
   def lon0(zone:Int):Double = 6.0*zone - 183.0 //degrees

   private val num = """([\+\-\d\.]+)\s*([A-Z]*)\s*""" //broad but brittle //a little more robust using drx.Parse[Double]
   private lazy val UtmPat   = raw".*?(?i)$num$num$num($num)?.*".r //three fields required 4th for altitutde
   private def make(d:Double,u:String):Length = {
      val utrim = u.trim
      (for(z <- utrim.lastOption if z == 'n' || z == 'N' || z == 'e' || z == 'E') yield
          Length(d,utrim.dropRight(1))
      ) getOrElse Length(d)
   }
   private def make(z:Int,zu:String,  ad:Double,au:String, bd:Double,bu:String, alt:Length):Utm = {
      val isNorth = (zu == "" && z >= 0) || zu.toUpperCase.startsWith("N") //answers north as well
      val hemi = isNorth.getOrElse(North,South)
      if(au == "" || (au endsWith "e") || (au endsWith "E")) Utm(z.abs,hemi, make(ad,au),  make(bd,bu), alt)
      else                                                   Utm(z.abs,hemi, make(bd,bu),  make(ad,au), alt)
   }
   def apply(loc:String):Utm = loc match {
      case UtmPat(ad,au, bd,bu, cd,cu, _, altd,altu) =>
         val a = Parse[Double](ad)
         val b = Parse[Double](bd)
         val c = Parse[Double](cd)
         val alt = Option(altd).map{s => make(Parse[Double](s),altu)}.getOrElse(0.m) //regex empty match is null
         if(     a.abs <= 60) make(a.toInt,au,   b,bu,  c,cu, alt)  //first slot is the zone
         else if(c.abs <= 60) make(c.toInt,cu,   a,au,  b,bu, alt)  //second slot is the zone
         else                 make(b.toInt,bu,   a,au,  c,cu, alt)   //third slot is the zone
      case _ => Utm(0,North,0.m,0.m) //TODO add throw of parse exception
   }
   def apply(loc:Loc):Utm = {//wikipedia simple implementation of lat,lon -> Utm coord
      val hemisphere = if(loc.lat >= 0) Utm.North else Utm.South
      val latsin = loc.lat.sind //assuming degrees
      val t = (  latsin.atanh - nrootJib*(nrootJib*latsin).atanh   ).sinh
      val zn:Int = zone(loc)
      val lonDelta = loc.lon - lon0(zn)
      val zetaP = (t/lonDelta.cosd).atan
      val muP = (lonDelta.sind/(1+t*t).sqrt).atanh
      val sigma = 1.0 + trigSum(j => alpha(j)*2*j, cos, zetaP, cosh, muP)
      val tau   =       trigSum(j => alpha(j)*2*j, sin, zetaP, sinh, muP)
      val E:Double =            E0 + k0A*(muP   + trigSum(j => alpha(j), cos, zetaP, sinh, muP))
      val N:Double = hemisphere.N0 + k0A*(zetaP + trigSum(j => alpha(j), sin, zetaP, cosh, muP))
      Utm(zn,hemisphere,Length(E),Length(N),loc.alt)
   }
   implicit object ParsableUtm extends Parsable[Utm]{
      def apply(v:String):Utm = Utm(v)
   }
}
case class Utm(val zone:Int, val hemisphere:Utm.Hemisphere, val e:Length, val n:Length, alt:Length=Length(0)){  //no altitude 
   assert(1 <= zone && zone <= 61) //TODO remove for compiled release
   override def toString = s"Utm($format, alt=${alt.m.toInt})"
   private def altToString =  if(alt == 0.m) "" else " " + alt.m.round.toInt
   def format = s"$zone${hemisphere.symbol} ${e.m.round.toInt} ${n.m.round.toInt}" + altToString
   lazy val loc = Utm.toLoc(this)

   def +(ned:Ned):Utm = Utm(zone, hemisphere, e+ned.e, n+ned.n, alt-ned.d)
   def -(ned:Ned):Utm = Utm(zone, hemisphere, e-ned.e, n-ned.n, alt+ned.d)
   //swap the north and east components (useful to see if someone switch the order of the units)
   def swap:Utm = Utm(zone, hemisphere, n,e, alt)

   def nedTo(that:Utm):Ned = {
      assert(this.zone == that.zone, s"Utm zones do not match in $this and $that")
      Ned(that.n-this.n, that.e-this.e, that.alt-this.alt)
   }
}