Calculate some angles so it's a little easier to visualize
Incorporated ICP. Compiles, but ICP doesn't quite work yet.