#!/bin/bash 

# Tweak the default interbotix_ws install to add a customized robot wx250s_custom

read -p "Enter the interbotix packages installation path [default: ~/interbotix_ws]:" DIR
DIR=${DIR:-~/interbotix_ws}

cp xs_common.py "$DIR/src/interbotix_ros_toolboxes/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_common/"
cp wx250s_custom.yaml "$DIR/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_control/config/"
cp -r wx250s_custom_meshes "$DIR/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/meshes/"

VAR=`awk 'NR==451{ print; exit }' wx250s_custom.urdf.xacro`
VAR=`awk -F'"' '{print $2}' <<< $VAR`

read -p "Enter the coordinate between wrist_rotate joint and SMA center in m [default: ${VAR}]:" COORD
COORD=${COORD:-${VAR}}

cp wx250s_custom.urdf.xacro wx250s_custom.urdf.xacro~

sed -i "451s/$VAR/$COORD/" wx250s_custom.urdf.xacro~

# convert to urdf file needed for kinematic_from_description package
ros2 run xacro xacro wx250s_custom.urdf.xacro~ > out.urdf

# move the urdf.xacro file
mv wx250s_custom.urdf.xacro~ "$DIR/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/wx250s_custom.urdf.xacro"

#
git clone https://github.com/Interbotix/kinematics_from_description.git
cd kinematics_from_description
python setup.py install --user
cd ..
rm -rf kinematics_from_description

cp mr_descriptions.py mr_descriptions.py~

python kinematics.py

mv mr_descriptions.py "$DIR/src/interbotix_ros_toolboxes/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/"

mv mr_descriptions.py~ mr_descriptions.py

rm out.urdf

cd $DIR

rm -rf build
rm -rf install

colcon build