Refactor C++ implementation

Use a single vector and struct instead of multiple maps.