{-# LANGUAGE DuplicateRecordFields #-}
{-# LANGUAGE NamedFieldPuns        #-}

module Walker.SRDF where

import qualified SRDF
import           Walker

getLegSRDFTree :: ExtendedLeg -> SRDF.GenericNode
getLegSRDFTree Leg { leg_name, root_joint = ExtendedJoint { urdf_name } } =
    SRDF.GenericNode
        { name       = "leg"
        , attributes = [ SRDF.Attribute "name" ("leg/" ++ leg_name)
                       , SRDF.Attribute "root_joint" urdf_name
                       ]
        , children   = []
        }

getLegSRDFJoints :: ExtendedLeg -> [SRDF.Joint]
getLegSRDFJoints Leg { leg_name, root_joint } = mapJoints convert_joint
                                                          root_joint
  where
    convert_joint :: ExtendedJoint -> SRDF.Joint
    convert_joint ExtendedJoint { joint, urdf_name } = SRDF.Joint
        { name          = urdf_name
        , generic_nodes = case jtype joint of
                              RevoluteJoint { g_steps } ->
                                  [ SRDF.GenericNode
                                        { name       = "graph_steps"
                                        , attributes = [ SRDF.Attribute "value"
                                                             $ show g_steps
                                                       ]
                                        , children   = []
                                        }
                                  ]
                              FixedJoint -> []
        }

getSRDF :: ExtendedWalker -> SRDF.SRDF
getSRDF Walker { walker_name, legs } = SRDF.SRDF { name          = walker_name
                                                 , groups        = [leg_group]
                                                 , items         = []
                                                 , generic_nodes = []
                                                 }
  where
    leg_group = SRDF.Group { name          = "legs"
                           , links         = []
                           , joints        = concatMap getLegSRDFJoints legs
                           , chains        = []
                           , groups        = []
                           , generic_nodes = map getLegSRDFTree legs
                           }
