/*
NAME
  adpcm.go

DESCRIPTION
  See Readme.md

AUTHOR
  Trek Hopton <trek@ausocean.org>

LICENSE
  adpcm.go is Copyright (C) 2018 the Australian Ocean Lab (AusOcean)

  It is free software: you can redistribute it and/or modify them
  under the terms of the GNU General Public License as published by the
  Free Software Foundation, either version 3 of the License, or (at your
  option) any later version.

  It is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 for more details.

  You should have received a copy of the GNU General Public License
  along with revid in gpl.txt.  If not, see [GNU licenses](http://www.gnu.org/licenses).
*/

/*
	Original IMA/DVI ADPCM specification: (http://www.cs.columbia.edu/~hgs/audio/dvi/IMA_ADPCM.pdf).
	Reference algorithms for ADPCM compression and decompression are in part 6.
*/

package adpcm

import (
	"encoding/binary"
	"fmt"
	"math"
)

// table of index changes (see spec)
var indexTable = []int16{
	-1, -1, -1, -1, 2, 4, 6, 8,
	-1, -1, -1, -1, 2, 4, 6, 8,
}

// quantize step size table (see spec)
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
)

// 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 {
	delta := sample - encPred

	var nibble byte

	// set sign bit and find absolute value of difference
	if delta < 0 {
		nibble = 8
		delta = -delta
	}

	step := stepTable[encIndex]
	diff := step >> 3
	var mask byte = 4

	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 {
	// 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, check for overflow
	if diff > 0 {
		if decPred > math.MaxInt16-diff {
			decPred = math.MaxInt16
		} else {
			decPred += diff
		}
	} else {
		if decPred < math.MaxInt16-diff {
			decPred = math.MaxInt16
		} else {
			decPred += diff
		}
	}

	// 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) {
	// check that we are given 1 16-bit sample (2 bytes)
	sampSize := 2
	if len(sample) != sampSize {
		return nil, fmt.Errorf("length of given byte array is: %v, expected: %v", len(sample), sampSize)
	}

	intSample := int16(binary.LittleEndian.Uint16(sample))
	encodeSample(intSample)

	head := make([]byte, 2)
	head[0] = sample[0]
	head[1] = sample[1]

	head = append(head, byte(uint16(encIndex)))
	head = append(head, byte(0x00))
	return head, nil
}

// EncodeBlock takes a slice of 1010 bytes (505 16-bit PCM samples).
// It returns a byte slice containing encoded (compressed) ADPCM nibbles (each byte contains two nibbles).
func EncodeBlock(block []byte) ([]byte, error) {
	bSize := 1010
	if len(block) != bSize {
		return nil, fmt.Errorf("unsupported block size. Given: %v, expected: %v, ie. 505 16-bit PCM samples", len(block), bSize)
	}

	result, err := calcHead(block[0:2])
	if err != nil {
		return nil, err
	}

	for i := 2; i < len(block); i++ {
		if (i+1)%4 == 0 {
			sample2 := encodeSample(int16(binary.LittleEndian.Uint16(block[i-1 : i+1])))
			sample := encodeSample(int16(binary.LittleEndian.Uint16(block[i+1 : i+3])))
			result = append(result, byte((sample<<4)|sample2))

		}
	}

	return result, nil
}

// DecodeBlock takes a slice of 256 bytes, each byte should contain two ADPCM encoded nibbles.
// It returns a byte slice containing the resulting decoded (uncompressed) 16-bit PCM samples.
func DecodeBlock(block []byte) ([]byte, error) {
	bSize := 256
	if len(block) != bSize {
		return nil, fmt.Errorf("unsupported block size. Given: %v, expected: %v", len(block), bSize)
	}

	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)

		firstBytes := make([]byte, 2)
		binary.LittleEndian.PutUint16(firstBytes, uint16(decodeSample(firstSample)))
		result = append(result, firstBytes...)

		secondBytes := make([]byte, 2)
		binary.LittleEndian.PutUint16(secondBytes, uint16(decodeSample(secondSample)))
		result = append(result, secondBytes...)
	}

	return result, nil
}