# A* による経路探索を提供します。
# Developers:: story@monoroch.net
# Version::    2007/07/15 22:36:00
class Astar
  # コンストラクタ。
  # start - スタートノード
  # goal - ゴールノード
  # calcHr - ゴールまでの楽観的コストを求めるプロシージャ
  #          実コスト >= 楽観的コスト
  #          f(node, goalNode)
  def initialize(start, goal, calcHr = Proc.new{0})
    @start = start
    @goal = goal
    @calc_hr = calcHr
    @open = Hash.new
    @close = Hash.new
  end
  
  # 最小経路を計算します。
  # return - スタートからゴールまでのノードを格納した配列
  #          計算に失敗したときは nil
  def calc
    @start.g_cost = 0 # COST(スタート, スタート) のコストは 0
    set_h_cost @start # ヒューリスティックコストを格納
    open @start # スタートノードを Open
    
    while true
      return nil if @open.empty? # Open リストが尽きたら探索失敗
      
      node = get_least_node
      return get_route if node == @goal
      
      close node
      
      for neighbor, cost in node.neighbor_list
        # 隣接ノードの暫定コストを求める
        set_h_cost neighbor
        provCost = node.g_cost + cost + neighbor.h_cost
        
        if @open.include? neighbor
          # 隣接ノードが Open リストに含まれる場合
          if provCost < neighbor.f_cost
            # もっと小さいコストの経路だったときは置き換える
            neighbor.g_cost = node.g_cost + cost
            neighbor.parent = node
          end
        elsif @close.include? neighbor
          # 隣接ノードが Close リストに含まれる場合
          if provCost < neighbor.f_cost
            # もっと小さいコストの経路だったときは置き換える
            neighbor.g_cost = node.g_cost + cost
            neighbor.parent = node
            open neighbor # 隣接ノードを Open する
          end
        else
          # Open リストにも Close リストにもなかった場合
          # 暫定コストを確定する
          neighbor.g_cost = node.g_cost + cost
          neighbor.parent = node
          open neighbor # 隣接ノードを Open する
        end
      end
    end
  end
 
private
 
  # ノードを Open リストに移動します。
  # node - ノード
  def open(node)
    @close.delete(node)
    @open.store(node, nil)
  end
  
  # ノードを Close リストに移動します。
  # node - ノード
  def close(node)
    @open.delete(node)
    @close.store(node, nil)
  end
  
  # ゴールまでのヒューリスティックコストをノードに格納します。
  # node - ノード
  def set_h_cost(node)
    # すでに計算済みの場合は再計算しない
    return node.h_cost if node.h_cost
    node.h_cost = @calc_hr.call(node, @goal)
  end
  
  # Open リストのうち、最小の予想コストを持つノードを取得します。
  # return - 最小の予想コストを持つノード
  def get_least_node
    for node, value in @open
      if (not defined? least) or least.f_cost > node.f_cost
        least = node
      end
    end
    least
  end
  
  # 経路となるノードを順番に配列にして返します。
  def get_route
    node = @goal
    list = Array.new
    while node.parent
      list << node
      node = node.parent
    end
    list << node
    list.reverse!
  end
  
end
 
class AstarNode
  attr_accessor :attr # ノード属性です。計算には利用しません。
  attr_accessor :g_cost # スタートノードからの最小コストです。
  attr_accessor :h_cost # ゴールまでのヒューリスティックコストです。
  attr_accessor :parent # 親ノードです。
  attr_accessor :neighbor_list # [隣接ノード, コスト] の配列です。
  
  # コンストラクタ。
  # attr - ノード属性。省略時は nil
  def initialize(attr = nil)
    @attr = attr
    @neighbor_list = Array.new
  end
  
  # 隣接ノードを追加します。
  # node - 隣接ノード
  # cost - 自身から隣接ノードに移動する際のコスト
  def add_neighbor(node, cost)
    @neighbor_list << [node, cost]
  end
  
  # 予想コストを取得します。
  # return - 予想コスト
  def f_cost 
    return g_cost + h_cost
  end
  
end