Prim's algorithm optimized