First working draft of matching algorithm