Updated thresholding code with Masanao's latest