diff --git a/src/main/java/net/floodlightcontroller/core/IOF13Switch.java b/src/main/java/net/floodlightcontroller/core/IOF13Switch.java
index 3c50cbc..d6fc9e4 100644
--- a/src/main/java/net/floodlightcontroller/core/IOF13Switch.java
+++ b/src/main/java/net/floodlightcontroller/core/IOF13Switch.java
@@ -142,6 +142,20 @@
      */
     public TableId getTableId(String tableType);
 
-    public int createTunnel(String tunnelId, List<String> route, NeighborSet ns);
+    /**
+     * Create a tunnel for policy routing
+     *
+     * @param tunnelId tunnel ID for the tunnel
+     * @param route list of router DPIDs for the tunnel
+     * @param ns NeighborSet to get to the first router of the tunnel
+     */
+    public void createTunnel(String tunnelId, List<String> route, NeighborSet ns);
+
+    /**
+     * Remove all groups for the tunnel
+     *
+     * @param tunnelId tunnel ID to remove
+     */
+    public void removeTunnel(String tunnelId);
 
 }
diff --git a/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java b/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
index 74f2a92..6f5d570 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
@@ -105,7 +105,7 @@
     private HashMap<String, LinkData> linksDown;
     private HashMap<String, LinkData> linksToAdd;
     private ConcurrentLinkedQueue<TopologyEvents> topologyEventQueue;
-    private HashMap<String, HashMap<String, TunnelRouteInfo>> stitchInfo;
+    //private HashMap<String, HashMap<String, TunnelRouteInfo>> stitchInfo;
     //private HashMap<String, HashMap<String, Integer>> tunnelGroupMap;
     private HashMap<String, PolicyInfo> policyTable;
     private HashMap<String, TunnelInfo> tunnelTable;
@@ -125,6 +125,8 @@
     private final int POLICY_ADD2 = 2;
     private final int POLICY_REMOVE1 = 3;
     private final int POLICY_REMOVE2 = 4;
+    private final int TUNNEL_REMOVE1 = 5;
+    private final int TUNNEL_REMOVE2 = 6;
 
 
     // ************************************
@@ -176,7 +178,7 @@
         linksDown = new HashMap<String, LinkData>();
         linksToAdd = new HashMap<String, LinkData>();
         topologyEventQueue = new ConcurrentLinkedQueue<TopologyEvents>();
-        stitchInfo = new HashMap<String, HashMap<String, TunnelRouteInfo>>();
+        //stitchInfo = new HashMap<String, HashMap<String, TunnelRouteInfo>>();
         packetService = context.getServiceImpl(IPacketService.class);
         //tunnelGroupMap = new HashMap<String, HashMap<String, Integer>>();
         restApi = context.getServiceImpl(IRestApiService.class);
@@ -1040,7 +1042,7 @@
             log.debug("Failed to get the policy rule.");
             return false;
         }
-        HashMap<String, Integer> switchGroupPair = new HashMap<String, Integer>();
+        //HashMap<String, Integer> switchGroupPair = new HashMap<String, Integer>();
         for (TunnelRouteInfo route: stitchingRule) {
 
             IOF13Switch targetSw = (IOF13Switch) floodlightProvider.getMasterSwitch(
@@ -1056,8 +1058,8 @@
                 ns.addDpid(dpid);
 
             printTunnelInfo(targetSw, tunnelId, route.getRoute(), ns);
-            int groupId = targetSw.createTunnel(tunnelId, route.getRoute(), ns);
-            switchGroupPair.put(route.srcSwDpid.toString(), groupId);
+            targetSw.createTunnel(tunnelId, route.getRoute(), ns);
+            //switchGroupPair.put(route.srcSwDpid.toString(), groupId);
 
         }
 
@@ -1299,13 +1301,44 @@
             }
         }
 
+        policyTable.remove(pid);
         log.debug("Policy {} is removed.", pid);
         return true;
     }
 
-
+    /**
+     * Remove a tunnel
+     * It removes all groups for the tunnel if the tunnel is not used for any
+     * policy.
+     *
+     * @param tunnelId tunnel ID to remove
+     */
     public boolean removeTunnel(String tunnelId) {
 
+        // Check if the tunnel is used for any policy
+        for (PolicyInfo policyInfo: policyTable.values()) {
+            if (policyInfo.tunnelId == tunnelId) {
+                log.debug("Tunnel {} is still used for the policy {}.",
+                        policyInfo.policyId, tunnelId);
+                return false;
+            }
+        }
+
+        TunnelInfo tunnelInfo = tunnelTable.get(tunnelId);
+
+        List<TunnelRouteInfo> routes = tunnelInfo.routes;
+        for (TunnelRouteInfo route: routes) {
+            IOF13Switch sw13 = (IOF13Switch) floodlightProvider.getMasterSwitch(
+                    getSwId(route.srcSwDpid));
+
+            if (sw13 != null) {
+                sw13.removeTunnel(tunnelId);
+            }
+        }
+
+        tunnelTable.remove(tunnelId);
+        log.debug("Tunnel {} was removed ", tunnelId);
+
         return true;
     }
 
@@ -1670,7 +1703,7 @@
                         dstIp, IPv4.PROTOCOL_ICMP, (short)-1, (short)-1, 10000,
                         "1");
                 testMode = POLICY_ADD2;
-                testTask.reschedule(10, TimeUnit.SECONDS);
+                testTask.reschedule(5, TimeUnit.SECONDS);
             }
             else {
                 // retry it
@@ -1695,7 +1728,7 @@
                         dstIp, IPv4.PROTOCOL_ICMP, (short)-1, (short)-1, 20000,
                         "2");
                 testMode = POLICY_REMOVE2;
-                testTask.reschedule(10, TimeUnit.SECONDS);
+                testTask.reschedule(5, TimeUnit.SECONDS);
             }
             else {
                 log.debug("Retry it");
@@ -1706,13 +1739,27 @@
             log.debug("Remove the policy 2");
             this.removePolicy("2");
             testMode = POLICY_REMOVE1;
-            testTask.reschedule(10, TimeUnit.SECONDS);
+            testTask.reschedule(5, TimeUnit.SECONDS);
         }
         else if (testMode == POLICY_REMOVE1){
             log.debug("Remove the policy 1");
             this.removePolicy("1");
-        }
 
+            testMode = TUNNEL_REMOVE1;
+            testTask.reschedule(5, TimeUnit.SECONDS);
+        }
+        else if (testMode == TUNNEL_REMOVE1) {
+            log.debug("Remove the tunnel 1");
+            this.removeTunnel("1");
+
+            testMode = TUNNEL_REMOVE2;
+            testTask.reschedule(5, TimeUnit.SECONDS);
+        }
+        else if (testMode == TUNNEL_REMOVE2) {
+            log.debug("Remove the tunnel 2");
+            this.removeTunnel("2");
+            log.debug("The end of test");
+        }
     }
 
     private void runTest1() {
diff --git a/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java b/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java
index aaaccdf..4726b8e 100644
--- a/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java
+++ b/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java
@@ -143,12 +143,10 @@
     private boolean isEdgeRouter;
     private int sid;
     private ConcurrentMap<NeighborSet, EcmpInfo> ecmpGroups;
-    private ConcurrentMap<String, List<Integer>> tunnelGroups;
+    private ConcurrentMap<String, List<Integer>> tunnelGroupIdTable;
     private ConcurrentMap<PortNumber, ArrayList<NeighborSet>> portNeighborSetMap;
     private AtomicInteger groupid;
 
-
-
     public OFSwitchImplCPqD13(OFDescStatsReply desc, boolean usePipeline13) {
         super();
         haltStateMachine = new AtomicBoolean(false);
@@ -160,7 +158,7 @@
         ecmpGroups = new ConcurrentHashMap<NeighborSet, EcmpInfo>();
         portNeighborSetMap =
                 new ConcurrentHashMap<PortNumber, ArrayList<NeighborSet>>();
-        tunnelGroups = new ConcurrentHashMap<String, List<Integer>>();
+        tunnelGroupIdTable = new ConcurrentHashMap<String, List<Integer>>();
         segmentIds = new ArrayList<Integer>();
         isEdgeRouter = false;
         groupid = new AtomicInteger(0);
@@ -1298,7 +1296,7 @@
             int gid = -1;
             GroupAction ga = (GroupAction)action;
             if (ga.getTunnelId() != null) {
-                List<Integer> groupIds = tunnelGroups.get(ga.getTunnelId());
+                List<Integer> groupIds = tunnelGroupIdTable.get(ga.getTunnelId());
                 gid = groupIds.get(groupIds.size()-1);
             }
             else {
@@ -1643,16 +1641,19 @@
         }
     }
 
-    public int createTunnel(String tunnelId, List<String> route, NeighborSet ns) {
+    @Override
+    public void createTunnel(String tunnelId, List<String> route, NeighborSet ns) {
+
+        List<Integer> groups = new ArrayList<Integer>();
 
         // create a last group of the group chaining
         int finalGroupId = groupid.incrementAndGet();
         createGroupForANeighborSet(ns, finalGroupId);
+        groups.add(Integer.valueOf(finalGroupId));
 
         int groupId = 0;
         int nextGroupId = finalGroupId;
         boolean bos = false;
-        List<Integer> groups = new ArrayList<Integer>();
 
         // process the node ID in order
         for (int i = 0; i < route.size(); i++) {
@@ -1664,11 +1665,19 @@
             createGroupForMplsLabel(groupId, nodeId, nextGroupId, bos);
             nextGroupId = groupId;
         }
-
-        tunnelGroups.putIfAbsent(tunnelId, groups);
-        return groupId;
+        tunnelGroupIdTable.putIfAbsent(tunnelId, groups);
     }
 
+    @Override
+    public void removeTunnel(String tunnelId) {
+        List<Integer> groups = tunnelGroupIdTable.get(tunnelId);
+        // we need to delete groups in reverse order
+        for (int i = groups.size() - 1; i >= 0; i--) {
+            int groupId = groups.get(i);
+            deleteGroup(groupId);
+        }
+        tunnelGroupIdTable.remove(tunnelId);
+    }
 
     // *****************************
     // Unused