/*
 * 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_StateIQ[32];//, 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" float 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
float ZF_IQ[4];
int BFO_Frequenz[2];
extern uint8_t Modulation;
float AGC_Speed = 1.0f;  // Abklingzeitkonstante AGC, wird bei USB, LSB und CW auf langsam gestellt

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 nf, nf_dc, Gain;

	extern int NCO_Frequenz[3];
	static float PLL_Gain = 0.001f;
	static float PLL_GainP = 1.0f;
	static float PLL_I;

	// float *Biq_Par;
	//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);
*/
	nf_dc = ADC_INT()/4096.0f;

	// ZF_IQ: Index 0 ist I, Index 1 ist Q, Index 2 ist USB und Index 3 ist LSB

	switch(Modulation)
	{
		case 0: // AM
			nf = nf_dc;
			NCO_Frequenz[1] = 0; // Frequenz-Offset zurücksetzen
			break;
		case 1: // Synchron AM
			nf = ZF_IQ[0]/4096.0f; // NF-Signal ist I, Skalierung für korrekte Amplitude
			// PI-Regler für PLL (PLL_Gain ist Zeitkonstante und PLL_GainP ist Verstärkung)
			// ZF_IQ[1] ist Q, wird auf Null geregelt
			PLL_I = PLL_I + PLL_Gain * ZF_IQ[1]; // Integrator der PLL
			if (fabs(PLL_I) > 1e7f)
			{	// Integrator auf ca. +/-3kHz begrenzen, damit er nicht beliebig wegläuft
				if (PLL_I < 0.0f)
					PLL_I = -1e7f;
				else
					PLL_I = 1e7f;
			}
			// Frequenz-Offset als P + I in NCO_Frequenz[1]
			NCO_Frequenz[1] = (int)(ZF_IQ[1] * PLL_GainP + PLL_I);
			break;
		case 2: // USB
			nf = ZF_IQ[2]/4096.0f/32768.0f;
			break;
		case 3: // LSB
			nf = ZF_IQ[3]/4096.0f/32768.0f;
			break;
		case 4: // CW (USB, aber mit anderer BFO-Frequenz)
			nf = ZF_IQ[2]/4096.0f/32768.0f;
			break;
		default: // Fehler
			nf = 0.0f;
			break;
	}
	// Hochpass zur Abtrennung des DC-Anteils vom NF-Signal
	nf = BiQuad_Stage_asm(nf, NF_HP+1, Biq_StateNFHP) * (*NF_HP);
	// AGC-Vorwärtskorrektur
	nf = nf * 100.0f / AGC_Gain; //500.0
	nf = nf + 2048.0f; // Offset für D/A-Wandler addieren
	// Signal auf Range vom DAC begrenzen
	if (nf > 4095.0f)
		nf = 4095.0f;
	else
		if (nf < 0.0f)
			nf = 0.0f;

	// 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;
		Gain = BiQuad_Stage_asm(nf_dc, AGC_Par+1, Biq_StateAGC) * Gain;

		if (Gain > AGC_Gain)
			AGC_Gain = Gain;
		else
			AGC_Gain = AGC_Gain - (AGC_Gain - Gain) * AGC_Speed;
		// 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;
}