close all
clear all



BaseFolder_main = 'MonoChromaticComparisons_main';
BaseFolder_snlWAMIT = 'MonoChromaticComparisons_snlWAMIT';

ResultsFolder = 'latest_batch_results';

results_table_main = readtable(['../' BaseFolder_main '/' ResultsFolder '/batch_runs.log']);
table_size = size(results_table_main);

for i = 1:5 %table_size(1)
  files = dir(['../' BaseFolder_main '/' ResultsFolder '/' results_table_main.pblogFilename{i} '/../matlab/*_raw.mat']);
  results_data_main{i} = load([files.folder '\' files.name]);

disp(num2str(i));
end

results_table_snlWAMIT = readtable(['../' BaseFolder_snlWAMIT '/' ResultsFolder '/batch_runs.log']);
table_size = size(results_table_snlWAMIT);

for i = 1:5 %table_size(1)
  files = dir(['../' BaseFolder_snlWAMIT '/' ResultsFolder '/' results_table_snlWAMIT.pblogFilename{i} '/../matlab/*_raw.mat']);
  results_data_snlWAMIT{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 =  1:5 %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_main{i}.XB_Time-results_data_main{i}.XB_Time(1))*24*3600;
plot(t,A(i)*cos(2*pi*t/T(i)));
hold all;
plot(t,results_data_main{i}.XB_Alt-mean(results_data_main{i}.XB_Alt)); 
plot(t,results_data_snlWAMIT{i}.XB_Alt-mean(results_data_snlWAMIT{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 (main)','Gazebo Buoy Pos (snlWAMIT)','WecSim Buoy Pos')

subplot(N_Plots,1,k); k = k+1;
t = (results_data_main{i}.SC_Time-results_data_main{i}.SC_Time(1))*24*3600;
pos = results_data_main{i}.SC_Range;
plot(t,results_data_main{i}.SC_Range)
hold all
plot(t,results_data_snlWAMIT{i}.SC_Range)

plot(WecSimResults.time_array(jdx), ...
    39.4*WecSimResults.sim_pistonpos_values{1}(jdx));
l_pos = length(results_data_main{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 (main)','Gazebo Piston Pos (snlWAMIT)','WecSim Piston Pos')
grid on;

subplot(N_Plots,1,k); k = k+1;
t = (results_data_main{i}.SC_Time-results_data_main{i}.SC_Time(1))*24*3600;
pos = results_data_main{i}.SC_Range;
plot(t(2:end),diff(results_data_main{i}.SC_Range)./diff(t));
hold all
plot(t(2:end),diff(results_data_snlWAMIT{i}.SC_Range)./diff(t));

plot(WecSimResults.time_array(jdx), ...
    39.4*WecSimResults.sim_pistonvel_values{1}(jdx));
l_pos = length(results_data_main{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 (main)','Gazebo Piston Vel (snlWAMIT)','WecSim Piston Vel')
grid on;

subplot(N_Plots,1,k); k = k+1;
t = (results_data_main{i}.PC_Time-results_data_main{i}.PC_Time(1))*24*3600;
plot(t,results_data_main{i}.PC_RPM)
hold all
plot(t,results_data_snlWAMIT{i}.PC_RPM)
plot(WecSimResults.time_array(jdx), ...
    WecSimResults.sim_motorrpm_values{1}(jdx));
xlabel('time (s)');
ylabel('RPM');
grid on;
legend('Gazebo RPM (main)','Gazebo RPM (snlWAMIT)','WecSim RPM')

subplot(N_Plots,1,k); k = k+1;
t = (results_data_main{i}.PC_Time-results_data_main{i}.PC_Time(1))*24*3600;
power_main = results_data_main{i}.PC_Voltage.*(results_data_main{i}.PC_LoadCurr+results_data_main{i}.PC_BattCurr);
plot(t,power_main)
hold all
power_snlWAMIT = results_data_snlWAMIT{i}.PC_Voltage.*(results_data_snlWAMIT{i}.PC_LoadCurr+results_data_snlWAMIT{i}.PC_BattCurr);
plot(t,power_snlWAMIT)
plot(WecSimResults.time_array(jdx), ...
    WecSimResults.sim_power_elec_values{1}(jdx));

ylim([-100 1000]);

xlabel('time (s)');
ylabel('Power (W)');
grid on;
legend('Gazebo Elec Power (main)','Gazebo Elec Power (snlWAMIT)','WecSim Elec Power')

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

plot(t,results_data_main{i}.SC_LowerPressure);
plot(t,results_data_snlWAMIT{i}.SC_LowerPressure);

plot(WecSimResults.time_array(jdx), ...
    WecSimResults.spring_L_pressure{1}(jdx));


xlabel('time (s)');
ylabel('psi');
grid on;
legend('Gazebo (main) UpperPressure (gas)','Gazebo (snlWAMIT) UpperPressure (gas)','WecSim UpperPressure (gas)','Gazebo (main) LowerPressure (gas)','Gazebo (snlWAMIT) 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_main = Al*results_data_main{i}.SC_LowerPressure-Au*results_data_main{i}.SC_UpperPressure;
GasForce_snlWAMIT = Al*results_data_snlWAMIT{i}.SC_LowerPressure-Au*results_data_snlWAMIT{i}.SC_UpperPressure;

WecSimGasForce = Al*WecSimResults.spring_L_pressure{1}-Au*WecSimResults.spring_U_pressure{1};
idx_main = round(length(GasForce_main)/2):length(GasForce_main);
idx_snlWAMIT = round(length(GasForce_snlWAMIT)/2):length(GasForce_snlWAMIT);
WecSim_idx = round(length(WecSimGasForce)/2):length(WecSimGasForce);

p_main = polyfit(results_data_main{i}.SC_Range(idx_main),GasForce_main(idx_main),1);
p_snlWAMIT = polyfit(results_data_snlWAMIT{i}.SC_Range(idx_snlWAMIT),GasForce_main(idx_snlWAMIT),1);

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

plot(results_data_main{i}.SC_Range(idx_main),GasForce_main(idx_main),'x');
hold all
plot(results_data_snlWAMIT{i}.SC_Range(idx_snlWAMIT),GasForce_snlWAMIT(idx_snlWAMIT),'x');

plot(39.4*WecSimResults.sim_pistonpos_values{1}(WecSim_idx),WecSimGasForce(WecSim_idx),'x');
x = xlim;
plot(x,polyval(p_main,x),'b--');
plot(x,polyval(p_snlWAMIT,x),'b--');

plot(x,polyval(WecSim_p,x),'r--');

legend(['Gazebo (main) Air Spring Force: ' num2str(p_main(1),"%.1f") ' lbs/in'],['Gazebo (snlWAMIT) Air Spring Force: ' num2str(p_snlWAMIT(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_main{i}.PC_RPM);
plot(results_data_main{i}.PC_RPM(round(len/2):len),results_data_main{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_main{i}.SC_Time-results_data_main{i}.SC_Time(1))*24*3600;
subplot(N_Plots,1,k); k = k+1;
hold all;
plot(diff(results_data_main{i}.SC_Range(round(len/2):len))./diff(t(round(len/2):len)),results_data_main{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
