Option explicit
Option default integer
Dim o
' set pins that will be used as outputs
SetPin gp4,pio0 'clock pulse
SetPin gp16,pio0 'hsync
SetPin gp17,pio0 'vsync
SetPin gp7,pio1 'DE
SetPin gp18,pio1 'low Blue
SetPin gp19,pio1 'high blue
SetPin gp20,pio1 'low green
SetPin gp21,pio1 'high green
SetPin gp22,pio1 'low red
SetPin gp23,pio1 '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)
Dim currentfont=MM.Info(font)
Dim fontadd=MM.Info(font address currentfont)
Dim fontdetail=Peek(word fontadd)
Dim fontbytes=(fontdetail And &HFF) * ((fontdetail>>8) And &HFF)/8
Dim asciioffset=((fontdetail>>16) And &HFF)
Inc fontadd,4 'pont to the actual characters


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 second program
  IRQ PREV 2 side 0
  IRQ 1 side 0 [2]  'set a flag for the third program
.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)
' create the setting to initialise program 1
'PIO init machine 1,0,f1,p1,e1,s1,o1,1,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 sync
  Mov y,osr side 1 'save the length of the sync pulse into y
  Pull block side 1' get the length of the visible data + front porch in osr
.wrap target
  Mov x,osr side 1 'get the length of the active+frontporch
  Wait 1 irq 1 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 1
  Mov x,y side 1 'restore the sync value
.label loop2
  Jmp x--,loop2 side 0
  Nop side 1
'we don't need to consider the back porch as we don't trigger again until the start of a new line
.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)

o=Pio(next line) 'note the start of the second pio program
Print "compiling vsyncclock @ line: ";o
PIO assemble 0
.program vsyncclock
.side set 1
.line next 'this will pick up o3 automatically
  Pull block side 1
  Mov y,osr side 1 'store the length of the back porch in y
  Pull block side 1
  Mov isr,osr side 1 'store the length of sync in isr
  Pull block side 1 'get the synch length
.wrap target
  Mov x,osr side 1 'get the length of the active + front porch
  Wait 1 irq 0 side 1 [1]'this instruction waits a line end which we use to be beginning of frame
  IRQ PREV 3 side 1
.label loop3
  Jmp x--,loop3 side 1 'main data loop
  Mov x,isr side 1 'restore the sync value
.label loop4
  Jmp x--,loop4 side 0 'sync loop
  Mov x,y side 0
.label loop5
  Jmp x--,loop5 side 1 'rest of line loop
  IRQ CLEAR 0 side 1
.wrap
.end program list

' create the setting to initialise program 3
PIO CONFIGURE 0,2,clock,o,gp17,1,1,,,,,,,,,Pio(.wrap target),Pio(.wrap)
o=Pio(next line 0) 'note the start of the first PIO instruction for program 1 - will be 0

Print "compiling datamaster @ line: ";0
PIO assemble 1
.program datamaster
.line 0
Pull block side 0
Mov y,osr 'store the count of active lines
.wrap target 'main loop
Mov x,y 'get the active line count
Wait 1 irq next 3'wait for the start of frame
.label loop6
Wait 1 irq next 2 'wait for the next line to start
IRQ  5 'trigger the output
Jmp x--,loop6
.wrap
.end program list
PIO CONFIGURE 1,0,clock,o,,,,,,,,,,,,Pio(.wrap target),Pio(.wrap)
o= Pio(next line)

Print "compiling data @ line: ";o
PIO assemble 1
.program linedata
.line next
.side set 1
Pull block side 0
Mov y,osr side 0 'store active pixel count divided by the number of pixels per word
.wrap target
Mov x,y side 0'get the active pixel count divided by the number of pixels per word
Wait 1 irq 5 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 [7]
Out NULL,2
Jmp x--,loop7 side 1
Mov pins, NULL side 0 [2]
.wrap
.end program list
' create the setting to initialise program 3
PIO CONFIGURE 1,1,clock,o,GP7,1,1,,,,GP18,6,1,,,Pio(.wrap target),Pio(.wrap)
PIO SYNC 'this is a new command that syncs the clocks of the PIO. It is needed for IRQ to work between PIO

PIO start 1,0
PIO write 1,0,1,vvisible-1
PIO start 1,1
PIO write 1,1,1,hvisible\pixelsperword-1
' initialise and start program 1 but note it will block on pull block
PIO start 0,0
' initialise and start program 2, it will block waiting for the irq
PIO start 0,1
PIO write 0,1,2,hsyncclock-1,hvisibleclock+hfrontporchclock-1
' initialise and start program 3, it will block waiting for the irq
PIO start 0,2
'rest of line value is reduced to make sure no overun but also don't miss a line
PIO write 0,2,3,vbackporchclock-1,vsyncclock-1,vvisibleclock+vfrontporchclock-1
' loop, although the PIO will keep running anyway even if the program finishes
PIO dma tx 1,1,wordstotransfer,b(),redo
' trigger the whole shebang by writing to program 1 fifo
PIO write 0,0,1,hwholeline
PIO sync
Do
mycls &H3C
Pause 1000
mycls &H3
Pause 1000
rectangle 100,100,199,199,&H6
rectangle 0,0,hvisible-1,0,&H3F
rectangle 0,vvisible-1,hvisible-1,vvisible-1,&H3F
rectangle 0,0,0,vvisible-1,&H3F
rectangle hvisible-1,0,hvisible-1,vvisible-1,&H3F
printstring "Hello",300,300,3,&H3F,-1
Pause 1000

Loop
Sub printchar s$,x,y,scale,fc,bc
 Local address=(Asc(s$)-asciioffset)*fontbytes+fontadd
 drawbitmap x,y,MM.FONTWIDTH,MM.FONTHEIGHT,scale,fc,bc,address
End Sub

Sub printstring s$,x,y,scale,fc,bc
Local currentx=x,currenty=y
Local j
For j=1 To Len(s$)
printchar Mid$(s$,j,1),currentx,currenty,scale,fc,bc
Inc currentx,MM.FONTWIDTH*scale
Next
End Sub

Sub redo
PIO dma tx 1,1,wordstotransfer,b(),redo
End Sub

Sub mycls col
Local c=col
c=c+(col<<6)
c=c+(col<<12)
c=c+(col<<18)
c=c+(col<<24)
Memory set word badd,c,wordstotransfer
End Sub


Sub rectangle x1,y1,x2,y2,col
Local x,y,p,t,wo,sw,ew,sp,ep,mask,w,i,count
Local c=col
c=c+(col<<6)
c=c+(col<<12)
c=c+(col<<18)
c=c+(col<<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 DrawBitmap x1,y1,width,height,scale,fc,bc,bitmap
Local f,b,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,mask_table(4)

' Pre-calculate pixel masks
mask_table(0)=&H3F
mask_table(1)=&HFC0
mask_table(2)=&H3F000
mask_table(3)=&HFC0000
mask_table(4)=&H3F000000

' Convert 6-bit colors to full word (5 pixels)
f=fc And &H3F
f=f Or (f<<6) Or (f<<12) Or (f<<18) Or (f<<24)

If bc>=0 Then
  b=bc And &H3F
  b=b Or (b<<6) Or (b<<12) Or (b<<18) Or (b<<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