Changed the algorithm a bit. Output works.