Added diagonal checking and cleaned up.