{-# OPTIONS_GHC -Wall #-}
{-# LANGUAGE NoImplicitPrelude #-}

module Data.Aviation.Navigation.Vector(
  Vector(..)
, vectorDegrees
, HasVector(..)
) where

import Control.Category ( Category(id, (.)) )
import Control.Lens ( view, Lens' )
import Data.Eq ( Eq )
import Data.Functor ( Functor(fmap) )
import Data.Monoid ( Monoid(mempty) )
import Data.Ord ( Ord )
import Data.Radian ( fromRadians )
import Data.Semigroup ( Semigroup((<>)) )
import GHC.Show(Show)
import Prelude(Double, Num((*), (-), (+)), Fractional((/)), sqrt, atan, sin, cos, pi)

data Vector =
  Vector
    Double -- angle
    Double -- magnitude
  deriving (Eq, Ord, Show)

vectorDegrees ::
  Double
  -> Double
  -> Vector
vectorDegrees =
  Vector . view fromRadians

instance Semigroup Vector where
  Vector aa am <> Vector ba bm =
    let square x = x * x
        t = aa - ba
        mag = sqrt (square am + square bm - 2 * am * bm * cos (pi - t))
        ang = atan (bm * sin t / (am + bm * cos t))
    in  Vector (aa - ang) mag

instance Monoid Vector where
  mempty =
    Vector 0 0

class HasVector a where
  vector ::
    Lens' a Vector
  {-# INLINE angle #-}
  angle ::
    Lens' a Double
  angle =
    vector . angle
  {-# INLINE magnitude #-}
  magnitude ::
    Lens' a Double
  magnitude =
    vector . magnitude

instance HasVector Vector where
  vector =
    id
  {-# INLINE angle #-}
  angle f (Vector a m) =
    fmap (\a' -> Vector a' m) (f a)
  {-# INLINE magnitude #-}
  magnitude f (Vector a m) =
    fmap (\m' -> Vector a m') (f m)