What I got so far. Still need to add comments, test, and debug. Also:
regen braking.