-- | This example shows an implementation of the Well-Clear Violation
-- algorithm, it follows the implementation described in 'Analysis of
-- Well-Clear Bounday Models for the Integration of UAS in the NAS',
-- https://ntrs.nasa.gov/citations/20140010078.

{-# LANGUAGE DataKinds #-}
{-# LANGUAGE RebindableSyntax #-}

module Copilot.Verifier.Examples.ShouldPass.WCV where

import Language.Copilot
-- import qualified Copilot.Theorem.What4 as CT
import Copilot.Compile.C99
import Copilot.Verifier ( Verbosity, VerifierOptions(..)
                        , defaultVerifierOptions, verifyWithOptions )

-- import Data.Foldable (forM_)
import qualified Control.Monad as Monad


-- | `dthr` is the horizontal distance threshold.
dthr :: Stream Double
dthr :: Stream Double
dthr = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"dthr" Maybe [Double]
forall a. Maybe a
Nothing

-- | `tthr` is the horizontal time threshold.
tthr :: Stream Double
tthr :: Stream Double
tthr = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"tthr" Maybe [Double]
forall a. Maybe a
Nothing

-- | `zthr` is the vertical distance / altitude threshold.
zthr :: Stream Double
zthr :: Stream Double
zthr = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"zthr" Maybe [Double]
forall a. Maybe a
Nothing

-- | `tcoathr` is the vertical time threshold.
tcoathr :: Stream Double
tcoathr :: Stream Double
tcoathr = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"tcoathr" Maybe [Double]
forall a. Maybe a
Nothing

type Vect2 = (Stream Double, Stream Double)


------------------
-- The following section contains basic libraries for working with vectors.
------------------

-- | Multiply two Vectors.
(|*|) :: Vect2 -> Vect2 -> Stream Double
|*| :: Vect2 -> Vect2 -> Stream Double
(|*|) (Stream Double
x1, Stream Double
y1) (Stream Double
x2, Stream Double
y2) = (Stream Double
x1 Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
x2) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
+ (Stream Double
y1 Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
y2)

-- | Calculate the square of a vector.
sq :: Vect2 -> Stream Double
sq :: Vect2 -> Stream Double
sq Vect2
x = Vect2
x Vect2 -> Vect2 -> Stream Double
|*| Vect2
x

-- | Calculate the length of a vector.
norm :: Vect2 -> Stream Double
norm :: Vect2 -> Stream Double
norm = Stream Double -> Stream Double
forall a. Floating a => a -> a
sqrt (Stream Double -> Stream Double)
-> (Vect2 -> Stream Double) -> Vect2 -> Stream Double
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vect2 -> Stream Double
sq

-- | Calculate the determinant of two vectors.
det :: Vect2 -> Vect2 -> Stream Double
det :: Vect2 -> Vect2 -> Stream Double
det (Stream Double
x1, Stream Double
y1) (Stream Double
x2, Stream Double
y2) = (Stream Double
x1 Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
y2) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
- (Stream Double
x2 Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
y1)

-- | Compare two vectors, taking into account the small error that is
-- introduced by the usage of `Double`s.
(~=) :: Stream Double -> Stream Double -> Stream Bool
Stream Double
a ~= :: Stream Double -> Stream Double -> Stream Bool
~= Stream Double
b = (Stream Double -> Stream Double
forall a. Num a => a -> a
abs (Stream Double
a Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
- Stream Double
b)) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
< Stream Double
0.001

-- | Negate a vector.
neg :: Vect2 -> Vect2
neg :: Vect2 -> Vect2
neg (Stream Double
x, Stream Double
y) = (Stream Double -> Stream Double
forall a. Num a => a -> a
negate Stream Double
x, Stream Double -> Stream Double
forall a. Num a => a -> a
negate Stream Double
y)


--------------------
-- From here on the algorithm, as described by the paper mentioned on the top
-- of this file, is implemented. Please refer to the paper for details.
--------------------

tau :: Vect2 -> Vect2 -> Stream Double
tau :: Vect2 -> Vect2 -> Stream Double
tau Vect2
s Vect2
v = if Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
< Stream Double
0
            then (-(Vect2 -> Stream Double
sq Vect2
s)) Stream Double -> Stream Double -> Stream Double
forall a. Fractional a => a -> a -> a
/ (Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v)
            else -Stream Double
1

tcpa :: Vect2 -> Vect2 -> Stream Double
tcpa :: Vect2 -> Vect2 -> Stream Double
tcpa Vect2
s v :: Vect2
v@(Stream Double
vx, Stream Double
vy) = if Stream Double
vx Stream Double -> Stream Double -> Stream Bool
~= Stream Double
0 Stream Bool -> Stream Bool -> Stream Bool
&& Stream Double
vy Stream Double -> Stream Double -> Stream Bool
~= Stream Double
0
                      then Stream Double
0
                      else -(Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v)Stream Double -> Stream Double -> Stream Double
forall a. Fractional a => a -> a -> a
/(Vect2 -> Stream Double
sq Vect2
v)

taumod :: Vect2 -> Vect2 -> Stream Double
taumod :: Vect2 -> Vect2 -> Stream Double
taumod Vect2
s Vect2
v = if Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
< Stream Double
0
               then (Stream Double
dthr Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
dthr Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
- (Vect2 -> Stream Double
sq Vect2
s))Stream Double -> Stream Double -> Stream Double
forall a. Fractional a => a -> a -> a
/(Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v)
               else -Stream Double
1

tep :: Vect2 -> Vect2 -> Stream Double
tep :: Vect2 -> Vect2 -> Stream Double
tep Vect2
s Vect2
v = if (Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
< Stream Double
0) Stream Bool -> Stream Bool -> Stream Bool
&& ((Vect2 -> Vect2 -> Stream Double -> Stream Double
delta Vect2
s Vect2
v Stream Double
dthr) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
>= Stream Double
0)
            then Vect2 -> Vect2 -> Stream Double -> Stream Double -> Stream Double
theta Vect2
s Vect2
v Stream Double
dthr (-Stream Double
1)
            else -Stream Double
1

delta :: Vect2 -> Vect2 -> Stream Double -> Stream Double
delta :: Vect2 -> Vect2 -> Stream Double -> Stream Double
delta Vect2
s Vect2
v Stream Double
d = (Stream Double
dStream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
*Stream Double
d) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* (Vect2 -> Stream Double
sq Vect2
v) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
- ((Vect2 -> Vect2 -> Stream Double
det Vect2
s Vect2
v)Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
*(Vect2 -> Vect2 -> Stream Double
det Vect2
s Vect2
v))
-- Here the formula says : (s . orth v)^2 which is the same as det(s,v)^2

theta :: Vect2 -> Vect2 -> Stream Double -> Stream Double -> Stream Double
theta :: Vect2 -> Vect2 -> Stream Double -> Stream Double -> Stream Double
theta Vect2
s Vect2
v Stream Double
d Stream Double
e = (-(Vect2
s Vect2 -> Vect2 -> Stream Double
|*| Vect2
v) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
+ Stream Double
e Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* (Stream Double -> Stream Double
forall a. Floating a => a -> a
sqrt (Stream Double -> Stream Double) -> Stream Double -> Stream Double
forall a b. (a -> b) -> a -> b
$ Vect2 -> Vect2 -> Stream Double -> Stream Double
delta Vect2
s Vect2
v Stream Double
d)) Stream Double -> Stream Double -> Stream Double
forall a. Fractional a => a -> a -> a
/ (Vect2 -> Stream Double
sq Vect2
v)


tcoa :: Stream Double -> Stream Double -> Stream Double
tcoa :: Stream Double -> Stream Double -> Stream Double
tcoa Stream Double
sz Stream Double
vz = if (Stream Double
sz Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
vz) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
< Stream Double
0
               then (-Stream Double
sz) Stream Double -> Stream Double -> Stream Double
forall a. Fractional a => a -> a -> a
/ Stream Double
vz
               else -Stream Double
1

dcpa :: Vect2 -> Vect2 -> Stream Double
dcpa :: Vect2 -> Vect2 -> Stream Double
dcpa s :: Vect2
s@(Stream Double
sx, Stream Double
sy) v :: Vect2
v@(Stream Double
vx, Stream Double
vy) = Vect2 -> Stream Double
norm (Stream Double
sx Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
+ (Vect2 -> Vect2 -> Stream Double
tcpa Vect2
s Vect2
v) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
vx, Stream Double
sy Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
+ (Vect2 -> Vect2 -> Stream Double
tcpa Vect2
s Vect2
v) Stream Double -> Stream Double -> Stream Double
forall a. Num a => a -> a -> a
* Stream Double
vy)


--------------------------
-- Well clear Violation --
--------------------------

-- | Determines if the well clear property is violated or not.
wcv :: (Vect2 -> Vect2 -> Stream Double) ->
       Vect2 -> Stream Double ->
       Vect2 -> Stream Double ->
       Stream Bool
wcv :: (Vect2 -> Vect2 -> Stream Double)
-> Vect2 -> Stream Double -> Vect2 -> Stream Double -> Stream Bool
wcv Vect2 -> Vect2 -> Stream Double
tvar Vect2
s Stream Double
sz Vect2
v Stream Double
vz = ((Vect2 -> Vect2 -> Stream Double) -> Vect2 -> Vect2 -> Stream Bool
horizontalWCV Vect2 -> Vect2 -> Stream Double
tvar Vect2
s Vect2
v) Stream Bool -> Stream Bool -> Stream Bool
&& (Stream Double -> Stream Double -> Stream Bool
verticalWCV Stream Double
sz Stream Double
vz)

verticalWCV :: Stream Double -> Stream Double -> Stream Bool
verticalWCV :: Stream Double -> Stream Double -> Stream Bool
verticalWCV Stream Double
sz Stream Double
vz =
  ((Stream Double -> Stream Double
forall a. Num a => a -> a
abs (Stream Double -> Stream Double) -> Stream Double -> Stream Double
forall a b. (a -> b) -> a -> b
$ Stream Double
sz) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= Stream Double
zthr) Stream Bool -> Stream Bool -> Stream Bool
||
  (Stream Double
0 Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= (Stream Double -> Stream Double -> Stream Double
tcoa Stream Double
sz Stream Double
vz) Stream Bool -> Stream Bool -> Stream Bool
&& (Stream Double -> Stream Double -> Stream Double
tcoa Stream Double
sz Stream Double
vz) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= Stream Double
tcoathr)

horizontalWCV :: (Vect2 -> Vect2 -> Stream Double) -> Vect2 -> Vect2 -> Stream Bool
horizontalWCV :: (Vect2 -> Vect2 -> Stream Double) -> Vect2 -> Vect2 -> Stream Bool
horizontalWCV Vect2 -> Vect2 -> Stream Double
tvar Vect2
s Vect2
v =
  (Vect2 -> Stream Double
norm Vect2
s Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= Stream Double
dthr) Stream Bool -> Stream Bool -> Stream Bool
||
  (((Vect2 -> Vect2 -> Stream Double
dcpa Vect2
s Vect2
v) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= Stream Double
dthr) Stream Bool -> Stream Bool -> Stream Bool
&& (Stream Double
0 Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= (Vect2 -> Vect2 -> Stream Double
tvar Vect2
s Vect2
v)) Stream Bool -> Stream Bool -> Stream Bool
&& ((Vect2 -> Vect2 -> Stream Double
tvar Vect2
s Vect2
v) Stream Double -> Stream Double -> Stream Bool
forall a. (Ord a, Typed a) => Stream a -> Stream a -> Stream Bool
<= Stream Double
tthr))

spec :: Spec
spec :: Spec
spec = do
  -- External streams for relative position and velocity.
  let -- The relative x velocity between ownship and the intruder.
      vx :: Stream Double
      vx :: Stream Double
vx = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"relative_velocity_x" Maybe [Double]
forall a. Maybe a
Nothing

      -- The relative y velocity between ownship and the intruder.
      vy :: Stream Double
      vy :: Stream Double
vy = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"relative_velocity_y" Maybe [Double]
forall a. Maybe a
Nothing

      -- The relative z velocity between ownship and the intruder.
      vz :: Stream Double
      vz :: Stream Double
vz = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"relative_velocity_z" Maybe [Double]
forall a. Maybe a
Nothing

      -- The relative velocity as a 2D vector.
      v :: (Stream Double, Stream Double)
      v :: Vect2
v = (Stream Double
vx, Stream Double
vy)


      -- The relative x position between ownship and the intruder.
      sx :: Stream Double
      sx :: Stream Double
sx = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"relative_position_x" Maybe [Double]
forall a. Maybe a
Nothing

      -- The relative y position between ownship and the intruder.
      sy :: Stream Double
      sy :: Stream Double
sy = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"relative_position_y" Maybe [Double]
forall a. Maybe a
Nothing

      -- The relative z position between ownship and the intruder.
      sz :: Stream Double
      sz :: Stream Double
sz = String -> Maybe [Double] -> Stream Double
forall a. Typed a => String -> Maybe [a] -> Stream a
extern String
"relative_position_z" Maybe [Double]
forall a. Maybe a
Nothing

      -- The relative position as a 2D vector.
      s :: (Stream Double, Stream Double)
      s :: Vect2
s = (Stream Double
sx, Stream Double
sy)
  WriterT [SpecItem] Identity (PropRef Universal) -> Spec
forall (f :: * -> *) a. Functor f => f a -> f ()
Monad.void (WriterT [SpecItem] Identity (PropRef Universal) -> Spec)
-> WriterT [SpecItem] Identity (PropRef Universal) -> Spec
forall a b. (a -> b) -> a -> b
$ String
-> Prop Universal
-> WriterT [SpecItem] Identity (PropRef Universal)
forall a. String -> Prop a -> Writer [SpecItem] (PropRef a)
prop String
"1a" (Stream Bool -> Prop Universal
forAll (Stream Bool -> Prop Universal) -> Stream Bool -> Prop Universal
forall a b. (a -> b) -> a -> b
$ (Vect2 -> Vect2 -> Stream Double
tau Vect2
s Vect2
v) Stream Double -> Stream Double -> Stream Bool
~= (Vect2 -> Vect2 -> Stream Double
tau (Vect2 -> Vect2
neg Vect2
s) (Vect2 -> Vect2
neg Vect2
v)))
  -- Monad.void $ prop "3d" (forAll $ (wcv tep s sz v vz)    == (wcv tep (neg s) (-sz) (neg v) (-vz)))
  String -> Stream Bool -> [Arg] -> Spec
trigger String
"well_clear_violation" ((Vect2 -> Vect2 -> Stream Double)
-> Vect2 -> Stream Double -> Vect2 -> Stream Double -> Stream Bool
wcv Vect2 -> Vect2 -> Stream Double
tep Vect2
s Stream Double
sz Vect2
v Stream Double
vz) []

verifySpec :: Verbosity -> IO ()
verifySpec :: Verbosity -> IO ()
verifySpec Verbosity
verb = do
  Spec
spec' <- Spec -> IO Spec
forall a. Spec' a -> IO Spec
reify Spec
spec
  VerifierOptions -> CSettings -> [String] -> String -> Spec -> IO ()
verifyWithOptions VerifierOptions
defaultVerifierOptions{verbosity = verb}
                    CSettings
mkDefaultCSettings [] String
"wcv" Spec
spec'

{-
  -- Use Z3 to prove the properties.
  results <- CT.prove CT.Z3 spec'

  -- Print the results.
  forM_ results $ \(nm, res) -> do
    putStr $ nm <> ": "
    case res of
      CT.Valid -> putStrLn "valid"
      CT.Invalid -> putStrLn "invalid"
      CT.Unknown -> putStrLn "unknown"
-}