Replace all int16_t with int32_t to hopefully get better performance. Makes more sense to operate on 32-bit numbers on a 32-bit MCU