Updated implementation to better pass tests, still not 100% though.