__author__ = 'sware'

import math, random, time, heapq

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 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):
    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]
    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]]
        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]]
    return max(dist_moved)

def simulate_annealing(moveorders, startpositions, assigned_task, func, width = 9):
    temperature = 30/math.log1p(0.8)
    stime = time.clock()
    stoptemp = .0001
    maxIT = 10000.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:
        if pos not in dist_maps:
            dist_maps[pos] = calculate_distance_map_variable(pos, width)
    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 = 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))
            #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 = 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 = 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):
    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(height, width, source, target, blocked_transitions,blocked_state,heur, movements, orig_blocked):
    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:
        return None
    arrival = calced_heur
    for i_step, pair in reversed(inserts):
        movements.insert(i_step, pair)
    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, goal, dist_map, dist_cache, 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 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 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:
        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(height, width, set(), blocked_states, vehicles, duration)
    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(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)