Initial working version for the redone c++ implementation