Algorithm 2 -> Converted to Simon's lib