from config import *
import heapq

class Astar():

    def __init__(self,game):

        self.g = game

        # Define the movement directions (up, down, left, right)
        self.directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]

    def heuristic(self,a, b):
        # Calculate the Manhattan distance between two points
        return abs(b[0] - a[0]) + abs(b[1] - a[1])

    def get_successors(self,position):
        # Get the neighboring cells that can be traversed
        successors = []
        for direction in self.directions:
            neighbor = (position[0] + direction[0], position[1] + direction[1])
            if 0 <= neighbor[0] < TILE_SIZE and 0 <= neighbor[1] < TILE_SIZE and self.g.obstacles[neighbor[0]][neighbor[1]] == False:
                successors.append(neighbor)
        return successors

    def print_path(self,came_from, current,path):
        # Recursively print the path from the start to the current position
        if current in came_from:
            path = self.print_path(came_from, came_from[current],path)
        path.append(self.g.bfs.get_cell_number(current[0]*TILE_SIZE,current[1]*TILE_SIZE))
        #print("Budowanie ścieżki: ",path)
        return path

    def a_star(self, goal):
        path = []
        start = (self.g.agent.rect.x//TILE_SIZE, self.g.agent.rect.y//TILE_SIZE)
        print(start,goal)
        open_set = []
        heapq.heappush(open_set, (0, start))  # Priority queue with the start position
        came_from = {}
        g_scores = {start: 0}  # Cost from start to each position
        f_scores = {start: self.heuristic(start, goal)}  # Total estimated cost from start to goal through each position

        while open_set:
            _, current = heapq.heappop(open_set)
            
            if current == goal:
                # Goal reached, print the path
                path = self.print_path(came_from, goal,path)
                return path

            for successor in self.get_successors(current):
                # Calculate the cost to move from the current position to the successor
                cost = self.g.cell_costs[successor[0]][successor[1]]
                tentative_g_score = g_scores[current] + cost

                if successor not in g_scores or tentative_g_score < g_scores[successor]:
                    # Update the cost and priority if it's a better path
                    came_from[successor] = current
                    g_scores[successor] = tentative_g_score
                    f_scores[successor] = tentative_g_score + self.heuristic(successor, goal)
                    heapq.heappush(open_set, (f_scores[successor], successor))
        # No path found
        print("No path found.")