.include "m8def.inc"

.def DATA = r17

.org 0
    rjmp  MAIN                                      ; Reset Handler

WAIT:
    ldi zh,high(60000) ;
    ldi zl,low(60000)
W_LOOP:
    sbiw zl,1
    brne W_LOOP
    ret

TWI_INIT:
    ldi   r16, (1<<TWINT)|(1<<TWSTA)|(1<<TWEN)
    out   TWCR, r16                                 ; Enable and transmit START

WAIT1:
    in    r16, TWCR
    sbrs  r16, TWINT
    rjmp  WAIT1                                     ; Wait for START to be transmitted

    in    r16, TWSR
    andi  r16, 0xF8
    cpi   r16, 0x08
    brne  ERROR                                     ; Check Status

    ldi   r16, 0x02                                 ; Slave address
    out   TWDR, r16
    ldi   r16, (1<<TWINT)|(1<<TWEN)
    out   TWCR, r16                                 ; Re-Enable and send request to slave
    ret                                             ; Återvänd

SEND:
    in    r16, TWCR
    sbrs  r16, TWINT
    rjmp  SEND                                      ; VÄNTA FIFAN

    out   TWDR, DATA
    ldi   r16, (1<<TWINT) | (1<<TWEN)
    out   TWCR, r16                                 ; Re-Enable and send data
    ret

STOP:
    rcall WAIT
    ldi   r16, (1<<TWINT)|(1<<TWEN)|(1<<TWSTO)
    out   TWCR, r16                                 ; Re-Enable and transmit STOP
    ret

MAIN:
    ldi r16, low(RAMEND)
    out spl, r16
    ldi r16, high(RAMEND)
    out sph, r16

    rcall WAIT
    rcall WAIT
    rcall WAIT
    rcall WAIT

    rcall TWI_INIT

    ldi DATA, 'Y'
    rcall SEND
    ldi DATA, ' '
    rcall SEND
    ldi DATA, 'u'
    rcall SEND
    ldi DATA, ' '
    rcall SEND
    ldi DATA, 'm'
    rcall SEND
    ldi DATA, 'a'
    rcall SEND
    ldi DATA, 'd'
    rcall SEND
    ldi DATA, '?'
    rcall SEND
    ldi DATA, '!'
    rcall SEND

    rcall STOP


INF:
    rjmp INF


ERROR:
    ldi r16, 0x11
    out portb, r16
    rjmp  ERROR