algorithm implemented, not 100% working