diff --git a/cores/esp8266/WMath.cpp b/cores/esp8266/WMath.cpp index d8dc1f82f2..e5bccf0782 100644 --- a/cores/esp8266/WMath.cpp +++ b/cores/esp8266/WMath.cpp @@ -72,11 +72,78 @@ long secureRandom(long howsmall, long howbig) { } long map(long x, long in_min, long in_max, long out_min, long out_max) { - const long dividend = out_max - out_min; - const long divisor = in_max - in_min; - const long delta = x - in_min; + long in_length = in_max - in_min; + long out_length = out_max - out_min; - return (delta * dividend + (divisor / 2)) / divisor + out_min; + if (in_length == 0 || out_length == 0) { + return out_min; + } + + long delta = x - in_min; + + if ((out_length < 0) && (in_length < 0)) { + std::swap(in_min, in_max); + std::swap(out_min, out_max); + } else if (out_length < 0) { + x = in_max - delta; + std::swap(out_min, out_max); + } else if (in_length < 0) { + x = in_max - delta; + std::swap(in_min, in_max); + } + + // Update length and delta as in/out values may have changed. + delta = x - in_min; + in_length = in_max - in_min; + out_length = out_max - out_min; + + if (out_length == in_length) { + return out_min + delta; + } + + // We now know in_min < in_max and out_min < out_max + // Make sure x is within range of in_min ... in_max + // Shift the in/out range to contain 'x' + if ((x < in_min) || (x > in_max)) { + long shift_factor = 0; + + if (x < in_min) { + const long before_min = in_min - x; + shift_factor = -1 - (before_min / in_length); + } else { + // x > in_max + const long passed_max = x - in_max; + shift_factor = 1 + (passed_max / in_length); + } + + const long in_shift = shift_factor * in_length; + const long out_shift = shift_factor * out_length; + in_min += in_shift; + in_max += in_shift; + out_min += out_shift; + out_max += out_shift; + delta = x - in_min; + } + + if (out_length > in_length) { + // Map to larger range + // Do not 'simplify' this calculation + // as this will result in integer overflow errors + const long factor = out_length / in_length; + const long error_mod = out_length % in_length; + const long error = (delta * error_mod) / in_length; + return (delta * factor) + out_min + error; + } + + // abs(out_length) < abs(in_length) + // Map to smaller range + // Do not 'simplify' this calculation + // as this will result in integer overflow errors + const long factor = (in_length / out_length); + const long estimate_full = in_length / factor + out_min; + const long error = (delta * (out_max - estimate_full)) / in_length; + + return delta / factor + out_min + error; } uint16_t makeWord(uint16_t w) {