Finished implementation in c++, added some tests