extends Reference

var SquareCell = preload("./square_cell.gd")

var origin = Vector3( 0.0, 0.0, 0.0 )
var size = Vector3( 0, 0, 0 )
var cell_size = Vector3( 1.0, 1.0, 1.0 )

var path_cost_default = 10.0

var cells_cost = {}
var obstacles = []
var barrieres = {}

func _init( p_size = Vector2( 100, 100 ), p_origin = Vector3( 0.0, 0.0, 0.0 ) ):    
    
    self.origin = p_origin
    self.size = p_size
    
func set_size( p_size ):
    self.size = p_size
    
func get_cell_coords( cell ):
    var cell_coords = Vector3()
    if typeof(cell) == TYPE_VECTOR3:
           cell_coords = cell
    else:
        cell_coords = cell.coords
    return cell_coords
    
func get_cell_at( p_position ):

    var position_local = p_position - self.origin        
    
    var coords = (position_local / self.cell_size) 
    
    var cell = SquareCell.new( coords )
    
    return cell

func get_cell_position( p_cell ):
    var position = (p_cell.coords * self.cell_size) + self.origin
    return position

func get_cell_direction( p_direction ):
    if p_direction is SquareCell.DIR_ALL:
        return p_direction
    
    var coords = self.get_cell_at( p_direction ).coords
    if coords is SquareCell.DIR_ALL:
        return coords
    
    return null


func add_obstacle( cell, cost=0 ):
    self.obstacles.push_back( self.get_cell_coords( cell ) )

func remove_obstacle( cell ):
    self.obstacles.erase( self.get_cell_coords( cell ) )

func is_obstacle( cell ):
    return ( self.get_cell_coords( cell ) in self.obstacles )

func add_barriere( cell, dir ):
    var coords = self.get_cell_coords( cell )
    if not coords in self.barrieres:
        self.barrieres[ coords ] = []
    if not dir in self.barrieres[ coords ]:
        self.barrieres[ coords ].push_back( dir )
    
func remove_barriere( cell, dir ):
    var coords = self.get_cell_coords( cell )
    if coords in self.barrieres and dir in self.barrieres[ coords ]:
        self.barrieres[ coords ].remove( dir )
    
func update_barrieres( p_cell, p_barrieres ):
    var coords = self.get_cell_coords( p_cell )
    self.barrieres[ coords ] = p_barrieres
        
    
    
func is_barriere( cell, dir ):
    var coords = self.get_cell_coords( cell )
    if not coords in self.barrieres:
        return false
    if not dir in self.barrieres[ coords ]:
        return false
    return true 

func get_move_cost(p_cell, p_direction):
    var cell_coords = self.get_cell_coords( p_cell )
        
    var next_cell = SquareCell.new( cell_coords + p_direction )
    var cost = abs(p_direction.x) + abs(p_direction.y) + abs(p_direction.z)*self.get_cell_cost( next_cell )
    
    if  self.is_obstacle( cell_coords ):
        return 0
    
    if cell_coords in self.barrieres and p_direction in self.barrieres[ cell_coords ]:
        return 0
        
    if next_cell.coords in self.barrieres and -p_direction in self.barrieres[ next_cell.coords ]:
        return 0
    
    return cost

func find_path(start, goal, exceptions=[]):

    # Light a starry path from the start to the goal, inclusive
    start = start.coords
    goal = goal.coords
    # Make sure all the exceptions are coords
    var exc = []
    for exception in exceptions:
        if exception is SquareCell:
            exc.append(exception.coords)
    exceptions = exc
    # Now we begin the A* search
    var frontier = [self.make_priority_item(start, 0)]
    var came_from = {start: null}
    var cost_so_far = {start: 0}
    while not frontier.empty():
        var current = frontier.pop_front().v
        if current == goal:
            break
        for next_cell in SquareCell.new(current).get_cells_adjacent():
            var next = next_cell.coords
            var next_cost = self.get_move_cost(current, next - current)
            if next == goal and (next in exceptions or get_cell_cost(next) == 0):
                # Our goal is an obstacle, but we're next to it
                # so our work here is done
                came_from[next] = current
                frontier.clear()
                break
            if not next_cost or next in exceptions:
                # We shall not pass
                continue
            next_cost += cost_so_far[current]
            if not next in cost_so_far or next_cost < cost_so_far[next]:
                # New shortest path to that node
                cost_so_far[next] = next_cost
                var priority = next_cost + next_cell.cell_distance_to(goal)
                # Insert into the frontier
                var item = make_priority_item(next, priority)
                var idx = frontier.bsearch_custom(item, self, "comp_priority_item")
                frontier.insert(idx, item)
                came_from[next] = current

    if not goal in came_from:
        # Not found
        print( "Path not found." )
        return []

    # Follow the path back where we came_from
    var path = []
    if not (self.get_cell_cost(goal) == 0 or goal in exceptions):
        # We only include the goal if it's traversable
        path.append(SquareCell.new(goal))

    var current = goal
    while current != start:
        current = came_from[current]
        path.push_front(SquareCell.new(current))

    return path

# Used to make a priority queue out of an array
func make_priority_item(val, priority):
    return {"v": val, "p": priority}
func comp_priority_item(a, b):
    return a.p < b.p

func get_cell_cost( cell ):
    var cell_coords = self.get_cell_coords( cell )
    
    if self.is_obstacle( cell_coords ):
        return 0
      
    if cell_coords in self.cells_cost:
        return self.cells_cost[ cell_coords ]
        
    return self.path_cost_default