Algorithm implemented, final debugs required