#include <math.h>
#include <stdint.h>

#include "daisysp.h"
#include "state.h"
#include "const.h"
#include "vco.h"
#include "dsp.h"

#include "synth.h"

using namespace daisysp;

Oscillator osc;
Svf filter;
AdEnv vco_env;
AdEnv filter_env;

DelayLine<float, COMB0_SIZE> comb0;
DelayLine<float, COMB1_SIZE> comb1;
DelayLine<float, COMB2_SIZE> comb2;
DelayLine<float, COMB3_SIZE> comb3;
DelayLine<float, AP0_SIZE> ap0;
DelayLine<float, AP1_SIZE> ap1;

float clock_phase = 0.0f;
bool clock_trig = false;

float amen_phase = 0.0f;
float amen_length = 0.0f;

static inline auto vco_mode_to_daisy(vco_mode_t mode) {
	switch (mode) {
	case VCO_SQUARE:   return daisysp::Oscillator::WAVE_SQUARE;
	case VCO_TRIANGLE: return daisysp::Oscillator::WAVE_TRI;
	case VCO_SAW: 	   return daisysp::Oscillator::WAVE_SAW;
	case VCO_SINE:
	default: return daisysp::Oscillator::WAVE_SIN;
	}
}

static inline float pot_to_freq(float v, float min, float max) { return min * powf(max / min, v); }

static inline float pot_to_time(float v, float min, float max) { return min * powf(max / min, v); }

static inline float quantize(float freq, float temp) {
	float midi = temp * log2f(freq / 440.0f) + 69.0f;
	float note = roundf(midi);
	return 440.0f * powf(2.0f, (midi - 69.0f) / temp);
}

static inline float low_pass(float in, float* z, float coeff) {
	*z += coeff * (in - *z);
	return *z;
}

float reverb(float in, float amount) {
	float damp0 = 0.0f, damp1 = 0.0f, damp2 = 0.0f, damp3 = 0.0f;
	float feedback = 0.84f;
	float damp = 0.3f;

	float c0 = comb0.Read(); low_pass(in + c0 * feedback, &damp0, damp0); comb0.Write(damp0);
	float c1 = comb1.Read(); low_pass(in + c1 * feedback, &damp1, damp1); comb1.Write(damp1);
	float c2 = comb2.Read(); low_pass(in + c2 * feedback, &damp2, damp2); comb2.Write(damp2);
	float c3 = comb3.Read(); low_pass(in + c3 * feedback, &damp3, damp3); comb3.Write(damp3);

	float wet = (c0 + c1 + c2 + c3) * 0.25f;

	const float g = 0.7f;
	float a0r = ap0.Read();
	float a0in = wet + (-g * a0r);
	ap0.Write(a0in);
	wet = a0r + g * a0in;

	float a1r = ap1.Read();
	float a1in = wet + (-g * a1r);
	ap1.Write(a1in);
	wet = a1r + g * a1in;

	return in + amount * (wet - in);
}

void synth_init(void) {
	osc.Init(SAMPLE_RATE);
	osc.SetWaveform(vco_mode_to_daisy(VCO_SINE));
	osc.SetFreq(440.0f);
	osc.SetAmp(1.0f);

	filter.Init(SAMPLE_RATE);
	filter.SetFreq(2000.0f);
	filter.SetRes(0.0f);

	vco_env.Init(SAMPLE_RATE);
	vco_env.SetTime(ADENV_SEG_ATTACK, 0.01f);
	vco_env.SetTime(ADENV_SEG_DECAY, 0.5f);
	vco_env.SetMin(0.0f);
	vco_env.SetMax(1.0f);

	filter_env.Init(SAMPLE_RATE);
	filter_env.SetTime(ADENV_SEG_ATTACK, 0.01f);
	filter_env.SetTime(ADENV_SEG_DECAY, 0.5f);
	filter_env.SetMin(0.0f);
	filter_env.SetMax(1.0f);

	state.clock_bpm = 0.2f;

	clock_phase = 0.0f;
	clock_trig = false;
}

float get_sample(void) {
	float bps = pot_to_time(state.clock_bpm, BPM_MIN, BPM_MAX) / 60.0f;
	float clock_inc = bps / SAMPLE_RATE;
	clock_phase += clock_inc;
	clock_trig = false;
	if (clock_phase >= 1.0f) {
		clock_phase -= 1.0f;
		clock_trig = true;
	}

	if (clock_trig) {
		vco_env.Trigger();
		filter_env.Trigger();
	}

	//vco_env.SetTime(ADENV_SEG_ATTACK, pot_to_time(state.env1_attack, ENV_ATTACK_MIN, ENV_ATTACK_MAX));
	//vco_env.SetTime(ADENV_SEG_DECAY, pot_to_time(state.env1_release, ENV_RELEASE_MIN, ENV_RELEASE_MAX));
	//filter_env.SetTime(ADENV_SEG_ATTACK, pot_to_time(state.env2_attack, ENV_ATTACK_MIN, ENV_ATTACK_MAX));
	//filter_env.SetTime(ADENV_SEG_DECAY, pot_to_time(state.env2_release, ENV_RELEASE_MIN, ENV_RELEASE_MAX));
	
	float vco_env_out = vco_env.Process();
	float filter_env_out = filter_env.Process();

	float vco_freq = pot_to_freq(state.vco_freq, VCO_FREQ_MIN, VCO_FREQ_MAX);
	if (state.quant_enabled) vco_freq = quantize(vco_freq, 12.0f);

	//osc.SetFreq(vco_freq);
	//osc.SetWaveform(vco_mode_to_daisy(state.vco_mode));
	//osc.SetAmp(1.0f);

	float vco_out = osc.Process();

	float base_cutoff = pot_to_freq(state.filter_freq, FILTER_FREQ_MIN, FILTER_FREQ_MAX);
	float mod_cutoff = base_cutoff + filter_env_out * (FILTER_FREQ_MAX - FILTER_FREQ_MIN);
	mod_cutoff = fclamp(mod_cutoff, FILTER_FREQ_MIN, FILTER_FREQ_MAX);

	//filter.SetFreq(mod_cutoff);
	//filter.SetRes(state.filter_resonance);
	filter.Process(vco_out);
	float filtered = filter.Low();

	float vca_out = filtered * vco_env_out * state.vco_volume;

	float reverb_out = reverb(vca_out, state.reverb_amount);

	float mix = reverb_out;
	mix = fclamp(mix, -1.0f, 1.0f);

	return mix;
}