%% please open Magic Formular APP first
clear
clc
n = 20;

%%
directoryPath  = "..\sorted_data";
subdirectories = getSubdirectories(directoryPath);
%%
% Use the dir function to get information about files and directories
contents = dir(directoryPath);

% Filter out the directories (excluding '.' and '..')
subdirectories_list = contents([contents.isdir] & ~ismember({contents.name}, {'.', '..'}));

% Extract and display the names of the subdirectories
subdirectoryNames = {subdirectories_list.name};
%%
% figure()
% for i=1:length(subdirectories)
%     fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%     parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%     measurements = parser.run(fileMeasurement);
%     measurement = measurements(n);
% 
%     [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%     FY = smoothdata(FY, "gaussian",100);
%     if i < length(subdirectories)/3
%         plot(SA, FY, 'LineWidth', 1)
%     elseif i <= length(subdirectories)/3*2
%         plot(SA, FY, '--','LineWidth', 1)
%     else
%         plot(SA, FY, '-.','LineWidth', 1)
%     end
%     SAA{i} = SA;
%     FYY{i} = FY;
%     hold on
% end
% legend(subdirectoryNames{:})
% title("Similarities, n=",n)
% xlabel("Slip Angle")
% ylabel("FY")


% SA-MX
% figure()
% for i=1:length(subdirectories)
%     fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%     parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%     measurements = parser.run(fileMeasurement);
%     measurement = measurements(n);
% 
%     [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%     MX = smoothdata(MX, "gaussian",100);
%     if i < length(subdirectories)/3
%         plot(SA, MX, 'LineWidth', 1)
%     elseif i <= length(subdirectories)/3*2
%         plot(SA, MX,'--','LineWidth', 1)
%     else
%         plot(SA, MX, '-.','LineWidth', 1)
%     end
%     hold on
% end
% legend(subdirectoryNames{:})
% title("Similarities, n=",n)
% xlabel("Slip Angle")
% ylabel("MX")


% SA - MZ
% figure()
% for i=1:length(subdirectories)
%     parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%     fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%     measurements = parser.run(fileMeasurement);
%     measurement = measurements(n);
% 
% 
%     [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%     MZ = smoothdata(MZ, "gaussian",100);
%     if i < length(subdirectories)/3
%         plot(SA, MZ, 'LineWidth', 1)
%     elseif i <= length(subdirectories)/3*2
%         plot(SA, MZ,'--','LineWidth', 1)
%     else
%         plot(SA, MZ, '-.','LineWidth', 1)
%     end
%     SAA{i} = SA;
%     MZZ{i} = MZ;
%     hold on
% end
% legend(subdirectoryNames{:})
% title("Similarities, n=",n)
% xlabel("Slip Angle")
% ylabel("MZ")


% for n=1:1
%     figure()
%     j = 1;
%     for i=8:8%length(subdirectories)
%         fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%         fileMeasurement1 = fullfile("..\sorted_data", subdirectories{i}, "\drivebrake.mat");
%         parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%         try
%             measurements = parser.run(fileMeasurement);
%             measurement = measurements(n);
% 
%             measurements1 = parser.run(fileMeasurement1);
%             measurement1 = measurements1(n);
% 
%             [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%             [SX1,SA1,FZ1,IP1,IA1,VX1,FX1,FY1,MZ1,MY1,MX1,W1,T1] = unpack(measurement1);
% 
%             MX = smoothdata(MX, "gaussian");
%             if i < length(subdirectories)/3
%                 plot(SA, FY, 'LineWidth', 1)
%             elseif i <= length(subdirectories)/3*2
%                 plot(SA, FY, '--','LineWidth', 1)
%             else
%                 plot(SA, FY, '-.','LineWidth', 1)
%             end
%             lengends{j} = subdirectoryNames{i};
%             j = j+1;
%             hold on
%         catch
%             disp(fileMeasurement)
%         end
%     end
%     legend(lengends{:})
%     title("Similarities, n=",n)
%     xlabel("SA")
%     ylabel("MZ")
% end

%% prove FY are the same
for n=1:10:60
    figure()
    for i=9:9%length(subdirectories)
        fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
        fileMeasurement1 = fullfile("..\sorted_data", subdirectories{i}, "\drivebrake.mat");
        parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
        try
            measurements = parser.run(fileMeasurement);
            measurement = measurements(n);

            measurements1 = parser.run(fileMeasurement1);
            measurement1 = measurements1(n);

            [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
            [SX1,SA1,FZ1,IP1,IA1,VX1,FX1,FY1,MZ1,MY1,MX1,W1,T1] = unpack(measurement1);

            MX = smoothdata(MX, "gaussian");
            plot(SA, FY, 'LineWidth', 1)
            hold on 
            plot(SA1, FY1, 'LineWidth', 1)
            legend("Cornering", "drivebrake")
        catch
            disp(fileMeasurement)
        end
    end
    title("Similarities, n=",n)
    xlabel("SA")
    ylabel("FY")
end
%% without plot
% mkdir('similarityDataFY');
% for n=1:60
%     for i=1:length(subdirectories)
%         fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%         parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%         measurements = parser.run(fileMeasurement);
%         measurement = measurements(n);
% 
%         [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%         FY = smoothdata(FY, "gaussian",100);
%         SAA{i} = SA;
%         FYY{i} = FY;
%     end
%     file_name = ['similarityDataFY\nEqu' num2str(n) '.mat'];
%     save(file_name, 'SAA', 'FYY');
% end


% mkdir('similarityDataFX');
% for n=1:60
%     for i=1:length(subdirectories)
%         fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%         parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%         measurements = parser.run(fileMeasurement);
%         measurement = measurements(n);
% 
%         [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%         FX = smoothdata(FX, "gaussian",100);
%         SAA{i} = SA;
%         FXX{i} = FX;
%     end
%     file_name = ['similarityDataFX\nEqu' num2str(n) '.mat'];
%     save(file_name, 'SAA', 'FXX');
% end
% 
% mkdir("similarityDataMZ")
% for n=1:60
%     for i=1:length(subdirectories)
%         fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
%         parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
%         measurements = parser.run(fileMeasurement);
%         measurement = measurements(n);
% 
%         [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
%         MZ = smoothdata(MZ, "gaussian",100);
%         SAA{i} = SA;
%         MZZ{i} = MZ;
%     end
%     file_name = ['similarityDataMZ\nEqu' num2str(n) '.mat'];
%     save(file_name, 'SAA', 'MZZ');
% end

mkdir("similarityDataMX")
for n=1:60
    for i=1:length(subdirectories)
        fileMeasurement = fullfile("..\sorted_data", subdirectories{i}, "\cornering.mat");
        parser = tydex.parsers.FSAETTC_SI_ISO_Mat();
        measurements = parser.run(fileMeasurement);
        measurement = measurements(n);

        [SX,SA,FZ,IP,IA,VX,FX,FY,MZ,MY,MX,W,T] = unpack(measurement);
        MX = smoothdata(MX, "gaussian",100);
        SAA{i} = SA;
        MXX{i} = MX;
    end
    file_name = ['similarityDataMX\nEqu' num2str(n) '.mat'];
    save(file_name, 'SAA', 'MXX');
end