WiimotePlayer: Handle IR signal

1 files changed, 64 insertions, 13 deletions

diff --git a/src/WiimotePlayer.hs b/src/WiimotePlayer.hs
index 3907615..7804b26 100644
--- a/src/WiimotePlayer.hs
+++ b/src/WiimotePlayer.hs
@@ -1,11 +1,13 @@
 {-# LANGUAGE DeriveDataTypeable #-}
 
-module WiimotePlayer ( WiimotePlayer(..)
+module WiimotePlayer ( WiimotePlayer
                      , newWiimotePlayer
                      ) where
 
 import Control.Monad
 import Data.Bits
+import Data.Function (on)
+import Data.List (sortBy)
 import Data.Ratio ((%))
 import Data.Typeable
 import HWiid
@@ -28,30 +30,79 @@ instance Player WiimotePlayer where
                                     y = (if (test buttons hwiidButtonDown) then (-1) else 0) + (if (test buttons hwiidButtonUp) then 1 else 0)
                                     ext = stateExt state
                                 
-                                (mx, my) <- if (extType ext) /= hwiidExtNunchuk
-                                            then return (x, y)
-                                            else do
-                                              let nx = ((fromIntegral . extNunchukStickX $ ext) - 0x80)/0x80
-                                                  ny = ((fromIntegral . extNunchukStickY $ ext) - 0x80)/0x80
-                                              return $ if (nx*nx + ny*ny) < 0.4 then (x, y) else (x+nx, y+ny)
+                                    (mx, my) = if (extType ext) /= hwiidExtNunchuk
+                                               then (x, y)
+                                               else
+                                                 let nx = ((fromIntegral . extNunchukStickX $ ext) - 0x80)/0x80
+                                                     ny = ((fromIntegral . extNunchukStickY $ ext) - 0x80)/0x80
+                                                 in if (nx*nx + ny*ny) < 0.4 then (x, y) else (x+nx, y+ny)
                                                                                                      
                                     --ax = aimx - (fromRational . toRational . tankX $ tank)
                                     --ay = aimy - (fromRational . toRational . tankY $ tank)
-                                let move = (mx /= 0 || my /= 0)
-                                    angle = if move then Just $ fromRational $ round ((atan2 my mx)*1000000*180/pi)%1000000 else Nothing
+                                    move = (mx /= 0 || my /= 0)
+                                    angle = atan2 my mx
+                                    moveangle = if move then Just $ fromRational $ round ((angle - (sin $ 8*x)/8)*1000000*180/pi)%1000000 else Nothing
                                     
                                     
                                     --aangle = if (ax /= 0 || ay /= 0) then Just $ fromRational $ round ((atan2 ay ax)*1000000*180/pi)%1000000 else Nothing
-                                when foo $ print state
-                                return (WiimotePlayer wiimote, angle, move, Nothing, shoot)
+                                when foo $ print $ handleIR state
+                                return (WiimotePlayer wiimote, moveangle, move, Nothing, shoot)
 
 
+irXScale :: Float
+irXScale = 1
+
+irXTranslate :: Float
+irXTranslate = 0
+
+irYScale :: Float
+irYScale = 1
+
+irYTranslate :: Float
+irYTranslate = 0
+
+handleIR :: WiimoteState -> Maybe (Float, Float)
+handleIR state = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ stateIRSources state
+    where
+      handle [ira,irb] = let pa = pos ira
+                             pb = pos irb
+                             p = negV pa
+                             b = pb `subV` pa
+                             x = (p `dotV` b)/(lengthV b)
+                             y = (sqrt ((lengthSqV p) - x*x)) * (signum $ sinV b p)
+                             in Just ((x - (lengthV b)/2)*irXScale + irXTranslate, y*irYScale + irYTranslate)
+                             
+      handle _ = Nothing
+      
+      pos src = (((fromIntegral $ irPosX src) - hMaxX)/hMaxX, ((fromIntegral $ irPosY src) - hMaxY)/hMaxY)
+      
+      rot (x, y) = let s = (fromIntegral . stateAccX $ state) - 0x80
+                       c = (fromIntegral . stateAccZ $ state) - 0x80
+                   in (c*x + s*y, -s*x + c*y)
+      
+      hMaxX = (fromIntegral hwiidIRMaxX)/2
+      hMaxY = (fromIntegral hwiidIRMaxY)/2
+      
+      negV (a1, a2) = (-a1, -a2)
+      subV (a1, a2) (b1, b2) = (a1-b1, a2-b2)
+      dotV (a1, a2) (b1, b2) = a1*b1 + a2*b2
+      mulV x (a1, a2) = (x*a1, x*a2)
+      sinV (a1, a2) (b1, b2) = (a1 * b2 - b1 * a2)
+      lengthSqV a = dotV a a
+      lengthV a = sqrt $ lengthSqV a
+      
+      sortIRSourcesBySize :: [WiimoteIRSource] -> [WiimoteIRSource]
+      sortIRSourcesBySize = sortBy (flip compare `on` irSize)
+      
+      sortIRSourcesByPos :: [WiimoteIRSource] -> [WiimoteIRSource]
+      sortIRSourcesByPos = sortBy (compare `on` (fst . rot . pos))
+
 newWiimotePlayer :: IO WiimotePlayer
 newWiimotePlayer = do
   wiimote <- hwiidOpen bdAddrAny (hwiidFlagMesgInterface .|. hwiidFlagNonblock)
   when (wiimote == nullWiimote) $ fail "Wiimote error"
-  hwiidSetReportMode wiimote (hwiidReportButtons .|. hwiidReportIR .|. hwiidReportNunchuk)
+  hwiidSetReportMode wiimote (hwiidReportButtons .|. hwiidReportAcc .|. hwiidReportIR .|. hwiidReportNunchuk)
   return $ WiimotePlayer wiimote
 
 test :: (Bits a) => a -> a -> Bool
-test field bits = (field .&. bits) == bits
\ No newline at end of file
+test field bits = (field .&. bits) == bits