partially fixed broken code

Enhanced, fixed and modified broken code to fit a class model better.
There's still one major issue which is the return value from
`add_vectors()`.

Also not sure the angle math is correct, but we'll see once this code
works, haha.