Clean ups and fix #2. 

Removed some redundant code.
Rewrote C-style cast to their C++ equivalent.
Fixed incompatible type casting when compiling.

Since Vector Classes are not clang compatible yet please use gcc instead.

This should fix #2.