Things are working! First basic implementation of algorithm!