%% Parameter zum rumspielen

fitOrder = 2; % Polynom Ordnung der gefitteten Inversen Funktion
dataSource = 'example'; % 'example' or 'measurement'

%% Lade Daten

% x = Eingangsgröße = wahre Messgröße
% y = Ausgangsgörße = vom Sensor gemessener Wert
% z = Linearisierter Wert. Idealerweise == x

switch dataSource
    case 'measurement'
        T = readtable('data.csv');
    case 'example'
        
        T = table();
        
        T.x = 0:100;
        % f = @(x)(x.^2*.05 + 2*x)/8; % f(x) = Exakte Sensor Kennlinie (häufig unbekannt)
        f = @(x) 0.1*x.^2 + 2*x + 20; % f(x) = Exakte Sensor Kennlinie (häufig unbekannt)
        T.y = f(T.x);
    otherwise
        error('dataSource must be "example" or "measurement"')
end

T.z = nan(height(T),1);

%% Fitte Ausgleichsgerade für Fehlerberechnung
bestFitCoeffs = polyfit(T.x, T.y, 1); % fit linear function
bestFitEval = polyval(bestFitCoeffs, T.x);
bestFitErr = T.y - bestFitEval;

fullScale = max(bestFitEval)-min(bestFitEval); % gesamter Messbereich der linearen Sensorantwort

% Berechne Fehler, nach http://techniker.pi-pro.de/fs/messtechnik/pdf/messfehler.pdf
% "Bestimmung des absoluten Linearitätsfehlers nach der Best-fitting Methode"
maxAbsLinError = max(abs(bestFitErr));

% Relativer Fehler, bezogen auf den Gesamten Messbereich.
maxRelLinError = maxAbsLinError/fullScale;

%% Umkehrfunktion fit
% y = f(x) -> ideale nichtlineare Sensor-Kennlinie
% y* = f*(x) -> approximierte Sensor-Kennlinie
% z = g(y) = f^(-1)(y) -> linearisierter Sensor

% wird nicht benutzt!
fStarCoeffs = polyfit(T.x, T.y, 3); % f*(x)
fStarEval = polyval(fStarCoeffs, T.x); % y* = f*(x) -> approximierte Sensorkennlinie.

% Theoretisch müssten wir jetzt y*=f*(x) invertieren. Das kann aber kompliziert
% sein, deswegen fitten wir einfach die umgekehrten Argumente. Das ist natürlich
% nicht die exakte Umkehrfunktion. Hier ist der Punkt, an dem je nach Sensor
% basteln muss. Ggf. reicht eben ein Polynom nicht aus
gCoeffs = polyfit(T.y, T.x, fitOrder); % g*(y) -> approximierte Umkehrfunktion
gEval = polyval(gCoeffs, T.y); % z* = g*(y)

% Da die lineare Übertragungsfunktion des Sensors nicht 1 ist, sondern eine
% Steigung + Offset hat, müssen wir diese hier für Vergleichbare Ergebnisse
% nochmal auf unsere Linearisierte Funktion anwenden.
z = polyval(bestFitCoeffs, gEval); % apply linear transfer function
T.z = z;

% Fehler berechnen
appliedCorrectionError = z - bestFitEval;
maxAppliedCorrectionLinError = max(abs(appliedCorrectionError));
maxRelAppliedCorrectionError = maxAppliedCorrectionLinError/fullScale;

%% Print summary
fprintf("Maximaler absoluter Linearitätsfehler vs Best fitted straight line: \n\t%1.1f (vor lin) | %1.1f (nach lin)\n" ...
    + "Maximaler relativer Linearitätsfehler, bezogen auf fullscale: \n\t%1.0f: %1.1f%% (vor lin) | %1.1f%% (nach lin)\n" ,...
    maxAbsLinError, maxAppliedCorrectionLinError, fullScale, maxRelLinError*100, maxRelAppliedCorrectionError*100);

%% plot data + (non)linearity error
figure,

subplot(221), hold all
plot(T.x, T.y, '*-', 'linewidth', 1.5, 'DisplayName', sprintf('Messdaten y=f(x), %1.1f%% Lin-Fehler', maxRelLinError*100))
plot(T.x, T.z, 'o-', 'linewidth', 1.5, 'DisplayName', sprintf('Linearisierte Messdaten z=x=f^{-1}(y), %1.1f%% Lin-Fehler', maxRelAppliedCorrectionError*100))
plot(T.x, bestFitEval, '--k', 'linewidth', 0.5, 'DisplayName', 'Best fit straight line')

grid on
xlabel('Wahre Messgröße x')
ylabel('Sensor Ausgabe z')
legend('show','location','southeast')
title('Nicht-Linearisierter vs Linearisierter Sensor')

subplot(222), hold all
plot(T.y, T.x, 'o-', 'linewidth', 1.5, 'DisplayName', sprintf('Ideale Umkehrfunktion x=g(y)'))
plot(T.y, gEval, 'x-', 'linewidth', 1.5, 'DisplayName', sprintf('Approximierte Umkehrfunktion x*=g*(y)'))
% plot(bestFitEval, T.x,  '--k', 'linewidth', 0.5, 'DisplayName', 'Best fit straight line')

grid on
ylabel('Wahre Messgröße x')
xlabel('Sensor Ausgabe y')
legend('show','location','southeast')
title('Umkehrfunktionen')

subplot(223), hold all
plot(T.x, bestFitErr, '*-', 'linewidth', 1.5, 'DisplayName', 'Best Fit Error before Linearization')
plot(T.x, appliedCorrectionError, 'o-', 'linewidth', 1.5, 'DisplayName', 'Best Fit Error after Linearization')

grid on
xlabel('Wahre Messgröße')
ylabel('Sensor Fehler')
legend('show','location','southeast')
title('Vergleich der Fehler')
