1 files changed, 71 insertions, 3 deletions
diff --git a/video/src/Video.hs b/video/src/Video.hs
index d98230a..3a06a75 100644
--- a/video/src/Video.hs
+++ b/video/src/Video.hs
@@ -3,8 +3,11 @@ module Video (ConfigSocket(..), video) where
 
 import Graphics.Gloss
 import Graphics.Gloss.Interface.IO.Simulate
-import Data.ByteString (pack, hPut, hGetNonBlocking)
+import Data.ByteString (ByteString, unpack, pack, hPut, hGetLine)
+import Data.List (elemIndex, isPrefixOf)
 import Data.Word
+import Data.Char (chr)
+import Data.Maybe
 import Data.IORef
 import Data.String
 import Network.Socket
@@ -12,6 +15,17 @@ import System.IO
 import Control.Monad
 
 
+device_get_high_byte = "h"
+device_get_low_byte  = "l"
+device_get_word      = "w"
+
+device_set_high_byte = "H"
+device_set_low_byte  = "L"
+device_set_word      = "W"
+
+device_ok = "o"
+
+
 data ConfigSocket = ConfigInet PortNumber | ConfigUnix String
 
 
@@ -32,10 +46,64 @@ producePicture pic = do
                                 (pack pic) False
 
 
+bsToStr :: ByteString -> String
+bsToStr = map (chr . fromEnum) . unpack
+
+
+toHex :: Word8 -> String
+toHex n = ["0123456789abcdef" !! (fromIntegral n `div` 16 `mod` 16)] ++
+          ["0123456789abcdef" !! (fromIntegral n `mod` 16)]
+
+fromHex :: String -> Int -> Int -> Int -> Int
+fromHex _ _ 0 a = a
+fromHex s i n a = fromHex s (i + 1) (n - 1)
+    (a * 16 + fromJust (Data.List.elemIndex (s !! i) "0123456789abcdef"))
+
+
+replace :: Int -> Int -> [Word8] -> [Word8]
+replace 0 v (_:t) = fromIntegral v : t
+replace n v (h:t) = h : replace (n - 1) v t
+
+
+doCommand :: Handle -> [Word8] -> IO [Word8]
+doCommand h pic = do
+    cmdbs <- Data.ByteString.hGetLine h
+    let cmd = bsToStr cmdbs
+    if device_get_high_byte `isPrefixOf` cmd
+    then let i = fromHex cmd 2 5 0 in do
+        hPut h $ fromString $ device_ok ++ " " ++ toHex (pic !! i) ++ "\n"
+        return pic
+    else if device_get_low_byte `isPrefixOf` cmd
+    then let i = fromHex cmd 2 5 0 in do
+        hPut h $ fromString $ device_ok ++ " " ++ toHex (pic!!(i+1)) ++ "\n"
+        return pic
+    else if device_get_word `isPrefixOf` cmd
+    then let i = fromHex cmd 2 5 0 in do
+        hPut h $ fromString $ device_ok ++ " "
+            ++ toHex (pic !! i) ++ toHex (pic!!(i+1)) ++ "\n"
+        return pic
+    else if device_set_high_byte `isPrefixOf` cmd
+    then let i = fromHex cmd 2 5 0
+             v = fromHex cmd 8 2 0 in do
+        hPut h $ fromString $ device_ok ++ "\n"
+        return $ replace i v pic
+    else if device_set_low_byte `isPrefixOf` cmd
+    then let i = fromHex cmd 2 5 0
+             v = fromHex cmd 8 2 0 in do
+        hPut h $ fromString $ device_ok ++ "\n"
+        return $ replace (i + 1) v pic
+    else if device_set_word `isPrefixOf` cmd
+    then let i = fromHex cmd 2 5 0
+             vh = fromHex cmd 8 2 0
+             vl = fromHex cmd 10 2 0 in do
+        hPut h $ fromString $ device_ok ++ "\n"
+        return $ replace i vh $ replace (i + 1) vl pic
+    else error "Unknown Device Protocol Line"
+
 control :: Handle -> ViewPort -> Float -> [Word8] -> IO [Word8]
 control h _ _ pic = do
-    cmd <- hGetNonBlocking h 1
-    return pic
+    cmd_ready <- hReady h
+    if cmd_ready then doCommand h pic else return pic
 
 
 video :: ConfigSocket -> IO ()