/*
NAME
  adpcm.go

DESCRIPTION
  adpcm.go contains functions for encoding/compressing pcm into adpcm and decoding/decompressing back to pcm.

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 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 (
	"bytes"
	"encoding/binary"
	"fmt"
	"math"
)

// encoder is used to encode to ADPCM from PCM data.
// est and index hold state that persists between calls to encodeSample and calcHead.
// dest is the output buffer that implements io.writer and io.bytewriter, ie. where the encoded ADPCM data is written to.
type encoder struct {
	dest  *bytes.Buffer
	est   int16
	index int16
}

// decoder is used to decode from ADPCM to PCM data.
// est, index, and step hold state that persists between calls to decodeSample.
// dest is the output buffer that implements io.writer and io.bytewriter, ie. where the decoded PCM data is written to.
type decoder struct {
	dest  *bytes.Buffer
	est   int16
	index int16
	step  int16
}

// PcmBS is the size of the blocks that an encoder uses.
// 'encodeBlock' will encode PcmBS bytes at a time and the output will be AdpcmBS bytes long.
const PcmBS = 1010

// AdpcmBS is the size of the blocks that a decoder uses.
// 'decodeBlock' will decode AdpcmBS bytes at a time and the output will be PcmBS bytes long.
const AdpcmBS = 256

// 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,
}

// NewEncoder retuns a new ADPCM encoder.
func NewEncoder(dst *bytes.Buffer) *encoder {
	e := encoder{
		dest: dst,
	}
	return &e
}

// NewDecoder retuns a new ADPCM decoder.
func NewDecoder(dst *bytes.Buffer) *decoder {
	d := decoder{
		dest: dst,
	}
	return &d
}

// encodeSample takes a single 16 bit PCM sample and
// returns a byte of which the last 4 bits are an encoded ADPCM nibble.
func (e *encoder) encodeSample(sample int16) byte {
	// Find difference between the sample and the previous estimation.
	delta := capAdd16(sample, -e.est)

	// Create and set sign bit for nibble and find absolute value of difference.
	var nib byte
	if delta < 0 {
		nib = 8
		delta = -delta
	}

	step := stepTable[e.index]
	diff := step >> 3
	var mask byte = 4

	for i := 0; i < 3; i++ {
		if delta > step {
			nib |= mask
			delta = capAdd16(delta, -step)
			diff = capAdd16(diff, step)
		}
		mask >>= 1
		step >>= 1
	}

	if nib&8 != 0 {
		diff = -diff
	}

	// Adjust estimated sample based on calculated difference.
	e.est = capAdd16(e.est, diff)

	e.index += indexTable[nib&7]

	// Check for underflow and overflow.
	if e.index < 0 {
		e.index = 0
	} else if e.index > int16(len(stepTable)-1) {
		e.index = int16(len(stepTable) - 1)
	}

	return nib
}

// 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 (d *decoder) decodeSample(nibble byte) int16 {
	// Calculate difference.
	var diff int16
	if nibble&4 != 0 {
		diff = capAdd16(diff, d.step)
	}
	if nibble&2 != 0 {
		diff = capAdd16(diff, d.step>>1)
	}
	if nibble&1 != 0 {
		diff = capAdd16(diff, d.step>>2)
	}
	diff = capAdd16(diff, d.step>>3)

	// Account for sign bit.
	if nibble&8 != 0 {
		diff = -diff
	}

	// Adjust estimated sample based on calculated difference.
	d.est = capAdd16(d.est, diff)

	// Adjust index into step size lookup table using nibble.
	d.index += indexTable[nibble]

	// Check for overflow and underflow.
	if d.index < 0 {
		d.index = 0
	} else if d.index > int16(len(stepTable)-1) {
		d.index = int16(len(stepTable) - 1)
	}

	// Find new quantizer step size.
	d.step = stepTable[d.index]

	return d.est
}

// capAdd16 adds two int16s together and caps at max/min int16 instead of overflowing
func capAdd16(a, b int16) int16 {
	c := int32(a) + int32(b)
	switch {
	case c < math.MinInt16:
		return math.MinInt16
	case c > math.MaxInt16:
		return math.MaxInt16
	default:
		return int16(c)
	}
}

// calcHead sets the state for the encoder by running the first sample through
// the encoder, and writing the first sample to the encoder's io.Writer (dest).
// It returns the number of bytes written to the encoder's io.Writer (dest) along with any errors.
func (e *encoder) calcHead(sample []byte, pad bool) (int, error) {
	// Check that we are given 1 16-bit sample (2 bytes).
	const sampSize = 2
	if len(sample) != sampSize {
		return 0, fmt.Errorf("length of given byte array is: %v, expected: %v", len(sample), sampSize)
	}

	n, err := e.dest.Write(sample)
	if err != nil {
		return n, err
	}

	err = e.dest.WriteByte(byte(int16(e.index)))
	if err != nil {
		return n, err
	}
	n++

	if pad {
		err = e.dest.WriteByte(0x01)
	} else {
		err = e.dest.WriteByte(0x00)
	}
	if err != nil {
		return n, err
	}
	n++
	return n, nil
}

// init initializes the encoder's estimation to the first uncompressed sample and the index to
// point to a suitable quantizer step size.
func (e *encoder) init(samps []byte) {
	int1 := int16(binary.LittleEndian.Uint16(samps[0:2]))
	int2 := int16(binary.LittleEndian.Uint16(samps[2:4]))
	e.est = int1

	halfDiff := math.Abs(math.Abs(float64(int1)) - math.Abs(float64(int2))/2.0)
	closest := math.Abs(float64(stepTable[0]) - halfDiff)
	var cInd int16
	for i, step := range stepTable {
		if math.Abs(float64(step)-halfDiff) < closest {
			closest = math.Abs(float64(step) - halfDiff)
			cInd = int16(i)
		}
	}
	e.index = cInd
}

// Write takes a slice of bytes of arbitrary length representing pcm and encodes in into adpcm.
// It writes its output to the encoder's dest.
// The number of bytes written out is returned along with any error that occured.
func (e *encoder) Write(inPcm []byte) (int, error) {
	// Check that pcm has enough data to initialize decoder
	pcmLen := len(inPcm)
	if pcmLen < 4 {
		return 0, fmt.Errorf("length of given byte array must be greater than 4")
	}

	// Determine if there will be a byte that won't contain two full nibbles and will need padding.
	pad := false
	if (pcmLen-2)%4 != 0 {
		pad = true
	}

	e.init(inPcm[0:4])
	n, err := e.calcHead(inPcm[0:2], pad)
	if err != nil {
		return n, err
	}
	// Skip the first sample and start at the end of the first two samples, then every two samples encode them into a byte of adpcm.
	// TODO: make all hard coded numbers variables so that other bitrates and compression ratios can be used.
	for i := 5; i < pcmLen; i += 4 {
		nib1 := e.encodeSample(int16(binary.LittleEndian.Uint16(inPcm[i-3 : i-1])))
		nib2 := e.encodeSample(int16(binary.LittleEndian.Uint16(inPcm[i-1 : i+1])))
		err = e.dest.WriteByte(byte((nib2 << 4) | nib1))
		if err != nil {
			return n, err
		}
		n++
	}
	// If we've reached the end of the pcm data and there's a sample (2 bytes) left over,
	// compress it to a nibble and leave the first half of the byte padded with 0s.
	if pad {
		nib := e.encodeSample(int16(binary.LittleEndian.Uint16(inPcm[pcmLen-2 : pcmLen])))
		err = e.dest.WriteByte(nib)
		if err != nil {
			return n, err
		}
		n++
	}
	return n, nil
}

// Write takes a slice of bytes of arbitrary length representing adpcm and decodes in into pcm.
// It writes its output to the decoder's dest.
// The number of bytes written out is returned along with any error that occured.
func (d *decoder) Write(inAdpcm []byte) (int, error) {
	// Initialize decoder with first 4 bytes of the inAdpcm.
	d.est = int16(binary.LittleEndian.Uint16(inAdpcm[0:2]))
	d.index = int16(inAdpcm[2])
	d.step = stepTable[d.index]
	n, err := d.dest.Write(inAdpcm[0:2])
	if err != nil {
		return n, err
	}

	// For each byte, seperate it into two nibbles (each nibble is a compressed sample),
	// then decode each nibble and output the resulting 16-bit samples.
	// If padding flag is true (Adpcm[3]), only decode up until the last byte, then decode that separately.
	for i := 4; i < len(inAdpcm)-int(inAdpcm[3]); i++ {
		twoNibs := inAdpcm[i]
		nib2 := byte(twoNibs >> 4)
		nib1 := byte((nib2 << 4) ^ twoNibs)

		firstBytes := make([]byte, 2)
		binary.LittleEndian.PutUint16(firstBytes, uint16(d.decodeSample(nib1)))
		_n, err := d.dest.Write(firstBytes)
		n += _n
		if err != nil {
			return n, err
		}

		secondBytes := make([]byte, 2)
		binary.LittleEndian.PutUint16(secondBytes, uint16(d.decodeSample(nib2)))
		_n, err = d.dest.Write(secondBytes)
		n += _n
		if err != nil {
			return n, err
		}
	}
	if inAdpcm[3] == 0x01 {
		padNib := inAdpcm[len(inAdpcm)-1]
		samp := make([]byte, 2)
		binary.LittleEndian.PutUint16(samp, uint16(d.decodeSample(padNib)))
		_n, err := d.dest.Write(samp)
		n += _n
		if err != nil {
			return n, err
		}
	}
	return n, nil
}

// BytesOutput will return the number of adpcm bytes that will be generated for the given pcm data byte size.
func BytesOutput(pcm int) int {
	// for X pcm bytes, byteDepth (eg. 2 bytes) are left uncompressed, the rest is compressed by a factor of 4
	// and a start index and padding-flag byte are added.
	// Also if there are an even number of samples, there will be half a byte of padding added to the last byte.
	byteDepth := 2
	if pcm%2*byteDepth == 0 { // %2 because samples are encoded 2 at a time.
		return (pcm-byteDepth)/4 + byteDepth + 1 + 1 + 1
	}
	return (pcm-byteDepth)/4 + byteDepth + 1 + 1
}