Everything works now! Just need to add Doxygen comments and I'm done.