#include <avr/io.h>

#define VERT_PORT PORTB
#define HORIZ_DDR DDRD
#define SHADE_CYCLES 16

    .section .data
iteration:
    .byte 0

    .section .text
    .extern cmd
    .extern screen
    .global TWI_vect
    .global TIMER2_COMP_vect

; TIMER2 interrupt routine.
; Registers:
;   r0  SREG
;   r18 Iteration number
;       Vertical position (VERT_PORT)
;       Busy loop
;       Image data (HORIZ_DDR)
;   X   *screen

TIMER2_COMP_vect:
    push r0
    push r18
    push XL
    push XH
    in   r0, _SFR_IO_ADDR(SREG) ; Save SREG.


    ldi  XL, lo8(screen)    ; Load start address of the screen into X.
    ldi  XH, hi8(screen)

    lds  r18, iteration
    add  XL, r18            ; add as offset to X.
    adc  XH, r1

    inc  r18                ; Increase iteration.
    andi r18, 0x0f          ; Clip iteration into range 0:7.
    sts  iteration, r18

    in   r18, _SFR_IO_ADDR(VERT_PORT) ; Load vertical position into r18.
    lsl  r18                ; Switch to the next vertical position.
    brne set_row
    ldi  r18, 1             ; Reset to 1 if 0.

    clt                     ; Clear T flag in SREG.

set_row:
    out  _SFR_IO_ADDR(HORIZ_DDR), r1    ; Zero out row.
    out  _SFR_IO_ADDR(VERT_PORT), r18   ; Push next position into io register.
load_img:
    ld   r18, X+                        ; Load image data into r18
    out  _SFR_IO_ADDR(HORIZ_DDR), r18   ; Push image data into row.

    brts exit
    set                                 ; Set T flag in SREG to exit after the second iteration.
    adiw X, 7
    ldi  r18, SHADE_CYCLES
wait:                                   ; Busy loop.
    dec  r18
    brne wait
    rjmp load_img

exit:
    out  _SFR_IO_ADDR(SREG), r0         ; Restore SREG.
    pop  XH
    pop  XL
    pop  r18
    pop  r0
    reti