# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: env_jonghyeok
#     language: python
#     name: env_jonghyeok
# ---

# ## Expand sequentialy

# +
# ----------
# User Instructions:
# 
# Define a function, search() that returns a list
# in the form of [optimal path length, row, col]. For
# the grid shown below, your function should output
# [11, 4, 5].
#
# If there is no valid path from the start point
# to the goal, your function should return the string
# 'fail'
# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space


grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 1, 0]]
        

init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0], # go up
         [ 0,-1], # go left
         [ 1, 0], # go down
         [ 0, 1]] # go right

delta_name = ['^', '<', 'v', '>']

def search(grid, init, goal, cost):
    # ----------------------------------------
    # insert code here
    # ----------------------------------------
    
    print('---grid init---')
    for i in range(len(grid)):
        print(grid[i])

    
    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    closed[init[0]][init[1]] = 1
    
    print('---closed init---')
    for i in range(len(closed)):
        print(closed[i])
        
        
    expand = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    expand[init[0]][init[1]] = 0
    print('---expand init---')
    for i in range(len(expand)):
        print(expand[i])

    
    value_g = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    value_g[init[0]][init[1]] = 0
    print('---value_g init---')
    for i in range(len(value_g)):
        print(value_g[i])
    
    
    
    action = [[9 for row in range(len(grid[0]))] for col in range(len(grid))]
    action[init[0]][init[1]] = 0
    print('---action init---')
    for i in range(len(action)):
        print(action[i])

    
    
    x = init[0]
    y = init[1]
    g = 0
    
    open = [[g,x,y]]
    
    found = False
    resign = False
    count = 0
    
    
    while found is False and resign is False:
        
        if len(open) == 0:
            resign = True
            path = 'fail'
            print('---expand final---')
            for i in range(len(expand)):
                print(expand[i])

            print('---closed final---')
            print('path : ', path)
            for i in range(len(closed)):
                print(closed[i])

            print('---value_g final---')
            for i in range(len(value_g)):
                print(value_g[i])

            print('---action final---')
            for i in range(len(action)):
                print(action[i])
            print('--------- Search fail ---------')
            
            
        else:
            print('---1. open list ---')
            for i in range(len(open)):
                print('   ', open[i]) 
                
                
            open.sort()
            open.reverse()
            next = open.pop()
            
            x = next[1]
            y = next[2]
            g = next[0]
            expand[x][y] = count
            count += 1
            
            print('---2. picked list---')
            print('   ',[g,x,y])

            
            if x == goal[0] and y == goal[1]:
                found = True                
                path = [g,x,y]
                
                print('---expand final---')
                for i in range(len(expand)):
                    print(expand[i])
                
                print('---closed final---')
                print('path : ', path)
                for i in range(len(closed)):
                    print(closed[i])
                    
                print('---value_g final---')
                for i in range(len(value_g)):
                    print(value_g[i])
                
                print('---action final---')
                for i in range(len(action)):
                    print(action[i])
    
                print('--------- Search successful ---------')
                
                
            else:
                
                
                    
                    
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2< len(grid[0]):
                        if closed[x2][y2] == 0 and grid[x2][y2] ==0:
                            g2 = g + cost
                            open.append([g2,x2,y2])
#                             print('---3. append list item---')
#                             print('   ',[g2,x2,y2])
                            
                            closed[x2][y2] = 1
                            value_g[x2][y2] = g2
                            action[x2][y2] = i
                
                
#                 print('---4.1 grid---')
#                 for i in range(len(grid)):
#                     print(grid[i])
                    
                    
#                 print('---4.2 expand---')
#                 for i in range(len(expand)):
#                     print(expand[i])         
                
#                 print('---4.3 closed---')
#                 for i in range(len(closed)):
#                     print(closed[i])
                    
#                 print('---4.4 value_g---')
#                 for i in range(len(value_g)):
#                     print(value_g[i])
                
#                 print('---4.5 action---')
#                 for i in range(len(action)):
#                     print(action[i])

    policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    x = goal[0]
    y = goal[1]
    policy[x][y] = '*'
    
    while x != init[0] or y != init[1]:
        x2 = x -delta[action[x][y]][0]
        y2 = y -delta[action[x][y]][1]
        
        policy[x2][y2] = delta_name[action[x][y]]
        x = x2
        y = y2
        
    for i in range(len(policy)):
        print(policy[i])
    
    
    return path


search(grid, init, goal, cost)
# -

# ## A* algorithm

# +
# -----------
# User Instructions:
#
# Modify the the search function so that it becomes
# an A* search algorithm as defined in the previous
# lectures.
#
# Your function should return the expanded grid
# which shows, for each element, the count when
# it was expanded or -1 if the element was never expanded.
# 
# If there is no path from init to goal,
# the function should return the string 'fail'
# ----------

grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 1, 0]]
heuristic = [[9, 8, 7, 6, 5, 4],
             [8, 7, 6, 5, 4, 3],
             [7, 6, 5, 4, 3, 2],
             [6, 5, 4, 3, 2, 1],
             [5, 4, 3, 2, 1, 0]]

init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0 ], # go up
         [ 0, -1], # go left
         [ 1, 0 ], # go down
         [ 0, 1 ]] # go right

delta_name = ['^', '<', 'v', '>']

def search(grid,init,goal,cost,heuristic):
    # ----------------------------------------
    # modify the code below
    # ----------------------------------------
   
    print('---grid init---')
    for i in range(len(grid)):
        print(grid[i])

    
    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    closed[init[0]][init[1]] = 1
    
    print('---closed init---')
    for i in range(len(closed)):
        print(closed[i])
        
        
    expand = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    expand[init[0]][init[1]] = 0
    print('---expand init---')
    for i in range(len(expand)):
        print(expand[i])

    
    value_g = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    value_g[init[0]][init[1]] = 0
    print('---value_g init---')
    for i in range(len(value_g)):
        print(value_g[i])
    
    action = [[9 for row in range(len(grid[0]))] for col in range(len(grid))]
    action[init[0]][init[1]] = 0
    print('---action init---')
    for i in range(len(action)):
        print(action[i])
    
    x = init[0]
    y = init[1]
    g = 0
    h = heuristic[x][y]
    f = g + h
    

    open = [[f, g, h, x, y]]

    found = False  # flag that is set when search is complete
    resign = False # flag set if we can't find expand
    count = 0
    
    while not found and not resign:
        if len(open) == 0:
            resign = True
            return "Fail"
        
            print('---expand final---')
            for i in range(len(expand)):
                print(expand[i])

            print('---closed final---')
            print('path : ', path)
            for i in range(len(closed)):
                print(closed[i])

            print('---value_g final---')
            for i in range(len(value_g)):
                print(value_g[i])

            print('---action final---')
            for i in range(len(action)):
                print(action[i])
            print('--------- Search fail ---------')
            
        else:
            
            print('---1. open list ---')
            for i in range(len(open)):
                print('   ', open[i]) 
                
            open.sort()
            open.reverse()
            next = open.pop()
            x = next[3]
            y = next[4]
            g = next[1]
            expand[x][y] = count
            count += 1
            
            print('---2. picked list---')
            print('   ',next)
            
            if x == goal[0] and y == goal[1]:
                found = True
                path = [g,x,y]
                print('---expand final---')
                for i in range(len(expand)):
                    print(expand[i])
                
                print('---closed final---')
                print('path : ', path)
                for i in range(len(closed)):
                    print(closed[i])
                    
                print('---value_g final---')
                for i in range(len(value_g)):
                    print(value_g[i])
                
                print('---action final---')
                for i in range(len(action)):
                    print(action[i])
    
                print('--------- Search successful ---------')
                
            else:
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]):
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            g2 = g + cost
                            h2 = heuristic[x2][y2]
                            f2 = g2 + h2
                            open.append([f2, g2, h2, x2, y2])
#                             print('append list item')
#                             print([f2, g2, h2, x2, y2])
                            closed[x2][y2] = 1
                            value_g[x2][y2] = g2
                            action[x2][y2] = i
               
#                 print('---4.1 grid---')
#                 for i in range(len(grid)):
#                     print(grid[i])
                    
                    
#                 print('---4.2 expand---')
#                 for i in range(len(expand)):
#                     print(expand[i])         
                
#                 print('---4.3 closed---')
#                 for i in range(len(closed)):
#                     print(closed[i])
                    
#                 print('---4.4 value_g---')
#                 for i in range(len(value_g)):
#                     print(value_g[i])
                
#                 print('---4.5 action---')
#                 for i in range(len(action)):
#                     print(action[i])
    
    
    policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    x = goal[0]
    y = goal[1]
    policy[x][y] = '*'
    
    while x != init[0] or y != init[1]:
        x2 = x -delta[action[x][y]][0]
        y2 = y -delta[action[x][y]][1]
        
        policy[x2][y2] = delta_name[action[x][y]]
        x = x2
        y = y2
        
    for i in range(len(policy)):
        print(policy[i])                         

    return path

search(grid,init,goal,cost,heuristic)
# -

# ## Dynamic Programming

# +
# ----------
# User Instructions:
# 
# Write a function optimum_policy that returns
# a grid which shows the optimum policy for robot
# motion. This means there should be an optimum
# direction associated with each navigable cell from
# which the goal can be reached.
# 
# Unnavigable cells as well as cells from which 
# the goal cannot be reached should have a string 
# containing a single space (' '), as shown in the 
# previous video. The goal cell should have '*'.
# ----------

grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1 # the cost associated with moving from a cell to an adjacent one

delta = [[-1, 0 ], # go up
         [ 0, -1], # go left
         [ 1, 0 ], # go down
         [ 0, 1 ]] # go right

delta_name = ['^', '<', 'v', '>']

def optimum_policy(grid,goal,cost):
    # ----------------------------------------
    # modify code below
    # ----------------------------------------
    value = [[99 for row in range(len(grid[0]))] for col in range(len(grid))]
    policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    change = True
    print('---grid init---')
    for i in range(len(grid)):
        print(grid[i])

    while change:
        change = False

        for x in range(len(grid)):
            for y in range(len(grid[0])):
                if goal[0] == x and goal[1] == y:
                    if value[x][y] > 0:
                        value[x][y] = 0
                        policy[x][y] = '*'

                        change = True
                        
                        print('---value---')
                        for i in range(len(value)):
                            print(value[i])

                elif grid[x][y] == 0:
                    for a in range(len(delta)):
                        x2 = x + delta[a][0]
                        y2 = y + delta[a][1]

                        if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0:
                            v2 = value[x2][y2] + cost

                            if v2 < value[x][y]:
                                change = True
                                value[x][y] = v2
                                policy[x][y] = delta_name[a]
                

        print('---4.1 grid---')
        for i in range(len(grid)):
            print(grid[i])    
            
        print('---value---')
        for i in range(len(value)):
             print(value[i])
        print('---policy---')
        for i in range(len(policy)):
             print(policy[i])
    
    print('---policy final---')
    for i in range(len(value)):
        print(policy[i])
        
    return change
    
optimum_policy(grid,goal,cost)

# +
# ----------
# User Instructions:
# 
# Implement the function optimum_policy2D below.
#
# You are given a car in grid with initial state
# init. Your task is to compute and return the car's 
# optimal path to the position specified in goal; 
# the costs for each motion are as defined in cost.
#
# There are four motion directions: up, left, down, and right.
# Increasing the index in this array corresponds to making a
# a left turn, and decreasing the index corresponds to making a 
# right turn.

forward = [[-1,  0], # go up
           [ 0, -1], # go left
           [ 1,  0], # go down
           [ 0,  1]] # go right
forward_name = ['up', 'left', 'down', 'right']

# action has 3 values: right turn, no turn, left turn
action = [-1, 0, 1]
action_name = ['R', '#', 'L']

# EXAMPLE INPUTS:
# grid format:
#     0 = navigable space
#     1 = unnavigable space 
grid = [[1, 1, 1, 0, 0, 0],
        [1, 1, 1, 0, 1, 0],
        [0, 0, 0, 0, 0, 0],
        [1, 1, 1, 0, 1, 1],
        [1, 1, 1, 0, 1, 1]]

init = [4, 3, 0] # given in the form [row,col,direction]
                 # direction = 0: up
                 #             1: left
                 #             2: down
                 #             3: right
                
goal = [2, 0] # given in the form [row,col]

cost = [2, 1, 12] # cost has 3 values, corresponding to making 
                  # a right turn, no turn, and a left turn

# EXAMPLE OUTPUT:
# calling optimum_policy2D with the given parameters should return 
# [[' ', ' ', ' ', 'R', '#', 'R'],
#  [' ', ' ', ' ', '#', ' ', '#'],
#  ['*', '#', '#', '#', '#', 'R'],
#  [' ', ' ', ' ', '#', ' ', ' '],
#  [' ', ' ', ' ', '#', ' ', ' ']]
# ----------

# ----------------------------------------
# modify code below
# ----------------------------------------

def optimum_policy2D(grid,init,goal,cost):
    
    value = [[[99 for row in range(len(grid[0]))] for col in range(len(grid))],
             [[99 for row in range(len(grid[0]))] for col in range(len(grid))],
             [[99 for row in range(len(grid[0]))] for col in range(len(grid))],
             [[99 for row in range(len(grid[0]))] for col in range(len(grid))]]
             
    policy = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
              [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
              [[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
              [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
              
    policy2D = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]

    change = True
    
    print('---value init---')
    for i in range(len(value)):
        print('---',forward_name[i],'---')
        for j in range(len(value[i])):
            print(value[i][j])
            
    print('---policy init---')
    for i in range(len(value)):
        print('---',forward_name[i],'---')
        for j in range(len(policy[i])):
            print(policy[i][j])
        
   

    while change:
        change = False

        for x in range(len(grid)):
            for y in range(len(grid[0])):
                for orientation in range(4):
                    if goal[0] == x and goal[1] == y:
                        if value[orientation][x][y]>0:
                            chage  = True
                            value[orientation][x][y] = 0
                            policy[orientation][x][y] = '*'
    
    
                    elif grid[x][y] == 0:
                        for i in range(3):
                            o2 = (orientation + action[i])%4
                            x2 = x + forward[o2][0]
                            y2 = y + forward[o2][1]
                            
                            if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0:
                                v2 = value[o2][x2][y2] + cost[i]
                                if v2 < value[orientation][x][y]:
                                    change = True
                                    value[orientation][x][y] = v2
                                    policy[orientation][x][y] = action_name[i]
#                                     print('orientation : ',(orientation))
#                                     print('x : ',(x))
#                                     print('y : ',(y))
#                                     print('value : ',value[orientation][x][y])
#                                     print('action name : ',action_name[i])
#                                     print('o2 : ',(o2))
#                                     print('x2 : ',(x2))
#                                     print('y2 : ',(y2))
#                                     print('v2 : ',(v2))
#                                     print('value_update : ',value[orientation][x][y])
#                                     print('policy : ',policy[orientation][x][y])
#                                     print('--------------------------')
    
        print('---value---')
        for i in range(len(value)):
            print('---',forward_name[i],'---')
            for j in range(len(value[i])):
                print(value[i][j])
            
        print('---policy---')
        for i in range(len(value)):
            print('---',forward_name[i],'---')
            for j in range(len(policy[i])):
                print(policy[i][j])

    x = init[0]
    y = init[1]
    orientation = init[2]
    
    policy2D[x][y] = policy[orientation][x][y]
    while policy[orientation][x][y] != '*':
        if policy[orientation][x][y] == '#':
            o2 = orientation
        elif policy[orientation][x][y] == 'R':
            o2 = (orientation-1)%4
        elif policy[orientation][x][y] == 'L':
            o2 = (orientation+1)%4
            
        x = x + forward[o2][0]
        y = y + forward[o2][1]
        orientation = o2
        policy2D[x][y] = policy[orientation][x][y]

    print('---policy 2D final ---')
    for i in range(len(policy2D)):
        print(policy2D[i])
        
    return change
    
optimum_policy2D(grid,init,goal,cost)

   

# -





StarcoderdataJupyterScriptsDedupFiltered