{-# LANGUAGE DeriveDataTypeable #-}

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

import Player


data WiimotePlayer = WiimotePlayer Wiimote
                     deriving (Typeable, Show)

instance Player WiimotePlayer where
    playerUpdate (WiimotePlayer wiimote) tank = do
                                state <- hwiidGetState wiimote
                                messages <- hwiidGetMesg wiimote
                                
                                let buttons = stateButtons state
                                    shoot = any (\m -> (mesgType m == hwiidMesgTypeButton) && (test (mesgButtons m) hwiidButtonB)) $ messages
                                    foo = any (\m -> (mesgType m == hwiidMesgTypeButton) && (test (mesgButtons m) hwiidButtonA)) $ messages
                                    x = (if (test buttons hwiidButtonLeft) then (-1) else 0) + (if (test buttons hwiidButtonRight) then 1 else 0)
                                    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 (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)
                                    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 $ 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 .|. hwiidReportAcc .|. hwiidReportIR .|. hwiidReportNunchuk)
  return $ WiimotePlayer wiimote

test :: (Bits a) => a -> a -> Bool
test field bits = (field .&. bits) == bits