% Experimentaldaten-basierte Modellierung des Dreifachpendels // Experimental data-based modeling of the triple pendulum 
% Max-Planck-Institut Magdeburg 
% Kevin Jakisch in co-operation with Jun. Prof. Jan Heiland

function [phi1_meas, phi2_meas, phi3_meas, s_meas, time] = Auswertung_Messdaten(Versuch, sampletime, length_V)

%% Trajektorien/Experimente // trajectories/experiments
V_Ramp = [01 02 04 05 06 07 08 09];            %Trajektorien Rampe // trajectories ramp
V_Const = [11 12 13 14 17 18];                 %Trajektorien Konstante // trajectories constant
V_Sinus = [23 24 25 27 28 29 30 31];           %Trajektorien Sinus-Welle // trajectories sine wave
V_ChirpSignal = [34];                          %Trajektorien Chirp Signal // trajectories chirp signal
V_All = [V_Ramp, V_Const, V_Sinus, V_ChirpSignal];

nv = length(V_All);                            %Anzahl aller Messversuche // Number of all experiments

%% Einlesen der Messdaten // Reading in the measurement data
Mess = cell(nv,5);  %Cell-Array zum Speichern der Messdaten // Cell array for saving the measurement data

for k=1:length(V_Ramp)
string = strcat('exp1_00', int2str(V_Ramp(k)), '.mat');
load(strcat('./messdata-211019/', string));
Data = eval(strcat('exp1_00', int2str(V_Ramp(k)))); 
Mess{k,1} = Data.Y(1).Data-Data.Y(1).Data(1); %Phi 1 %Offset von Phi 1 entfernen // Remove offset from phi 1
Mess{k,2} = Data.Y(2).Data; %Phi 2
Mess{k,3} = Data.Y(3).Data; %Phi 3
Mess{k,4} = Data.Y(4).Data; %s
Mess{k,5} = Data.X.Data;    %Zeitvektor // time vector
end

for k=length(V_Ramp)+1:nv
string = strcat('exp1_0', int2str(V_All(k)), '.mat');
load(strcat('./messdata-211019/', string));
Data = eval(strcat('exp1_0', int2str(V_All(k)))); 
Mess{k,1} = Data.Y(1).Data-Data.Y(1).Data(1);
Mess{k,2} = Data.Y(2).Data;
Mess{k,3} = Data.Y(3).Data;
Mess{k,4} = Data.Y(4).Data;
Mess{k,5} = Data.X.Data;
end

%% Cutten der Messdaten // Cutting the measurement data
if length_V <= 50000
    length_short = length_V;
    length_middle = length_V;
    length_long = length_V;
elseif 50000 <= length_V && length_V <= 100000
    length_short = 50000;
    length_middle = length_V;
    length_long = length_V;
else 
    length_short = 50000;
    length_middle = 80000;
    length_long = length_V;
end

Mess_new = cell(nv,5); %Cell-Array zum Speichern der verarbeiteten Messdaten
                       %Cell array for saving the edited measurement data
for i = 1:5
% Rampe & Konstantes Eingangssignal // Input signal: ramp & constant
Mess_new{1,i} = Mess{1,i}(33001:length_short); %33001:80000
Mess_new{2,i} = Mess{2,i}(33001:length_short); %33001:65000
Mess_new{3,i} = Mess{3,i}(33001:length_middle); %33001:98000
Mess_new{4,i} = Mess{4,i}(33001:length_short); %33001:55000
Mess_new{5,i} = Mess{5,i}(33001:length_short); %33001:81000
Mess_new{6,i} = Mess{6,i}(33001:length_short); %33001:62000
Mess_new{7,i} = Mess{7,i}(33001:length_middle); %33001:100000
Mess_new{8,i} = Mess{8,i}(33001:length_short); %33001:54500 
Mess_new{9,i} = Mess{9,i}(33001:length_short); %33001:56000
Mess_new{10,i} = Mess{10,i}(33001:length_short); %33001:77000
Mess_new{11,i} = Mess{11,i}(33001:length_long); %33001:245000
Mess_new{12,i} = Mess{12,i}(33001:length_short); %33001:54000
Mess_new{13,i} = Mess{13,i}(33001:length_short); %33001:77000
Mess_new{14,i} = Mess{14,i}(33001:length_long); %33001:250000

% Sinus-Welle // sine wave
Mess_new{15,i} = Mess{15,i}(33001:length_long); %33001:end
Mess_new{16,i} = Mess{16,i}(35005:length_long); %35005:end
Mess_new{17,i} = Mess{17,i}(33001:length_long); %33001:end
Mess_new{18,i} = Mess{18,i}(33001:length_long); %33001:end
Mess_new{19,i} = Mess{19,i}(33001:length_long); %35005:end
Mess_new{20,i} = Mess{20,i}(33001:length_long); %33001:end
Mess_new{21,i} = Mess{21,i}(33001:length_long); %33001:end
Mess_new{22,i} = Mess{22,i}(32840:length_long); %32840:end

% Chirp Signal // chirp signal
Mess_new{23,i} = Mess{23,i}(33001:length_short); %33001:70000
end

Mess_interpolated = cell(nv,4); 

%% Interpolieren der Messdaten // Interpolating the measurement data
for k=1:nv
    time_value{k,1} = Mess_new{k,5};
    for i = 1:4
        tmpMess_new1{k,i} = diff(Mess_new{k,i});                         
        tmpfindMess_new1{k,i} = find(tmpMess_new1{k,i} ~= 0);
        tmpMess_new2{k,i} = circshift (tmpfindMess_new1{k,i}', +1)';    %damit der 1. neue Wert für die Interpolation genommen wird und nicht der letzte alte Wert
                                                                        %so that the first new value is taken for the interpolation and not the last old value
        tmpMess_new2{k,i}(1) = 1;                                       %damit von dem Anfangswert aus interpoliert wird 
                                                                        %so that is interpolated from the initial value
        Mess_test{k,i} = Mess_new{k,i}(tmpfindMess_new1{k,i});
        timeMess{k,i} = time_value{k,1}(tmpMess_new2{k,i});

        interval{k,i} = time_value{k,1}(1):sampletime:time_value{k,1}(end);
        Mess_interpolated{k,i} = interp1(timeMess{k,i},Mess_test{k,i},interval{k,i},'linear');
        interval{k,i} = interval{k,i} -interval{k,i}(1);                %damit die Zeit bei 0 beginnt // %so that the time starts at 0
        Mess_interpolated_new{k,i} = Mess_interpolated{k,i}(~isnan(Mess_interpolated{k,i}(:))); %NaN-Werte entfernen // remove NaN
    end
end

%% NaN-Werte entfernen // remove NaN 
for k=1:nv 
    for i = 1:4
        length_vec(k) = min(min(length(Mess_interpolated_new{k,1}),length(Mess_interpolated_new{k,2})),min(length(Mess_interpolated_new{k,3}),length(Mess_interpolated_new{k,4})));
        Mess_interp_new{k,i} = Mess_interpolated_new{k,i}(1:length_vec(k));
        interval_new{k,i} = interval{k,i}(1:length_vec(k));
    end
end

%% Ausgabe der Funktionsparameter // Output of the function parameters
phi1_meas = Mess_interp_new{Versuch,1}; %Phi 1
phi2_meas = Mess_interp_new{Versuch,2}; %Phi 2
phi3_meas = Mess_interp_new{Versuch,3}; %Phi 3
s_meas = Mess_interp_new{Versuch,4}./5411; %Position des Wagens s // position of the shuttle s
time = interval_new{Versuch,1}; %Zeit // time
end
