#!/usr/bin/env python3
import os

INSTRUMENTATION = (
    "// BUGZOO :: INSTRUMENTATION :: START\n"
    "#include <stdio.h>\n"
    "#include <stdlib.h>\n"
    "#include <signal.h>\n"
    "void bugzoo_sighandler(int sig){\n"
    "  exit(1);\n"
    "}\n"
    "void bugzoo_ctor (void) __attribute__ ((constructor));\n"
    "void bugzoo_ctor (void) {\n"
    "  struct sigaction new_action;\n"
    "  new_action.sa_handler = bugzoo_sighandler;\n"
    "  sigemptyset(&new_action.sa_mask);\n"
    "  new_action.sa_flags = 0;\n"
    "  sigaction(SIGTERM, &new_action, NULL);\n"
    "  sigaction(SIGINT, &new_action, NULL);\n"
    "  sigaction(SIGKILL, &new_action, NULL);\n"
    "  sigaction(SIGSEGV, &new_action, NULL);\n"
    "  sigaction(SIGFPE, &new_action, NULL);\n"
    "  sigaction(SIGBUS, &new_action, NULL);\n"
    "  sigaction(SIGILL, &new_action, NULL);\n"
    "  sigaction(SIGABRT, &new_action, NULL);\n"
    "}\n"
    "// BUGZOO :: INSTRUMENTATION :: END\n"
)
ROS_WS = "/ros_ws"
FILES_TO_INSTRUMENT = [
    "src/actionlib/test/action_client_destruction_test.cpp",
    "src/actionlib/test/add_two_ints_client.cpp",
    "src/actionlib/test/add_two_ints_server.cpp",
    "src/actionlib/test/destruction_guard_test.cpp",
    "src/actionlib/test/exercise_simple_client.cpp",
    "src/actionlib/test/ref_server.cpp",
    "src/actionlib/test/server_goal_handle_destruction.cpp",
    "src/actionlib/test/simple_client_allocator_test.cpp",
    "src/actionlib/test/simple_client_test.cpp",
    "src/actionlib/test/simple_client_wait_test.cpp",
    "src/actionlib/test/simple_execute_ref_server.cpp",
    "src/actionlib/test/test_cpp_simple_client_cancel_crash.cpp",
    "src/angles/test/utest.cpp",
    "src/class_loader/test/unique_ptr_test.cpp",
    "src/class_loader/test/utest.cpp",
    "src/common_msgs/sensor_msgs/test/main.cpp",
    "src/depthimage_to_laserscan/src/depthimage_to_laserscan.cpp",
    "src/depthimage_to_laserscan/test/depthimage_to_laserscan_rostest.cpp",
    "src/depthimage_to_laserscan/test/DepthImageToLaserScanTest.cpp",
    "src/diagnostics/diagnostic_aggregator/src/aggregator_node.cpp",
    "src/diagnostics/diagnostic_aggregator/test/analyzer_loader.cpp",
    "src/diagnostics/diagnostic_updater/src/example.cpp",
    "src/diagnostics/diagnostic_updater/test/diagnostic_updater_test.cpp",
    "src/dynamic_reconfigure/test/ref_server.cpp",
    "src/dynamic_reconfigure/test/test_client.cpp",
    "src/dynamic_reconfigure/test/testserver.cpp",
    "src/ecl_core/ecl_command_line/src/test/command_line.cpp",
    "src/ecl_core/ecl_concepts/src/test/concepts.cpp",
    "src/ecl_core/ecl_containers/src/test/arrays.cpp",
    "src/ecl_core/ecl_containers/src/test/arrays_mem_check.cpp",
    "src/ecl_core/ecl_containers/src/test/container_converters.cpp",
    "src/ecl_core/ecl_containers/src/test/fifo.cpp",
    "src/ecl_core/ecl_containers/src/test/push_and_pop.cpp",
    "src/ecl_core/ecl_containers/src/test/stencil.cpp",
    "src/ecl_core/ecl_converters/src/test/byte_array_converters.cpp",
    "src/ecl_core/ecl_converters/src/test/converters.cpp",
    "src/ecl_core/ecl_devices/src/test/files.cpp",
    "src/ecl_core/ecl_devices/src/test/shared_files.cpp",
    "src/ecl_core/ecl_geometry/graveyard/pose3d_cpp.old",
    "src/ecl_core/ecl_geometry/src/test/angles.cpp",
    "src/ecl_core/ecl_geometry/src/test/cartesian_points.cpp",
    "src/ecl_core/ecl_geometry/src/test/cubic_splines.cpp",
    "src/ecl_core/ecl_geometry/src/test/homogeneous_points.cpp",
    "src/ecl_core/ecl_geometry/src/test/legacy_pose2d.cpp",
    "src/ecl_core/ecl_geometry/src/test/legacy_pose3d.cpp",
    "src/ecl_core/ecl_geometry/src/test/linear_segment.cpp",
    "src/ecl_core/ecl_geometry/src/test/odometry_helper.cpp",
    "src/ecl_core/ecl_geometry/src/test/polynomials.cpp",
    "src/ecl_core/ecl_geometry/src/test/smooth_linear_splines.cpp",
    "src/ecl_core/ecl_geometry/src/test/tension_function.cpp",
    "src/ecl_core/ecl_geometry/src/test/tension_splines.cpp",
    "src/ecl_core/ecl_linear_algebra/src/test/formatters.cpp",
    "src/ecl_core/ecl_linear_algebra/src/test/interpolators.cpp",
    "src/ecl_core/ecl_linear_algebra/src/test/plugin.cpp",
    "src/ecl_core/ecl_math/src/test/constants.cpp",
    "src/ecl_core/ecl_math/src/test/fuzzy.cpp",
    "src/ecl_core/ecl_math/src/test/norms.cpp",
    "src/ecl_core/ecl_math/src/test/simple.cpp",
    "src/ecl_core/ecl_mpl/src/test/enable_if.cpp",
    "src/ecl_core/ecl_sigslots/src/test/sigslots.cpp",
    "src/ecl_core/ecl_streams/src/test/file_streams.cpp",
    "src/ecl_core/ecl_streams/src/test/string_streams.cpp",
    "src/ecl_core/ecl_threads/src/test/mutex.cpp",
    "src/ecl_core/ecl_threads/src/test/priorities.cpp",
    "src/ecl_core/ecl_threads/src/test/threadable.cpp",
    "src/ecl_core/ecl_threads/src/test/threads.cpp",
    "src/ecl_core/ecl_time/src/test/cpuwatch_rt.cpp",
    "src/ecl_core/ecl_time/src/test/frequency.cpp",
    "src/ecl_core/ecl_time/src/test/sleep.cpp",
    "src/ecl_core/ecl_time/src/test/snooze.cpp",
    "src/ecl_core/ecl_time/src/test/stopwatch.cpp",
    "src/ecl_core/ecl_time/src/test/time_data.cpp",
    "src/ecl_core/ecl_time/src/test/timestamp.cpp",
    "src/ecl_core/ecl_type_traits/src/test/fundamental_types.cpp",
    "src/ecl_core/ecl_type_traits/src/test/numeric_limits.cpp",
    "src/ecl_core/ecl_utilities/src/test/flags.cpp",
    "src/ecl_core/ecl_utilities/src/test/function_objects.cpp",
    "src/ecl_core/ecl_utilities/src/test/parameters.cpp",
    "src/ecl_core/ecl_utilities/src/test/references.cpp",
    "src/ecl_core/ecl_utilities/src/test/singleton.cpp",
    "src/ecl_lite/ecl_config/src/test/char_sign.cpp",
    "src/ecl_lite/ecl_config/src/test/endianness.cpp",
    "src/ecl_lite/ecl_config/src/test/portable_types.cpp",
    "src/ecl_navigation/ecl_mobile_robot/src/test/partial_inverse.cpp",
    "src/freenect_stack/freenect_camera/src/nodes/freenect_node.cpp",
    "src/geometry2/tf2_kdl/test/test_tf2_kdl.cpp",
    "src/geometry2/tf2_ros/src/buffer_server_main.cpp",
    "src/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp",
    "src/geometry2/tf2_ros/test/listener_unittest.cpp",
    "src/geometry2/tf2_ros/test/time_reset_test.cpp",
    "src/geometry2/tf2/test/cache_unittest.cpp",
    "src/geometry2/tf2/test/simple_tf2_core.cpp",
    "src/geometry2/tf2/test/static_cache_test.cpp",
    "src/geometry/tf_conversions/test/test_eigen_tf.cpp",
    "src/geometry/tf_conversions/test/test_kdl_tf.cpp",
    "src/geometry/tf/src/change_notifier.cpp",
    "src/geometry/tf/src/empty_listener.cpp",
    "src/geometry/tf/src/static_transform_publisher.cpp",
    "src/geometry/tf/src/tf_echo.cpp",
    "src/geometry/tf/src/tf_monitor.cpp",
    "src/geometry/tf/test/cache_unittest.cpp",
    "src/geometry/tf/test/quaternion.cpp",
    "src/geometry/tf/test/testBroadcaster.cpp",
    "src/geometry/tf/test/testListener.cpp",
    "src/geometry/tf/test/test_message_filter.cpp",
    "src/geometry/tf/test/test_transform_datatypes.cpp",
    "src/geometry/tf/test/tf_benchmark.cpp",
    "src/geometry/tf/test/tf_unittest.cpp",
    "src/geometry/tf/test/tf_unittest_future.cpp",
    "src/geometry/tf/test/transform_listener_unittest.cpp",
    "src/geometry/tf/test/transform_twist_test.cpp",
    "src/geometry/tf/test/velocity_test.cpp",
    "src/image_common/camera_info_manager/tests/unit_test.cpp",
    "src/image_common/image_transport/src/republish.cpp",
    "src/image_common/image_transport/tutorial/src/my_publisher.cpp",
    "src/image_common/image_transport/tutorial/src/my_subscriber.cpp",
    "src/image_pipeline/image_proc/src/nodes/image_proc.cpp",
    "src/image_pipeline/image_proc/test/rostest.cpp",
    "src/image_pipeline/image_proc/test/test_rectify.cpp",
    "src/kdl_parser/kdl_parser/test/test_inertia_rpy.cpp",
    "src/kdl_parser/kdl_parser/test/test_kdl_parser.cpp",
    "src/kobuki/kobuki_keyop/src/main.cpp",
    "src/laser_geometry/test/projection_test.cpp",
    "src/navigation/amcl/src/amcl_node.cpp",
    "src/navigation/base_local_planner/test/gtest_main.cpp",
    "src/navigation/base_local_planner/test/line_iterator_test.cpp",
    "src/navigation/clear_costmap_recovery/test/clear_tester.cpp",
    "src/navigation/costmap_2d/src/costmap_2d_cloud.cpp",
    "src/navigation/costmap_2d/src/costmap_2d_markers.cpp",
    "src/navigation/costmap_2d/src/costmap_2d_node.cpp",
    "src/navigation/costmap_2d/test/array_parser_test.cpp",
    "src/navigation/costmap_2d/test/costmap_tester.cpp",
    "src/navigation/costmap_2d/test/footprint_tests.cpp",
    "src/navigation/costmap_2d/test/inflation_tests.cpp",
    "src/navigation/costmap_2d/test/module_tests.cpp",
    "src/navigation/costmap_2d/test/obstacle_tests.cpp",
    "src/navigation/costmap_2d/test/static_tests.cpp",
    "src/navigation/fake_localization/fake_localization.cpp",
    "src/navigation/global_planner/src/plan_node.cpp",
    "src/navigation/map_server/src/main.cpp",
    "src/navigation/map_server/src/map_saver.cpp",
    "src/navigation/map_server/test/rtest.cpp",
    "src/navigation/map_server/test/utest.cpp",
    "src/navigation/move_base/src/move_base_node.cpp",
    "src/navigation/navfn/src/navfn_node.cpp",
    "src/navigation/navfn/test/path_calc_test.cpp",
    "src/navigation/robot_pose_ekf/src/odom_estimation_node.cpp",
    "src/navigation/robot_pose_ekf/test/test_robot_pose_ekf.cpp",
    "src/navigation/robot_pose_ekf/test/test_robot_pose_ekf_zero_covariance.cpp",
    "src/navigation/voxel_grid/test/voxel_grid_tests.cpp",
    "src/nodelet_core/nodelet/src/nodelet.cpp",
    "src/openni2_camera/openni2_camera/ros/openni2_camera_node.cpp",
    "src/pcl_conversions/test/test_pcl_conversions.cpp",
    "src/perception_pcl/pcl_ros/tools/bag_to_pcd.cpp",
    "src/perception_pcl/pcl_ros/tools/convert_pcd_to_image.cpp",
    "src/perception_pcl/pcl_ros/tools/convert_pointcloud_to_image.cpp",
    "src/perception_pcl/pcl_ros/tools/pcd_to_pointcloud.cpp",
    "src/perception_pcl/pcl_ros/tools/pointcloud_to_pcd.cpp",
    "src/pluginlib/test/unique_ptr_test.cpp",
    "src/pluginlib/test/utest.cpp",
    "src/realsense_camera/src/get_debug_info.cpp",
    "src/realsense_camera/test/camera_core.cpp",
    "src/realsense_camera/test/rgbd_topics.cpp",
    "src/robot_state_publisher/src/joint_state_listener.cpp",
    "src/robot_state_publisher/test/test_joint_states_bag.cpp",
    "src/robot_state_publisher/test/test_one_link.cpp",
    "src/robot_state_publisher/test/test_two_links_fixed_joint.cpp",
    "src/robot_state_publisher/test/test_two_links_moving_joint.cpp",
    "src/ros_comm/message_filters/test/msg_cache_unittest.cpp",
    "src/ros_comm/message_filters/test/test_approximate_time_policy.cpp",
    "src/ros_comm/message_filters/test/test_chain.cpp",
    "src/ros_comm/message_filters/test/test_exact_time_policy.cpp",
    "src/ros_comm/message_filters/test/test_simple.cpp",
    "src/ros_comm/message_filters/test/test_subscriber.cpp",
    "src/ros_comm/message_filters/test/test_synchronizer.cpp",
    "src/ros_comm/message_filters/test/time_sequencer_unittest.cpp",
    "src/ros_comm/message_filters/test/time_synchronizer_unittest.cpp",
    "src/ros_comm/rosbag/src/play.cpp",
    "src/ros_comm/rosbag/src/record.cpp",
    "src/ros_comm/rosconsole/test/assertion_test.cpp",
    "src/ros_comm/rosconsole/test/thread_test.cpp",
    "src/ros_comm/rosconsole/test/utest.cpp",
    "src/ros_comm/roscpp/src/libros/node_handle.cpp",
    "src/ros_comm/roslz4/test/roslz4_test.cpp",
    "src/ros_comm/rosout/rosout.cpp",
    "src/ros_comm/rostest/test/test_permuter.cpp",
    "src/ros_comm/topic_tools/src/demux.cpp",
    "src/ros_comm/topic_tools/src/drop.cpp",
    "src/ros_comm/topic_tools/src/mux.cpp",
    "src/ros_comm/topic_tools/src/relay.cpp",
    "src/ros_comm/topic_tools/src/switch_mux.cpp",
    "src/ros_comm/topic_tools/src/throttle.cpp",
    "src/ros_comm/topic_tools/test/test_shapeshifter.cpp",
    "src/ros_comm/topic_tools/test/utest.cpp",
    "src/roscpp_core/rostime/test/time.cpp",
    "src/rospack/test/test/utest.cpp",
    "src/ros/roslib/test/package.cpp",
    "src/ros/roslib/test/utest.cpp",
    "src/slam_gmapping/gmapping/src/main.cpp",
    "src/slam_gmapping/gmapping/src/replay.cpp",
    "src/slam_gmapping/gmapping/test/rtest.cpp",
    "src/stage_ros/src/stageros.cpp",
    "src/turtlebot_apps/turtlebot_navigation/src/laser_footprint_filter.cpp",
    "src/turtlebot/turtlebot_description/test/test_urdf.cpp",
    "src/unique_identifier/unique_id/tests/test_unique_id.cpp",
    "src/urdf/urdf/test/test_robot_model_parser.cpp",
    "src/vision_opencv/cv_bridge/test/utest.cpp",
    "src/vision_opencv/image_geometry/test/utest.cpp",
    "src/yujin_ocs/yocs_virtual_sensor/src/virtual_sensor_node.cpp",
    "src/zeroconf_avahi_suite/zeroconf_avahi/src/node/zeroconf.cpp",
    "src/orocos_kinematics_dynamics/orocos_kdl/tests/test-runner.cpp",
    "src/bfl/tests/model_test.cpp",
    "src/bfl/tests/pdf_test.cpp",
    "src/bfl/tests/smoother_test.cpp",
    "src/bfl/tests/matrixwrapper_test.cpp",
    "src/bfl/tests/complete_filter_test.cpp",
    "src/bfl/tests/model_test_ginac.cpp",
    "src/bfl/tests/ekf_test.cpp",
    "src/bfl/tests/sample_test.cpp"
]


def instrument() -> None:
    for fn in FILES_TO_INSTRUMENT:
        print("instrumenting file: {}".format(fn))
        fn = os.path.join(ROS_WS, fn)
        with open(fn, 'r') as f:
            contents = INSTRUMENTATION + f.read()
        with open(fn, 'w') as f:
            f.write(contents)
        print("instrumented file: {}".format(fn))


if __name__ == '__main__':
    instrument()
