/*************************************************
*           BK Emulator v3.8 for Windows         *
*************************************************/

/*
File: Speaker.cpp
Version: 1.0.0
Written by: Yuriy Kalmykov <kalmykov@stoik.com>
    Copyright (c) 2002-2004 Yuriy Kalmykov

    BK Emulator is a program emulated hardware environment for running
code for BK 0010(01) in different configurations.

    This code may be used in compiled form in any way you desire.
This file or it's parts can't be redistributed without the authors
written consent, but can be modified for your private needs.
    Providing that this notice and the authors name and all copyright
notices remains intact.

    Please, an email me to know how you are using it and where. You can
ask me for any information about this below code or any attendant
knowledge.

    This file is provided "as is" with no expressed or implied warranty.
The author accepts no liability for any damage or loss of business that
this product may cause.
*/


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


#include "stdafx.h"
#include "Board.h"
#include "Speaker.h"

#define _USE_MATH_DEFINES
#include <math.h>
//////////////////////////////////////////////////////////////////////
// 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];

	//!!! как-то самонадеянно надеяться, что тут никогда не будет ошибок
	//хоть и маленькие буферы, но всегда найдётся система, где не будет и этой памяти
	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;
	}

	m_pH = new double[FIR_LENGTH];
	//CalcFIR(m_pH, FIR_LENGTH, 5400.0, 0.0, LOWPASS);
	CalcFIR(m_pH, FIR_LENGTH, 8000.0, 0.0, LOWPASS);
}

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


void CSpeaker::OnReset()
{
}


void CSpeaker::SetSampleBuffer(int ips)
{
	SAFE_DELETE_ARRAY(m_pRecieveSamples);
	m_ips = ips;
	m_pRecieveSamples = new SAMPLE_TYPE[m_ips];
	ZeroMemory(m_pRecieveSamples, m_ips * sizeof(SAMPLE_TYPE));
}


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

	for (register int n = 0, i = 0; n < m_ips; ++n, i += 2) //m_ips == inLen должно быть!
	{
		//i всегда должно быть чётное
		m_pRecieveSamples[n] = t = ((L = inBuf[i]) > (R = inBuf[i + 1])) ? L : R; //max(L,R)
		dt += (double)t;
	}

	m_nAverage = (SAMPLE_TYPE)(dt / 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 UINT 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)
{
	register bool outVal = (m_pRecieveSamples[m_tickCount] >= m_nAverage);
	NextTick();
	//register double fL = m_dLeftAcc;
	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 - тип фильра
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))
	{
		double *pHf = new double[nN];
		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;
	}
}


//вход:
//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;
	}
}


