Algorithm Foundation

Took me way too long, but this is the foundation to the algo. I need to
keep building, but the rest should be fairly easy (or at least,
easier...)