// State : a protocole for states in the search space
protocol State : Equatable {

    // successors() : returns an array of successors states in the search space
    func successors() -> [Successor<Self>]

    // heuristic(goal) : returns the heuristic value for a given states in relation to a given goal state
    func heuristic(goal:Self) -> Double

    // id : a string identifying a state
    var id : String { get }

}

// States are compared by their id
func ==<T:State>(lhs:T, rhs:T) -> Bool {
    return lhs.id==rhs.id
}

// Successor : represents a successor state and its cost
struct Successor<T:State> {
    var state: T
    var cost: Double
}

// Plan : a plan of states
struct Plan<T:State> {

    // states : an array of states that make a plan
    var states: [T]

    // cost : the total cost of the plan
    var cost: Double

    // isNot(another) : checks if another plan is different from the current one
    func isNot(another: Plan) -> Bool {
        return !(states == another.states && cost == another.cost)
    }

}

// AStar<TState> : finds the A* solution (nil if no solution found) given a start state and goal state
func AStar<TState:State>(start: TState, goal: TState) -> Plan<TState>? {

    var fringe : [Plan<TState>] = [Plan(states: [start], cost: 0)]

    while fringe.count>0 {

        let bestPlan = fringe.minElement({
            a,b
            in
            a.cost + a.states.last!.heuristic(goal) < b.cost + b.states.last!.heuristic(goal)
        })!

        fringe = fringe.filter({
            plan in plan.isNot(bestPlan)
        })

        if bestPlan.states.last! == goal {
            return bestPlan
        }else{
            let successors = bestPlan.states.last!.successors()
            for successor in successors.filter({ s in !bestPlan.states.contains(s.state) }) {
                let newPlan = Plan(states: bestPlan.states+[successor.state], cost: bestPlan.cost+successor.cost)
                fringe.append(newPlan)
            }
        }

    }

    return nil

}