type PointData = {
  pointId: string;
  position: [number, number, number];
  isCurved: boolean;
  handleA: [number, number, number] | null;
  handleB: [number, number, number] | null;
};

interface PathDataInterface {
  pathId: string;
  pathPoints: [PointData, PointData];
}

type PathData = PathDataInterface[];

function distance(a: PointData, b: PointData): number {
  const dx = a.position[0] - b.position[0];
  const dy = a.position[1] - b.position[1];
  const dz = a.position[2] - b.position[2];
  return Math.sqrt(dx * dx + dy * dy + dz * dz);
}

type AStarResult = {
  path: PointData[]; // ordered list of points along the path
  distance: number; // total distance
};

export function aStarShortestPath(
  startId: string,
  goalId: string,
  points: PointData[],
  paths: PathData
): AStarResult | null {
  const openSet = new Set<string>([startId]);
  const cameFrom: Record<string, string | null> = {};
  const gScore: Record<string, number> = {};
  const fScore: Record<string, number> = {};

  points.forEach((p) => {
    gScore[p.pointId] = Infinity;
    fScore[p.pointId] = Infinity;
    cameFrom[p.pointId] = null;
  });

  gScore[startId] = 0;
  fScore[startId] = 0;

  while (openSet.size > 0) {
    // Pick node with lowest fScore
    let current = [...openSet].reduce((a, b) =>
      fScore[a] < fScore[b] ? a : b
    );

    if (current === goalId) {
      // ✅ Reconstruct path
      const path: PointData[] = [];
      let node: string | null = current;
      while (node) {
        const pt = points.find((p) => p.pointId === node);
        if (pt) path.unshift(pt);
        node = cameFrom[node];
      }

      return {
        path,
        distance: gScore[goalId],
      };
    }

    openSet.delete(current);

    // Find neighbors from paths
    const neighbors = paths.filter((p) =>
      p.pathPoints.some((pt) => pt.pointId === current)
    );

    for (let n of neighbors) {
      const [p1, p2] = n.pathPoints;
      const neighbor = p1.pointId === current ? p2 : p1;

      const tentativeG =
        gScore[current] +
        distance(points.find((pt) => pt.pointId === current)!, neighbor);

      if (tentativeG < gScore[neighbor.pointId]) {
        cameFrom[neighbor.pointId] = current;
        gScore[neighbor.pointId] = tentativeG;
        fScore[neighbor.pointId] = tentativeG; // no heuristic for now
        openSet.add(neighbor.pointId);
      }
    }
  }

  return null; // no path found
}
