From 59a5145a09b10a89ede7b37e7565891da7345c6c Mon Sep 17 00:00:00 2001
From: Paul Gardner-Stephen <paul@servalproject.org>
Date: Thu, 25 Apr 2019 18:04:08 +0930
Subject: [PATCH] implement more of word packer routine

---
 c64/basic/pack_word.s | 130 ++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 125 insertions(+), 5 deletions(-)

diff --git a/c64/basic/pack_word.s b/c64/basic/pack_word.s
index 7cf7c4af..ea828641 100644
--- a/c64/basic/pack_word.s
+++ b/c64/basic/pack_word.s
@@ -33,16 +33,136 @@ pack_word:
 
 pack_char_loop:
 	;; Get the next char
-*	lda $0200,x
+	lda $0200,x
 	;; Work out what type of character we have, and thus
 	;; how we need to pack it.
 	jsr get_packed_char_index
-	sta $0100,x
+	
+	cmp #$00
+	bne not_exception_char
+
+	;; Char is an exception char.
+	;; We need to either finish the previous byte with $xF,
+	;; Or if there is no existing nybl, then we need to output
+	;; either $FE or $FF, depending on whether this is the last
+	;; byte or not
+
+	lda tokenise_work3
+	bne have_nybl_before_exception_char
+
+	;; No nybl exists before the exception char, so prefix with $FE or $FF
+	;; based on whether we are at the end of the input string or not
+	inx
+	cpx tokenise_work1
+	beq at_end
+	lda #$FF
+at_end: .byte $2C
+	LDA #$FE
+	dex
+	ldy tokenise_work2
+	sta $0100,y
+	inc tokenise_work2
+	
+	jmp output_exception_byte
+
+have_nybl_before_exception_char:
+	;; Or existing byte with #$0F, and clear nybl flag
+	ldy tokenise_work2
+	dey
+	lda $0100,y
+	ora #$0f
+	sta $0100,y
+
+	;; Clear have nybl flag
+	lda #$00
+	sta tokenise_work3
+	
+	;; FAll through
+
+output_exception_byte:	
+	ldy tokenise_work2
+	lda $0200,x
+	sta $0100,y
+	iny
+	sty tokenise_work2
+	jmp consider_next_char
+
+not_exception_char:
+	;; It wasn't an exception char, so write the symbol out
+	cmp #$10
+	bcs whole_byte_symbol
+	;; Nybl value
+	ldy tokenise_work3
+	bne store_low_nybl
+	;; Store in high nybl -- so shift it into the hi nybl
+	asl
+	asl
+	asl
+	asl
+	ldy tokenise_work2
+	sta $0100,y
+	bne stored_nybl 	; Must be taken, as A cannot be $00
+	
+store_low_nybl:	
+	ldy tokenise_work2
+	ora $0100,y
+	sta $0100,y
+stored_nybl:	
+	;; Now toggle the nybl flag
+	lda tokenise_work3
+	eor #$ff
+	sta tokenise_work3
+	;; And advance the offset if required
+	bne consider_next_char
+	inc tokenise_work2
+	jmp consider_next_char
+
+whole_byte_symbol:
+	ldy tokenise_work3
+	beq nothing_to_flush
+	;; Set low nybl to $F to mark next byte as literal
+	ldy tokenise_work2
+	lda $0100,y
+	ora #$0f
+	sta $0100,y
+	inc tokenise_work2
+	;; Clear nybl flag
+	lda #$00
+	sta tokenise_work3
+	;; Load literal byte for storing in output
+	lda $0200,x
+	;; FALL THROUGH
+
+nothing_to_flush:
+	;; Write the symbol as it is, or the literal
+	;; if it has been passed to us via fall-through
+	ldy tokenise_work2
+	sta $0100,y
+	inc tokenise_work2
+	
+	;; Fall through
+	
+consider_next_char:	
+	;; Pack next char
 	inx 
 	cpx tokenise_work1
-	bne -
-	lda tokenise_work1
-	sta tokenise_work2
+	bne pack_char_loop
+
+	;; Add 1 to length if nybl waiting to be flushed
+	;; (but leave flag set so the caller knows if the
+	;; bottom nybl might be different if the string were
+	;; longer).
+	lda tokenise_work3
+	beq +
+	inc tokenise_work2
+	rts
+*	
+	;; Last byte was full, so we need at $00 on the end
+	;; (unless a $FE token was written 2 bytes ago)
+	ldy tokenise_work2
+	lda #$00
+	sta $0100,y
+	inc tokenise_work2	
 	
 	rts