#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)samplesPerBlock / samplerate);
    volume = 1;
    helm.setframesize(1024);
    helm.setoverlap(1);
    for (int i = 0; i < MAX_VOICES; ++i)
    {
        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);
    }
}

void Shifter::Process(const float* const* in,
    float** out,
    size_t size)
{
    DetectPitch(in, out, size);
    SetRates();
    // for (size_t i = 0; i < size; ++i) {
    //     out[0][i] = 0;
    // }
    GetSamples(out, in[0], size);
    //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)
{
    // 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], out[0], 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)
    static 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);
}

int Shifter::GetPeakIndex() {
    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];
        if (val > max_value)
        {
            max_index = index;
            max_value = val;
        }
        if (++index >= BUFFER_SIZE)
        {
            index -= BUFFER_SIZE;
        }
    }
    return max_index;
}

//void Shifter::AddInterpolatedFrame(int voice, int max_index, float resampling_period) {
//    float period_ratio = resampling_period / out_periods[voice];
//    float f_index;
//    f_index = max_index - resampling_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;
//        out_buffer[0][out_index] += value * (1 - out_panning[voice]);
//        out_buffer[1][out_index] += value * out_panning[voice];
//
//
//        f_index += period_ratio;
//        if (f_index >= BUFFER_SIZE)
//        {
//            f_index -= BUFFER_SIZE;
//        }
//        if (++out_index >= BUFFER_SIZE)
//        {
//            out_index -= BUFFER_SIZE;
//        }
//    }
//}

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)
{
    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);
            }
        }
        if (out_period_counter > out_period)
        {
            out_period_counter -= out_period;
            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];

        //output samples, set to 0
        for (int ch = 0; ch < 2; ++ch) {
            output[ch][i] = out_buffer[ch][out_playhead];
            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;
    }
}