Final logic fix, added tests