#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

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()
{
    volume = 1;
    helm.setframesize(1024);
    helm.setoverlap(1);
    for (int i = 0; i < MAX_VOICES + 1; ++i)
    {
        out_midi[i] = -1;
    }
    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);
    }
	out_panning[MAX_VOICES] = 0.5f;
}

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.7f; // 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 = 48000 / in_period;
    
    int midi = (int)(12 * log2f(in_freq / 440) + 69.5f);
    float target_out_period = 48000.0f / mtof(midi);
	
    if (midi != last_autotune_midi) {
        last_autotune_midi = midi;
        out_periods[MAX_VOICES] = in_period;
    }

	float error = target_out_period - out_periods[MAX_VOICES];
    float adjustment = error * out_period_filter_amount;

    //target_out_period = in_period * out_period_filter_amount + target_out_period * (1 - out_period_filter_amount);
    out_midi[MAX_VOICES] = midi;
	out_periods[MAX_VOICES] += adjustment;
}

void Shifter::SetRates() {}

float Shifter::GetOutputEnvelopePeriod(int out_voice) {
    //TODO add something so that low pitch ratios end up reducing formant_preservation
    return in_period * formant_preserve + out_periods[out_voice] * (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;
        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::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 + 1; ++out_p)
        {
            if (out_midi[out_p] == -1) continue;
            if (out_period_counters[out_p] > out_periods[out_p])
            {
                out_period_counters[out_p] -= out_periods[out_p];

                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 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;
        }
        for (int out_p = 0; out_p < MAX_VOICES + 1; ++out_p)
        {
            if (out_midi[out_p] == -1) continue;
            out_period_counters[out_p] += 1;
        }
    }
}


void Shifter::AddMidiNote(int note) {
    for (int i = 0; i < MAX_VOICES; ++i) {
        if (out_midi[i] == note) {
            return;
        }
    }
    for (int i = 0; i < MAX_VOICES; ++i) {
        if (out_midi[i] == -1) {
            out_midi[i] = note;
            out_periods[i] = 48000.0f / mtof(note);
            out_period_counters[i] = 0;
			out_panning[i] = rand() / (float)RAND_MAX;
            return;
        }
    }
}


void Shifter::RemoveMidiNote(int note) {
    for (int i = 0; i < MAX_VOICES; ++i) {
        if (out_midi[i] == note) {
            out_midi[i] = -1;
            return;
        }
    }
}