#! /usr/bin/env python

import sys
import os
import commands


def eus2urdf_for_gazebo (name, collada_path, urdf_path = (os.getenv("HOME") + "/.gazebo/models"), overwrite = True):

    urdf_dir_path = urdf_path + "/" + name
    if overwrite:
        os.system("rm -rf %s" % urdf_dir_path)
    else:
        print "[eus2urdf] check if the same name model already exits"
        if os.path.exists(urdf_dir_path):
            print '[ERROR eus2urdf] the same name model already exits'
            exit(1)

    print "[eus2urdf] make directory for urdf"
    os.mkdir(urdf_dir_path)

    add_line_string = '\<uri\>file://%s\</uri\>' % name
    model_config_path = '%s/../model.config' % urdf_dir_path
    if len(commands.getoutput("grep %s %s" % (add_line_string, model_config_path))) == 0:
        print "[eus2urdf] add file path to model.config"
        os.system('sed -i -e \"s@  </models>@    %s\\n  </models>@g\" %s' % (add_line_string, model_config_path))

    print "[eus2urdf] make model.config in urdf directory"
    config_path = '%s/model.config' % urdf_dir_path
    os.system('echo "<?xml version=\'1.0\'?>\n<model>\n  <name>%s</name>\n  <version>0.1.0</version>\n  <sdf>model.urdf</sdf>\n  <description>\n     This model was automatically generated by converting the eus model.\n  </description>\n</model>\n" > %s' % (name, config_path))

    print "[eus2urdf] convert collada to urdf"
    meshes_path = urdf_dir_path + '/meshes'
    urdf_path = urdf_dir_path + '/' + 'model.urdf'
    os.mkdir(meshes_path)
    os.system('rosrun collada_urdf_jsk_patch collada_to_urdf %s -G -A --mesh_output_dir %s --mesh_prefix "model://%s/meshes" -O %s' % (collada_path, meshes_path, name, urdf_path))
    os.system('sed -i -e "s@continuous@revolute@g" %s' % urdf_path)
    os.system('sed -i -e \"s@<robot name=\\"inst_kinsystem\\"@<robot name=\\"%s\\"@g\" %s' % (name, urdf_path))
    os.system('sed -i -e \"1,/  <link /s/  <link /  <gazebo>\\n    <static>false<\/static>\\n  <\/gazebo>\\n  <link /\" %s' % urdf_path)

    # print "[eus2urdf] add inertia property to urdf   # Inertia value is not correct. Inertia should be added at eus2collada."
    #os.system('sed -i -e \"s@<inertia ixx=.*/>@<inertia ixx=\\\"1e-03\\\" ixy=\\\"0\\\" ixz=\\\"0\\\" iyy=\\\"1e-03\\\" iyz=\\\"0\\\" izz=\\\"1e-03\\\"/>@g\" %s' % urdf_path)


if __name__ == '__main__':
    if len(sys.argv) > 3:
        eus2urdf_for_gazebo(sys.argv[1], sys.argv[2], sys.argv[3])
    elif len(sys.argv) > 2:
        eus2urdf_for_gazebo(sys.argv[1], sys.argv[2])
