module WhereMyRomba where

open import Function
open import Data.Integer
open import Data.List
open import Data.Product
open import Data.List.Membership.Propositional
open import Data.List.Membership.DecPropositional using (_∈?_)
open import Data.List.Relation.Unary.Any using (here; there)
open import Relation.Nullary.Decidable
open import Relation.Binary.Definitions using (Decidable)
open import Relation.Binary.PropositionalEquality hiding ([_])

-- This lemma simply says that list membership for tuples of integers is decidable.
-- I.e. given a tuple of integers x, and a list of tuples of integers xs, there is an
-- algorithmn that determines wheter or not x is in xs...
lemm : ∀ (x : ℤ × ℤ) xs → Dec (x ∈ xs)
lemm = _∈?_ lemm₂
  where
  lemm₂ : ∀ (x y : ℤ × ℤ) → Dec (x ≡ y)
  lemm₂ (x₁ , x₂) (y₁ , y₂) with x₁ ≟ y₁ | x₂ ≟ y₂
  ...                          | no h     | _     = no λ{ refl → h refl}
  ...                          | yes _    | no t  = no λ{ refl → t refl}
  ...                          | yes refl | yes refl  = yes refl

data Direction : Set where
  N : Direction
  E : Direction
  S : Direction
  W : Direction

data Action : Set where
  F : Action
  L : Action
  R : Action

-- This define a datatype for the romba state.
-- It simply consists of a direction and a position, you can use the constructor
-- `＠` like this: <direction> ＠ <position>
-- You can also use 𝒹 and 𝓅 to extract the direction and position.
record RombaState : Set where
  constructor
    _＠_
  field
    𝒹 : Direction
    𝓅 : ℤ × ℤ
open RombaState

move : Direction → ℤ × ℤ → ℤ × ℤ
move N (x , y) = x , y - + 1
move E (x , y) = x + + 1 , y
move S (x , y) = x , y + + 1
move W (x , y) = x - + 1 , y

turnLeft : Direction → Direction
turnLeft N = W
turnLeft E = N
turnLeft S = E
turnLeft W = S

-- Turning right is the same as turning left 3 times :-)
turnRight = turnLeft ∘ turnLeft ∘ turnLeft

{-

This function is where the important stuff happens, it describes a single movement
of the romba, reacting to possible obstacles.

In this function we encode a basic invariant: The romba is assumed to not be inside
an obstacle! Given this assumption, we also prove that the romba won't end up inside a
wall.

-}
step : Action                                 -- Given an action,
     → (obstacles : List (ℤ × ℤ))             -- a set of obstacles
     → (s₁ : RombaState)                      -- a romba state,
     → 𝓅 s₁ ∉ obstacles                       -- and a proof that the romba isn't inside an obstacle.
     → Σ RombaState (λ s₂ → 𝓅 s₂ ∉ obstacles) -- Returns a new romba state, and a proof that it still isn't
                                              -- on an obstacle.

{-
 With is like a `case .. of`. Here we use the with to check wheter or not moving the
 romba in its current direction would place it inside an obstacle, i.e. if it's new
 position would be an element in the list of obstacles. This is what we need the
 lemma for.
-}
step F obstacles (𝒹 ＠ 𝓅) h with lemm (move 𝒹 𝓅) obstacles
-- If moving it would not place it inside an obstacle, then we know... well, that
-- moving it would not place it inside an obstacle of course! This the proof that
-- the variable `t` contains.
...                            | no t = 𝒹 ＠ move 𝒹 𝓅 , t
-- Othewise, we simply don't move, and use the fact the we are not currently inside
-- and obstecla to prove that we will not end up inside and obstale (`h`)
...                            | yes _ = 𝒹 ＠ 𝓅 , h
step L obstacles (𝒹 ＠ 𝓅) h = turnLeft 𝒹 ＠ 𝓅 , h
step R obstacles (𝒹 ＠ 𝓅) h = turnRight 𝒹 ＠ 𝓅 , h

-- In romba' and romba we just chain a bunch of `step` calls together.

romba' : (s₁ : RombaState)
       → (obstacles : List (ℤ × ℤ))
       → (𝓅 s₁ ∉ obstacles)
       → List Action
       → Σ RombaState (λ s₂ → 𝓅 s₂ ∉ obstacles)
romba' s₁ obstacles h [] = s₁ , h
romba' s₁ obstacles h (action ∷ actions) =
  let
    s₂ , t = step action obstacles s₁ h
  in
  romba' s₂ obstacles t actions

romba : (obstacles : List (ℤ × ℤ))
      → (+ 0 , + 0) ∉ obstacles
      → List Action
      → RombaState
romba obstacles h actions = proj₁ (romba' (N ＠ (+ 0 , + 0)) obstacles h actions)


-- Examples

-- For example, if the robot starts at (0, 0) and faces north, moving forward would
-- place it at (0, -1)
ex₀ : move N (+ 0 , + 0) ≡ (- + 0 , - + 1)
ex₀ = refl

-- For example, if the robot starts at (0, 0) and faces north, moving forward
-- would place it at (0, -1). If it turned right and moved forward again, it
-- would be at (1, -1)
ex₁ : move (turnRight N) (move N (+ 0 , + 0)) ≡ (+ 1 , - + 1)
ex₁ = refl

-- Actions: "FRFFRFFRFFRF"
-- Obstacles: No obstacles
-- Expected: (0, 0)
ex₂ : 𝓅 (romba [] (λ()) (F ∷ R ∷ F ∷ F ∷ R ∷ F ∷ F ∷ R ∷ F ∷ F ∷ R ∷ F ∷ [])) ≡ (+ 0 , + 0)
ex₂ = refl

-- Actions: "FRFFRFF"
-- Obstacles: (0, -1)
-- Expected: (2, 2)
ex₃ : 𝓅 (romba [ + 0 , - + 1 ] (λ{ (here ()); (there ())}) (F ∷ R ∷ F ∷ F ∷ R ∷ F ∷ F ∷ [])) ≡ (+ 2 , + 2)
ex₃ = refl

-- Actions: "FRFRRFLF"
-- Obstacles: (0, -1), (1, 0)
-- Expected: (-1, 1) 
ex₄ : 𝓅 (romba ((+ 0 , - + 1) ∷ (+ 1 , + 0) ∷ []) (λ{ (there (here ()))}) (F ∷ R ∷ F ∷ R ∷ R ∷ F ∷ L ∷ F ∷ [])) ≡ (- + 1 , + 1)
ex₄ = refl