Wall following insertion

It still does not follow the wall appropriately. Anyway, I will work on it more later on.