added tests and collision detection

Tests pass but collision detection is broken and super slow.