From aef08fc28921f3544d379b6ca58c1da98ee6a71a Mon Sep 17 00:00:00 2001
From: Manuel Pitz <manuel.pitz@eonerc.rwth-aachen.de>
Date: Wed, 16 Jun 2021 15:23:55 +0000
Subject: [PATCH] first version of amplitude and frequency estimation

---
 lib/hooks/dft.cpp | 20 ++++++++++++++++++--
 1 file changed, 18 insertions(+), 2 deletions(-)

diff --git a/lib/hooks/dft.cpp b/lib/hooks/dft.cpp
index 82a052835..a7c847306 100644
--- a/lib/hooks/dft.cpp
+++ b/lib/hooks/dft.cpp
@@ -1158,7 +1158,8 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 	 * 
 	 * This function is based on the following paper:
 	 * https://mgasior.web.cern.ch/pap/biw2004.pdf
-	 * 
+	 * https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/
+	 * 	 * 
 	 * In particular equation 10
 	 * 
 	 */
@@ -1189,7 +1190,22 @@ static HookPlugin<DftHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flag
 		
 		y_Fmax = startFreqency + maxBinEst * frequencyResolution;
 
-		Point ret = {y_Fmax, 0};
+
+		//calc amplitude
+		//double h = (y2 - ( (y1 * x2^2) / x1^2)) / (1 - x2^2 / x1^2 )
+
+		//double h = (y2 - ( (y1 * pow(x2, 2)) / pow(x1, 2))) / (1 - pow(x2, 2) / pow(x1, 2));
+
+		//double h = ( - y3 + ( (y1 * pow(x3, 2)) / pow(x1, 2))) / (pow(x3, 2) / pow(x1, 2) - 1 );
+
+		double a = (x1 * (y2-y3)+x2 * (y3-y1)+x3 * (y1-y2))/((x1-x2) * (x1-x3) * (x3-x2));
+		double b = (pow(x1,2)*(y2-y3)+pow(x2,2)*(y3-y1)+pow(x3,2) * (y1-y2))/((x1-x2) * (x1-x3) * (x2-x3));
+		double c = (pow(x1,2) * (x2 * y3 - x3 *y2)+x1 * (pow(x3,2) * y2 - pow(x2,2) * y3)+ x2 * x3 * y1 * (x2-x3))/((x1-x2) * (x1-x3) * (x2-x3));
+
+		 
+		double h = a * pow(y_Fmax,2) + b * y_Fmax + c;
+
+		Point ret = {y_Fmax, h};
 
 		return ret;
 	}