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.25,  0.5,  1.0,  1.0,  1.0, 2.0, 2.0];
      T =  [4.0,    10.0, 12.0,  8.0, 10.0, 14.0, 8.0, 10.0];

    WecSimResultsFilenames{1} = 'Andy_monochro_results3/0-25_4-00_withpressures.mat';
    WecSimResultsFilenames{2} = 'Andy_monochro_results3/0-50_10-00_withpressures.mat';
    WecSimResultsFilenames{3} = 'Andy_monochro_results3/1-00_12-00_withpressures.mat';
    WecSimResultsFilenames{4} = 'Andy_monochro_results4/2-00_8-00_withpressures.mat';    
    WecSimResultsFilenames{5} = 'Andy_monochro_results4/2-00_10-00_withpressures.mat';    
    WecSimResultsFilenames{6} = 'Andy_monochro_results3/2-00_14-00_withpressures.mat';    
    WecSimResultsFilenames{7} = 'Andy_monochro_results4/4-00_8-00_withpressures.mat';    
    WecSimResultsFilenames{8} = 'Andy_monochro_results4/4-00_10-00_withpressures.mat';    
%       A = [0.5];
%       T = [12.0];
% 
%     WecSimResultsFilenames{1} = 'Andy_monochro_results3/1-00_12-00_withpressures.mat';

for i =  5 %1:6 %length(A)

    WecSimResults = load(['../' 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)); 
jdx = 1:min([length(WecSimResults.buoy_heave) length(WecSimResults.time_array)]);
plot(WecSimResults.time_array(jdx), ...
     WecSimResults.buoy_heave(jdx));
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(jdx), ...
    39.4*WecSimResults.sim_pistonpos_values{1}(jdx));
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(jdx), ...
    39.4*WecSimResults.sim_pistonvel_values{1}(jdx));
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(jdx), ...
    WecSimResults.sim_motorrpm_values{1}(jdx));
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}.PC_Time-results_data{i}.PC_Time(1))*24*3600;
power = results_data{i}.PC_Voltage.*(results_data{i}.PC_LoadCurr+results_data{i}.PC_BattCurr);
plot(t,power)
hold all
plot(WecSimResults.time_array(jdx), ...
    WecSimResults.sim_power_elec_values{1}(jdx));
%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('Power (W)');
grid on;
legend('Gazebo Elec Power','WecSim Elec Power')

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(jdx), ...
    WecSimResults.spring_U_pressure{1}(jdx));

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


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

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



for i = 5 %1:length(A)

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

N_Plots = 2;
k = 1;
figure('Position',[  134         236         920        1200]);
subplot(N_Plots,1,k); k = k+1;
hold all;
len = length(results_data{i}.PC_RPM);
plot(results_data{i}.PC_RPM(round(len/2):len),results_data{i}.PC_TargCurr(round(len/2):len),'x');
xlabel('RPM   Positive is retraction, Negative is extension');
ylabel('Winding Current (A)');
grid on;
title(['A = ' num2str(A(i)) 'm   T = ' num2str(T(i)) 's'])


t = (results_data{i}.SC_Time-results_data{i}.SC_Time(1))*24*3600;
subplot(N_Plots,1,k); k = k+1;
hold all;
plot(diff(results_data{i}.SC_Range(round(len/2):len))./diff(t(round(len/2):len)),results_data{i}.PC_TargCurr(round(len/2):len-1),'x');
xlabel('Piston Vel (in/s) Positive is extension, Negative is retraction');
ylabel('Winding Current (A)');
grid on;
title(['A = ' num2str(A(i)) 'm   T = ' num2str(T(i)) 's'])
end
