import { Injectable } from '@angular/core';
import { Node} from '../pathfinding.models';

@Injectable({
  providedIn: 'root'
})
export class PathfindingService {

  // Helper function to get all unvisited neighbors of a given node
  getUnvisitedNeighbors(node: Node, grid: Node[][]): Node[] {
    const neighbors: Node[] = [];
    const { col, row } = node;

    if (row > 0) neighbors.push(grid[row - 1][col]);
    if (row < grid.length - 1) neighbors.push(grid[row + 1][col]);
    if (col > 0) neighbors.push(grid[row][col - 1]);
    if (col < grid[0].length - 1) neighbors.push(grid[row][col + 1]);

    return neighbors.filter(neighbor => !neighbor.isVisited && !neighbor.isWall);
  }

  // Helper function to get the nodes in the shortest path
  getNodesInShortestPath(endNode: Node): Node[] {
    const shortestPathNodes: Node[] = [];
    let currentNode: Node | null = endNode;
    while (currentNode !== null) {
      shortestPathNodes.unshift(currentNode);
      currentNode = currentNode.linkedNode;
    }
    return shortestPathNodes;
  }

  // Dijkstra's Algorithm
  dijkstra(grid: Node[][], startNode: Node, endNode: Node): { visitedNodesInOrder: Node[], nodesInShortestPathOrder: Node[] } {
    const visitedNodesInOrder: Node[] = [];
    startNode.nodeData = 0;
    const unvisitedNodes: Node[] = this.getAllNodes(grid);

    while (unvisitedNodes.length > 0) {
      this.sortNodesByDistance(unvisitedNodes);
      const closestNode = unvisitedNodes.shift() as Node;

      if (closestNode.isWall) {
        continue;
      }

      const isTrapped = closestNode.nodeData === Infinity;
      if (isTrapped)
      {
        return { visitedNodesInOrder, nodesInShortestPathOrder: [] };
      }

      closestNode.isVisited = true;
      visitedNodesInOrder.push(closestNode);

      const reachedTheEnd = closestNode === endNode;
      if (reachedTheEnd) {
        return {visitedNodesInOrder, nodesInShortestPathOrder: this.getNodesInShortestPath(endNode)};
      }

      this.updateUnvisitedNeighbors(closestNode, grid);
    }

    return { visitedNodesInOrder, nodesInShortestPathOrder: [] };
  }

  private sortNodesByDistance(unvisitedNodes: Node[]): void {
    unvisitedNodes.sort((nodeA, nodeB) => nodeA.nodeData - nodeB.nodeData);
  }

  private updateUnvisitedNeighbors(node: Node, grid: Node[][]): void {
    const unvisitedNeighbors = this.getUnvisitedNeighbors(node, grid);
    for (const neighbor of unvisitedNeighbors) {
      neighbor.nodeData = node.nodeData + 1;
      neighbor.linkedNode = node;
    }
  }

  // A* Search Algorithm
  aStar(grid: Node[][], startNode: Node, endNode: Node): { visitedNodesInOrder: Node[], nodesInShortestPathOrder: Node[] } {
    const visitedNodesInOrder: Node[] = [];
    startNode.nodeData = 0;
    startNode['hScore'] = this.calculateHeuristic(startNode, endNode);
    // fScore = gScore + hScore
    startNode['fScore'] = startNode.nodeData + startNode['hScore'];

    const openSet: Node[] = [startNode];
    const allNodes = this.getAllNodes(grid);

    this.initNodesForAStar(allNodes, startNode);

    while (openSet.length > 0) {
      this.sortOpenSet(openSet);
      const currentNode = openSet.shift() as Node;

      if (currentNode.isWall) {
        continue;
      }

      const isTrapped = currentNode.nodeData === Infinity;
      if (isTrapped)
      {
        return {visitedNodesInOrder, nodesInShortestPathOrder: []};
      }


      currentNode.isVisited = true;
      visitedNodesInOrder.push(currentNode);

      const reachedTheEnd = currentNode === endNode;
      if (reachedTheEnd) {
        return { visitedNodesInOrder, nodesInShortestPathOrder: this.getNodesInShortestPath(endNode) };
      }

      const neighbors = this.getUnvisitedNeighbors(currentNode, grid);
      for (const neighbor of neighbors) {
        const tentativeGScore = currentNode.nodeData + 1; // Distance from start to neighbor

        if (tentativeGScore < neighbor.nodeData) {
          this.updateNeighborNode(neighbor, currentNode, tentativeGScore, endNode, openSet);
        }
      }
    }

    return { visitedNodesInOrder, nodesInShortestPathOrder: [] };
  }

  private sortOpenSet(openSet: Node[]) {
    openSet.sort((a, b) => {
      const f = a['fScore'] - b['fScore'];
      if (f !== 0) return f;

      // tie-break: smaller heuristic first (more goal-directed)
      return (a['hScore'] ?? 0) - (b['hScore'] ?? 0);
    });
  }

  private updateNeighborNode(neighbor: Node, currentNode: Node, tentativeGScore: number, endNode: Node, openSet: Node[]) {
    neighbor.linkedNode = currentNode;
    neighbor.nodeData = tentativeGScore;
    neighbor['nodeData'] = this.calculateHeuristic(neighbor, endNode);
    neighbor['hScore'] = this.calculateHeuristic(neighbor, endNode);
    neighbor['fScore'] = neighbor.nodeData + neighbor['hScore'];

    if (!openSet.includes(neighbor)) {
      openSet.push(neighbor);
    }
  }

  private initNodesForAStar(allNodes: Node[], startNode: Node) {
    for (const node of allNodes) {
      if (node !== startNode) {
        node['fScore'] = Infinity;
        node.nodeData = Infinity; // gScore
      }
    }
  }

  private calculateHeuristic(node: Node, endNode: Node): number {
    // Manhattan distance heuristic
    return Math.abs(node.row - endNode.row) + Math.abs(node.col - endNode.col);
  }

  private getAllNodes(grid: Node[][]): Node[] {
    const nodes: Node[] = [];
    for (const row of grid) {
      for (const node of row) {
        nodes.push(node);
      }
    }
    return nodes;
  }
}
