{-# LANGUAGE DuplicateRecordFields #-}

module SRDF.Xml where

import           Shared
import           SRDF
import           Filesystem.Path
import           Filesystem.Path.CurrentOS
import           Linear.V3
import           System.Directory
import           Text.XML.HXT.Core

cGenericNodesToXML :: ArrowXml a => GenericNode -> a XmlTree XmlTree
cGenericNodesToXML (GenericNode name attributes children) = mkelem
    name
    (map convert_attr attributes)
    (map cGenericNodesToXML children)
    where convert_attr (Attribute name value) = sattr name value

cChainToXML :: ArrowXml a => Chain -> a XmlTree XmlTree
cChainToXML (Chain base_link tip_link) =
    aelem "chain" [sattr "base_link" base_link, sattr "tip_link" tip_link]

cJointToXML :: ArrowXml a => Joint -> a XmlTree XmlTree
cJointToXML (Joint name generic_nodes) =
    mkelem "joint" [sattr "name" name] (cGenericNodesToXML <$> generic_nodes)

cJointStateToXML :: ArrowXml a => JointState -> a XmlTree XmlTree
cJointStateToXML (JointState name value) =
    aelem "link" [sattr "name" name, sattr "value" $ show value]

cLinkToXML :: ArrowXml a => Link -> a XmlTree XmlTree
cLinkToXML (Link name) = aelem "link" [sattr "name" name]

cItemToXML :: ArrowXml a => SRDFItem -> a XmlTree XmlTree
cItemToXML (EndEffector name group parent_link parent_group) = aelem
    "end_effector"
    [ sattr "name"         name
    , sattr "group"        group
    , sattr "parent_link"  parent_link
    , sattr "parent_group" parent_group
    ]
cItemToXML (VirtualJoint name child_link parent_frame j_type) = aelem
    "virtual_joint"
    [ sattr "name"         name
    , sattr "child_link"   child_link
    , sattr "parent_frame" parent_frame
    , sattr "type"         type_string
    ]
  where
    type_string = case j_type of
        Fixed    -> "fixed"
        Floating -> "floating"
        Planar   -> "planar"
cItemToXML (GroupState joints name group) = mkelem
    "group_state"
    [sattr "name" name, sattr "group" group]
    (cJointStateToXML <$> joints)
cItemToXML (DisableCollisions link1 link2 reason) = aelem
    "disable_collisions"
    [sattr "link1" link1, sattr "link2" link2, sattr "reason" reason]


cGroupToXML :: ArrowXml a => Group -> a XmlTree XmlTree
cGroupToXML (Group name links joints chains groups generic_nodes) = mkelem
    "group"
    [sattr "name" name]
    (  (cLinkToXML <$> links)
    ++ (cJointToXML <$> joints)
    ++ (cChainToXML <$> chains)
    ++ (cGroupToXML <$> groups)
    ++ (cGenericNodesToXML <$> generic_nodes)
    )

cSRDFToXML :: ArrowXml a => SRDF -> a XmlTree XmlTree
cSRDFToXML (SRDF name groups items generic_nodes) = mkelem
    "robot"
    [sattr "name" name]
    (  (cGroupToXML <$> groups)
    ++ (cItemToXML <$> items)
    ++ (cGenericNodesToXML <$> generic_nodes)
    )

writeSRDFXML :: SRDF -> Filename -> IO ()
writeSRDFXML srdf filename = do
    createDirectoryIfMissing True $ encodeString $ directory $ decodeString
        filename
    runX
        $   Text.XML.HXT.Core.root [] [cSRDFToXML srdf]
        >>> writeDocument [withIndent yes] filename
    return ()
