{-# LANGUAGE DeriveDataTypeable #-}

module HWiidPlayer ( HWiidPlayer
                   , newHWiidPlayer
                   ) where

import Control.Monad
import Data.Bits hiding (rotate)
import Data.Function (on)
import Data.List (sortBy)
import Data.Maybe
import Data.Ratio ((%))
import Data.Typeable
import HWiid
import Graphics.Rendering.OpenGL.GL (GLfloat, Vector3(..), Vertex2(..))
import Graphics.Rendering.OpenGL.GL.BeginEnd (unsafeRenderPrimitive, PrimitiveMode(..))
import Graphics.Rendering.OpenGL.GL.CoordTrans (unsafePreservingMatrix, translate, rotate)
import Graphics.Rendering.OpenGL.GL.VertexSpec


import Player
import Tank
import qualified Transformable as T
import qualified Vector as V


data HWiidPlayer = HWiidPlayer Wiimote WiimoteAccCal [(Float, Float)]
                     deriving (Typeable, Show)

instance Player HWiidPlayer where
    playerUpdate (HWiidPlayer wiimote cal oldaims) tank = do
                                state <- getState wiimote
                                messages <- getMesg wiimote
                                
                                let buttons = stateButtons state
                                    shoot = any (\m -> (mesgType m == mesgTypeButton) && (test (mesgButtons m) buttonB)) $ messages
                                    foo = any (\m -> (mesgType m == mesgTypeButton) && (test (mesgButtons m) buttonA)) $ messages
                                    x = (if (test buttons buttonLeft) then (-1) else 0) + (if (test buttons buttonRight) then 1 else 0)
                                    y = (if (test buttons buttonDown) then (-1) else 0) + (if (test buttons buttonUp) then 1 else 0)
                                    ext = stateExt state
                                
                                    (mx, my) = if (extType ext) /= extNunchuk
                                               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)
                                    
                                    iraim = handleIR state cal
                                    newaims = if isJust iraim then take irSmooth ((fromJust iraim):oldaims) else oldaims
                                    newaim = if null newaims then (0, 0) else mulV (1/(fromIntegral $ length newaims)) (foldr addV (0, 0) newaims)
                                    aims = if not (null newaims) && (lengthV $ subV (head newaims) newaim) > irSkip
                                           then take irSkipSmooth newaims
                                           else newaims
                                    (aimx, aimy) = if null aims then (0, 0) else mulV (1/(fromIntegral $ length aims)) (foldr addV (0, 0) aims)
                                    ax = realToFrac $ aimx - (fromRational . toRational . tankX $ tank)
                                    ay = realToFrac $ aimy - (fromRational . toRational . tankY $ tank)
                                    aim = if (ax /= 0 || ay /= 0) then Just . fst . V.fromVector $ V.Vector ax ay else Nothing
                                                 
                                    move = (mx /= 0 || my /= 0)
                                    angle = atan2 my mx
                                    dir = if move then Just $ V.fromAngle (fromRational $ round ((angle - (sin $ 8*x)/8)*1000000)%1000000) else Nothing
                                when foo $ print state
                                return (HWiidPlayer wiimote cal aims, dir, move, aim, shoot)
    
    renderPlayer (HWiidPlayer _ _ [])   = return ()
    renderPlayer (HWiidPlayer _ _ aims) = unsafePreservingMatrix $ do
                                                  let (x, y) = mulV (1/(fromIntegral $ length aims)) $ foldr addV (0, 0) aims
                                                  
                                                  translate $ Vector3 (realToFrac x) (realToFrac y) (0.2 :: GLfloat)
                                                  rotate 30 $ Vector3 1 0 (0 :: GLfloat)
                                       
                                                  unsafeRenderPrimitive Quads $ do
                                                                     texCoord $ TexCoord2 (0 :: GLfloat) (0 :: GLfloat)
                                                                     vertex $ Vertex2 (-0.2 :: GLfloat) (-0.2 :: GLfloat)
                                                                     
                                                                     texCoord $ TexCoord2 (0 :: GLfloat) (1 :: GLfloat)
                                                                     vertex $ Vertex2 (-0.2 :: GLfloat) (0.2 :: GLfloat)
                                                                     
                                                                     texCoord $ TexCoord2 (1 :: GLfloat) (1 :: GLfloat)
                                                                     vertex $ Vertex2 (0.2 :: GLfloat) (0.2 :: GLfloat)
                                                                     
                                                                     texCoord $ TexCoord2 (1 :: GLfloat) (0 :: GLfloat)
                                                                     vertex $ Vertex2 (0.2 :: GLfloat) (-0.2 :: GLfloat)


irSmooth = 10
irSkip = 0.2
irSkipSmooth = 4

irXScale :: Float
irXScale = 20

irXTranslate :: Float
irXTranslate = 0 + 7

irYScale :: Float
irYScale = 20

irYTranslate :: Float
irYTranslate = -10 + 8

handleIR :: WiimoteState -> WiimoteAccCal -> Maybe (Float, Float)
handleIR state cal = 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 fi = fromIntegral
                       s = ((fi . stateAccX $ state)-(fi . accCalZeroX $ cal))/((fi . accCalOneX $ cal)-(fi . accCalZeroX $ cal))
                       c = ((fi . stateAccZ $ state)-(fi . accCalZeroZ $ cal))/((fi . accCalOneZ $ cal)-(fi . accCalZeroZ $ cal))
                   in (c*x + s*y, -s*x + c*y)
      
      hMaxX = (fromIntegral irMaxX)/2
      hMaxY = (fromIntegral irMaxY)/2
      
      sortIRSourcesBySize :: [WiimoteIRSource] -> [WiimoteIRSource]
      sortIRSourcesBySize = sortBy (flip compare `on` irSize)
      
      sortIRSourcesByPos :: [WiimoteIRSource] -> [WiimoteIRSource]
      sortIRSourcesByPos = sortBy (compare `on` (fst . rot . pos))

negV (a1, a2) = (-a1, -a2)
addV (a1, a2) (b1, b2) = (a1+b1, a2+b2)
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
      

newHWiidPlayer :: IO HWiidPlayer
newHWiidPlayer = do
  wiimote <- open bdAddrAny (flagMesgInterface .|. flagNonblock)
  when (wiimote == nullWiimote) $ fail "Wiimote error"
  setReportMode wiimote (reportButtons .|. reportAcc .|. reportIR .|. reportNunchuk)
  cal <- getAccCal wiimote extNone
  return $ HWiidPlayer wiimote cal []

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