Added some filtering to find511() and a lot of diagnostic outputs. Filter needs to be improved and outputs need to be cleaned before merging with master