#!/usr/bin/env python3

import problem


def astar_heuristic(start, goal):
    return start.left_dogs + start.left_cats
    #  if start.left_cats == 3 and start.left_dogs == 2 and (start.boat_side !=
    #                                                        "left"):
    #      return 999
    #  return 1
    #  return 3 - goal.right_cats + 3 - goal.right_dogs
    #  return abs(start.left_cats - goal.left_cats) + abs(start.left_dogs -
    #                                                     goal.left_dogs)


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])

    while len(unseen_nodes) > 0:
        current = 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 current == end:
            print("Goal state found")
            return current
        unseen_nodes.remove(current)
        seen_nodes.add(current)
        if current == problem.start:
            print("Initial state")
            print(current.to_string())
        else:
            #  print(current_estimate, act.to_string())
            print(current.to_string(),
                  "(weight: " + str(current_estimate) + ")")
        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)