#include "phd_Converter.h"

#include <QVarLengthArray>

//--- endian convenience ------------------------------------------------------

typedef enum {
	LittleEndian = QSysInfo::LittleEndian,
	BigEndian = QSysInfo::BigEndian,
	ByteOrder = QSysInfo::ByteOrder
} Endian;

#define qFromLE qFromLittleEndian
#define qToLE qToLittleEndian
#define qFromBE qFromBigEndian
#define qToBE qToBigEndian

//--- toInt32 -----------------------------------------------------------------

template <typename T>
inline qint32 toInt32(T sample);

template <>
inline qint32 toInt32<float>(float sample) {
	double tmp = sample * 2147483647.0 + 0.5; // sample * (2^31-1) + 0.5
	
	return (qint32)qBound(-2147483648.0, tmp, 2147483647.0);
}

template <>
inline qint32 toInt32<qint24>(qint24 sample) {
#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
	qint32 tmp = sample.bytes[0] | (sample.bytes[1] << 8) 
	             | (sample.bytes[2] << 16);
#else
	qint32 tmp = sample.bytes[2] | (sample.bytes[1] << 8) 
	             | (sample.bytes[0] << 16);
#endif

	return tmp << 8;
}

template <>
inline qint32 toInt32<quint24>(quint24 sample) {
#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
	qint32 tmp = sample.bytes[0] | (sample.bytes[1] << 8) 
	             | (sample.bytes[2] << 16);
#else
	qint32 tmp = sample.bytes[2] | (sample.bytes[1] << 8) 
	             | (sample.bytes[0] << 16);
#endif

	return (tmp << 8) ^ (1 << 31);
}

template <>
inline qint32 toInt32<qint16>(qint16 sample) {
	return (qint32)sample << 16;
}

template <>
inline qint32 toInt32<quint16>(quint16 sample) {
	return ((qint32)sample << 16) ^ (1 << 31);
}

template <>
inline qint32 toInt32<qint8>(qint8 sample) {
	return (qint32)sample << 24;
}

template <>
inline qint32 toInt32<quint8>(quint8 sample) {
	return ((qint32)sample << 24) ^ (1 << 31);
}

//--- fromInt32 ---------------------------------------------------------------

template <typename T>
inline T fromInt32(qint32 sample);

template <>
inline float fromInt32<float>(qint32 sample) {
	return (float)sample * 4.6566128752457969e-10; // sample * (1 / (2^31-1))
}

template <>
inline qint24 fromInt32<qint24>(qint32 sample) {
	qint32 tmp1 = sample >> 8;
	
#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
	qint24 tmp2 = { tmp1 & 0xff, (tmp1 >> 8) & 0xff, (tmp1 >> 16) & 0xff };
#else
	qint24 tmp2 = { (tmp1 >> 16) & 0xff, (tmp1 >> 8) & 0xff, tmp1 & 0xff };
#endif

	return tmp2;
}

template <>
inline quint24 fromInt32<quint24>(qint32 sample) {
	qint32 tmp1 = (sample ^ (1 << 31)) >> 8;
	
#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
	quint24 tmp2 = { tmp1 & 0xff, (tmp1 >> 8) & 0xff, (tmp1 >> 16) & 0xff };
#else
	quint24 tmp2 = { (tmp1 >> 16) & 0xff, (tmp1 >> 8) & 0xff, tmp1 & 0xff };
#endif

	return tmp2;
}

template <>
inline qint16 fromInt32<qint16>(qint32 sample) {
	return sample >> 16;
}

template <>
inline quint16 fromInt32<quint16>(qint32 sample) {
	return (sample ^ (1 << 31)) >> 16;
}

template <>
inline qint8 fromInt32<qint8>(qint32 sample) {
	return sample >> 24;
}

template <>
inline quint8 fromInt32<quint8>(qint32 sample) {
	return (sample ^ (1 << 31)) >> 24;
}

//--- dither ------------------------------------------------------------------

inline qint32 dither(qint32 input) {
	return input;
}

//--- convert -----------------------------------------------------------------

#define INNER_CONVERT(_arg) fromInt32<T2>(toInt32<T1>(_arg))

template <typename T1, Endian E1, typename T2, Endian E2, bool dithered>
struct PartialConvert {
	static inline void convert(T1* input, T2* output, int size);
};

template <typename T1, typename T2>
struct PartialConvert<T1, LittleEndian, T2, LittleEndian, false> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToLE(INNER_CONVERT(qFromLE(*input++)));
	}
};

template <typename T1, typename T2>
struct PartialConvert<T1, BigEndian, T2, BigEndian, false> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToBE(INNER_CONVERT(qFromBE(*input++)));
	}
};

template <typename T1, typename T2>
struct PartialConvert<T1, LittleEndian, T2, BigEndian, false> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToBE(INNER_CONVERT(qFromLE(*input++)));
	}
};

template <typename T1, typename T2>
struct PartialConvert<T1, BigEndian, T2, LittleEndian, false> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToLE(INNER_CONVERT(qFromBE(*input++)));
	}
};

#undef INNER_CONVERT

#define INNER_CONVERT(_arg) fromInt32<T2>(dither(toInt32<T1>(_arg)))

template <typename T1, typename T2>
struct PartialConvert<T1, LittleEndian, T2, LittleEndian, true> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToLE(INNER_CONVERT(qFromLE(*input++)));
	}
};

template <typename T1, typename T2>
struct PartialConvert<T1, BigEndian, T2, BigEndian, true> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToBE(INNER_CONVERT(qFromBE(*input++)));
	}
};

template <typename T1, typename T2>
struct PartialConvert<T1, LittleEndian, T2, BigEndian, true> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToBE(INNER_CONVERT(qFromLE(*input++)));
	}
};

template <typename T1, typename T2>
struct PartialConvert<T1, BigEndian, T2, LittleEndian, true> {
	static inline void convert(T1* input, T2* output, int size) {
		for (int i = 0; i < size; ++i)
			*output++ = qToLE(INNER_CONVERT(qFromBE(*input++)));
	}
};

#undef INNER_CONVERT

//--- wrapper -----------------------------------------------------------------

template <typename T1, Endian E1, typename T2, Endian E2, bool dithered>
inline void wrapper(void* input, void* output, int size) {
	PartialConvert<T1, E1, T2, E2, dithered>::convert((T1*)input, (T2*)output,
	                                                  size);
}

typedef void (*Wrapper)(void* input, void* output, int size);

#define WRAPPER(_input_type, _input_endian, _output_type, _output_endian) { \
	/* dithered */ \
	wrapper<_input_type, _input_endian, _output_type, _output_endian, true>, \
	/* non-dithered */ \
	wrapper<_input_type, _input_endian, _output_type, _output_endian, false> \
}

#define WRAPPERS(_input_type, _input_endian) { \
	/* F32 */ \
	WRAPPER(_input_type, _input_endian, float, ByteOrder), \
	/* S24LE */ \
	WRAPPER(_input_type, _input_endian, qint24, LittleEndian), \
	/* S24BE */ \
	WRAPPER(_input_type, _input_endian, qint24, BigEndian), \
	/* S16LE */ \
	WRAPPER(_input_type, _input_endian, qint16, LittleEndian), \
	/* S16BE */ \
	WRAPPER(_input_type, _input_endian, qint16, BigEndian), \
	/* U16LE */ \
	WRAPPER(_input_type, _input_endian, quint16, LittleEndian), \
	/* U16BE */ \
	WRAPPER(_input_type, _input_endian, quint16, BigEndian), \
	/* S8 */ \
	WRAPPER(_input_type, _input_endian, qint8, ByteOrder), \
	/* U8 */ \
	WRAPPER(_input_type, _input_endian, quint8, ByteOrder) \
}

// wrappers[input-format][output-format][dithered ? 0 : 1]
static Wrapper wrappers[9][9][2] = {
	// F32
	WRAPPERS(float, ByteOrder),
	// S24LE
	WRAPPERS(qint24, LittleEndian),
	// S24BE
	WRAPPERS(qint24, BigEndian),
	// S16LE
	WRAPPERS(qint16, LittleEndian),
	// S16BE
	WRAPPERS(qint16, BigEndian),
	// U16LE
	WRAPPERS(quint16, LittleEndian),
	// U16BE
	WRAPPERS(quint16, BigEndian),
	// S8
	WRAPPERS(qint8, ByteOrder),
	// U8
	WRAPPERS(quint8, ByteOrder)
};

//--- partial reader ----------------------------------------------------------

template <typename T1, typename T2, bool interleaved, bool inplace>
struct PartialReader {
	static inline int reader(Wrapper wrapper, ISource* source, void* buffer, 
	                  int size, int channels);
};

template <typename T1, typename T2>
struct PartialReader<T1, T2, true, true> {
	static inline int reader(Wrapper wrapper, ISource* source, void* buffer,
	                        int size, int channels) {
		int offset = size - (size * sizeof(T1)) / sizeof(T2);
		T2* output = (T2*)buffer;
		T1* input = (T1*)(output + offset * channels);
		
		int result = source->read(input, size);
		
		if (result <= 0)
			return result;
		
		wrapper(input, output, size * channels);
		
		return result;
	}
};

template <typename T1, typename T2>
struct PartialReader<T1, T2, true, false> {
	static inline int reader(Wrapper wrapper, ISource* source, void* buffer,
	                         int size, int channels) {
		QVarLengthArray<T1, 2048> input;
		T2* output = (T2*)buffer;
		
		input.resize(size * channels);
		
		int result = source->read(input.data(), size);
		
		if (result <= 0)
			return result;
		
		wrapper(input.data(), output, size * channels);
		
		return result;
	}
};

template <typename T1, typename T2>
struct PartialReader<T1, T2, false, true> {
	static inline int reader(Wrapper wrapper, ISource* source, void* buffer, 
	                         int size, int channels) {
		T1** input = new T1*[channels];
		T2** output = (T2**)buffer;
		int offset = size - (size * sizeof(T1)) / sizeof(T2);
		
		for (int i = 0; i < channels; ++i)
			input[i] = (T1*)(output[i] + offset);
		
		int result = source->read(input, size);
		
		if (result <= 0)
			return result;
		
		for (int i = 0; i < channels; ++i)
			wrapper(input[i], output[i], size);
		
		delete[] input;
		
		return result;
	}
};

template <typename T1, typename T2>
struct PartialReader<T1, T2, false, false> {
	static inline int reader(Wrapper wrapper, ISource* source, void* buffer,
	                         int size, int channels) {
		typedef QVarLengthArray<T1, 1024> TypedArray;
		
		TypedArray* base = new TypedArray[channels];
		T1** input = new T1*[channels];
		T2** output = (T2**)buffer;
		
		for (int i = 0; i < channels; ++i) {
			base[i].resize(size);
			input[i] = base[i].data();
		}
		
		int result = source->read(input, size);
		
		if (result <= 0)
			return result;
		
		for (int i = 0; i < channels; ++i)
			wrapper(input[i], output[i], size);
		
		delete[] input;
		delete[] base;
		
		return result;
	}
};

//--- reader ------------------------------------------------------------------

typedef int (*Reader)(Wrapper wrapper, ISource* source, void* buffer, int size,
                      int channels);

#define READER(_input_type, _output_type) { \
	/* interleaved */ \
	PartialReader<_input_type, _output_type, true, \
	              sizeof(_input_type) <= sizeof(_output_type)>::reader, \
	/* deinterleaved */ \
	PartialReader<_input_type, _output_type, false, \
	              sizeof(_input_type) <= sizeof(_output_type)>::reader \
}

#define READERS(_input_type) { \
	/* F32 */ \
	READER(_input_type, float), \
	/* S24LE */ \
	READER(_input_type, qint24), \
	/* S24BE */ \
	READER(_input_type, qint24), \
	/* S16LE */ \
	READER(_input_type, qint16), \
	/* S16BE */ \
	READER(_input_type, qint16), \
	/* U16LE */ \
	READER(_input_type, quint16), \
	/* U16BE */ \
	READER(_input_type, quint16), \
	/* S8 */ \
	READER(_input_type, qint8), \
	/* U8 */ \
	READER(_input_type, quint8), \
}

// readers[input-format][output-format][interleaved ? 0 : 1]
static Reader readers[9][9][2] = { 
	// F32
	READERS(float),
	// S24LE
	READERS(qint24),
	// S24BE
	READERS(qint24),
	// S16LE
	READERS(qint16),
	// S16BE
	READERS(qint16),
	// U16LE
	READERS(quint16),
	// U16BE
	READERS(quint16),
	// S8
	READERS(qint8),
	// U8
	READERS(quint8)
};

//--- Converter::Private ------------------------------------------------------

class Converter::Private {
public:
	Private(Sample::Format inputFormat)
		: mSource(NULL),
		  mSeekable(NULL),
		  mInputFormat(inputFormat),
		  mOutputFormat(Sample::F32),
		  mReader(NULL),
		  mWrapper(NULL) {
		mInputPort.mime = "audio/pcm";
		mInputPort.params["format"] = mInputFormat;
		
		mOutputPort.mime = "audio/pcm";
	}
	
	Port mInputPort;
	Port mOutputPort;
	ISource* mSource;
	ISeekable* mSeekable;
	Sample::Format mInputFormat;
	Sample::Format mOutputFormat;
	int mChannels;
	bool mInterleaved;
	Reader mReader;
	Wrapper mWrapper;
};

//--- Converter ---------------------------------------------------------------
		     
Converter::Converter(Sample::Format inputFormat, QObject* parent /*= NULL*/)
	: SeekableTransport(parent) {
	mp = new Private(inputFormat);
}

Converter::~Converter() {
	delete mp;
}

// reimplemented from Element
bool Converter::initialize() {	
	return true;
}

// reimplemented from Element
QString Converter::name() const {
	if (mp->mSource)
		return QString("Converter [%1->%2,%3,%4]")
		       .arg(Sample::name(mp->mInputFormat))
		       .arg(Sample::name(mp->mOutputFormat))
		       .arg(mp->mChannels)
		       .arg(mp->mInterleaved ? "i" : "d");
		
	return QString("Converter [%1]").arg(Sample::name(mp->mInputFormat));
}

// reimplemented from ISeekable
bool Converter::seek(qint64 position) {
	if (mp->mSeekable)
		return mp->mSeekable->seek(position);
	
	return false;
}

// reimplemented from ISeekable
qint64 Converter::position() const {
	if (mp->mSeekable)
		return mp->mSeekable->position();
	
	return 0;
}

// reimplemented from ISeekable
qint64 Converter::size() const {
	if (mp->mSeekable)
		return mp->mSeekable->size();
	
	return 0;
}

// reimplemented from ISink
Port Converter::input() const {
	return mp->mInputPort;
}

// reimplemented from ISink
bool Converter::reconnect() {
	if (mp->mSource) {
		mp->mSource = NULL;
		mp->mSeekable = NULL;
	}
	
	if (!dequeue())
		return false;
	
	mp->mSource = current(mp->mSeekable);
	mp->mChannels = mp->mSource->output().params["channels"].toInt();
	mp->mInterleaved = mp->mSource->output().params["interleaved"].toBool();
	
	Port::fill(mp->mSource->output(), mp->mOutputPort);
	
	mp->mReader = readers[mp->mInputFormat][mp->mOutputFormat]
	                     [mp->mInterleaved ? 0 : 1];

	mp->mWrapper = wrappers[mp->mInputFormat][mp->mOutputFormat][1];
	
	return true;
}

// reimplemented from ISource
bool Converter::setOutputHint(const Port& hint) {
	if (!hint.params.contains("format")) {
		qWarning("Converter setOutputHint: Hint doesn't specify a format, "
		         "so why use a Converter.");
		return false;
	}
	
	mp->mOutputFormat = (Sample::Format)hint.params["format"].toInt();
	mp->mOutputPort.params["format"] = mp->mOutputFormat;
	
	if (mp->mInputFormat == mp->mOutputFormat) {
		qWarning("Converter setOutputHint: Input- and output-format are "
		         "equal, so why use a Converter.");
		return false;
	}
	
	return true;
}

// reimplemented from ISource
Port Converter::output() const {
	return mp->mOutputPort;
}

// reimplemented from ISource
void Converter::resolved(const QByteArray& mime) {
}

// reimplemented from ISource
int Converter::read(void* buffer, int size) {
	if (!mp->mSource)
		return -1;
		
	return mp->mReader(mp->mWrapper, mp->mSource, buffer, size, mp->mChannels);
}

// reimplemented from ISource
int Converter::peek(void* buffer, int size) {
	return -1;
}