Collision seems to be fixed but some cleaning is needed in the code.