;; Copyright 2021 AlaskanEmily
;; 
;; Permission to use, copy, modify, and/or distribute this software for any
;; purpose with or without fee is hereby granted, provided that the above
;; copyright notice and this permission notice appear in all copies.
;; 
;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
;; POSSIBILITY OF SUCH DAMAGE.

; ASM implementation of this C routine:
; unsigned interpolate(uint8_t t, uint8_t a, uint8_t b){
;   uint8_t value, diff, dx;
;   if(a > b){
;       value = a;
;       a = b;
;       b = value;
;       t = 255 - t;
;   }
;   diff = b - a;
;   value = a;
;   do{
;       dx = diff & 1;
;       diff >>= 1;
;       if(t & 0x80)
;           value += diff + dx;
;   }while(t <<= 1);
;   return value;
; }
; The extra offset of dx here is a very rough form of rounding.
; If you do not include this, then the results will be about 1/16th too low.

; Calculates the interpolation at a (0 to 255) of b to c
; Returns in a.
interpolate:
    push de
    ld d, a ; Save the T-value in d
    
    ; Swap the interpolation range and T value if necessary
    ld a, c
    sub b
    jrnc @$interpolate_inner
    ; Swap b and c
    ld e, b
    ld b, c
    ld c, e
    
    ; Negate the T-value
    ld a, 0xFF
    sub d
    ld d, a
$interpolate_inner:
    
    ld a, c
    sub b
    ld e, a
    ; e has the difference now.
    ld a, b
$interpolate_bit:
    ; srl will set the carry bit, and bit doesn't modify it.
    ; This lets us use adc rather than add plus an inc and a jump below.
    srl e
    bit7 d
    jrz @.no_add
    adc e
.no_add:
    sla d
    jrnz @$interpolate_bit
    pop de
    ret