diff --git a/libmcpf/cbss.py b/libmcpf/cbss.py
index 2d2d2b0..58d1385 100644
--- a/libmcpf/cbss.py
+++ b/libmcpf/cbss.py
@@ -6,7 +6,7 @@
 """
 
 import kbtsp as kb
-import cbss_lowlevel as sipp
+import cbss_lowlevel as sipp_ml
 import common as cm
 import copy
 import time
@@ -62,17 +62,19 @@ def __init__(self):
   def __str__(self):
     return str(self.paths)
 
-  def AddPath(self, i, lv, lt):
+  def AddPath(self, i, lv, lt,lo=[]):
     """
     lv is a list of loc id
     lt is a list of time
     """
     nlv = lv
     nlt = lt
+    nlo = lo
     # add a final infinity interval
     nlv.append(lv[-1])
     nlt.append(np.inf)
-    self.paths[i] = [nlv,nlt]
+    nlo.append(lo[-1])
+    self.paths[i] = [nlv, nlt,nlo]
     return 
 
   def DelPath(self, i):
@@ -109,6 +111,39 @@ def CheckConflict(self, i,j):
       ix = ix + 1
     return []
 
+  def CheckConflict_ml(self, i,j):
+    """
+    return the first constraint found along path i and j.
+    If no conflict, return empty list.
+    """
+    ix = 0
+    while ix < len(self.paths[i][1])-1:
+      for jx in range(len(self.paths[j][1])-1):
+        jtb = self.paths[j][1][jx+1]
+        jta = self.paths[j][1][jx]
+        itb = self.paths[i][1][ix+1]
+        ita = self.paths[i][1][ix]
+        iva = self.paths[i][0][ix]
+        ivb = self.paths[i][0][ix+1]
+        jva = self.paths[j][0][jx]
+        jvb = self.paths[j][0][jx+1]
+        ioa = self.paths[i][2][ix]
+        iob = self.paths[i][2][ix + 1]
+        joa = self.paths[j][2][jx]
+        job = self.paths[j][2][jx + 1]
+        overlaps, t_lb, t_ub = cm.ItvOverlap(ita,itb,jta,jtb)
+        if not overlaps:
+          continue
+        #occupylistoverlapstag = 0
+        common_elements = [value for value in iob if value in job]
+        if len(common_elements) != 0: # vertex conflict at ivb (=jvb)
+          return [CbsConstraint(i, common_elements[0], common_elements[0], t_lb+1, t_lb+1, j, 1), CbsConstraint(j, common_elements[0], common_elements[0], t_lb+1, t_lb+1, i, 1)] # t_ub might be inf?
+          # use min(itb,jtb) to avoid infinity
+        if (ivb == jva) and (iva == jvb): # swap location
+          return [CbsConstraint(i, iva, ivb, t_lb, t_lb+1, j, 2), CbsConstraint(j, jva, jvb, t_lb, t_lb+1, i, 2)]
+      ix = ix + 1
+    return []
+
   def ComputeCost(self):
     """
     """
@@ -140,7 +175,7 @@ def __init__(self, id0, sol=CbsSol(), cstr=CbsConstraint(-1,-1,-1,-1,-1,-1), c=0
     return
 
   def __str__(self):
-    str1 = "{id:"+str(self.id)+",c:"+str(self.cost)+",par:"+str(self.parent)
+    str1 = "{id:"+str(self.id)+",cost:"+str(self.cost)+",parent:"+str(self.parent)
     return str1+",cstr:"+str(self.cstr)+",sol:"+str(self.sol)+"}"
 
   def CheckConflict(self):
@@ -164,6 +199,27 @@ def CheckConflict(self):
     # end for k1
     return [] # no conflict
 
+  def CheckConflict_ml(self):
+    """
+    check for conflicts along paths of all pairs of robots.
+    record the first one conflict.
+    Note that one conflict should be splited to 2 constraints.
+    """
+    done_set = set()
+    for k1 in self.sol.paths:
+      for k2 in self.sol.paths:
+        if k2 in done_set or k2 == k1:
+          continue
+        # check for collision
+        res = self.sol.CheckConflict_ml(k1,k2)
+        if len(res) > 0:
+          # self.cstr = res # update member
+          return res
+      # end for k2
+      done_set.add(k1)
+    # end for k1
+    return [] # no conflict
+
   def ComputeCost(self):
     """
     compute sic cost, update member, also return cost value
@@ -303,6 +359,107 @@ def _GenNewRoot(self):
     self._UpdateEpsCost(c)
     return True
 
+
+  def _GenNewRoot_multi_ml(self):
+    """
+    called at the beginning of the search.
+    generate first High level node.
+    compute individual optimal path for each robot.
+    """
+
+    ### Generate the first HL node, a root node ###
+    nid = self.node_id_gen
+    self.nodes[nid] = self._GenCbssNode(nid)
+    self.node_id_gen = self.node_id_gen + 1
+    self.root_set.add(nid)
+    self.nodes[nid].root_id = nid
+
+    ### Init sequencing related ###
+    if not self.next_seq:
+      if (nid == 1): # init
+        tlimit = self.time_limit - (time.perf_counter() - self.tstart)
+        flag = self.kbtsp.ComputeNextBest(tlimit, self.total_num_nodes)
+        if not flag:
+          print("[ERROR] CBSS: No feasible joint sequence or time out at init!")
+          sys.exit("[ERROR]")
+        self.root_seq_dict[nid] = self.kbtsp.GetKthBestSol() # assign seq data to root node.
+      else:
+        return False # no new root to be generated.
+    else: # during search
+      self.root_seq_dict[nid] = self.next_seq
+      self.next_seq = None # next_seq has been used, make it empty.
+
+    ### plan path based on goal sequence for all agents ###
+    for ri in range(self.num_robots): # loop over agents, plan their paths
+      lv, lt,lo, stats = self.Lsearch_ml(nid, ri)
+      if len(lv) == 0: # fail to init, time out or sth.
+        return False
+      self.nodes[nid].sol.AddPath(ri,lv,lt,lo)
+
+    ### update cost and insert into OPEN ###
+    c = self.nodes[nid].ComputeCost() # update node cost and return cost value
+    self.open_list.add(c,nid)
+    self._UpdateEpsCost(c)
+    return True
+
+  def Lsearch_ml(self, nid, ri):
+    """
+    input a high level node, ri is optional.
+    """
+    if DEBUG_CBSS:
+      print("Lsearch, nid:",nid)
+    nd = self.nodes[nid]
+    tlimit = self.time_limit - (time.perf_counter() - self.tstart)
+
+    # plan from start to assigned goals and to dest as specified in goal sequence
+    gseq = self.root_seq_dict[self.nodes[nid].root_id].sol[ri]
+    ss = gseq[0]
+    kth = 1
+    t0 = 0
+    all_lv = [] # all vertex agent ri occupyed
+    all_lv.append(self.starts[ri])
+    all_lt = []
+    all_lt.append(0)
+    all_lo = []
+    all_lo.append([self.starts[ri]])
+    success = True
+    ignore_goal_cstr = True # ask robot can stay destination forever
+    nd = self.nodes[nid]
+    if ri < 0:  # to support init search.
+      ri = nd.cstr.i
+    tlimit = self.time_limit - (time.perf_counter() - self.tstart)
+    ncs, ecs = self.BacktrackCstrs(nid, ri)
+
+    res_path, sipp_stats = sipp_ml.RunSipp_ml(self.grids, gseq,t0, ignore_goal_cstr, 3.0, 0.0, tlimit, ncs, ecs) # note the t0 here!
+
+    if DEBUG_CBSS:
+      invalidtag = 0
+      for iv in range(len(res_path[2])):
+        for incs in ncs:
+          if incs[0] in res_path[2][iv] and incs[1] == res_path[1][iv]:
+            print(" no satisfy constraints",incs)
+            invalidtag = 1
+        if invalidtag==1:
+          break
+      if invalidtag==0:
+        print("satisfy constraints")
+      else:
+        print(" no satisfy constraints")
+        return [], [], False
+
+    if len(res_path[0]) == 0:  # failed
+      success = False
+    else:  # good
+      self.UpdateStats(sipp_stats)
+      #all_lv, all_lt = self.ConcatePath(all_lv, all_lt, res_path[0], res_path[1])
+      all_lv, all_lt, all_lo = self.ConcatePath_ml(all_lv, all_lt, all_lo, res_path[0], res_path[1],res_path[2])
+
+    if not success:
+      return [], [], success
+    else:
+      return all_lv, all_lt, all_lo, success
+
+
   def Lsearch(self, nid, ri):
     """
     input a high level node, ri is optional.
@@ -317,13 +474,13 @@ def Lsearch(self, nid, ri):
     ss = gseq[0]
     kth = 1
     t0 = 0
-    all_lv = []
+    all_lv = [] # all vertex agent ri occupyed
     all_lv.append(self.starts[ri])
     all_lt = []
     all_lt.append(0)
     success = True
     for kth in range(1, len(gseq)):
-      # TODO, this can be optimized, 
+      # TODO, this can be optimized,
       # no need to plan path between every pair of waypoints each time! Impl detail.
       gg = gseq[kth]
       ignore_goal_cstr = True
@@ -377,9 +534,21 @@ def ConcatePath(self, all_lv, all_lt, lv, lt):
       sys.exit("[ERROR] ConcatePath, time step mismatch !")
     return all_lv + lv[1:], all_lt + lt[1:]
 
+  def ConcatePath_ml(self, all_lv, all_lt,all_lo, lv, lt,lo):
+    """
+    remove the first node in lv,lt and then concate with all_xx.
+    """
+    if (len(all_lt) > 0) and (lt[0] != all_lt[-1]):
+      print("[ERROR] ConcatePath lv = ", lv, " lt = ", lt, " all_lv = ", all_lv, " all_lt = ", all_lt)
+      sys.exit("[ERROR] ConcatePath, time step mismatch !")
+    return all_lv + lv[1:], all_lt + lt[1:], all_lo + lo[1:]
+
   def FirstConflict(self, nd):
     return nd.CheckConflict()
 
+  def FirstConflict_ml(self, nd):
+    return nd.CheckConflict_ml()
+
   def UpdateStats(self, stats):
     """
     """
@@ -393,6 +562,7 @@ def ReconstructPath(self, nid):
     """
     """
     path_set = dict()
+    #print("num_robots",self.num_robots)
     for i in range(self.num_robots):
       lx = list()
       ly = list()
@@ -403,13 +573,17 @@ def ReconstructPath(self, nid):
         ly.append(y)
         lx.append(x)
       lt = self.nodes[nid].sol.GetPath(i)[1]
+      lo = self.nodes[nid].sol.GetPath(i)[2]
+
+
       path_set[i] = [lx,ly,lt]
+    #print(path_set)
     return path_set
 
   def _HandleRootGen(self, curr_node):
     """
     generate new root if needed
-    """  
+    """
     # print(" popped node ID = ", curr_node.id)
     if self._IfNewRoot(curr_node):
       if DEBUG_CBSS:
@@ -425,6 +599,26 @@ def _HandleRootGen(self, curr_node):
     # print("### CBSS, expand high-level node ID = ", curr_node.id)
     return curr_node
 
+  def _HandleRootGen_ml(self, curr_node):
+    """
+    generate new root if needed
+    """
+    # print(" popped node ID = ", curr_node.id)
+    if self._IfNewRoot(curr_node):
+      if DEBUG_CBSS:
+        print(" ### CBSS _GenNewRoot...")
+      self._GenNewRoot_multi_ml()
+      self.open_list.add(curr_node.cost, curr_node.id) # re-insert into OPEN for future expansion.
+      popped = self.open_list.pop() # pop_node = (f-value, high-level-node-id)
+      curr_node = self.nodes[popped[1]]
+    else:
+      # print(" self._IfNewRoot returns false...")
+      place_holder = 1
+    # end of if/while _IfNewRoot
+    # print("### CBSS, expand high-level node ID = ", curr_node.id)
+    return curr_node
+
+
   def Search(self):
     """
     = high level search
@@ -532,4 +726,112 @@ def Search(self):
       return self.ReconstructPath(reached_goal_id), output_res
     else:
       return dict(), output_res
-    
+
+
+  def Search_ml(self):
+    """
+    = high level search
+    """
+    print("CBSS search begin!")
+    self.time_limit = self.configs["time_limit"]
+    self.tstart = time.perf_counter()
+
+    good = self._GenNewRoot_multi_ml()
+    if not good:
+      output_res = [int(0), float(-1), int(0), int(0), \
+                    int(self.num_closed_low_level_states), 0, float(time.perf_counter() - self.tstart), \
+                    int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set))]
+      return dict(), output_res
+
+    tnow = time.perf_counter()
+    # print("After init, tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit)
+    if (tnow - self.tstart > self.time_limit):
+      print(" FAIL! timeout! ")
+      search_success = False
+      output_res = [int(0), float(-1), int(0), int(0), \
+                    int(self.num_closed_low_level_states), 0, float(time.perf_counter() - self.tstart), \
+                    int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set))]
+      return dict(), output_res
+
+    search_success = False
+    best_g_value = -1
+    reached_goal_id = -1
+
+    while True:
+      tnow = time.perf_counter()
+      rd = len(self.closed_set)
+      # print("tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit)
+      if (tnow - self.tstart > self.time_limit):
+        print(" FAIL! timeout! ")
+        search_success = False
+        break
+      if (self.open_list.size()) == 0:
+        print(" FAIL! openlist is empty! ")
+        search_success = False
+        break
+
+      popped = self.open_list.pop()  # pop_node = (f-value, high-level-node-id)
+      curr_node = self.nodes[popped[1]]
+      curr_node = self._HandleRootGen_ml(curr_node)  # generate new root if needed
+      tnow = time.perf_counter()
+      # print("tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit)
+
+      if (tnow - self.tstart > self.time_limit):
+        print(" FAIL! timeout! ")
+        search_success = False
+        break
+
+      self.closed_set.add(popped[1])  # only used to count numbers
+
+      if DEBUG_CBSS:
+        print("### CBSS popped node: ", curr_node)
+
+      cstrs = self.FirstConflict_ml(curr_node)
+      #print(len(cstrs))
+      if len(cstrs) == 0:  # no conflict, terminates
+        print("! CBSS succeed !")
+        search_success = True
+        best_g_value = curr_node.cost
+        reached_goal_id = curr_node.id
+        break
+
+      max_child_cost = 0
+      for cstr in cstrs:
+        if DEBUG_CBSS:
+          print("CBSS loop over cstr:", cstr)
+
+        ### generate constraint and new HL node ###
+        new_id = self.node_id_gen
+        self.node_id_gen = self.node_id_gen + 1
+        self.nodes[new_id] = copy.deepcopy(curr_node)
+        self.nodes[new_id].id = new_id
+        self.nodes[new_id].parent = curr_node.id
+        self.nodes[new_id].cstr = cstr
+        self.nodes[new_id].root_id = self.nodes[curr_node.id].root_id  # copy root id.
+        ri = cstr.i
+
+        ### replan paths for the agent, subject to new constraint ###
+        lv, lt, lo,flag = self.Lsearch_ml(new_id, ri)
+        self.num_low_level_calls = self.num_low_level_calls + 1
+        if len(lv) == 0:
+          # this branch fails, robot ri cannot find a consistent path.
+          continue
+        self.nodes[new_id].sol.DelPath(ri)
+        self.nodes[new_id].sol.AddPath(ri, lv, lt,lo)
+        nn_cost = self.nodes[new_id].ComputeCost()
+        if DEBUG_CBSS:
+          print("### CBSS add node ", self.nodes[new_id], " into OPEN,,, nn_cost = ", nn_cost)
+        self.open_list.add(nn_cost, new_id)
+        max_child_cost = np.max([nn_cost, max_child_cost])
+      # end of for cstr
+      # print(">>>>>>>>>>>>>>>>>>>> end of an iteration")
+    # end of while
+
+    output_res = [int(len(self.closed_set)), float(best_g_value), int(0), int(self.open_list.size()), \
+                  int(self.num_closed_low_level_states), int(search_success), float(time.perf_counter() - self.tstart), \
+                  int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set))]
+
+    if search_success:
+      return self.ReconstructPath(reached_goal_id), output_res
+    else:
+      return dict(), output_res
\ No newline at end of file
diff --git a/libmcpf/cbss_lowlevel.py b/libmcpf/cbss_lowlevel.py
index b9b671c..bc0c077 100644
--- a/libmcpf/cbss_lowlevel.py
+++ b/libmcpf/cbss_lowlevel.py
@@ -12,15 +12,20 @@
 import time
 import copy
 import sys
+import common as cm
+import os
+import matplotlib.pyplot as plt
 
 # this debug flag is due to some legacy, just leave it there.
-DEBUG_MOSTASTAR=False
+DEBUG_MOSTASTAR = False
+
+tasktime = 0
 
 class SippState:
   """
   Search state for SIPP, changed MO-SIPP to single-objective.
   """
-  def __init__(self, sid, loc, gval, t, tb):
+  def __init__(self, sid, loc, gval, t, tb, occupylist=[], carttag=0):
     """
     """
     self.id = sid
@@ -28,9 +33,11 @@ def __init__(self, sid, loc, gval, t, tb):
     self.g = gval # g-value
     self.t = t # arrival time step
     self.tb = tb # ending time step, mark the ending time of an interval
+    self.occupylist  = occupylist #occupy location id
+    self.carttag = carttag # equal to the number of the executed tasks
   
   def __str__(self):
-    return "{"+str(self.id)+","+str(self.loc)+",g("+str(self.g)+"),itv("+str(self.t)+","+str(self.tb)+")}" 
+    return "{id:"+str(self.id)+",loc:"+str(self.loc)+",g("+str(self.g)+"),itv(t,tb:"+str(self.t)+","+str(self.tb)+"),carttag("+str(self.carttag)+"),lenofoccupylist("+str(len(self.occupylist))+")}"
 
   def Equal(self, other):
     return cm.Equal(self.cost_vec, other.cost_vec)
@@ -125,7 +132,7 @@ def GetSuccs(self, u, v, tlb, tub):
     """
     move from u to v, with time in closed interval [tlb, tub].
     return all reachable safe intervals with earliest arrival time at node v.
-    The returned arg is a list of SippState. 
+    The returned arg is a list of SippState.
     NOTE that the ID and cost_vec in those returned states are invalid!
     """
     out = list()
@@ -136,9 +143,31 @@ def GetSuccs(self, u, v, tlb, tub):
       if not success:
         sys.exit("[ERROR] GetSuccs, look up itv fails !")
         continue
+      # sid, loc, gval, t, tb, occupylist=[], carttag=0
       out.append( SippState(-1, v, -1.0, int(itv[0]), itv[1] ) ) # Note the order, t, tb, ta
     return out
 
+  def GetSuccs_ml(self, u, v, tlb, tub,newoccupylist,oldcarttag):
+    """
+    move from u to v, with time in closed interval [tlb, tub].
+    return all reachable safe intervals with earliest arrival time at node v.
+    The returned arg is a list of SippState.
+    NOTE that the ID and cost_vec in those returned states are invalid!
+    """
+    out = list()
+    tlist = self.GetSuccSafeItv(u,v,tlb,tub)
+    # print("tlist = ", tlist)
+    for itv in tlist:
+      itv0, success = self.LookUpItv(v, itv[0])
+      if not success:
+        sys.exit("[ERROR] GetSuccs, look up itv fails !")
+        continue
+      # sid, loc, gval, t, tb, occupylist=[], carttag=0
+      out.append( SippState(-1, v, -1.0, int(itv[0]), itv[1] ,newoccupylist,oldcarttag) ) #keep the same carttag
+    return out
+
+
+
   def AddNodeCstr(self, nid, t):
     """
     """
@@ -163,7 +192,7 @@ class SIPP:
   SIPP, modified from MO-SIPP (and remove the inheritance from MOSTA*). years ago...
   no wait action in the action_set_x,_y.
   """
-  def __init__(self, grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w=1.0, eps=0.0, action_set_x = [-1,0,1,0], action_set_y = [0,-1,0,1]):
+  def __init__(self, grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w=1.0, eps=0.0, action_set_x = [-1,0,1,0], action_set_y = [0,-1,0,1], gseq =[]):
     """
     """
     self.grids = grids
@@ -195,6 +224,7 @@ def __init__(self, grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w=1.0, eps=0.0,
     self.swap_constr = dict()
       # a dict that maps a vertex id to a dict with (forbidden time, set(forbidden next-vertex)) as key-value pair.
     self.sipp_space = SippGridSpace(grids)
+    self.gseq = gseq#for multi astar
     return
 
   def GetHeuristic(self, s):
@@ -205,6 +235,23 @@ def GetHeuristic(self, s):
     cx = int(s.loc%self.nxt) # curr x
     return abs(cy-self.gy) + abs(cx - self.gx)
 
+  def GetHeuristic_ml(self, s):
+    """
+    sum of targets' Manhattan distance
+    """
+    cy = int(np.floor(s.loc/self.nxt)) # curr y
+    cx = int(s.loc%self.nxt) # curr x
+    hdistance = 0
+    #tasktime = 1 # should be updated
+    for i in self.gseq[1+s.carttag:]:
+      iy = int(np.floor(i/self.nxt))
+      ix = int(i%self.nxt)
+      hdistance = hdistance + abs(cy-iy) + abs(cx - ix) + tasktime
+      cy = iy
+      cx = ix
+    hdistance = hdistance - tasktime
+    return hdistance
+
   def GetCost(self, loc, nloc, dt=1):
     """
     minimize arrival time. cost is time.
@@ -217,6 +264,11 @@ def GetStateIdentifier(self, s):
     """
     return (s.loc, s.tb) # this is a new attempt @2021-03-31
 
+  def GetStateIdentifier_ml(self, s):
+    """
+    override the method in parent class.
+    """
+    return (s.loc, s.tb, s.carttag) #add carttag
   def GetNeighbors(self, s, tstart):
     """
     tstart is useless... can be deleted. 
@@ -254,27 +306,123 @@ def GetNeighbors(self, s, tstart):
         print(" get ngh from ", s, " to node ", nnid)
     return s_ngh, True
 
+
+  def GetNeighbors_ml(self, s, tstart):
+    """
+    tstart is useless... can be deleted.
+    input a state s, compute its neighboring states.
+    output a list of states.
+    do not engage carts
+    use sipp
+    """
+    s_ngh = list()
+    cy = int(np.floor(s.loc/self.nxt)) # current x
+    cx = int(s.loc%self.nxt) # current y
+
+    # loop over all four possible actions
+    for action_idx in range(len(self.action_set_x)):
+      nx = cx+self.action_set_x[action_idx] # next x
+      ny = cy+self.action_set_y[action_idx] # next y
+      nnid = ny*self.nxt+nx
+      if (nx >= self.nxt) or (nx < 0) or (ny >= self.nyt) or (ny < 0): # out of border of grid
+        continue
+      if (self.grids[ny,nx] > 0): # static obstacle
+        continue
+      ### this is where SIPP takes effects ###
+      snghs = self.sipp_space.GetSuccs_ml(s.loc, nnid, s.t, s.tb,[nnid],s.carttag)
+      for sngh in snghs:
+        if sngh.t > sngh.tb:
+          sys.exit("[ERROR] state " + str(sngh) + " t > tb !!!")
+        ## updat state id
+        sngh.id = self.state_gen_id
+        self.state_gen_id = self.state_gen_id + 1
+        ## update state cost vector
+        dt = 1 # @2021-04-09
+        if sngh.t > s.t:
+          dt = sngh.t - s.t
+        sngh.g = s.g+self.GetCost(s.loc, nnid, dt)
+        s_ngh.append(sngh)
+      if DEBUG_MOSTASTAR:
+        print(" get ngh from ", s, " to node ", nnid)
+
+    if s.carttag < len(self.gseq) - 2:  # not include destination
+      goalloc = self.gseq[s.carttag + 1]
+      if s.loc == goalloc and  s.tb-s.t >= tasktime:
+        sngh_t = SippState(-1, goalloc, -1.0, s.t + tasktime, s.tb, [goalloc],
+                         s.carttag + 1)
+        sngh_t.id = self.state_gen_id
+        self.state_gen_id = self.state_gen_id + 1
+        ## update state cost vector
+        dt = 0  # @2021-04-09
+        if sngh_t.t > s.t:
+          dt = sngh_t.t - s.t
+        sngh_t.g = s.g + self.GetCost(s.loc, goalloc, dt)
+        s_ngh.append(sngh_t)
+
+
+    return s_ngh, True
+
+
   def ReconstructPath(self, sid):
+    """
+    input state is the one that reached,
+    return a list of joint vertices in right order.
+    """
+    jpath = [] # in reverse order
+    tt = [] # in reverse order
+    while sid in self.backtrack_dict:
+      jpath.append(self.all_visited_s[sid].loc)
+      tt.append(self.all_visited_s[sid].t)
+      sid = self.backtrack_dict[sid]
+    jpath.append(self.all_visited_s[sid].loc)
+    tt.append(self.all_visited_s[sid].t)
+
+    # reverse output path here.
+    nodes = []
+    times = []
+    for idx in range(len(jpath)):
+      nodes.append(jpath[len(jpath)-1-idx])
+      times.append(tt[len(jpath)-1-idx])
+    return nodes, times
+
+  def ReconstructPath_ml(self, sid):
     """
     input state is the one that reached, 
     return a list of joint vertices in right order.
     """
     jpath = [] # in reverse order
+    joccupylist = [] # in reverse order
     tt = [] # in reverse order
     while sid in self.backtrack_dict:
       jpath.append(self.all_visited_s[sid].loc)
+      joccupylist.append(self.all_visited_s[sid].occupylist)
       tt.append(self.all_visited_s[sid].t)
       sid = self.backtrack_dict[sid] 
     jpath.append(self.all_visited_s[sid].loc)
+    joccupylist.append(self.all_visited_s[sid].occupylist)
     tt.append(self.all_visited_s[sid].t)
 
     # reverse output path here.
     nodes = []
+    occupylists = []
     times = []
     for idx in range(len(jpath)):
       nodes.append(jpath[len(jpath)-1-idx])
+      occupylists.append(joccupylist[len(jpath)-1-idx])
       times.append(tt[len(jpath)-1-idx])
-    return nodes, times
+
+    # unload the cart
+    mylen = len(occupylists[-1])-1
+    myfinal = occupylists[-1]
+    myfinalnode = nodes[-1]
+    myfinaltime = times[-1]
+    for i in range(mylen):
+      occupylists.append(myfinal[i+1:])
+      #print(lo[-1][i+1:])
+      nodes.append(myfinalnode)
+      times.append(myfinaltime+1)
+      myfinaltime = myfinaltime + 1
+    return nodes, times, occupylists
 
   def CheckReachGoal(self, s):
     """
@@ -293,6 +441,26 @@ def CheckReachGoal(self, s):
       return True
     return False
 
+  def CheckReachGoal_ml(self, s):
+    """
+    verify if s is a state that reaches goal and the head of robot can stay there forever !!
+    """
+    if (s.loc != self.s_f.loc):
+      return False
+    ## s.loc == s_f.loc is True now
+    if self.ignore_goal_cstr:
+      return True # no need to consider constraints at goal.
+
+
+    if s.loc not in self.node_constr:
+      return True
+    # print("s.t = ", s.t, " last cstr = ", self.node_constr[s.loc][-1])
+    if s.t > self.node_constr[s.loc][-1]:
+      # print("true")
+      return True
+    return False
+
+
   def InitSearch(self):
     """
     override parent method
@@ -311,6 +479,26 @@ def InitSearch(self):
     self.all_visited_s[self.s_o.id] = self.s_o
     self.open_list.add(self.s_o.g + self.GetHeuristic(self.s_o), self.s_o.id)
     return
+
+  def InitSearch_ml(self, occupylist):
+    """
+    override parent method
+    """
+    # start state
+    self.s_o = SippState(1, self.sy*self.nxt+self.sx, 0.0, self.t0, np.inf, occupylist)
+    if (self.s_o.loc in self.node_constr) and len(self.node_constr[self.s_o.loc]) > 0:
+      self.s_o.tb = min(self.node_constr[self.s_o.loc]) - 1 # leave the start node earlier than the min time in node constraints.
+    # goal state
+    self.s_f = SippState(2, self.gy*self.nxt+self.gx, -1.0, 0, np.inf)
+    if (self.s_f.loc in self.node_constr) and len(self.node_constr[self.s_f.loc]) > 0:
+      self.s_f.t = max(self.node_constr[self.s_f.loc]) + 1 # arrive at destination later than the max time in node constraints.
+    if DEBUG_MOSTASTAR:
+      print(" s_o = ", self.s_o, " self.node_constr = ", self.node_constr)
+      print(" s_f = ", self.s_f, " self.node_constr = ", self.node_constr)
+    self.all_visited_s[self.s_o.id] = self.s_o
+    #self.open_list.add(self.s_o.g + self.GetHeuristic(self.s_o), self.s_o.id)
+    self.open_list.add(self.s_o.g + self.GetHeuristic_ml(self.s_o), self.s_o.id)
+    return
   
   def Pruning(self, s):
     """
@@ -322,6 +510,18 @@ def Pruning(self, s):
       return True
     return False # should not be pruned
 
+
+  def Pruning_ml(self, s):
+    """
+    """
+    cfg_t = self.GetStateIdentifier_ml(s)
+    if cfg_t not in self.frontier_map:
+      return False # this je is never visited before, should not prune
+    elif self.frontier_map[cfg_t] <= s.g:
+      #print(self.frontier_map[cfg_t],s.g)
+      return True
+    return False # should not be pruned
+
   def Search(self, time_limit=10):
     if DEBUG_MOSTASTAR:
       print(" SIPP Search begin ")
@@ -382,6 +582,72 @@ def Search(self, time_limit=10):
       output_res = ( int(rd), int(search_success), float(time.perf_counter()-tstart) )
       return list(), output_res
 
+  def Search_ml(self, time_limit=10, s_occupylist=[], gseq=[]):
+    if DEBUG_MOSTASTAR:
+      print(" multi label Search begin ")
+    self.time_limit = time_limit
+    tstart = time.perf_counter()
+    self.InitSearch_ml(s_occupylist)
+    search_success = True
+    rd = 0
+    while (True):
+      tnow = time.perf_counter()
+      rd = rd + 1
+      if (tnow - tstart > self.time_limit):
+        print(" multi label Search Fail! timeout! ")
+        search_success = False
+        break
+      if self.open_list.size() == 0:
+        # print(" SIPP Fail! Open empty! ")
+        search_success = False
+        break
+      pop_node = self.open_list.pop()  # ( sum(f), sid )
+      curr_s = self.all_visited_s[pop_node[1]]
+      if DEBUG_MOSTASTAR:
+        print("##curr_s : ", curr_s, " g=", curr_s.g, " h=", self.GetHeuristic_ml(curr_s))
+        print(curr_s.occupylist)
+      if DEBUG_MOSTASTAR:
+        if rd % 1000 == 0:
+          print(" search round = ", rd, " open_list sz = ", self.open_list.size(), \
+                " time used = ", tnow - tstart)
+      if self.CheckReachGoal_ml(curr_s) and curr_s.carttag == len(
+              self.gseq) - 2:  # check if reach goal(and robot can stay there!)
+        self.reached_goal_state_id = curr_s.id
+        search_success = True
+        break
+
+      # get neighbors
+      ngh_ss, ngh_success = self.GetNeighbors_ml(curr_s, tnow)  # neighboring states
+      if not ngh_success:
+        search_success = False
+        break
+      # loop over neighbors
+      for idx in range(len(ngh_ss)):
+        ngh_s = ngh_ss[idx]
+
+        if DEBUG_MOSTASTAR:
+          print (" -- loop ngh ", ngh_s)
+        #if self.conflict_ml(curr_s, ngh_s):  # avoid the conflict
+        if (not self.Pruning_ml(ngh_s)):
+          self.frontier_map[self.GetStateIdentifier_ml(ngh_s)] = ngh_s.g
+          self.backtrack_dict[ngh_s.id] = curr_s.id
+          fval = ngh_s.g + self.weight * self.GetHeuristic_ml(ngh_s)
+          self.open_list.add(fval, ngh_s.id)
+          self.all_visited_s[ngh_s.id] = ngh_s
+        else:
+
+          if DEBUG_MOSTASTAR:
+            print(" XX dom pruned XX ", self.GetStateIdentifier_ml(ngh_s))
+    if search_success:
+      # output jpath is in reverse order, from goal to start
+      sol_path = self.ReconstructPath_ml(self.reached_goal_state_id)
+      search_success = 1
+      output_res = (int(rd), int(search_success), float(time.perf_counter() - tstart))
+      return sol_path, output_res
+    else:
+      output_res = (int(rd), int(search_success), float(time.perf_counter() - tstart))
+      return list(), output_res
+
   def AddNodeConstrBase(self, nid, t):
     """
   	This one is borrowed from MOSTA*, may be redundant...
@@ -468,6 +734,34 @@ def EnforceUnitTimePath(lv,lt):
   nlt.append(lt[-1])
   return nlv, nlt
 
+def EnforceUnitTimePath_ml(lv,lt,locp):
+  """
+  Given a path (without the final node with infinite timestamp),
+   insert missing (v,t) to ensure every pair of adjacent (v,t)
+   has time difference of one.
+  """
+  dt = 1
+  nlv = list()
+  nlt = list()
+  nlocp = list()
+  for ix in range(len(lt)-1):
+    if lt[ix]!=lt[ix+1]:
+      nlv.append(lv[ix])
+      nlt.append(lt[ix])
+      nlocp.append(locp[ix])
+      if lt[ix+1]-lt[ix] > 1.001:
+        ct = lt[ix]
+        while lt[ix+1] - ct > 1.001:
+          nlv.append(lv[ix])
+          nlt.append(ct+1)
+          nlocp.append(locp[ix])#execute the task
+          ct = ct + 1
+  # end for
+  nlv.append(lv[-1])
+  nlt.append(lt[-1])
+  nlocp.append(locp[-1])
+  return nlv, nlt, nlocp
+
 def RunSipp(grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w, eps, time_limit, node_cstrs=[], swap_cstrs=[]):
   """
   TODO[Done@2021-05-26], from implementation perspective, may need to consider igonring the node constraint at goal.
@@ -490,18 +784,57 @@ def RunSipp(grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w, eps, time_limit, nod
   else:
     return sol_path, stats
 
-def Test1():
+def RunSipp_ml(grids, gseq, t0, ignore_goal_cstr, w, eps, time_limit, node_cstrs=[], swap_cstrs=[]):
   """
+  run multi label A star
   """
-  grids = np.zeros((3,3))
-  # sol_path, stats = RunSipp(grids, 0,0, 2,0, 0, 1.0, 0.0, 10.0, [(1,1)], []) # node cstr
-  # sol_path, stats = RunSipp(grids, 0,0, 2,0, 0, 1.0, 0.0, 10.0, [], [(0,1,0)]) # edge cstr
-  # sol_path, stats = RunSipp(grids, 0,0, 2,0, 0, 1.0, 0.0, 10.0, [(1,1)], [(1,2,2)]) # both node and edge cstr
-  # sol_path, stats = RunSipp(grids, 0,0, 2,0, 0, 1.0, 0.0, 10.0, [(1,1), (2,6)], [(1,2,2)]) # node cstr at destination.
-  sol_path, stats = RunSipp(grids, 0,0, 2,0, 5, 1.0, 0.0, 10.0, [(1,1), (2,6)], [(1,2,2)]) # non-zero starting time step.
-  print(sol_path)
-  print(stats)
-  return
+
+  (nyt, nxt) = grids.shape
+  sx = gseq[0] % nxt
+  sy = int(np.floor(gseq[0] / nxt))
+  gx = gseq[-1] % nxt
+  gy = int(np.floor(gseq[-1] / nxt))
+  if DEBUG_MOSTASTAR:
+    print(gseq)
+    print("...Run multi a star... ")
+    print("sx:", sx, " sy:", sy, " gx:", gx, " gy:", gy, " ignore_goal_cstr:",ignore_goal_cstr)
+    print("node_cstrs:", node_cstrs, " swap_cstrs:", swap_cstrs)
+
+  sipp = SIPP(grids, sx,sy,gx,gy, t0, ignore_goal_cstr, w, eps,[-1,0,1,0], [0,-1,0,1], gseq)
+  for node_cstr in node_cstrs:
+    sipp.AddNodeConstr(node_cstr[0], node_cstr[1])
+  for swap_cstr in swap_cstrs:
+    sipp.AddSwapConstr(swap_cstr[0], swap_cstr[1], swap_cstr[2])
+
+  occupylist = [gseq[0]]
+  sol_path, stats = sipp.Search_ml(time_limit, occupylist, gseq) #mulit_Search_dl_ML_sipp mulit_Search_dl
+  #print(sol_path)
+
+  if len(sol_path) != 0:
+    unit_time_path = EnforceUnitTimePath_ml(sol_path[0], sol_path[1], sol_path[2])
+    return unit_time_path, stats
+  else:
+    return sol_path, stats
+
+# def Test1():
+#   """
+#   """
+#   grids = np.zeros((5, 5))
+#   gseq = [2,4,10]
+#
+#   t0 = 0
+#   ignore_goal_cstr = False
+#   w = 3.0
+#   eps = 0.0
+#   tlimit = 60
+#   ncs = [(4,2)]
+#   ecs = []
+#   print(grids)
+#
+#   sol_path, stats = RunSipp_ml(grids, gseq,  t0, ignore_goal_cstr, w, eps, tlimit, ncs,ecs)
+#   print(sol_path)
+#   print(stats)
+#   return
 
 if __name__ == "__main__":
   Test1()
diff --git a/libmcpf/cbss_mcpf.py b/libmcpf/cbss_mcpf.py
index d9aa468..d179a96 100644
--- a/libmcpf/cbss_mcpf.py
+++ b/libmcpf/cbss_mcpf.py
@@ -26,7 +26,7 @@ def RunCbssMCPF(grids, starts, targets, dests, ac_dict, configs):
   heu_weight and prune_delta are not in use. @2021-05-26
   """
   ccbs_planner = CbssMCPF(grids, starts, targets, dests, ac_dict, configs)
-  path_set, search_res = ccbs_planner.Search()
+  path_set, search_res = ccbs_planner.Search_ml()
   # print(path_set)
   # print(res_dict)
   res_dict = dict()
diff --git a/libmcpf/cbss_msmp.py b/libmcpf/cbss_msmp.py
index b3153e1..7dc6731 100644
--- a/libmcpf/cbss_msmp.py
+++ b/libmcpf/cbss_msmp.py
@@ -24,7 +24,7 @@ def RunCbssMSMP(grids, starts, goals, dests, configs):
   heu_weight and prune_delta are not in use. @2021-05-26
   """
   ccbs_planner = CbssMSMP(grids, starts, goals, dests, configs)
-  path_set, search_res = ccbs_planner.Search()
+  path_set, search_res = ccbs_planner.Search_ml()
   res_dict = dict()
   res_dict["path_set"] = path_set
   res_dict["round"] = search_res[0] # = num of high level nodes closed.