module AGV where

type Angle = Float
type Speed = (Angle,Float)
type Pos   = (Float,Float)

calcpos  :: [Angle] -> [Pos] -> Pos
regulate :: Pos -> Pos -> Speed -> Speed
room     :: [Pos]

calcpos  = undefined
regulate = undefined
room     = undefined

----------------------------------------------------------------
struct Driver =
   new_scan   :: [Angle] -> Action
   new_path   :: [Pos] -> Action
   
driver servo =
   template
      speed := (0.0,0.0)
      path  := repeat (0.0,0.0)
   in struct
      new_scan angles = action
         let is_pos           = calcpos angles room
             should_pos:path' = path
             speed'           = regulate is_pos should_pos speed
         speed := speed'
         path  := path'
         servo.set_speed speed'
      new_path p = action
         path := p

-----------------------------------------------------------------
struct Scanner =
   detect     :: Action
   zero_cross :: Action

scanner reg driver =
   template
      angles := []
   in struct
      detect = action
         a <- reg.load
         angles := 2*pi*(fromIntegral a)/4000 : angles
      zero_cross = action
         driver.new_scan angles
         angles := []

-----------------------------------------------------------------
struct Servo =
   set_speed  :: Speed -> Action
   
servo :: Register -> Register -> Template Servo
servo  = undefined

-----------------------------------------------------------------
struct Radio =
   incoming :: Action
   
radio :: Register -> Driver -> Template Radio
radio  = undefined

-----------------------------------------------------------------
struct Register =
   load  :: Request Int
   store :: Int -> Action

memory_map :: Int -> Template Register
memory_map  = undefined

-----------------------------------------------------------------
struct Interrupts =
    install :: Int -> Action -> Action

main interrupts = do
   reg1 <- memory_map 0xFFFF0001
   reg2 <- memory_map 0xFFFF0002
   reg3 <- memory_map 0xFFFF0003
   reg4 <- memory_map 0xFFFF0004

   serv <- servo reg1 reg2
   driv <- driver serv
   scan <- scanner reg3 driv
   comm <- radio reg4 driv
   
   interrupts.install 0x80 scan.detect
   interrupts.install 0x81 scan.zero_cross
   interrupts.install 0x82 comm.incoming