Added hysteresis code (Neil Martin's version fixed and simplified)