WIP: Add real algorithm

I've added one algorithm for detection of current surface. Had to work through some annoyances, but its working now. This pull request is just for reference in case anyone needs to play with a real algorithm. Several functions probably need to be pulled into a util folder since they will be used in multiple algorithms.