clear
clc
close all


% Data Parameter zuordnen
LD = load('Winkelverlauf_Video2.mat');
T = LD.T;
t = T.t(1:2710)/8;  % 240fps/30fps transform slowmotion to sek
theta = T.theta(1:2710);
theta = fillmissing(theta,'linear');
x = t;    

[pks,locs] = findpeaks(theta); %pks = peak values y-achse ; locs = pks index locations in the original vector x-Achse

%Eigenkreisfrequenz
omega0 = mean(diff(x(locs)));

% Periodendauer
T_0 = 2*pi/omega0 ;

% Anliegende an Schwingung pplotten
env = envelope(theta, 250, 'peak');
figure
plot(x, theta)
hold on
plot(x,env)
hold off
grid
legend('Signal','Upper Envelope', 'Location','best')


yc(:,1) = theta; % spaltenvektor zu Zeilenvektor 
[mv,ix] = max(yc); % [y-wert,index] = maximaler auslenkung
lcs = find(islocalmax(yc(yc(:,1)>0), 'MinProminence',0.05)); %alle maxima 
lims = [ix max(lcs)];
yc = yc(lims(1):lims(2));  % lims 1 = 91 ;lims 2 = 1454
x = linspace(0, numel(yc)-1, numel(yc))';

for k = 1:size(yc,2)

    yk = yc(:,k);
    y = yk;
    % y = detrend(y);
    ym = mean(y);                                                                   % Estimate offset
    y = y - ym;
    yu = max(y);
    yl = min(y);
    yr = (yu-yl);                                                                   % Range of ‘y’
    yz = y-yu+(yr/2);
    zci = @(v) find(diff(sign(v)));                                                 % Returns Approximate Zero-Crossing Indices Of Argument Vector
    zt = x(zci(y));
    per = 2*mean(diff(zt));                                                         % Estimate period
    fit = @(b,x)  b(1) .* exp(b(2).*x) .* (sin(2*pi*x./b(3) + 2*pi/b(4))) + b(5);   % Objective Function to fit
    fcn = @(b) norm(fit(b,x) - yk);                                                 % Least-Squares cost function
    s0 = [yr; -1E-2;  per;  -1;  ym];                                               % Initial Parameter Estimates
    [s,nmrs] = fminsearch(fcn, s0);                                                  % Minimise Least-Squares
    xp = linspace(min(x),max(x), 500);


    figure
    plot(x,yk,':b', 'LineWidth',1.5)
    hold on
    plot(xp,fit(s,xp), '--r')
    hold off
    grid
    xlabel('Time (s)')
    ylabel('Amplitude')
%     title(ttlstr{k})
    legend('Original Data',  'Fitted Curve')
    text(0.1*max(xlim),0.7*min(ylim), sprintf('$y = %.3f\\cdot e^{%.3f\\cdot x}\\cdot sin(2\\pi\\cdot x\\cdot %.3f%+.3f) %+.4f$', [s(1:2); 1./s(3:4); s(5)]), 'Interpreter','latex')
end