__author__ = 'sware'

import math, random, time, heapq, collections, numpy, os

def calculate_distance_map(target, blocked_states = [], blocked_intersections = []):
    destinations1 = [-5,-4,-1,1,4,5]
    destinations2 = [-5,-4,-9,9,4,5]
    def destinations(index):
        dests = []
        if (index-1) % 9 < 4:
            if index % 9 != 1:
                dests.append(-1)
            if index % 9 != 4:
                dests.append(1)
            dests += [-5,-4,4,5]
        else:
            dests += [-9, 9]
            if index % 9 != 5:
                dests += [-5,4]
            if index % 9 != 0:
                dests += [-4,5]
        return dests
    dist_map = {}
    visited = set([target,35,36,39,40])
    t = 0
    fringe = [target]
    while len(fringe) != 0:
        new_fringe = []
        for state in fringe:
            dist_map[state] = t
            for dest in destinations(state):
                nstate = state + dest
                if nstate > 0 and nstate < 41 and nstate not in visited:
                    visited.add(nstate)
                    new_fringe.append(nstate)
        fringe = new_fringe
        t += 1
    return dist_map

def calculate_distance_map_variable(target, width, blocked_states = [], blocked_intersections = []):
    #destinations1 = [-((width/2)+1),-(width/2),-1,1,(width/2),(width/2)+1]
    #destinations2 = [-((width/2)+1),-(width/2),-width,width,(width/2),(width/2)+1]
    def destinations(index):
        dests = []
        if (index-1) % width < width/2:
            if index % width != 1:
                dests.append(-1)
            if index % width != width/2:
                dests.append(1)
            dests += [-((width/2)+1),-(width/2),(width/2),((width/2)+1)]
        else:
            dests += [-width, width]
            if index % width != ((width/2)+1):
                dests += [-((width/2)+1),(width/2)]
            if index % width != 0:
                dests += [-(width/2),((width/2)+1)]
        return dests
    dist_map = {}
    visited = set([target])
    t = 0
    fringe = [target]
    while len(fringe) != 0:
        new_fringe = []
        for state in fringe:
            dist_map[state] = t
            for dest in destinations(state):
                nstate = state + dest
                if nstate > 0 and nstate <= (width*(width/2)+(width/2)) and nstate not in visited:
                    visited.add(nstate)
                    new_fringe.append(nstate)
        fringe = new_fringe
        t += 1
    return dist_map

def swap(arr):
    i1, i2 = random.sample(xrange(len(arr)), 2)
    #print 'arrbef={}'.format(arr)
    temp = arr[i1]
    arr[i1] = arr[i2]
    arr[i2] = temp
    #print 'arraft={}'.format(arr)
    #print 'swaped={},{}'.format(i1, i2)

def equalize(arr, most_epuck, least_epuck):
    imost = [i for i in range(len(arr)) if arr[i] == most_epuck]
    index = random.choice(imost)
    arr[index] = least_epuck

def insert(arr):
    val = arr.pop(random.randint(0,len(arr)-1))
    arr.insert(random.randint(0,len(arr)-1), val)

def reverse(arr):
    ii1, ii2 = random.sample(xrange(len(arr)), 2)
    i1,i2 = min(ii1, ii2), max(ii1, ii2)
    for i in range((i2-i1)/2):
        x,y = i1+i, i2-i
        temp = arr[x]
        arr[x] = arr[y]
        arr[y] = temp

def assign(arr, num_epucks = 3):
    i, stk = random.randint(0, len(arr)-1), random.randint(0,num_epucks-1)
    arr[i] = stk

def neighbour(arr, ass, num_epucks, mostleast = None):
    def multi(arr):
        swap(arr)
        insert(arr)
        reverse(arr)
        if ass:
            assign(arr, num_epucks)
    if ass:
        poss = [multi, swap, insert, reverse, lambda x: assign(x, num_epucks)]
        #poss = [swap, assign]
    else:
        poss = [multi, swap, insert, reverse]
        #poss = [swap]
    if mostleast != None:
        poss.append(lambda x: equalize(x, mostleast[0], mostleast[1]))
    random.choice(poss)(arr)
    #choose = random.randint(0, 3+ass)
    #if choose == 0:
    #    swap(arr)
    #    insert(arr)
    #    reverse(arr)
    #    if ass:
    #        assign(arr)
    #elif choose == 1:
    #    swap(arr)
    #elif choose == 2:
    #    insert(arr)
    #elif choose == 3:
    #    reverse(arr)
    #else:
    #    assign(arr)

def calc_cost_of_assign(moveorders, solutionorders, epuckassigned, epuck_start, dist_maps):
    positions = [pos for _,pos in epuck_start]
    dist_moved = [0 for _ in range(len(epuck_start))]
    for i in range(len(moveorders)):
        epuck = epuckassigned[i]
        moveorder = moveorders[solutionorders[i]]
        #print epuck
        #print moveorder
        #print positions[epuck]
        dist_moved[epuck] += dist_maps[moveorder[0]][positions[epuck]]
        positions[epuck] = moveorder[1]
        dist_moved[epuck] += dist_maps[moveorder[0]][positions[epuck]]
    #for i in range(len(epuck_start)):
    #    dist_moved[i] += dist_maps[epuck_start[i][1]][positions[i]]
    leastindex = 0
    mostindex = 0
    for i in range(1, len(dist_moved)):
        if dist_moved[i] < dist_moved[leastindex]:
            leastindex = i
        if dist_moved[i] > dist_moved[mostindex]:
            mostindex = i
    return dist_moved[mostindex], mostindex, leastindex

def simulate_annealing(moveorders, startpositions, assigned_task, func, width = 9):
    temperature = 30/math.log1p(0.8)
    stime = time.clock()
    stoptemp = .0001
    maxIT = 100000.0
    staytemp = 1
    maxIT = maxIT/float(staytemp)
    solutionorders = range(len(moveorders))
    random.shuffle(solutionorders)
    stkassign = [random.randint(0,len(startpositions)-1) for i in range(len(moveorders))]
    alpha = (stoptemp/temperature)**(1/maxIT)
    dist_maps = {}
    for _, pos in startpositions:
        #print pos
        if pos not in dist_maps:
            dist_maps[pos] = calculate_distance_map_variable(pos, width)
            #print dist_maps[pos]
    for pos, _ in moveorders:
        if pos not in dist_maps:
            dist_maps[pos] = calculate_distance_map_variable(pos, width)
    def currassigngen(arr1, arr2):
        #print moveorders
        #print arr1
        #print arr2
        return [(moveorders[arr1[i]], arr2[i]) for i in range(len(moveorders))]
    #for vindex, _ in startpositions:
    #    assigned_task[vindex] = []
    #for i in range(len(solutionorders)):
    #    vindex = startpositions[stkassign[i]][0]
    #    assigned_task[vindex].extend(moveorders[solutionorders[i]])
    #sposes = [(pos, assigned_task[vindex]) for vindex, pos in startpositions]
    currsolutionweight, mindex, lindex = calc_cost_of_assign(moveorders, solutionorders, stkassign, startpositions, dist_maps)
    #currsolutionweight = func(startpositions, assigned_task)
    #currsolutionweight = grid_complex(sposes, 9)
    bestsolweight = currsolutionweight
    bestsolorder, bestepkassign = list(solutionorders), list(stkassign)
    tick = 1
    while (temperature >= stoptemp):
        for j in range(staytemp):
            nsol = list(solutionorders)
            nstk = list(stkassign)
            neighbour(nsol, 0, len(startpositions))
            neighbour(nstk, 1, len(startpositions), (mindex, lindex))
            #assigned_task = {}
            #for vindex, _ in startpositions:
            #    assigned_task[vindex] = []
            #for i in range(len(solutionorders)):
            #    vindex = startpositions[nstk[i]][0]
            #    assigned_task[vindex].extend(moveorders[nsol[i]])
            nsolwe, mindex, lindex = calc_cost_of_assign(moveorders, nsol, nstk, startpositions, dist_maps)
            #nsolwe = func(startpositions, assigned_task)
            #sposes = [(pos, assigned_task[vindex]) for vindex, pos in startpositions]
            #nsolwe = grid_complex(sposes, 9)
            fdelta = nsolwe - currsolutionweight
            if fdelta < 0 or random.random() < math.exp(-fdelta/temperature):
                currsolutionweight, solutionorders, stkassign = nsolwe, nsol, nstk
            if currsolutionweight < bestsolweight:
                bestsolweight = currsolutionweight
                bestsolorder, bestepkassign = list(solutionorders), list(stkassign)
        dur = time.clock() - stime
        if  dur > tick:
            tick += 1
            print 'time={} temp={} best={} curr={} newsol={}'.format(dur, temperature, bestsolweight, currsolutionweight, nsolwe)
        temperature = alpha * temperature
    for vindex, _ in startpositions:
        assigned_task[vindex] = []
    for i in range(len(solutionorders)):
        vindex = startpositions[bestepkassign[i]][0]
        assigned_task[vindex].extend(moveorders[solutionorders[i]])
    #for vindex, pos in startpositions:
    #    assigned_task[vindex].append(pos)
    sposes = [(pos, assigned_task[vindex]) for vindex, pos in startpositions]
    print 'vehicle-routing={}'.format(bestsolweight)
    #bestsolweight, moves = grid_complex(sposes, width)
    bestsolweight, moves = grid_complex_greedy([pos for _, pos in startpositions], moveorders, width)
    #bestsolweight = func(startpositions, assigned_task)
    print bestsolweight
    #print moves
    #if moves != None:
    #    print assigned_task
    #    for i, movelist in enumerate(moves):
    #        first_pos = movelist[0][0]
    #        for vindex, pos in startpositions:
    #            if pos == first_pos:
    #                break
    #        assert pos == first_pos
    #        moves[i] = map(lambda (x, y): 'P{}_{}_{}'.format(vindex, str(x).zfill(2), str(y).zfill(2)) if x != y else None,
    #                       movelist)
    #    i = 0
    #    string = []
    #    while len(moves) != 0:
    #        newmoves = []
    #        for movelist in moves:
    #            if len(movelist) > i:
    #                newmoves.append(movelist)
    #                if (movelist[i] != None):
    #                    string.append(movelist[i])
    #        moves = newmoves
    #        i += 1
    #    #print string
    #    handle = file('epuckevents.txt', 'w')
    #    handle.write('{}\n{}\n'.format(bestsolweight, ','.join(string)))
    #    handle.close()

def calc_heur(height, width, source, blocked_transitions,blocked_state):
    if hasattr(source[0],'__iter__'):
        to_visit = [(0, place) for place in source]
    else:
        to_visit = [(0, source)]
    heur = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    while len(to_visit) != 0:
        t, (sx,sy) = heapq.heappop(to_visit)
        if (sx, sy) in heur:
            continue
        heur[(sx, sy)] = t
        for dx, dy in adjacency_matrix:
            nx, ny = sx + dx, sy + dy
            if nx < 1 or nx > width or ny < 1 or ny > height:
                continue
            if (nx, ny) in blocked_state or \
                            ((sx, sy), (nx, ny)) in blocked_transitions \
                    or ((nx, ny), (sx, sy)) in blocked_transitions:
                continue
            heapq.heappush(to_visit, (t+1, (nx,ny)))
    return heur

def a_star_search_increment(height, width, source, target, blocked_transitions,blocked_state,heur, movements, orig_blocked):
    movement_heur = {}
    def calc_movement_heur(step, blocked, blocked_one_step):
        if step >= len(movements):
            return 0 if len(blocked_one_step) == 0 else 1
        res = movement_heur.get((step, blocked, blocked_one_step), None)
        if res != None:
            return res
        fublocked, fublocked_one_step = set(blocked), set(blocked_one_step)
        t_needed = 0
        t_needed_map = {}
        for i in range(step, len(movements)):
            fblocked, fblocked_one_step = frozenset(fublocked), frozenset(fublocked_one_step)
            if (i, fblocked, fblocked_one_step) in movement_heur:
                t_needed += movement_heur[(i, fblocked, fblocked_one_step)]
                fublocked_one_step.clear()
                break
            t_needed_map[(i, fblocked, fblocked_one_step)] = t_needed
            fr, to = movements[step]
            #assert fr in fublocked
            #assert to not in fublocked
            if to in fublocked_one_step or fr in fublocked_one_step:
                fublocked_one_step.clear()
                t_needed += 1
            fublocked.remove(fr)
            fublocked.add(to)
            fublocked_one_step.add(fr)
        if len(fublocked_one_step) != 0:
            t_needed += 1
        for (key, val) in t_needed_map.iteritems():
            movement_heur[key] = t_needed - val
        return movement_heur[(step, blocked, blocked_one_step)]
    to_visit = [(max(heur[source], calc_movement_heur(0,orig_blocked, frozenset())), 0, source, None,
                 orig_blocked, frozenset(), 0, [])]
    pred = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    while len(to_visit) != 0:
        calced_heur, t, (sx,sy), one_step_curr, blocked_move, one_step_blocked_move, step, inserts = heapq.heappop(to_visit)
        #if ((sx, sy), one_step_curr, step, one_step_blocked_move) in pred:
        #    continue
        #pred[(sx, sy), one_step_curr, step, one_step_blocked_move] = inserts
        if ((sx, sy), step) in pred:
            continue
        pred[(sx, sy), step] = inserts
        if (sx, sy) == target:
            break
        for dx, dy in adjacency_matrix:
            nx, ny = sx + dx, sy + dy
            if nx < 1 or nx > width or ny < 1 or ny > height:
                continue
            if (nx, ny) in blocked_state or (nx, ny) in blocked_move or \
                    ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                continue
            if (nx, ny) in one_step_blocked_move or one_step_curr != None:
                n_one_step_blocked_move = frozenset()
                mod = 1
            else:
                n_one_step_blocked_move = one_step_blocked_move
                mod = 0
            h = t + mod + max(heur[(nx,ny)], calc_movement_heur(step, blocked_move, n_one_step_blocked_move))
            n_inserts = list(inserts)
            n_inserts.append((step, ((sx, sy), (nx, ny))))
            heapq.heappush(to_visit, (h, t+mod, (nx,ny), (sx,sy), blocked_move, n_one_step_blocked_move, step, n_inserts))
        if (step < len(movements)):
            fr, to = movements[step]
            if to != (sx, sy):
                n_blocked = set(blocked_move)
                n_blocked.remove(fr)
                n_blocked.add(to)
                n_one_step_blocked_move = set(blocked_move)
                if fr in one_step_blocked_move or to == one_step_curr or to in one_step_blocked_move:
                    mod = 1
                    n_one_step_blocked_move.clear()
                    n_one_step = None
                else:
                    mod = 0
                    n_one_step = one_step_curr
                n_one_step_blocked_move.add(fr)
                n_blocked = frozenset(n_blocked)
                n_one_step_blocked_move = frozenset(n_one_step_blocked_move)
                h = t + mod + max(heur[(sx,sy)], calc_movement_heur(step+1, n_blocked, n_one_step_blocked_move))
                heapq.heappush(to_visit, (h, t+mod, (sx,sy), n_one_step, n_blocked, n_one_step_blocked_move, step+1, inserts))
    if (sx, sy) != target:
        return None
    arrival = calced_heur
    for i_step, pair in reversed(inserts):
        movements.insert(i_step, pair)
    return (arrival, movements)

def a_star_search_increment_opt_improved_intersections(height, width, v_index, source, target,blocked_state, heur, movements, orig_blocked):
    print 'this one', len(movements)
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    moved = set(orig_blocked)
    #moved_at = collections.defaultdict(list)
    moved_at = [[[] for _ in range(height+1)] for _ in range(width+1)]
    state_count = 0
    cache = {}
    last_move = {}
    for i, (index, fr, int, to) in enumerate(movements):
        last_move[index] = i
        moved.discard(fr)
    cblocked.difference_update(moved)
    for i, (index, fr, int, to) in enumerate(movements):
        #print cblocked
        #print movements[i]
        cblocked.remove(fr)
        if last_move[index] != i:
            cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[to[0]][to[1]].append(i)
        moved_at[int[0]][int[1]].append(i)
        moved_at[fr[0]][fr[1]].append(i)
    def need_to_step(state, step, last_step):
        res = cache.get((state, step, last_step))
        if res != None:
            return res
        arr = moved_at[state[0]][state[1]]
        index = numpy.searchsorted(arr, step) - 1
        res = index >= 0 and arr[index] >= last_step
        if len(cache) >= 10000:
            cache.clear()
        cache[(state, step, last_step)] = res
        #if index < 0:
            #return False
        return res
    #for key in moved_at:
    #    moved_at[key] = numpy.array(moved_at[key])
    for x in range(width+1):
        for y in range(height+1):
            moved_at[x][y] = numpy.array(moved_at[x][y])
    t = 1
    emptyset = frozenset()
    #traceheur = [0]
    #fringe = set()
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    if fr in fringe or to in fringe:
    #        t += 1
    #        fringe.clear()
    #    traceheur.append(t)
    #    fringe.update([fr, to])
    #traceheur.reverse()
    traceheur = [None for i in range(len(movements) + 1)]
    stepindex = [None for i in range(len(movements) + 1)]
    traceheur[len(movements)] = 0
    stepindex[len(movements)] = len(movements)
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    fringe = set([fr, to])
    #    time_taken = 1
    #    stepindex[i] = len(movements)
    #    for j in range(i+1, len(movements)):
    #        _, fr, to = movements[j]
    #        if fr in fringe or to in fringe:
    #            stepindex[i] = j
    #            time_taken += traceheur[j]
    #            break
    #        fringe.update([fr, to])
    #    traceheur[i] = time_taken
    for i in reversed(range(len(movements))):
        _, fr, int, to = movements[i]
        arr1 = moved_at[fr[0]][fr[1]]
        arr2 = moved_at[to[0]][to[1]]
        arr3 = moved_at[int[0]][int[1]]
        i1 = numpy.searchsorted(arr1, i+1)
        i2 = numpy.searchsorted(arr2, i+1)
        i3 = numpy.searchsorted(arr3, i+1)
        i1 = len(movements) if i1 == len(arr1) else arr1[i1]
        i2 = len(movements) if i2 == len(arr2) else arr2[i2]
        i3 = len(movements) if i3 == len(arr3) else arr3[i3]
        #print i, stepindex[i+1], i1, i2
        stepindex[i] = min(stepindex[i+1], i1, i2, i3)
        traceheur[i] = traceheur[stepindex[i]] + 1
    #print stepindex
    #print traceheur
    to_visit = collections.defaultdict(list)
    k = max(traceheur[0], heur[source]/2)
    to_visit[k] = [(0, source, emptyset, 0, 0, None)]
    #to_visit = [(max(traceheur[0], heur[source]), state_count, 0, source, emptyset, 0, 0, None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    num_to_visit = 1
    while len(to_visit) != 0:
        if len(to_visit[k]) == 0:
            del to_visit[k]
            k += 1
            continue
        calced_heur = k
        t, (sx,sy), one_step_curr, step, stepped, inserts = to_visit[k].pop()
        num_to_visit -= 1
        #calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts = heapq.heappop(to_visit)
        #print calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts
        #assert(calced_heur >= prev_heur)
        prev_heur = calced_heur
        #t = -t
        blocked_move = blocked_move_at_step[step]
        if ((sx, sy), step) in pred:
            continue
        pred.add(((sx, sy), step))
        if (sx, sy) == target and step == len(movements):
            break
        for dx, dy in adjacency_matrix:
            for dx2, dy2 in adjacency_matrix:
                nx, ny = sx + dx, sy + dy
                nx2, ny2 = nx + dx2, ny + dy2
                if ((nx2, ny2), step) in pred:
                    continue
                if (sx, sy) == (nx2, ny2):
                    continue
                if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                    continue
                if (nx, ny) in blocked_state or (nx, ny) in blocked_move:
                    continue
                if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move:
                    continue
                make_step = need_to_step((nx, ny), step, stepped) or \
                            need_to_step((nx2, ny2), step, stepped) or \
                            not one_step_curr.isdisjoint([(sx, sy), (nx, ny), (nx2, ny2)])
                #print one_step_curr, (sx, sy), (nx, ny)
                new_one_step = frozenset([(sx, sy), (nx, ny), (nx2, ny2)])
                new_stepped = step if make_step else stepped
                index = stepindex[new_stepped]
                for (x,y) in new_one_step:
                    arr = moved_at[x][y]
                    id = numpy.searchsorted(arr, step)#, side='right')
                    if id == len(arr):
                        continue
                    index = min(index, arr[id])
                #for i in range(step, len(movements)):
                #    _, fr, to = movements[i]
                #    if not new_one_step.isdisjoint([fr, to]) or\
                #        need_to_step(fr, i, new_stepped) or\
                #        need_to_step(to, i, new_stepped):
                #        index = i
                #        break
                h = t + max(traceheur[index]+1, (heur[(nx2, ny2)]/2) + 1)
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    h += 1
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t+1), (nx,ny), \
                    #                          new_one_step,
                    #                          step, step,
                    #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                    to_visit[h].append(((t+1), (nx2,ny2), \
                                  new_one_step,
                                  step, step,
                                  ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts)))
                else:
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    assert(h >= calced_heur)
                    #print 'not make'
                    #heapq.heappush(to_visit, (h, -state_count, (t), (nx,ny), \
                    #                          new_one_step,
                    #                          step, stepped,
                    #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                    to_visit[h].append(((t), (nx2,ny2), \
                                   new_one_step,
                                   step, stepped,
                                   ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts)))
        if (step < len(movements)):
            _, fr, int, to = movements[step]
            if to != (sx, sy):
                if ((sx, sy), step + 1) in pred:
                    continue
                make_step = need_to_step(fr, step, stepped) or \
                            need_to_step(to, step, stepped) or \
                            need_to_step(int, step, stepped) or \
                            not one_step_curr.isdisjoint([to, int, fr])
                new_one_step = emptyset if make_step else one_step_curr
                #if make_step:
                #    new_one_step = emptyset
                #else:
                #    new_one_step = one_step_curr
                mod_heur = (new_one_step != emptyset)
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    #print heur[(sx, sy)], mod_heur
                    h = t + max(traceheur[step], (heur[(sx, sy)]/2) + mod_heur)
                    h += 1
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t+1), (sx,sy), \
                    #                          new_one_step, step+1, step,
                    #                          inserts))
                    to_visit[h].append(((t+1), (sx,sy), \
                                   new_one_step, step+1, step,
                                   inserts))
                else:
                    index = len(movements)
                    index = stepindex[stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step+1)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step+1, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]):# or\
                    #        #need_to_step(fr, i, stepped) or\
                    #        #need_to_step(to, i, stepped):
                    #        index = i
                    #        break
                    #index = min(index, stepindex[stepped])
                    #print (sx, sy), heur[(sx, sy)], mod_heur, 'no makestep'
                    h = t + max(traceheur[index] + 1, (heur[(sx, sy)]/2) + mod_heur)
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t), (sx,sy),
                    #                          new_one_step, step+1, stepped,
                    #                          inserts))
                    to_visit[h].append(((t), (sx,sy),
                                   new_one_step, step+1, stepped,
                                   inserts))
    if (sx, sy) != target or step != len(movements):
        print orig_blocked
        print source
        print target
        return None
    arrival = calced_heur
    new_movements = []
    prev_index = len(movements)
    while inserts != None:
        tup, i_step, inserts = inserts
        for i in reversed(xrange(i_step, prev_index)):
            new_movements.append(movements[i])
        new_movements.append(tup)
        prev_index = i_step
        #movements.insert(i_step, tup)
    for i in reversed(xrange(0, prev_index)):
        new_movements.append(movements[i])
    return (arrival, list(reversed(new_movements)))

def a_star_search_increment_opt_improved_intersections_mod(height, width, v_index, source, target,blocked_state, heur, movements, orig_blocked,
                                                           stopped_vehicles = set(), def_keep = [], skip = True):
    print 'this one', len(movements)
    print 'stopped', stopped_vehicles
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    not_moved = set(orig_blocked)
    moved_into = set()
    #moved_at = collections.defaultdict(list)
    moved_at = [[[] for _ in range(height+1)] for _ in range(width+1)]
    state_count = 0
    cache = {}
    last_move = {}
    for i, (index, fr, int, to) in enumerate(movements):
        last_move[index] = i
    #    moved_into.add(to)
    #    if fr not in moved_into:
    #        not_moved.discard(fr)
    #cblocked.difference_update(not_moved)
    #cblocked.update(def_keep)
    assert source not in cblocked
    for i, (index, fr, int, to) in enumerate(movements):
        #print cblocked
        #print movements[i]
        if fr not in cblocked:
            print i
        cblocked.remove(fr)
        if last_move[index] != i or index in stopped_vehicles:
            cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[to[0]][to[1]].append(i)
        moved_at[int[0]][int[1]].append(i)
        moved_at[fr[0]][fr[1]].append(i)
    def need_to_step(state, step, last_step):
        res = cache.get((state, step, last_step))
        if res != None:
            return res
        arr = moved_at[state[0]][state[1]]
        index = numpy.searchsorted(arr, step) - 1
        res = index >= 0 and arr[index] >= last_step
        if len(cache) >= 10000:
            cache.clear()
        cache[(state, step, last_step)] = res
        #if index < 0:
            #return False
        return res
    #for key in moved_at:
    #    moved_at[key] = numpy.array(moved_at[key])
    for x in range(width+1):
        for y in range(height+1):
            moved_at[x][y] = numpy.array(moved_at[x][y])
    t = 1
    emptyset = frozenset()
    #traceheur = [0]
    #fringe = set()
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    if fr in fringe or to in fringe:
    #        t += 1
    #        fringe.clear()
    #    traceheur.append(t)
    #    fringe.update([fr, to])
    #traceheur.reverse()
    traceheur = [None for i in range(len(movements) + 1)]
    stepindex = [None for i in range(len(movements) + 1)]
    traceheur[len(movements)] = 0
    stepindex[len(movements)] = len(movements)
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    fringe = set([fr, to])
    #    time_taken = 1
    #    stepindex[i] = len(movements)
    #    for j in range(i+1, len(movements)):
    #        _, fr, to = movements[j]
    #        if fr in fringe or to in fringe:
    #            stepindex[i] = j
    #            time_taken += traceheur[j]
    #            break
    #        fringe.update([fr, to])
    #    traceheur[i] = time_taken
    skipto = [None for i in range(len(movements) + 1)]
    skipto[len(movements)] = len(movements)
    last_in_valid = len(movements)
    for i in reversed(range(len(movements))):
        _, fr, int, to = movements[i]
        if not blocked_state.issuperset([fr, int, to]):
            last_in_valid = i
        arr1 = moved_at[fr[0]][fr[1]]
        arr2 = moved_at[to[0]][to[1]]
        arr3 = moved_at[int[0]][int[1]]
        i1 = numpy.searchsorted(arr1, i+1)
        i2 = numpy.searchsorted(arr2, i+1)
        i3 = numpy.searchsorted(arr3, i+1)
        i1 = len(movements) if i1 == len(arr1) else arr1[i1]
        i2 = len(movements) if i2 == len(arr2) else arr2[i2]
        i3 = len(movements) if i3 == len(arr3) else arr3[i3]
        #print i, stepindex[i+1], i1, i2
        stepindex[i] = min(stepindex[i+1], i1, i2, i3)
        traceheur[i] = traceheur[stepindex[i]] + 1
        skipto[i] = last_in_valid if skip else i
    #print skipto
    #print stepindex
    #print traceheur
    to_visit = collections.defaultdict(list)
    ch = heur[source] if target != None else 0
    th = traceheur[0]*2
    diff = th - ch
    k = ch + max(diff/2, 0)
    #k = max(traceheur[0], heur[source]/2)
    if target != None:
        to_visit[k] = [(0, source, emptyset, 0, 0, None, target == None)]
    else:
        to_visit[k] = [(0, 0, source, emptyset, 0, 0, None, target == None)]
    #to_visit = [(max(traceheur[0], heur[source]), state_count, 0, source, emptyset, 0, 0, None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    num_to_visit = 1
    while len(to_visit) != 0:
        if len(to_visit[k]) == 0:
            del to_visit[k]
            k += 1
            continue
        calced_heur = k
        if target != None:
            t, (sx,sy), one_step_curr, step, stepped, inserts, finished = to_visit[k].pop()
        else:
            moves, t, (sx,sy), one_step_curr, step, stepped, inserts, finished = heapq.heappop(to_visit[k])
        t = -t
        num_to_visit -= 1
        #calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts = heapq.heappop(to_visit)
        #print calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts
        #assert(calced_heur >= prev_heur)
        prev_heur = calced_heur
        #t = -t
        blocked_move = blocked_move_at_step[step]
        if ((sx, sy), step) in pred:
            continue
        pred.add(((sx, sy), step))
        if finished and step == len(movements):
            break
        if not finished or target == None:
            for dx, dy in adjacency_matrix:
                for dx2, dy2 in adjacency_matrix:
                    nx, ny = sx + dx, sy + dy
                    nx2, ny2 = nx + dx2, ny + dy2
                    if ((nx2, ny2), step) in pred:
                        continue
                    if (sx, sy) == (nx2, ny2):
                        continue
                    #if (sx, sy) in blocked_move:
                    #    print "wtf", (sx,sy), source
                    #    print step
                    #    print skipto
                    #    print (sx, sy) in blocked_state
                    #    i = step
                    #    while not (sx,sy) in movements[i]:
                    #        print movements[i], skipto[i]
                    #        i -= 1
                    #    print movements[i], skipto[i]
                    #    io = di
                    #    continue
                    if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                        continue
                    if (nx, ny) in blocked_state or (nx, ny) in blocked_move:
                        continue
                    if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move:
                        continue
                    make_step = need_to_step((nx, ny), step, stepped) or \
                                need_to_step((nx2, ny2), step, stepped) or \
                                not one_step_curr.isdisjoint([(sx, sy), (nx, ny), (nx2, ny2)])
                    #print one_step_curr, (sx, sy), (nx, ny)
                    new_one_step = frozenset([(sx, sy), (nx, ny), (nx2, ny2)])
                    #new_stepped = step if make_step else stepped
                    new_stepped = step if make_step else stepped
                    new_step = min(skipto[step], stepindex[new_stepped])
                    #new_step = step
                    assert new_step >= step
                    index = stepindex[new_stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]) or\
                    #        need_to_step(fr, i, new_stepped) or\
                    #        need_to_step(to, i, new_stepped):
                    #        index = i
                    #        break
                    ch = heur[(nx2, ny2)] + 2
                    th = (traceheur[index]+1)*2
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[index]+1, (heur[(nx2, ny2)]/2) + 1)
                    t_mod = 2 if target != None and (not finished) else 1
                    num_to_visit += 1
                    if num_to_visit % 10000 == 0:
                        print 'to visit', num_to_visit
                    if make_step:
                        h += 2
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #heapq.heappush(to_visit, (h, -state_count, (t+1), (nx,ny), \
                        #                          new_one_step,
                        #                          step, step,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        if target != None:
                            to_visit[h].append((-(t+t_mod), (nx2,ny2), \
                                      new_one_step,
                                      new_step, step,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) == target))
                        else:
                            heapq.heappush(to_visit[k], (moves +1, -(t+t_mod), (nx2,ny2), \
                                      new_one_step,
                                      new_step, step,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) == target))
                    else:
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #print 'not make'
                        #heapq.heappush(to_visit, (h, -state_count, (t), (nx,ny), \
                        #                          new_one_step,
                        #                          step, stepped,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        if target != None:
                            to_visit[h].append((-(t), (nx2,ny2), \
                                            new_one_step,
                                            new_step, stepped,
                                            ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) == target))
                        else:
                            heapq.heappush(to_visit[k], (moves +1, -(t), (nx2,ny2), \
                                      new_one_step,
                                      new_step, stepped,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) == target))
        if (step < len(movements)):
            _, fr, int, to = movements[step]
            if to != (sx, sy) or (finished and target != None):
                if ((sx, sy), step + 1) in pred:
                    continue
                make_step = need_to_step(fr, step, stepped) or \
                            need_to_step(to, step, stepped) or \
                            need_to_step(int, step, stepped) or \
                            not one_step_curr.isdisjoint([to, int, fr])
                new_one_step = emptyset if make_step else one_step_curr
                #if make_step:
                #    new_one_step = emptyset
                #else:
                #    new_one_step = one_step_curr
                mod_heur = (new_one_step != emptyset)
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    #print heur[(sx, sy)], mod_heur
                    ch = heur[(sx, sy)] + mod_heur * 2 if not finished else mod_heur * 2
                    th = (traceheur[step])*2
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[step], (heur[(sx, sy)]/2) + mod_heur)
                    #h += 1
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    t_mod = 2 if target != None and (not finished or one_step_curr != emptyset) else 1
                    h += t_mod
                    new_step = min(skipto[step+1], stepindex[step])
                    #new_step = step+1
                    assert new_step >= step
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t+1), (sx,sy), \
                    #                          new_one_step, step+1, step,
                    #                          inserts))
                    if target != None:
                        to_visit[h].append((-(t+t_mod), (sx,sy), \
                                   new_one_step, new_step, step,
                                   inserts, finished))
                    else:
                        heapq.heappush(to_visit[k], (moves, -(t+t_mod), (sx,sy), \
                                   new_one_step, new_step, step,
                                   inserts, finished))
                else:
                    index = stepindex[stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step+1)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step+1, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]):# or\
                    #        #need_to_step(fr, i, stepped) or\
                    #        #need_to_step(to, i, stepped):
                    #        index = i
                    #        break
                    #index = min(index, stepindex[stepped])
                    #print (sx, sy), heur[(sx, sy)], mod_heur, 'no makestep'
                    ch = heur[(sx, sy)] + mod_heur * 2 if not finished else mod_heur * 2
                    th = (traceheur[index])*2 + 2
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[index] + 1, (heur[(sx, sy)]/2) + mod_heur)
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t), (sx,sy),
                    #                          new_one_step, step+1, stepped,
                    #                          inserts))
                    new_step = min(skipto[step+1], stepindex[stepped])
                    #new_step = step + 1
                    assert new_step >= step
                    if target != None:
                        to_visit[h].append((-(t), (sx,sy),
                                   new_one_step, new_step, stepped,
                                   inserts, finished))
                    else:
                        heapq.heappush(to_visit[k], (moves, -(t), (sx,sy),
                                   new_one_step, new_step, stepped,
                                   inserts, finished))
    if not finished or step != len(movements):
        print orig_blocked
        print source
        print target
        return None
    arrival = calced_heur
    new_movements = []
    prev_index = len(movements)
    while inserts != None:
        tup, i_step, inserts = inserts
        for i in reversed(xrange(i_step, prev_index)):
            new_movements.append(movements[i])
        new_movements.append(tup)
        prev_index = i_step
        #movements.insert(i_step, tup)
    for i in reversed(xrange(0, prev_index)):
        new_movements.append(movements[i])
    return (arrival, list(reversed(new_movements)))

def a_star_search_increment_opt_improved_intersections_mod_stop_pick_window(height, width, v_index, source, target,
                                                                            window_size,
                                                                            blocked_state, heur, movements, orig_blocked,
                                                                            stopped_vehicles = set(), skip = True):
    ox, oy = source
    #print movements
    minexectime = movements[-1][4]*2 + 2 if movements else 0
    MODIFIER = 2
    print 'this one', len(movements)
    #print 'stopped', stopped_vehicles
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    moved_at = [[[] for _ in range(height+1)] for _ in range(width+1)]
    cache = {}
    last_move = {}
    for i, (index, fr, int, to, _) in enumerate(movements):
        last_move[index] = i
    print source, target
    assert source not in cblocked
    for i, (index, fr, int, to, _) in enumerate(movements):
        #if fr not in cblocked:
        #    print i
        #    print movements[i:]
        #print 'vid', fr, to, index, v_index, i, last_move[index]
        if fr[0] <= ox + window_size and fr[0] >= ox - window_size and\
            fr[1] <= oy + window_size and fr[1] >= ox + window_size:
            cblocked.remove(fr)
        if last_move[index] != i or index in stopped_vehicles:
            cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[to[0]][to[1]].append(i)
        moved_at[int[0]][int[1]].append(i)
        moved_at[fr[0]][fr[1]].append(i)
    def need_to_step(state, step, last_step):
        res = cache.get((state, step, last_step))
        if res != None:
            return res
        arr = moved_at[state[0]][state[1]]
        index = numpy.searchsorted(arr, step) - 1
        res = index >= 0 and arr[index] >= last_step
        if len(cache) >= 10000:
            cache.clear()
        cache[(state, step, last_step)] = res
        return res
    for x in range(width+1):
        for y in range(height+1):
            moved_at[x][y] = numpy.array(moved_at[x][y])
    emptyset = frozenset()
    traceheur = [None for i in range(len(movements) + 1)]
    stepindex = [None for i in range(len(movements) + 1)]
    traceheur[len(movements)] = 0
    stepindex[len(movements)] = len(movements)
    skipto = [None for i in range(len(movements) + 1)]
    skipto[len(movements)] = len(movements)
    last_in_valid = len(movements)
    for i in reversed(range(len(movements))):
        _, fr, int, to, _ = movements[i]
        if not blocked_state.issuperset([fr, int, to]):
            last_in_valid = i
        arr1 = moved_at[fr[0]][fr[1]]
        arr2 = moved_at[to[0]][to[1]]
        arr3 = moved_at[int[0]][int[1]]
        i1 = numpy.searchsorted(arr1, i+1)
        i2 = numpy.searchsorted(arr2, i+1)
        i3 = numpy.searchsorted(arr3, i+1)
        i1 = len(movements) if i1 == len(arr1) else arr1[i1]
        i2 = len(movements) if i2 == len(arr2) else arr2[i2]
        i3 = len(movements) if i3 == len(arr3) else arr3[i3]
        stepindex[i] = min(stepindex[i+1], i1, i2, i3)
        traceheur[i] = traceheur[stepindex[i]] + 1
        skipto[i] = last_in_valid if skip else i
    to_visit = collections.defaultdict(list)
    ch = heur[source] if target != None else 0
    th = max(traceheur[0]*2, minexectime)
    diff = th - ch
    k = ch + max(diff/MODIFIER, 0)
    if target != None:
        to_visit[k] = [(0, source, emptyset, 0, 0, None, target == None)]
    else:
        to_visit[k] = [(0, 0, source, emptyset, 0, 0, None, target == None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    num_to_visit = 1
    while len(to_visit) != 0:
        if len(to_visit[k]) == 0:
            del to_visit[k]
            k += 1
            continue
        calced_heur = k
        if target != None:
            t, (sx,sy), one_step_curr, step, stepped, inserts, finished = to_visit[k].pop()
        else:
            moves, t, (sx,sy), one_step_curr, step, stepped, inserts, finished = heapq.heappop(to_visit[k])
        #if inserts != None:
            #print (sx, sy), inserts[0]
        assert inserts == None or inserts[0][-1] == (sx,sy)
        t = -t
        num_to_visit -= 1
        blocked_move = blocked_move_at_step[step]
        if ((sx, sy), step, finished) in pred:
            continue
        pred.add(((sx, sy), step, finished))
        if not(sx <= ox + window_size-2 and sx >= ox - window_size+2  and\
            sy <= oy + window_size-2 and sy >= oy - window_size+2):
            break
        if finished and step == len(movements):
            break
        if not finished or target == None:
            for dx, dy in adjacency_matrix:
                for dx2, dy2 in adjacency_matrix:
                    nx, ny = sx + dx, sy + dy
                    nx2, ny2 = nx + dx2, ny + dy2
                    if ((nx2, ny2), step) in pred:
                        continue
                    if (sx, sy) == (nx2, ny2):
                        continue
                    if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                        continue
                    if (nx, ny) in blocked_state or (nx, ny) in blocked_move:
                        continue
                    if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move:
                        continue
                    make_step = need_to_step((nx, ny), step, stepped) or \
                                need_to_step((nx2, ny2), step, stepped) or \
                                not one_step_curr.isdisjoint([(sx, sy), (nx, ny), (nx2, ny2)])
                    new_one_step = frozenset([(sx, sy), (nx, ny), (nx2, ny2)])
                    new_stepped = step if make_step else stepped
                    new_step = min(skipto[step], stepindex[new_stepped])
                    assert new_step >= step
                    index = stepindex[new_stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    ch = heur[(nx2, ny2)] + 4 if target != None else 2
                    th = max((traceheur[index]+1)*2, minexectime)
                    diff = th - ch
                    if target == None:
                        ch = ch/MODIFIER
                    h = t + ch + max(diff/MODIFIER, 0)
                    t_mod = 2 if target != None and (not finished) else 2/MODIFIER
                    num_to_visit += 1
                    if num_to_visit % 10000 == 0:
                        print 'to visit', num_to_visit
                    if make_step:
                        h += 2
                        #assert(h >= calced_heur)
                        if target != None:
                            to_visit[h].append((-(t+t_mod), (nx2,ny2), \
                                      new_one_step,
                                      new_step, step,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished))
                        else:
                            heapq.heappush(to_visit[k], (moves +1, -(t+t_mod), (nx2,ny2), \
                                      new_one_step,
                                      new_step, step,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished))
                    else:
                        #assert(h >= calced_heur)
                        if target != None:
                            to_visit[h].append((-(t), (nx2,ny2), \
                                            new_one_step,
                                            new_step, stepped,
                                            ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished))
                        else:
                            heapq.heappush(to_visit[k], (moves +1, -(t), (nx2,ny2), \
                                      new_one_step,
                                      new_step, stepped,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished))
            if (sx, sy) == target:
                make_step = need_to_step((sx, sy), step, stepped) or \
                            not one_step_curr.isdisjoint([(sx, sy)])
                new_one_step = frozenset([(sx, sy)])
                new_stepped = step if make_step else stepped
                new_step = min(skipto[step], stepindex[new_stepped])
                assert new_step >= step
                index = stepindex[new_stepped]
                for (x,y) in new_one_step:
                    arr = moved_at[x][y]
                    id = numpy.searchsorted(arr, step)#, side='right')
                    if id == len(arr):
                        continue
                    index = min(index, arr[id])
                ch = 2
                th = max((traceheur[index]+1)*2, minexectime)
                diff = th - ch
                if target == None:
                    ch = ch/MODIFIER
                h = t + ch + max(diff/MODIFIER, 0)
                t_mod = 2 if target != None and (not finished) else 2/MODIFIER
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    h += 2
                    #assert(h >= calced_heur)
                    if target != None:
                        to_visit[h].append((-(t+t_mod), (sx,sy), \
                                  new_one_step,
                                  new_step, step,
                                  ((v_index, (sx, sy), (sx, sy), (sx, sy)), step, inserts), t+t_mod+2))
                    else:
                        heapq.heappush(to_visit[k], (moves +1, -(t+t_mod), (sx,sy), \
                                  new_one_step,
                                  new_step, step,
                                  ((v_index, (sx, sy), (sx, sy), (sx, sy)), step, inserts), t+t_mod+2))
                else:
                    #assert(h >= calced_heur)
                    if target != None:
                        to_visit[h].append((-(t), (sx,sy), \
                                        new_one_step,
                                        new_step, stepped,
                                        ((v_index, (sx, sy), (sx, sy), (sx, sy)), step, inserts), t+2))
                    else:
                        heapq.heappush(to_visit[k], (moves +1, -(t), (sx,sy), \
                                  new_one_step,
                                  new_step, stepped,
                                  ((v_index, (sx, sy), (sx, sy), (sx, sy)), step, inserts), t+2))
        if (step < len(movements)):
            _, fr, int, to, earliest = movements[step]
            if to != (sx, sy) or (finished and target != None):
                if ((sx, sy), step + 1) in pred:
                    continue
                if finished:
                    fintime = finished if target != None else 0
                    earlydiff = earliest - fintime
                    timediff = t - fintime
                    earlydiff /= 2
                    earlydiff -= timediff
                else:
                    earlydiff = earliest - t
                make_step = need_to_step(fr, step, stepped) or \
                            need_to_step(to, step, stepped) or \
                            need_to_step(int, step, stepped) or \
                            earlydiff > 0 or \
                            not one_step_curr.isdisjoint([to, int, fr])
                new_one_step = emptyset if make_step else one_step_curr
                mod_heur = (new_one_step != emptyset)
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    if earlydiff > 0:
                        mod_heur = earlydiff/2
                    ch = heur[(sx, sy)] + mod_heur * 2 + 2 if not finished else mod_heur * 2
                    th = max((traceheur[step])*2, minexectime)
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/MODIFIER, 0)
                    if earlydiff > 0:
                        t_mod = earlydiff if target != None and (not finished or one_step_curr != emptyset) else (earlydiff)/MODIFIER
                    else:
                        t_mod = 2 if target != None and (not finished or one_step_curr != emptyset) else 2/MODIFIER
                    h += t_mod
                    new_step = min(skipto[step+1], stepindex[step])
                    assert new_step >= step
                    #assert(h >= calced_heur)
                    if target != None:
                        to_visit[h].append((-(t+t_mod), (sx,sy), \
                                   new_one_step, new_step, step,
                                   inserts, finished))
                    else:
                        heapq.heappush(to_visit[k], (moves, -(t+t_mod), (sx,sy), \
                                   new_one_step, new_step, step,
                                   inserts, finished))
                else:
                    index = stepindex[stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step+1)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    ch = heur[(sx, sy)] + mod_heur * 2 + 2 if not finished else mod_heur * 2
                    th = max((traceheur[index])*2 + 2, minexectime)
                    diff = th - ch
                    if target == None:
                        ch = ch/MODIFIER
                    h = t + ch + max(diff/MODIFIER, 0)
                    #assert(h >= calced_heur)
                    new_step = min(skipto[step+1], stepindex[stepped])
                    assert new_step >= step
                    if target != None:
                        to_visit[h].append((-(t), (sx,sy),
                                   new_one_step, new_step, stepped,
                                   inserts, finished))
                    else:
                        heapq.heappush(to_visit[k], (moves, -(t), (sx,sy),
                                   new_one_step, new_step, stepped,
                                   inserts, finished))
    if (not finished or step != len(movements)) and\
        sx <= ox + window_size -2 and sx >= ox - window_size+2 and\
        sy <= oy + window_size -2 and sy >= oy - window_size+2:
        print sx, sy, source, window_size, target
        print orig_blocked
        print source
        print target
        return None
    arrival = calced_heur
    new_movements = []
    prev_index = len(movements)
    #if not(sx <= ox + window_size and sx >= ox - window_size and\
    #    sy <= oy + window_size and sy >= oy - window_size):
    #    assert (inserts[0] != inserts[2][0])
    #    _, _, inserts = inserts
    while inserts != None:
        if inserts[2] != None:
            if inserts[0] == inserts[2][0]:
                print source, target, inserts[0]
            assert (inserts[0] != inserts[2][0])
        tup, i_step, inserts = inserts
        for i in reversed(xrange(i_step, prev_index)):
            new_movements.append(movements[i][:-1])
        new_movements.append(tup)
        prev_index = i_step
        #movements.insert(i_step, tup)
    for i in reversed(xrange(0, prev_index)):
        new_movements.append(movements[i][:-1])
    #print new_movements
    #os.system('pause')
    return (finished != 0, list(reversed(new_movements)))

def a_star_search_increment_opt_improved_intersections_mod_stop_pick_window_outer(height, width, v_index, source, target,
                                                                            window_size,
                                                                            blocked_state, heur, movements, orig_blocked,
                                                                            stopped_vehicles = set(), skip = True):
    print 'outer', orig_blocked
    window_size = 10
    x, y = source
    osource = source
    minx, miny = x - window_size, y - window_size
    maxx, maxy = x + window_size, y + window_size
    count = 0
    def inbetween(tup):
        x, y = tup
        return minx <= x and x <= maxx and miny <= y and y <= maxy
    while True:
        count += 1
        lastwithindex = 0
        lastindex = {}
        for i in xrange(len(movements)):
            if v_index == movements[i][0]:
                #print movements[i], v_index
                lastwithindex = i+1
            lastindex[movements[i][0]] = i
        if lastwithindex > 0:
            #print source, movements[lastwithindex-1], v_index
            source = movements[lastwithindex-1][-1]
            x, y = source
            minx, miny = x - window_size, y - window_size
            maxx, maxy = x + window_size, y + window_size
        newblocked = set(orig_blocked)
        beforemoves, aftmoves = movements[:lastwithindex], movements[lastwithindex:]
        for tup in aftmoves:
            assert tup[0] != v_index
        if len(beforemoves) != 0:
            assert beforemoves[-1][0] == v_index
        windowmoves = []
        occupied = set()
        earliest = 0
        for i, (vid, s, _, t) in enumerate(beforemoves):
            if vid == v_index:
                #print 'befmov', beforemoves[i], lastindex[vid] == i
                continue
            else:
                #print 'bef', beforemoves[i], lastindex[vid] == i
                newblocked.remove(s)
                if i != lastindex[vid]:
                    newblocked.add(t)
        for pid, s, i, t in aftmoves:
            nt = [s,i,t]
            if not occupied.isdisjoint(nt):
                occupied.clear()
                earliest += 1
            occupied.update(nt)
            if inbetween(s) or inbetween(t):
                windowmoves.append((pid, s, i, t, earliest))
        #print beforemoves
        #print movements
        nmoves = \
            a_star_search_increment_opt_improved_intersections_mod_stop_pick_window(height, width,
            v_index, source, target, window_size, blocked_state, heur, windowmoves, newblocked,
            stopped_vehicles, skip)
        if nmoves == None:
            print 'count', count
            ai = idoi
            return None
        windmoves = nmoves[1]
        wmoveinds = 0
        while wmoveinds < len(windmoves):
            if windmoves[wmoveinds][0] != v_index:
                break
            beforemoves.append(windmoves[wmoveinds])
            wmoveinds +=1
        for tup in aftmoves:
            beforemoves.append(tup)
            if wmoveinds >= len(windmoves):
                continue
            if tup == windmoves[wmoveinds]:
                wmoveinds += 1
                while wmoveinds < len(windmoves):
                    if windmoves[wmoveinds][0] != v_index:
                        break
                    beforemoves.append(windmoves[wmoveinds])
                    wmoveinds +=1
        while wmoveinds < len(windmoves):
            if windmoves[wmoveinds][0] != v_index:
                break
            beforemoves.append(windmoves[wmoveinds])
            wmoveinds +=1
        movements = beforemoves
        newblocked = set(orig_blocked)
        newblocked.add(osource)
        for i in xrange(len(movements)):
            if v_index == movements[i][0]:
                lastwithindex = i+1
            lastindex[movements[i][0]] = i
        for i, (vid, s, _, t) in enumerate(movements):
            print movements[i], lastindex[vid] == i
            newblocked.remove(s)
            if lastindex[vid] != i:
                assert t not in newblocked
                newblocked.add(t)
        #for tup in movements:
        #    if tup[0] == v_index:
        #        print tup
        makespan, movements = compress_trace(movements)
        if nmoves[0]:
            print 'count', count
            return 0, movements

def a_star_search_increment_opt_improved_intersections_mod_multtarget(height, width, v_index, source, target,blocked_state, heur, movements, orig_blocked,
                                                           stopped_vehicles = set(), def_keep = [], skip = True):
    print 'this one', len(movements)
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    not_moved = set(orig_blocked)
    moved_into = set()
    #moved_at = collections.defaultdict(list)
    moved_at = [[[] for _ in range(height+1)] for _ in range(width+1)]
    state_count = 0
    cache = {}
    last_move = {}
    for i, (index, fr, int, to) in enumerate(movements):
        last_move[index] = i
        moved_into.add(to)
        if fr not in moved_into:
            not_moved.discard(fr)
    cblocked.difference_update(not_moved)
    cblocked.update(def_keep)
    print def_keep, source
    assert source not in cblocked
    for i, (index, fr, int, to) in enumerate(movements):
        #print cblocked
        #print movements[i]
        if fr not in cblocked:
            print i
        cblocked.remove(fr)
        if last_move[index] != i or index in stopped_vehicles:
            cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[to[0]][to[1]].append(i)
        moved_at[int[0]][int[1]].append(i)
        moved_at[fr[0]][fr[1]].append(i)
    def need_to_step(state, step, last_step):
        res = cache.get((state, step, last_step))
        if res != None:
            return res
        arr = moved_at[state[0]][state[1]]
        index = numpy.searchsorted(arr, step) - 1
        res = index >= 0 and arr[index] >= last_step
        if len(cache) >= 10000:
            cache.clear()
        cache[(state, step, last_step)] = res
        #if index < 0:
            #return False
        return res
    #for key in moved_at:
    #    moved_at[key] = numpy.array(moved_at[key])
    for x in range(width+1):
        for y in range(height+1):
            moved_at[x][y] = numpy.array(moved_at[x][y])
    t = 1
    emptyset = frozenset()
    #traceheur = [0]
    #fringe = set()
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    if fr in fringe or to in fringe:
    #        t += 1
    #        fringe.clear()
    #    traceheur.append(t)
    #    fringe.update([fr, to])
    #traceheur.reverse()
    traceheur = [None for i in range(len(movements) + 1)]
    stepindex = [None for i in range(len(movements) + 1)]
    traceheur[len(movements)] = 0
    stepindex[len(movements)] = len(movements)
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    fringe = set([fr, to])
    #    time_taken = 1
    #    stepindex[i] = len(movements)
    #    for j in range(i+1, len(movements)):
    #        _, fr, to = movements[j]
    #        if fr in fringe or to in fringe:
    #            stepindex[i] = j
    #            time_taken += traceheur[j]
    #            break
    #        fringe.update([fr, to])
    #    traceheur[i] = time_taken
    skipto = [None for i in range(len(movements) + 1)]
    skipto[len(movements)] = len(movements)
    last_in_valid = len(movements)
    for i in reversed(range(len(movements))):
        _, fr, int, to = movements[i]
        if not blocked_state.issuperset([fr, int, to]):
            last_in_valid = i
        arr1 = moved_at[fr[0]][fr[1]]
        arr2 = moved_at[to[0]][to[1]]
        arr3 = moved_at[int[0]][int[1]]
        i1 = numpy.searchsorted(arr1, i+1)
        i2 = numpy.searchsorted(arr2, i+1)
        i3 = numpy.searchsorted(arr3, i+1)
        i1 = len(movements) if i1 == len(arr1) else arr1[i1]
        i2 = len(movements) if i2 == len(arr2) else arr2[i2]
        i3 = len(movements) if i3 == len(arr3) else arr3[i3]
        #print i, stepindex[i+1], i1, i2
        stepindex[i] = min(stepindex[i+1], i1, i2, i3)
        traceheur[i] = traceheur[stepindex[i]] + 1
        skipto[i] = last_in_valid if skip else i
    #print skipto
    #print stepindex
    #print traceheur
    to_visit = collections.defaultdict(list)
    ch = heur[source] if target != None else 0
    th = traceheur[0]*2
    diff = th - ch
    k = ch + max(diff/2, 0)
    #k = max(traceheur[0], heur[source]/2)
    if target != None:
        to_visit[k] = [(0, source, emptyset, 0, 0, None, target == None)]
    else:
        to_visit[k] = [(0, 0, source, emptyset, 0, 0, None, target == None)]
    #to_visit = [(max(traceheur[0], heur[source]), state_count, 0, source, emptyset, 0, 0, None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    num_to_visit = 1
    while len(to_visit) != 0:
        if len(to_visit[k]) == 0:
            del to_visit[k]
            k += 1
            continue
        calced_heur = k
        if target != None:
            t, (sx,sy), one_step_curr, step, stepped, inserts, finished = to_visit[k].pop()
        else:
            moves, t, (sx,sy), one_step_curr, step, stepped, inserts, finished = heapq.heappop(to_visit[k])
        t = -t
        num_to_visit -= 1
        #calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts = heapq.heappop(to_visit)
        #print calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts
        #assert(calced_heur >= prev_heur)
        prev_heur = calced_heur
        #t = -t
        blocked_move = blocked_move_at_step[step]
        if ((sx, sy), step) in pred:
            continue
        pred.add(((sx, sy), step))
        if finished and step == len(movements):
            break
        if not finished or target == None:
            for dx, dy in adjacency_matrix:
                for dx2, dy2 in adjacency_matrix:
                    nx, ny = sx + dx, sy + dy
                    nx2, ny2 = nx + dx2, ny + dy2
                    if ((nx2, ny2), step) in pred:
                        continue
                    if (sx, sy) == (nx2, ny2):
                        continue
                    #if (sx, sy) in blocked_move:
                    #    print "wtf", (sx,sy), source
                    #    print step
                    #    print skipto
                    #    print (sx, sy) in blocked_state
                    #    i = step
                    #    while not (sx,sy) in movements[i]:
                    #        print movements[i], skipto[i]
                    #        i -= 1
                    #    print movements[i], skipto[i]
                    #    io = di
                    #    continue
                    if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                        continue
                    if (nx, ny) in blocked_state or (nx, ny) in blocked_move:
                        continue
                    if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move:
                        continue
                    make_step = need_to_step((nx, ny), step, stepped) or \
                                need_to_step((nx2, ny2), step, stepped) or \
                                not one_step_curr.isdisjoint([(sx, sy), (nx, ny), (nx2, ny2)])
                    #print one_step_curr, (sx, sy), (nx, ny)
                    new_one_step = frozenset([(sx, sy), (nx, ny), (nx2, ny2)])
                    #new_stepped = step if make_step else stepped
                    new_stepped = step if make_step else stepped
                    new_step = min(skipto[step], stepindex[new_stepped])
                    #new_step = step
                    assert new_step >= step
                    index = stepindex[new_stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]) or\
                    #        need_to_step(fr, i, new_stepped) or\
                    #        need_to_step(to, i, new_stepped):
                    #        index = i
                    #        break
                    ch = heur[(nx2, ny2)] + 2
                    th = (traceheur[index]+1)*2
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[index]+1, (heur[(nx2, ny2)]/2) + 1)
                    t_mod = 2 if target != None and (not finished) else 1
                    num_to_visit += 1
                    if num_to_visit % 10000 == 0:
                        print 'to visit', num_to_visit
                    if make_step:
                        h += 2
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #heapq.heappush(to_visit, (h, -state_count, (t+1), (nx,ny), \
                        #                          new_one_step,
                        #                          step, step,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        if target != None:
                            to_visit[h].append((-(t+t_mod), (nx2,ny2), \
                                      new_one_step,
                                      new_step, step,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) in target))
                        else:
                            heapq.heappush(to_visit[k], (moves +1, -(t+t_mod), (nx2,ny2), \
                                      new_one_step,
                                      new_step, step,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) in target))
                    else:
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #print 'not make'
                        #heapq.heappush(to_visit, (h, -state_count, (t), (nx,ny), \
                        #                          new_one_step,
                        #                          step, stepped,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        if target != None:
                            to_visit[h].append((-(t), (nx2,ny2), \
                                            new_one_step,
                                            new_step, stepped,
                                            ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) == target))
                        else:
                            heapq.heappush(to_visit[k], (moves +1, -(t), (nx2,ny2), \
                                      new_one_step,
                                      new_step, stepped,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) == target))
        if (step < len(movements)):
            _, fr, int, to = movements[step]
            if to != (sx, sy) or (finished and target != None):
                if ((sx, sy), step + 1) in pred:
                    continue
                make_step = need_to_step(fr, step, stepped) or \
                            need_to_step(to, step, stepped) or \
                            need_to_step(int, step, stepped) or \
                            not one_step_curr.isdisjoint([to, int, fr])
                new_one_step = emptyset if make_step else one_step_curr
                #if make_step:
                #    new_one_step = emptyset
                #else:
                #    new_one_step = one_step_curr
                mod_heur = (new_one_step != emptyset)
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    #print heur[(sx, sy)], mod_heur
                    ch = heur[(sx, sy)] + mod_heur * 2 if not finished else mod_heur * 2
                    th = (traceheur[step])*2
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[step], (heur[(sx, sy)]/2) + mod_heur)
                    #h += 1
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    t_mod = 2 if target != None and (not finished or one_step_curr != emptyset) else 1
                    h += t_mod
                    new_step = min(skipto[step+1], stepindex[step])
                    #new_step = step+1
                    assert new_step >= step
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t+1), (sx,sy), \
                    #                          new_one_step, step+1, step,
                    #                          inserts))
                    if target != None:
                        to_visit[h].append((-(t+t_mod), (sx,sy), \
                                   new_one_step, new_step, step,
                                   inserts, finished))
                    else:
                        heapq.heappush(to_visit[k], (moves, -(t+t_mod), (sx,sy), \
                                   new_one_step, new_step, step,
                                   inserts, finished))
                else:
                    index = stepindex[stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step+1)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step+1, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]):# or\
                    #        #need_to_step(fr, i, stepped) or\
                    #        #need_to_step(to, i, stepped):
                    #        index = i
                    #        break
                    #index = min(index, stepindex[stepped])
                    #print (sx, sy), heur[(sx, sy)], mod_heur, 'no makestep'
                    ch = heur[(sx, sy)] + mod_heur * 2 if not finished else mod_heur * 2
                    th = (traceheur[index])*2 + 2
                    diff = th - ch
                    if target == None:
                        ch = ch/2
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[index] + 1, (heur[(sx, sy)]/2) + mod_heur)
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t), (sx,sy),
                    #                          new_one_step, step+1, stepped,
                    #                          inserts))
                    new_step = min(skipto[step+1], stepindex[stepped])
                    #new_step = step + 1
                    assert new_step >= step
                    if target != None:
                        to_visit[h].append((-(t), (sx,sy),
                                   new_one_step, new_step, stepped,
                                   inserts, finished))
                    else:
                        heapq.heappush(to_visit[k], (moves, -(t), (sx,sy),
                                   new_one_step, new_step, stepped,
                                   inserts, finished))
    if not finished or step != len(movements):
        print orig_blocked
        print source
        print target
        return None
    arrival = calced_heur
    new_movements = []
    prev_index = len(movements)
    while inserts != None:
        tup, i_step, inserts = inserts
        for i in reversed(xrange(i_step, prev_index)):
            new_movements.append(movements[i])
        new_movements.append(tup)
        prev_index = i_step
        #movements.insert(i_step, tup)
    for i in reversed(xrange(0, prev_index)):
        new_movements.append(movements[i])
    return (arrival, list(reversed(new_movements)), (sx, sy))

def a_star_search_increment_opt_improved_intersections_mod_multtarget2(height, width, v_index, source, targets,blocked_state, heur, movements, orig_blocked,
                                                                      stopped_vehicles = set(), def_keep = [], skip = True):
    print 'this one', len(movements)
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    not_moved = set(orig_blocked)
    moved_into = set()
    #moved_at = collections.defaultdict(list)
    moved_at = [[[] for _ in range(height+1)] for _ in range(width+1)]
    state_count = 0
    cache = {}
    last_move = {}
    for i, (index, fr, int, to) in enumerate(movements):
        last_move[index] = i
        moved_into.add(to)
        if fr not in moved_into:
            not_moved.discard(fr)
    cblocked.difference_update(not_moved)
    cblocked.update(def_keep)
    print def_keep, source
    assert source not in cblocked
    for i, (index, fr, int, to) in enumerate(movements):
        #print cblocked
        #print movements[i]
        if fr not in cblocked:
            print i
        cblocked.remove(fr)
        if last_move[index] != i or index in stopped_vehicles:
            cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[to[0]][to[1]].append(i)
        moved_at[int[0]][int[1]].append(i)
        moved_at[fr[0]][fr[1]].append(i)
    def need_to_step(state, step, last_step):
        res = cache.get((state, step, last_step))
        if res != None:
            return res
        arr = moved_at[state[0]][state[1]]
        index = numpy.searchsorted(arr, step) - 1
        res = index >= 0 and arr[index] >= last_step
        if len(cache) >= 10000:
            cache.clear()
        cache[(state, step, last_step)] = res
        #if index < 0:
            #return False
        return res
    #for key in moved_at:
    #    moved_at[key] = numpy.array(moved_at[key])
    for x in range(width+1):
        for y in range(height+1):
            moved_at[x][y] = numpy.array(moved_at[x][y])
    t = 1
    emptyset = frozenset()
    #traceheur = [0]
    #fringe = set()
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    if fr in fringe or to in fringe:
    #        t += 1
    #        fringe.clear()
    #    traceheur.append(t)
    #    fringe.update([fr, to])
    #traceheur.reverse()
    traceheur = [None for i in range(len(movements) + 1)]
    stepindex = [None for i in range(len(movements) + 1)]
    traceheur[len(movements)] = 0
    stepindex[len(movements)] = len(movements)
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    fringe = set([fr, to])
    #    time_taken = 1
    #    stepindex[i] = len(movements)
    #    for j in range(i+1, len(movements)):
    #        _, fr, to = movements[j]
    #        if fr in fringe or to in fringe:
    #            stepindex[i] = j
    #            time_taken += traceheur[j]
    #            break
    #        fringe.update([fr, to])
    #    traceheur[i] = time_taken
    skipto = [None for i in range(len(movements) + 1)]
    skipto[len(movements)] = len(movements)
    last_in_valid = len(movements)
    for i in reversed(range(len(movements))):
        _, fr, int, to = movements[i]
        if not blocked_state.issuperset([fr, int, to]):
            last_in_valid = i
        arr1 = moved_at[fr[0]][fr[1]]
        arr2 = moved_at[to[0]][to[1]]
        arr3 = moved_at[int[0]][int[1]]
        i1 = numpy.searchsorted(arr1, i+1)
        i2 = numpy.searchsorted(arr2, i+1)
        i3 = numpy.searchsorted(arr3, i+1)
        i1 = len(movements) if i1 == len(arr1) else arr1[i1]
        i2 = len(movements) if i2 == len(arr2) else arr2[i2]
        i3 = len(movements) if i3 == len(arr3) else arr3[i3]
        #print i, stepindex[i+1], i1, i2
        stepindex[i] = min(stepindex[i+1], i1, i2, i3)
        traceheur[i] = traceheur[stepindex[i]] + 1
        skipto[i] = last_in_valid if skip else i
    #print skipto
    #print stepindex
    #print traceheur
    to_visit = collections.defaultdict(list)
    ch = heur[source]
    th = traceheur[0]*2
    diff = th - ch
    k = ch + max(diff/2, 0)
    #k = max(traceheur[0], heur[source]/2)
    to_visit[k] = [(0, source, emptyset, 0, 0, None, source in targets)]
    #to_visit = [(max(traceheur[0], heur[source]), state_count, 0, source, emptyset, 0, 0, None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    num_to_visit = 1
    while len(to_visit) != 0:
        if len(to_visit[k]) == 0:
            del to_visit[k]
            k += 1
            continue
        calced_heur = k
        t, (sx,sy), one_step_curr, step, stepped, inserts, finished = to_visit[k].pop()
        num_to_visit -= 1
        #calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts = heapq.heappop(to_visit)
        #print calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts
        #assert(calced_heur >= prev_heur)
        prev_heur = calced_heur
        #t = -t
        blocked_move = blocked_move_at_step[step]
        if ((sx, sy), step) in pred:
            continue
        pred.add(((sx, sy), step))
        if finished and step == len(movements):
            break
        if not finished:
            for dx, dy in adjacency_matrix:
                for dx2, dy2 in adjacency_matrix:
                    nx, ny = sx + dx, sy + dy
                    nx2, ny2 = nx + dx2, ny + dy2
                    if ((nx2, ny2), step) in pred:
                        continue
                    if (sx, sy) == (nx2, ny2):
                        continue
                    #if (sx, sy) in blocked_move:
                    #    print "wtf", (sx,sy), source
                    #    print step
                    #    print skipto
                    #    print (sx, sy) in blocked_state
                    #    i = step
                    #    while not (sx,sy) in movements[i]:
                    #        print movements[i], skipto[i]
                    #        i -= 1
                    #    print movements[i], skipto[i]
                    #    io = di
                    #    continue
                    if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                        continue
                    if (nx, ny) in blocked_state or (nx, ny) in blocked_move:
                        continue
                    if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move:
                        continue
                    make_step = need_to_step((nx, ny), step, stepped) or \
                                need_to_step((nx2, ny2), step, stepped) or \
                                not one_step_curr.isdisjoint([(sx, sy), (nx, ny), (nx2, ny2)])
                    #print one_step_curr, (sx, sy), (nx, ny)
                    new_one_step = frozenset([(sx, sy), (nx, ny), (nx2, ny2)])
                    #new_stepped = step if make_step else stepped
                    new_stepped = step if make_step else stepped
                    new_step = min(skipto[step], stepindex[new_stepped])
                    #new_step = step
                    assert new_step >= step
                    index = stepindex[new_stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]) or\
                    #        need_to_step(fr, i, new_stepped) or\
                    #        need_to_step(to, i, new_stepped):
                    #        index = i
                    #        break
                    ch = heur[(nx2, ny2)] + 2
                    th = (traceheur[index]+1)*2
                    diff = th - ch
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[index]+1, (heur[(nx2, ny2)]/2) + 1)
                    t_mod = 2 if not finished else 1
                    num_to_visit += 1
                    if num_to_visit % 10000 == 0:
                        print 'to visit', num_to_visit
                    if make_step:
                        h += 2
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        #assert(h >= calced_heur)
                        #heapq.heappush(to_visit, (h, -state_count, (t+1), (nx,ny), \
                        #                          new_one_step,
                        #                          step, step,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))

                        to_visit[h].append((-(t+t_mod), (nx2,ny2), \
                                  new_one_step,
                                  new_step, step,
                                  ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) in targets))
                    else:
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        #assert(h >= calced_heur)
                        #print 'not make'
                        #heapq.heappush(to_visit, (h, -state_count, (t), (nx,ny), \
                        #                          new_one_step,
                        #                          step, stepped,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        to_visit[h].append((-(t), (nx2,ny2), \
                                        new_one_step,
                                        new_step, stepped,
                                        ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts), finished or (nx2, ny2) in targets))
        if (step < len(movements)):
            _, fr, int, to = movements[step]
            if to != (sx, sy) or (finished):
                if ((sx, sy), step + 1) in pred:
                    continue
                make_step = need_to_step(fr, step, stepped) or \
                            need_to_step(to, step, stepped) or \
                            need_to_step(int, step, stepped) or \
                            not one_step_curr.isdisjoint([to, int, fr])
                new_one_step = emptyset if make_step else one_step_curr
                #if make_step:
                #    new_one_step = emptyset
                #else:
                #    new_one_step = one_step_curr
                mod_heur = (new_one_step != emptyset)
                num_to_visit += 1
                if num_to_visit % 10000 == 0:
                    print 'to visit', num_to_visit
                if make_step:
                    #print heur[(sx, sy)], mod_heur
                    ch = heur[(sx, sy)] + mod_heur * 2 if not finished else mod_heur * 2
                    th = (traceheur[step])*2
                    diff = th - ch
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[step], (heur[(sx, sy)]/2) + mod_heur)
                    #h += 1
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    t_mod = 2 if (not finished or one_step_curr != emptyset) else 1
                    h += t_mod
                    new_step = min(skipto[step+1], stepindex[step])
                    #new_step = step+1
                    assert new_step >= step
                    #assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t+1), (sx,sy), \
                    #                          new_one_step, step+1, step,
                    #                          inserts))
                    to_visit[h].append((-(t+t_mod), (sx,sy), \
                               new_one_step, new_step, step,
                               inserts, finished))
                else:
                    index = stepindex[stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step+1)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step+1, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]):# or\
                    #        #need_to_step(fr, i, stepped) or\
                    #        #need_to_step(to, i, stepped):
                    #        index = i
                    #        break
                    #index = min(index, stepindex[stepped])
                    #print (sx, sy), heur[(sx, sy)], mod_heur, 'no makestep'
                    ch = heur[(sx, sy)] + mod_heur * 2 if not finished else mod_heur * 2
                    th = (traceheur[index])*2 + 2
                    diff = th - ch
                    h = t + ch + max(diff/2, 0)
                    #h = t + max(traceheur[index] + 1, (heur[(sx, sy)]/2) + mod_heur)
                    #print step+1, index, len(movements), h, calced_heur, t
                    #print traceheur
                    #assert(h >= calced_heur)
                    #heapq.heappush(to_visit, (h, -state_count, (t), (sx,sy),
                    #                          new_one_step, step+1, stepped,
                    #                          inserts))
                    new_step = min(skipto[step+1], stepindex[stepped])
                    #new_step = step + 1
                    assert new_step >= step
                    to_visit[h].append((-(t), (sx,sy),
                               new_one_step, new_step, stepped,
                               inserts, finished))
    if not finished or step != len(movements):
        print orig_blocked
        print source
        return None
    arrival = calced_heur
    new_movements = []
    prev_index = len(movements)
    while inserts != None:
        tup, i_step, inserts = inserts
        for i in reversed(xrange(i_step, prev_index)):
            new_movements.append(movements[i])
        new_movements.append(tup)
        prev_index = i_step
        #movements.insert(i_step, tup)
    for i in reversed(xrange(0, prev_index)):
        new_movements.append(movements[i])
    return (arrival, list(reversed(new_movements)), (sx, sy))

def a_star_search_increment_opt_improved_intersections_goals(height, width, v_index, source, goals,blocked_state, movements, orig_blocked, dist_cache, dist_map):
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    not_moved = set(orig_blocked)
    cblocked = set(orig_blocked)
    #moved_at = collections.defaultdict(list)
    moved_at = [[[] for _ in range(height+1)] for _ in range(width+1)]
    state_count = 0
    cache = {}
    last_move = {}
    for i, (index, fr, int, to) in enumerate(movements):
        last_move[index] = i
        not_moved.discard(fr)
    cblocked.difference_update(not_moved)
    for i, (index, fr, int, to) in enumerate(movements):
        #print cblocked
        #print movements[i]
        cblocked.remove(fr)
        if last_move[index] != i:
            cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[to[0]][to[1]].append(i)
        moved_at[int[0]][int[1]].append(i)
        moved_at[fr[0]][fr[1]].append(i)
    def need_to_step(state, step, last_step):
        res = cache.get((state, step, last_step))
        if res != None:
            return res
        arr = moved_at[state[0]][state[1]]
        index = numpy.searchsorted(arr, step) - 1
        res = index >= 0 and arr[index] >= last_step
        cache[(state, step, last_step)] = res
        #if index < 0:
            #return False
        return res
    #for key in moved_at:
    #    moved_at[key] = numpy.array(moved_at[key])
    for x in range(width+1):
        for y in range(height+1):
            moved_at[x][y] = numpy.array(moved_at[x][y])
    t = 1
    emptyset = frozenset()
    #traceheur = [0]
    #fringe = set()
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    if fr in fringe or to in fringe:
    #        t += 1
    #        fringe.clear()
    #    traceheur.append(t)
    #    fringe.update([fr, to])
    #traceheur.reverse()
    traceheur = [None for i in range(len(movements) + 1)]
    stepindex = [None for i in range(len(movements) + 1)]
    traceheur[len(movements)] = 0
    stepindex[len(movements)] = len(movements)
    #for i in reversed(range(len(movements))):
    #    _, fr, to = movements[i]
    #    fringe = set([fr, to])
    #    time_taken = 1
    #    stepindex[i] = len(movements)
    #    for j in range(i+1, len(movements)):
    #        _, fr, to = movements[j]
    #        if fr in fringe or to in fringe:
    #            stepindex[i] = j
    #            time_taken += traceheur[j]
    #            break
    #        fringe.update([fr, to])
    #    traceheur[i] = time_taken
    for i in reversed(range(len(movements))):
        _, fr, int, to = movements[i]
        arr1 = moved_at[fr[0]][fr[1]]
        arr2 = moved_at[to[0]][to[1]]
        arr3 = moved_at[int[0]][int[1]]
        i1 = numpy.searchsorted(arr1, i+1)
        i2 = numpy.searchsorted(arr2, i+1)
        i3 = numpy.searchsorted(arr3, i+1)
        i1 = len(movements) if i1 == len(arr1) else arr1[i1]
        i2 = len(movements) if i2 == len(arr2) else arr2[i2]
        i3 = len(movements) if i3 == len(arr3) else arr3[i3]
        #print i, stepindex[i+1], i1, i2
        stepindex[i] = min(stepindex[i+1], i1, i2, i3)
        traceheur[i] = traceheur[stepindex[i]] + 1
    #print stepindex
    #print traceheur
    to_visit = collections.defaultdict(list)
    print dist_cache
    k = max(traceheur[0], calc_heur_complex_intersection(source, goals, dist_map, dist_cache, 0))
    to_visit[k] = [(0, source, emptyset, 0, 0, 0, None)]
    #to_visit = [(max(traceheur[0], heur[source]), state_count, 0, source, emptyset, 0, 0, None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    while len(to_visit) != 0:
        if len(to_visit[k]) == 0:
            del to_visit[k]
            k += 1
            continue
        calced_heur = k
        t, (sx,sy), one_step_curr, step, stepped, goal_index, inserts = to_visit[k].pop()
        #calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts = heapq.heappop(to_visit)
        #print calced_heur, _, t, (sx,sy), one_step_curr, step, stepped, inserts
        #assert(calced_heur >= prev_heur)
        prev_heur = calced_heur
        #t = -t
        blocked_move = blocked_move_at_step[step]
        if ((sx, sy), goal_index, step) in pred:
            continue
        pred.add(((sx, sy), goal_index, step))
        if goal_index == len(goals) and step == len(movements):
            break
        def curr_moves():
            for dx, dy in adjacency_matrix:
                for dx2, dy2 in adjacency_matrix:
                    nx, ny = sx + dx, sy + dy
                    nx2, ny2 = nx + dx2, ny + dy2
                    if ((nx2, ny2), step) in pred:
                        continue
                    if (sx, sy) == (nx2, ny2):
                        continue
                    if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                        continue
                    if (nx, ny) in blocked_state or (nx, ny) in blocked_move:
                        continue
                    if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move:
                        continue
                    make_step = need_to_step((nx, ny), step, stepped) or \
                                need_to_step((nx2, ny2), step, stepped) or \
                                not one_step_curr.isdisjoint([(sx, sy), (nx, ny), (nx2, ny2)])
                    #print one_step_curr, (sx, sy), (nx, ny)
                    new_one_step = frozenset([(sx, sy), (nx, ny), (nx2, ny2)])
                    new_stepped = step if make_step else stepped
                    index = stepindex[new_stepped]
                    for (x,y) in new_one_step:
                        arr = moved_at[x][y]
                        id = numpy.searchsorted(arr, step)#, side='right')
                        if id == len(arr):
                            continue
                        index = min(index, arr[id])
                    #for i in range(step, len(movements)):
                    #    _, fr, to = movements[i]
                    #    if not new_one_step.isdisjoint([fr, to]) or\
                    #        need_to_step(fr, i, new_stepped) or\
                    #        need_to_step(to, i, new_stepped):
                    #        index = i
                    #        break
                    new_goal_index = goal_index + 1 if goal_index != len(goals) and (nx2, ny2) == goals[goal_index] \
                                     else goal_index
                    h = t + max(traceheur[index]+1, calc_heur_complex_intersection((nx2, ny2), goals,
                                                                                   dist_map, dist_cache, new_goal_index)+1)
                    if make_step:
                        h += 1
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #heapq.heappush(to_visit, (h, -state_count, (t+1), (nx,ny), \
                        #                          new_one_step,
                        #                          step, step,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        to_visit[h].append(((t+1), (nx2,ny2), \
                                      new_one_step,
                                      step, step, new_goal_index,
                                      ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts)))
                    else:
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #print 'not make'
                        #heapq.heappush(to_visit, (h, -state_count, (t), (nx,ny), \
                        #                          new_one_step,
                        #                          step, stepped,
                        #                          ((v_index, (sx, sy), (nx, ny)), step, inserts)))
                        to_visit[h].append(((t), (nx2,ny2), \
                                       new_one_step,
                                       step, stepped, new_goal_index,
                                       ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts)))
        curr_moves()
        def prev_moves():
            if (step < len(movements)):
                _, fr, int, to = movements[step]
                if to != (sx, sy):
                    if ((sx, sy), step + 1) in pred:
                        return
                    make_step = need_to_step(fr, step, stepped) or \
                                need_to_step(to, step, stepped) or \
                                need_to_step(int, step, stepped) or \
                                not one_step_curr.isdisjoint([to, int, fr])
                    new_one_step = emptyset if make_step else one_step_curr
                    #if make_step:
                    #    new_one_step = emptyset
                    #else:
                    #    new_one_step = one_step_curr
                    mod_heur = (new_one_step != emptyset)
                    if make_step:
                        #print heur[(sx, sy)], mod_heur
                        h = t + max(traceheur[step], calc_heur_complex_intersection((sx, sy), goals, dist_map,
                                                                                    dist_cache, goal_index) + mod_heur)
                        h += 1
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #heapq.heappush(to_visit, (h, -state_count, (t+1), (sx,sy), \
                        #                          new_one_step, step+1, step,
                        #                          inserts))
                        to_visit[h].append(((t+1), (sx,sy), \
                                       new_one_step, step+1, step, goal_index,
                                       inserts))
                    else:
                        index = len(movements)
                        index = stepindex[stepped]
                        for (x,y) in new_one_step:
                            arr = moved_at[x][y]
                            id = numpy.searchsorted(arr, step+1)#, side='right')
                            if id == len(arr):
                                continue
                            index = min(index, arr[id])
                        #for i in range(step+1, len(movements)):
                        #    _, fr, to = movements[i]
                        #    if not new_one_step.isdisjoint([fr, to]):# or\
                        #        #need_to_step(fr, i, stepped) or\
                        #        #need_to_step(to, i, stepped):
                        #        index = i
                        #        break
                        #index = min(index, stepindex[stepped])
                        #print (sx, sy), heur[(sx, sy)], mod_heur, 'no makestep'
                        h = t + max(traceheur[index] + 1,
                                    calc_heur_complex_intersection((sx, sy), goals, dist_map,
                                                                   dist_cache, goal_index) + mod_heur)
                        #print step+1, index, len(movements), h, calced_heur, t
                        #print traceheur
                        assert(h >= calced_heur)
                        #heapq.heappush(to_visit, (h, -state_count, (t), (sx,sy),
                        #                          new_one_step, step+1, stepped,
                        #                          inserts))
                        to_visit[h].append(((t), (sx,sy),
                                       new_one_step, step+1, stepped, goal_index,
                                       inserts))
        prev_moves()
    if goal_index != len(goals) or step != len(movements):
        return None
    arrival = calced_heur
    while inserts != None:
        tup, i_step, inserts = inserts
        movements.insert(i_step, tup)
    return (arrival, movements)

def a_star_search_increment_opt(height, width, source, target, blocked_transitions,blocked_state,heur, movements, orig_blocked):
    print 'this one'
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    for fr, to in movements:
        cblocked.remove(fr)
        cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
    def calc_movement_heur(step, blocked_one_step):
        if step >= len(movements):
            return 0 if len(blocked_one_step) == 0 else 1
        res = movement_heur.get((step, blocked_one_step), None)
        if res != None:
            return res
        fublocked_one_step = set(blocked_one_step)
        t_needed = 0
        t_needed_map = {}
        for i in range(step, len(movements)):
            fblocked_one_step = frozenset(fublocked_one_step)
            if (i, fblocked_one_step) in movement_heur:
                t_needed += movement_heur[(i, fblocked_one_step)]
                fublocked_one_step.clear()
                break
            t_needed_map[(i, fblocked_one_step)] = t_needed
            fr, to = movements[step]
            #assert fr in fublocked
            #assert to not in fublocked
            if to in fublocked_one_step or fr in fublocked_one_step:
                fublocked_one_step.clear()
                t_needed += 1
            fublocked_one_step.add(fr)
        if len(fublocked_one_step) != 0:
            t_needed += 1
        for (key, val) in t_needed_map.iteritems():
            movement_heur[key] = t_needed - val
        return movement_heur[(step, blocked_one_step)]
    to_visit = [(max(heur[source], calc_movement_heur(0, frozenset())), 0, source, None,
                 frozenset(), 0, [])]
    pred = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    while len(to_visit) != 0:
        calced_heur, t, (sx,sy), one_step_curr, one_step_blocked_move, step, inserts = heapq.heappop(to_visit)
        blocked_move = blocked_move_at_step[step]
        #if ((sx, sy), one_step_curr, step, one_step_blocked_move) in pred:
        #    continue
        #pred[(sx, sy), one_step_curr, step, one_step_blocked_move] = inserts
        if ((sx, sy), step) in pred:
            continue
        pred[(sx, sy), step] = inserts
        if (sx, sy) == target:
            break
        for dx, dy in adjacency_matrix:
            nx, ny = sx + dx, sy + dy
            if nx < 1 or nx > width or ny < 1 or ny > height:
                continue
            if (nx, ny) in blocked_state or (nx, ny) in blocked_move or \
                    ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                continue
            if (nx, ny) in one_step_blocked_move or one_step_curr != None:
                n_one_step_blocked_move = frozenset()
                mod = 1
            else:
                n_one_step_blocked_move = one_step_blocked_move
                mod = 0
            h = t + mod + max(heur[(nx,ny)], calc_movement_heur(step, n_one_step_blocked_move))
            n_inserts = list(inserts)
            n_inserts.append((step, ((sx, sy), (nx, ny))))
            heapq.heappush(to_visit, (h, t+mod, (nx,ny), (sx,sy), n_one_step_blocked_move, step, n_inserts))
        if (step < len(movements)):
            fr, to = movements[step]
            if to != (sx, sy):
                n_one_step_blocked_move = set(one_step_blocked_move)
                if fr in one_step_blocked_move or to == one_step_curr or to in one_step_blocked_move:
                    mod = 1
                    n_one_step_blocked_move.clear()
                    n_one_step = None
                else:
                    mod = 0
                    n_one_step = one_step_curr
                n_one_step_blocked_move.add(fr)
                n_one_step_blocked_move = frozenset(n_one_step_blocked_move)
                h = t + mod + max(heur[(sx,sy)], calc_movement_heur(step+1, n_one_step_blocked_move))
                heapq.heappush(to_visit, (h, t+mod, (sx,sy), n_one_step, n_one_step_blocked_move, step+1, inserts))
    if (sx, sy) != target:
        print 'failed'
        print orig_blocked
        print source
        print target
        return None
    arrival = calced_heur
    for i_step, pair in reversed(inserts):
        movements.insert(i_step, pair)
    return (arrival, movements)

def compress_trace(movements):
    steps = []
    step_for_pos = collections.defaultdict(lambda : 0)
    t = 0
    #fringe = set()
    #for tup in movements:
    #    if not fringe.isdisjoint(tup):
    #        t+= 1
    #        fringe.clear()
    #    fringe.update(tup)
    #print (t+1)
    for tup in movements:
        new_step = max([step_for_pos[item] for item in tup])
        #print "{}-{}".format(new_step, len(steps))
        if new_step == len(steps):
            steps.append((set(), []))
        for item in tup:
            step_for_pos[item] = new_step + 1
        confset, movelist = steps[new_step]
        confset.update(tup)
        movelist.append(tup)
    newmovements = []
    for _, step in steps:
        newmovements.extend(step)
#        for tup in step:
#            print tup
    #os.system('pause')
    ave = float(len(movements))/float(len(steps)) if len(steps) != 0 else float('inf')
    print 'compressedlength:{}-averagestep:{}-totsteps:{}'.format(len(steps), ave, len(newmovements))
    #print [len(step) for _, step in steps]
    return len(steps), newmovements

def a_star_search_increment_opt_intersections(height, width, v_index, source, target, blocked_transitions,blocked_state, heur, movements, orig_blocked, orig_one_step_blocked = []):
    movement_heur = {}
    blocked_move_at_step = [orig_blocked]
    cblocked = set(orig_blocked)
    moved_at = collections.defaultdict(list)
    last_move = {}
    for i, (index, _, _, _) in enumerate(movements):
        last_move[index] = i
    for i, (index, fr, int, to) in enumerate(movements):
        #print cblocked
        #print movements[i]
        cblocked.remove(fr)
        #if last_move[index] != i:
        cblocked.add(to)
        blocked_move_at_step.append(frozenset(cblocked))
        moved_at[int].append(i)
        moved_at[to].append(i)
        moved_at[fr].append(i)
    def need_to_step(state, step, last_step):
        arr = moved_at[state]
        if len(arr) == 0:
            return False
        index = numpy.searchsorted(arr, step) - 1
        if index < 0:
            return False
        return arr[index] >= last_step
    t = 1
    fringe = set()
    traceheur = [0]
    for (_, fr, int, to) in reversed(movements):
        if fr in fringe or int in fringe or to in fringe:
            t += 1
            fringe.clear()
        fringe.update([fr, int, to])
        traceheur.append(t)
    traceheur.reverse()
    emptyset = frozenset()
    to_visit = [(max(traceheur[0], heur[source]), 0, source, emptyset, 0, -1, None)]
    pred = set()
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1)]
    while len(to_visit) != 0:
        calced_heur, t, (sx,sy), one_step_curr, step, stepped, inserts = heapq.heappop(to_visit)
        t = -t
        blocked_move = blocked_move_at_step[step]
        #if ((sx, sy), one_step_curr, step, one_step_blocked_move) in pred:
        #    continue
        #pred[(sx, sy), one_step_curr, step, one_step_blocked_move] = inserts
        if ((sx, sy), step) in pred:
            continue
        pred.add(((sx, sy), step))
        if (sx, sy) == target:
            break
        for dx, dy in adjacency_matrix:
            for dx2, dy2 in adjacency_matrix:
                if dx == dy == 0 or dx2 == dy2 == 0 and not (dx == dy == dx2 == dy2 == 0):
                    continue
                nx, ny = sx + dx, sy + dy
                nx2, ny2 = nx + dx2, ny + dy2
                if nx2 == sx and ny2 == sy and not (dx == dy == dx2 == dy2 == 0):
                    continue
                if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                    continue
                if (nx, ny) in blocked_state or (nx, ny) in blocked_move or \
                        ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                    continue
                if (nx2, ny2) in blocked_state or (nx2, ny2) in blocked_move or \
                        ((nx, ny), (nx2, ny2)) in blocked_transitions or ((nx2, ny2), (nx, ny)) in blocked_transitions:
                    continue
                make_step = need_to_step((nx, ny), step, stepped) or \
                            need_to_step((nx2, ny2), step, stepped) or \
                            not one_step_curr.isdisjoint([(sx, sy), (nx, nx), (nx2, ny2)])
                h = t + max(traceheur[step], heur[(nx2, ny2)])
                if make_step:
                    h += 1
                    heapq.heappush(to_visit, (h, -(t+1), (nx2,ny2), \
                                              frozenset([(sx, sy), (nx, ny), (nx2, ny2)]),
                                              step, step,
                                              ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts)))
                else:
                    heapq.heappush(to_visit, (h, -(t), (nx2,ny2), \
                                              frozenset([(sx, sy), (nx, ny), (nx2, ny2)]),
                                              step, stepped,
                                              ((v_index, (sx, sy), (nx, ny), (nx2, ny2)), step, inserts)))
        if (step < len(movements)):
            _, fr, int, to = movements[step]
            if to != (sx, sy):
                make_step = need_to_step(fr, step, stepped) or \
                            need_to_step(to, step, stepped) or \
                            need_to_step(int, step, stepped)
                h = t + max(traceheur[step+1], heur[(sx, sy)])
                if make_step:
                    h += 1
                    heapq.heappush(to_visit, (h, -(t+1), (sx,sy), \
                                              emptyset, step+1, step,
                                              inserts))
                else:
                    heapq.heappush(to_visit, (h, -(t+1), (sx,sy), \
                                              one_step_curr, step+1, stepped,
                                              inserts))
    if (sx, sy) != target:
        return None
    arrival = calced_heur
    while inserts != None:
        tup, i_step, inserts = inserts
        #print 'tup', tup
        movements.insert(i_step, tup)
    return (arrival, movements)

def a_star_search(height, width, source, target, blocked_transitions,blocked_state,heur,blocked_at_time, plan_out_to, endby, final_poses):
    if ((0, source) in blocked_at_time):
        return None
    to_visit = [(heur[source], 0, source, None)]
    pred = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1),(0,0)]
    while len(to_visit) != 0:
        _, t, (sx,sy), prev = heapq.heappop(to_visit)
        t_used = min(t, endby)
        if (t_used, (sx, sy)) in pred:
            continue
        pred[(t_used, (sx, sy))] = prev
        if (sx, sy) == target:
            break
        for dx, dy in adjacency_matrix:
            nx, ny = sx + dx, sy + dy
            if nx < 1 or nx > width or ny < 1 or ny > height:
                continue
            if (nx, ny) in blocked_state or (t + 1, (nx, ny)) in blocked_at_time or \
                    (t >= endby and (nx, ny) in final_poses) or \
                    ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                continue
            h = t + 1 + heur[(nx,ny)]
            #if h <= endby:
            heapq.heappush(to_visit, (h, t+1, (nx,ny), (sx,sy)))
    if (sx, sy) != target:
        return None
    arrival = t
    blockedtimes = []
    for i in range(arrival, plan_out_to+1):
        blockedtimes.append((i, (sx, sy)))
    curr = (sx, sy)
    #print 's:{} t:{}'.format(source, target)
    while True:
    #    print curr
        t_used = min(t, endby)
        prev = pred[(t_used, curr)]
        blockedtimes.append((t, curr))
        if prev == None:
            break
        blockedtimes.append((t, prev))
        curr = prev
        t-=1
    #print blocked_transitions
    #ai =d
    assert t == 0
    return (arrival, blockedtimes)

def calc_heur_complex(state, goals, dist_map, dist_cache, goal_index):
    if goal_index == len(goals):
        return 0
    goal = goals[goal_index]
    dist = dist_cache[goal_index]
    return dist if goal == None else dist + dist_map[goal][state]

def calc_heur_complex_intersection(state, goal, dist_map, dist_cache, goal_index):
    return calc_heur_complex(state, goal, dist_map, dist_cache, goal_index)/2

def a_star_search_goals(height, width, source, goals, blocked_transitions,blocked_state,blocked_at_time, plan_out_to, endby, final_poses, dist_map, dist_cache):
    #if ((0, source) in blocked_at_time):
    #    return None
    to_visit = [(calc_heur_complex(source, goals[0], dist_map, dist_cache, 0), 0, 0, source, None)]
    pred = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1),(0,0)]
    while len(to_visit) != 0:
        he, t, goal, (sx,sy), prev = heapq.heappop(to_visit)
        goal *= -1
        t *= -1
        #print goal
        #print goals
        #print goal
        t_used = min(t, endby)
        if (t_used, (sx, sy), goal) in pred:
            continue
        pred[(t_used, (sx, sy), goal)] = prev
        if goal >= len(goals):
            break
        for dx, dy in adjacency_matrix:
            if dx == 0 and dy == 0 and sx % 2 == 1 and sy % 2 == 1:
                continue
            nx, ny = sx + dx, sy + dy
            if nx < 1 or nx > width or ny < 1 or ny > height:
                continue
            if (nx, ny) in blocked_state or (t + 1, (nx, ny)) in blocked_at_time or \
                    (t >= endby and (nx, ny) in final_poses) or \
                    ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                continue
            h = t + 1 + calc_heur_complex((nx,ny), goals[goal], dist_map, dist_cache, goal)
            #if h <= endby:
            if goals[goal] == (nx, ny):
                heapq.heappush(to_visit, (h, -(t+1), -(goal +1), (nx,ny), ((sx,sy), goal)))
            else:
                heapq.heappush(to_visit, (h, -(t+1), -goal, (nx,ny), ((sx,sy), goal)))
    if goal < len(goals):
        return None
    arrival = t
    blockedtimes = []
    for i in range(arrival, plan_out_to+1):
        blockedtimes.append((i, (sx, sy)))
    curr = (sx, sy)
    #print 's:{} t:{}'.format(source, target)
    while True:
        t_used = min(t, endby)
        blockedtimes.append((t, curr))
        tup = pred[(t_used, curr, goal)]
        if tup == None:
            break
        prev, goal = tup
        blockedtimes.append((t, prev))
        curr = prev
        t-=1
    #print blocked_transitions
    #ai =d
    assert t == 0
    return (arrival, blockedtimes)

def a_star_search_goals_intersections(height, width, source, goals, blocked_transitions,blocked_state,blocked_at_time, plan_out_to, endby, final_poses, dist_map, dist_cache):
    #if ((0, source) in blocked_at_time):
    #    return None
    assert goals[-1] not in final_poses
    endafter = 0
    for t in range(endby+1):
        if (t, goals[-1]) in blocked_at_time:
            endafter = t
    to_visit = [(calc_heur_complex_intersection(source, goals[0], dist_map, dist_cache, 0), 0, 0, source, None)]
    pred = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1),(0,0)]
    while len(to_visit) != 0:
        he, t, goal, (sx,sy), prev = heapq.heappop(to_visit)
        goal *= -1
        t *= -1
        #print goal
        #print goals
        #print goal
        t_used = min(t, endby)
        if (t_used, (sx, sy), goal) in pred:
            continue
        pred[(t_used, (sx, sy), goal)] = prev
        if goal >= len(goals):
            break
        for dx, dy in adjacency_matrix:
            for dx2, dy2 in adjacency_matrix:
                if dx == dy == 0 or dx2 == dy2 == 0 and not (dx == dy == dx2 == dy2 == 0):
                    continue
                nx, ny = sx + dx, sy + dy
                nx2, ny2 = nx + dx2, ny + dy2
                if nx2 == sx and ny2 == sy and not (dx == dy == dx2 == dy2 == 0):
                    continue
                if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                    continue
                if (nx, ny) in blocked_state or (t + 1, (nx, ny)) in blocked_at_time or \
                        (t >= endby and (nx, ny) in final_poses) or \
                        ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                    continue
                if (nx2, ny2) in blocked_state or (t + 1, (nx2, ny2)) in blocked_at_time or \
                        (t + 2, (nx2, ny2)) in blocked_at_time or \
                        (t >= endby and (nx2, ny2) in final_poses) or \
                        ((nx, ny), (nx2, ny2)) in blocked_transitions or ((nx2, ny2), (nx, ny)) in blocked_transitions:
                    continue
                h = t + 1 + calc_heur_complex_intersection((nx2,ny2), goals[goal], dist_map, dist_cache, goal)
                #if h <= endby:
                if goals[goal] == (nx2, ny2) and (goal != len(goals)-1 or t+1 > endafter):
                    heapq.heappush(to_visit, (h, -(t+1), -(goal +1), (nx2,ny2), ((sx,sy), (nx, ny), goal)))
                else:
                    heapq.heappush(to_visit, (h, -(t+1), -goal, (nx2,ny2), ((sx,sy), (nx, ny), goal)))
    if goal < len(goals):
        return None
    arrival = t
    blockedtimes = []
    for i in range(arrival, plan_out_to+1):
        blockedtimes.append((i, (sx, sy)))
    curr = (sx, sy)
    #print 's:{} t:{}'.format(source, target)
    movements = []
    while True:
        t_used = min(t, endby)
        blockedtimes.append((t, curr))
        tup = pred[(t_used, curr, goal)]
        if tup == None:
            break
        prev, prev2, goal = tup
        blockedtimes.append((t, prev))
        blockedtimes.append((t, prev2))
        movements.append((prev, curr))
        curr = prev
        t-=1
    movements.reverse()
    #print blocked_transitions
    #ai =d
    assert t == 0
    return (arrival, blockedtimes, movements)

def a_star_search_goals_intersections(height, width, source, goals, blocked_transitions,blocked_state,blocked_at_time, plan_out_to, endby, final_poses, dist_map, dist_cache):
    #if ((0, source) in blocked_at_time):
    #    return None
    assert goals[-1] not in final_poses
    endafter = 0
    for t in range(endby+1):
        if (t, goals[-1]) in blocked_at_time:
            endafter = t
    to_visit = [(calc_heur_complex_intersection(source, goals[0], dist_map, dist_cache, 0), 0, 0, source, None)]
    pred = {}
    adjacency_matrix = [(-1,0),(0,-1),(1,0),(0,1),(0,0)]
    while len(to_visit) != 0:
        he, t, goal, (sx,sy), prev = heapq.heappop(to_visit)
        goal *= -1
        t *= -1
        #print goal
        #print goals
        #print goal
        t_used = min(t, endby)
        if (t_used, (sx, sy), goal) in pred:
            continue
        pred[(t_used, (sx, sy), goal)] = prev
        if goal >= len(goals):
            break
        for dx, dy in adjacency_matrix:
            for dx2, dy2 in adjacency_matrix:
                if dx == dy == 0 or dx2 == dy2 == 0 and not (dx == dy == dx2 == dy2 == 0):
                    continue
                nx, ny = sx + dx, sy + dy
                nx2, ny2 = nx + dx2, ny + dy2
                if nx2 == sx and ny2 == sy and not (dx == dy == dx2 == dy2 == 0):
                    continue
                if nx2 < 1 or nx2 > width or ny2 < 1 or ny2 > height:
                    continue
                if (nx, ny) in blocked_state or (t + 1, (nx, ny)) in blocked_at_time or \
                        (t >= endby and (nx, ny) in final_poses) or \
                        ((sx, sy), (nx, ny)) in blocked_transitions or ((nx, ny), (sx, sy)) in blocked_transitions:
                    continue
                if (nx2, ny2) in blocked_state or (t + 1, (nx2, ny2)) in blocked_at_time or \
                        (t + 2, (nx2, ny2)) in blocked_at_time or \
                        (t >= endby and (nx2, ny2) in final_poses) or \
                        ((nx, ny), (nx2, ny2)) in blocked_transitions or ((nx2, ny2), (nx, ny)) in blocked_transitions:
                    continue
                h = t + 1 + calc_heur_complex_intersection((nx2,ny2), goals[goal], dist_map, dist_cache, goal)
                #if h <= endby:
                if goals[goal] == (nx2, ny2) and (goal != len(goals)-1 or t+1 > endafter):
                    heapq.heappush(to_visit, (h, -(t+1), -(goal +1), (nx2,ny2), ((sx,sy), (nx, ny), goal)))
                else:
                    heapq.heappush(to_visit, (h, -(t), -goal, (nx2,ny2), ((sx,sy), (nx, ny), goal)))
    if goal < len(goals):
        return None
    arrival = t
    blockedtimes = []
    for i in range(arrival, plan_out_to+1):
        blockedtimes.append((i, (sx, sy)))
    curr = (sx, sy)
    #print 's:{} t:{}'.format(source, target)
    movements = []
    while True:
        t_used = min(t, endby)
        blockedtimes.append((t, curr))
        tup = pred[(t_used, curr, goal)]
        if tup == None:
            break
        prev, prev2, goal = tup
        blockedtimes.append((t, prev))
        blockedtimes.append((t, prev2))
        movements.append((prev, curr))
        curr = prev
        t-=1
    movements.reverse()
    #print blocked_transitions
    #ai =d
    assert t == 0
    return (arrival, blockedtimes, movements)

def prioritized_scheduling_increment(height, width, blocked_transitions,blocked_state, vehicles):
    sttime = -time.clock()
    heurs = {}
    for _, target in vehicles:
        heurs[target] = calc_heur(height, width, target, blocked_transitions, blocked_state)
    LBE = max([heurs[target][source] for source, target in vehicles])
    bestmakespan = float('inf')
    for i in range(10):
        blocked_at_time = set()
        random.shuffle(vehicles)
        makespan = 0
        failed = False
        movements = []
        orig_blocked = []
        for source, target in vehicles:
            res = a_star_search_increment_opt(height, width, source, target, blocked_transitions, blocked_state,
                                            heurs[target], movements, frozenset(orig_blocked))
            orig_blocked.append(source)
            if res == None:
                failed = True
                break
            else:
                a, movements = res
                makespan = max(makespan, a)
        if failed:
            #print 'failed'
            continue
        bestmakespan = min(makespan, bestmakespan)
        #print 'makespan={}'.format(makespan)
    return float(bestmakespan)/float(LBE), sttime + time.clock()

def prioritized_scheduling(height, width, blocked_transitions,blocked_state, vehicles, duration = -1):
    sttime = -time.clock()
    heurs = {}
    for _, target in vehicles:
        heurs[target] = calc_heur(height, width, target, blocked_transitions, blocked_state)
    LBE = max([heurs[target][source] for source, target in vehicles])
    bestmakespan = float('inf')
    until = 100 if duration <= 0 else 999999
    for i in range(until):
        blocked_at_time = set()
        random.shuffle(vehicles)
        makespan = 0
        failed = False
        final_poses = set()
        for source, target in vehicles:
            res = a_star_search(height, width, source, target, blocked_transitions, blocked_state,
                                            heurs[target], blocked_at_time, LBE*5, makespan, final_poses)
            if res == None:
                failed = True
                break
            else:
                final_poses.add(target)
                a, blockedtimes = res
                makespan = max(makespan, a)
                blocked_at_time.update(blockedtimes)
        if failed:
            if duration > 0 and sttime + time.clock() >= duration:
                break
            #print 'failed'
            continue
        bestmakespan = min(makespan, bestmakespan)
        #print 'makespan={}'.format(makespan)
        if duration > 0 and sttime + time.clock() >= duration:
            break
    return bestmakespan, float(bestmakespan)/float(LBE), sttime + time.clock(), i

def prioritized_scheduling_goals(height, width, blocked_transitions,blocked_state, vehicles, duration = -1):
    sttime = -time.clock()
    dist_maps = {}
    print 'start heur calc'
    for _, goals in vehicles:
        for goal in goals:
            if goal not in  dist_maps:
                dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    for i in range(len(vehicles)):
        dist_cache = [0]
        spos, goals = vehicles[i]
        former_goal = goals[-1]
        for goal in list(reversed(goals))[1:]:
            dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
            #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
            former_goal = goal
        vehicles[i] = (spos, goals, list(reversed(dist_cache)))
    LBE = max([dc[0] for _, _, dc in vehicles])
    bestmakespan = float('inf')
    bestmoves = None
    until = 100 if duration <= 0 else 999999
    for i in range(until):
        blocked_at_time = set()
        for source,_,_ in vehicles:
            blocked_at_time.update([(0, source), (1, source)])
        random.shuffle(vehicles)
        makespan = 0
        failed = False
        final_poses = set()
        moves = []
        for j, (source, goals, dist_cache) in enumerate(vehicles):
            #print 'j:{}'.format(j)
            res = a_star_search_goals_intersections(height, width, source, goals, blocked_transitions, blocked_state,
                                      blocked_at_time, LBE*3, makespan, final_poses, dist_maps, dist_cache)
            if res == None:
                print 'failed'
                failed = True
                break
            else:
                final_poses.add(goals[-1])
                a, blockedtimes, movements = res
                moves.append(movements)
                makespan = max(makespan, a)
                blocked_at_time.update(blockedtimes)
        if failed:
            if duration > 0 and sttime + time.clock() >= duration:
                break
            #print 'failed'
            continue
        print makespan
        if makespan < bestmakespan:
            bestmoves = moves
            bestmakespan = makespan
        #print 'makespan={}'.format(makespan)
        if duration > 0 and sttime + time.clock() >= duration:
            break
    return bestmakespan, bestmoves

def prioritized_scheduling_goals_increment_one_shot(height, width, blocked_transitions,blocked_state, vehicles, duration = -1):
    sttime = -time.clock()
    dist_maps = {}
    print 'start heur calc'
    for _, goals in vehicles:
        for goal in goals:
            if goal not in  dist_maps:
                dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    for i in range(len(vehicles)):
        dist_cache = [0]
        spos, goals = vehicles[i]
        former_goal = goals[-1]
        for goal in list(reversed(goals))[1:]:
            dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
            #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
            former_goal = goal
        vehicles[i] = (spos, goals, list(reversed(dist_cache)))
    LBE = max([dc[0] for _, _, dc in vehicles])
    bestmakespan = float('inf')
    bestmoves = None
    until = 1 if duration <= 0 else 999999
    for i in range(until):
        random.shuffle(vehicles)
        makespan = 0
        failed = False
        final_poses = set()
        movements = []
        orig_blocked = set([])
        for j, (source, goals, dist_cache) in enumerate(vehicles):
            print 'j:{}-{}'.format(j, len(vehicles))
            res = a_star_search_increment_opt_improved_intersections_goals(
                height, width, j, source, goals, blocked_state, movements, orig_blocked,
                dist_cache, dist_maps)
            if res == None:
                print 'failed'
                failed = True
                break
            else:
                orig_blocked.add(source)
                final_poses.add(goals[-1])
                a, newmovements = res
                movements = newmovements
                a, movements = compress_trace(movements)
                makespan = max(makespan, a)
        if failed:
            if duration > 0 and sttime + time.clock() >= duration:
                break
            print 'failed'
            continue
        print 'makespan', makespan
        if makespan < bestmakespan:
            bestmoves = movements
            bestmakespan = makespan
        #print 'makespan={}'.format(makespan)
        if duration > 0 and sttime + time.clock() >= duration:
            break
    return bestmakespan, bestmoves

def prioritized_scheduling_goals_increment_greedy(height, width, blocked_transitions,blocked_state, vehicles, tasks):
    dist_maps = {}
    print 'start heur calc'
    #for _, goals in vehicles:
    #    for goal in goals:
    #        if goal not in  dist_maps:
    #            dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    lowerbound = 0
    def dist_between((x1, y1), (x2, y2)):
        dx = abs(x1-x2)
        dy = abs(y1-y2)
        return (dx + dy)/2
    for (fr, to) in tasks:
        dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
        lowerbound += dist_map[to]/2
        froms = vehicles + [tup[1] for tup in tasks if (fr, to) != tup]
        lowerbound += min([dist_map[end]/2 for end in froms])
    for i in range(len(vehicles)):
        spos = vehicles[i]
        #for goal in list(reversed(goals))[1:]:
        #    dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
        #    #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
        #    former_goal = goal
        vehicles[i] = [i, spos, None]
    #os.system('pause')
    dist_maps.clear()
    #LBE = max([dc[0] for _, _, _, dc in vehicles])
    movements = []
    vehicles_stopped = set()
    orig_locations = set()
    tasks_in_prog = set()
    makespan = 0
    for j, (v_index, source, goals) in enumerate(vehicles):
        orig_locations.add(source)
    failed = False
    while True:
        if len(tasks) != 0:
            print 'have left'
            vehicle_set = set(range(len(vehicles)))
        elif len(tasks_in_prog) != 0:
            print 'only last moves'
            vehicle_set = set([vid for vid, _, task in vehicles if task != None])
        else:
            print 'no left'
            #print vehicles_stopped
            vehicle_set = set(range(len(vehicles)))
            vehicle_set.difference_update(vehicles_stopped)
            if len(vehicle_set) == 0:
                break
        move_to = 0
        do_no_left = len(tasks) + len(tasks_in_prog) == 0
        #print 'movements', movements
        for move_to in reversed(xrange(len(movements))):
            #print movements[move_to]
            (v_index, fr, int, to) = movements[move_to]
            if v_index in vehicle_set:
                if len(vehicle_set) == 1:
                    move_to += 1
                    break
                vehicle_set.remove(v_index)
        curr_index = random.choice(list(vehicle_set))
        if not do_no_left:
            print 'do this stuff'
            _, source, task = vehicles[curr_index]
            orig_blocked = set(orig_locations)
            fringe = set()
            start_location = source
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index == curr_index:
                    start_location = to
            vehicle_position = {}
            for v_index, pos, _ in vehicles:
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles if (v_index not in
                                                                                         vehicle_not_moved)])
            new_blocked = set(blocked_state)
            adjacency = [(0,1),(1,0),(-1,0),(0,-1)]
            to_explore = [[start_location],[],[],[],[]]
            to_keep = set([start_location])
            #dist_map = dist_maps[goals[0]]
            if task == None:
                dist_map = calc_heur(height, width, start_location, blocked_transitions, blocked_state)
                min_task = None
                for task in tasks:
                    if min_task == None:
                        min_task = task
                    elif dist_map[task[0]] < dist_map[min_task[0]]:
                        min_task = task
                end_location = min_task[0]
                tasks.remove(min_task)
                tasks_in_prog.add(min_task)
                vehicles[curr_index][2] = min_task
                print 'min_task', min_task
            else:
                end_location = task[1]
                vehicles[curr_index][2] = None
                tasks_in_prog.remove(task)
            dist_map = calc_heur(height, width, end_location, blocked_transitions, blocked_state)
            print 'start loc', start_location
            delay = 0
            num_in = [0 for _ in xrange(len(to_explore))]
            while True:
                if delay >= len(to_explore):
                    break
                if len(to_explore[delay]) == 0:
                    delay += 1
                    continue
                x,y = to_explore[delay].pop()
                for dx, dy in adjacency:
                    nx, ny = x + dx, y + dy
                    if (nx, ny) in blocked_state or nx < 1 or nx > width or ny < 1 or ny > height:
    #                    print 'blocked'
                        continue
                    #print dist_map[(x,y)], dist_map[(nx,ny)]
                    if (nx, ny) in to_keep:
                        continue
                    dmod = dist_map[(nx,ny)] - dist_map[(x,y)] + 1
                    #print dist_map[(x,y)], dist_map[(nx,ny)], delay + 1, len(to_explore)
                    if delay + dmod >= len(to_explore):
                        continue
                    #elif dist_map[(x,y)] <= dist_map[(nx,ny)]:
                    #    continue
                    #print (nx, ny)
                    to_keep.add((nx,ny))
                    num_in[delay+dmod] += 1
                    to_explore[delay+dmod].append((nx,ny))
            print num_in
            #for x in range(1, width+1):
            #    for y in range(1, height+1):
            #        if (x,y) not in to_keep:
            #            new_blocked.add((x,y))
            orig_locations.add(source)
            print curr_index
            def_keep = []
        else:
            print 'do no left stuff'
            start_location = to
            end_location = None
            new_blocked = blocked_state
            dist_map == None
            orig_blocked = set(orig_locations)
            fringe = set()
            last_place_stopped = {}
            vehicle_position = {}
            for v_index, pos, _ in vehicles:
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles if (v_index not in
                                                                                         vehicle_not_moved)
                                or v_index in vehicles_stopped])
            print 'stopped_vehicles', [(v_index, vehicle_position[v_index]) for v_index, _, _, in vehicles if
                                       (v_index) in vehicles_stopped]
            #def_keep = [pos for _, pos in last_place_stopped.iteritems()]
            #print def_keep
        #orig_blocked.remove(start_location)
        print start_location, end_location,
        #print curr_index
        #print movements
        #print movements[move_to:]
        if start_location != end_location:
            res = a_star_search_increment_opt_improved_intersections_mod(height, width, curr_index, start_location,
                                                                         end_location, new_blocked,#blocked_state,
                                                                         dist_map, movements[move_to:],
                                                                         orig_blocked,stopped_vehicles=vehicles_stopped,
                                                                         def_keep=def_keep)
        else:
            res = (makespan, movements[move_to:])
        if res == None:
            print 'failed'
            failed = True
            break
        else:
            a, newmovements = res
            #print '1', newmovements
            movements = movements[:move_to] + newmovements
            #print '2', movements
            makespan, movements = compress_trace(movements)
            #print '3', movements
            if do_no_left:
                vehicles_stopped.add(curr_index)
            makespan = a
    if failed:
        print 'failed'
        return None
    orig_blocked = set(orig_locations)
    #print movements
    for i in xrange(len(movements)):
        print movements[i]
        (v_index, fr, int, to) = movements[i]
        assert fr in orig_blocked
        orig_blocked.remove(fr)
        if to in orig_blocked:
            print movements[i+1:]
        assert to not in orig_blocked
        orig_blocked.add(to)
    #print makespan
    print 'lowerbound', float(lowerbound)/float(len(vehicles)), 'tot', lowerbound, len(vehicles)
    return makespan, movements

def prioritized_scheduling_goals_increment_greedy_pick_drop(height, width, blocked_transitions,blocked_state, vehicles, tasks):
    dist_maps = {}
    task_numbers = {}
    for i, task in tasks:
        task_numbers[i] = task
    task_assigned = collections.defaultdict(list)
    orig_vehicle_set = set()
    print 'start heur calc'
    #for _, goals in vehicles:
    #    for goal in goals:
    #        if goal not in  dist_maps:
    #            dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    vehicle_map = {}
    lowerbound = 2 * len(tasks)
    def dist_between((x1, y1), (x2, y2)):
        dx = abs(x1-x2)
        dy = abs(y1-y2)
        return (dx + dy)/2
    for (_, (fr, to)) in tasks:
        #print fr, to
        dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
        lowerbound += dist_map[to]/2
        froms = [spos for _, spos in vehicles] + [tup[1] for (_, tup) in tasks if (fr, to) != tup]
        #print froms
        #print tasks
        lowerbound += min([dist_map[end]/2 for end in froms])
    for i in range(len(vehicles)):
        v_index, spos = vehicles[i]
        #for goal in list(reversed(goals))[1:]:
        #    dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
        #    #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
        #    former_goal = goal
        vehicles[i] = [v_index, spos, None]
        vehicle_map[v_index] = [v_index, spos, None]
        orig_vehicle_set.add(v_index)
    vehicles = vehicle_map
    #os.system('pause')
    dist_maps.clear()
    #LBE = max([dc[0] for _, _, _, dc in vehicles])
    movements = []
    vehicles_stopped = set()
    orig_locations = set()
    tasks_in_prog = set()
    makespan = 0
    for v_index, source, goals in vehicles.values():
        orig_locations.add(source)
    failed = False
    while True:
        if len(tasks) != 0:
            print 'have left'
            vehicle_set = set(orig_vehicle_set)
        elif len(tasks_in_prog) != 0:
            print 'only last moves'
            vehicle_set = set([vid for vid, _, task in vehicles.values() if task != None])
        else:
            print 'no left'
            #print vehicles_stopped
            vehicle_set = set(orig_vehicle_set)
            vehicle_set.difference_update(vehicles_stopped)
            if len(vehicle_set) == 0:
                break
        move_to = 0
        do_no_left = len(tasks) + len(tasks_in_prog) == 0
        #print 'movements', movements
        for move_to in reversed(xrange(len(movements))):
            #print movements[move_to]
            (v_index, fr, int, to) = movements[move_to]
            if v_index in vehicle_set:
                if len(vehicle_set) == 1:
                    move_to += 1
                    break
                vehicle_set.remove(v_index)
        curr_index = random.choice(list(vehicle_set))
        if not do_no_left:
            print 'do this stuff'
            _, source, task = vehicles[curr_index]
            orig_blocked = set(orig_locations)
            fringe = set()
            start_location = source
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index == curr_index:
                    start_location = to
            vehicle_position = {}
            for v_index, pos, _ in vehicles.values():
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles.values()])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles.values() if (v_index not in
                                                                                         vehicle_not_moved)])
            new_blocked = set(blocked_state)
            adjacency = [(0,1),(1,0),(-1,0),(0,-1)]
            to_explore = [[start_location],[],[],[],[]]
            to_keep = set([start_location])
            #dist_map = dist_maps[goals[0]]
            if task == None:
                dist_map = calc_heur(height, width, start_location, blocked_transitions, blocked_state)
                min_task = None
                for i, task in tasks:
                    if min_task == None:
                        min_task = (i, task)
                    elif dist_map[task[0]] < dist_map[min_task[1][0]]:
                        min_task = (i, task)
                end_location = min_task[1][0]
                tasks.remove(min_task)
                tasks_in_prog.add(min_task)
                vehicles[curr_index][2] = min_task
                task_assigned[curr_index].append(min_task[0])
            else:
                end_location = task[1][1]
                vehicles[curr_index][2] = None
                tasks_in_prog.remove(task)
            dist_map = calc_heur(height, width, end_location, blocked_transitions, blocked_state)
            print 'start loc', start_location
            delay = 0
            num_in = [0 for _ in xrange(len(to_explore))]
            while True:
                if delay >= len(to_explore):
                    break
                if len(to_explore[delay]) == 0:
                    delay += 1
                    continue
                x,y = to_explore[delay].pop()
                for dx, dy in adjacency:
                    nx, ny = x + dx, y + dy
                    if (nx, ny) in blocked_state or nx < 1 or nx > width or ny < 1 or ny > height:
    #                    print 'blocked'
                        continue
                    #print dist_map[(x,y)], dist_map[(nx,ny)]
                    if (nx, ny) in to_keep:
                        continue
                    dmod = dist_map[(nx,ny)] - dist_map[(x,y)] + 1
                    #print dist_map[(x,y)], dist_map[(nx,ny)], delay + 1, len(to_explore)
                    if delay + dmod >= len(to_explore):
                        continue
                    #elif dist_map[(x,y)] <= dist_map[(nx,ny)]:
                    #    continue
                    #print (nx, ny)
                    to_keep.add((nx,ny))
                    num_in[delay+dmod] += 1
                    to_explore[delay+dmod].append((nx,ny))
            print num_in
            #for x in range(1, width+1):
            #    for y in range(1, height+1):
            #        if (x,y) not in to_keep:
            #            new_blocked.add((x,y))
            orig_locations.add(source)
            print curr_index
            def_keep = []
        else:
            print 'do no left stuff'
            start_location = to
            end_location = None
            new_blocked = blocked_state
            dist_map = None
            orig_blocked = set(orig_locations)
            fringe = set()
            last_place_stopped = {}
            vehicle_position = {}
            for v_index, pos, _ in vehicles.values():
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles.values()])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles.values() if (v_index not in
                                                                                         vehicle_not_moved)
                                or v_index in vehicles_stopped])
            #print 'stopped_vehicles', [(v_index, vehicle_position[v_index]) for v_index, _, _, in vehicles.values() if
            #                           (v_index) in vehicles_stopped]
            #def_keep = [pos for _, pos in last_place_stopped.iteritems()]
            #print def_keep
        #orig_blocked.remove(start_location)
        print start_location, end_location,
        #print curr_index
        #print movements
        #print movements[move_to:]
        res = a_star_search_increment_opt_improved_intersections_mod_stop_pick(height, width, curr_index, start_location,
                                                                     end_location, new_blocked,#blocked_state,
                                                                     dist_map, movements[move_to:],
                                                                     orig_blocked,stopped_vehicles=vehicles_stopped)
        if res == None:
            print 'failed'
            failed = True
            break
        else:
            a, newmovements = res
            #print '1', newmovements
            movements = movements[:move_to] + newmovements
            #print '2', movements
            makespan, movements = compress_trace(movements)
            #print '3', movements
            if do_no_left:
                vehicles_stopped.add(curr_index)
    if failed:
        print 'failed'
        return None, do_no_left
    orig_blocked = set(orig_locations)
    #print movements
    for i in xrange(len(movements)):
        print movements[i]
        (v_index, fr, int, to) = movements[i]
        assert fr in orig_blocked
        orig_blocked.remove(fr)
        if to in orig_blocked:
            print movements[i+1:]
        assert to not in orig_blocked
        orig_blocked.add(to)
    #print makespan
    print 'lowerbound', float(lowerbound)/float(len(vehicles)), 'tot', lowerbound, len(vehicles)
    index_to_pos = {}
    max_span = 0
    for (index, assigned) in task_assigned.iteritems():
        #print fr, to
        span = 0
        _, curposs, _ = vehicle_map[index]
        for task_i in assigned:
            fr, to = task_numbers[task_i]
            dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
            span += dist_map[to]/2
            span += dist_map[curposs]/2
            span += 1
            curposs = to
        max_span = max(span, max_span)
    return makespan, movements, task_assigned, lowerbound, max_span

def prioritized_scheduling_goals_increment_greedy_pick_drop(height, width, blocked_transitions,blocked_state, vehicles, tasks):
    dist_maps = {}
    task_numbers = {}
    for i, task in tasks:
        task_numbers[i] = task
    task_assigned = collections.defaultdict(list)
    orig_vehicle_set = set()
    print 'start heur calc'
    #for _, goals in vehicles:
    #    for goal in goals:
    #        if goal not in  dist_maps:
    #            dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    vehicle_map = {}
    lowerbound = 2 * len(tasks)
    def dist_between((x1, y1), (x2, y2)):
        dx = abs(x1-x2)
        dy = abs(y1-y2)
        return (dx + dy)/2
    for (_, (fr, to)) in tasks:
        #print fr, to
        dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
        lowerbound += dist_map[to]/2
        froms = [spos for _, spos in vehicles] + [tup[1] for (_, tup) in tasks if (fr, to) != tup]
        #print froms
        #print tasks
        lowerbound += min([dist_map[end]/2 for end in froms])
    for i in range(len(vehicles)):
        v_index, spos = vehicles[i]
        #for goal in list(reversed(goals))[1:]:
        #    dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
        #    #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
        #    former_goal = goal
        vehicles[i] = [v_index, spos, None]
        vehicle_map[v_index] = [v_index, spos, None]
        orig_vehicle_set.add(v_index)
    vehicles = vehicle_map
    #os.system('pause')
    dist_maps.clear()
    #LBE = max([dc[0] for _, _, _, dc in vehicles])
    movements = []
    vehicles_stopped = set()
    orig_locations = set()
    tasks_in_prog = set()
    makespan = 0
    for v_index, source, goals in vehicles.values():
        orig_locations.add(source)
    failed = False
    while True:
        if len(tasks) != 0:
            print 'have left'
            vehicle_set = set(orig_vehicle_set)
        elif len(tasks_in_prog) != 0:
            print 'only last moves'
            vehicle_set = set([vid for vid, _, task in vehicles.values() if task != None])
        else:
            print 'no left'
            #print vehicles_stopped
            vehicle_set = set(orig_vehicle_set)
            vehicle_set.difference_update(vehicles_stopped)
            if len(vehicle_set) == 0:
                break
        move_to = 0
        do_no_left = len(tasks) + len(tasks_in_prog) == 0
        #print 'movements', movements
        for move_to in reversed(xrange(len(movements))):
            #print movements[move_to]
            (v_index, fr, int, to) = movements[move_to]
            if v_index in vehicle_set:
                if len(vehicle_set) == 1:
                    move_to += 1
                    break
                vehicle_set.remove(v_index)
        curr_index = random.choice(list(vehicle_set))
        if not do_no_left:
            print 'do this stuff'
            _, source, task = vehicles[curr_index]
            orig_blocked = set(orig_locations)
            fringe = set()
            start_location = source
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index == curr_index:
                    start_location = to
            vehicle_position = {}
            for v_index, pos, _ in vehicles.values():
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles.values()])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles.values() if (v_index not in
                                                                                         vehicle_not_moved)])
            new_blocked = set(blocked_state)
            adjacency = [(0,1),(1,0),(-1,0),(0,-1)]
            to_explore = [[start_location],[],[],[],[]]
            to_keep = set([start_location])
            #dist_map = dist_maps[goals[0]]
            if task == None:
                dist_map = calc_heur(height, width, start_location, blocked_transitions, blocked_state)
                min_task = None
                for i, task in tasks:
                    if min_task == None:
                        min_task = (i, task)
                    elif dist_map[task[0]] < dist_map[min_task[1][0]]:
                        min_task = (i, task)
                end_location = min_task[1][0]
                tasks.remove(min_task)
                tasks_in_prog.add(min_task)
                vehicles[curr_index][2] = min_task
                task_assigned[curr_index].append(min_task[0])
            else:
                end_location = task[1][1]
                vehicles[curr_index][2] = None
                tasks_in_prog.remove(task)
            dist_map = calc_heur(height, width, end_location, blocked_transitions, blocked_state)
            print 'start loc', start_location
            delay = 0
            num_in = [0 for _ in xrange(len(to_explore))]
            while True:
                if delay >= len(to_explore):
                    break
                if len(to_explore[delay]) == 0:
                    delay += 1
                    continue
                x,y = to_explore[delay].pop()
                for dx, dy in adjacency:
                    nx, ny = x + dx, y + dy
                    if (nx, ny) in blocked_state or nx < 1 or nx > width or ny < 1 or ny > height:
    #                    print 'blocked'
                        continue
                    #print dist_map[(x,y)], dist_map[(nx,ny)]
                    if (nx, ny) in to_keep:
                        continue
                    dmod = dist_map[(nx,ny)] - dist_map[(x,y)] + 1
                    #print dist_map[(x,y)], dist_map[(nx,ny)], delay + 1, len(to_explore)
                    if delay + dmod >= len(to_explore):
                        continue
                    #elif dist_map[(x,y)] <= dist_map[(nx,ny)]:
                    #    continue
                    #print (nx, ny)
                    to_keep.add((nx,ny))
                    num_in[delay+dmod] += 1
                    to_explore[delay+dmod].append((nx,ny))
            print num_in
            #for x in range(1, width+1):
            #    for y in range(1, height+1):
            #        if (x,y) not in to_keep:
            #            new_blocked.add((x,y))
            orig_locations.add(source)
            print curr_index
            def_keep = []
        else:
            print 'do no left stuff'
            start_location = to
            end_location = None
            new_blocked = blocked_state
            dist_map = None
            orig_blocked = set(orig_locations)
            fringe = set()
            last_place_stopped = {}
            vehicle_position = {}
            for v_index, pos, _ in vehicles.values():
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles.values()])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles.values() if (v_index not in
                                                                                         vehicle_not_moved)
                                or v_index in vehicles_stopped])
            #print 'stopped_vehicles', [(v_index, vehicle_position[v_index]) for v_index, _, _, in vehicles.values() if
            #                           (v_index) in vehicles_stopped]
            #def_keep = [pos for _, pos in last_place_stopped.iteritems()]
            #print def_keep
        #orig_blocked.remove(start_location)
        #print start_location, end_location,
        #print curr_index
        #print movements
        #print movements[move_to:]
        res = a_star_search_increment_opt_improved_intersections_mod_stop_pick_window_outer(height, width, curr_index, start_location,
                                                                                      end_location, 10, new_blocked,#blocked_state,
                                                                                      dist_map, movements[move_to:],
                                                                                      orig_blocked,stopped_vehicles=vehicles_stopped)
        if res == None:
            print 'failed'
            failed = True
            break
        else:
            a, newmovements = res
            #print '1', newmovements
            movements = movements[:move_to] + newmovements
            #for tup in movements:
            #    if tup[0] == curr_index:
            #        print tup
            #print '2', movements
            makespan, movements = compress_trace(movements)
            #for tup in movements:
            #    if tup[0] == curr_index:
            #        print tup
            #print '3', movements
            if do_no_left:
                vehicles_stopped.add(curr_index)
        if failed:
            print 'failed'
            return None, do_no_left
    if failed:
        print 'failed'
        return None, do_no_left
    orig_blocked = set(orig_locations)
    #print movements
    for i in xrange(len(movements)):
        print movements[i]
        (v_index, fr, int, to) = movements[i]
        assert fr in orig_blocked
        orig_blocked.remove(fr)
        if to in orig_blocked:
            print movements[i+1:]
        assert to not in orig_blocked
        orig_blocked.add(to)
    #print makespan
    print 'lowerbound', float(lowerbound)/float(len(vehicles)), 'tot', lowerbound, len(vehicles)
    index_to_pos = {}
    max_span = 0
    for (index, assigned) in task_assigned.iteritems():
        #print fr, to
        span = 0
        _, curposs, _ = vehicle_map[index]
        for task_i in assigned:
            fr, to = task_numbers[task_i]
            dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
            span += dist_map[to]/2
            span += dist_map[curposs]/2
            span += 1
            curposs = to
        max_span = max(span, max_span)
    return makespan, movements, task_assigned, lowerbound, max_span

def prioritized_scheduling_goals_increment_greedy_pick_drop_retry(height, width, blocked_transitions,blocked_state, vehicles, tasks):
    RETRIES = 10
    retry = 0
    dist_maps = {}
    task_numbers = {}
    for i, task in tasks:
        task_numbers[i] = task
    task_assigned = collections.defaultdict(list)
    orig_vehicle_set = set()
    print 'start heur calc'
    #for _, goals in vehicles:
    #    for goal in goals:
    #        if goal not in  dist_maps:
    #            dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    vehicle_map = {}
    lowerbound = 2 * len(tasks)
    def dist_between((x1, y1), (x2, y2)):
        dx = abs(x1-x2)
        dy = abs(y1-y2)
        return (dx + dy)/2
    for (_, (fr, to)) in tasks:
        #print fr, to
        dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
        lowerbound += dist_map[to]/2
        froms = [spos for _, spos in vehicles] + [tup[1] for (_, tup) in tasks if (fr, to) != tup]
        #print froms
        #print tasks
        lowerbound += min([dist_map[end]/2 for end in froms])
    for i in range(len(vehicles)):
        v_index, spos = vehicles[i]
        #for goal in list(reversed(goals))[1:]:
        #    dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
        #    #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
        #    former_goal = goal
        vehicles[i] = [v_index, spos, None]
        vehicle_map[v_index] = [v_index, spos, None]
        orig_vehicle_set.add(v_index)
    current_dest = {}
    vehicles = vehicle_map
    #os.system('pause')
    dist_maps.clear()
    #LBE = max([dc[0] for _, _, _, dc in vehicles])
    movements = []
    vehicles_stopped = set()
    orig_locations = set()
    tasks_in_prog = set()
    makespan = 0
    for v_index, source, goals in vehicles.values():
        orig_locations.add(source)
    failed = False
    reroute = set()
    while True:
        if len(tasks) != 0:
            print 'have left'
            vehicle_set = set(orig_vehicle_set)
        elif len(tasks_in_prog) != 0:
            print 'only last moves'
            vehicle_set = set([vid for vid, _, task in vehicles.values() if task != None])
        else:
            print 'no left'
            #print vehicles_stopped
            vehicle_set = set(orig_vehicle_set)
            vehicle_set.difference_update(vehicles_stopped)
            if len(vehicle_set) == 0:
                break
        move_to = 0
        do_no_left = len(tasks) + len(tasks_in_prog) == 0
        #print 'movements', movements
        for move_to in reversed(xrange(len(movements))):
            #print movements[move_to]
            (v_index, fr, int, to) = movements[move_to]
            if v_index in vehicle_set:
                if len(vehicle_set) == 1:
                    move_to += 1
                    break
                vehicle_set.remove(v_index)
        curr_index = random.choice(list(vehicle_set))
        stopped_vehicles = set()
        if not do_no_left:
            print 'do this stuff'
            _, source, task = vehicles[curr_index]
            orig_blocked = set(orig_locations)
            fringe = set()
            start_location = source
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index == curr_index:
                    start_location = to
            vehicle_position = {}
            for v_index, pos, _ in vehicles.values():
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles.values()])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles.values() if (v_index not in
                                                                                         vehicle_not_moved)])
            new_blocked = set(blocked_state)
            adjacency = [(0,1),(1,0),(-1,0),(0,-1)]
            to_explore = [[start_location],[],[],[],[]]
            to_keep = set([start_location])
            #dist_map = dist_maps[goals[0]]
            if curr_index in reroute:
                end_location = current_dest[curr_index]
                reroute.remove(curr_index)
            elif task == None:
                dist_map = calc_heur(height, width, start_location, blocked_transitions, blocked_state)
                min_task = None
                for i, task in tasks:
                    if min_task == None:
                        min_task = (i, task)
                    elif dist_map[task[0]] < dist_map[min_task[1][0]]:
                        min_task = (i, task)
                end_location = min_task[1][0]
                tasks.remove(min_task)
                tasks_in_prog.add(min_task)
                vehicles[curr_index][2] = min_task
                task_assigned[curr_index].append(min_task[0])
            else:
                end_location = task[1][1]
                vehicles[curr_index][2] = None
                tasks_in_prog.remove(task)
            dist_map = calc_heur(height, width, end_location, blocked_transitions, blocked_state)
            print 'start loc', start_location
            delay = 0
            num_in = [0 for _ in xrange(len(to_explore))]
            while True:
                if delay >= len(to_explore):
                    break
                if len(to_explore[delay]) == 0:
                    delay += 1
                    continue
                x,y = to_explore[delay].pop()
                for dx, dy in adjacency:
                    nx, ny = x + dx, y + dy
                    if (nx, ny) in blocked_state or nx < 1 or nx > width or ny < 1 or ny > height:
    #                    print 'blocked'
                        continue
                    #print dist_map[(x,y)], dist_map[(nx,ny)]
                    if (nx, ny) in to_keep:
                        continue
                    dmod = dist_map[(nx,ny)] - dist_map[(x,y)] + 1
                    #print dist_map[(x,y)], dist_map[(nx,ny)], delay + 1, len(to_explore)
                    if delay + dmod >= len(to_explore):
                        continue
                    #elif dist_map[(x,y)] <= dist_map[(nx,ny)]:
                    #    continue
                    #print (nx, ny)
                    to_keep.add((nx,ny))
                    num_in[delay+dmod] += 1
                    to_explore[delay+dmod].append((nx,ny))
            print num_in
            #for x in range(1, width+1):
            #    for y in range(1, height+1):
            #        if (x,y) not in to_keep:
            #            new_blocked.add((x,y))
            orig_locations.add(source)
            print curr_index
            def_keep = []
        else:
            print 'do no left stuff'
            start_location = to
            end_location = None
            new_blocked = blocked_state
            dist_map = None
            orig_blocked = set(orig_locations)
            fringe = set()
            last_place_stopped = {}
            vehicle_position = {}
            for v_index, pos, _ in vehicles.values():
                vehicle_position[v_index] = pos
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                vehicle_position[v_index] = to
                #if fr in fringe or int in fringe or to in fringe:
                #    fringe.clear()
                #fringe.update([fr, int, to])
                #if fr not in orig_blocked:
                #    print i
                #orig_blocked.remove(fr)
                #orig_blocked.add(to)
                #if v_index in vehicles_stopped:
                #    last_place_stopped[v_index] = to
            vehicle_not_moved = set([v_index for v_index, _, _ in vehicles.values()])
            vehicle_not_moved.difference_update([v_index for v_index, _, _, _ in movements[move_to:]])
            stopped_vehicles = vehicles_stopped.difference(reroute)
            orig_blocked = set([vehicle_position[v_index] for v_index, _, _ in vehicles.values() if (v_index not in
                                                                                         vehicle_not_moved)
                                or v_index in stopped_vehicles])
            if curr_index in reroute:
                reroute.remove(curr_index)
                end_location = current_dest[curr_index]
                if end_location != None:
                    dist_map = calc_heur(height, width, end_location, blocked_transitions, blocked_state)
            #print 'stopped_vehicles', [(v_index, vehicle_position[v_index]) for v_index, _, _, in vehicles.values() if
            #                           (v_index) in vehicles_stopped]
            #def_keep = [pos for _, pos in last_place_stopped.iteritems()]
            #print def_keep
        #orig_blocked.remove(start_location)
        print start_location, end_location,
        #print curr_index
        #print movements
        #print movements[move_to:]
        current_dest[curr_index] = end_location
        res = a_star_search_increment_opt_improved_intersections_mod_stop_pick(height, width, curr_index, start_location,
                                                                     end_location, new_blocked,#blocked_state,
                                                                     dist_map, movements[move_to:],
                                                                     orig_blocked,stopped_vehicles=stopped_vehicles)
        if res == None:
            retry += 1
            if retry >= RETRIES:
                print 'failed'
                failed = True
                break
            else:
                reroute.add(curr_index)
                reroute.update([v_index for v_index, _, _, _ in movements[move_to:]])
                movements = list(movements[:move_to])
        else:
            a, newmovements = res
            #print '1', newmovements
            movements = movements[:move_to] + newmovements
            #print '2', movements
            makespan, movements = compress_trace(movements)
            #print '3', movements
            if do_no_left:
                vehicles_stopped.add(curr_index)
    if failed:
        print 'failed'
        return None, do_no_left
    orig_blocked = set(orig_locations)
    #print movements
    for i in xrange(len(movements)):
        print movements[i]
        (v_index, fr, int, to) = movements[i]
        assert fr in orig_blocked
        orig_blocked.remove(fr)
        if to in orig_blocked:
            print movements[i+1:]
        assert to not in orig_blocked
        orig_blocked.add(to)
    #print makespan
    print 'lowerbound', float(lowerbound)/float(len(vehicles)), 'tot', lowerbound, len(vehicles)
    index_to_pos = {}
    max_span = 0
    for (index, assigned) in task_assigned.iteritems():
        #print fr, to
        span = 0
        _, curposs, _ = vehicle_map[index]
        for task_i in assigned:
            fr, to = task_numbers[task_i]
            dist_map = calc_heur(height, width, fr, blocked_transitions, blocked_state)
            span += dist_map[to]/2
            span += dist_map[curposs]/2
            span += 1
            curposs = to
        max_span = max(span, max_span)
    return makespan, movements, task_assigned, lowerbound, max_span, retry

def prioritized_scheduling_goals_increment_greedy_mult_target(height, width, blocked_transitions,blocked_state, vehicles, tasks):
    dist_maps = {}
    print 'start heur calc'
    #for _, goals in vehicles:
    #    for goal in goals:
    #        if goal not in  dist_maps:
    #            dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    for i in range(len(vehicles)):
        spos = vehicles[i]
        #for goal in list(reversed(goals))[1:]:
        #    dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
        #    #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
        #    former_goal = goal
        vehicles[i] = [i, spos, None]
    #os.system('pause')
    dist_maps.clear()
    #LBE = max([dc[0] for _, _, _, dc in vehicles])
    movements = []
    vehicles_stopped = set()
    orig_locations = set()
    tasks_in_prog = set()
    for j, (v_index, source, goals) in enumerate(vehicles):
        orig_locations.add(source)
    failed = False
    makespan = 0
    while True:
        use_mult = False
        if len(tasks) != 0:
            print 'have left'
            vehicle_set = set(range(len(vehicles)))
        elif len(tasks_in_prog) != 0:
            print 'only last moves'
            vehicle_set = set([vid for vid, _, task in vehicles if task != None])
        else:
            print 'no left'
            #print vehicles_stopped
            vehicle_set = set(range(len(vehicles)))
            vehicle_set.difference_update(vehicles_stopped)
            if len(vehicle_set) == 0:
                break
        move_to = 0
        #print 'movements', movements
        for move_to in reversed(xrange(len(movements))):
            #print movements[move_to]
            (v_index, fr, int, to) = movements[move_to]
            if v_index in vehicle_set:
                if len(vehicle_set) == 1:
                    move_to += 1
                    break
                vehicle_set.remove(v_index)
        curr_index = random.choice(list(vehicle_set))
        if len(tasks) + len(tasks_in_prog) != 0:
            _, source, task = vehicles[curr_index]
            orig_blocked = set(orig_locations)
            fringe = set()
            start_location = source
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index == curr_index:
                    start_location = to
            new_blocked = set(blocked_state)
            adjacency = [(0,1),(1,0),(-1,0),(0,-1)]
            to_explore = [[start_location],[],[],[],[]]
            to_keep = set([start_location])
            #dist_map = dist_maps[goals[0]]
            if task == None:
                dist_map = calc_heur(height, width, [fr for fr, _ in tasks], blocked_transitions, blocked_state)
                end_location = [fr for fr, _ in tasks]
                use_mult = True
            else:
                end_location = task[1]
                vehicles[curr_index][2] = None
                tasks_in_prog.remove(task)
                dist_map = calc_heur(height, width, end_location, blocked_transitions, blocked_state)
            delay = 0
            #for x in range(1, width+1):
            #    for y in range(1, height+1):
            #        if (x,y) not in to_keep:
            #            new_blocked.add((x,y))
            orig_locations.add(source)
            print curr_index
            def_keep = []
        else:
            start_location = to
            end_location = None
            new_blocked = blocked_state
            dist_map == None
            orig_blocked = set(orig_locations)
            fringe = set()
            last_place_stopped = {}
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                if fr not in orig_blocked:
                    print i
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index in vehicles_stopped:
                    last_place_stopped[v_index] = to
            def_keep = [pos for _, pos in last_place_stopped.iteritems()]
            #print def_keep
        print 'sl', start_location, 'el', end_location
        orig_blocked.remove(start_location)
        if start_location != end_location:
            if not use_mult:
                res = a_star_search_increment_opt_improved_intersections_mod(height, width, curr_index, start_location,
                                                                             end_location, new_blocked,#blocked_state,
                                                                             dist_map, movements[move_to:],
                                                                             orig_blocked,stopped_vehicles=vehicles_stopped,
                                                                             def_keep=def_keep)
            else:
                if start_location not in [fr for (fr, to) in tasks]:
                    a, moves, endloc = \
                        a_star_search_increment_opt_improved_intersections_mod_multtarget\
                            (height, width, curr_index, start_location,
                                end_location, new_blocked,#blocked_state,
                                dist_map, movements[move_to:],
                                orig_blocked,stopped_vehicles=vehicles_stopped,
                                def_keep=def_keep)
                else:
                    a, moves, endloc = makespan, movements[move_to:], start_location
                task = [(fr, to) for (fr, to) in tasks if fr == endloc][0]
                tasks.remove(task)
                tasks_in_prog.add(task)
                vehicles[curr_index][2] = task
                res = (a, moves)
        else:
            res = (makespan, movements[move_to:])
        if res == None:
            print 'failed'
            failed = True
            break
        else:
            a, newmovements = res
            #print '1', newmovements
            movements = movements[:move_to] + newmovements
            #print '2', movements
            makespan, movements = compress_trace(movements)
            #print '3', movements
            if len(tasks) + len(tasks_in_prog) == 0:
                vehicles_stopped.add(curr_index)
            makespan = a
    if failed:
        print 'failed'
        return None
    orig_blocked = set(orig_locations)
    #print movements
    for i in xrange(len(movements)):
        (v_index, fr, int, to) = movements[i]
        assert fr in orig_blocked
        orig_blocked.remove(fr)
        if to in orig_blocked:
            print movements[i+1:]
        assert to not in orig_blocked
        orig_blocked.add(to)
    #print makespan
    return makespan, movements

def prioritized_scheduling_goals_increment(height, width, blocked_transitions,blocked_state, vehicles, duration = -1):
    sttime = -time.clock()
    dist_maps = {}
    print 'start heur calc'
    #for _, goals in vehicles:
    #    for goal in goals:
    #        if goal not in  dist_maps:
    #            dist_maps[goal] = calc_heur(height, width, goal, blocked_transitions, blocked_state)
    print 'end heur calc'
    vehicles = list(vehicles)
    num_tasks = []
    for i in range(len(vehicles)):
        spos, goals = vehicles[i]
        #for goal in list(reversed(goals))[1:]:
        #    dist_cache.append(dist_maps[goal][former_goal] + dist_cache[-1])
        #    #print '{}-{}-{}-{}'.format(goal, former_goal, dist_maps[goal][former_goal], dist_cache[-1])
        #    former_goal = goal
        vehicles[i] = (i, spos, goals)
        num_tasks.append(len(goals))
    print num_tasks
    os.system('pause')
    dist_maps.clear()
    #LBE = max([dc[0] for _, _, _, dc in vehicles])
    movements = []
    vehicles_stopped = set()
    goalsleft = {}
    orig_locations = set()
    for j, (v_index, source, goals) in enumerate(vehicles):
        goalsleft[v_index] = (source, goals)
        orig_locations.add(source)
    failed = False
    while True:
        if len(goalsleft.keys()) != 0:
            print 'have left'
            vehicle_set = set(goalsleft.keys())
        else:
            print 'no left'
            print vehicles_stopped
            vehicle_set = set(range(len(vehicles)))
            vehicle_set.difference_update(vehicles_stopped)
            if len(vehicle_set) == 0:
                break
        move_to = 0
        #print 'movements', movements
        for move_to in reversed(xrange(len(movements))):
            #print movements[move_to]
            (v_index, fr, int, to) = movements[move_to]
            if v_index in vehicle_set:
                if len(vehicle_set) == 1:
                    move_to += 1
                    break
                vehicle_set.remove(v_index)
        curr_index = random.choice(list(vehicle_set))
        if len(goalsleft.keys()) != 0:
            source, goals = goalsleft[curr_index]
            if source == goals[0]:
                if len(goals) != 1:
                    goalsleft[curr_index] = (source, goals[1:])
                else:
                    goalsleft.pop(curr_index)
                continue
            orig_blocked = set(orig_locations)
            fringe = set()
            start_location = source
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index == curr_index:
                    start_location = to
            new_blocked = set(blocked_state)
            adjacency = [(0,1),(1,0),(-1,0),(0,-1)]
            to_explore = [[start_location],[],[],[],[]]
            to_keep = set([start_location])
            #dist_map = dist_maps[goals[0]]
            dist_map = calc_heur(height, width, goals[0], blocked_transitions, blocked_state)
            print 'goal', goalsleft[curr_index]
            print 'start loc', start_location
            delay = 0
            num_in = [0 for _ in xrange(len(to_explore))]
            while True:
                if delay >= len(to_explore):
                    break
                if len(to_explore[delay]) == 0:
                    delay += 1
                    continue
                x,y = to_explore[delay].pop()
                for dx, dy in adjacency:
                    nx, ny = x + dx, y + dy
                    if (nx, ny) in blocked_state or nx < 1 or nx > width or ny < 1 or ny > height:
    #                    print 'blocked'
                        continue
                    #print dist_map[(x,y)], dist_map[(nx,ny)]
                    if (nx, ny) in to_keep:
                        continue
                    dmod = dist_map[(nx,ny)] - dist_map[(x,y)] + 1
                    #print dist_map[(x,y)], dist_map[(nx,ny)], delay + 1, len(to_explore)
                    if delay + dmod >= len(to_explore):
                        continue
                    #elif dist_map[(x,y)] <= dist_map[(nx,ny)]:
                    #    continue
                    #print (nx, ny)
                    to_keep.add((nx,ny))
                    num_in[delay+dmod] += 1
                    to_explore[delay+dmod].append((nx,ny))
            print num_in
            #for x in range(1, width+1):
            #    for y in range(1, height+1):
            #        if (x,y) not in to_keep:
            #            new_blocked.add((x,y))
            orig_locations.add(source)
            end_location = goals[0]
            print curr_index
            def_keep = []
        else:
            start_location = to
            end_location = None
            new_blocked = blocked_state
            dist_map == None
            orig_blocked = set(orig_locations)
            fringe = set()
            last_place_stopped = {}
            for i in xrange(move_to):
                (v_index, fr, int, to) = movements[i]
                if fr in fringe or int in fringe or to in fringe:
                    fringe.clear()
                fringe.update([fr, int, to])
                if fr not in orig_blocked:
                    print i
                orig_blocked.remove(fr)
                orig_blocked.add(to)
                if v_index in vehicles_stopped:
                    last_place_stopped[v_index] = to
            def_keep = [pos for _, pos in last_place_stopped.iteritems()]
            print def_keep
        res = a_star_search_increment_opt_improved_intersections_mod(height, width, curr_index, start_location,
                                                                     end_location, new_blocked,#blocked_state,
                                                                     dist_map, movements[move_to:],
                                                                     orig_blocked,stopped_vehicles=vehicles_stopped,
                                                                     def_keep=def_keep)
        if res == None:
            print 'failed'
            failed = True
            break
        else:
            a, newmovements = res
            #print '1', newmovements
            movements = movements[:move_to] + newmovements
            #print '2', movements
            makespan, movements = compress_trace(movements)
            #print '3', movements
            if len(goalsleft.keys()) == 0:
                vehicles_stopped.add(curr_index)
            else:
                if len(goals) != 1:
                    goalsleft[curr_index] = (source, goals[1:])
                else:
                    goalsleft.pop(curr_index)
            makespan = a
    if failed:
        print 'failed'
        return None
    orig_blocked = set(orig_locations)
    #print movements
    for i in xrange(len(movements)):
        (v_index, fr, int, to) = movements[i]
        assert fr in orig_blocked
        orig_blocked.remove(fr)
        if to in orig_blocked:
            print movements[i+1:]
        assert to not in orig_blocked
        orig_blocked.add(to)
    #print makespan
    return makespan, movements

def blocked_states_from_string(strings):
    height = len(strings)
    width = len(strings[0])
    blocked_states = set()
    for y, line in enumerate(strings):
        for x, char in enumerate(line):
            if char != ' ':
                blocked_states.add((x+1,y+1))
    return height, width, blocked_states

def create_grid(width):
    grid = []
    slots = 0
    for x in range(width):
        grid.append([])
        for y in range(width):
            #grid[x].append(' ')
            #slots += 1
            grid[x].append('W' if x % 2 == 1 and y % 2 == 1 else ' ')
            if (x % 2 != 1 or y % 2 != 1):
                slots += 1
    return grid, slots

def grid_complex(epuck_goals, width, duration = -1):
    grid, slots = create_grid(width)
    height, width, blocked_states = \
        blocked_states_from_string(grid)
    vehicles = []
    def convert_pos_tuple(pos):
        level = ((pos - 1) / width)
        highergrain = (pos - 1)% width
        if highergrain < (width/2):
            return ((highergrain + 1) * 2, level * 2 + 1)
        else:
            highergrain -= width/2
            return ((highergrain*2)+1,(level*2)+2)
    def convert_pos_single(pos):
        x, y = pos
        level = (y-1) / 2
        p = level * width
        if (y-1) % 2 == 1:
            p += (width / 2)+1
        p += x / 2
        return p
    for startpos, goals, in epuck_goals:
        assert len(goals)>0
        vehicles.append((convert_pos_tuple(startpos), map(convert_pos_tuple, goals)))
        prevpos = vehicles[-1][1][-1]
        for i in reversed(range(len(vehicles[-1][1])-1)):
            pos = vehicles[-1][1][i]
            if pos == prevpos:
                print 'popped'
                vehicles[-1][1].pop(i)
            prevpos = pos
    #blocked_states.update(map(convert_pos_tuple, [35,36,39,40]))
    mspan, moves = prioritized_scheduling_goals_increment(height, width, set(), blocked_states, vehicles, duration)
    if moves == None:
        io =did
    #if moves != None:
    #    moves = map(lambda l: map(lambda (p1, p2): (convert_pos_single(p1), convert_pos_single(p2)), l), moves)
    return mspan, moves

def grid_complex_greedy(vehicles, tasks, width = -1, duration = -1, resfile = None):
    print resfile
    if width == -1:
        grid, slots = create_grid(9)
    else:
        grid, slots = create_grid(width)
    height, _, blocked_states = \
        blocked_states_from_string(grid)
    def convert_pos_tuple(pos):
        w = width
        if width == -1:
            w = 9
        if width == -1 and pos >= 35:
            pos += 2
            w = 9
        level = ((pos - 1) / w)
        highergrain = (pos - 1)% w
        if highergrain < (w/2):
            return ((highergrain + 1) * 2, level * 2 + 1)
        else:
            highergrain -= w/2
            return ((highergrain*2)+1,(level*2)+2)
    def convert_pos_single(pos):
        w = width if width != -1 else 9
        x, y = pos
        level = (y-1) / 2
        p = level * w
        if (y-1) % 2 == 1:
            p += (w / 2)+1
        p += x / 2
        if p >= 35 and width == -1:
            p -= 2
        return p
    #print vehicles
    vehicles = [(v_i, convert_pos_tuple(pos)) for v_i, pos in vehicles]#map(convert_pos_tuple, vehicles)
    #print vehicles
    #print tasks
    tasks = set([(i, (convert_pos_tuple(fr), convert_pos_tuple(to))) for (i, (fr, to)) in enumerate(tasks)])
    #print tasks
    #print tasks
    if width == -1:
        blocked_states.update([(7,8),(9,8),(6,9),(8,9)])#map(convert_pos_tuple, [35,36,39,40]))
    #mspan, moves = prioritized_scheduling_goals_increment(height, width, set(), blocked_states, vehicles, duration)
    #mspan, moves = prioritized_scheduling_goals_increment_greedy(height, width, set(), blocked_states, vehicles, tasks)
    calctime = -time.clock()
    res = prioritized_scheduling_goals_increment_greedy_pick_drop(
        height, width if width != -1 else 9, set(), blocked_states, vehicles, tasks)
    if res[0] == None:
        if resfile != None:
            resfile.write('{}:{}:fail:{}\n'.format(width,len(vehicles),res[1]))
        return None
    calctime += time.clock()
    #print res
    mspan, moves, assigned_tasks, lowerbound, lowerbound2 = res
    if resfile != None:
        resfile.write('{}:{}:{}:{}:{}:{}\n'.format(width, len(vehicles), mspan, float(lowerbound)/float(len(vehicles)), lowerbound2, calctime))
    resfile.flush()
    pickup = set()
    for key in assigned_tasks:
        assigned_tasks[key].reverse()
    if moves != None:
        print moves
        moves = [(l, convert_pos_single(x), convert_pos_single(y)) for l, x, _, y in moves]
        mevents = []
        for v_index, x, y in moves:
            print v_index, x, y
            if x != y:
                mevents.append('P{}-{}-{}'.format(v_index, str(x).zfill(2), str(y).zfill(2)))
            else:
                if v_index in pickup:
                    mevents.append('P{}-DROP-{}'.format(v_index, assigned_tasks[v_index].pop()))
                    pickup.remove(v_index)
                else:
                    mevents.append('P{}-PICK-{}'.format(v_index, assigned_tasks[v_index][-1]))
                    pickup.add(v_index)
    handle = file('epuckevents.txt', 'w')
    handle.write('{}\n'.format(mspan))
    handle.write(','.join(mevents))
    handle.close()
    #os.system('pause')
        #moves = map(lambda l: map(lambda (p1, p2): (convert_pos_single(p1), convert_pos_single(p2)), l), moves)
    return mspan, moves

def grid(epucks, width, duration = -1):
    grid, slots = create_grid(width)
    height, width, blocked_states = \
        blocked_states_from_string(grid)
    epucks *= slots
    epucks = int(epucks)
    possible_sources = [(x,y) for x in range(1,width+1) for y in range(1, height+1) if (x,y) not in
                        blocked_states]
    sources = random.sample(possible_sources, epucks)
    targets = random.sample(possible_sources, epucks)
    vehicles = [(sources[i], targets[i]) for i in range(len(sources))]
    return prioritized_scheduling(height, width, set(), blocked_states, vehicles, duration)

def closed_corridor(epucks, restricted = False, duration = -1):
    height, width, blocked_states = \
        blocked_states_from_string(['WW WW WW WW',
                                    'WW WW WW WW',
                                    'WW WW WW WW',
                                    '           ',
                                    '           ',
                                    'WW WW WW WW',
                                    'WW WW WW WW',
                                    'WW WW WW WW'])
    possible_sources = [(x,y) for x in range(1,width+1) for y in range(1, height+1) if (x,y) not in
                        blocked_states and (not restricted or (y != 4 and y != 5))]
    sources = random.sample(possible_sources, epucks)
    targets = random.sample(possible_sources, epucks)
    vehicles = [(sources[i], targets[i]) for i in range(len(sources))]
    return prioritized_scheduling(height, width, set(), blocked_states, vehicles, duration)

def cyclic_corridor(epucks, duration = -1):
    rooms = 4
    width = rooms * 3 + 4
    height = 8
    blocked_transitions = []
    blocked_transitions.extend([((x, y), (x, y+1)) for y in [2, 6] for x in range(3, width-1) if x % 3 != 1])
    blocked_transitions.extend([((x, y), (x, y+1)) for y in [4] for x in range(3, width-1)])
    blocked_transitions.extend([((x, y), (x+1, y)) for y in range(3,7) for x in range(2, width,3)])
    blocked_transitions = set(blocked_transitions)
    possible_sources = [(x,y) for x in range(1,width+1) for y in range(1, height+1)]
    sources = random.sample(possible_sources, epucks)
    targets = random.sample(possible_sources, epucks)
    vehicles = [(sources[i], targets[i]) for i in range(len(sources))]
    return prioritized_scheduling(height, width, blocked_transitions, set(), vehicles, duration)

def tight_cyclic_corridor(epucks, duration = -1):
    rooms = 4
    width = rooms * 3 + 2
    height = 6
    blocked_transitions = []
    blocked_transitions.extend([((x, y), (x, y+1)) for y in [1, 5] for x in range(2, width) if x % 3 != 0])
    blocked_transitions.extend([((x, y), (x, y+1)) for y in [3] for x in range(2, width)])
    blocked_transitions.extend([((x, y), (x+1, y)) for y in range(2,6) for x in range(1, width,3)])
    blocked_transitions = set(blocked_transitions)
    #for y in range(1, height +1):
    #    line = ''
    #    for x in range(1, width +1):
    #        if ((x,y), (x+1,y)) in blocked_transitions:
    #            line += '|'
    #        else:
    #            line += ' '
    #    print line
    #for y in range(1, height +1):
    #    line = ''
    #    for x in range(1, width +1):
    #        if ((x,y), (x,y+1)) in blocked_transitions:
    #            line += '-'
    #        else:
    #            line += ' '
    #    print line
    #ei = due
    possible_sources = [(x,y) for x in range(1,width+1) for y in range(1, height+1)]
    sources = random.sample(possible_sources, epucks)
    targets = random.sample(possible_sources, epucks)
    vehicles = [(sources[i], targets[i]) for i in range(len(sources))]
    return prioritized_scheduling(height, width, blocked_transitions, set(), vehicles, duration)