#include "Shifter.h"
#include <math.h>
#include <juce_core/juce_core.h>
// #define SAMPLE_BY_SAMPLE
#ifndef PI_F
#define PI_F 3.1415927410125732421875f
#endif


void Shifter::SetAutoTuneSpeed(float val) {
	out_midi_smoother.SetTimeConstant(val);
}

void Shifter::SetAutoTuneDepth(float val) {
	out_midi_smoother.SetDepth(val);
}

static inline float mtof(float m)
{
	return powf(2, (m - 69.0f) / 12.0f) * 440.0f;
}

static inline bool float_equal(float one, float two) {
	return abs(one - two) < 1e-5f;
}

void Shifter::Init(float samplerate, int samplesPerBlock)
{
	sample_rate_ = samplerate;
	blocksize = samplesPerBlock;
	out_midi_smoother.SetFrameTime((float)256 / samplerate);
	volume = 1;
	helm.setframesize(1024);
	helm.setoverlap(2);
	for (int i = 0; i < MAX_VOICES; ++i)
	{
		voices[i].Init(samplerate);
		freeze_voices[i].Init(samplerate);
	}
	for (int i = 0; i < BUFFER_SIZE; ++i)
	{
		in_buffer[i] = 0;
		out_buffer[0][i] = 0;
		out_buffer[1][i] = 0;
	}
	for (int i = 0; i < 8192; ++i) {
		cos_lookup[i] = cos(2 * PI_F * i / 8192.0);
	}
	in_period = 0;
}

void Shifter::Process(const float* const* in,
	float** out,
	size_t size)
{
	for (size_t i = 0; i < size; i += 256) {
		size_t offset = i;
		size_t lefover_size = std::min((size_t)256, size - i);
		DetectPitch(in, out, lefover_size, offset);
		SetRates();
		GetSamples(out, in[0], lefover_size, offset);
	}
	
	//for (size_t i = 0; i < size; ++i)
	//{
	//    // out[0][i] = osc.Process();
	//    // out[0][i] = in[0][i];
	//    //out[0][i] = out[0][i] + in[0][i];
	//    out[1][i] = out[0][i];
	//}
	// }
}

float findMedian(float a, float b, float c) {
	if ((a >= b && a <= c) || (a <= b && a >= c))
		return a;
	else if ((b >= a && b <= c) || (b <= a && b >= c))
		return b;
	else
		return c;
}


void Shifter::DetectPitch(const float* const* in, float** out, size_t size, size_t offset)
{
	// detect current pitch
	// pitch_detect.update(in[0], size);
	// if(pitch_detect.available())
	// {
	//     float read = pitch_detect.read();
	//     if(read >= 35 && read <= 2000)
	//     {
	//         for(int i = 2; i > 0; --i){
	//             last_freqs[i] = last_freqs[i-1];
	//         }
	//         last_freqs[0] = read;
	//     }
	// }

	// current_pitch = findMedian(last_freqs[0], last_freqs[1], last_freqs[2]);
	// in_period = 1.0 / current_pitch * 48000;

	helm.iosamples(in[0]+offset, out[0]+offset, size);
	float period = helm.getperiod();
	float fidel = helm.getfidelity();
	//DBG("frequency: " << 48000 / period << " fidel: " << fidel);

	// Adjustable filter amount (0.0f = no filtering, 1.0f = max filtering)
	const float in_period_filter_amount = 0.5f; // You can expose this as a parameter

	if (fidel > 0.95) {
		// Smoothly filter in_`period changes
		in_period = in_period * in_period_filter_amount + period * (1.0f - in_period_filter_amount);
	}
	float in_freq = sample_rate_ / in_period;

	float midi = (12 * log2f(in_freq / 440) + 69.0f);

	//target_out_period = in_period * out_period_filter_amount + target_out_period * (1 - out_period_filter_amount);
	out_midi = out_midi_smoother.update(midi, (int)(midi + .5));
	out_period = sample_rate_ / mtof(out_midi);
}

void Shifter::SetRates() {}

float Shifter::GetOutputEnvelopePeriod(int out_voice) {
	if (out_voice >= MAX_VOICES) {
		return in_period * formant_preserve + out_period * (1.0 - formant_preserve);
	}
	//TODO add something so that low pitch ratios end up reducing formant_preservation
	return in_period * formant_preserve + voices[out_voice].CurrentPeriod() * (1.0 - formant_preserve);
}

float Shifter::GetOutputEnvelopePeriodFreeze(int freeze_voice) {
	//TODO add something so that low pitch ratios end up reducing formant_preservation
	return freeze_period * formant_preserve + freeze_voices[freeze_voice].CurrentPeriod() * (1.0 - formant_preserve);
}

int Shifter::GetPeakIndex() {
	static float current_index = 0;
	float smooth = 1 / 200.0f;
	int diff = in_playhead - current_index;
	if (diff < 0)
		diff += BUFFER_SIZE;
	if (diff < in_period * 2) {
		diff = last_max_index - current_index;
		if (diff < -(BUFFER_SIZE / 2))
			diff += BUFFER_SIZE;
		if (diff > BUFFER_SIZE / 2)
			diff -= BUFFER_SIZE;

		float adjust = (float)diff * smooth;
		if (adjust > 1 || adjust < -1) {
			int dummy = 0;
		}
		current_index += adjust;
		if ((int)current_index > BUFFER_SIZE) {
			current_index -= BUFFER_SIZE;
		}
		if((int)current_index < 0) {
			current_index += BUFFER_SIZE;
		}
		return (int)current_index;
	}
	else {
		current_index += in_period;
		if (current_index > BUFFER_SIZE) {
			current_index -= BUFFER_SIZE;
		}
	}

	float threshold = .0075f;
	int index = in_playhead - in_period * 2;
	if (index < 0)
		index += BUFFER_SIZE;

	//search for max absolute value
	int   max_index = -1;
	float max_value = -2;
	for (int j = 0; j < in_period; ++j)
	{
		//float val = fabs(in_buffer[index]);
		float val = in_buffer[index];
		int period_difference = index - last_max_index;
		if (period_difference < 0)
			period_difference += BUFFER_SIZE;
		//prefer peaks near last peak or near next peak
		/*if(!((period_difference > in_period * (0-threshold) && period_difference < in_period* (0+threshold)) || (period_difference > in_period * (1-threshold) && period_difference < in_period * (1+threshold)))) {
			val *= 0.75f;
		}*/

		if (val > max_value)
		{
			
			max_index = index;
			max_value = val;
		}
		if (++index >= BUFFER_SIZE)
		{
			index -= BUFFER_SIZE;
		}
	}
	last_max_index = max_index;
	return (int)current_index;
}


void Shifter::AddInterpolatedFrame(int voice, int max_index, float resampling_period) {
	float period_ratio = in_period / resampling_period;
	float f_index;
	f_index = max_index - in_period;
	if (f_index < 0)
	{
		f_index += BUFFER_SIZE;
	}
	float mult = 0;
	int   out_index = out_playhead;
	for (int j = 0; j < resampling_period * 2; ++j)
	{
		// mult = .5
		//        * (1 - cosf(2 * PI_F * j / (period_to_use * 2 - 1)));
		float interp = f_index - (int)f_index;
		mult = .5 * (1 - cos_lookup[(int)((float)j / (resampling_period * 2.0) * 8191.0)]);
		float value = ((1 - interp) * in_buffer[(int)f_index] + (interp)*in_buffer[(int)(f_index + 1) % 8192]) * mult;
		if (voice >= MAX_VOICES) {
			//value *= volume;
			out_buffer[0][out_index] += value * melody_mix;
			out_buffer[1][out_index] += value * melody_mix;
		}
		else {
			value *= voices[voice].CurrentAmplitude() * volume;
			out_buffer[0][out_index] += value * voices[voice].GetPanning(0) * harmony_mix;
			out_buffer[1][out_index] += value * voices[voice].GetPanning(1) * harmony_mix;
		}



		f_index += period_ratio;
		if (f_index >= BUFFER_SIZE)
		{
			f_index -= BUFFER_SIZE;
		}
		if (++out_index >= BUFFER_SIZE)
		{
			out_index -= BUFFER_SIZE;
		}
	}
}

void Shifter::GetSamples(float** output, const float* input, size_t size, size_t offset)
{
	if (freeze_needs_copy) {
		freeze_needs_copy = false;
		//copy current buffer to freeze buffer
		int index = GetPeakIndex();
		memset(freeze_buffer, 0, sizeof(freeze_buffer));
		CopyInputToFreezeBuffer(index);
		//init freeze voices
		for (int i = 0; i < MAX_VOICES; ++i) {
			//freeze_voices[i].Init(sample_rate_);
			if (voices[i].IsActive()) {
				freeze_voices[i].Trigger(voices[i].GetMidiNote());
				freeze_voices[i].panning = voices[i].panning;
				freeze_voices[i].SetPortamentoTime(0.05f);
			}
		}
	}
	for (int i = 0; i < size; ++i)
	{

		//add new samples if necessary
		for (int out_p = 0; out_p < MAX_VOICES; ++out_p)
		{
			voices[out_p].Process();
			if (!voices[out_p].IsActive()) continue;
			if (voices[out_p].PeriodOverflow())
			{
				float resampling_period = GetOutputEnvelopePeriod(out_p);


				//find the start index
				int max_index = GetPeakIndex();

				//add samples centered on that max
				AddInterpolatedFrame(out_p, max_index, resampling_period);
			}
		}

		//add freeze samples if necessary
		for (int out_p = 0; out_p < MAX_VOICES; ++out_p)
		{
			freeze_voices[out_p].Process();
			if (!freeze_voices[out_p].IsActive()) continue;
			if (freeze_voices[out_p].PeriodOverflow())
			{
				float resampling_period = GetOutputEnvelopePeriodFreeze(out_p);

				//add samples centered on that max
				AddFreezeToOutput(out_p, resampling_period);
			}
		}

		if (out_period_counter > out_period)
		{
			out_period_counter -= out_period;
			if (enable_autotune) {
				float resampling_period = GetOutputEnvelopePeriod(MAX_VOICES);



				//find the start index
				int max_index = GetPeakIndex();

				//add samples centered on that max
				AddInterpolatedFrame(MAX_VOICES, max_index, resampling_period);
			}
		}



		//add input samples
		in_buffer[in_playhead] = input[i+offset];

		//output samples, set to 0
		for (int ch = 0; ch < 2; ++ch) {
			output[ch][i+offset] = out_buffer[ch][out_playhead];
			if (!enable_autotune) {
				output[ch][i+offset] += input[i+offset] * melody_mix;
			}
			out_buffer[ch][out_playhead] = 0;
		}


		//increment playheads
		if (++in_playhead >= BUFFER_SIZE)
		{
			in_playhead -= BUFFER_SIZE;
		}
		if (++out_playhead >= BUFFER_SIZE)
		{
			out_playhead -= BUFFER_SIZE;
		}
		out_period_counter++;
	}
}


void Shifter::AddMidiNote(int note) {
	for (int i = 0; i < MAX_VOICES; ++i) {
		if (voices[i].onoff_ && voices[i].GetMidiNote() == note) {
			voices[i].Release();
		}
	}
	for (int i = 0; i < MAX_VOICES; ++i) {
		if (!voices[i].onoff_) {
			voices[i].Trigger(note);
			return;
		}
	}
}


void Shifter::RemoveMidiNote(int note) {
	for (int i = 0; i < MAX_VOICES; ++i) {
		if (voices[i].GetMidiNote() == note) {
			voices[i].Release();
		}
	}
}

void Shifter::SetHarmonyMix(float mix) {
	if (mix < .5) {
		melody_mix = 1.0f;
		harmony_mix = mix * 2.0f;
	}
	else {
		harmony_mix = 1.0f;
		melody_mix = (1.0f - mix) * 2.0f;
	}
}

void Shifter::CopyInputToFreezeBuffer(int max_index) {
	freeze_period = in_period;
	float period_ratio = 1;
	float f_index;
	f_index = max_index - in_period;
	if (f_index < 0)
	{
		f_index += BUFFER_SIZE;
	}
	float mult = 0;
	for (int j = 0; j < in_period * 2; ++j)
	{
		mult = .5 * (1 - cos_lookup[(int)((float)j / (in_period * 2.0) * 8191.0)]);
		float value = in_buffer[(int)f_index] * mult;
		freeze_buffer[j] = value;
		f_index += 1;
		if (f_index >= BUFFER_SIZE)
		{
			f_index -= BUFFER_SIZE;
		}
	}
}

void Shifter::AddFreezeToOutput(int voice, float resampling_period) {
	float period_ratio = freeze_period / resampling_period;
	float f_index;
	f_index = 0;
	if (f_index < 0)
	{
		f_index += BUFFER_SIZE;
	}
	float mult = 0;
	int   out_index = out_playhead;
	for (int j = 0; j < resampling_period * 2; ++j)
	{
		// mult = .5
		//        * (1 - cosf(2 * PI_F * j / (period_to_use * 2 - 1)));
		float interp = f_index - (int)f_index;
		//mult = .5 * (1 - cos_lookup[(int)((float)j / (resampling_period * 2.0) * 8191.0)]);
		float value = ((1 - interp) * freeze_buffer[(int)f_index] + (interp)*freeze_buffer[(int)(f_index + 1) % 8192]);
		value *= freeze_voices[voice].CurrentAmplitude();
		out_buffer[0][out_index] += value * freeze_voices[voice].GetPanning(0) * freeze_volume;
		out_buffer[1][out_index] += value * freeze_voices[voice].GetPanning(1) * freeze_volume;


		f_index += period_ratio;
		if (f_index >= BUFFER_SIZE)
		{
			f_index -= BUFFER_SIZE;
		}
		if (++out_index >= BUFFER_SIZE)
		{
			out_index -= BUFFER_SIZE;
		}
	}
}

void Shifter::SetFreeze(bool freeze) {
	if (!freeze_mode && freeze) {
		freeze_needs_copy = true;
		
	}
	else if (freeze_mode && !freeze) {
		//release freeze voices
		for (int i = 0; i < MAX_VOICES; ++i) {
			freeze_voices[i].Release();
		}
	}
	freeze_mode = freeze;
}

void Shifter::SetFreezePitchAdjust(float val) {
	for(int i = 0; i < MAX_VOICES; ++i) {
		freeze_voices[i].SetPitchAdjust(val);
	}
}

void Shifter::SetPanWidth(float val) {
	for (int i = 0; i < MAX_VOICES; ++i) {
		freeze_voices[i].SetPanWidth(val);
		voices[i].SetPanWidth(val);
	}
}