.include    "m8def.inc"

.def temp = r16
.def shiftcount = r17
.def letter = r18
.def row = r19
.equ row1 = 0x80
.equ row2 = 0xC0
.equ RS_HI_E_HI = 0x03
.equ RS_LO_E_HI = 0x02
.equ RS_HI_E_LO = 0x01
.equ RS_LO_E_LO = 0x00

.org 0x00
    rjmp main

.org 0x20
main:
    ldi temp, low(RAMEND)
    out spl, temp
    ldi temp, high(RAMEND)
    out sph, temp

    ldi temp, 0xFF
    out ddrb, temp

    ldi temp, 0x03
    out ddrd, temp

    rcall init
    rcall TWI
    rcall GET
    rjmp inf

TWI:
    ldi   r16, 0x02                     ; My adress = 1, I don't respond to generic calls
    out   TWAR, r16                     ; Set address in TWAR
    ldi   r16, (1<<TWEA)|(1<<TWEN)
                                        ; Enable, respond to address calls
    out   TWCR, r16                     ; Write to TWI Control Register

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

    in    r16, TWSR                     ; Check TWI status
    andi  r16, 0xF0
    cpi   r16, 0x60
    brne  ERROR                         ; Check if SLA+W is received
    ret

GET:
    ldi   r16, (1<<TWINT)|(1<<TWEN)|(1<<TWEA)
    out   TWCR, r16                     ; Clear TWINT and enable again

    WAIT2:
    in    r16, TWCR
    sbrs  r16, TWINT
    rjmp  WAIT2


    in    r16, TWSR
    andi  r16, 0xF0
    cpi   r16, 0x80                     ; Om ingen stopbit hittats;
    brne  inf                           ; Fortsätt hämta data

    in    letter, TWDR                  ; Wait for data send
    rcall write

    rjmp  GET

init:
    ldi temp, 0x30
    out portb, temp

    rcall enable

    ldi temp, 0x30
    out portb, temp

    rcall enable

    ldi temp, 0x30
    out portb, temp

    rcall enable

    ldi temp, 0x3C ; Function Set
    out portb, temp

    rcall enable
    rcall clear

    ldi temp, 0x06 ; Entry Mode set
    out portb, temp

    rcall enable

    ldi temp, 0x0C ; Display on
    out portb, temp

    rcall enable
    ret

inf:
    rjmp inf

ERROR:
    rjmp  inf

clear:
    ldi temp, 0x01 ; Clear Disp
    out portb, temp
    rcall enable
enable:
    rcall s_wait
    ldi temp, RS_LO_E_HI
    out portd,temp
    rcall s_wait
    ldi temp, RS_LO_E_LO
    out portd,temp
    rcall s_wait
    ret
s_wait:
    ldi zh, high(350)
    ldi zl, low(350)
s_loop:
    sbiw zl,1
    brne s_loop
    ret


write:
    ldi temp, RS_HI_E_LO
    out portd, temp

    rcall s_wait

    ldi temp, RS_HI_E_HI
    out portd,temp

    rcall s_wait

    out portb, letter

    rcall s_wait

    ldi temp, RS_HI_E_LO
    out portd, temp

    rcall s_wait

    ldi temp, RS_LO_E_LO
    out portd, temp

    ret

shift:
    mov temp, shiftcount
    add temp, row
    out portb, temp

    rcall enable

    ret