{-# LANGUAGE DataKinds #-}
{-# LANGUAGE OverloadedRecordDot #-}
{-# LANGUAGE OverloadedStrings #-}

module Hsm.Drive
  ( Speed(Slow, Medium, Fast)
  , Direction(Forward, Backward)
  , Duration
  , DriveEnv(..)
  , Command(Move)
  , pwmMapperDefault
  , driveEnvDefault
  , drive
  ) where

import Control.Concurrent (threadDelay)
import Data.Text (pack)
import Data.Typeable (Typeable)
import Foreign.Ptr (Ptr)
import GHC.Records (HasField)
import Hsm.GPIO (GPIO(..), LineRequest, active, inactive, setPins)
import Hsm.Log qualified as L
import Hsm.PWM (PWMChannel(PWM3), PWMHandle, setDutyCycle)

data Speed
  = Slow
  | Medium
  | Fast
  deriving (Read, Show, Typeable)

data Direction
  = Forward
  | Backward
  deriving (Read, Show, Typeable)

type Duration = Float

data Command
  = Move Speed Direction Duration
  | Stop Duration
  deriving (Read, Show, Typeable)

data DriveEnv = DriveEnv
  { gpioM1F :: GPIO
  , gpioM1B :: GPIO
  , gpioM2F :: GPIO
  , gpioM2B :: GPIO
  , gpioM3F :: GPIO
  , gpioM3B :: GPIO
  , gpioM4F :: GPIO
  , gpioM4B :: GPIO
  , pwmChannel :: PWMChannel
  , pwmPeriod :: Int
  , pwmMapper :: Speed -> Int
  }

pwmMapperDefault :: Speed -> Int
pwmMapperDefault Slow = 500000
pwmMapperDefault Medium = 750000
pwmMapperDefault Fast = 1000000

driveEnvDefault :: DriveEnv
driveEnvDefault =
  DriveEnv
    { gpioM1F = GPIO24
    , gpioM1B = GPIO25
    , gpioM2F = GPIO8
    , gpioM2B = GPIO7
    , gpioM3F = GPIO12
    , gpioM3B = GPIO16
    , gpioM4F = GPIO20
    , gpioM4B = GPIO21
    , pwmChannel = PWM3
    , pwmPeriod = 1000000 -- 1ms
    , pwmMapper = pwmMapperDefault
    }

drive ::
     HasField "driveEnv" env DriveEnv
  => PWMHandle
  -> Ptr LineRequest
  -> env
  -> [Command]
  -> IO Bool
drive pwmHandle lineRequest env commands = do
  mapM_ runCommand commands
  return True
  where
    logMsg = L.logMsg ["drive"]
    pinsForward = [gpioM1F, gpioM2F, gpioM3F, gpioM4F] <*> [env.driveEnv]
    pinsBackward = [gpioM1B, gpioM2B, gpioM3B, gpioM4B] <*> [env.driveEnv]
    toMicroSeconds = round . (* 1000000)
    runCommand command = do
      logMsg $ "Running command: " <> pack (show command)
      case command of
        (Move speed direction duration) -> do
          case direction of
            Forward -> setPins lineRequest pinsForward active
            Backward -> setPins lineRequest pinsBackward active
          setDutyCycle pwmHandle env.driveEnv.pwmChannel
            $ env.driveEnv.pwmMapper speed
          threadDelay $ toMicroSeconds duration
          setDutyCycle pwmHandle env.driveEnv.pwmChannel 0
          setPins lineRequest (pinsForward <> pinsBackward) inactive
        (Stop duration) -> threadDelay $ toMicroSeconds duration