Fixed some logic error and cleaned up the code