module GPS where
import ByteParser
import List
import Data.Word

-- take a list of bytes and return a list of packets, where each packet
-- is a list of 16-bit words.
decode :: [Word8] -> [[Word16]]
decode stream =
  -- if there's a complete packet and its checksums are OK:
  if length head == 5 && sum head == 0 && length body == len && sum body == 0
  -- then return the packet as the first element of a list of packets
  -- and recurse;
  then (head ++ body) : decode after
  -- else skip past the first byte that looked like part of a header,
  -- and try decoding again there.
  else if null pkt then [] else decode (tail pkt)
  where
    -- find the first packet in the byte stream
    pkt = sync stream
    pktwds :: [Word16]
    pktwds = map condenseLE $ unfoldr (\b-> case b of (x:y:_) -> Just $ splitAt 2 b; _ -> Nothing) pkt
    -- get the 5-word header out of the packet
    head = take 5 pktwds
    -- get the body word count out of the header, and add 1 for the checksum
    len = fromEnum (head !! 2) + 1
    -- get the len-word body
    body = take len (drop 5 pktwds)
    -- stash the byte stream that follows this packet
    after = drop ((5 + len) * 2) pkt

-- find sync bytes that might indicate a packet header, and throw away
-- everything that precedes them.
sync :: [Word8] -> [Word8]
sync z@(0xFF : 0x81 : tl) = z
sync (_ : tl) = sync tl
sync [] = []