{-# LANGUAGE RecordWildCards #-}

module System.Hardware.Acquisition.Adaptor.PID
	( Parameters (..)
	, adaptor
	) where

import Prelude
import Control.Monad.Error
import Data.Maybe
import Data.IORef
import Text.Printf

import System.Hardware.Acquisition as Acquisition

data Parameters = Parameters
	{ kp :: Value {-^ gain; proportional constant -}
	, ti :: Value {-^ integral time -}
	, td :: Value {-^ derivative time -}
	} deriving (Show)

type State = (Value, Value)
{-^ (integral, previous) -}

{-|
	PID (proportional-integral-derivative) controller. Implements a
	discrete version of the following idealised equation:

	@out[t] = kp ( ϵ[t]  +  1\/ti × ∫ϵ[t]dt  +  td × dϵ[t]/dt )@

	where @ϵ[t] = setPoint[t] − in[t]@; increasing values of
	@t@ refer to earlier inputs. The actual implementation is:

	@out[t] = kp ( ϵ[t]  +  1\/ti × ∑ϵ[t]  +  td × (in[t] - in[t-1]) )@

	Note that the derivative term is modified to measure changes to @in[t]@
	rather than @ϵ[t]@, to avoid discontinuities caused by
	instantaneous step changes to the @setPoint@.

	The @setPoint@ and 'Parameters' are set via 'IORef's, and may be changed
	at run-time. (TODO: Perhaps these inputs ought to be 'Protocol's or
	'Channel's? At least the @setPoint@, to allow for cascaded PID
	controllers...)

	See <http://en.wikipedia.org/wiki/PID_controller> for more details.
-}
adaptor
	:: MonadIO m
	=> Channel {-^ setPoint: desired value of process variable -}
	-> IORef Parameters
	-> Adaptor m
adaptor setPointChan params c = liftIO $ do
	state <- newIORef (0.0, 0.0)

	let getSample :: (Error e, MonadError e m, MonadIO m) => m Sample
	    getSample = do
		Parameters {..} <- liftIO (readIORef params)
		setPoint <- return value `ap` Acquisition.getSample setPointChan
		Sample {..} <- Acquisition.getSample c
		let proportional = setPoint - value
		(integral, derivative) <- liftIO $ atomicModifyIORef state $
			\ (int, previous) -> let
				integral = int + proportional
				derivative = value - previous in
				( (integral, value)
				, (integral, derivative) )
		let value = kp *
			( proportional
			+ integral / ti
			+ derivative * td )
		liftIO (printf "SP: %.1f, PID: %.1f, %.1f, %.1f\n"
			setPoint proportional integral derivative)
		return Sample {..}

	let close :: MonadIO m => m ()
	    close = return ()

	return Channel {..}