#ifndef SHIFTER_H
#define SHIFTER_H

#include "Helmholtz.h"
#include "shifter_voice.h"

#define BUFFER_SIZE 8192
#define MAX_VOICES 12

template <typename T, size_t max_capacity>
class circ_queue {
public:
    circ_queue() {
        head = buffer;
        tail = buffer;
        size = 0;
        capacity = max_capacity;
    }

    void push(T val) {
        if (size == max_capacity) {
            pop();
        }
        *tail = val;
        if (++tail >= buffer + max_capacity) {
            tail -= max_capacity;
        }
        size++;
        //*head = val;
    }

    void clear() {
        head = buffer;
        tail = buffer;
        size = 0;
    }

    T pop() {
        if (size > 0) {
            T to_ret = *head;
            if (++head >= buffer + max_capacity) {
                head -= max_capacity;
            }
            --size;
            return to_ret;
        }
        else {
            return T();
        }
    }

    T& get(int indx) {
        T* ret_ptr = head + indx;
        if (ret_ptr >= buffer + max_capacity) {
            ret_ptr -= max_capacity;
        }
        return *ret_ptr;
    }

    T& operator[](int indx) {
        T* ret_ptr = head + indx;
        if (ret_ptr >= buffer + max_capacity) {
            ret_ptr -= max_capacity;
        }
        return *ret_ptr;
    }

    size_t capacity;
    // int size() { return capacity; }
    size_t size;
    T buffer[max_capacity];
    T* head;
    T* tail;
};



class MidiPitchSmoother {
public:

    MidiPitchSmoother() {
        tau = .1;
        dt = 2048.0f / 48000.0f;
        PcorrPrev = 60.0f;
        PtargPrev = 60;
        depth = 1.0f;
    }
    void SetFrameTime(float frame_time) {
        dt = frame_time;
    }
    void SetDepth(float d) {
        // clamp to [0,1]
        if (d < 0.0f) d = 0.0f;
        if (d > 1.0f) d = 1.0f;
        depth = d;
    }

    void SetTimeConstant(float t) {
        tau = t;
    }
    float update(float Pdet, int Ptarget) {
        // Detect large jump (new note)
        float diff = Pdet - (int)Pdet;
       
        if (Ptarget != PtargPrev) {
            // Immediately reset to new note
            PcorrPrev = diff;
            PtargPrev = Ptarget;
            inputPrev = Pdet;
            return Ptarget+ PcorrPrev;
        }
        PtargPrev = Ptarget;
        diff = Pdet - inputPrev;

        // Compute smoothing coefficient
        float alpha = 1.0 - std::exp(-dt / tau);

        // Compute smoothed pitch toward target
        float PcorrFull = PcorrPrev + alpha * (0 - PcorrPrev) * depth + (1 - depth) * diff;

        // Apply depth: scale the correction amount
        inputPrev = Pdet;
        PcorrPrev = PcorrFull;
        return Ptarget + PcorrFull;
    }


private:

    float tau;          // Time constant (s)
    float dt;           // Frame time (s)
    float PcorrPrev;    // Previous corrected pitch (MIDI)
    int PtargPrev;    // Previous corrected pitch (MIDI)
    float depth;        // Amount of correction applied [0..1]
    float inputPrev;
};



class Shifter {
public:
    void Init(float samplerate, int samplesPerBlock);
    void Process(const float* const* in,
        float** out,
        size_t size);

    void AddMidiNote(int note);
    void RemoveMidiNote(int note);
	void SetFormantPreserve(float val) { formant_preserve = val; }
    void SetAutoTuneSpeed(float val);
    void SetAutoTuneDepth(float val);
    void SetPortamentoTime(float time) {
        for (int i = 0; i < MAX_VOICES; ++i) {
            voices[i].SetPortamentoTime(time);
        }
	}
	float getInputPitch() const { return sample_rate_ / in_period; }
	float getOutputPitch() const { return sample_rate_ / out_period; }
    void SetHarmonyMix(float mix);
	void SetAutoTuneEnable(bool enable) { enable_autotune = enable; }
    void SetFreeze(bool);

    float out_midi = 40;
    ShifterVoice voices[MAX_VOICES];
    ShifterVoice freeze_voices[MAX_VOICES];

private:
    void DetectPitch(const float* const* in, float** out, size_t size);
    void SetRates();
    void GetSamples(float** output, const float* input, size_t size); 
    float GetOutputEnvelopePeriod(int out_voice);
	float GetOutputEnvelopePeriodFreeze(int freeze_voice);
    int GetPeakIndex();
    void AddInterpolatedFrame(int voice, int max_index, float period_to_use);
    void AddFreezeToOutput(int voice, float resampling_period);
    void CopyInputToFreezeBuffer(int);

    Helmholtz helm;
    // GranularSustain player;

    int selected_sample;
    bool playing;
    bool looping;
    bool loop_engaged;
    float play_head;
    float rate_factor;
    float sample_freq = 440;
    float freq_target = 440;
    float last_freq = 440;
    double current_pitch = 440;
    double smear_thresh = .085;
    double smear_step = 0.003;
    bool onset_trigger = false;
    bool pitch_trigger = false;
	float harmony_mix = 0.0f;
	float melody_mix = 0.0f;
    bool freeze_needs_copy;

    int trigger_bank;

    circ_queue<float, 3> prev_freqs;

    float formant_preserve = 0;


    float volume;
    float pitch_adj;
    bool last_record;
    int record_playhead;
    bool dry_wet = false;
    bool is_pitched = true;
    float last_freqs[3];
    float in_buffer[BUFFER_SIZE];
    float out_buffer[2][BUFFER_SIZE];
	float freeze_buffer[BUFFER_SIZE];
    int out_playhead = 0;
    int in_playhead = 0;
	int last_autotune_midi = -1;

	float out_period_filter_amount = 0.7f; // You can expose this as a parameter

	bool freeze_mode = false;
    
    float out_period = 0; //C3
    float in_period = 366.936;
    float out_period_counter = 0;
    float cos_lookup[8192];
    float sample_rate_;
    int blocksize;
	bool enable_autotune = false;
    float freeze_period;
    float freeze_volume = 1;
    MidiPitchSmoother out_midi_smoother;
};
#endif