Split algorithm into separate functions
Optimise line intersection tests with bounding boxes
Enable visualisation