/*
 * Automatic upmix plugin
 *
 * Copyright (c) 2006 by Takashi Iwai <tiwai@suse.de>
 *
 * This library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation; either version 2.1 of
 * the License, or (at your option) any later version.
 *
 * This program 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <alsa/asoundlib.h>
#include <alsa/pcm_external.h>

typedef struct snd_pcm_upmix snd_pcm_upmix_t;

typedef void (*upmixer_t)(snd_pcm_upmix_t *mix,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size);

struct snd_pcm_upmix {
	snd_pcm_extplug_t ext;
	/* setup */
	int delay_ms;
	/* privates */
	upmixer_t upmix;
	unsigned int curpos;
	int delay;
	short *delayline[2];
};

static inline void *area_addr(const snd_pcm_channel_area_t *area,
			      snd_pcm_uframes_t offset)
{
	unsigned int bitofs = area->first + area->step * offset;
	return (char *) area->addr + bitofs / 8;
}

static inline unsigned int area_step(const snd_pcm_channel_area_t *area)
{
	return area->step / 8;
}

/* Delayed copy SL & SR */
static void delayed_copy(snd_pcm_upmix_t *mix,
			 const snd_pcm_channel_area_t *dst_areas,
			 snd_pcm_uframes_t dst_offset,
			 const snd_pcm_channel_area_t *src_areas,
			 snd_pcm_uframes_t src_offset,
			 unsigned int size)
{
	unsigned int i, p, delay, curpos, dst_step, src_step;
	short *dst, *src;

	if (! mix->delay_ms) {
		snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
				   2, size, SND_PCM_FORMAT_S16);
		return;
	}

	delay = mix->delay;
	if (delay > size)
		delay = size;
	for (i = 0; i < 2; i++) {
		dst = (short *)area_addr(dst_areas + i, dst_offset);
		dst_step = area_step(dst_areas + i) / 2;
		curpos = mix->curpos;
		for (p = 0; p < delay; p++) {
			*dst = mix->delayline[i][curpos];
			dst += dst_step;
			curpos = (curpos + 1) % mix->delay;
		}
		snd_pcm_area_copy(dst_areas + i, dst_offset + delay,
				  src_areas + i, src_offset,
				  size - delay, SND_PCM_FORMAT_S16);
		src = (short *)area_addr(src_areas + i,
					 src_offset + size - delay);
		src_step = area_step(src_areas + i) / 2;
		curpos = mix->curpos;
		for (p = 0; p < delay; p++) {
			mix->delayline[i][curpos] = *src;
			src += src_step;
			curpos = (curpos + 1) % mix->delay;
		}
	}
	mix->curpos = curpos;
}

/* Average of L+R -> C and LFE */
static void average_copy(const snd_pcm_channel_area_t *dst_areas,
			 snd_pcm_uframes_t dst_offset,
			 const snd_pcm_channel_area_t *src_areas,
			 snd_pcm_uframes_t src_offset,
			 unsigned int nchns,
			 unsigned int size)
{
	short *dst[2], *src[2];
	unsigned int i, dst_step[2], src_step[2];

	for (i = 0; i < nchns; i++) {
		dst[i] = (short *)area_addr(dst_areas + i, dst_offset);
		dst_step[i] = area_step(dst_areas + i) / 2;
	}
	for (i = 0; i < 2; i++) {
		src[i] = (short *)area_addr(src_areas + i, src_offset);
		src_step[i] = area_step(src_areas + i) / 2;
	}
	while (size--) {
		short val = (*src[0] >> 1) + (*src[1] >> 1);
		for (i = 0; i < nchns; i++) {
			*dst[i] = val;
			dst[i] += dst_step[i];
		}
		src[0] += src_step[0];
		src[1] += src_step[1];
	}
}

static void upmix_1_to_71(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	int i;
	for (i = 0; i < 8; i++)
		snd_pcm_area_copy(dst_areas + i, dst_offset,
				  src_areas, src_offset,
				  size, SND_PCM_FORMAT_S16);
}

static void upmix_1_to_51(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	int i;
	for (i = 0; i < 6; i++)
		snd_pcm_area_copy(dst_areas + i, dst_offset,
				  src_areas, src_offset,
				  size, SND_PCM_FORMAT_S16);
}

static void upmix_1_to_40(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	int i;
	for (i = 0; i < 4; i++)
		snd_pcm_area_copy(dst_areas + i, dst_offset,
				  src_areas, src_offset,
				  size, SND_PCM_FORMAT_S16);
}

static void upmix_2_to_71(snd_pcm_upmix_t *mix,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
	delayed_copy(mix, dst_areas + 2, dst_offset, src_areas, src_offset,
		     size);
	average_copy(dst_areas + 4, dst_offset, src_areas, src_offset,
		     2, size);
	snd_pcm_areas_copy(dst_areas + 6, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
	
}

static void upmix_2_to_51(snd_pcm_upmix_t *mix,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
	delayed_copy(mix, dst_areas + 2, dst_offset, src_areas, src_offset,
		     size);
	average_copy(dst_areas + 4, dst_offset, src_areas, src_offset,
		     2, size);
}

static void upmix_2_to_40(snd_pcm_upmix_t *mix,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
	delayed_copy(mix, dst_areas + 2, dst_offset, src_areas, src_offset,
		     size);
}

static void upmix_3_to_51(snd_pcm_upmix_t *mix,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
	delayed_copy(mix, dst_areas + 2, dst_offset, src_areas, src_offset,
		     size);
	snd_pcm_areas_copy(dst_areas + 4, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
}

static void upmix_3_to_40(snd_pcm_upmix_t *mix,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
	delayed_copy(mix, dst_areas + 2, dst_offset, src_areas, src_offset,
		     size);
}

static void upmix_4_to_51(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   4, size, SND_PCM_FORMAT_S16);
	snd_pcm_areas_copy(dst_areas + 4, dst_offset, src_areas, src_offset,
			   2, size, SND_PCM_FORMAT_S16);
}

static void upmix_4_to_40(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   4, size, SND_PCM_FORMAT_S16);
}

static void upmix_5_to_51(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   5, size, SND_PCM_FORMAT_S16);
	snd_pcm_area_copy(dst_areas + 5, dst_offset, src_areas + 4, src_offset,
			  size, SND_PCM_FORMAT_S16);
}

static void upmix_6_to_51(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   6, size, SND_PCM_FORMAT_S16);
}

static void upmix_8_to_71(snd_pcm_upmix_t *mix ATTRIBUTE_UNUSED,
			  const snd_pcm_channel_area_t *dst_areas,
			  snd_pcm_uframes_t dst_offset,
			  const snd_pcm_channel_area_t *src_areas,
			  snd_pcm_uframes_t src_offset,
			  snd_pcm_uframes_t size)
{
	snd_pcm_areas_copy(dst_areas, dst_offset, src_areas, src_offset,
			   8, size, SND_PCM_FORMAT_S16);
}

static const upmixer_t do_upmix[8][3] = {
	{ upmix_1_to_40, upmix_1_to_51, upmix_1_to_71 },
	{ upmix_2_to_40, upmix_2_to_51, upmix_2_to_71 },
	{ upmix_3_to_40, upmix_3_to_51, upmix_3_to_51 },
	{ upmix_4_to_40, upmix_4_to_51, upmix_4_to_51 },
	{ upmix_4_to_40, upmix_5_to_51, upmix_5_to_51 },
	{ upmix_4_to_40, upmix_6_to_51, upmix_6_to_51 },
	{ upmix_4_to_40, upmix_6_to_51, upmix_6_to_51 },
	{ upmix_4_to_40, upmix_6_to_51, upmix_8_to_71 },
};

static snd_pcm_sframes_t
upmix_transfer(snd_pcm_extplug_t *ext,
	       const snd_pcm_channel_area_t *dst_areas,
	       snd_pcm_uframes_t dst_offset,
	       const snd_pcm_channel_area_t *src_areas,
	       snd_pcm_uframes_t src_offset,
	       snd_pcm_uframes_t size)
{
	snd_pcm_upmix_t *mix = (snd_pcm_upmix_t *)ext;
	mix->upmix(mix, dst_areas, dst_offset,
		   src_areas, src_offset, size);
	return size;
}

static int upmix_init(snd_pcm_extplug_t *ext)
{
	snd_pcm_upmix_t *mix = (snd_pcm_upmix_t *)ext;
	int ctype, stype;

	switch (ext->slave_channels) {
		case	6:
			stype = 1;
			break;
		case 8:
			stype = 2;
			break;
		default:
			stype = 0;
	}
	ctype = ext->channels - 1;
	if (ctype < 0 || ctype > 7) {
		SNDERR("Invalid channel numbers for upmix: %d", ctype + 1);
		return -EINVAL;
	}
	mix->upmix = do_upmix[ctype][stype];

	if (mix->delay_ms) {
		free(mix->delayline[0]);
		free(mix->delayline[1]);
		mix->delay = ext->rate * mix->delay_ms / 1000;
		mix->delayline[0] = calloc(2, mix->delay);
		mix->delayline[1] = calloc(2, mix->delay);
		if (! mix->delayline[0] || ! mix->delayline[1])
			return -ENOMEM;
		mix->curpos = 0;
	}
	return 0;
}

static int upmix_close(snd_pcm_extplug_t *ext)
{
	snd_pcm_upmix_t *mix = (snd_pcm_upmix_t *)ext;
	free(mix->delayline[0]);
	free(mix->delayline[1]);
	return 0;
}

#if SND_PCM_EXTPLUG_VERSION >= 0x10002
static unsigned int chmap[8][8] = {
	{ SND_CHMAP_MONO },
	{ SND_CHMAP_FL, SND_CHMAP_FR },
	{ SND_CHMAP_FL, SND_CHMAP_FR, SND_CHMAP_FC },
	{ SND_CHMAP_FL, SND_CHMAP_FR, SND_CHMAP_RL, SND_CHMAP_RR },
	{ SND_CHMAP_FL, SND_CHMAP_FR, SND_CHMAP_RL, SND_CHMAP_RR, SND_CHMAP_FC },
	{ SND_CHMAP_FL, SND_CHMAP_FR, SND_CHMAP_RL, SND_CHMAP_RR, SND_CHMAP_FC, SND_CHMAP_LFE },
	{ SND_CHMAP_FL, SND_CHMAP_FR, SND_CHMAP_RL, SND_CHMAP_RR, SND_CHMAP_FC, SND_CHMAP_LFE, SND_CHMAP_UNKNOWN },
	{ SND_CHMAP_FL, SND_CHMAP_FR, SND_CHMAP_RL, SND_CHMAP_RR, SND_CHMAP_FC, SND_CHMAP_LFE, SND_CHMAP_SL, SND_CHMAP_SR },
};

static snd_pcm_chmap_query_t **upmix_query_chmaps(snd_pcm_extplug_t *ext ATTRIBUTE_UNUSED)
{
	snd_pcm_chmap_query_t **maps;
	int i;

	maps = calloc(9, sizeof(void *));
	if (!maps)
		return NULL;
	for (i = 0; i < 8; i++) {
		snd_pcm_chmap_query_t *p;
		p = maps[i] = calloc(i + 1 + 2, sizeof(int));
		if (!p) {
			snd_pcm_free_chmaps(maps);
			return NULL;
		}
		p->type = SND_CHMAP_TYPE_FIXED;
		p->map.channels = i + 1;
		memcpy(p->map.pos, &chmap[i][0], (i + 1) * sizeof(int));
	}
	return maps;
}

static snd_pcm_chmap_t *upmix_get_chmap(snd_pcm_extplug_t *ext)
{
	snd_pcm_chmap_t *map;

	if (ext->channels < 1 || ext->channels > 8)
		return NULL;
	map = malloc((ext->channels + 1) * sizeof(int));
	if (!map)
		return NULL;
	map->channels = ext->channels;
	memcpy(map->pos, &chmap[ext->channels - 1][0], ext->channels * sizeof(int));
	return map;
}
#endif /* SND_PCM_EXTPLUG_VERSION >= 0x10002 */

static const snd_pcm_extplug_callback_t upmix_callback = {
	.transfer = upmix_transfer,
	.init = upmix_init,
	.close = upmix_close,
#if SND_PCM_EXTPLUG_VERSION >= 0x10002
	.query_chmaps = upmix_query_chmaps,
	.get_chmap = upmix_get_chmap,
#endif
};

SND_PCM_PLUGIN_DEFINE_FUNC(upmix)
{
	snd_config_iterator_t i, next;
	snd_pcm_upmix_t *mix;
	snd_config_t *sconf = NULL;
	static const unsigned int chlist[3] = {4, 6, 8};
	unsigned int channels = 0;
	int delay = 10;
	int err;

	snd_config_for_each(i, next, conf) {
		snd_config_t *n = snd_config_iterator_entry(i);
		const char *id;
		if (snd_config_get_id(n, &id) < 0)
			continue;
		if (strcmp(id, "comment") == 0 || strcmp(id, "type") == 0 || strcmp(id, "hint") == 0)
			continue;
		if (strcmp(id, "slave") == 0) {
			sconf = n;
			continue;
		}
		if (strcmp(id, "delay") == 0) {
			long val;
			err = snd_config_get_integer(n, &val);
			if (err < 0) {
				SNDERR("Invalid value for %s", id);
				return err;
			}
			delay = val;
			continue;
		}
		if (strcmp(id, "channels") == 0) {
			long val;
			err = snd_config_get_integer(n, &val);
			if (err < 0) {
				SNDERR("Invalid value for %s", id);
				return err;
			}
			channels = val;
			if (channels != 4 && channels != 6 && channels != 0 && channels != 8) {
				SNDERR("channels must be 4, 6, 8 or 0");
				return -EINVAL;
			}
			continue;
		}
		SNDERR("Unknown field %s", id);
		return -EINVAL;
	}

	if (! sconf) {
		SNDERR("No slave configuration for filrmix pcm");
		return -EINVAL;
	}

	mix = calloc(1, sizeof(*mix));
	if (mix == NULL)
		return -ENOMEM;

	mix->ext.version = SND_PCM_EXTPLUG_VERSION;
	mix->ext.name = "Upmix Plugin";
	mix->ext.callback = &upmix_callback;
	mix->ext.private_data = mix;
	if (delay < 0)
		delay = 0;
	else if (delay > 1000)
		delay = 1000;
	mix->delay_ms = delay;

	err = snd_pcm_extplug_create(&mix->ext, name, root, sconf, stream, mode);
	if (err < 0) {
		free(mix);
		return err;
	}

	snd_pcm_extplug_set_param_minmax(&mix->ext,
					 SND_PCM_EXTPLUG_HW_CHANNELS,
					 1, 8);
	if (channels)
		snd_pcm_extplug_set_slave_param_minmax(&mix->ext,
						       SND_PCM_EXTPLUG_HW_CHANNELS,
						       channels, channels);
	else
		snd_pcm_extplug_set_slave_param_list(&mix->ext,
						     SND_PCM_EXTPLUG_HW_CHANNELS,
						     3, chlist);
	snd_pcm_extplug_set_param(&mix->ext, SND_PCM_EXTPLUG_HW_FORMAT,
				  SND_PCM_FORMAT_S16);
	snd_pcm_extplug_set_slave_param(&mix->ext, SND_PCM_EXTPLUG_HW_FORMAT,
					SND_PCM_FORMAT_S16);

	*pcmp = mix->ext.pcm;
	return 0;
}

SND_PCM_PLUGIN_SYMBOL(upmix);