#!/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 modes.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/"

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

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

read -p "Enter the Roll-Pitch-Yaw angles (in radians) between the wrist_rotate frame and SMA frame [default: ${RPY}]:" RPY2
RPY2=${RPY2:-${RPY}}

sed -i "494s/$RPY/$RPY2/" wx250s_custom.urdf.xacro~

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

read -p "Enter the translation vector (in m) between wrist_rotate frame and SMA frame in [default: ${XYZ}]:" XYZ2
XYZ2=${XYZ2:-${XYZ}}

sed -i "451s/$XYZ/$XYZ2/" 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