Add Substitute algorithm refactoring