#!/usr/bin/env python
# 
#  helper script for spawning models in gazebo
# 
# Provides quick access to the services exposed by gazebo

import roslib, time
roslib.load_manifest('gazebo')

import rospy, sys, os
import string

from gazebo import gazebo_interface

from gazebo_msgs.msg import *
from gazebo_msgs.srv import *
from std_srvs.srv import Empty
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench
import tf.transformations as tft
#from pr2_mechanism_msgs.srv import SwitchController, SwitchControllerRequest, ListControllers, ListControllersRequest

world_joint_template = """<link name="world"/>
  <joint name="JOINT_NAME" type="floating" >
    <axis xyz="0 0 1" />
    <parent link="world" />
    <child link="LINK_NAME" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>"""
model_template = """<robot name="MODEL_NAME">
  <link name="LINK_NAME">
    <inertial>
      <mass value="MASS" />
      <origin xyz="0 0 0" /> 
      <inertia  ixx="IXX" ixy="0.0"  ixz="0.0"  iyy="IYY"  iyz="0.0"  izz="IZZ" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        VISUAL_GEOMETRY
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0 " />
      <geometry>
        COLLISION_GEOMETRY
      </geometry>
    </collision>
  </link>
  <gazebo reference="LINK_NAME">
    <material>MATERIAL</material>
    <turnGravityOff>GRAVITY</turnGravityOff>
  </gazebo>
</robot>"""

def usage():
    print '''Commands:
    -[urdf|gazebo|trimesh] - specify incoming xml is urdf or gazebo format
    -[file|param] [<file_name>|<param_name>] - source of the model xml or the trimesh file
    -model <model_name> - name of the model to be spawned.
    -reference_frame <entity_name> - optinal: name of the model/body where initial pose is defined.
                                     If left empty or specified as "world", gazebo world frame is used.
    -namespace <ros_namespace> - optional: all subsequent ROS interface plugins will be inside of this namespace.
    -unpause - optional: !!!Experimental!!! unpause physics after spawning model
    -wait - optional: !!!Experimental!!! wait for model to exist
    -trimesh_mass <mass in kg> - required if -trimesh is used: linear mass
    -trimesh_ixx <moment of inertia in kg*m^2> - required if -trimesh is used: moment of inertia about x-axis
    -trimesh_iyy <moment of inertia in kg*m^2> - required if -trimesh is used: moment of inertia about y-axis
    -trimesh_izz <moment of inertia in kg*m^2> - required if -trimesh is used: moment of inertia about z-axis
    -trimesh_gravity <bool> - required if -trimesh is used: gravity turned on for this trimesh model
    -trimesh_material <material name as a string> - required if -trimesh is used: E.g. Gazebo/Blue
    -trimesh_name <link name as a string> - required if -trimesh is used: name of the link containing the trimesh
    -x <x in meters> - optional: initial pose, use 0 if left out
    -y <y in meters> - optional: initial pose, use 0 if left out
    -z <z in meters> - optional: initial pose, use 0 if left out
    -R <roll in radians> - optional: initial pose, use 0 if left out
    -P <pitch in radians> - optional: initial pose, use 0 if left out
    -Y <yaw in radians> - optional: initial pose, use 0 if left out
    -J <joint_name joint_position> - optional: initialize the specified joint at the specified value
    '''
    sys.exit(1)

class SpawnModel():
    def __init__(self):
        self.initial_xyz             = [0,0,0]
        self.initial_rpy             = [0,0,0]
        self.initial_q               = [0,0,0,1]
        self.file_name               = ""
        self.param_name              = ""
        self.model_name              = ""
        self.robot_namespace         = rospy.get_namespace()
        self.reference_frame         = ""
        self.unpause_physics         = False
        self.wait_for_model          = ""
        self.wait_for_model_exists   = False
        self.urdf_format             = False
        self.gazebo_format           = False
        self.joint_names             = []
        self.joint_positions         = []

    def parseUserInputs(self):
        # get goal from commandline
        for i in range(0,len(sys.argv)):
          if sys.argv[i] == '-unpause':
            self.unpause_physics = True
          if sys.argv[i] == '-urdf':
            if self.gazebo_format == True:
                rospy.logerr("Error: you cannot specify both urdf and gazebo format xml, must pick one")
                sys.exit(0)
            else:
              self.urdf_format = True;
          if sys.argv[i] == '-gazebo':
            if self.urdf_format == True:
                rospy.logerr("Error: you cannot specify both urdf and gazebo format xml, must pick one")
                sys.exit(0)
            else:
              self.gazebo_format = True;
          if sys.argv[i] == '-J':
            if len(sys.argv) > i+2:
              self.joint_names.append(sys.argv[i+1])
              self.joint_positions.append(float(sys.argv[i+2]))
            else:
              rospy.logerr("Error: must specify a joint name and joint value pair")
              sys.exit(0)
          if sys.argv[i] == '-param':
            if len(sys.argv) > i+1:
              if self.file_name != "":
                rospy.logerr("Error: you cannot specify file name if parameter name is given, must pick one source of model xml")
                sys.exit(0)
              else:
                self.param_name = sys.argv[i+1]
          if sys.argv[i] == '-file':
            if len(sys.argv) > i+1:
              if self.param_name != "":
                rospy.logerr("Error: you cannot specify parameter if file name is given, must pick one source of model xml")
                sys.exit(0)
              else:
                self.file_name = sys.argv[i+1]
          if sys.argv[i] == '-model':
            if len(sys.argv) > i+1:
              self.model_name = sys.argv[i+1]
          if sys.argv[i] == '-wait':
            if len(sys.argv) > i+1:
              self.wait_for_model = sys.argv[i+1]
          if sys.argv[i] == '-reference_frame':
            if len(sys.argv) > i+1:
              self.reference_frame = sys.argv[i+1]
          if sys.argv[i] == '-namespace':
            if len(sys.argv) > i+1:
              self.robot_namespace = sys.argv[i+1]
          if sys.argv[i] == '-x':
            if len(sys.argv) > i+1:
              self.initial_xyz[0] = float(sys.argv[i+1])
          if sys.argv[i] == '-y':
            if len(sys.argv) > i+1:
              self.initial_xyz[1] = float(sys.argv[i+1])
          if sys.argv[i] == '-z':
            if len(sys.argv) > i+1:
              self.initial_xyz[2] = float(sys.argv[i+1])
          if sys.argv[i] == '-R':
            if len(sys.argv) > i+1:
              self.initial_rpy[0] = float(sys.argv[i+1])
          if sys.argv[i] == '-P':
            if len(sys.argv) > i+1:
              self.initial_rpy[1] = float(sys.argv[i+1])
          if sys.argv[i] == '-Y':
            if len(sys.argv) > i+1:
              self.initial_rpy[2] = float(sys.argv[i+1])

        if not self.gazebo_format and not self.urdf_format:
          rospy.logerr("Error: you must specify incoming format as either urdf or gazebo format xml")
          sys.exit(0)
        if self.model_name == "":
          rospy.logerr("Error: you must specify robot name")
          sys.exit(0)

    def checkForModel(self,model):
        for n in model.name:
          #rospy.logdebug("models: ",n)
          if n == self.wait_for_model:
            self.wait_for_model_exists = True


    def callSpawnService(self):

        # wait for model to exist
        rospy.init_node('spawn_model')
        if not self.wait_for_model == "":
          rospy.Subscriber("/gazebo/model_states", ModelStates, self.checkForModel)
          while not self.wait_for_model_exists:
            rospy.logdebug("waiting for model %s",self.wait_for_model)
            time.sleep(1)


        if self.file_name != "":
          rospy.logdebug("loading model xml from file")
          if os.path.exists(self.file_name):
            if os.path.isdir(self.file_name):
              rospy.logerr("Error: file[%s] is a path?",self.file_name)
              sys.exit(0)
            if not os.path.isfile(self.file_name):
              rospy.logerr("Error: unable to open file[%s]",self.file_name)
              sys.exit(0)
          else:
            rospy.logerr("Error: file[%s] does not exist",self.file_name)
            sys.exit(0)
          # load file
          f = open(self.file_name,'r')
          model_xml = f.read()
          if model_xml == "":
            rospy.logerr("Error: file name [%s] is an empty string",self.file_name)
            sys.exit(0)

        elif self.param_name != "":
          rospy.logdebug("loading model xml from ros parameter")
          model_xml = rospy.get_param(self.param_name)
          if model_xml == "":
            rospy.logerr("Error: param name[%s] does not exist",self.param_name)
            sys.exit(0)
        else:
          rospy.logerr("Error: user specified param or filename [%s] is an empty string",self.param_name)
          sys.exit(0)

        # setting initial pose
        initial_pose = Pose()
        initial_pose.position.x = self.initial_xyz[0]
        initial_pose.position.y = self.initial_xyz[1]
        initial_pose.position.z = self.initial_xyz[2]
        # convert rpy to quaternion for Pose message
        tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2])
        q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
        initial_pose.orientation = q;


        # pause simulation before spawning model
        # but first check if simulation is already paused, store status for restore later.
        simulation_was_paused = False;
        rospy.wait_for_service('/gazebo/get_physics_properties')
        try:
          get_physics_properties = rospy.ServiceProxy('/gazebo/get_physics_properties',GetPhysicsProperties)
          physics_resp = get_physics_properties()
          simulation_was_paused = physics_resp.pause
        except rospy.ServiceException, e:
          rospy.logerr("Service call to check if physics is paused before spawning object failed: [%s]",e)

        # pause simulation before spawning model
        if not simulation_was_paused:
          rospy.wait_for_service('/gazebo/pause_physics')
          try:
            pause_physics = rospy.ServiceProxy('/gazebo/pause_physics',Empty)
            resp = pause_physics()
          except rospy.ServiceException, e:
            rospy.logerr("Service call to pause_physics before spawning object failed: [%s]",e)

        # spawn model
        if self.urdf_format:
          success = gazebo_interface.spawn_urdf_model_client(self.model_name,model_xml,self.robot_namespace,initial_pose,self.reference_frame)
        elif self.gazebo_format:
          success = gazebo_interface.spawn_gazebo_model_client(self.model_name,model_xml,self.robot_namespace,initial_pose,self.reference_frame)
        else:
          rospy.logerr("Error: should not be here in spawner helper script, there is a bug")
          sys.exit(0)

        # set model configuration before unpause if user requested
        if len(self.joint_names) != 0:
          # # turn off all controllers
          # rospy.wait_for_service('/pr2_controller_manager/switch_controller')
          # rospy.wait_for_service('/pr2_controller_manager/list_controllers')
          # try:
          #   scr = SwitchControllerRequest()
          #   del scr.start_controllers[:]
          #   del scr.stop_controllers[:]
          #   # get list of all active controllers
          #   list_controllers_service = rospy.ServiceProxy('/pr2_controller_manager/list_controllers',ListControllers)
          #   controllers_states = list_controllers_service()
          #   # pick only the running ones and add to request for stopping
          #   for i in range(0,len(controllers_states.controllers)-1):
          #     if controllers_states.state[i] == 'running':
          #       scr.stop_controllers.append(controllers_states.controllers[i])
          #   # stop controllers
          #   switch_controller_service = rospy.ServiceProxy('/pr2_controller_manager/switch_controller',SwitchController)
          #   scr.strictness = scr.STRICT
          #   result = switch_controller_service(scr)
          #   rospy.logerr("stopped controllers service call failed: %s",result)
          # except rospy.ServiceException, e:
          #   rospy.logerr("turn off controllers service call failed: %s",e)


          # set joint positions
          rospy.wait_for_service('/gazebo/set_model_configuration')
          try:
            set_model_configuration = rospy.ServiceProxy('/gazebo/set_model_configuration',SetModelConfiguration)
            rospy.logdebug("debug: joint [%s] position [%s]",self.joint_names,self.joint_positions)
            set_model_configuration(self.model_name,self.param_name,self.joint_names,self.joint_positions)
          except rospy.ServiceException, e:
            rospy.logerr("set model configuration service call failed: %s",e)

          # # resume all controllers
          # try:
          #   scr = SwitchControllerRequest()
          #   del scr.start_controllers[:]
          #   del scr.stop_controllers[:]
          #   # pick only the running ones and add to request for stopping
          #   for i in range(0,len(controllers_states.controllers)-1):
          #     if controllers_states.state[i] == 'running':
          #       scr.start_controllers.append(controllers_states.controllers[i])
          #   # stop controllers
          #   switch_controller_service = rospy.ServiceProxy('/pr2_controller_manager/switch_controller',SwitchController)
          #   scr.strictness = scr.STRICT
          #   result = switch_controller_service(scr)
          # except rospy.ServiceException, e:
          #   rospy.logerr("turn off controllers service call failed: %s",e)

        # return simulation pause state before spawn or unpause physics if user requested
        if not simulation_was_paused or self.unpause_physics:
          rospy.wait_for_service('/gazebo/unpause_physics')
          try:
            unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics',Empty)
            unpause_physics()
          except rospy.ServiceException, e:
            rospy.logerr("unpause physics service call failed: %s",e)

        return

if __name__ == "__main__":
    if len(sys.argv) < 2:
        print usage()
    else:
        sm = SpawnModel()
        sm.parseUserInputs()
        sm.callSpawnService()
