Option explicit
Option default integer
Dim o
'
' state machine usage
' PIO0 SM0 pixel clock and new line trigger for master controller, vsync and hsync
' PIO0 SM1 hsync clock and end of horizontal backporch trigger
' PIO0 SM2 data output triggered by master controller, aalso outputs DE signal
' PIO1 SM0 vsync clock and end of vertical backporch trigger
' PIO1 SM1 master controller triggered by newline, vertical backporch and horizontal bacporch triggers
'irq usage
'PIO0 irq 0 'new line triggers hsync
'PIO1 irq 1 'master controller triggers data output
'PIO1 irq 0 'new line triggers vsync
'PIO1 irq 1 'end of vertical backporch triggers master controller
'PIO1 irq 2 'end of horizontal backporch triggers master controller active line start
' set pins that will be used as outputs
SetPin gp4,pio0 'clock pulse
SetPin gp16,pio0 'hsync
SetPin gp17,pio1 'vsync
SetPin gp7,pio0 'DE
SetPin gp8,pio1
SetPin gp18,pio0 'low Blue
SetPin gp19,pio0 'high blue
SetPin gp20,pio0 'low green
SetPin gp21,pio0 'high green
SetPin gp22,pio0 'low red
SetPin gp23,pio0 'high red
Const hsync=96
Const hfrontporch=16
Const cyclesperpixel=10
Const hvisible=640
Const hbackporch=48
Const vsync=2
Const vbackporch=33
Const vfrontporch=10
Const vvisible=480
Const pixelsperword=5
Const wordsperline=hvisible\pixelsperword
Const wordstotransfer=wordsperline*vvisible
Const vlines=vsync+vbackporch+vvisible+vfrontporch
Const hwholeline=hsync+hfrontporch+hvisible+hbackporch
Const hvisibleclock=hvisible*cyclesperpixel
Const hsyncclock=hsync*cyclesperpixel
Const hfrontporchclock=hfrontporch*cyclesperpixel
Const hbackporchclock=hbackporch*cyclesperpixel
Const vwholeframe=vlines*hwholeline*cyclesperpixel
Const vvisibleclock=vvisible*hwholeline*cyclesperpixel
Const vsyncclock=vsync*hwholeline*cyclesperpixel
Const vfrontporchclock=vfrontporch*hwholeline*cyclesperpixel
Const vbackporchclock=vbackporch*hwholeline*cyclesperpixel
Const clock=Val(MM.Info(cpuspeed))
Const HRes=hvisible
Const VRes=vvisible
Dim i,b(wordstotransfer\2-1)
Dim mask_table(4)=(&H3F,&HFC0,&H3F000,&HFC0000,&H3F000000)
Print vsyncclock,vbackporchclock,vvisibleclock,vfrontporchclock

Dim badd=Peek(varaddr b())
'
o=Pio(next line 1) 'note the start of the first PIO instruction for program 1 - will be 0
Print "compiling pclk"
PIO assemble 0
.program pclk
.side set 1
.line next 'we can always use next unless we specifically want to place the code
 Pull block 'read in the loop counter which will define the timing of the second program
.wrap target
 Mov x,osr side 1 [4] 'we can use osr to keep this and re-use it
.label loop 'loop as specified by the counter
 Nop side 0 [4]
 Jmp x--,loop side 1 [4]
 IRQ 0 side 0 'set a flag for the hsync
 IRQ NEXT 0 side 0 [3] 'set a flag for the vsync
.wrap
.end program list
'PIO CONFIGURE pio, sm,clock [,startaddress][,sidesetbase] [,sidesetno][,sidesetout][,setbase] [,setno] [,setout]
'[,outbase] [,outno] [,outout][,inbase][,jmppin] [,wraptarget] [,wrap][,sideenable] [,sidepindir][,pushthreshold]
'[,pullthreshold] [,autopush][,autopull] [,inshiftdir][,outshiftdir][,joinrxfifo] [,jointxfifo][,joinrxfifoget] [,joinrxfifoput]
' create the setting to initialise program 1
PIO CONFIGURE 0,0,clock,o,gp4,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap)
' initialise and start program 1 but note it will block on pull block
PIO start 0,0

'
o=Pio(next line) 'note the start of the second pio program
Print "compiling hsyncclock @ line: ";o
PIO assemble 0
.program hsyncclock
.side set 1
.line next 'this will pick up o2 automatically
 Pull block side 1 'get the length of the backporch
 Mov y,osr side 1 'save the length of the sync pulse into y
 Pull block side 1' get the length of the sync
.wrap target
 Mov x,osr side 1 'get the length of the sync
 Wait 1 irq 0 side 1 'this instruction waits for irq 1 to be set by the pixel clock and automatically clears it
 ' this happens at the start of each line
.label loop1
 Jmp x--,loop1 side 0 'sync pulse
 Mov x,y side 1 'restore the backporch
.label loop2
 Jmp x--,loop2 side 1
 IRQ NEXT 2 side 1
'the data should start here followed by the frontporch
.wrap
.end program list
' create the setting to initialise program 2
PIO CONFIGURE 0,1,clock,o,gp16,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap)
' initialise and start program 2, it will block waiting for the irq
PIO start 0,1
PIO write 0,1,2,hbackporchclock-1,hsyncclock-1
'

'data output program
o= Pio(next line)
Print "compiling data @ line: ";o
PIO assemble 0
.program linedata
.line next
.side set 1
Pull block side 0
Mov y,osr
.wrap target
Mov x,y side 0'get the active pixel count divided by the number of pixels per word
Wait 1 irq 1 side 0 [9]'wait for next valid line to start
.label loop7
Pull  side 1 'get the next data - don't block
Out pins,6 side 1 [9]
Out pins,6 side 1 [9]
Out pins,6 side 1 [9]
Out pins,6 side 1 [9]
Out pins,6 side 1 [6]
Out NULL,2 side 1
Jmp x--,loop7 side 1
Mov pins, NULL side 0
.wrap
.end program list
' create the setting to initialise program 3
PIO CONFIGURE 0,2,clock,o,GP7,1,1,,,,GP18,6,1,,,Pio(.wrap target),Pio(.wrap)
PIO start 0,2
PIO write 0,2,1,hvisible\pixelsperword-1

o=Pio(next line 1) 'note the start of the first PIO instruction for program 1 - will be 0
Print "compiling vsyncclock @ line: ";0
PIO assemble 1
.program vsyncclock1
.side set 1
.line next '
'
 Pull block side 1  ' get sync period
 Mov isr,osr side 1
 Mov rxfifo[0],isr side 1 'store in rxf[0]
'
 Pull block side 1  ' get backporch period
 Mov isr,osr side 1
 Mov rxfifo[1],isr side 1 'store in rxf[1]
'
 Pull block side 1  ' get active + frontporch period
 Mov y,osr 'store in y
'
.wrap target

 Mov osr,rxfifo[0] side 1'get the sync period
 Mov x,osr side 1
 Wait 1 irq 0 side 1
.label loop3
 Wait 1 irq 0 side 0
 Jmp x--,loop3 side 0 'sync loop
'
 Mov osr,rxfifo[1] side 1 'get the backporch period
 Mov x,osr side 1
.label loop4
 Wait 1 irq 0 side 1
 Jmp x--,loop4 side 1 'backporch loop
'
' trigger the master that active lines should start
 IRQ 1 side 1
'
 Mov x,y side 1
.label loop5
 Jmp x--,loop5 side 1 'active + frontporch loop
 IRQ CLEAR 0 side 1 'get rid of accumulated line start irqs
.wrap
.end program list
' create the setting to initialise program 3
PIO CONFIGURE 1,0,clock,o,gp17,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap),,,,,,,,,,,1,1
' initialise and start program 3, it will block waiting for the irq
PIO start 1,0
'rest of line value is reduced to make sure no overun but also don't miss a line
PIO write 1,0,3,vsync-1,vbackporch-1,vvisibleclock+vfrontporchclock-1000

' master data output control program
o=Pio(next line) 'note the start of the first PIO instruction for program 1 - will be 0
Print "compiling datamaster @ line: ";o
PIO assemble 1
.program datamaster
.side set 1
.line next
Pull block side 0
Mov y,osr
.wrap target 'main loop
Mov x,y side 1'get the active line count
Wait 1 irq 1 side 1'wait for the start of frame
IRQ CLEAR 2 side 1
.label loop6
Wait 1 irq 2 side 0 'wait for the next line to start
IRQ PREV 1 side 0'trigger the output
Jmp x--,loop6 side 0
.wrap
.end program list
PIO CONFIGURE 1,1,clock,o,gp8,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap)
PIO start 1,1
PIO write 1,1,1,vvisible-1

*/
' loop, although the PIO will keep running anyway even if the program finishes
' trigger the whole shebang by writing to program 1 fifo
PIO sync
PIO dma tx 0,2,wordstotransfer,b(),redo
PIO write 0,0,1,hwholeline-1

'
On error skip
Option lcdpanel user,640,480
Do
CLS RGB(0,128,0)
Line 0,0,MM.HRES-1,MM.VRES-1
Line 0,MM.VRES-1,MM.HRES-1,0
Pause 1000
CLS RGB(0,127,128)
Circle MM.HRES\2,MM.VRES\2,150,5,1.3,RGB(0,128,0),RGB(128,0,127)
Pause 1000
Text MM.HRES\2,MM.VRES\2,"Hello",CM,,4,RGB(0,128,0),RGB(128,0,127))
Pause 2000
Loop

Sub redo
PIO dma tx 0,2,wordstotransfer,b(),redo
End Sub

Function rgb222(c)
 rgb222= (c And &HC00000) >> 18
 rgb222 = rgb222 Or (c And &HC000)>>12
 rgb222 = rgb222 Or (c And &Hc0) >>6
End Function


Sub mm.user_rectangle x1,y1,x2,y2,col
Local x,y,p,t,wo,sw,ew,sp,ep,mask,w,i,count
Local c,b=rgb222(col)
c=b
c=c+(b<<6)
c=c+(b<<12)
c=c+(b<<18)
c=c+(b<<24)

' Pre-calculate common masks (major speedup!)

For y=y1 To y2
  wo=y*wordsperline
  sw=x1\pixelsperword
  sp=x1 Mod pixelsperword
  ew=x2\pixelsperword
  ep=x2 Mod pixelsperword

  If sw=ew Then
    ' Single word case
    mask=0
    For i=sp To ep
      mask=mask Or mask_table(i)
    Next
    p=badd+(wo+sw)*4
    t=Peek(word p)
    t=t And INV mask
    t=t Or (c And mask)
    Poke word p,t
  Else
    ' Handle first partial word
    If sp<>0 Then
      mask=0
      For i=sp To 4
        mask=mask Or mask_table(i)
      Next
      p=badd+(wo+sw)*4
      t=Peek(word p)
      t=t And INV mask
      t=t Or (c And mask)
      Poke word p,t
      sw=sw+1
    EndIf

    ' Handle full words in the middle - use MEMORY SET WORD
    count=ew-sw
    If count>0 Then
      p=badd+(wo+sw)*4
      Memory SET WORD p,c,count
    EndIf

    ' Handle last partial word
    If ep<>4 Then
      mask=0
      For i=0 To ep
        mask=mask Or mask_table(i)
      Next
      p=badd+(wo+ew)*4
      t=Peek(word p)
      t=t And INV mask
      t=t Or (c And mask)
      Poke word p,t
    Else
      ' End pixel is exactly at word boundary (pixel 4)
      p=badd+(wo+ew)*4
      Poke word p,c
    EndIf
  EndIf
Next
End Sub

Sub mm.user_bitmap x1,y1,width,height,scale,fc,bc,bitmap
Local f,ff,b,bb,i,j,k,x,y,final_width,final_height
Local draw_x_start,draw_y_start,draw_x_end,draw_y_end
Local src_y_start,src_x_start,draw_background
Local bit_index,byte_index,bit_position,pixel_on
Local x_start,x_end
Local p,wo,sw,sp,mask,t,px
Local c_word
' Convert 6-bit colors to full word (5 pixels)
ff=rgb222(fc)
f=ff Or (ff<<6) Or (ff<<12) Or (ff<<18) Or (ff<<24)
If bc>=0 Then
  bb=rgb222(bc)
  b=bb Or (bb<<6) Or (bb<<12) Or (bb<<18) Or (bb<<24)
  draw_background=1
Else
  draw_background=0
EndIf
' Calculate final dimensions
final_width=width*scale
final_height=height*scale
' Early exit if completely off-screen
If x1>=HRes Or y1>=VRes Or x1+final_width<=0 Or y1+final_height<=0 Then Exit Sub
' Calculate clipping bounds
If x1<0 Then draw_x_start=0 Else draw_x_start=x1
If y1<0 Then draw_y_start=0 Else draw_y_start=y1
If x1+final_width>HRes Then draw_x_end=HRes Else draw_x_end=x1+final_width
If y1+final_height>VRes Then draw_y_end=VRes Else draw_y_end=y1+final_height
' Calculate starting bitmap position (for clipping)
If y1<0 Then src_y_start=(-y1)\scale Else src_y_start=0
If x1<0 Then src_x_start=(-x1)\scale Else src_x_start=0
' Optimize for scale=1 (common case)
If scale=1 Then
  For i=src_y_start To height-1
    y=y1+i
    If y>=VRes Then Exit For
    If y<0 Then Continue For
    wo=y*wordsperline
    For k=src_x_start To width-1
      x=x1+k
      If x>=HRes Then Exit For
      If x<0 Then Continue For
      ' Get bit from bitmap
      bit_index=i*width+k
      byte_index=bit_index>>3
      bit_position=7-(bit_index And 7)
      pixel_on=(PEEK(BYTE bitmap+byte_index)>>bit_position) And 1
      ' Calculate word and pixel position
      sw=x\5
      sp=x Mod 5
      p=badd+(wo+sw)*4
      If pixel_on Then
        ' Draw foreground pixel
        mask=mask_table(sp)
        t=Peek(word p)
        t=t And INV mask
        t=t Or (f And mask)
        Poke word p,t
      ElseIf draw_background Then
        ' Draw background pixel
        mask=mask_table(sp)
        t=Peek(word p)
        t=t And INV mask
        t=t Or (b And mask)
        Poke word p,t
      EndIf
    Next k
  Next i
Else
  ' Scaled version
  For i=src_y_start To height-1
    ' Process scale lines for this source row
    For j=0 To scale-1
      y=y1+i*scale+j
      If y>=VRes Then Exit For
      If y<0 Then Continue For
      wo=y*wordsperline
      For k=src_x_start To width-1
        ' Get bit from bitmap
        bit_index=i*width+k
        byte_index=bit_index>>3
        bit_position=7-(bit_index And 7)
        pixel_on=(PEEK(BYTE bitmap+byte_index)>>bit_position) And 1
        ' Determine color to draw
        If pixel_on Then
          c_word=f
        ElseIf draw_background Then
          c_word=b
        Else
          Continue For ' Skip transparent pixels
        EndIf
        ' Draw scale pixels horizontally
        x_start=x1+k*scale
        x_end=x_start+scale
        ' Clip horizontal span
        If x_start<0 Then x_start=0
        If x_end>HRes Then x_end=HRes
        If x_start<x_end Then
          ' Draw the horizontal run of pixels
          For px=x_start To x_end-1
            sw=px\5
            sp=px Mod 5
            p=badd+(wo+sw)*4
            mask=mask_table(sp)
            t=Peek(word p)
            t=t And INV mask
            t=t Or (c_word And mask)
            Poke word p,t
          Next px
        EndIf
      Next k
    Next j
  Next i
EndIf
End Sub

Option explicit
Option default integer
Dim o
'
' state machine usage
' PIO0 SM0 pixel clock and new line trigger for master controller, vsync and hsync
' PIO0 SM1 master controller triggered by newline, vertical backporch and horizontal backporch triggers
' PIO0 SM2 data output triggered by master controller, aalso outputs DE signal
' PIO1 SM0 vsync clock and end of vertical backporch trigger
' PIO1 SM1 hsync clock and end of horizontal backporch trigger
'irq usage
'PIO0 irq 0 'master controller triggers data output
'PIO0 irq 1 'end of vertical backporch triggers master controller
'PIO0 irq 2 'end of horizontal backporch triggers master controller active line start
'PIO1 irq 0 'new line triggers vsync
'PIO1 irq 1 'new line triggers hsync
' set pins that will be used as outputs
SetPin gp4,pio0 'clock pulse
SetPin gp16,pio1 'hsync
SetPin gp17,pio1 'vsync
SetPin gp7,pio0 'DE
SetPin gp18,pio0 'low Blue
SetPin gp19,pio0 'high blue
SetPin gp20,pio0 'low green
SetPin gp21,pio0 'high green
SetPin gp22,pio0 'low red
SetPin gp23,pio0 'high red
Const hnative=640
Const cyclesperpixel=10
Const hscale=640\MM.HRES
Const hsync=96\hscale
Const hfrontporch=16\hscale
Const hvisible=hnative\hscale
Const hbackporch=48\hscale
Const vsync=1
Const vbackporch=33
Const vfrontporch=10
Const vvisible=480
Const pixelsperword=5
Const wordsperline=hvisible\pixelsperword
Const wordstotransfer=wordsperline*vvisible
Const vlines=vsync+vbackporch+vvisible+vfrontporch
Const hwholeline=hsync+hfrontporch+hvisible+hbackporch
Const hvisibleclock=hvisible*cyclesperpixel
Const hsyncclock=hsync*cyclesperpixel
Const hfrontporchclock=hfrontporch*cyclesperpixel
Const hbackporchclock=hbackporch*cyclesperpixel
Const clock=Val(MM.Info(cpuspeed))
Const HRes=hvisible
Const VRes=vvisible\hscale
Dim i,b(wordstotransfer\2-1)
Dim mask_table(4)=(&H3F,&HFC0,&H3F000,&HFC0000,&H3F000000)

Dim badd=Peek(varaddr b())
'
o=Pio(next line 1) 'note the start of the first PIO instruction for program 1 - will be 0
Print "compiling pclk"
PIO assemble 0
.program pclk
.side set 1
.line next 'we can always use next unless we specifically want to place the code
Pull block 'read in the loop counter which will define the timing of the second program
.wrap target
Mov x,osr side 1 [4] 'we can use osr to keep this and re-use it
.label loop 'loop as specified by the counter
Nop side 0 [4]
Jmp x--,loop side 1 [4]
IRQ NEXT 1 side 0 'set a flag for the hsync
IRQ NEXT 0 side 0 [3] 'set a flag for the vsync
.wrap
.end program list
'PIO CONFIGURE pio, sm,clock [,startaddress][,sidesetbase] [,sidesetno][,sidesetout][,setbase] [,setno] [,setout]
'[,outbase] [,outno] [,outout][,inbase][,jmppin] [,wraptarget] [,wrap][,sideenable] [,sidepindir][,pushthreshold]
'[,pullthreshold] [,autopush][,autopull] [,inshiftdir][,outshiftdir][,joinrxfifo] [,jointxfifo][,joinrxfifoget] [,joinrxfifoput]
' create the setting to initialise program 1
PIO CONFIGURE 0,0,clock\hscale,o,gp4,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap)
' initialise and start program 1 but note it will block on pull block
PIO start 0,0

'
' master data output control program
o=Pio(next line) 'note the start of the first PIO instruction for program 1 - will be 0
Print "compiling datamaster @ line: ";o
PIO assemble 0
.program datamaster
'.side set 1
.line next
Pull block
Mov y,osr
.wrap target 'main loop
Mov x,y 'get the active line count
Wait 1 irq 1 'wait for the start of frame
IRQ CLEAR 2
.label loop6
Wait 1 irq 2  'wait for the next line to start
IRQ 0 'trigger the output
Jmp x--,loop6
.wrap
.end program list
PIO CONFIGURE 0,1,clock,o,,,,,,,,,,,,Pio(.wrap target),Pio(.wrap)
PIO start 0,1
PIO write 0,1,1,vvisible-1

'data output program
o= Pio(next line)
Print "compiling data @ line: ";o
PIO assemble 0
.program linedata
.line next
.side set 1
Pull block side 0
Mov y,osr
.wrap target
Mov x,y side 0'get the active pixel count divided by the number of pixels per word
Wait 1 irq 0 side 0 [9]'wait for next valid line to start
.label loop7
Pull  side 1 'get the next data - don't block
Out pins,6 side 1 [9]
Out pins,6 side 1 [9]
Out pins,6 side 1 [9]
Out pins,6 side 1 [9]
Out pins,6 side 1 [6]
Out NULL,2 side 1
Jmp x--,loop7 side 1
Mov pins, NULL side 0
.wrap
.end program list
' create the setting to initialise program 3
PIO CONFIGURE 0,2,clock\hscale,o,GP7,1,1,,,,GP18,6,1,,,Pio(.wrap target),Pio(.wrap)
PIO start 0,2
PIO write 0,2,1,hvisible\pixelsperword-1

o=Pio(next line 1) 'note the start of the first PIO instruction for program 1 - will be 0
Print "compiling vsyncclock @ line: ";0
PIO assemble 1
.program vsyncclock1
.side set 1
.line next '
'
Pull block side 1  ' get sync period
Mov isr,osr side 1
Mov rxfifo[0],isr side 1 'store in rxf[0]
'
Pull block side 1  ' get backporch period
Mov isr,osr side 1
Mov rxfifo[1],isr side 1 'store in rxf[1]
'
Pull block side 1  ' get active + frontporch period
Mov y,osr 'store in y
'
.wrap target

Mov osr,rxfifo[0] side 1'get the sync period
Mov x,osr side 1
Wait 1 irq 0 side 1
.label loop3
Wait 1 irq 0 side 0
Jmp x--,loop3 side 0 'sync loop
'
Mov osr,rxfifo[1] side 1 'get the backporch period
Mov x,osr side 1
.label loop4
Wait 1 irq 0 side 1
Jmp x--,loop4 side 1 'backporch loop
'
' trigger the master that active lines should start
IRQ PREV 1 side 1
'
Mov x,y side 1
.label loop5
Wait 1 irq 0 side 1
Jmp x--,loop5 side 1 'active + frontporch loop
.wrap
.end program list
' create the setting to initialise program 3
PIO CONFIGURE 1,0,clock,o,gp17,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap),,,,,,,,,,,1,1
' initialise and start program 3, it will block waiting for the irq
PIO start 1,0
'rest of line value is reduced to make sure no overun but also don't miss a line
PIO write 1,0,3,vsync-1,vbackporch-1,vvisible+vfrontporch-2

o=Pio(next line) 'note the start of the second pio program
Print "compiling hsyncclock @ line: ";o
PIO assemble 1
.program hsyncclock
.side set 1
.line next 'this will pick up o2 automatically
Pull block side 1 'get the length of the backporch
Mov y,osr side 1 'save the length of the sync pulse into y
Pull block side 1' get the length of the sync
.wrap target
Mov x,osr side 1 'get the length of the sync
Wait 1 irq 1 side 1 'this instruction waits for irq 0 to be set by the pixel clock and automatically clears it
' this happens at the start of each line
.label loop1
Jmp x--,loop1 side 0 'sync pulse
Mov x,y side 1 'restore the backporch
.label loop2
Jmp x--,loop2 side 1
IRQ PREV 2 side 1
'the data should start here followed by the frontporch
.wrap
.end program list
' create the setting to initialise program 2
PIO CONFIGURE 1,1,clock\hscale,o,gp16,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap)
' initialise and start program 2, it will block waiting for the irq
PIO start 1,1
PIO write 1,1,2,hbackporchclock-1,hsyncclock-1
'
' loop, although the PIO will keep running anyway even if the program finishes
' trigger the whole shebang by writing to program 1 fifo
PIO sync
PIO dma tx 0,2,wordstotransfer,b(),repo
PIO write 0,0,1,hwholeline-1
'
Do
CLS RGB(0,128,0)
Line 0,0,MM.HRES-1,MM.VRES-1
Line 0,MM.VRES-1,MM.HRES-1,0
Pause 1000
CLS RGB(0,127,128)
Circle MM.HRES\2,MM.VRES\2,150\hscale,5,1.3,RGB(0,128,0),RGB(128,0,127)
Pause 1000
Text MM.HRES\2,MM.VRES\2,"Hello",CM,,3,RGB(0,128,0),RGB(128,0,127))
Pause 2000
Loop

Sub repo
PIO dma tx 0,2,wordstotransfer,b(),repo
End Sub

Function rgb222(c)
rgb222= (c And &HC00000) >> 18
rgb222 = rgb222 Or (c And &HC000)>>12
rgb222 = rgb222 Or (c And &Hc0) >>6
End Function


Sub mm.user_rectangle x1,y1,x2,y2,col
 Local x,y,p,t,wo,sw,ew,sp,ep,mask,w,i,count
 Local c,b=rgb222(col)
 Local dup, output_y, copy_sw, copy_ew

 c=b
 c=c+(b<<6)
 c=c+(b<<12)
 c=c+(b<<18)
 c=c+(b<<24)

 ' Pre-calculate common masks (major speedup!)
 output_y=y1*hscale
 For y=y1 To y2
   wo=output_y*wordsperline
   sw=x1\pixelsperword
   sp=x1 Mod pixelsperword
   ew=x2\pixelsperword
   ep=x2 Mod pixelsperword

   ' Save original sw/ew for copying
   copy_sw=sw
   copy_ew=ew

   If sw=ew Then
     ' Single word case
     mask=0
     For i=sp To ep
       mask=mask Or mask_table(i)
     Next
     p=badd+(wo+sw)*4
     t=Peek(word p)
     t=t And INV mask
     t=t Or (c And mask)
     Poke word p,t
   Else
     ' Handle first partial word
     If sp<>0 Then
       mask=0
       For i=sp To 4
         mask=mask Or mask_table(i)
       Next
       p=badd+(wo+sw)*4
       t=Peek(word p)
       t=t And INV mask
       t=t Or (c And mask)
       Poke word p,t
       sw=sw+1
     EndIf

     ' Handle full words in the middle - use MEMORY SET WORD
     count=ew-sw
     If count>0 Then
       p=badd+(wo+sw)*4
       Memory SET WORD p,c,count
     EndIf

     ' Handle last partial word
     If ep<>4 Then
       mask=0
       For i=0 To ep
         mask=mask Or mask_table(i)
       Next
       p=badd+(wo+ew)*4
       t=Peek(word p)
       t=t And INV mask
       t=t Or (c And mask)
       Poke word p,t
     Else
       ' End pixel is exactly at word boundary (pixel 4)
       p=badd+(wo+ew)*4
       Poke word p,c
     EndIf
   EndIf

   ' Duplicate this line hscale-1 times
   If hscale > 1 Then
     count=copy_ew-copy_sw+1
     For dup=1 To hscale-1
       Memory COPY badd+(wo+copy_sw)*4, badd+((wo+dup*wordsperline)+copy_sw)*4, count*4
     Next
   EndIf

   output_y=output_y+hscale
 Next
End Sub

Sub mm.user_bitmap x1,y1,width,height,scale,fc,bc,bitmap
 Local f,ff,b,bb,i,j,k,x,y,final_width,final_height
 Local draw_x_start,draw_y_start,draw_x_end,draw_y_end
 Local src_y_start,src_x_start,draw_background
 Local bit_index,byte_index,bit_position,pixel_on
 Local x_start,x_end
 Local p,wo,sw,sp,mask,t,px
 Local c_word
 Local output_y,dup,count,copy_sw,copy_ew

 ' Convert 6-bit colors to full word (5 pixels)
 ff=rgb222(fc)
 f=ff Or (ff<<6) Or (ff<<12) Or (ff<<18) Or (ff<<24)
 If bc>=0 Then
   bb=rgb222(bc)
   b=bb Or (bb<<6) Or (bb<<12) Or (bb<<18) Or (bb<<24)
   draw_background=1
 Else
   draw_background=0
 EndIf

 ' Calculate final dimensions
 final_width=width*scale
 final_height=height*scale

 ' Early exit if completely off-screen
 If x1>=HRes Or y1>=VRes Or x1+final_width<=0 Or y1+final_height<=0 Then Exit Sub

 ' Calculate clipping bounds
 If x1<0 Then draw_x_start=0 Else draw_x_start=x1
 If y1<0 Then draw_y_start=0 Else draw_y_start=y1
 If x1+final_width>HRes Then draw_x_end=HRes Else draw_x_end=x1+final_width
 If y1+final_height>VRes Then draw_y_end=VRes Else draw_y_end=y1+final_height

 ' Calculate starting bitmap position (for clipping)
 If y1<0 Then src_y_start=(-y1)\scale Else src_y_start=0
 If x1<0 Then src_x_start=(-x1)\scale Else src_x_start=0

 ' Optimize for scale=1 (common case)
 If scale=1 Then
   output_y=y1*hscale
   For i=src_y_start To height-1
     y=y1+i
     If y>=VRes Then Exit For
     If y<0 Then
       output_y=output_y+hscale
       Continue For
     EndIf
     wo=output_y*wordsperline

     ' Save copy range for duplication
     copy_sw=x1\5
     copy_ew=(x1+width-1)\5

     For k=src_x_start To width-1
       x=x1+k
       If x>=HRes Then Exit For
       If x<0 Then Continue For
       ' Get bit from bitmap
       bit_index=i*width+k
       byte_index=bit_index>>3
       bit_position=7-(bit_index And 7)
       pixel_on=(PEEK(BYTE bitmap+byte_index)>>bit_position) And 1
       ' Calculate word and pixel position
       sw=x\5
       sp=x Mod 5
       p=badd+(wo+sw)*4
       If pixel_on Then
         ' Draw foreground pixel
         mask=mask_table(sp)
         t=Peek(word p)
         t=t And INV mask
         t=t Or (f And mask)
         Poke word p,t
       ElseIf draw_background Then
         ' Draw background pixel
         mask=mask_table(sp)
         t=Peek(word p)
         t=t And INV mask
         t=t Or (b And mask)
         Poke word p,t
       EndIf
     Next k

     ' Duplicate this line hscale-1 times
     If hscale > 1 Then
       count=copy_ew-copy_sw+1
       For dup=1 To hscale-1
         Memory COPY badd+(wo+copy_sw)*4, badd+((wo+dup*wordsperline)+copy_sw)*4, count*4
       Next
     EndIf

     output_y=output_y+hscale
   Next i
 Else
   ' Scaled version
   output_y=y1*hscale
   For i=src_y_start To height-1
     ' Process scale lines for this source row
     For j=0 To scale-1
       y=y1+i*scale+j
       If y>=VRes Then Exit For
       If y<0 Then Continue For
       wo=output_y*wordsperline

       ' Save copy range for duplication
       copy_sw=x1\5
       copy_ew=(x1+width*scale-1)\5

       For k=src_x_start To width-1
         ' Get bit from bitmap
         bit_index=i*width+k
         byte_index=bit_index>>3
         bit_position=7-(bit_index And 7)
         pixel_on=(PEEK(BYTE bitmap+byte_index)>>bit_position) And 1
         ' Determine color to draw
         If pixel_on Then
           c_word=f
         ElseIf draw_background Then
           c_word=b
         Else
           Continue For ' Skip transparent pixels
         EndIf
         ' Draw scale pixels horizontally
         x_start=x1+k*scale
         x_end=x_start+scale
         ' Clip horizontal span
         If x_start<0 Then x_start=0
         If x_end>HRes Then x_end=HRes
         If x_start<x_end Then
           ' Draw the horizontal run of pixels
           For px=x_start To x_end-1
             sw=px\5
             sp=px Mod 5
             p=badd+(wo+sw)*4
             mask=mask_table(sp)
             t=Peek(word p)
             t=t And INV mask
             t=t Or (c_word And mask)
             Poke word p,t
           Next px
         EndIf
       Next k

       ' Duplicate this line hscale-1 times
       If hscale > 1 Then
         count=copy_ew-copy_sw+1
         For dup=1 To hscale-1
           Memory COPY badd+(wo+copy_sw)*4, badd+((wo+dup*wordsperline)+copy_sw)*4, count*4
         Next
       EndIf

       output_y=output_y+hscale
     Next j
   Next i
 EndIf
End Sub