Added support for insert to REACH_PAIRWISE_POLICY
Fixed typo
optimized WAYPOINT_POLICY
Optimized DISJOINT_POLICY