Added saturation adjustment, improved algorithm