#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2010, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: rosws 14389 2011-07-20 18:38:40Z tfoote $
# $Author: tfoote $

"""
usage: rosworkspace [OPTIONS] {init PATH | {add | delete} STACK}
"""

from __future__ import print_function
import traceback
import sys
import os
import shutil
import yaml
import distutils.version
from subprocess import Popen, PIPE
from optparse import OptionParser

class ROSWorkspaceException(Exception): pass

class ROSWorkspace:

    def __init__(self, args):
        self._parse_args(args)
        if self.mode != 'init':
            self._read_rosinstall_file()

    def _parse_args(self, args):
        parser = OptionParser(usage='usage: %prog {init PATH | {add | delete} STACK}', version='%prog 0.0.1')
        parser.add_option("-p", "--path", dest="path", default=None,
                          help="path to workspace; overrides ROS_WORKSPACE",
                          action="store")
        parser.add_option("-c", "--configure", dest="noupdate", default=False,
                          help="change configuration, but don't run rosinstall",
                          action="store_true")
        parser.add_option("-N", "--non-recursive", dest="norecurse", default=False,
                          help="don't change configuration for dependent stacks",
                          action="store_true")
        parser.add_option("--released", dest="released", default=False,
                          help="Pull stack from release tag instead of development branch",
                          action="store_true")
        parser.add_option("-d", "--delete-working-copies", dest="delete", default=False,
                          help="when deleting a stack from the configuration, also delete the working copy (DANGEROUS!)",
                          action="store_true")

        # We take all rosinstall args and pass them through; there's
        # probably some refactoring to be done to share the args more
        # effectively.
        parser.add_option("-n", "--nobuild", dest="rosinstall_options", default=[],
                          help="(rosinstall) skip the build step for the ROS stack",
                          action="append_const", const='-n')
        parser.add_option("--rosdep-yes", dest="rosinstall_options",
        default=[],
                          help="(rosinstall) Pass through --rosdep-yes to rosmake", 
                          action="append_const", const='--rosdep-yes')
        parser.add_option("--continue-on-error", dest="rosinstall_options", default=[],
                          help="(rosinstall) Continue despite checkout errors", 
                          action="append_const", const='--continue-on-error')
        parser.add_option("--delete-changed-uris", dest="rosinstall_options", default=[],
                          help="(rosinstall) Delete the local copy of a directory before changing uri.", 
                          action="append_const", const='--delete-changed-uris')
        parser.add_option("--abort-changed-uris", dest="rosinstall_options", default=[],
                          help="(rosinstall) Abort if changed uri detected", 
                          action="append_const", const='--abort-changed-uris')
        # Not easy to pass these options through, because they take
        # arguments.
        #parser.add_option("--backup-changed-uris", dest="backup_changed", default='',
        #                  help="backup the local copy of a directory before changing uri to this directory.", 
        #                  action="store")
        #parser.add_option("--generate-versioned-rosinstall", dest="generate_versioned", default=None,
        #                  help="generate a versioned rosintall file", action="store")

        (options, args) = parser.parse_args(args)
        self.noupdate = options.noupdate
        self.norecurse = options.norecurse
        self.released = options.released
        self.rosinstall_options = options.rosinstall_options
        self.path = options.path
        self.delete = options.delete
    
        if len(args) < 2:
            parser.error('rosworkspace requires at least 1 arguments')

        self.mode = args[1]
        self.init_args = None
        self.stacks = []
        if self.mode == 'init':
            if len(args) < 3:
                parser.error('init command requires exactly at least 2 arguments')
            self.path = args[2]
            self.init_args = args[3:]
        elif self.mode == 'add' or self.mode == 'delete':
            if len(args) < 3:
                parser.error('add and delete commands require at least 2 arguments')
            self.stacks = args[2:]

            if self.path is None:
                if 'ROS_WORKSPACE' in os.environ:
                    self.path = os.environ['ROS_WORKSPACE']
                else:
                    parser.error('no ROS_WORKSPACE and no --path option given')
        else:
            parser.error('unknown mode "%s"'%(self.mode))
        self.rosinstall_fname = os.path.join(self.path, '.rosinstall')

    def _read_rosinstall_file(self):
        if not os.path.isfile(self.rosinstall_fname):
            raise ROSWorkspaceException('No such file %s'%self.rosinstall_fname)
        with open(self.rosinstall_fname, 'r') as f:
            self.rosinstall = yaml.load(f)
        
        if [e for e in self.rosinstall if not 'local-name' in e.values()[0]]:
            raise ROSWorkspaceException("invalid rosinstall file: missing local-name key")

    def _get_dependent_stacks(self, stack):
        # roslib.stacks doesn't expose the dependency parts of rosstack, so
        # we'll call it manually
        cmd = ['rosstack', 'depends-on', stack]
        try:
            p = Popen(cmd, stdout=PIPE, stderr=PIPE)
        except OSError as e:
            raise ROSWorkspaceException('%s\nfailed to execute rosstack; is your ROS environment configured?'%(e))
        stdout, stderr = p.communicate()
        if p.returncode != 0:
            raise ROSWorkspaceException('rosstack failed: %s'%(stderr))
        # Make sure to exclude empty lines
        deps = []
        for l in stdout.split('\n'):
            if len(l) > 0:
                deps.append(l)
        return deps

    def _remove_stack_from_overlay(self, stack):
        x = [e for e in self.rosinstall if e.values()[0]['local-name'] == stack]
        if x:
            self.rosinstall.remove(x[0])
            return True
        else:
            return False

    def _roslocate_info(self, stack, distro, dev):
        # TODO: use roslocate from code
        cmd = ['roslocate', 'info', '--distro=%s'%(distro), stack]
        if dev == True:
            cmd.append('--dev')
        try:
            p = Popen(cmd, stdout=PIPE, stderr=PIPE)
        except OSError as e:
            raise ROSWorkspaceException('%s\nfailed to execute roslocate; is your ROS environment configured?'%(e))

        stdout, stderr = p.communicate()
        if p.returncode != 0:
            sys.stderr.write('[rosws] Warning: failed to locate stack "%s" in distro "%s".  Falling back on non-distro-specific search; compatibility problems may ensue.\n'%(stack,distro))
            # Could be that the stack hasn't been released; try roslocate
            # again, without specifying the distro.
            cmd = ['roslocate', 'info', stack]
            if dev == True:
                cmd.append('--dev')
            try:
                p = Popen(cmd, stdout=PIPE, stderr=PIPE)
            except OSError as e:
                raise ROSWorkspaceException('%s\nfailed to execute roslocate; is your ROS environment configured?'%(e))
    
            stdout, stderr = p.communicate()
            if p.returncode != 0:
                raise ROSWorkspaceException('roslocate failed: %s'%(stderr))
        return yaml.load(stdout)

    def _rosversion_to_distro_name(self):
        # TODO: switch to `rosversion -d` after it's been released (r14279,
        # r14280)
        cmd = ['rosversion', 'ros']
        try:
            p = Popen(cmd, stdout=PIPE, stderr=PIPE)
        except OSError as e:
            raise ROSWorkspaceException('%s\nfailed to execute rosversion; is your ROS environment configured?'%(e))

        stdout, stderr = p.communicate()
        if p.returncode != 0:
            raise ROSWorkspaceException('rosversion failed: %s'%(stderr))
        ver = distutils.version.StrictVersion(stdout).version
        if len(ver) < 2:
            raise ROSWorkspaceException('invalid ros version: %s'%(stdout))
        major, minor = ver[0:2]
        if major == 1 and minor == 6:
            return 'electric'
        elif major == 1 and minor == 5:
            return 'unstable'
        elif major == 1 and minor == 4:
            return 'diamondback'
        else:
            raise ROSWorkspaceException('unknown ros version: %s'%(stdout))

    def _invoke_rosinstall(self, args=[], my_stdout=sys.stdout, my_stderr=sys.stderr):
        cmd = ['rosinstall', self.path]
        cmd.extend(self.rosinstall_options)
        cmd.extend(args)
        p = Popen(cmd, stdout=my_stdout, stderr=my_stderr)
        stdout, stderr = p.communicate()
        if p.returncode != 0:
            raise ROSWorkspaceException('rosinstall failed: %s'%(stderr))

    # Main entry point
    def switch(self):
        if self.mode == 'init':
            # If any arguments where given, we pass them to rosinstall.
            # Otherwise, we infer bootstrap info from the environment.
            if not self.init_args:
                print('No arguments given; initializing from current environment')
                if 'ROS_ROOT' not in os.environ:
                    raise ROSWorkspaceException('ROS_ROOT not set, and no arguments given to init')
                self.init_args = [os.environ['ROS_ROOT']]
                if 'ROS_PACKAGE_PATH' in os.environ:
                    for p in os.environ['ROS_PACKAGE_PATH'].split(':'):
                        self.init_args.append(p)
            self._invoke_rosinstall(args=self.init_args)

        else:
            distro = self._rosversion_to_distro_name()
            for stack in self.stacks:
                self._remove_stack_from_overlay(stack)
                if self.mode == 'add':
                    info = self._roslocate_info(stack, distro, not self.released)
                    self.rosinstall.extend(info)    
                    if not self.norecurse:
                        deps = self._get_dependent_stacks(stack)
                        # Also switch anything that depends on this stack
                        for s in deps:
                            self._remove_stack_from_overlay(s)
                            info = self._roslocate_info(s, distro, not self.released)
                            self.rosinstall.extend(info)
                elif self.mode == 'delete':
                    if self.delete:
                        shutil.rmtree(os.path.join(self.path, stack), ignore_errors=True)
                    if not self.norecurse:
                        deps = self._get_dependent_stacks(stack)
                        for s in deps:
                            self._remove_stack_from_overlay(s)
                            if self.delete:
                                shutil.rmtree(os.path.join(self.path, s), ignore_errors=True)
                else:
                    raise ROSWorkspaceException("unknown mode: %s"%(self.mode))

            # Back up the original rosinstall file
            backup_fname = os.path.join(self.path, '.rosinstall.bak')
            shutil.copyfile(self.rosinstall_fname, backup_fname)
    
            # Output a new rosinstall file
            with open(self.rosinstall_fname, 'w') as f:
                yaml.dump(self.rosinstall, f, default_flow_style=False)
    
            print('Wrote new rosinstall configuration to %s'%(self.rosinstall_fname))
            if self.noupdate:
                print('You should now update your overlay like so:')
                print('  rosinstall %s'%(self.path))
            else:
                print('Runnning rosinstall to update your overlay...')
                self._invoke_rosinstall()

if __name__ == '__main__':
    try:
        ri = ROSWorkspace(sys.argv)
        ri.switch()
    except Exception as e:
        print('Error: %s'%(e))
        #traceback.print_exc()
        sys.exit(1)
