module Data.RTCM3.SBP.Time
( currentGpsTime
, rolloverTowGpsTime
, rolloverEpochGpsTime
) where
import BasicPrelude
import Control.Lens
import Data.Time
import Data.Time.Calendar.WeekDate
import Data.Word
import SwiftNav.SBP
gpsEpoch :: Day
gpsEpoch = fromGregorian 1980 1 6
gpsLeapSeconds :: Integer
gpsLeapSeconds = 18
gpsLeapMillis :: Integer
gpsLeapMillis = 1000 * gpsLeapSeconds
hourSeconds :: Integer
hourSeconds = 60 * 60
hourMillis :: Integer
hourMillis = 1000 * hourSeconds
daySeconds :: Integer
daySeconds = 24 * hourSeconds
dayMillis :: Integer
dayMillis = 1000 * daySeconds
weekSeconds :: Integer
weekSeconds = 7 * daySeconds
weekMillis :: Integer
weekMillis = 1000 * weekSeconds
toWn :: UTCTime -> Word16
toWn t = fromIntegral weeks
where
days = diffDays (utctDay t) gpsEpoch
weeks = days `div` 7
toTow :: UTCTime -> Word32
toTow t = floor since
where
(y, w, _d) = toWeekDate (utctDay t)
begin = addDays (1) $ fromWeekDate y w 1
since = 1000 * diffUTCTime t (UTCTime begin 0)
currentGpsTime :: MonadIO m => m GpsTimeNano
currentGpsTime = do
t <- liftIO $ addUTCTime (fromIntegral gpsLeapSeconds) <$> getCurrentTime
pure $ GpsTimeNano (toTow t) 0 (toWn t)
rolloverTowGpsTime :: Word32 -> GpsTimeNano -> GpsTimeNano
rolloverTowGpsTime tow t = t & gpsTimeNano_tow .~ tow & rollover
where
rollover
| increment = gpsTimeNano_wn +~ 1
| decrement = gpsTimeNano_wn +~ 1
| otherwise = gpsTimeNano_wn +~ 0
new = fromIntegral tow
old = fromIntegral (t ^. gpsTimeNano_tow)
increment = old > new && old new > weekMillis `div` 2
decrement = new > old && new old > weekMillis `div` 2
rolloverEpochGpsTime :: Word32 -> GpsTimeNano -> GpsTimeNano
rolloverEpochGpsTime epoch t = rolloverTowGpsTime tow t
where
epoch' = fromIntegral epoch 3 * hourMillis + gpsLeapMillis
epoch''
| epoch' < 0 = epoch' + dayMillis
| otherwise = epoch'
dow = fromIntegral (t ^. gpsTimeNano_tow) `div` dayMillis
tod = fromIntegral (t ^. gpsTimeNano_tow) dow * dayMillis
dow'
| increment = dow + 1 `mod` 7
| decrement = dow 1 `mod` 7
| otherwise = dow
increment = epoch'' > tod && epoch'' tod > dayMillis `div` 2
decrement = tod > epoch'' && tod epoch'' > dayMillis `div` 2
tow = fromIntegral $ dow' * dayMillis + epoch''