Fixed some compile errors
Added a slight hysteresis with reverse/forward transition so that we don't get stuck in a between state to easy