ADPCM: Encoding and decoding blocks fully functional.

cmd/adpcm/codec.go

@@ -0,0 +1,265 @@
+package adpcm
+
+import (
+	"encoding/binary"
+	"fmt"
+	"math"
+)
+
+// table of index changes
+var indexTable = []int16{
+	-1, -1, -1, -1, 2, 4, 6, 8,
+	-1, -1, -1, -1, 2, 4, 6, 8,
+}
+
+// quantize step size table
+var stepTable = []int16{
+	7, 8, 9, 10, 11, 12, 13, 14,
+	16, 17, 19, 21, 23, 25, 28, 31,
+	34, 37, 41, 45, 50, 55, 60, 66,
+	73, 80, 88, 97, 107, 118, 130, 143,
+	157, 173, 190, 209, 230, 253, 279, 307,
+	337, 371, 408, 449, 494, 544, 598, 658,
+	724, 796, 876, 963, 1060, 1166, 1282, 1411,
+	1552, 1707, 1878, 2066, 2272, 2499, 2749, 3024,
+	3327, 3660, 4026, 4428, 4871, 5358, 5894, 6484,
+	7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899,
+	15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794,
+	32767,
+}
+
+var (
+	encPred  int16
+	encIndex int16
+	decPred  int16
+	decIndex int16
+	decStep  int16 = 7
+
+	pCount int
+	pNum   = 8
+)
+
+// EncodeSample takes a single 16 bit PCM sample and
+// returns a byte of which the last 4 bits are an encoded ADPCM nibble
+func EncodeSample(sample int16) byte {
+
+	// find difference from predicted sample
+	delta := sample - encPred
+
+	// set sign bit and find absolute value of difference
+	var nibble byte
+	if delta < 0 {
+		nibble = 8
+		delta = -delta
+	}
+
+	step := stepTable[encIndex]
+	diff := step >> 3
+	var mask byte = 4
+
+	// quantize delta down to four bits
+	for i := 0; i < 3; i++ {
+		if delta > step {
+			nibble |= mask
+			delta -= step
+			diff += step
+		}
+		mask >>= 1
+		step >>= 1
+	}
+
+	// adjust predicted sample based on calculated difference
+	if nibble&8 != 0 {
+		encPred -= diff
+	} else {
+		encPred += diff
+	}
+
+	// check for underflow and overflow
+	if encPred < math.MinInt16 {
+		encPred = math.MinInt16
+	} else if encPred > math.MaxInt16 {
+		encPred = math.MaxInt16
+	}
+
+	encIndex += indexTable[nibble&7]
+
+	// check for underflow and overflow
+	if encIndex < 0 {
+		encIndex = 0
+	} else if encIndex > int16(len(stepTable)-1) {
+		encIndex = int16(len(stepTable) - 1)
+	}
+
+	return nibble
+}
+
+// DecodeSample takes a byte, the last 4 bits of which contain a single
+// 4 bit ADPCM nibble, and returns a 16 bit decoded PCM sample
+func DecodeSample(nibble byte) int16 {
+
+	// compute predicted sample estimate (decPred):
+
+	// calculate difference
+	var diff int16
+	if nibble&4 != 0 {
+		diff += decStep
+	}
+	if nibble&2 != 0 {
+		diff += decStep >> 1
+	}
+	if nibble&1 != 0 {
+		diff += decStep >> 2
+	}
+	diff += decStep >> 3
+	// account for sign bit
+	if nibble&8 != 0 {
+		diff = -diff
+	}
+	// adjust predicted sample based on calculated difference
+	decPred += diff
+
+	// check for overflow and underflow
+	if decPred > math.MaxInt16 {
+		decPred = math.MaxInt16
+	} else if decPred < math.MinInt16 {
+		decPred = math.MinInt16
+	}
+
+	// compute new step size:
+
+	// adjust index into step size lookup table using nibble
+	decIndex += indexTable[nibble]
+	// check for overflow and underflow
+	if decIndex < 0 {
+		decIndex = 0
+	} else if decIndex > int16(len(stepTable)-1) {
+		decIndex = int16(len(stepTable) - 1)
+	}
+	// find new quantizer step size
+	decStep = stepTable[decIndex]
+
+	return decPred
+}
+
+func calcHead(sample []byte) ([]byte, error) {
+	if len(sample) != 2 {
+		return nil, fmt.Errorf("length of given byte array is: %v, expected: 2", len(sample))
+	}
+
+	intSample := int16(binary.LittleEndian.Uint16(sample))
+
+	// if pCount < pNum {
+	// 	fmt.Println("calcHead enc", intSample)
+	// 	fmt.Println(encIndex, encPred)
+	// }
+
+	EncodeSample(intSample)
+
+	head := make([]byte, 2)
+
+	head[0] = sample[0]
+	head[1] = sample[1]
+
+	// if pCount < pNum {
+	// 	fmt.Println("after calcHead enc")
+	// 	fmt.Println(encIndex, encPred)
+	// }
+
+	head = append(head, byte(uint16(encIndex)))
+	head = append(head, byte(0x00))
+	return head, nil
+}
+
+func EncodeBlock(block []byte) ([]byte, error) {
+
+	if len(block) != 1010 {
+		return nil, fmt.Errorf("unsupported block size. Given: %v, expected: 1010, ie. 505 16-bit PCM samples", len(block))
+	}
+
+	result, err := calcHead(block[0:2])
+	if err != nil {
+		return nil, err
+	}
+
+	for i := 2; i < len(block); i++ {
+		if (i+1)%4 == 0 {
+
+			// var intsample int16
+			// if pCount < pNum {
+			// 	fmt.Println(block[2], block[3])
+			// 	fmt.Println("first enc", block[i-1:i+1])
+			// 	fmt.Println(encIndex, encPred)
+			// }
+			sample2 := EncodeSample(int16(binary.LittleEndian.Uint16(block[i-1 : i+1])))
+
+			// if pCount < pNum {
+			// 	intsample = int16(binary.LittleEndian.Uint16(block[i+1 : i+3]))
+			// 	fmt.Println("second enc", int16(intsample))
+			// 	fmt.Println(encIndex, encPred)
+			// }
+			sample := EncodeSample(int16(binary.LittleEndian.Uint16(block[i+1 : i+3])))
+
+			// if pCount < pNum {
+			// 	fmt.Println("after enc")
+			// 	fmt.Println(encIndex, encPred)
+			// 	fmt.Println()
+			// }
+
+			result = append(result, byte((sample<<4)|sample2))
+
+			pCount++
+
+		}
+	}
+
+	return result, nil
+}
+
+func DecodeBlock(block []byte) ([]byte, error) {
+
+	if len(block) != 256 {
+		return nil, fmt.Errorf("unsupported block size. Given: %v, expected: 256", len(block))
+	}
+
+	if pCount < pNum {
+		fmt.Println("pre dec", block[0:8])
+		fmt.Println(decIndex, decPred, decStep)
+	}
+
+	var result []byte
+	decPred = int16(binary.LittleEndian.Uint16(block[0:2]))
+	decIndex = int16(block[2])
+	decStep = stepTable[decIndex]
+	result = append(result, block[0:2]...)
+
+	for i := 4; i < len(block); i++ {
+		originalSample := block[i]
+		secondSample := byte(originalSample >> 4)
+		firstSample := byte((secondSample << 4) ^ originalSample)
+
+		if pCount < pNum {
+			fmt.Println("first dec", firstSample, originalSample, len(block))
+			fmt.Println(decIndex, decPred, decStep)
+		}
+
+		firstBytes := make([]byte, 2)
+		binary.LittleEndian.PutUint16(firstBytes, uint16(DecodeSample(firstSample)))
+		result = append(result, firstBytes...)
+
+		if pCount < pNum {
+			fmt.Println("second dec", secondSample)
+			fmt.Println(decIndex, decPred, decStep)
+			fmt.Println()
+		}
+
+		secondBytes := make([]byte, 2)
+		binary.LittleEndian.PutUint16(secondBytes, uint16(DecodeSample(secondSample)))
+		result = append(result, secondBytes...)
+
+		pCount++
+
+	}
+
+	return result, nil
+}