SpeedAngle2Offset

.macro neg16 lo, hi
  sec             ;Ensure carry is set
  lda #0          ;Load constant zero
  sbc lo          ;... subtract the least significant byte
  sta lo          ;... and store the result
  lda #0          ;Load constant zero again
  sbc hi          ;... subtract the most significant byte
  sta hi          ;... and store the result
.endmacro

; Calculates a horizontal and vertical speed from a speed and an angle
; input: A (speed) Y (angle, 0-31)
; output: 0,1 (X offset), 2,3 (Y offset)
.proc SpeedAngle2Offset
Angle = 4
Speed = 5
  sty Angle
  sta Speed

  lda CosineTable,y
  php         ; Save whether it's negative or positive
  bpl :+      ; Take the absolute value
  eor #255
  add #1
: ldy Speed
  jsr mul8    ; Multiply the absolute value by the speed
  sty 0       ; Write the offset
  sta 1

  plp
  bpl :+      ; If it was negative, make it negative again
  neg16 0, 1
:

  ldy Angle
  lda SineTable,y
  php        ; Save whether it's positive or negative
  bpl :+     ; Take the absolute value
  eor #255
  add #1
: ldy Speed
  jsr mul8   ; Multiply the absolute value by the speed
  sty 2
  sta 3
  plp
  bpl :+     ; If it was negative, make it negative again
  neg16 2, 3
:
  rts
.endproc