Working code

Partially correct, still counts some things wrong.
It's not pretty, but it works.