close all
clear all



BaseFolder = 'MonoChromaticComparisons';
ResultsFolder = 'latest_batch_results';

results_table = readtable(['../' BaseFolder '/' ResultsFolder '/batch_runs.log']);
table_size = size(results_table);

for i = 1:table_size(1)
  files = dir(['../' BaseFolder '/' ResultsFolder '/' results_table.pblogFilename{i} '/../matlab/*_raw.mat']);
  results_data{i} = load([files.folder '\' files.name]);

disp(num2str(i));
end

      A = [0.125, 0.125, 0.125, 0.25,  0.5,  .75,  1.0];
      T = [4.0,    14.0,  30.0, 10.0,  12.0,  14.0,  14.0];

    WecSimResultsFilenames{1} = '0-25_4-00_withpressures.mat';
    WecSimResultsFilenames{2} = '0-25_14-00_withpressures.mat';
    WecSimResultsFilenames{3} = '0-25_30-00_withpressures.mat';

    WecSimResultsFilenames{4} = '0-50_10-00_withpressures.mat';

    WecSimResultsFilenames{5} = '1-00_12-00_withpressures.mat';
    WecSimResultsFilenames{6} = '1-50_14-00_withpressures.mat';

    WecSimResultsFilenames{7} = '2-00_14-00_withpressures.mat';    

%       A = [0.5];
%       T = [12.0];
% 
%     WecSimResultsFilenames{1} = '1-00_12-00_withpressures.mat';

WecSimResultsFolder = 'Andy_monochro_results3';

for i = 1:length(A)

    WecSimResults = load(['../' WecSimResultsFolder '/' WecSimResultsFilenames{i}]);

N_Plots = 7;
k = 1;
figure('Position',[  134         236         920        1200]);
subplot(N_Plots,1,k); k = k+1;
t = (results_data{i}.XB_Time-results_data{i}.XB_Time(1))*24*3600;
plot(t,A(i)*cos(2*pi*t/T(i)));
hold all;
plot(t,results_data{i}.XB_Alt-mean(results_data{i}.XB_Alt)); 
plot(WecSimResults.time_array,WecSimResults.buoy_heave);
xlabel('time (s)');
ylabel('meters');
grid on;
title(['A = ' num2str(A(i)) 'm   T = ' num2str(T(i)) 's'])
legend('wave height','Gazebo Buoy Pos','WecSim Buoy Pos')

subplot(N_Plots,1,k); k = k+1;
t = (results_data{i}.SC_Time-results_data{i}.SC_Time(1))*24*3600;
pos = results_data{i}.SC_Range;
plot(t,results_data{i}.SC_Range)
hold all
plot(WecSimResults.time_array,39.4*WecSimResults.sim_pistonpos_values{1});
l_pos = length(results_data{i}.SC_Range);
%ylim([0.9*min(pos(l_pos/2:end)) 1.1*max(pos(l_pos/2:end))]);
xlabel('time (s)');
ylabel('Piston Pos (in)');
legend('Gazebo Piston Pos','WecSim Piston Pos')
grid on;

subplot(N_Plots,1,k); k = k+1;
t = (results_data{i}.SC_Time-results_data{i}.SC_Time(1))*24*3600;
pos = results_data{i}.SC_Range;
plot(t(2:end),diff(results_data{i}.SC_Range)./diff(t));
hold all
plot(WecSimResults.time_array,39.4*WecSimResults.sim_pistonvel_values{1});
l_pos = length(results_data{i}.SC_Range);
%ylim([0.9*min(pos(l_pos/2:end)) 1.1*max(pos(l_pos/2:end))]);
xlabel('time (s)');
ylabel('Piston Vel (in/s)');
legend('Gazebo Piston Vel','WecSim Piston Vel')
grid on;

subplot(N_Plots,1,k); k = k+1;
t = (results_data{i}.PC_Time-results_data{i}.PC_Time(1))*24*3600;
rpm = results_data{i}.PC_RPM;
plot(t,rpm)
hold all
plot(WecSimResults.time_array,WecSimResults.sim_motorrpm_values{1});
l_rpm = length(rpm);
%ylim([1.1*min(rpm(l_rpm/2:end)) 1.1*max(rpm(l_rpm/2:end))]);
xlabel('time (s)');
ylabel('RPM');
grid on;
legend('Gazebo RPM','WecSim RPM')

subplot(N_Plots,1,k); k = k+1;
t = (results_data{i}.SC_Time-results_data{i}.SC_Time(1))*24*3600;
plot(t,results_data{i}.SC_UpperPressure);
hold all
plot(WecSimResults.time_array,WecSimResults.spring_U_pressure{1});

plot(t,results_data{i}.SC_LowerPressure);
plot(WecSimResults.time_array,WecSimResults.spring_L_pressure{1});


xlabel('time (s)');
ylabel('psi');
grid on;
legend('Gazebo UpperPressure','WecSim UpperPressure','Gazebo LowerPressure','WecSim LowerPressure')

subplot(N_Plots,1,k); k = k+1;
Au = 0.25*pi*5^2; 
Al = Au-0.25*pi*1.5^2;

GasForce = Al*results_data{i}.SC_LowerPressure-Au*results_data{i}.SC_UpperPressure;
WecSimGasForce = Al*WecSimResults.spring_L_pressure{1}-Au*WecSimResults.spring_U_pressure{1};
idx = round(length(GasForce)/2):length(GasForce);
WecSim_idx = round(length(WecSimGasForce)/2):length(WecSimGasForce);

p = polyfit(results_data{i}.SC_Range(idx),GasForce(idx),1);

WecSim_p = polyfit(39.4*WecSimResults.sim_pistonpos_values{1}(WecSim_idx),WecSimGasForce(WecSim_idx),1);

plot(results_data{i}.SC_Range(idx),GasForce(idx),'x');
hold all
plot(39.4*WecSimResults.sim_pistonpos_values{1}(WecSim_idx),WecSimGasForce(WecSim_idx),'x');
x = xlim;
plot(x,polyval(p,x),'b--');
plot(x,polyval(WecSim_p,x),'r--');

legend(['Gazebo Air Spring Force: ' num2str(p(1),"%.1f") ' lbs/in'],['WecSim Air Spring Force: ' num2str(WecSim_p(1),"%.1f") ' lbs/in']);
xlabel('Piston Position (in)');
ylabel('Gas Spring Force (lbs)');
%title(['A = ' num2str(A(i)) 'm   T = ' num2str(T(i)) 's']);
grid on;



%print(['MonoChromaticResults_A' num2str(A) '_T' num2str(T(i)) '.pdf'],'-dpdf','-fillpage');
end
