Fixed a small out of range error occurring from std::vector.reserve(). Minor tweaks as well.