/*
 * Interrupthandler ADC: numerischer Oszillator (NCO), I- und Q-Mischer, CIC-Filter, Dezimation,
 * IIR-Filter, Demodulation, Vorwärts-Schwundregelung, D/A-Ausgabe
 *
 * Die zeitkritischen Funktionen werden teilweise in Assembler implementiert.
 */

#include "stm32f446xx.h"
#include "ADC_Interrupt.h"
#include "math.h"
// Filterkoeffizienten für AGC-Tiefpassfilter, definiert die Konstante 'AGC_Par'
#include "AGCcoeff.c"
// Filterkoeffizienten für NF-Hochpass zur DC-Abtrennung
#include "NF_HP.c"

// ******************* lokale Typdefinitionen **************************************************

// Hilfstyp für Rückgabe der auf 16 Bit umskalierten CIC-Ausgangssignale von der Assemblerfunktion (provisorisch)
typedef struct
{
	int16_t I;
	int16_t Q;
} IQ_Type;

// ******************* Konstanten **************************************************************

// Anzahl Samples pro Interrupt, Wert ist in der Assemblerfunktion 'ADC_INT' definiert
extern const int ADC_BUFFER_LENGTH;

// Die folgenden Variablen sind aus Effizienzgründen in der Assemblerfunktion 'ADC_INT' definiert.
// Hier werden sie nur für Initialisierungszwecke benötigt.

// ******************* globale Variablen *******************************************************

// Samplebuffer, zwei Hälften für Doppelbuffer
extern int16_t ADC_DMA_Buffer[];
// Anzahl cos/sin-Paare der Sinustabelle
extern int NCO_SinLength;
// Sinus- und Cosinustabelle für NCO, abwechselnd cos(phi) und sin(phi)
extern int16_t NCO_Sinus[];
// BiQuad-Parameter für 500Hz..10kHz Bandbreite
extern float IIR_LP_PARAM;

// Index auf das zu wählende ZF-Filter (Bandbreite), provisorisch
int Filter = 40;
// Zustandsvariablen für das I- und Q-ZF-Filter (4 Elemente pro BiQuad, 8-polige Filter = 4 BiQuad-Stufen)
// Reihenfolge der Elemente eines BiQuads: x(n-1), x(n-2), y(n-1), y(n-2)
float Biq_StateI[16], Biq_StateQ[16];

// ******************* Funktionen *************************************************************

// Initialisierung Sinustabelle
void ADC_SinusInit(void)
{
	int i;
	// Amplitude +/- 16 Bit (32767)
	for(i=0; i < NCO_SinLength; i++)
	{
		NCO_Sinus[2*i]   = (int16_t)(32767.0 * cos((float)(i)/(float)(NCO_SinLength) * 2 * M_PI));
		NCO_Sinus[2*i+1] = (int16_t)(32767.0 * sin((float)(i)/(float)(NCO_SinLength) * 2 * M_PI));
	}
}

// Startadresse des RAM-Buffers für DMA, wird für die DMA-Initialisierung benötigt
unsigned int ADC_DMA_getBufferAddress(void)
{
	return (unsigned int)(ADC_DMA_Buffer);
}

// Länge des DMA-Buffers, wird für die DMA-Initialisierung benötigt
int ADC_DMA_getBufferLength(void)
{
	return ADC_BUFFER_LENGTH;
}


// Assemblerfunktion für CIC-Filter, NCO und Mischer, liefert dezimiertes I- und Q-Signal,
// muss als erstes im ADC-Interrupt aufgerufen werden
extern "C" IQ_Type ADC_INT(void);


// 2-poliges BiQuad generell: y(n) = b0 * x(n) + b1 * x(n-1) + b2 * x(n-2) - a1 * y(n-1) - a2 * y(n-2)
// Achtung: Reihenfolge der a- und b-Koeffizienten entspricht der Matlab-Definition, in Scilab ist
// es genau verkehrt! Im Code ist die Matlab-Reihenfolge gewählt.

// Berechnung einer Biquad-Stufe (2poliges Filter)
// input: Eingangssignal, Biq_Par: Pointer auf Parameter, Biq_State: Pointer auf Zustand
// Biq_Par: b0, b1, b2, a1, a2
// Berechnung: y = b0 * input + b1 * State(0) + b2 * State(1) - a1 * State(2) - a2 * State(3)
// State(3) <- State(2), State(2) <- y, State(1) <- State(0), State(0) <- input
float BiQuad_Stage(float input, float *Biq_Par, float *Biq_State)
{
	float y;

	y = Biq_Par[0] * input + Biq_Par[1] * Biq_State[0] + Biq_Par[2] * Biq_State[1]
	  - Biq_Par[3] * Biq_State[2] - Biq_Par[4] * Biq_State[3];
	Biq_State[1] = Biq_State[0];
	Biq_State[0] = input;
	Biq_State[3] = Biq_State[2];
	Biq_State[2] = y;
	return y;
}

// Assemblervariante der obigen Funktion 'BiQuad_Stage', ist funktional gleich (rechnet aber etwas genauer),
// braucht aber nur etwa halbe Ausführungszeit
extern "C" float BiQuad_Stage_asm(float input, const float *Biq_Par, float *Biq_State);


/*
 * ADC-Interrupthandler, wird nach jedem kompletten DMA-Transfer
 * gestartet, hat somit die Frequenz nach der Dezimation (50kHz).
 * Damit die Ausgabe auf dem DAC unabhängig von der Ausführungszeit des Interrupthandlers wird,
 * werden die Werte für den D/A-Wandler ganz am Anfang in das DAC-Register geschrieben, so dass sie
 * beim Update-Event für den DAC, der gegen Ende des Interrupthandlers kommt, sicher im DAC-Register sind.
 * Somit werden also immer die Werte des vorherigen Interrupts ausgegeben, daher müssen diese in einer
 * statischen Variablen gespeichert sein.
*/
float AGC_Gain = 1.0;   // 'Regelspannung', Initialisierung, damit keine Division durch Null
void ADC_DMA_Interrupthandler(void)
{
	static int DAC_Out;        // NF-Ausgangssignal für DAC, skaliert auf 12 Bit rechtsbündig
	static int AGC_Ctr = 0;    // Dezimationszähler für Samplingrate AGC
	static float Biq_StateAGC[4];  // Zustand Tiefpassfilter AGC
	static float Biq_StateNFHP[4]; // Zustand NF-Hochpassfilter
	// Hilfsvariablen
	float i, q, nf, nf_dc, *Biq_Par, Gain;
	IQ_Type IQ;

	// Mess-Signal zur Bestimmung der Prozessorlast: PB9 wird am Anfang des Interrupthandlers gesetzt
	// und am Ende gelöscht, so kann mit einem Oszilloskop die Interruptfrequenz überprüft werden und
	// die 'High'-Zeit entspricht der Ausführungszeit.

	// PB9 (Pin 5 von CN 10 auf dem Nucleo) setzen zur Messung der Ausführungszeit
	GPIOB->BSRR = 0x200;

	// Ausgabe des Samples vom letzten Durchgang auf den DAC, effektive Ausgabe erfolgt durch Trigger von Timer 5
	DAC->DHR12R2 = DAC_Out;

	// Aufruf Assemblerfunktion für NCO, Mischer und CIC-Dezimationsfilter, Rückgabe der auf
	// 16 Bit skalierten I- und Q-Sample
	IQ = ADC_INT();

	// Pointer auf ZF-Filterparameter entsprechend dem gewählten Filter berechnen:
	// Pro Filter sind es 20 Parameter (4 BiQuad-Stufen zu je 5 Parametern) und ein Gain-Korrekturwert, also
	// insgesamt 21 Parameter pro Filter.
	Biq_Par = &IIR_LP_PARAM + 21*Filter;
	// Gain-Korrekturwert ist erster Parameter, hier lesen und Pointer inkrementieren,
	// dann zeigt er auf den Anfang der Filterparameter
	Gain = *Biq_Par;
	Biq_Par = Biq_Par + 1;

	// I filtern, 4 BiQuad-Stufen hintereinander
	i = BiQuad_Stage_asm((float)(IQ.I), Biq_Par, Biq_StateI);
	i = BiQuad_Stage_asm(i, Biq_Par+5, Biq_StateI+4);
	i = BiQuad_Stage_asm(i, Biq_Par+10, Biq_StateI+8);
	i = BiQuad_Stage_asm(i, Biq_Par+15, Biq_StateI+12);
	// Gainkorrektur, abhängig von Filterfrequenz
	i = i * Gain;

	// Q filtern
	q = BiQuad_Stage_asm((float)(IQ.Q), Biq_Par, Biq_StateQ);
	q = BiQuad_Stage_asm(q, Biq_Par+5, Biq_StateQ+4);
	q = BiQuad_Stage_asm(q, Biq_Par+10, Biq_StateQ+8);
	q = BiQuad_Stage_asm(q, Biq_Par+15, Biq_StateQ+12);
	// Gainkorrektur, abhängig von Filterfrequenz
	q = q * Gain;

	// AM-Hüllkurvendemodulation und Addition des Offsets für D/A-Wandler
	nf_dc = sqrtf(i * i + q * q);

	// AGC-Vorwärtskorrektur
	nf = nf_dc * 500.0 / AGC_Gain;

	// Hochpass zur Abtrennung des DC-Anteils vom NF-Signal
	nf = BiQuad_Stage_asm(nf, NF_HP+1, Biq_StateNFHP) * (*NF_HP);
	nf = nf + 2048.0; // Offset für D/A-Wandler addieren

	// Begrenzung auf 0..4095 (zulässiger Wertebereich des DA-Wandlers)
	// negative Werte können hier nicht vorkommen
	if (nf > 4095.0)
		nf = 4095.0;
	// Wert für DAC merken, wird am Anfang des nächsten Interrupts in das DAC-Register geschrieben
	DAC_Out = (int)(nf);

	// AGC
	// Dezimation wegen Tiefpass (Numerikproblem)
	AGC_Ctr = AGC_Ctr + 1;
	if (AGC_Ctr == 5) // Dezimationsfaktor 5
	{
		AGC_Ctr = 0;
		//Tiefpass
		Gain = *AGC_Par;
		AGC_Gain = BiQuad_Stage_asm(nf_dc, AGC_Par+1, Biq_StateAGC) * Gain;

		// Regel-Gain bei nicht vorhandenem Signal begrenzen (es wird durch AGC_Gain dividiert, also
		// kleiner Wert = grosse Verstärkung), damit Quantisierungsrauschen nicht übermässig laut wird,
		// so wird auch Division durch Null verhindert.
		if (AGC_Gain < 0.1)
			AGC_Gain = 0.1;
	}

	// Ende des Interrupts, somit Mess-Ausgang PB9 zurücksetzen
	GPIOB->BSRR = 0x200 << 16;
}