Wrote proper vector class(vec2-4) and ported most of the code to use it.