ColorText: .dd $ffffff 
ColorBackground: .dd 0

Rectangle: ; R0 - Color, R1 - X, R2 - Y, R3 - Width, R4 - Height   
  PUSH R5 ; loop X
  PUSH R6 ; loop Y
  PUSH R7 ; color
  PUSH R8 ; X
  PUSH R9 ; Y
  COPY R7, R0
  COPY R8, R1
  COPY R9, R2
  COPY R6, R4
  PUSH R0
  PUSH R1
RectangleStart:
  COPY R0, R8
  COPY R1, R9
  CALL SetPixelAddr  
  COPY R5, R3
RectangleLine:
  OUT  (ScreenWriteData), R7
  DEC  R5
  JPNZ R5, RectangleLine
  DEC  R6
  INC  R9
  JPNZ R6, RectangleStart
  POP  R1
  POP  R0
  POP  R9
  POP  R8
  POP  R7
  POP  R6
  POP  R3  
  RET
  
SetPixelAddr: ; R0 - X, R1 - Y
  PUSH R2
  IN   R2, (ScreenGetWidth)
  SHL  R2, 2 ; multiply by 4
  SHL  R0, 2 ; multiply by 4
  MUL  R1, R2
  ADD  R0, R1
  OUT  (ScreenSetAddr), R0
  POP  R2
  RET
  
  .const CharWidth 8
  .const CharHeight 8
  
WriteChar: ; R0 - Character, R1 - X, R2 - Y
  PUSH R3 ; Address of character
  PUSH R4 ; Loop register Y
  PUSH R5 ; X
  PUSH R6 ; Y
  PUSH R7 ; value
  PUSH R8 ; screen width
  PUSH R9 ; Loop register X
  PUSH R10 ; Foreground color
  PUSH R11 ; Background color
  PUSH R12 ; temp value
  COPY R5, R1
  COPY R6, R2
  SET  R3, Font8x8
  SHL  R0, 3
  ADD  R3, R0
  IN   R8, (ScreenGetWidth)
  SET  R4, CharHeight
  LDI  R10, (ColorText)
  LDI  R11, (ColorBackground)
WriteCharLoopY:
  COPY R0, R5
  COPY R1, R6
  CALL SetPixelAddr
  LD   R7, (R3)
  SET  R9, CharWidth
WriteCharLoopX:
  COPY R12, R7
  ANDI R12, 1
  JPZ  R12, WriteCharNext
  OUT  (ScreenWriteData), R10
  JP   WriteCharNext2
WriteCharNext:
  OUT  (ScreenWriteData), R11
WriteCharNext2:
  DEC  R9
  SHR  R7, 1
  JPNZ R9, WriteCharLoopX
  DEC  R4
  INC  R6
  INC  R3
  JPNZ R4, WriteCharLoopY
  POP  R12
  POP  R11
  POP  R10
  POP  R9
  POP  R8
  POP  R7
  POP  R6
  POP  R5
  POP  R4
  POP  R3
  RET
  
TextOut: ; R0 - Text address, R1 - X, R2 - Y
  PUSH R3 ; loop variable
  PUSH R4 ; character width
  PUSH R6 ; X
  PUSH R7 ; Y
  PUSH R1
  PUSH R2
  PUSH R0
  COPY R6, R1
  COPY R7, R2
  COPY R3, R0
  SET  R4, CharWidth  
TextOutLoop:
  LD   R0, (R3)
  ANDI R0, $ff
  JPZ  R0, TextOutLoop2
  COPY R1, R6
  COPY R2, R7
  CALL WriteChar
  ADD  R6, R4
  INC  R3
  JP   TextOutLoop
TextOutLoop2:
  POP  R0
  POP  R2
  POP  R1
  POP  R7
  POP  R6
  POP  R4
  POP  R3
  RET

  .include Font.asm