#!/usr/bin/env python3

import problem


def astar_heuristic(start, goal):
    side = 1 if start.boat_side == "left" else 0
    return (start.left_cats + start.left_dogs) * (1 - side) * 5 + (
        start.right_dogs) * side * 5


def astar_search(start, end):
    actual_cost = {}
    estimated_cost = {}

    actual_cost[start] = 0
    estimated_cost[start] = astar_heuristic(start, end)

    seen_nodes = set()
    unseen_nodes = set([start])

    previous_node = None
    while len(unseen_nodes) > 0:
        current = None
        current_action = None
        current_estimate = None
        for item in unseen_nodes:
            if current is None or estimated_cost[item] < current_estimate:
                current_estimate = estimated_cost[item]
                current = item
                if previous_node is not None:
                    current_action = previous_node.get_action_to(current)
                    if current_action is None:
                        current_action = current.get_action_from(seen_nodes)
        unseen_nodes.remove(current)
        seen_nodes.add(current)
        if current == problem.start:
            print("Initial state")
            print(current.to_string())
        else:
            if current_action is not None:
                print(current_action.to_string(),
                      "(cost: " + str(current_estimate) + ")")
                print(current.to_string())
            else:
                print(current.to_string())
        if current == end:
            print("Goal state found")
            return current
        previous_node = current
        for act in current.actions:
            child = act.result_state
            if child in seen_nodes:
                continue
            child_actual = actual_cost[current] + 1
            if child not in unseen_nodes:
                unseen_nodes.add(child)
            elif child_actual >= actual_cost[child]:
                continue
            actual_cost[child] = child_actual
            estimated_cost[child] = actual_cost[child] + astar_heuristic(
                child, end)
    print("Goal state could not be found")


finish = astar_search(problem.start, problem.end)