Added SAO and SUB functionality - debugging still needed on the edges