vectors seem to work and be nicer
added waypoint system and proper path finding