Made the code a little neater

I added some System.out.println statements in the code for testing
purposes. Feel free to remove them.