// Speaker.cpp: implementation of the CSpeaker class.
//


#include "stdafx.h"
#include "Config.h"
#include "Speaker.h"
#include "BKErrMsgSys.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <utility> //#include <algorithm>
//////////////////////////////////////////////////////////////////////
// Construction/Destruction

CSpeaker::CSpeaker()
	: m_bEnableSound(true)
	, m_bFilter(true)
	, m_pRecieveSamples(nullptr)
	, m_nAverage(MIN_SAMPLE)
	, m_nBufferPosL(0)
	, m_nBufferPosR(0)
	, m_dAvgL(0.0)
	, m_dAvgR(0.0)
	, m_nLeftBufPos(0)
	, m_nRightBufPos(0)
	, m_dLeftAcc(0.0)
	, m_dRightAcc(0.0)
	, m_bRCProc(false)
	, m_ft(0.0)
	, m_fmaxvol(0.0)
	, m_fminvol(0.0)
	, m_fdeltavol(0.0)
	, m_fUi_prev(0.0)
{
	m_pdBufferL = new double[AUTOGAIN_BUFFER_LEN];
	m_pdBufferR = new double[AUTOGAIN_BUFFER_LEN];

	if (m_pdBufferL && m_pdBufferR)
	{
		for (int i = 0; i < AUTOGAIN_BUFFER_LEN; ++i)
		{
			m_pdBufferL[i] = m_pdBufferR[i] = 0.0;
		}

		for (int i = 0; i < FIR_LENGTH; ++i)
		{
			m_LeftBuf[i] = m_RightBuf[i] = 0.0;
		}
	}
	else
	{
		CBKMessageBox(g_pstrErrors[BK_ERROR_NOTENMEMR], MB_OK);
	}

	m_pH = new double[FIR_LENGTH];
	if (m_pH)
	{
		ReInit();
	}
	else
	{
		CBKMessageBox(g_pstrErrors[BK_ERROR_NOTENMEMR], MB_OK);
	}

}

CSpeaker::~CSpeaker()
{
	SAFE_DELETE_ARRAY(m_pRecieveSamples);
	SAFE_DELETE_ARRAY(m_pdBufferL);
	SAFE_DELETE_ARRAY(m_pdBufferR);
	SAFE_DELETE_ARRAY(m_pH);
}

void CSpeaker::ReInit()
{
	//CalcFIR(m_pH, FIR_LENGTH, 5400.0, 0.0, LOWPASS); //как-то не очень
	CalcFIR(m_pH, FIR_LENGTH, 8000.0, 0.0, LOWPASS);
}


void CSpeaker::OnReset()
{
}


void CSpeaker::SetSampleBuffer(int ips)
{
	SAFE_DELETE_ARRAY(m_pRecieveSamples);
	m_ips = ips;
	m_pRecieveSamples = new SAMPLE_TYPE[m_ips];
	if (!m_pRecieveSamples)
	{
		CBKMessageBox(g_pstrErrors[BK_ERROR_NOTENMEMR], MB_OK);
	}
	ZeroMemory(m_pRecieveSamples, m_ips * sizeof(SAMPLE_TYPE));
}


/*
Вход: pBuff - указатель на общий буфер, данные там типа SAMPLE_TYPE
      nLength - длина буфера pBuff в байтах
*/
void CSpeaker::ReceiveSamples(void *pBuff, int nLength)
{
	register auto inBuf = reinterpret_cast<SAMPLE_TYPE *>(pBuff);
	register int inLen = nLength / SAMPLE_LENGTH; //размер в сэмплах
	register double avg = 0.0; //инт не хватает, случается переполнение
	register SAMPLE_TYPE t;
	// Заполняем из входного буфера внутренний. из стерео делаем моно.
	ASSERT(m_ips == inLen); //m_ips == inLen должно быть!

	for (register int n = 0; n < m_ips; ++n)
	{
		m_pRecieveSamples[n] = t = ((*inBuf++) + (*inBuf++)) / BUFFER_CHANNELS; //берём среднее арифметическое
		avg += double(t);
	}

	m_nAverage = SAMPLE_TYPE(avg / m_ips);
}

/*
 Спикер использует только верхний полупериод сэмпла
 Посмотрев амплитуды звуков БК и эмулятора, у меня закрадывается подозрение,
 что нужно использовать преобразование ШИМ->АМ или как-то иначе эмулировать
 поведение конденсатора.
 Значения амплитуд неточные, измерены на глаз.
*/
const double CSpeaker::m_dSpeakerValues[8] =
{
	double(MIN_SAMPLE) / FLOAT_BASE,        //0 0   0%
	double(MAX_SAMPLE) * 0.15 / FLOAT_BASE, //1 4   15%
	double(MAX_SAMPLE) * 0.30 / FLOAT_BASE, //2 40  30%
	double(MAX_SAMPLE) * 0.40 / FLOAT_BASE, //3 44  40%
	double(MAX_SAMPLE) * 0.70 / FLOAT_BASE, //4 100 70%
	double(MAX_SAMPLE) * 0.80 / FLOAT_BASE, //5 104 80%
	double(MAX_SAMPLE) * 0.94 / FLOAT_BASE, //6 140 94%
	double(MAX_SAMPLE) / FLOAT_BASE         //7 144 100%
};

void CSpeaker::SetSample(WORD inVal)
{
	if (m_bEnableSound)
	{
		register int w = inVal & 0144; //получим маску звуковых битов

		if (w & 04) //2й бит переместим в 4й
		{
			w |= 020;
		}

		w >>= 4; //сдвинем на 4 бита вправо. в результате получится число 0..7
		m_dLeftAcc = m_dSpeakerValues[w];
	}
	else
	{
		m_dLeftAcc = 0.0;
	}
}

// почти работает.
// в реальности там получается разное сопротивление для разных уровней,
// но и так работает почти как в оригинале.
void CSpeaker::RCSHIM(double fTime)
{
	if (m_dLeftAcc > m_fUi_prev)
	{
		m_fUi_prev = m_dLeftAcc;
		m_fminvol = RCSHIMCalc(); //от этого уровня начинаем
		m_fmaxvol = m_dLeftAcc; //до этого уровня постараемся дойти
		m_bRCProc = true; //надо заряжать
		m_ft = fTime;

		if (m_fmaxvol < m_fminvol) //так не бывает, но всё же, если конденсатор был заряжен сильнее, чем сейчас уровень
		{
			std::swap(m_fmaxvol, m_fminvol); //то надо разряжать
			m_bRCProc = false;
		}

		m_fdeltavol = m_fmaxvol - m_fminvol; //дельта - величина, насколько подскочило напряжение относительно заряда конденсатора
	}
	else if (m_dLeftAcc < m_fUi_prev)
	{
		m_fUi_prev = m_dLeftAcc;
		m_fmaxvol = RCSHIMCalc(); //от этого уровня начинаем
		m_fminvol = m_dLeftAcc; //к этому уровню постараемся дойти
		m_bRCProc = false; //надо разряжать
		m_ft = fTime;

		if (m_fmaxvol < m_fminvol) //если конденсатор был заряжен меньше, чем сейчас уровень
		{
			std::swap(m_fmaxvol, m_fminvol); //то надо заряжать
			m_bRCProc = true;
		}

		m_fdeltavol = m_fmaxvol - m_fminvol;  //дельта - величина, насколько изменилось напряжение относительно заряда конденсатора
	}
	else
	{
		m_ft += fTime; //если напряжение держится одного уровня - просто продолжаем процесс
	}
}

double CSpeaker::RCSHIMCalc()
{
	if (m_bRCProc)
	{
		//зарядка
		//return m_fminvol + m_fdeltavol * (1 - exp(-(m_ft / (6.8e-9 * 8200))));
		//ниже - эта же функция с раскрытыми скобками, на одно действие меньше.
		return m_fmaxvol - m_fdeltavol * exp(-(m_ft / (6.8e-9 * 8200)));
	}
	else
	{
		//разрядка
		return m_fminvol + m_fdeltavol * exp(-(m_ft / (6.8e-9 * 8200)));
	}
}


bool CSpeaker::GetSample(double &sampleL, double &sampleR)
{
	bool outVal = (m_pRecieveSamples[m_tickCount] >= m_nAverage);
	NextTick();
	register double fL = RCSHIMCalc();
	fL = AutoGain(fL, m_dAvgL, m_pdBufferL, m_nBufferPosL); //на выходе m_nBufferPosL указывает на следующую позицию.
	//фильтр
	fL = FIRFilter(fL, m_LeftBuf, m_nLeftBufPos);
	sampleL = sampleR = fL;
	return outVal;
}


//автоматическое выравнивание уровня, очень полезно для ковокса, для AY - так себе,
double CSpeaker::AutoGain(register double sample, register double &fAvg, register double *pBuf, register int &nBufPos)
{
	/*
	идея такая:
	avg0 = (buf[0] + buf[1] + ... + buf[buf_len-1]) / buf_len

	avg1 = avg0 - buf[buf_len-1] / buf_len + sample / buf_len

	не нужно сдвигать содержимое буфера, выкидывая последнее значение и добавляя в начало текущее.
	и не нужно каждый раз суммировать всё содержимое буфера, для расчёта среднего значения.
	имеем указатель на позицию в буфере, прочитанное значение из буфера - это выкидываемый последний сэмпл
	а занесение нового значения - запись текущего сэмпла в буфер на заданную позицию.
	*/
	fAvg += (sample - pBuf[nBufPos]) / double(AUTOGAIN_BUFFER_LEN);
	pBuf[nBufPos++] = sample;
	nBufPos &= AUTOGAIN_BUFFER_MASK;
	//устраняем клиппинг, может появляться из-за резкой смены знака амплитуды
	register double t = sample - fAvg;

	if (t < 0.0)
	{
		t = -t;
	}

	if (t > double(MAX_SAMPLE) / FLOAT_BASE)
	{
		return sample + fAvg;
	}
	else
	{
		return sample - fAvg;
	}
}

//на входе - формируемый массив и его размер
// dFs1 - частота среза фильтров НЧ или ВЧ
// dFs2 - верхняя частота среза фильтров ПЗ или ПП
// nFilterType - тип фильра
// nN - минимум 3. защиты от дурака нет. слишком малое значение вызовет выход за границы массива
void CSpeaker::CalcFIR(double *pH, int nN, double dFs1, double dFs2, FILTER_TYPE nFilterType)
{
	FILTER_WINDOW_TYPE nWindowType = BLACKMAN_HARRIS;
	//Расчет импульсной характеристики фильтра
	double dFc1 = dFs1 / g_Config.m_nSoundSampleRate;
	double dWc1 = 2.0 * M_PI * dFc1;
	int n = nN / 2;

	//ФНЧ
	for (int i = 0; i <= n; ++i)
	{
		if (i == 0)
		{
			pH[n] = dWc1;
		}
		else
		{
			pH[n + i] = pH[n - i] = sinl(dWc1 * i) / (i);
		}
	}

	//весовые коэффициенты, и заодно сумму подсчитаем
	double dSUM = 0.0;

	for (int i = 0; i < nN; ++i)
	{
		pH[i] *= getWindow(i, nN, nWindowType);
		dSUM += pH[i];
	}

	//Нормализация импульсной характеристики
	for (int i = 0; i < nN; ++i)
	{
		pH[i] /= dSUM; //сумма коэффициентов равна 1
		//TRACE(_T("%.10f\n"), pH[i]);
	}

	if (nFilterType == HIGHPASS)
	{
		for (int i = 0; i < nN; ++i)
		{
			pH[i] = -pH[i];
		}

		pH[n] += 1.0;
	}
	else if ((nFilterType == BANDPASS) || (nFilterType == BANDSTOP))
	{
		auto pHf = new double[nN];
		if (pHf)
		{

			double dFc2 = dFs2 / g_Config.m_nSoundSampleRate;
			double dWc2 = 2.0 * M_PI * dFc2;

			for (int i = 0; i <= n; ++i)
			{
				if (i == 0)
				{
					pHf[n] = dWc2;
				}
				else
				{
					pHf[n + i] = pHf[n - i] = sinl(dWc2 * i) / (i);
				}
			}

			//весовые коэффициенты, и заодно сумму подсчитаем
			double dSUM = 0.0;

			for (int i = 0; i < nN; ++i)
			{
				pHf[i] *= getWindow(i, nN, nWindowType);
				dSUM += pHf[i];
			}

			//Нормализация импульсной характеристики
			for (int i = 0; i < nN; ++i)
			{
				pHf[i] /= dSUM; //сумма коэффициентов равна 1
			}

			//инвертируем и объединяем с ФНЧ
			for (int i = 0; i < nN; ++i)
			{
				pH[i] -= pHf[i];
			}

			pH[n] += 1.0;

			if (nFilterType == BANDPASS)
			{
				for (int i = 0; i < nN; ++i)
				{
					pH[i] = -pH[i];
				}

				pH[n] += 1.0;
			}

			delete[] pHf;
		}
		else
		{
			CBKMessageBox(g_pstrErrors[BK_ERROR_NOTENMEMR], MB_OK);
		}
	}
}


//вход:
//i - текущая позиция
//n - порядок фильтра
double CSpeaker::getWindow(int i, int n, FILTER_WINDOW_TYPE window)
{
	if (window == BARTLETT)
	{
		// устраняем нулевые значения
		double a = i - (n - 1) / 2.0;

		if (a < 0)
		{
			a = -a;
		}

		return 2.0 / n * (n / 2.0 - a);
	}
	else if (window == HANNING)
	{
		// устраняем нулевые значения
		return 0.5 - 0.5 * cosl(M_PI / n * (1.0 + 2.0 * i));
	}

	if (window == BLACKMAN)
	{
		// устраняем нулевые значения
		double a = M_PI / n * (1.0 + 2.0 * i);
		return 0.5 * (1.0 - 0.16 - cosl(a) + 0.16 * cosl(2.0 * a));
	}
	else
	{
		double a = 2.0 * M_PI * i / (n - 1);

		if (window == HAMMING)
		{
			return 0.54 - 0.46 * cosl(a);
		}
		else if (window == BLACKMAN_HARRIS)
		{
			return 0.35875 - 0.48829 * cosl(a) + 0.14128 * cosl(2.0 * a) - 0.01168 * cosl(3.0 * a);
		}
		else if (window == BLACKMAN_NUTTAL)
		{
			return 0.35819 - 0.4891775 * cosl(a) + 0.1365995 * cosl(2.0 * a) - 0.0106411 * cosl(3.0 * a);
		}
		else if (window == NUTTAL)
		{
			return 0.355768 - 0.487396 * cosl(a) + 0.144232 * cosl(2.0 * a) - 0.012604 * cosl(3.0 * a);
		}
	}

	return 1.0;
}
/*На входе:
sample - новый, поступающий сэмпл.
pBuf - кольцевой буфер сэмплов, длиной FIR_LENGTH
nBufPos - позиция в буфере, pBuf, куда помещать сэмпл.
*/
double CSpeaker::FIRFilter(register double sample, register double *pBuf, register int &nBufPos)
{
	//поместим новый сэмпл в буфер.
	pBuf[nBufPos] = sample;

	if (++nBufPos >= FIR_LENGTH)
	{
		nBufPos = 0;
	}

	if (m_bFilter)
	{
		//теперь nBufPos указывает на начало, откуда брать сэмплы для фильтрации
		register double dAcc = 0.0;
		register int j = nBufPos;

		for (register int i = 0; i < FIR_LENGTH; ++i)
		{
			dAcc += m_pH[i] * pBuf[j];

			if (++j >= FIR_LENGTH)
			{
				j = 0;
			}
		}

		return dAcc;
	}
	else
	{
		return sample;
	}
}


