/*
 * Copyright (c) 2010-2022 Belledonne Communications SARL.
 *
 * This file is part of mediastreamer2
 * (see https://gitlab.linphone.org/BC/public/mediastreamer2).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as
 * published by the Free Software Foundation, either version 3 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include "mediastreamer2/msfilter.h"

#ifdef _MSC_VER
#include <malloc.h>
#endif

#include <speex/speex.h>
#include <speex/speex_resampler.h>

#ifdef __ANDROID__
#include "cpu-features.h"
#endif

typedef struct _ResampleData {
	MSBufferizer *bz;
	uint32_t ts;
	uint32_t input_rate;
	uint32_t output_rate;
	int in_nchannels;
	int out_nchannels;
	SpeexResamplerState *handle;
	int cpuFeatures; /*store because there is no SPEEX_LIB_GET_CPU_FEATURES*/
} ResampleData;

static ResampleData *resample_data_new(void) {
	ResampleData *obj = ms_new0(ResampleData, 1);
	obj->bz = ms_bufferizer_new();
	obj->ts = 0;
	obj->input_rate = 8000;
	obj->output_rate = 16000;
	obj->handle = NULL;
	obj->in_nchannels = obj->out_nchannels = 1;
	obj->cpuFeatures = 0;
	return obj;
}

static void resample_data_destroy(ResampleData *obj) {
	if (obj->handle != NULL) speex_resampler_destroy(obj->handle);
	ms_bufferizer_destroy(obj->bz);
	ms_free(obj);
}

static void resample_init(MSFilter *obj) {
	ResampleData *data = resample_data_new();
#ifdef SPEEX_LIB_SET_CPU_FEATURES
#ifdef __ANDROID__
	if (((android_getCpuFamily() == ANDROID_CPU_FAMILY_ARM) &&
	     ((android_getCpuFeatures() & ANDROID_CPU_ARM_FEATURE_NEON) != 0)) ||
	    (android_getCpuFamily() == ANDROID_CPU_FAMILY_ARM64)) {
		data->cpuFeatures = SPEEX_LIB_CPU_FEATURE_NEON;
	}
#elif MS_HAS_ARM_NEON
	data->cpuFeatures = SPEEX_LIB_CPU_FEATURE_NEON;
#endif
	ms_message("speex_lib_ctl init with neon ? %d", (data->cpuFeatures == SPEEX_LIB_CPU_FEATURE_NEON));
	speex_lib_ctl(SPEEX_LIB_SET_CPU_FEATURES, &data->cpuFeatures);
#else
	ms_message("speex_lib_ctl does not support SPEEX_LIB_CPU_FEATURE_NEON");
#endif
	obj->data = data;
}

static void resample_uninit(MSFilter *obj) {
	resample_data_destroy((ResampleData *)obj->data);
}

static int resample_channel_adapt(int in_nchannels, int out_nchannels, mblk_t *im, mblk_t **om) {
	if(out_nchannels != in_nchannels) {
		size_t msgsize = msgdsize(im) * out_nchannels / in_nchannels ;	// Allocate memory for all out channels.
		*om = allocb(msgsize, 0);
		// Copy first input channel into all output channels. We don't care about other input channels because stereo is not fully supported (because of echo canceller, volume controller, ...)
		for (; im->b_rptr < im->b_wptr; im->b_rptr += sizeof(int16_t)*in_nchannels, (*om)->b_wptr += sizeof(int16_t)*out_nchannels) {
			for(int i = 0 ; i < out_nchannels ; ++i)
				((int16_t *)(*om)->b_wptr)[i] = *(int16_t *)im->b_rptr;
		}
		mblk_meta_copy(im, *om);
		return 1;
	}
	return 0;
}

static void resample_init_speex(ResampleData *dt) {
	int err = 0;
	int quality = SPEEX_RESAMPLER_QUALITY_VOIP; /*default value is voip*/
#if MS_HAS_ARM /*on ARM, NEON optimization are mandatory to support this quality, else using basic mode*/
#if SPEEX_LIB_SET_CPU_FEATURES
	if (dt->cpuFeatures != SPEEX_LIB_CPU_FEATURE_NEON) quality = SPEEX_RESAMPLER_QUALITY_MIN;
#elif !MS_HAS_ARM_NEON
	quality = SPEEX_RESAMPLER_QUALITY_MIN;
#endif /*SPEEX_LIB_SET_CPU_FEATURES*/
#endif /*MS_HAS_ARM*/
	ms_message("Initializing speex resampler in mode [%s] from %d channels",
	           (quality==SPEEX_RESAMPLER_QUALITY_VOIP?"voip":"min"), dt->in_nchannels);
	dt->handle = speex_resampler_init(dt->in_nchannels, dt->input_rate, dt->output_rate, quality, &err);
}

static void resample_preprocess(MSFilter *obj) {
	ResampleData *dt = (ResampleData *)obj->data;
	if (dt->handle == NULL) resample_init_speex(dt);
}

static void resample_process_ms2(MSFilter *obj) {
	ResampleData *dt = (ResampleData *)obj->data;
	mblk_t *im, *om = NULL, *om_chan = NULL;

	if (dt->output_rate == dt->input_rate) {
		while ((im = ms_queue_get(obj->inputs[0])) != NULL) {
			if (resample_channel_adapt(dt->in_nchannels, dt->out_nchannels, im, &om) == 0) {
				ms_queue_put(obj->outputs[0], im);
			} else {
				ms_queue_put(obj->outputs[0], om);
				freemsg(im);
			}
		}
		return;
	}
	ms_filter_lock(obj);
	if (dt->handle != NULL) {
		spx_uint32_t inrate = 0, outrate = 0;
		speex_resampler_get_rate(dt->handle, &inrate, &outrate);
		if ((uint32_t)inrate != dt->input_rate || (uint32_t)outrate != dt->output_rate) {
			speex_resampler_destroy(dt->handle);
			dt->handle = 0;
		}
	}
	if (dt->handle == NULL) {
		resample_init_speex(dt);
	}

	while ((im = ms_queue_get(obj->inputs[0])) != NULL) {
		spx_uint32_t inlen = (spx_uint32_t)((im->b_wptr - im->b_rptr) / (2 * dt->in_nchannels));
		spx_uint32_t outlen = (spx_uint32_t)(((inlen * dt->output_rate) / dt->input_rate) + 1);
		spx_uint32_t inlen_orig = inlen;
		om = allocb(outlen * 2 * dt->in_nchannels, 0);
		mblk_meta_copy(im, om);
		if (dt->in_nchannels == 1) {
			speex_resampler_process_int(dt->handle, 0, (spx_int16_t *)im->b_rptr, &inlen, (spx_int16_t *)om->b_wptr,
			                            &outlen);
		} else {
			speex_resampler_process_interleaved_int(dt->handle, (int16_t *)im->b_rptr, &inlen, (int16_t *)om->b_wptr,
			                                        &outlen);
		}
		if (inlen_orig != inlen) {
			ms_error("Bug in resampler ! only %u samples consumed instead of %u, out=%u", (unsigned int)inlen,
			         (unsigned int)inlen_orig, (unsigned int)outlen);
		}
		om->b_wptr += outlen * 2 * dt->in_nchannels;
		mblk_set_timestamp_info(om, dt->ts);
		dt->ts += outlen;
		if (resample_channel_adapt(dt->in_nchannels, dt->out_nchannels, om, &om_chan) == 0) {
			ms_queue_put(obj->outputs[0], om);
		} else {
			ms_queue_put(obj->outputs[0], om_chan);
			freemsg(om);
		}
		freemsg(im);
	}
	ms_filter_unlock(obj);
}

static int ms_resample_set_sr(MSFilter *f, void *arg) {
	ResampleData *dt = (ResampleData *)f->data;
	unsigned int rate = *(unsigned int *)arg;
	ms_filter_lock(f);
	if (dt->input_rate != rate && dt->handle != NULL) {
		speex_resampler_destroy(dt->handle);
		dt->handle = NULL;
	}
	dt->input_rate = rate;
	ms_filter_unlock(f);
	return 0;
}

static int ms_resample_set_output_sr(MSFilter *f, void *arg) {
	ResampleData *dt = (ResampleData *)f->data;
	unsigned int rate = *(unsigned int *)arg;
	ms_filter_lock(f);
	if (dt->output_rate != rate && dt->handle != NULL) {
		speex_resampler_destroy(dt->handle);
		dt->handle = NULL;
	}
	dt->output_rate = rate;
	ms_filter_unlock(f);
	return 0;
}

static int set_input_nchannels(MSFilter *f, void *arg) {
	ResampleData *dt = (ResampleData *)f->data;
	int chans = *(int *)arg;
	ms_filter_lock(f);
	if (dt->in_nchannels != chans && dt->handle != NULL) {
		speex_resampler_destroy(dt->handle);
		dt->handle = NULL;
	}
	dt->in_nchannels = chans;
	ms_filter_unlock(f);
	return 0;
}

static int set_output_nchannels(MSFilter *f, void *arg) {
	ResampleData *dt = (ResampleData *)f->data;
	int chans = *(int *)arg;
	ms_filter_lock(f);
	dt->out_nchannels = chans;
	ms_filter_unlock(f);
	return 0;
}

static MSFilterMethod methods[] = {{MS_FILTER_SET_SAMPLE_RATE, ms_resample_set_sr},
                                   {MS_FILTER_SET_OUTPUT_SAMPLE_RATE, ms_resample_set_output_sr},
                                   {MS_FILTER_SET_NCHANNELS, set_input_nchannels},
                                   {MS_FILTER_SET_OUTPUT_NCHANNELS, set_output_nchannels},
                                   {0, NULL}};

#ifdef _MSC_VER

MSFilterDesc ms_resample_desc = {MS_RESAMPLE_ID,
                                 "MSResample",
                                 N_("Audio resampler"),
                                 MS_FILTER_OTHER,
                                 NULL,
                                 1,
                                 1,
                                 resample_init,
                                 resample_preprocess,
                                 resample_process_ms2,
                                 NULL,
                                 resample_uninit,
                                 methods};

#else

MSFilterDesc ms_resample_desc = {.id = MS_RESAMPLE_ID,
                                 .name = "MSResample",
                                 .text = N_("Audio resampler"),
                                 .category = MS_FILTER_OTHER,
                                 .ninputs = 1,
                                 .noutputs = 1,
                                 .init = resample_init,
                                 .preprocess = resample_preprocess,
                                 .process = resample_process_ms2,
                                 .uninit = resample_uninit,
                                 .methods = methods};

#endif

MS_FILTER_DESC_EXPORT(ms_resample_desc)
