class_name StreamBuffer
extends Resource

# This maximum capacity is just to ensure we're not doing something wrong,
# should be enough for our purpose: Encoding and decoding packets.
const MAXIMUM_CAPACITY = 256 * 1024 # 256 KiB

var buffer: PackedByteArray
var capacity: int
var size: int
var cursor: int
## Bit index (0 to 7) for writing / reading the next bit.
var bit: int

func _init(_buffer: PackedByteArray) -> void:
	buffer   = _buffer
	capacity = buffer.size()

## Creates a new StreamBuffer with the specified capacity.
## This is intended for writing / encoding data.
static func with_capacity(initial_capacity: int) -> StreamBuffer:
	var _buffer = PackedByteArray()
	_buffer.resize(initial_capacity)
	return StreamBuffer.new(_buffer)

## Creates a new StreamBuffer from the specified buffer, pre-initializing "size".
## This is intended for reading / decoding data.
static func from_buffer(_buffer: PackedByteArray) -> StreamBuffer:
	var stream := StreamBuffer.new(_buffer)
	stream.size = stream.capacity
	return stream


## Returns the remaining capacity before the buffer needs to be resized to fit more data.
func remaining_capacity() -> int:
	return capacity - size

## Returns the remaining number of bytes to read.
func remaining_bytes() -> int:
	return size - cursor


## Returns a slice of this StreamBuffer.
## By default returns a slice of the currently written bytes.
func slice(begin: int = 0, end: int = -1) -> PackedByteArray:
	if end < 0: end = size
	return buffer.slice(begin, end)

## Clears the buffer and resets the cursor, ready to encode new data.
## For performance, does not clear the existing data in the underlying buffer.
func clear() -> void:
	size = 0
	cursor = 0
	bit = 0

## Ensures that the capacity for this buffer is large enough for the specified number of bytes to be written.
## For the sake of not resizing too often, this simply doubles the current capacity.
func ensure_capacity(required_bytes: int) -> void:
	var total_required_capacity := size + required_bytes
	if capacity < total_required_capacity:
		while capacity < total_required_capacity: capacity *= 2
		assert(capacity <= MAXIMUM_CAPACITY)
		buffer.resize(capacity)

func write_int8(value: int) -> void: ensure_capacity(1); buffer.encode_s8(size, value); size += 1; bit = 0
func write_int16(value: int) -> void: ensure_capacity(2); buffer.encode_s16(size, value); size += 2; bit = 0
func write_int32(value: int) -> void: ensure_capacity(4); buffer.encode_s32(size, value); size += 4; bit = 0
func write_int64(value: int) -> void: ensure_capacity(8); buffer.encode_s64(size, value); size += 8; bit = 0

func write_byte(value: int) -> void: write_uint8(value)
func write_uint8(value: int) -> void: ensure_capacity(1); buffer.encode_u8(size, value); size += 1; bit = 0
func write_uint16(value: int) -> void: ensure_capacity(2); buffer.encode_u16(size, value); size += 2; bit = 0
func write_uint32(value: int) -> void: ensure_capacity(4); buffer.encode_u32(size, value); size += 4; bit = 0
func write_uint64(value: int) -> void: ensure_capacity(8); buffer.encode_u64(size, value); size += 8; bit = 0

func write_float16(value: float) -> void: ensure_capacity(2); buffer.encode_half(size, value); size += 2; bit = 0
func write_float32(value: float) -> void: ensure_capacity(4); buffer.encode_float(size, value); size += 4; bit = 0
func write_float64(value: float) -> void: ensure_capacity(8); buffer.encode_double(size, value); size += 8; bit = 0

func write_vector16(value: Vector3) -> void: write_float16(value.x); write_float16(value.y); write_float16(value.z)
func write_vector32(value: Vector3) -> void: write_float32(value.x); write_float32(value.y); write_float32(value.z)

func write_transform16(value: Transform3D) -> void: write_vector16(value.basis.x); write_vector16(value.basis.y); write_vector16(value.basis.z); write_vector16(value.origin)
func write_transform32(value: Transform3D) -> void: write_vector32(value.basis.x); write_vector32(value.basis.y); write_vector32(value.basis.z); write_vector32(value.origin)


func read_int8() -> int: assert(remaining_bytes() >= 1); var result := buffer.decode_s8(cursor); cursor += 1; bit = 0; return result
func read_int16() -> int: assert(remaining_bytes() >= 2); var result := buffer.decode_s16(cursor); cursor += 2; bit = 0; return result
func read_int32() -> int: assert(remaining_bytes() >= 4); var result := buffer.decode_s32(cursor); cursor += 4; bit = 0; return result
func read_int64() -> int: assert(remaining_bytes() >= 8); var result := buffer.decode_s64(cursor); cursor += 8; bit = 0; return result

func read_byte() -> int: return read_uint8()
func read_uint8() -> int: assert(remaining_bytes() >= 1); var result := buffer.decode_u8(cursor); cursor += 1; bit = 0; return result
func read_uint16() -> int: assert(remaining_bytes() >= 2); var result := buffer.decode_u16(cursor); cursor += 2; bit = 0; return result
func read_uint32() -> int: assert(remaining_bytes() >= 4); var result := buffer.decode_u32(cursor); cursor += 4; bit = 0; return result
func read_uint64() -> int: assert(remaining_bytes() >= 8); var result := buffer.decode_u64(cursor); cursor += 8; bit = 0; return result

func read_float16() -> float: assert(remaining_bytes() >= 2); var result := buffer.decode_half(cursor); cursor += 2; bit = 0; return result
func read_float32() -> float: assert(remaining_bytes() >= 4); var result := buffer.decode_float(cursor); cursor += 4; bit = 0; return result
func read_float64() -> float: assert(remaining_bytes() >= 8); var result := buffer.decode_double(cursor); cursor += 8; bit = 0; return result

func read_vector16() -> Vector3: return Vector3(read_float16(), read_float16(), read_float16())
func read_vector32() -> Vector3: return Vector3(read_float32(), read_float32(), read_float32())

func read_transform16() -> Transform3D: return Transform3D(Basis(read_vector16(), read_vector16(), read_vector16()), read_vector16())
func read_transform32() -> Transform3D: return Transform3D(Basis(read_vector32(), read_vector32(), read_vector32()), read_vector32())


func write_bit(value: bool) -> void:
	if bit == 0: ensure_capacity(1); buffer[size] = 0; size += 1
	buffer[size - 1] = buffer[size - 1] | (int(value) << bit)
	bit = (bit + 1) % 8

func read_bit() -> bool:
	if bit == 0: assert(remaining_bytes() >= 1); cursor += 1
	var result := bool((buffer[cursor - 1] >> bit) & 1)
	bit = (bit + 1) % 8
	return result


func write_raw_buffer(value: PackedByteArray) -> void:
	ensure_capacity(value.size())
	for i in value.size():
		buffer[size] = value[i]
		size += 1

func read_raw_buffer(length: int) -> PackedByteArray:
	assert(remaining_bytes() >= length)
	var result := PackedByteArray()
	result.resize(length)
	for i in length:
		result[i] = buffer[cursor]
		cursor += 1
	return result


func write_string(value: String) -> void:
	var bytes := value.to_utf8_buffer()
	write_uint16(bytes.size())
	write_raw_buffer(bytes)

func read_string() -> String:
	var length := read_uint16()
	var bytes  := read_raw_buffer(length)
	return bytes.get_string_from_utf8()


# Optimized way to write a bone transform, since bones are likely to not contain offset or scale.

func write_bone_pose(value: Transform3D) -> void:
	var pos   := value.origin
	var rot   := value.basis.get_euler()
	var scale := value.basis.get_scale()
	var has_pos    := !pos.is_zero_approx()
	var has_rot    := !rot.is_zero_approx()
	var has_scale  := !scale.is_equal_approx(Vector3.ONE)
	var has_scale3 := !is_equal_approx(scale.x, scale.y) or !is_equal_approx(scale.x, scale.y)
	write_bit(has_pos)
	write_bit(has_rot)
	write_bit(has_scale)
	write_bit(has_scale3)
	if has_pos: write_vector16(pos)
	# TODO: Could optimize this by using 16-bit fixed-point values.
	#       Since these values can only be in the range 0 to 1.
	if has_rot: write_vector16(rot)
	if has_scale3: write_vector16(scale)
	elif has_scale: write_float16((scale.x + scale.y + scale.z) / 3)

func read_bone_pose() -> Transform3D:
	var pos   := Vector3.ZERO
	var rot   := Vector3.ZERO
	var scale := Vector3.ONE
	var has_pos    := read_bit()
	var has_rot    := read_bit()
	var has_scale  := read_bit()
	var has_scale3 := read_bit()
	if has_pos: pos = read_vector16()
	if has_rot: rot = read_vector16()
	if has_scale3: scale = read_vector16()
	elif has_scale: var s := read_float16(); scale = Vector3(s, s, s)
	var basis := Basis.from_scale(scale) * Basis.from_euler(rot)
	return Transform3D(basis, pos)