#include "scsp.h"
#include "reverb.h"
#include "dsp.h"

#define COEF_ONE 	0
#define COEF_ZERO	1
#define COEF_GAIN 	2
#define COEF_C_D1 	3
#define COEF_C_D2 	4
#define COEF_C_FB 	5
#define COEF_A_FB 	6

#define TEMP_OUTPUT 	0
#define TEMP_OUT2 	1
#define TEMP_C0_STATE 	2
#define TEMP_C1_STATE 	3
#define TEMP_C2_STATE 	4
#define TEMP_C3_STATE 	5

#define MADRS_C0_O1 	0
#define MADRS_C1_O1 	1
#define MADRS_C2_O1 	2
#define MADRS_C3_O1 	3
#define MADRS_C0_O2 	4
#define MADRS_C1_O2 	5
#define MADRS_C2_O2 	6
#define MADRS_C3_O2 	7
#define MADRS_AP0_O1 	8
#define MADRS_AP1_O1 	9
#define MADRS_AP0_O2 	10
#define MADRS_AP1_O2 	11

#define COMB0_LEN 	1120
#define COMB1_LEN 	1280
#define COMB2_LEN 	1424
#define COMB3_LEN 	1560
#define AP0_LEN 	560
#define AP1_LEN 	334

#define COMB0_O1 	0
#define COMB0_O2 	(COMB0_O1 + COMB0_LEN - 1)
#define COMB1_O1 	(COMB0_O2 + 1)
#define COMB1_O2 	(COMB1_O1 + COMB1_LEN - 1)
#define COMB2_O1 	(COMB1_O2 + 1)
#define COMB2_O2 	(COMB2_O1 + COMB2_LEN - 1)
#define COMB3_O1 	(COMB2_O2 + 1)
#define COMB3_O2 	(COMB3_O1 + COMB3_LEN - 1)
#define AP0_O1 		(COMB3_O2 + 1)
#define AP0_O2 		(AP0_O1 + AP0_LEN - 1)
#define AP1_O1 		(AP0_O2 + 1)
#define AP1_O2 		(AP1_O1 + AP1_LEN - 1)

void CreateReverbProgram(uint64_t* program) {	
	reg16* COEF = scsp->reg.dsp.COEF;
	reg16* MADRS = scsp->reg.dsp.MADRS;

	DSP_BEGIN(program)
		// set up coefficients
		COEF[COEF_ONE]      = DSP_MAKE_COEF(4095);  // 1.0
		COEF[COEF_ZERO]     = DSP_MAKE_COEF(0);     // 0.0

		COEF[COEF_GAIN]     = DSP_MAKE_COEF(2048);  // gain = 0.5
		COEF[COEF_C_D1]     = DSP_MAKE_COEF(3276);  // c_d1 = damp = 0.8
		COEF[COEF_C_D2]     = DSP_MAKE_COEF(820);   // c_d2 = 1.0 - damp
		COEF[COEF_C_FB]     = DSP_MAKE_COEF(3328);  // c_fb = (roomsize * 0.25) + 0.75
		COEF[COEF_A_FB]     = DSP_MAKE_COEF(2048);  // a_fb = 0.5

		MADRS[MADRS_C0_O1]      = COMB0_O1;
		MADRS[MADRS_C0_O2]      = COMB0_O2;
		MADRS[MADRS_C1_O1]      = COMB1_O1;
		MADRS[MADRS_C1_O2]      = COMB1_O2;
		MADRS[MADRS_C2_O1]      = COMB2_O1;
		MADRS[MADRS_C2_O2]      = COMB2_O2;
		MADRS[MADRS_C3_O1]      = COMB3_O1;
		MADRS[MADRS_C3_O2]      = COMB3_O2;
		MADRS[MADRS_AP0_O1]     = AP0_O1;
		MADRS[MADRS_AP0_O2]     = AP0_O2;
		MADRS[MADRS_AP1_O1]     = AP1_O1;
		MADRS[MADRS_AP1_O2]     = AP1_O2;

		// Initialize TEMP_OUTPUT to zero
		DSP_EMIT(DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_ZERO)
		DSP_EMIT(DSP_TWT | DSP_TWA(TEMP_OUTPUT))

		// === COMB FILTER ===
		// 10 steps per comb * 4 combs = 40 steps
		for (int i = 0; i < 4; i++) {
			// fetch previous comb state into X, set Y to comb D1, set B to zero
			DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_C0_STATE + i + 1) | DSP_YSEL_COEF | DSP_CRA(COEF_C_D1) | DSP_ZERO)

			// begin read from RB[comb offs + comb len - 1], transfer acc to B
			DSP_EMIT(DSP_MRD | DSP_MASA(MADRS_C0_O2 + i) | DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_BSEL_ACC)

			// not ready yet, preserve B
			DSP_EMIT(DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_BSEL_ACC)

			// transfer read data to MEM[0], preserve B
			DSP_EMIT(DSP_IWT | DSP_IWA(0) | DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_BSEL_ACC)

			// read MEM[0] into X, set Y to comb D2, preserve B
			DSP_EMIT(DSP_XSEL_INPUTS | DSP_IRA(0) | DSP_YSEL_COEF | DSP_CRA(COEF_C_D2) | DSP_BSEL_ACC)

			// read MIXS[0] to X, read comb fb to Y, transfer ACC to B, write SHIFTED to new comb state
			DSP_EMIT(DSP_XSEL_INPUTS | DSP_IRA(0x20) | DSP_YSEL_COEF | DSP_CRA(COEF_C_FB) | DSP_BSEL_ACC | DSP_TWT | DSP_TWA(TEMP_C0_STATE + i))

			// can't write on odd step, preserve B
			DSP_EMIT(DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_BSEL_ACC)

			// read TEMP_OUTPUT into X, set Y to 1.0, set B to 0.0, write to RB[comb offs]
			DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_OUTPUT) | DSP_YSEL_COEF | DSP_CRA(COEF_ONE) | DSP_ZERO | DSP_MWT | DSP_MASA(MADRS_C0_O1 + i))

			// read MEM[0] into X, set Y to 1.0, transfer ACC to B
			DSP_EMIT(DSP_XSEL_INPUTS | DSP_IRA(0) | DSP_YSEL_COEF | DSP_CRA(COEF_ONE) | DSP_BSEL_ACC)

			// write new TEMP_OUTPUT
			DSP_EMIT(DSP_TWT | DSP_TWA(TEMP_OUTPUT))
		}

		// === ALLPASS FILTER ===
		// 12 steps per allpass * 2 allpass = 24 steps

		for (int i = 0; i < 2; i++) {
			// can't do MRD on odd steps
			DSP_EMIT(DSP_ZERO)

			// begin read from RB[ap offs + ap len]
			DSP_EMIT(DSP_MRD | DSP_MASA(AP0_O2 + i))

			// not ready yet
			DSP_EMIT(DSP_ZERO)

			// transfer read data to MEM[0]
			DSP_EMIT(DSP_IWT | DSP_IWA(0))

			// read TEMP_OUTPUT into X, set Y to 1.0, set B to zero
			DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_OUTPUT) | DSP_YSEL_COEF | DSP_CRA(COEF_ONE) | DSP_ZERO)

			// read MEM[0] into X, set Y to 1.0, transfer acc to B and negate
			DSP_EMIT(DSP_XSEL_INPUTS | DSP_IRA(0) | DSP_YSEL_COEF | DSP_CRA(COEF_ONE) | DSP_BSEL_ACC | DSP_NEGB)

			// read MEM[0] into X, set Y to allpass FB, set B to zero, write SHIFTED to TEMP_OUT2
			DSP_EMIT(DSP_XSEL_INPUTS | DSP_IRA(0) | DSP_YSEL_COEF | DSP_CRA(COEF_A_FB) | DSP_ZERO | DSP_TWT | DSP_TWA(TEMP_OUT2))

			// read TEMP_OUTPUT into X, set Y to 1.0, transfer ACC to B
			DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_OUTPUT) | DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_BSEL_ACC)

			// can't do MWT on odd steps, preserve B
			DSP_EMIT(DSP_YSEL_COEF | DSP_CRA(COEF_ZERO) | DSP_BSEL_ACC)

			// read TEMP_OUT2 into X, set Y to 1.0, set B to 0.0, begin write to RB[ap offs]
			DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_OUT2) | DSP_YSEL_COEF | DSP_CRA(COEF_ONE) | DSP_ZERO | DSP_MWT | DSP_MASA(MADRS_AP0_O1))

			// read TEMP_OUT2 into X, set Y to 1.0, set B to 0.0
			DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_OUT2) | DSP_YSEL_COEF | DSP_CRA(COEF_ONE) | DSP_BSEL_ACC)

			// write new TEMP_OUTPUT
			DSP_EMIT(DSP_TWT | DSP_TWA(TEMP_OUTPUT))
		}

		// === FINAL OUTPUT ===

		// load TEMP_OUTPUT into X, set Y to COEF_GAIN, set B to 0
		DSP_EMIT(DSP_XSEL_TEMP | DSP_TRA(TEMP_OUTPUT) | DSP_YSEL_COEF | DSP_CRA(COEF_GAIN) | DSP_ZERO)

		// write to efreg 0
		DSP_EMIT(DSP_EWT | DSP_EWA(0))
	DSP_END
}

void SetReverbParameters(int gain, int roomsize, int damp) {
	reg16* COEF = scsp->reg.dsp.COEF;

	COEF[COEF_GAIN] 	= DSP_MAKE_COEF(gain);
	COEF[COEF_C_D1] 	= DSP_MAKE_COEF(damp);                      // c_d1 = damp
	COEF[COEF_C_D2] 	= DSP_MAKE_COEF(4096 - damp);               // c_d2 = 1.0 - damp
	COEF[COEF_C_FB] 	= DSP_MAKE_COEF((roomsize >> 2) + 3072);    // c_fb = (roomsize * 0.25) + 0.75
}