First working version, not yet complete but it calculates correctly and has animation functionality.