module Dodge.Creature.HandPos (
--    aimingWeaponZeroPos,
    translatePointToLeftHand,
    translatePointToRightHand,
    translatePointToHead,
    translateToLeftWrist,
    translateToRightWrist,
    translateToLeftLeg,
    translateToRightLeg,
    translateToHead,
    translateToChest,
--    translateToBack,
    translateToLeftHand,
    translateToRightHand,
    backPQ,
    headPQ,
) where

import qualified Quaternion as Q
import Control.Lens
import Dodge.Creature.Test
import Dodge.Data.Creature
import Geometry
import ShapePicture

translatePointToRightHand :: Creature -> Point3 -> Point3
translatePointToRightHand cr p =  fst (rightHandPQ cr `Q.comp` (p,Q.qID))
    --translatePointToRightHand' cr . mirrorV3xz

--mirrorV3xz :: Point3 -> Point3
--mirrorV3xz (V3 x y z) = V3 x (- y) z

--translatePointToRightHand' :: Creature -> Point3 -> Point3
--translatePointToRightHand' cr
--    | oneH cr = (+.+.+ V3 11 (-3) 20) . rotate3 0.4 -- . rotate3 (-0.5)-- . scaleSH (V3 1 1.5 1)
--    | twists cr = (+.+.+ V3 0 5 20) . rotate3 (-1) . (+.+.+ V3 4 (-10) 0)
--    | twoFlat cr = (+.+.+ V3 4 (-8) 10)
--    | otherwise = case cr ^? crStance . carriage of
--        Just (Walking sa LeftForward) -> (+.+.+ V3 (- f sa) (- off) 10)
--        _ -> (+.+.+ V3 0 (- off) 10)
--  where
--    off = 8
--    sLen = _strideLength $ _crStance cr
--    f i = negate 2 + negate 6 * (sLen - i) / sLen

rightHandPQ :: Creature -> Point3Q
rightHandPQ cr
    | oneH cr = (V3 11 (-3) 20, Q.qID)
    | twists cr = (V3 0 5 20, Q.qz (-1)) `Q.comp` (V3 4 (-10) 0,Q.qID)
    | twoFlat cr = (V3 4 (-8) 10, Q.qID)
    | otherwise = case cr ^? crStance . carriage of
        Just (Walking sa LeftForward) -> (V3 (- f sa) (- off) 10, Q.qID)
        Just (Walking sa RightForward) -> (V3 (- g sa) (- off) 10, Q.qID)
        _ -> (V3 0 (- off) 10, Q.qID)
  where
    off = 8
    sLen = _strideLength $ _crStance cr
    f i = negate 2 + negate 6 * (sLen - i) / sLen
    g i = negate 2 + negate 6 * (i) / sLen

translateToRightHand :: Creature -> SPic -> SPic
translateToRightHand = overPosSP . translatePointToRightHand
--translateToRightHand = translateToRightHand' -- . mirrorSPxz
--
--translateToRightHand' :: Creature -> SPic -> SPic
--translateToRightHand' cr
--    | oneH cr = shoulderSP . translateSPxy 11 (-3) . rotateSP (-0.5) -- . scaleSH (V3 1 1.5 1)
--    | twists cr = shoulderSP . translateSPxy 0 5 . rotateSP (-1) . translateSPxy 4 (-10)
--    | twoFlat cr = waistSP . translateSPxy 4 (-8)
--    | otherwise = case cr ^? crStance . carriage of
--        Just (Walking sa LeftForward) -> waistSP . translateSPxy (- f sa) (- off)
--        Just (Walking sa RightForward) -> waistSP . translateSPxy (- g sa) (- off)
--        _ -> waistSP . translateSPxy 0 (- off)
--  where
--    off = 8
--    sLen = _strideLength $ _crStance cr
--    f i = negate 2 + negate 6 * (sLen - i) / sLen
--    g i = negate 2 + negate 6 * (i) / sLen

translateToRightWrist :: Creature -> SPic -> SPic
translateToRightWrist cr =  overPosSP
    (\p -> fst $ rightHandPQ cr `Q.comp` (V3 0 (-4) (-4)+p, Q.qID))
--    --translateToRightWrist' -- . mirrorSPxz
--
--translateToRightWrist' :: Creature -> SPic -> SPic
--translateToRightWrist' cr
--    | oneH cr = shoulderSP . translateSPxy 11 (-3) . rotateSP (-0.5) . offTrans -- . scaleSH (V3 1 1.5 1)
--    | twists cr = shoulderSP . translateSPxy 0 5 . rotateSP (-1) . translateSPxy 4 (-10) . offTrans
--    | twoFlat cr = waistSP . translateSPxy 4 (-8) . offTrans
--    | otherwise = case cr ^? crStance . carriage of
--        Just (Walking sa LeftForward) -> waistSP . translateSPxy (- f sa) (- off) . offTrans
--        Just (Walking sa RightForward) -> waistSP . translateSPxy (- g sa) (- off) . offTrans
--        _ -> waistSP . translateSPxy 0 (- off) . offTrans
--  where
--    offTrans = translateSP (V3 0 4 (-4))
--    off = 8
--    sLen = _strideLength $ _crStance cr
--    f i = negate 2 + negate 6 * (sLen - i) / sLen
--    g i = negate 2 + negate 6 * (i) / sLen

leftHandPQ :: Creature -> Point3Q
leftHandPQ cr
    | oneH cr = (V3 0 off 10, Q.qz 0.4)
    | twists cr = (V3 0 5 20, Q.qz (-1)) `Q.comp` (V3 12 4 0, Q.qz 0.4)
    | twoFlat cr = (V3 4 8 10, Q.qID)
    | otherwise = case cr ^? crStance . carriage of
        Just (Walking sa RightForward) -> (V3 (- f sa) off 10 , Q.qID)
        Just (Walking sa LeftForward) -> (V3 (- g sa) off 10 , Q.qID)
        _ -> (V3 0 off 10, Q.qID)
  where
    off = 8
    sLen = _strideLength $ _crStance cr
    f i = negate 2 + negate 6 * (sLen - i) / sLen
    g i = negate 2 + negate 6 * ( i) / sLen
    

translatePointToLeftHand :: Creature -> Point3 -> Point3
translatePointToLeftHand cr p = fst (leftHandPQ cr `Q.comp` (p,Q.qID))
--    | oneH cr = (+.+.+ V3 0 0 10) . rotate3 0.4 . (+.+.+ V3 0 off 0)
--    | twists cr = (+.+.+ V3 0 5 20) . rotate3 (-1) . (+.+.+ V3 12 4 0) . rotate3 0.4
--    | twoFlat cr = (+.+.+ V3 4 8 10)
--    | otherwise = case cr ^? crStance . carriage of
--        Just (Walking sa RightForward) -> (+.+.+ V3 (- f sa) off 10)
--        _ -> (+.+.+ V3 0 off 10)
--  where
--    off = 8
--    sLen = _strideLength $ _crStance cr
--    f i = negate 2 + negate 6 * (sLen - i) / sLen

translateToLeftHand :: Creature -> SPic -> SPic
translateToLeftHand = overPosSP . translatePointToLeftHand
--translateToLeftHand cr =
--    | oneH cr = waistSP . rotateSP 0.4 . translateSPxy 0 off
--    | twists cr = shoulderSP . translateSPxy 0 5 . rotateSP (-1) . translateSPxy 12 4
--    | twoFlat cr = waistSP . translateSPxy 4 8
--    | otherwise = case cr ^? crStance . carriage of
--        Just (Walking sa RightForward) -> waistSP . translateSPxy (- f sa) off
--        Just (Walking sa LeftForward) -> waistSP . translateSPxy (- g sa) off
--        _ -> waistSP . translateSPxy 0 off
--  where
--    off = 8
--    sLen = _strideLength $ _crStance cr
--    f i = negate 2 + negate 6 * (sLen - i) / sLen
--    g i = negate 2 + negate 6 * (i) / sLen

translateToLeftWrist :: Creature -> SPic -> SPic
translateToLeftWrist cr = overPosSP 
    (\p -> fst $ leftHandPQ cr `Q.comp` (V3 0 4 (-4)+p, Q.qID))
--    | oneH cr = waistSP . rotateSP 0.4 . translateSPxy 0 off . offTrans
--    | twists cr = shoulderSP . translateSPxy 0 5 . rotateSP (-1) . translateSPxy 12 4 . offTrans
--    | twoFlat cr = waistSP . translateSPxy 4 8 . offTrans
--    | otherwise = case cr ^? crStance . carriage of
--        Just (Walking sa RightForward) -> waistSP . translateSPxy (- f sa) off . offTrans
--        Just (Walking sa LeftForward) -> waistSP . translateSPxy (- g sa) off . offTrans
--        _ -> waistSP . translateSPxy 0 off . offTrans
--  where
--    offTrans = translateSP (V3 0 4 (-4))
--    off = 8
--    sLen = _strideLength $ _crStance cr
--    f i = negate 2 + negate 6 * (sLen - i) / sLen
--    g i = negate 2 + negate 6 * (i) / sLen

translateToLeftLeg :: Creature -> SPic -> SPic
translateToLeftLeg cr =
    rotateSP (_crMvDir cr - _crDir cr) . case cr ^? crStance . carriage of
        Just (Walking sa LeftForward) -> translateSPxy (f sa) off
        Just (Walking sa RightForward) -> translateSPxy (- f sa) off
        _ -> translateSPxy 0 off
  where
    off = 5
    sLen = _strideLength $ _crStance cr
    f i = 8 * (sLen - i) / sLen

translateToRightLeg :: Creature -> SPic -> SPic
translateToRightLeg cr =
    rotateSP (_crMvDir cr - _crDir cr) . case cr ^? crStance . carriage of
        Just (Walking sa LeftForward) -> translateSPxy (- f sa) (- off)
        Just (Walking sa RightForward) -> translateSPxy (f sa) (- off)
        _ -> translateSPxy 0 (- off)
  where
    off = 5
    sLen = _strideLength $ _crStance cr
    f i = 8 * (sLen - i) / sLen

translateToHead :: Creature -> SPic -> SPic
translateToHead cr = overPosSP (\p -> fst $ (headPQ cr `Q.comp` (p,Q.qID)))
--    | twists cr =
--        translateSPz 20 . translateSPxy 0 5 . rotateSP (-1) . translateSPxy (negate 2.5) 0.25
--            . rotateSP 1
--    | oneH cr =
--        translateSPz 20 . rotateSP 0.5 . translateSPxy 2.5 0
--            . rotateSP (negate 0.5)
--    | otherwise = translateSPz 20 . translateSPxy 2.5 0

headPQ :: Creature -> Point3Q
headPQ cr
    | twists cr = (V3 0 2 20, Q.qz (-1)) `Q.comp` (V3 (negate 2.5) 0.25 0, Q.qz 1)
    | oneH cr = (V3 0 0 20, Q.qz 0.5) `Q.comp` (V3 2.5 0 0, Q.qz (-0.5))
    | otherwise = (V3 2.5 0 20, Q.qID)

translatePointToHead :: Creature -> Point3 -> Point3
translatePointToHead cr p = fst (headPQ cr `Q.comp` (p,Q.qID))
--    | twists cr =
--        (+.+.+ V3 0 5 20) . rotate3 (-1) . (+.+.+ V3 (negate 2.5) 0.25 0)
--            . rotate3 1
--    | oneH cr =
--        (+.+.+ V3 0 0 20) . rotate3 0.5 . (+.+.+ V3 2.5 0 0)
--            . rotate3 (negate 0.5)
--    | otherwise = (+.+.+ V3 2.5 0 20)

translateToChest :: Creature -> SPic -> SPic
translateToChest cr
    | oneH cr = rotateSP 0.5
    --    | twists cr = translateSPxy 0 5 . rotateSP (-1)
    | twists cr = rotateSP (-1)
    | otherwise = id

backPQ :: Creature -> Point3Q
backPQ cr
    | oneH cr = (V3 0 0 10, Q.qz 0.5)
    | twists cr = (V3 0 3 10, Q.qz (-1.5))
    | otherwise = (V3 0 0 10, Q.qz 0)

--translateToBack :: Creature -> Point3 -> SPic -> SPic
--translateToBack cr p
--    | oneH cr = rotateSP 0.5 . translateSP p
--    | twists cr = rotateSP (-1.5) . translateSP (p - V3 5 0 0)
--    | otherwise = translateSP p

--shoulderSP :: SPic -> SPic
--shoulderSP = translateSPz 20
--
--waistSP :: SPic -> SPic
--waistSP = translateSPz 10