#!/usr/bin/env python
# Copyright (c) 2020 - The Procedural Generation for Gazebo authors
# For information on the respective copyright owner see the NOTICE file
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import os
from pcg_gazebo.simulation import SimulationModel
from pcg_gazebo.parsers import parse_xacro, parse_urdf
import argparse


if __name__ == '__main__':
    parser = argparse.ArgumentParser(
        'Parse a URDF file and display its geometries'
        ' and meshes. XACRO files can also be provided'
        ' and will processed to generate the URDF files.')
    parser.add_argument(
        '--filename',
        '-f', type=str, help='URDF or XACRO file')
    parser.add_argument(
        '--collision',
        '-c', action='store_true',
        help='View collision meshes')

    args = parser.parse_args()

    assert os.path.exists(args.filename), 'Invalid filename provided'

    if args.filename.endswith('.xacro'):
        urdf = parse_xacro(args.filename, output_type='urdf')
    elif args.filename.endswith('.urdf'):
        urdf = parse_urdf(args.filename)
    else:
        raise ValueError('Invalid file format, use .urdf or .xacro')

    model = SimulationModel.from_urdf(urdf)

    if args.collision:
        model.show(mesh_type='collision')
    else:
        model.show(mesh_type='visual')
