More work on algorithm, still not fully functional (but getting there)