content: {-# OPTIONS_GHC -fno-warn-orphans #-}
  {-# LANGUAGE DataKinds #-}
  {-# LANGUAGE TypeOperators #-}
  {-# LANGUAGE QuasiQuotes #-}
  {-# LANGUAGE FlexibleInstances #-}
  
  module PID where
  
  import Ivory.Compile.C.CmdlineFrontend
  import Ivory.Language
  
  [ivory|
  
  struct PID
    { pid_mv  :: Stored IFloat
    ; pid_i   :: Stored IFloat
    ; pid_err :: Stored IFloat
    }
  
  |]
  
  type SP   = IFloat -- Set point
  type PV   = IFloat -- Process (measured) value
  type Time = IFloat
  
  {-
  void pid_update(struct PID * pid,
                  sp_t sp,
                  pv_t pv,
                  timeinc_t dt
                  )
  {
    float err = sp - pv;
    float i   = pid->i + err*dt;
    float d   = (err - pid->err) / dt;
    pid->i    = ki*i;
    pid->mv   = kp*err + pid->i + kd*d;
    pid->err  = err;
    return;
  }
  -}
  
  kp, ki, kd :: IFloat
  kp = 1.0
  ki = 0.1
  kd = 0.1
  
  pidUpdate :: Def ('[ Ref s ('Struct "PID")
                     , SP
                     , PV
                     , Time ]
                    ':-> IFloat)
  pidUpdate = proc "pid_update" $
    \ pid sp pv dt ->
    -- These are made up requires/ensures for testing purposes.
      requires (checkStored (pid ~> pid_err) (\err -> err <? 1))
    $ ensures  (\res -> checkStored (pid ~> pid_err) (\err -> err <? res))
    $ body
    $ do
      err     <- assign (sp - pv)
      i       <- deref $ pid ~> pid_i
      i'      <- assign  $ ki * (i + err*dt)
      prevErr <- deref $ pid ~> pid_err
      d       <- assign  $ (err - prevErr) / dt
      store (pid ~> pid_i)   i'
      store (pid ~> pid_mv)  (kp*err + i' + kd*d)
      store (pid ~> pid_err) err
      ret err
  
  foo :: Def ('[ Ref s ('Array 3 ('Stored Uint32))
               , Ref s ('Array 3 ('Stored Uint32)) ] ':-> ())
  foo = proc "foo" $ \a b ->
  --  requires (*a!0 < *b!0)
    requires (checkStored (a ! 0)
                (\v -> (checkStored (b ! 0)
                       (\v1 -> v <? v1))))
    $ body $ do
      retVoid
  
  runPID :: IO ()
  runPID = runCompiler [cmodule] []
    initialOpts { outDir = Nothing, bitShiftCheck = True, divZero = True }
  
  cmodule :: Module
  cmodule = package "PID" $ do
    incl foobar
    -- defStruct (Proxy :: Proxy "PID")
    -- incl pidUpdate
    -- incl alloc_test
  
  foobar :: Def ('[Uint8] ':-> Uint8)
  foobar = proc "foobar" $ \x -> body $ do
    ret (x `iShiftR` (3 `iDiv` 2))
  
  alloc_test :: Def ('[] ':-> IFloat)
  alloc_test  = proc "alloc_test" $ body $ do
    pid <- local (istruct [pid_i .= ival 1])
    ret =<< deref (pid ~> pid_i)
  