/*-
 * Copyright (c) 2015 Nathanial Sloss <nathanialsloss@yahoo.com.au>
 *
 *		This software is dedicated to the memory of -
 *	   Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
 *
 *		Barry was a man who loved his music.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/param.h>
#include <sys/endian.h>
#include <sys/uio.h>

#include <stdio.h>
#include <errno.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>

#include "sbc_coeffs.h"
#include "bt.h"

#define	SYNCWORD	0x9c
#define	ABS(x)		(((x) < 0) ? -(x) : (x))
#define	BIT30		(1U << 30)
#define	BM(x)		((1LL << (x)) - 1LL)

/* Loudness offset allocations. */
static const int loudnessoffset8[4][8] = {
	{-2, 0, 0, 0, 0, 0, 0, 1},
	{-3, 0, 0, 0, 0, 0, 1, 2},
	{-4, 0, 0, 0, 0, 0, 1, 2},
	{-4, 0, 0, 0, 0, 0, 1, 2},
};

static const int loudnessoffset4[4][4] = {
	{-1, 0, 0, 0},
	{-2, 0, 0, 1},
	{-2, 0, 0, 1},
	{-2, 0, 0, 1},
};

static uint8_t
calc_scalefactors_joint(struct sbc_encode *sbc)
{
	float sb_j[16][2];
	uint32_t x;
	uint32_t y;
	uint8_t block;
	uint8_t joint;
	uint8_t sb;
	uint8_t lz;

	joint = 0;
	for (sb = 0; sb != sbc->bands - 1; sb++) {
		for (block = 0; block < sbc->blocks; block++) {
			sb_j[block][0] = (sbc->samples[block][0][sb] +
			    sbc->samples[block][1][sb]) / 2.0f;
			sb_j[block][1] = (sbc->samples[block][0][sb] -
			    sbc->samples[block][1][sb]) / 2.0f;
		}

		x = 1 << 15;
		y = 1 << 15;
		for (block = 0; block < sbc->blocks; block++) {
			x |= (uint32_t)ABS(sb_j[block][0]);
			y |= (uint32_t)ABS(sb_j[block][1]);
		}

		lz = 1;
		while (!(x & BIT30)) {
			lz++;
			x <<= 1;
		}
		x = 16 - lz;

		lz = 1;
		while (!(y & BIT30)) {
			lz++;
			y <<= 1;
		}
		y = 16 - lz;

		if ((sbc->scalefactor[0][sb] + sbc->scalefactor[1][sb]) > x + y) {
			joint |= 1 << (sbc->bands - sb - 1);
			sbc->scalefactor[0][sb] = x;
			sbc->scalefactor[1][sb] = y;
			for (block = 0; block < sbc->blocks; block++) {
				sbc->samples[block][0][sb] = sb_j[block][0];
				sbc->samples[block][1][sb] = sb_j[block][1];
			}
		}
	}
	return (joint);
}

static void
calc_scalefactors(struct sbc_encode *sbc)
{
	uint8_t block;
	uint8_t ch;
	uint8_t sb;

	for (ch = 0; ch != sbc->channels; ch++) {
		for (sb = 0; sb != sbc->bands; sb++) {
			uint32_t x = 1 << 15;
			uint8_t lx = 1;

			for (block = 0; block != sbc->blocks; block++)
				x |= (uint32_t)ABS(sbc->samples[block][ch][sb]);

			while (!(x & BIT30)) {
				lx++;
				x <<= 1;
			}
			sbc->scalefactor[ch][sb] = 16 - lx;
		}
	}
}

static void
calc_bitneed(struct bt_config *cfg)
{
	struct sbc_encode *sbc = cfg->handle.sbc_enc;
	int32_t bitneed[2][8];
	int32_t max_bitneed, bitcount;
	int32_t slicecount, bitslice;
	int32_t loudness;
	int ch, sb, start_chan = 0;

	if (cfg->chmode == MODE_DUAL)
		sbc->channels = 1;

next_chan:
	max_bitneed = 0;
	bitcount = 0;
	slicecount = 0;

	if (cfg->allocm == ALLOC_SNR) {
		for (ch = start_chan; ch < sbc->channels; ch++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				bitneed[ch][sb] = sbc->scalefactor[ch][sb];

				if (bitneed[ch][sb] > max_bitneed)
					max_bitneed = bitneed[ch][sb];
			}
		}
	} else {
		for (ch = start_chan; ch < sbc->channels; ch++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if (sbc->scalefactor[ch][sb] == 0) {
					bitneed[ch][sb] = -5;
				} else {
					if (sbc->bands == 8) {
						loudness = sbc->scalefactor[ch][sb] -
						    loudnessoffset8[cfg->freq][sb];
					} else {
						loudness = sbc->scalefactor[ch][sb] -
						    loudnessoffset4[cfg->freq][sb];
					}
					if (loudness > 0)
						bitneed[ch][sb] = loudness / 2;
					else
						bitneed[ch][sb] = loudness;
				}
				if (bitneed[ch][sb] > max_bitneed)
					max_bitneed = bitneed[ch][sb];
			}
		}
	}

	slicecount = bitcount = 0;
	bitslice = max_bitneed + 1;
	do {
		bitslice--;
		bitcount += slicecount;
		slicecount = 0;
		for (ch = start_chan; ch < sbc->channels; ch++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if ((bitneed[ch][sb] > bitslice + 1) &&
				    (bitneed[ch][sb] < bitslice + 16))
					slicecount++;
				else if (bitneed[ch][sb] == bitslice + 1)
					slicecount += 2;
			}
		}
	} while (bitcount + slicecount < cfg->bitpool);

	/* check if exactly one more fits */
	if (bitcount + slicecount == cfg->bitpool) {
		bitcount += slicecount;
		bitslice--;
	}
	for (ch = start_chan; ch < sbc->channels; ch++) {
		for (sb = 0; sb < sbc->bands; sb++) {
			if (bitneed[ch][sb] < bitslice + 2) {
				sbc->bits[ch][sb] = 0;
			} else {
				sbc->bits[ch][sb] = bitneed[ch][sb] - bitslice;
				if (sbc->bits[ch][sb] > 16)
					sbc->bits[ch][sb] = 16;
			}
		}
	}

	if (cfg->chmode == MODE_DUAL)
		ch = start_chan;
	else
		ch = 0;
	sb = 0;
	while (bitcount < cfg->bitpool && sb < sbc->bands) {
		if ((sbc->bits[ch][sb] >= 2) && (sbc->bits[ch][sb] < 16)) {
			sbc->bits[ch][sb]++;
			bitcount++;
		} else if ((bitneed[ch][sb] == bitslice + 1) &&
		    (cfg->bitpool > bitcount + 1)) {
			sbc->bits[ch][sb] = 2;
			bitcount += 2;
		}
		if (sbc->channels == 1 || start_chan == 1)
			sb++;
		else if (ch == 1) {
			ch = 0;
			sb++;
		} else
			ch = 1;
	}

	if (cfg->chmode == MODE_DUAL)
		ch = start_chan;
	else
		ch = 0;
	sb = 0;
	while (bitcount < cfg->bitpool && sb < sbc->bands) {
		if (sbc->bits[ch][sb] < 16) {
			sbc->bits[ch][sb]++;
			bitcount++;
		}
		if (sbc->channels == 1 || start_chan == 1)
			sb++;
		else if (ch == 1) {
			ch = 0;
			sb++;
		} else
			ch = 1;
	}

	if (cfg->chmode == MODE_DUAL && start_chan == 0) {
		start_chan = 1;
		sbc->channels = 2;
		goto next_chan;
	}
}

static void
sbc_store_bits_crc(struct sbc_encode *sbc, uint32_t numbits, uint32_t value)
{
	uint32_t off = sbc->bitoffset;

	while (numbits-- && off != sbc->maxoffset) {
		if (value & (1 << numbits)) {
			sbc->data[off / 8] |= 1 << ((7 - off) & 7);
			sbc->crc ^= 0x80;
		}
		sbc->crc *= 2;
		if (sbc->crc & 0x100)
			sbc->crc ^= 0x11d;	/* CRC-8 polynomial */

		off++;
	}
	sbc->bitoffset = off;
}

static int
sbc_encode(struct bt_config *cfg)
{
	struct sbc_encode *sbc = cfg->handle.sbc_enc;
	const int16_t *input = sbc->music_data;
	float delta[2][8];
	float levels[2][8];
	float mask[2][8];
	float S;
	float *X;
	float Z[80];
	float Y[80];
	float audioout;
	int16_t left[8];
	int16_t right[8];
	int16_t *data;
	int numsamples;
	int i;
	int k;
	int block;
	int chan;
	int sb;

	for (block = 0; block < sbc->blocks; block++) {

		for (i = 0; i < sbc->bands; i++) {
			left[i] = *input++;
			if (sbc->channels == 2)
				right[i] = *input++;
		}

		for (chan = 0; chan < sbc->channels; chan++) {

			/* select right or left channel */
			if (chan == 0) {
				X = sbc->left;
				data = left;
			} else {
				X = sbc->right;
				data = right;
			}

			/* shift up old data */
			for (i = (sbc->bands * 10) - 1; i > sbc->bands - 1; i--)
				X[i] = X[i - sbc->bands];
			k = 0;
			for (i = sbc->bands - 1; i >= 0; i--)
				X[i] = data[k++];
			for (i = 0; i < sbc->bands * 10; i++) {
				if (sbc->bands == 8)
					Z[i] = sbc_coeffs8[i] * X[i];
				else
					Z[i] = sbc_coeffs4[i] * X[i];
			}
			for (i = 0; i < sbc->bands * 2; i++) {
				Y[i] = 0;
				for (k = 0; k < 5; k++)
					Y[i] += Z[i + k * sbc->bands * 2];
			}
			for (i = 0; i < sbc->bands; i++) {
				S = 0;
				for (k = 0; k < sbc->bands * 2; k++) {
					if (sbc->bands == 8) {
						S += cosdata8[i][k] * Y[k];
					} else {
						S += cosdata4[i][k] * Y[k];
					}
				}
				sbc->samples[block][chan][i] = S * (1 << 15);
			}
		}
	}

	calc_scalefactors(sbc);

	if (cfg->chmode == MODE_JOINT)
		sbc->join = calc_scalefactors_joint(sbc);
	else
		sbc->join = 0;

	calc_bitneed(cfg);

	for (chan = 0; chan < sbc->channels; chan++) {
		for (sb = 0; sb < sbc->bands; sb++) {
			if (sbc->bits[chan][sb] == 0)
				continue;
			mask[chan][sb] = BM(sbc->bits[chan][sb]);
			levels[chan][sb] = mask[chan][sb] *
			    (1LL << (15 - sbc->scalefactor[chan][sb]));
			delta[chan][sb] =
			    (1LL << (sbc->scalefactor[chan][sb] + 16));
		}
	}

	numsamples = 0;
	for (block = 0; block < sbc->blocks; block++) {
		for (chan = 0; chan < sbc->channels; chan++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if (sbc->bits[chan][sb] == 0)
					continue;
				audioout = (levels[chan][sb] *
				    (delta[chan][sb] + sbc->samples[block][chan][sb]));
				audioout /= (1LL << 32);

				audioout = roundf(audioout);

				/* range check */
				if (audioout > mask[chan][sb])
					audioout = mask[chan][sb];

				sbc->output[numsamples++] = audioout;
			}
		}
	}
	return (numsamples);
}

static void
sbc_decode(struct bt_config *cfg)
{
	struct sbc_encode *sbc = cfg->handle.sbc_enc;
	float delta[2][8];
	float levels[2][8];
	float audioout;
	float *X;
	float *V;
	float left[160];
	float right[160];
	float U[160];
	float W[160];
	float S[8];
	int position;
	int block;
	int chan;
	int sb;
	int i;
	int k;

	for (chan = 0; chan < sbc->channels; chan++) {
		for (sb = 0; sb < sbc->bands; sb++) {
			levels[chan][sb] = (1 << sbc->bits[chan][sb]) - 1;
			delta[chan][sb] = (1 << sbc->scalefactor[chan][sb]);
		}
	}

	i = 0;
	for (block = 0; block < sbc->blocks; block++) {
		for (chan = 0; chan < sbc->channels; chan++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if (sbc->bits[chan][sb] == 0) {
					audioout = 0;
				} else {
					audioout =
					  ((((sbc->output[i] * 2.0f) + 1.0f) * delta[chan][sb]) /
					  levels[chan][sb]) - delta[chan][sb];
				}
				sbc->output[i++] = audioout;
			}
		}
	}

	if (cfg->chmode == MODE_JOINT) {
		i = 0;
		while (i < (sbc->blocks * sbc->bands * sbc->channels)) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if (sbc->join & (1 << (sbc->bands - sb - 1))) {
					audioout = sbc->output[i];
					sbc->output[i] = (2.0f * sbc->output[i]) +
					    (2.0f * sbc->output[i + sbc->bands]);
					sbc->output[i + sbc->bands] =
					    (2.0f * audioout) -
					    (2.0f * sbc->output[i + sbc->bands]);
					sbc->output[i] /= 2.0f;
					sbc->output[i + sbc->bands] /= 2.0f;
				}
				i++;
			}
			i += sbc->bands;
		}
	}
	position = 0;
	for (block = 0; block < sbc->blocks; block++) {
		for (chan = 0; chan < sbc->channels; chan++) {
			/* select right or left channel */
			if (chan == 0) {
				X = left;
				V = sbc->left;
			} else {
				X = right;
				V = sbc->right;
			}
			for (i = 0; i < sbc->bands; i++)
				S[i] = sbc->output[position++];

			for (i = (sbc->bands * 20) - 1; i >= (sbc->bands * 2); i--)
				V[i] = V[i - (sbc->bands * 2)];
			for (k = 0; k < sbc->bands * 2; k++) {
				float vk = 0;
				for (i = 0; i < sbc->bands; i++) {
					if (sbc->bands == 8) {
						vk += cosdecdata8[i][k] * S[i];
					} else {
						vk += cosdecdata4[i][k] * S[i];
					}
				}
				V[k] = vk;
			}
			for (i = 0; i <= 4; i++) {
				for (k = 0; k < sbc->bands; k++) {
					U[(i * sbc->bands * 2) + k] =
					    V[(i * sbc->bands * 4) + k];
					U[(i * sbc->bands
					    * 2) + sbc->bands + k] =
					    V[(i * sbc->bands * 4) +
					    (sbc->bands * 3) + k];
				}
			}
			for (i = 0; i < sbc->bands * 10; i++) {
				if (sbc->bands == 4) {
					W[i] = U[i] * (sbc_coeffs4[i] * -4.0f);
				} else if (sbc->bands == 8) {
					W[i] = U[i] * (sbc_coeffs8[i] * -8.0f);
				} else {
					W[i] = 0;
				}
			}

			for (k = 0; k < sbc->bands; k++) {
				unsigned int offset = k + (block * sbc->bands);

				X[offset] = 0;
				for (i = 0; i < 10; i++) {
					X[offset] += W[k + (i * sbc->bands)];
				}

				if (X[offset] > 32767.0)
					X[offset] = 32767.0;
				else if (X[offset] < -32767.0)
					X[offset] = -32767.0;
			}
		}
	}

	for (i = 0, k = 0; k != (sbc->blocks * sbc->bands); k++) {
		sbc->music_data[i++] = left[k];
		if (sbc->channels == 2)
			sbc->music_data[i++] = right[k];
	}
}

size_t
sbc_encode_frame(struct bt_config *cfg)
{
	struct sbc_encode *sbc = cfg->handle.sbc_enc;
	uint8_t config;
	uint8_t block;
	uint8_t chan;
	uint8_t sb;
	uint8_t j;
	uint8_t i;

	config = (cfg->freq << 6) | (cfg->blocks << 4) |
	    (cfg->chmode << 2) | (cfg->allocm << 1) | cfg->bands;

	sbc_encode(cfg);

	/* set initial CRC */
	sbc->crc = 0x5e;

	/* reset data position and size */
	sbc->bitoffset = 0;
	sbc->maxoffset = sizeof(sbc->data) * 8;

	sbc_store_bits_crc(sbc, 8, SYNCWORD);
	sbc_store_bits_crc(sbc, 8, config);
	sbc_store_bits_crc(sbc, 8, cfg->bitpool);

	/* skip 8-bit CRC */
	sbc->bitoffset += 8;

	if (cfg->chmode == MODE_JOINT) {
		if (sbc->bands == 8)
			sbc_store_bits_crc(sbc, 8, sbc->join);
		else if (sbc->bands == 4)
			sbc_store_bits_crc(sbc, 4, sbc->join);
	}
	for (i = 0; i < sbc->channels; i++) {
		for (j = 0; j < sbc->bands; j++)
			sbc_store_bits_crc(sbc, 4, sbc->scalefactor[i][j]);
	}

	/* store 8-bit CRC */
	sbc->data[3] = (sbc->crc & 0xFF);

	i = 0;
	for (block = 0; block < sbc->blocks; block++) {
		for (chan = 0; chan < sbc->channels; chan++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if (sbc->bits[chan][sb] == 0)
					continue;

				sbc_store_bits_crc(sbc, sbc->bits[chan][sb], sbc->output[i++]);
			}
		}
	}
	return ((sbc->bitoffset + 7) / 8);
}

static uint32_t
sbc_load_bits_crc(struct sbc_encode *sbc, uint32_t numbits)
{
	uint32_t off = sbc->bitoffset;
	uint32_t value = 0;

	while (numbits-- && off != sbc->maxoffset) {
		if (sbc->rem_data_ptr[off / 8] & (1 << ((7 - off) & 7))) {
			value |= (1 << numbits);
			sbc->crc ^= 0x80;
		}
		sbc->crc *= 2;
		if (sbc->crc & 0x100)
			sbc->crc ^= 0x11d;	/* CRC-8 polynomial */

		off++;
	}
	sbc->bitoffset = off;
	return (value);
}

size_t
sbc_decode_frame(struct bt_config *cfg, int bits)
{
	struct sbc_encode *sbc = cfg->handle.sbc_enc;
	uint8_t config;
	uint8_t block;
	uint8_t chan;
	uint8_t sb;
	uint8_t j;
	uint8_t i;

	sbc->rem_off = 0;
	sbc->rem_len = 0;

	config = (cfg->freq << 6) | (cfg->blocks << 4) |
	    (cfg->chmode << 2) | (cfg->allocm << 1) | cfg->bands;

	/* set initial CRC */
	sbc->crc = 0x5e;

	/* reset data position and size */
	sbc->bitoffset = 0;
	sbc->maxoffset = bits;

	/* verify SBC header */
	if (sbc->maxoffset < (8 * 4))
		return (0);
	if (sbc_load_bits_crc(sbc, 8) != SYNCWORD)
		return (0);
	if (sbc_load_bits_crc(sbc, 8) != config)
		return (0);
	cfg->bitpool = sbc_load_bits_crc(sbc, 8);

	(void)sbc_load_bits_crc(sbc, 8);/* CRC */

	if (cfg->chmode == MODE_JOINT) {
		if (sbc->bands == 8)
			sbc->join = sbc_load_bits_crc(sbc, 8);
		else if (sbc->bands == 4)
			sbc->join = sbc_load_bits_crc(sbc, 4);
		else
			sbc->join = 0;
	} else {
		sbc->join = 0;
	}

	for (i = 0; i < sbc->channels; i++) {
		for (j = 0; j < sbc->bands; j++)
			sbc->scalefactor[i][j] = sbc_load_bits_crc(sbc, 4);
	}

	calc_bitneed(cfg);

	i = 0;
	for (block = 0; block < sbc->blocks; block++) {
		for (chan = 0; chan < sbc->channels; chan++) {
			for (sb = 0; sb < sbc->bands; sb++) {
				if (sbc->bits[chan][sb] == 0) {
					i++;
					continue;
				}
				sbc->output[i++] =
				    sbc_load_bits_crc(sbc, sbc->bits[chan][sb]);
			}
		}
	}

	sbc_decode(cfg);

	sbc->rem_off = 0;
	sbc->rem_len = sbc->blocks * sbc->channels * sbc->bands;

	return ((sbc->bitoffset + 7) / 8);
}