First real version, still not fully converted to c++