.include "lynx.inc"
.include "../starfly/3d.h"
.include "../starfly/starfly.inc"
.import __sprsys :zp

.global _MInitialise
.global _MLoad
.global _MMultiply
.global _MPerform
.global _Project

.segment "CODE"
.macro WAITSUZY
.local notready
notready:
    bit SPRSYS
    bmi notready
.endmacro

.macro ABSOLUTE_READ val
   ldy #(val)
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y
.endmacro

.macro ABSOLUTE_TO_DC val
   ldy #(val)
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC
.endmacro

; MInitialise - initialiases the matrix stack

_MInitialise:

  ldx #_MatrixStart
  stx MatrixOffs
  ldx #$c4
  stx SPRSYS
  stx __sprsys

  rts

; MPush - takes (_MatrixAbsolute) as an argument, does a push/multiply to the
; stack

MPush:

  clc
  lda MatrixOffs
  sta MatrixZero
  adc #MatrixSize

  sta _MatrixTarget
  sta MatrixOffs

  jmp MMultiplyInternal

; MPop - pops a matrix from the stack

MPop:

  sec
  lda MatrixOffs
  sbc #MatrixSize
  sta MatrixOffs

  rts

; MMultiply - multiplies the matrix at the top of the stack by (_MatrixAbsolute)

_MMultiply:
  sta _MatrixAbsolute
  stx _MatrixAbsolute+1

  lda MatrixOffs
  sta _MatrixTarget
  sta MatrixZero

MMultiplyInternal:

  ;
  ;new.right.x = zero.right.x * absolute.right.x + zero.up.x * absolute.right.y + zero.front.x * absolute.right.z
  ;

  ;zero hardware accumulator

  stz MATHM
  stz MATHK

  ;zero.right.x * absolute.right.x

  ;absolute.right.x
  ABSOLUTE_TO_DC VBASIS_RIGHT_X

  ;zero.right.x
  ldy MatrixZero
  lda MATRIX_RIGHT_X, y
  sta MATHB
  lda MATRIX_RIGHT_X+1, y
  sta MATHA

  ; + zero.up.x * absolute.right.y

  ;get absolute.right.y into registers while multiply proceeds
  ldy #VBASIS_RIGHT_Y
  lda (_MatrixAbsolute), y
  tax
  iny
  lda (_MatrixAbsolute), y

  ;wait for end of multiply
  WAITSUZY

  stx MATHD
  sta MATHC

  ;zero.up.x
  ldy MatrixZero
  lda MATRIX_UP_X, y
  sta MATHB
  lda MATRIX_UP_X+1, y
  sta MATHA

  ; + zero.front.x * absolute.right.z

  ;get absolute.right.z into registers while multiply proceeds
  ldy #MATRIX_RIGHT_Z
  lda (_MatrixAbsolute), y
  tax
  iny
  lda (_MatrixAbsolute), y

  ;wait for end of multiply
  WAITSUZY

  stx MATHD
  sta MATHC

  ;zero.front.x
  ldy MatrixZero
  lda MATRIX_FRONT_X, y
  sta MATHB
  lda MATRIX_FRONT_X+1, y
  sta MATHA

  ;wait and store to new.right.x

  WAITSUZY

  ;need to 'shift right 14' = (shift right 16) + shift left 2

  ldx MATHJ
  stx _MatrixWorkArea+MATRIX_RIGHT_X+1

  lda MATHK

  asl MATHL
  rol
  rol _MatrixWorkArea+MATRIX_RIGHT_X+1
  asl MATHL
  rol
  rol _MatrixWorkArea+MATRIX_RIGHT_X+1

  sta _MatrixWorkArea+MATRIX_RIGHT_X

  ;
  ;new.up.x = zero.right.x * absolute.up.x + zero.up.x * absolute.up.y + zero.front.x * absolute.up.z
  ;

  ;zero hardware accumulator

  stz MATHM
  stz MATHK

  ;zero.right.x * absolute.up.x

  ;absolute.up.x
  ABSOLUTE_TO_DC VBASIS_UP_X

  ;zero.right.x
  ldy MatrixZero
  lda MATRIX_RIGHT_X, y
  sta MATHB
  lda MATRIX_RIGHT_X+1, y
  sta MATHA

  ; + zero.up.x * absolute.up.y

  ;get absolute.up.y into registers while multiply proceeds
  ldy #VBASIS_UP_Y
  lda (_MatrixAbsolute), y
  tax
  iny
  lda (_MatrixAbsolute), y

  ;wait for end of multiply
  WAITSUZY

  stx MATHD
  sta MATHC

  ;zero.up.x
  ldy MatrixZero
  lda MATRIX_UP_X, y
  sta MATHB
  lda MATRIX_UP_X+1, y
  sta MATHA

  ; + zero.front.x * absolute.up.z

  ;get absolute.up.z into registers while multiply proceeds
  ldy #VBASIS_UP_Z
  lda (_MatrixAbsolute), y
  tax
  iny
  lda (_MatrixAbsolute), y

  ;wait for end of multiply
  WAITSUZY

  stx MATHD
  sta MATHC

  ;zero.front.x
  ldy MatrixZero
  lda MATRIX_FRONT_X, y
   sta MATHB
   lda MATRIX_FRONT_X+1, y
   sta MATHA

  ;wait and store to new.up.x

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_UP_X+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_UP_X+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_UP_X+1

   sta _MatrixWorkArea+MATRIX_UP_X

 ;
 ;new.front.x = zero.right.x * absolute.front.x + zero.up.x * absolute.front.y + zero.front.x * absolute.front.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.x * absolute.front.x

  ;absolute.front.x
  ABSOLUTE_TO_DC VBASIS_FRONT_X

    ;zero.right.x
   ldy MatrixZero
   lda MATRIX_RIGHT_X, y
   sta MATHB
   lda MATRIX_RIGHT_X+1, y
   sta MATHA

  ; + zero.up.x * absolute.front.y

    ;get absolute.front.y into registers while multiply proceeds
   ldy #VBASIS_FRONT_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.x
   ldy MatrixZero
   lda MATRIX_UP_X, y
   sta MATHB
   lda MATRIX_UP_X+1, y
   sta MATHA

  ; + zero.front.x * absolute.front.z

    ;get absolute.front.z into registers while multiply proceeds
   ldy #VBASIS_FRONT_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.x
   ldy MatrixZero
   lda MATRIX_FRONT_X, y
   sta MATHB
   lda MATRIX_FRONT_X+1, y
   sta MATHA

  ;wait and store to new.front.x

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_FRONT_X+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_FRONT_X+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_FRONT_X+1

   sta _MatrixWorkArea+MATRIX_FRONT_X

 ;
 ;new.pos.x = zero.right.x * absolute.pos.x + zero.up.x * absolute.pos.y + zero.front.x * absolute.pos.z + zero.pos.x
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.x * absolute.pos.x

    ;absolute.pos.x
  ABSOLUTE_TO_DC VBASIS_POS_X

    ;zero.right.x
   ldy MatrixZero
   lda MATRIX_RIGHT_X, y
   sta MATHB
   lda MATRIX_RIGHT_X+1, y
   sta MATHA

  ; + zero.up.x * absolute.pos.y

    ;get absolute.pos.y into registers while multiply proceeds
   ldy #VBASIS_POS_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.x
   ldy MatrixZero
   lda MATRIX_UP_X, y
   sta MATHB
   lda MATRIX_UP_X+1, y
   sta MATHA

  ; + zero.front.x * absolute.pos.z

    ;get absolute.pos.z into registers while multiply proceeds
   ldy #VBASIS_POS_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.x
   ldy MatrixZero
   lda MATRIX_FRONT_X, y
   sta MATHB
   lda MATRIX_FRONT_X+1, y
   sta MATHA

  ;wait, add zero.pos.x and store to new.pos.x

            WAITSUZY

   ;only need to shift right 8

   ldy MatrixZero
   clc
   lda MATRIX_POS_X, y
   adc MATHL
   sta _MatrixWorkArea+MATRIX_POS_X
   lda MATRIX_POS_X+1, y
   adc MATHK
   sta _MatrixWorkArea+MATRIX_POS_X+1

 ;
 ;new.right.y = zero.right.y * absolute.right.x + zero.up.y * absolute.right.y + zero.front.y * absolute.right.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.y * absolute.right.x

    ;absolute.right.x
  ABSOLUTE_TO_DC VBASIS_RIGHT_X

    ;zero.right.y
   ldy MatrixZero
   lda MATRIX_RIGHT_Y, y
   sta MATHB
   lda MATRIX_RIGHT_Y+1, y
   sta MATHA

  ; + zero.up.y * absolute.right.y

    ;get absolute.right.y into registers while multiply proceeds
   ldy #VBASIS_RIGHT_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.y
   ldy MatrixZero
   lda MATRIX_UP_Y, y
   sta MATHB
   lda MATRIX_UP_Y+1, y
   sta MATHA

  ; + zero.front.y * absolute.right.z

    ;get absolute.right.z into registers while multiply proceeds
   ldy #VBASIS_RIGHT_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.y
   ldy MatrixZero
   lda MATRIX_FRONT_Y, y
   sta MATHB
   lda MATRIX_FRONT_Y+1, y
   sta MATHA

  ;wait and store to new.right.y

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_RIGHT_Y+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_RIGHT_Y+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_RIGHT_Y+1

   sta _MatrixWorkArea+MATRIX_RIGHT_Y

 ;
 ;new.up.y = zero.right.y * absolute.up.x + zero.up.y * absolute.up.y + zero.front.y * absolute.up.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.y * absolute.up.x

    ;absolute.up.x
   ldy #VBASIS_UP_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.y
   ldy MatrixZero
   lda MATRIX_RIGHT_Y, y
   sta MATHB
   lda MATRIX_RIGHT_Y+1, y
   sta MATHA

  ; + zero.up.y * absolute.up.y

    ;get absolute.up.y into registers while multiply proceeds
   ldy #VBASIS_UP_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.y
   ldy MatrixZero
   lda MATRIX_UP_Y, y
   sta MATHB
   lda MATRIX_UP_Y+1, y
   sta MATHA

  ; + zero.front.y * absolute.up.z

    ;get absolute.up.z into registers while multiply proceeds
   ldy #VBASIS_UP_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.y
   ldy MatrixZero
   lda MATRIX_FRONT_Y, y
   sta MATHB
   lda MATRIX_FRONT_Y+1, y
   sta MATHA

  ;wait and store to new.up.y

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_UP_Y+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_UP_Y+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_UP_Y+1

   sta _MatrixWorkArea+MATRIX_UP_Y

 ;
 ;new.front.y = zero.right.y * absolute.front.x + zero.up.y * absolute.front.y + zero.front.y * absolute.front.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.y * absolute.front.x

    ;absolute.front.x
   ldy #VBASIS_FRONT_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.y
   ldy MatrixZero
   lda MATRIX_RIGHT_Y, y
   sta MATHB
   lda MATRIX_RIGHT_Y+1, y
   sta MATHA

  ; + zero.up.y * absolute.front.y

    ;get absolute.front.y into registers while multiply proceeds
   ldy #VBASIS_FRONT_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.y
   ldy MatrixZero
   lda MATRIX_UP_Y, y
   sta MATHB
   lda MATRIX_UP_Y+1, y
   sta MATHA

  ; + zero.front.y * absolute.front.z

    ;get absolute.front.z into registers while multiply proceeds
   ldy #VBASIS_FRONT_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.y
   ldy MatrixZero
   lda MATRIX_FRONT_Y, y
   sta MATHB
   lda MATRIX_FRONT_Y+1, y
   sta MATHA

  ;wait and store to new.front.y

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_FRONT_Y+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_FRONT_Y+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_FRONT_Y+1

   sta _MatrixWorkArea+MATRIX_FRONT_Y

 ;
 ;new.pos.y = zero.right.y * absolute.pos.x + zero.up.y * absolute.pos.y + zero.front.y * absolute.pos.z + zero.pos.y
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.y * absolute.pos.x

    ;absolute.pos.x
   ldy #VBASIS_POS_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.y
   ldy MatrixZero
   lda MATRIX_RIGHT_Y, y
   sta MATHB
   lda MATRIX_RIGHT_Y+1, y
   sta MATHA

  ; + zero.up.y * absolute.pos.y

    ;get absolute.pos.y into registers while multiply proceeds
   ldy #VBASIS_POS_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.y
   ldy MatrixZero
   lda MATRIX_UP_Y, y
   sta MATHB
   lda MATRIX_UP_Y+1, y
   sta MATHA

  ; + zero.front.y * absolute.pos.z

    ;get absolute.pos.z into registers while multiply proceeds
   ldy #VBASIS_POS_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.y
   ldy MatrixZero
   lda MATRIX_FRONT_Y, y
   sta MATHB
   lda MATRIX_FRONT_Y+1, y
   sta MATHA

  ;wait, add zero.pos.y and store to new.pos.y

            WAITSUZY

   ;only need to shift right 8

   ldy MatrixZero
   clc
   lda MATRIX_POS_Y, y
   adc MATHL
   sta _MatrixWorkArea+MATRIX_POS_Y
   lda MATRIX_POS_Y+1, y
   adc MATHK
   sta _MatrixWorkArea+MATRIX_POS_Y+1

 ;
 ;new.right.z = zero.right.z * absolute.right.x + zero.up.z * absolute.right.y + zero.front.z * absolute.right.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.z * absolute.right.x

    ;absolute.right.x
   ldy #VBASIS_RIGHT_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.z
   ldy MatrixZero
   lda MATRIX_RIGHT_Z, y
   sta MATHB
   lda MATRIX_RIGHT_Z+1, y
   sta MATHA

  ; + zero.up.z * absolute.right.y

    ;get absolute.right.y into registers while multiply proceeds
   ldy #VBASIS_RIGHT_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.z
   ldy MatrixZero
   lda MATRIX_UP_Z, y
   sta MATHB
   lda MATRIX_UP_Z+1, y
   sta MATHA

  ; + zero.front.z * absolute.right.z

    ;get absolute.right.z into registers while multiply proceeds
   ldy #VBASIS_RIGHT_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixZero
   lda MATRIX_FRONT_Z, y
   sta MATHB
   lda MATRIX_FRONT_Z+1, y
   sta MATHA

  ;wait and store to new.right.z

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_RIGHT_Z+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_RIGHT_Z+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_RIGHT_Z+1

   sta _MatrixWorkArea+MATRIX_RIGHT_Z

 ;
 ;new.up.z = zero.right.z * absolute.up.x + zero.up.z * absolute.up.y + zero.front.z * absolute.up.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.z * absolute.up.x

    ;absolute.up.x
   ldy #VBASIS_UP_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.z
   ldy MatrixZero
   lda MATRIX_RIGHT_Z, y
   sta MATHB
   lda MATRIX_RIGHT_Z+1, y
   sta MATHA

  ; + zero.up.z * absolute.up.y

    ;get absolute.up.y into registers while multiply proceeds
   ldy #VBASIS_UP_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.z
   ldy MatrixZero
   lda MATRIX_UP_Z, y
   sta MATHB
   lda MATRIX_UP_Z+1, y
   sta MATHA

  ; + zero.front.z * absolute.up.z

    ;get absolute.up.z into registers while multiply proceeds
   ldy #VBASIS_UP_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixZero
   lda MATRIX_FRONT_Z, y
   sta MATHB
   lda MATRIX_FRONT_Z+1, y
   sta MATHA

  ;wait and store to new.up.z

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_UP_Z+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_UP_Z+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_UP_Z+1

   sta _MatrixWorkArea+MATRIX_UP_Z

 ;
 ;new.front.z = zero.right.z * absolute.front.x + zero.up.z * absolute.front.y + zero.front.z * absolute.front.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.z * absolute.front.x

    ;absolute.front.x
   ldy #VBASIS_FRONT_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.z
   ldy MatrixZero
   lda MATRIX_RIGHT_Z, y
   sta MATHB
   lda MATRIX_RIGHT_Z+1, y
   sta MATHA

  ; + zero.up.z * absolute.front.y

    ;get absolute.front.y into registers while multiply proceeds
   ldy #VBASIS_FRONT_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.z
   ldy MatrixZero
   lda MATRIX_UP_Z, y
   sta MATHB
   lda MATRIX_UP_Z+1, y
   sta MATHA

  ; + zero.front.z * absolute.front.z

    ;get absolute.front.z into registers while multiply proceeds
   ldy #VBASIS_FRONT_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixZero
   lda MATRIX_FRONT_Z, y
   sta MATHB
   lda MATRIX_FRONT_Z+1, y
   sta MATHA

  ;wait and store to new.front.z

            WAITSUZY

   ;need to 'shift right 14' = (shift right 16) + shift left 2

   ldx MATHJ
   stx _MatrixWorkArea+MATRIX_FRONT_Z+1

   lda MATHK

   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_FRONT_Z+1
   asl MATHL
   rol
   rol _MatrixWorkArea+MATRIX_FRONT_Z+1

   sta _MatrixWorkArea+MATRIX_FRONT_Z

 ;
 ;new.pos.z = zero.right.z * absolute.pos.x + zero.up.z * absolute.pos.y + zero.front.z * absolute.pos.z + zero.pos.z
 ;

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;zero.right.z * absolute.pos.x

    ;absolute.pos.x
   ldy #VBASIS_POS_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;zero.right.z
   ldy MatrixZero
   lda MATRIX_RIGHT_Z, y
   sta MATHB
   lda MATRIX_RIGHT_Z+1, y
   sta MATHA

  ; + zero.up.z * absolute.pos.y

    ;get absolute.pos.y into registers while multiply proceeds
   ldy #VBASIS_POS_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.up.z
   ldy MatrixZero
   lda MATRIX_UP_Z, y
   sta MATHB
   lda MATRIX_UP_Z+1, y
   sta MATHA

  ; + zero.front.z * absolute.pos.z

    ;get absolute.pos.z into registers while multiply proceeds
   ldy #VBASIS_POS_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixZero
   lda MATRIX_FRONT_Z, y
   sta MATHB
   lda MATRIX_FRONT_Z+1, y
   sta MATHA

  ;wait, add zero.pos.z and store to new.pos.z

            WAITSUZY

   ;only need to shift right 8

   ldy MatrixZero
   clc
   lda MATRIX_POS_Z, y
   adc MATHL
   sta _MatrixWorkArea+MATRIX_POS_Z
   lda MATRIX_POS_Z+1, y
   adc MATHK
   sta _MatrixWorkArea+MATRIX_POS_Z+1


 ;(MatrixTarget) = new
 ;
 ; !!this is not the best way of doing things for MPush!!

  ldy _MatrixTarget
  lda _MatrixWorkArea
  sta 0, y
  lda _MatrixWorkArea+1
  sta 1, y
  lda _MatrixWorkArea+2
  sta 2, y
  lda _MatrixWorkArea+3
  sta 3, y
  lda _MatrixWorkArea+4
  sta 4, y
  lda _MatrixWorkArea+5
  sta 5, y
  lda _MatrixWorkArea+6
  sta 6, y
  lda _MatrixWorkArea+7
  sta 7, y
  lda _MatrixWorkArea+8
  sta 8, y
  lda _MatrixWorkArea+9
  sta 9, y
  lda _MatrixWorkArea+10
  sta 10, y
  lda _MatrixWorkArea+11
  sta 11, y
  lda _MatrixWorkArea+12
  sta 12, y
  lda _MatrixWorkArea+13
  sta 13, y
  lda _MatrixWorkArea+14
  sta 14, y
  lda _MatrixWorkArea+15
  sta 15, y
  lda _MatrixWorkArea+16
  sta 16, y
  lda _MatrixWorkArea+17
  sta 17, y
  lda _MatrixWorkArea+18
  sta 18, y
  lda _MatrixWorkArea+19
  sta 19, y
  lda _MatrixWorkArea+20
  sta 20, y
  lda _MatrixWorkArea+21
  sta 21, y
  lda _MatrixWorkArea+22
  sta 22, y
  lda _MatrixWorkArea+23
  sta 23, y

 rts


; MLoad - loads a camera matrix from (_MatrixAbsolute)

.macro STOREHIGH pos
.local local1
.local local2
 bmi local1
 stz pos, x
 bra local2
local1:
 lda #$ff
 sta pos, x
local2:
.endmacro

; AX is base address
_MLoad:
  sta _MatrixAbsolute
  stx _MatrixAbsolute+1

  ;simply transpose 3x3 part

  ldx MatrixOffs

  ; stack.right.x = absolute.right.x

  ldy #VBASIS_RIGHT_X+1
  lda (_MatrixAbsolute), y
  sta MATRIX_RIGHT_X, x

  STOREHIGH MATRIX_RIGHT_X+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_RIGHT_X, x
  rol MATRIX_RIGHT_X+1, x
  asl
  rol MATRIX_RIGHT_X, x
  rol MATRIX_RIGHT_X+1, x

  ; stack.right.y = absolute.up.x

  ldy #VBASIS_UP_X+1
  lda (_MatrixAbsolute), y
  sta MATRIX_RIGHT_Y, x
  STOREHIGH MATRIX_RIGHT_Y+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_RIGHT_Y, x
  rol MATRIX_RIGHT_Y+1, x
  asl
  rol MATRIX_RIGHT_Y, x
  rol MATRIX_RIGHT_Y+1, x

  ; stack.right.z = absolute.front.x

  ldy #VBASIS_FRONT_X+1
  lda (_MatrixAbsolute), y
  sta MATRIX_RIGHT_Z, x
  STOREHIGH MATRIX_RIGHT_Z+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_RIGHT_Z, x
  rol MATRIX_RIGHT_Z+1, x
  asl
  rol MATRIX_RIGHT_Z, x
  rol MATRIX_RIGHT_Z+1, x

  ; stack.up.x = absolute.right.y

  ldy #VBASIS_RIGHT_Y+1
  lda (_MatrixAbsolute), y
  sta MATRIX_UP_X, x
  STOREHIGH MATRIX_UP_X+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_UP_X, x
  rol MATRIX_UP_X+1, x
  asl
  rol MATRIX_UP_X, x
  rol MATRIX_UP_X+1, x

  ; stack.up.y = absolute.up.y

  ldy #VBASIS_UP_Y+1
  lda (_MatrixAbsolute), y
  sta MATRIX_UP_Y, x
  STOREHIGH MATRIX_UP_Y+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_UP_Y, x
  rol MATRIX_UP_Y+1, x
  asl
  rol MATRIX_UP_Y, x
  rol MATRIX_UP_Y+1, x

  ; stack.up.z = absolute.front.y

  ldy #VBASIS_FRONT_Y+1
  lda (_MatrixAbsolute), y
  sta MATRIX_UP_Z, x
  STOREHIGH MATRIX_UP_Z+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_UP_Z, x
  rol MATRIX_UP_Z+1, x
  asl
  rol MATRIX_UP_Z, x
  rol MATRIX_UP_Z+1, x

  ; stack.front.x = absolute.right.z

  ldy #VBASIS_RIGHT_Z+1
  lda (_MatrixAbsolute), y
  sta MATRIX_FRONT_X, x
  STOREHIGH MATRIX_FRONT_X+1

  dey
  lda (_MatrixAbsolute), y

  asl
  rol MATRIX_FRONT_X, x
  rol MATRIX_FRONT_X+1, x
  asl
  rol MATRIX_FRONT_X, x
  rol MATRIX_FRONT_X+1, x

  ; stack.front.y = absolute.up.z

    ldy #VBASIS_UP_Z+1
    lda (_MatrixAbsolute), y
    sta MATRIX_FRONT_Y, x
    STOREHIGH MATRIX_FRONT_Y+1

    dey
    lda (_MatrixAbsolute), y

    asl
    rol MATRIX_FRONT_Y, x
    rol MATRIX_FRONT_Y+1, x
    asl
    rol MATRIX_FRONT_Y, x
    rol MATRIX_FRONT_Y+1, x

   ; stack.front.z = absolute.front.z

    ldy #VBASIS_FRONT_Z+1
    lda (_MatrixAbsolute), y
    sta MATRIX_FRONT_Z, x
    STOREHIGH MATRIX_FRONT_Z+1

    dey
    lda (_MatrixAbsolute), y

    asl
    rol MATRIX_FRONT_Z, x
    rol MATRIX_FRONT_Z+1, x
    asl
    rol MATRIX_FRONT_Z, x
    rol MATRIX_FRONT_Z+1, x

  ;perform slightly more complicated conversion of translation parts :

; stack.pos.x = - (stack.right.x*absolute.pos.x + stack.up.x*absolute.pos.y + stack.front.x*absolute.pos.z);

  WAITSUZY

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;stack.right.z * absolute.pos.x

    ;absolute.pos.x
   ldy #VBASIS_POS_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;stack.right.x
   ldy MatrixOffs
   lda MATRIX_RIGHT_X, y
   sta MATHB
   lda MATRIX_RIGHT_X+1, y
   sta MATHA

  ; + stack.up.x * absolute.pos.y

    ;get absolute.pos.y into registers while multiply proceeds
   ldy #VBASIS_POS_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;stack.up.z
   ldy MatrixOffs
   lda MATRIX_UP_X, y
   sta MATHB
   lda MATRIX_UP_X+1, y
   sta MATHA

  ; + stack.front.x * absolute.pos.z

    ;get absolute.pos.z into registers while multiply proceeds
   ldy #VBASIS_POS_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixOffs
   lda MATRIX_FRONT_X, y
   sta MATHB
   lda MATRIX_FRONT_X+1, y
   sta MATHA

  ;wait, invert and store to stack.pos.x

            WAITSUZY

   ldy MatrixOffs
   sec
   lda #0
   sbc MATHL
   sta MATRIX_POS_X, y
   lda #0
   sbc MATHK
   sta MATRIX_POS_X+1, y

; stack.pos.y = - (stack.right.y*absolute.pos.x + stack.up.y*absolute.pos.y + stack.front.y*absolute.pos.z);

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;stack.right.y * absolute.pos.x

    ;absolute.pos.x
   ldy #VBASIS_POS_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;stack.right.y
   ldy MatrixOffs
   lda MATRIX_RIGHT_Y, y
   sta MATHB
   lda MATRIX_RIGHT_Y+1, y
   sta MATHA

  ; + stack.up.y * absolute.pos.y

    ;get absolute.pos.y into registers while multiply proceeds
   ldy #VBASIS_POS_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;stack.up.z
   ldy MatrixOffs
   lda MATRIX_UP_Y, y
   sta MATHB
   lda MATRIX_UP_Y+1, y
   sta MATHA

  ; + stack.front.y * absolute.pos.z

    ;get absolute.pos.z into registers while multiply proceeds
   ldy #VBASIS_POS_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixOffs
   lda MATRIX_FRONT_Y, y
   sta MATHB
   lda MATRIX_FRONT_Y+1, y
   sta MATHA

  ;wait, invert and store to stack.pos.y

            WAITSUZY

   ldy MatrixOffs
   sec
   lda #0
   sbc MATHL
   sta MATRIX_POS_Y, y
   lda #0
   sbc MATHK
   sta MATRIX_POS_Y+1, y

; stack.pos.z = - (stack.right.z*absolute.pos.x + stack.up.z*absolute.pos.y + stack.front.z*absolute.pos.z);

  ;zero hardware accumulator

   stz MATHM
   stz MATHK

  ;stack.right.z * absolute.pos.x

    ;absolute.pos.x
   ldy #VBASIS_POS_X
   lda (_MatrixAbsolute), y
   sta MATHD
   iny
   lda (_MatrixAbsolute), y
   sta MATHC

    ;stack.right.z
   ldy MatrixOffs
   lda MATRIX_RIGHT_Z, y
   sta MATHB
   lda MATRIX_RIGHT_Z+1, y
   sta MATHA

  ; + stack.up.z * absolute.pos.y

    ;get absolute.pos.y into registers while multiply proceeds
   ldy #VBASIS_POS_Y
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;stack.up.z
   ldy MatrixOffs
   lda MATRIX_UP_Z, y
   sta MATHB
   lda MATRIX_UP_Z+1, y
   sta MATHA

  ; + stack.front.z * absolute.pos.z

    ;get absolute.pos.z into registers while multiply proceeds
   ldy #VBASIS_POS_Z
   lda (_MatrixAbsolute), y
   tax
   iny
   lda (_MatrixAbsolute), y

   ;wait for end of multiply
            WAITSUZY

   stx MATHD
   sta MATHC

    ;zero.front.z
   ldy MatrixOffs
   lda MATRIX_FRONT_Z, y
   sta MATHB
   lda MATRIX_FRONT_Z+1, y
   sta MATHA

  ;wait, invert and store to stack.pos.x

            WAITSUZY

   ldy MatrixOffs
   sec
   lda #0
   sbc MATHL
   sta MATRIX_POS_Z, y
   lda #0
   sbc MATHK
   sta MATRIX_POS_Z+1, y

 rts

; MPerform - takes (VertexSource), (VertexTarget) & VertexCount as arguments,
; applies the current matrix as requested

_MPerform:

 stz Temp1
 stz Temp2

MPerform1:

  ;
  ;dest.x = matrix.right.x * src.x + matrix.up.x * src.y + matrix.front.x * src.z + matrix.pos.x
  ;

  ;clear accumulator

  stz MATHM
  stz MATHK

  ;matrix.right.x * src.x

  ldx MatrixOffs
  lda MATRIX_RIGHT_X, x
  sta MATHD
  lda MATRIX_RIGHT_X+1, x
  sta MATHC

  ldy Temp1
  lda (_VertexSource), y
  sta MATHB
  iny
  lda (_VertexSource), y
  sta MATHA
  iny

  ; + matrix.up.x * src.y

  lda (_VertexSource), y
  tax
  iny
  lda (_VertexSource), y
  iny

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_UP_X, x
   sta MATHB
   lda MATRIX_UP_X+1, x
   sta MATHA

   ; + matrix.front.x * src.z

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_FRONT_X, x
   sta MATHB
   lda MATRIX_FRONT_X+1, x
   sta MATHA

   ; + matrix.pos.x

   ldy Temp2
   clc
   lda MATRIX_POS_X, x

            WAITSUZY

   adc MATHL
   sta (_VertexTarget), y
   iny
   lda MATRIX_POS_X+1, x
   adc MATHK
   sta (_VertexTarget), y
   iny
   sty Temp2

  ;
  ;dest.y = matrix.right.y * src.x + matrix.up.y * src.y + matrix.front.y * src.z + matrix.pos.y
  ;

   ;clear accumulator

   stz MATHM
   stz MATHK

   ;matrix.right.y * src.x

   ldx MatrixOffs
   lda MATRIX_RIGHT_Y, x
   sta MATHD
   lda MATRIX_RIGHT_Y+1, x
   sta MATHC

   ldy Temp1
   lda (_VertexSource), y
   sta MATHB
   iny
   lda (_VertexSource), y
   sta MATHA
   iny

   ; + matrix.up.y * src.y

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y
   iny

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_UP_Y, x
   sta MATHB
   lda MATRIX_UP_Y+1, x
   sta MATHA

   ; + matrix.front.y * src.z

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_FRONT_Y, x
   sta MATHB
   lda MATRIX_FRONT_Y+1, x
   sta MATHA

   ; + matrix.pos.y

   ldy Temp2
   clc
   lda MATRIX_POS_Y, x

            WAITSUZY

   adc MATHL
   sta (_VertexTarget), y
   iny
   lda MATRIX_POS_Y+1, x
   adc MATHK
   sta (_VertexTarget), y
   iny
   sty Temp2

  ;
  ;dest.z = matrix.right.y * src.x + matrix.up.y * src.y + matrix.front.y * src.z + matrix.pos.y
  ;

   ;clear accumulator

   stz MATHM
   stz MATHK

   ;matrix.right.z * src.x

   ldx MatrixOffs
   lda MATRIX_RIGHT_Z, x
   sta MATHD
   lda MATRIX_RIGHT_Z+1, x
   sta MATHC

   ldy Temp1
   lda (_VertexSource), y
   sta MATHB
   iny
   lda (_VertexSource), y
   sta MATHA
   iny

   ; + matrix.up.z * src.y

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y
   iny

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_UP_Z, x
   sta MATHB
   lda MATRIX_UP_Z+1, x
   sta MATHA

   ; + matrix.front.z * src.z

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y

   ; NOTICE : store y this time, hence incrementing source vertex
   iny
   sty Temp1

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_FRONT_Z, x
   sta MATHB
   lda MATRIX_FRONT_Z+1, x
   sta MATHA

   ; + matrix.pos.Z

   ldy Temp2
   clc
   lda MATRIX_POS_Z, x

            WAITSUZY

   adc MATHL
   sta (_VertexTarget), y
   iny
   lda MATRIX_POS_Z+1, x
   adc MATHK
   sta (_VertexTarget), y
   iny
   sty Temp2

  ;loop if more vertices remain

  dec _VertexCount
  beq @L2 ;conditional branch to MPerformDo is too far
  jmp MPerform1

@L2:
 rts


; MPerformRotation - as MPerform, but uses the rotation part of the matrix
; only, i.e. translation is ignored

MPerformRotation:

 stz Temp1
 stz Temp2
 ldy MatrixOffs

MPerformRotation1:

  ;
  ;dest.x = matrix.right.x * src.x + matrix.up.x * src.y + matrix.front.x * src.z + matrix.pos.x
  ;

   ;clear accumulator

   stz MATHM
   stz MATHK

   ;matrix.right.x * src.x

   lda MATRIX_RIGHT_X, y
   sta MATHD
   lda MATRIX_RIGHT_X+1, y
   sta MATHC

   ldy Temp1
   lda (_VertexSource), y
   sta MATHB
   iny
   lda (_VertexSource), y
   sta MATHA
   iny

   ; + matrix.up.x * src.y

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y
   iny

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_UP_X, x
   sta MATHB
   lda MATRIX_UP_X+1, x
   sta MATHA

   ; + matrix.front.x * src.z

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_FRONT_X, x
   sta MATHB
   lda MATRIX_FRONT_X+1, x
   sta MATHA

   ; & store

   ldy Temp2

            WAITSUZY

   lda MATHL
   sta (_VertexTarget), y
   iny
   lda MATHK
   sta (_VertexTarget), y
   iny
   sty Temp2

  ;
  ;dest.y = matrix.right.y * src.x + matrix.up.y * src.y + matrix.front.y * src.z + matrix.pos.y
  ;

   ;clear accumulator

   stz MATHM
   stz MATHK

   ;matrix.right.y * src.x

   lda MATRIX_RIGHT_Y, y
   sta MATHD
   lda MATRIX_RIGHT_Y+1, y
   sta MATHC

   ldy Temp1
   lda (_VertexSource), y
   sta MATHB
   iny
   lda (_VertexSource), y
   sta MATHA
   iny

   ; + matrix.up.y * src.y

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y
   iny

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_UP_Y, x
   sta MATHB
   lda MATRIX_UP_Y+1, x
   sta MATHA

   ; + matrix.front.y * src.z

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_FRONT_Y, x
   sta MATHB
   lda MATRIX_FRONT_Y+1, x
   sta MATHA

   ; & store

   ldy Temp2

            WAITSUZY

   lda MATHL
   sta (_VertexTarget), y
   iny
   lda MATHK
   sta (_VertexTarget), y
   iny
   sty Temp2

  ;
  ;dest.z = matrix.right.y * src.x + matrix.up.y * src.y + matrix.front.y * src.z + matrix.pos.y
  ;

   ;clear accumulator

   stz MATHM
   stz MATHK

   ;matrix.right.z * src.x

   lda MATRIX_RIGHT_Z, y
   sta MATHD
   lda MATRIX_RIGHT_Z+1, y
   sta MATHC

   ldy Temp1
   lda (_VertexSource), y
   sta MATHB
   iny
   lda (_VertexSource), y
   sta MATHA
   iny

   ; + matrix.up.z * src.y

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y
   iny

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_UP_Z, x
   sta MATHB
   lda MATRIX_UP_Z+1, x
   sta MATHA

   ; + matrix.front.z * src.z

   lda (_VertexSource), y
   tax
   iny
   lda (_VertexSource), y

   ; NOTICE : store y this time, incrementing source vertex
   iny
   sty Temp1

            WAITSUZY

   stx MATHD
   sta MATHC

   ldx MatrixOffs
   lda MATRIX_FRONT_Z, x
   sta MATHB
   lda MATRIX_FRONT_Z+1, x
   sta MATHA

   ; & store

   ldy Temp2

            WAITSUZY

   lda MATHL
   sta (_VertexTarget), y
   iny
   lda MATHK
   sta (_VertexTarget), y
   iny
   sty Temp2

  ;loop if more vertices remain

  dec _VertexCount
  beq @L2 ;conditional branch to MPerformDo is too far
  jmp MPerformRotation1

@L2:

 rts

; Project - takes (VertexSource), (VertexTarget) & VertexCount as arguments,
; and leaves perspectified versions of the three byte vertices in VertexSource
; in the two byte VertexTarget

Project_sign: .byte 0

Abs_tmp: .byte 0

Abs_word:
  cpx #$80
  bcs @L1
    rts
@L1:
  sec
  sta Abs_tmp
  lda #0
  sbc Abs_tmp
  stx Abs_tmp
  tax
  lda #0
  sbc Abs_tmp
  stx Abs_tmp
  tax
  lda Abs_tmp
  inc Project_sign 
  rts

_Project:

  stz Temp1
  stz Temp2
  lda #4
  sta Temp3
  stz Project_sign

ProjectDo:

  ; Take abs value of Z, store sign
  ldy Temp3
  lda (_VertexSource), y
  pha
  iny
  lda (_VertexSource), y
  tax
  pla
  jsr Abs_word
    
  ; put z into P:N

  sta MATHP
  stx MATHN

  ; set H to 0

  stz MATHH

  ; advance ptr to next z - y has already been inc'd once

  clc
  tya
  adc #5
  sta Temp3

  ;
  ; x / z
  ;

  ldy Temp1
  iny
  lda (_VertexSource), y
  bmi DivXNeg

   dey
   lda (_VertexSource), y
   sta MATHG
   iny
   lda (_VertexSource), y
   sta MATHF
   stz MATHE
   iny
   sty Temp1

  ; prepare for addition

   clc
   lda #4
   ldy Temp2
   iny

  ; wait for end of divide

            WAITSUZY

  ; read result, shift once, store

   adc MATHC
   lsr
   sta (_VertexTarget), y
   dey
   lda MATHD
   ror
   sta (_VertexTarget), y
   iny
   iny
   sty Temp2

   bra DivY

DivXNeg:

   dey
   sec
   lda #0
   sbc (_VertexSource), y
   sta MATHG
   iny
   lda #0
   sbc (_VertexSource), y
   sta MATHF
   stz MATHE
   iny
   sty Temp1

  ; prepare for subtraction

   sec
   lda #0
   ldy Temp2

  ; wait for end of divide

            WAITSUZY

  ; read result, shift once, store

   sbc MATHD
   sta (_VertexTarget), y
   iny
   lda #4
   sbc MATHC
   lsr
   sta (_VertexTarget), y

   dey
   lda (_VertexTarget), y
   ror
   sta (_VertexTarget), y

   iny
   iny
   sty Temp2

DivY:
  ; If negative, invert
  lda Project_sign
  beq @L1
    ldy Temp2
    dey
    dey
    sec
    lda #0
    sbc (_VertexTarget), y
    sta (_VertexTarget), y
    iny
    lda #0
    sbc (_VertexTarget), y
    sta (_VertexTarget), y
@L1:

  ;
  ; y / z
  ;

   ldy Temp1
   iny
   lda (_VertexSource), y
   bmi DivYNeg

   dey
   lda (_VertexSource), y
   sta MATHG
   iny
   lda (_VertexSource), y
   sta MATHF
   stz MATHE
   iny
   iny
   iny
   sty Temp1

  ; prepare for addition

   clc
   lda #4
   ldy Temp2
   iny

  ; wait for end of divide

            WAITSUZY

  ; read result, shift once, store

   adc MATHC
   lsr
   sta (_VertexTarget), y
   dey
   lda MATHD
   ror
   sta (_VertexTarget), y
   iny
   iny
   sty Temp2

   bra ConsiderLoop

DivYNeg:

   dey
   sec
   lda #0
   sbc (_VertexSource), y
   sta MATHG
   iny
   lda #0
   sbc (_VertexSource), y
   sta MATHF
   stz MATHE
   iny
   iny
   iny
   sty Temp1

  ; prepare for subtraction

   sec
   lda #0
   ldy Temp2

  ; wait for end of divide

            WAITSUZY

  ; read result, shift once, store

   sbc MATHD
   sta (_VertexTarget), y
   iny
   lda #4
   sbc MATHC
   lsr
   sta (_VertexTarget), y

   dey
   lda (_VertexTarget), y
   ror
   sta (_VertexTarget), y

   iny
   iny
   sty Temp2

  ;loop if more vertices remain
ConsiderLoop:
  ; If negative, invert
  lda Project_sign
  and #1
  beq @L1
    ldy Temp2
    dey
    dey
    sec
    lda #0
    sbc (_VertexTarget), y
    sta (_VertexTarget), y
    iny
    lda #0
    sbc (_VertexTarget), y
    sta (_VertexTarget), y
@L1:

  dec _VertexCount
  beq ProjectEnd
  jmp ProjectDo

ProjectEnd:

 rts