diff --git a/circbuf.c b/circbuf.c
index bc965f6..ec1d476 100644
--- a/circbuf.c
+++ b/circbuf.c
@@ -6,10 +6,10 @@
 #include "circbuf.h"
 
 
-void cbuf_init(circbuf_t *inst, uint16_t length)
+void cbuf_init(circbuf_t *inst, size_t capacity)
 {
-	inst->buffer = malloc(length);
-	inst->capacity = length;
+	inst->buf = malloc(capacity);
+	inst->cap = capacity;
 
 	cbuf_clear(inst);
 }
@@ -17,29 +17,32 @@ void cbuf_init(circbuf_t *inst, uint16_t length)
 
 void cbuf_deinit(circbuf_t *inst)
 {
-	if (inst->buffer != NULL) {
-		free(inst->buffer);
+	if (inst->buf != NULL) {
+		free(inst->buf);
 	}
 }
 
 
 bool cbuf_full(circbuf_t *inst)
 {
-	if (inst->read_pos == 0) {
-		return inst->write_pos == inst->capacity - 1;
+	return (inst->lr == inst->nw);
+}
+
+
+static bool can_read_at(circbuf_t *inst, size_t i)
+{
+	if (inst->lr < inst->nw) {
+		return i > inst->lr && i < inst->nw;
 	} else {
-		return inst->write_pos == inst->read_pos;
+		// NW < LR
+		return (i < inst->cap && i > inst->lr) || i < inst->nw;
 	}
 }
 
 
 bool cbuf_empty(circbuf_t *inst)
 {
-	if (inst->write_pos == 0) {
-		return inst->read_pos == inst->capacity - 1;
-	} else {
-		return inst->read_pos == inst->write_pos - 1;
-	}
+	return ((inst->lr + 1) % inst->cap) == inst->nw;
 }
 
 
@@ -47,13 +50,11 @@ bool cbuf_write(circbuf_t *inst, uint8_t b)
 {
 	if (cbuf_full(inst)) return false;
 
-	inst->buffer[inst->write_pos] = b;
-	inst->write_pos++;
+	inst->buf[inst->nw] = b;
 
-	// wrap
-	if (inst->write_pos >= inst->capacity) {
-		inst->write_pos = 0;
-	}
+	// increment
+	inst->nw++;
+	if (inst->nw == inst->cap) inst->nw = 0;
 
 	return true;
 }
@@ -63,33 +64,38 @@ bool cbuf_read(circbuf_t *inst, uint8_t *b)
 {
 	if (cbuf_empty(inst)) return false;
 
-	inst->read_pos++;
-
-	// wrap
-	if (inst->read_pos >= inst->capacity) {
-		inst->read_pos = 0;
-	}
+	// increment
+	inst->lr++;
+	if (inst->lr == inst->cap) inst->lr = 0;
 
-	*b = inst->buffer[inst->read_pos];
+	*b = inst->buf[inst->lr];
 
-	// zero out the read byte (for debug)
-	//inst->buffer[inst->read_pos] = 0;
+	//zero out the read byte (for debug)
+	inst->buf[inst->lr] = 0;
 
 	return true;
 }
 
 
-bool cbuf_peek(circbuf_t *inst, uint8_t *b)
+bool cbuf_peek(circbuf_t *inst, size_t nth, uint8_t *b)
 {
-	if (cbuf_empty(inst)) return false;
+	// check if can read there (can't use module, could "wrap")
+	size_t n = inst->lr;
+	for (size_t i = 0; i <= nth; i++) {
+		// increment N
+		n++;
+		if (n == inst->cap) n = 0;
+
+		if (!can_read_at(inst, n)) return false;
+	}
 
-	*b = inst->buffer[inst->read_pos];
+	*b = inst->buf[n];
 	return true;
 }
 
 
 void cbuf_clear(circbuf_t *inst)
 {
-	inst->read_pos = inst->capacity - 1;
-	inst->write_pos = 0;
+	inst->lr = inst->cap - 1;
+	inst->nw = 0;
 }
diff --git a/circbuf.h b/circbuf.h
index d51a494..4e15bfb 100644
--- a/circbuf.h
+++ b/circbuf.h
@@ -1,19 +1,20 @@
 #pragma once
 #include <stdint.h>
 #include <stdbool.h>
+#include <stdlib.h>
 
 // Circular Character Buffer implementation
 
-typedef struct {
-	uint8_t *buffer;
-	uint16_t capacity;
-	uint16_t read_pos;
-	uint16_t write_pos;
+typedef struct CircularBuffer_struct {
+	uint8_t *buf;
+	size_t cap;
+	size_t lr; // last read pos
+	size_t nw; // next write pos
 } circbuf_t;
 
 
 /** Init a buffer */
-void cbuf_init(circbuf_t *inst, uint16_t length);
+void cbuf_init(circbuf_t *inst, size_t capacity);
 
 
 /** Deinit a buffer (free the memory) */
@@ -40,10 +41,10 @@ bool cbuf_read(circbuf_t *inst, uint8_t *b);
 
 
 /**
- * Get byte at the read cursor, without incrementing it.
- * False on empty.
+ * Get n-th byte from the buffer, without removing it.
+ * Returns false if offset is invalid (ie. empty buffer).
  */
-bool cbuf_peek(circbuf_t *inst, uint8_t *b);
+bool cbuf_peek(circbuf_t *inst, size_t nth, uint8_t *b);
 
 
 /** Remove all data from buffer */