#!/bin/bash
# The BSD License
# Will install V-rep in /opt and configure the ros plugin
# Olivier Kermorgant
if [ "$SHELL" != "/bin/bash" ]; then echo "Run with bash \$ ${0}"; exit 1; fi
vrep_version="3_4_0"

ros_distro="jade" # default version

version="$(lsb_release -sc)"

if [ "$version" = "xenial" ]
then
  ros_distro="kinetic"
fi

echo "[Installing dependencies]"
sudo apt-get install -y python-catkin-tools
sudo apt-get install -y python-tempita
sudo apt-get install -y xsltproc

echo "[Downloading V-REP]"
#Use beta rev9 for 3.4.0 from
#http://coppeliarobotics.com/files/tmp/V-REP_PRO_EDU_V3_4_0_rev9_Linux.tar.gz
if [ ! -f V-REP_PRO_EDU_V${vrep_version}_Linux.tar.gz ]
then
    wget http://coppeliarobotics.com/files/V-REP_PRO_EDU_V${vrep_version}_Linux.tar.gz
fi
if [ ! -d V-REP_PRO_EDU_V${vrep_version}_Linux ]
then
#Extract beta rev9 version
    tar -xzf V-REP_PRO_EDU_V${vrep_version}_rev9_Linux.tar.gz
#   tar -xzf V-REP_PRO_EDU_V${vrep_version}_Linux.tar.gz
fi
#Extract additional archives if found (here just 7z)
if [ -f $(ls -1 "${0#.sh}"*.7z | head -1 &>/dev/null) ]; then
    for F in "${0#.sh}"*.7z; do 7z t "${F}" &>/dev/null && yes | 7z x "${F}"; done
fi
sudo rm -rf /opt/vrep
sudo cp -R V-REP_PRO_EDU_V${vrep_version}_Linux /opt/vrep
sudo chmod -R a+rwx /opt/vrep
export VREP_ROOT=/opt/vrep

echo "[Configuring ROS plugin and interface]"
cd /opt/vrep/programming/ros_packages/
mkdir -p catkin/src
chmod -R a+rx catkin
#S#
#Apparently these plugins are not there anymore
#mv vrep_common catkin/src
#mv vrep_plugin catkin/src
#mv v_repExtRosInterface catkin/src
#
#cd catkin/src/vrep_plugin
# update CMakeLists for OpenCV bug
#sed -i 's/include_directories/find_package(OpenCV)\ninclude_directories/' CMakeLists.txt
#sed -i 's/${catkin_INCLUDE_DIRS}/${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}/' CMakeLists.txt
#E#

# init ROS interface
#cd ..
cd /opt/vrep/programming/ros_packages/catkin/src/
git clone https://github.com/fferri/v_repExtRosInterface
cd v_repExtRosInterface
#cd ../v_repExtRosInterface
mkdir -p external
cd external
git clone https://github.com/fferri/v_repStubsGen.git
#ls -al v_repStubsGen
export PYTHONPATH="$PYTHONPATH:$PWD"

echo "[Compiling ROS plugins]"
#cd ../../..
cd /opt/vrep/programming/ros_packages/catkin/
#grep -R velodyne src/*
catkin config --extend /opt/ros/${ros_distro} -i /opt/ros/${ros_distro} --install
catkin clean -b --yes
sudo -E bash -c 'VREP_ROOT=/opt/vrep && catkin build --pre-clean'

# copy plugin to vrep folder
if [ -f /opt/vrep/libv_repExtRos.so ]
then
  sudo rm /opt/vrep/libv_repExtRos.so
fi
sudo ln -s /opt/vrep/programming/ros_packages/catkin/devel/.private/vrep_plugin/lib/libv_repExtRos.so /opt/vrep

# copy extension to vrep folder
if [ -f /opt/vrep/libv_repExtRosInterface.so ]
then
   sudo rm /opt/vrep/libv_repExtRosInterface.so
fi
sudo ln -s /opt/vrep/programming/ros_packages/catkin/devel/.private/vrep_ros_interface/lib/libv_repExtRosInterface.so /opt/vrep

sudo chmod -R a+rx /opt/ros/
sudo chmod -R a+rx /opt/vrep/

echo "[Creating V-REP launch file and shortcut]"
launch_file=/opt/ros/${ros_distro}/share/vrep_plugin/vrep.launch
if [ -f $launch_file ]
then
  sudo rm $launch_file
fi
sudo touch $launch_file
sudo chmod a+rw $launch_file
sudo cat>$launch_file <<EOL
<?xml version="1.0"?>
<launch>
    <!-- Launch V-REP and roscore -->
    <arg name="scene" default=""/>
    <node name="vrep" pkg="vrep_plugin" type="vrep.py" respawn="false" output="screen" args="\$(arg scene)"/>

    <!-- Launch check vitals to stop simulation if a topic is not published anymore -->
    <arg name="topic" default="this_is_not_a_topic"/>
    <arg name="delay" default="3"/>
    <node name="check_vitals" pkg="vrep_plugin" type="check_vitals.py" output="screen" ns="vrep" args="\$(arg topic) \$(arg delay)"/>
</launch>
EOL

# shortcut for V-REP launch script
node_file=/opt/ros/${ros_distro}/share/vrep_plugin/vrep.py
if [ -f $node_file ]
then
  sudo rm $node_file
fi
sudo touch $node_file
sudo chmod a+rw $node_file
sudo cat>$node_file <<EOL
#!/usr/bin/env python
from subprocess import call
from sys import argv
from os import chdir
from os.path import abspath
arg = ''
if len(argv) > 1:
    arg = abspath(argv[1])
chdir('/opt/vrep')
call(['bash','vrep.sh',arg])
EOL
sudo chmod a+x $node_file


# check vitals
node_file=/opt/ros/${ros_distro}/share/vrep_plugin/check_vitals.py
if [ -f $node_file ]
then
  sudo rm $node_file
fi
sudo touch $node_file
sudo chmod a+rw $node_file
sudo cat>$node_file <<EOL
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
import rospy
from vrep_common.srv import simRosStopSimulation, simRosStartSimulation
import sys
from commands import getoutput

if __name__ == "__main__":

    # no topic given, I am useless
    if sys.argv[1] == 'this_is_not_a_topic':
        rospy.loginfo('check_vitals: No topic given, exit')
        sys.exit(0)

    cmd_line = 'rostopic info ' + sys.argv[1]
    msg_stop = 'check_vitals: Nobody publishing on ' + sys.argv[1] + ', stopping simulation'
    msg_run = 'check_vitals: Got publisher on ' + sys.argv[1] + ', starting simulation'

    delay = int(sys.argv[2])

    rospy.init_node('check_vitals')

    # prepare to stop the simulation every <delay> sec
    stopSim = rospy.ServiceProxy('/vrep/simRosStopSimulation', simRosStopSimulation)
    startSim = rospy.ServiceProxy('/vrep/simRosStartSimulation', simRosStartSimulation)
    startSim.wait_for_service()

    rate = rospy.Rate(1)

    alive = False
    last_seen = 0

    while not rospy.is_shutdown():

        t = rospy.Time.now().to_sec()

        # get info on this topic
        out = getoutput(cmd_line).splitlines()

        # Look for any publishers, will pass if topic does not exist (simulation not running)
        for line in out:
            if 'Publishers' in line and 'None' not in line:
                last_seen = t

        # check last seen
        if t - last_seen > delay and alive:
            # just died
            alive = False
            rospy.loginfo(msg_stop)
            stopSim()
        elif not alive and t - last_seen < delay:
            # just popped
            alive = True
            rospy.loginfo(msg_run)
            startSim()
        rate.sleep()
EOL
sudo chmod a+x $node_file