close all
clear all

tf = 300;
dt = .1;

Hs = .82;

m = 910; %kg
mu = 7193;  %kg
g = 9.81;
k = m*g;
b = 15000;
Area = 10;
Cd = 2;
rho = 1025;



Ks = 6;
std_e_trg = .18;
    

Ki = 4/tf;
    
%kb = .4:.1:1.2;
%for j = 1:length(kb)

Hs = [.82 .82 .82 .82 .82 .82 1 1 1 1 1 1 1 1 1];
Hs = [.5:.05:1 1:-.05:.5 .5:.01];
HsLast = 0;
scale = NaN(length(Hs),1);
std_e = NaN(length(Hs),1);
scale_offset = 0.6;
scale(1) = scale_offset;
 for j = 1:length(Hs)
     
     
     b = 25000*scale(j);
    close all;
    
    if(Hs(j) ~= HsLast)
      disp(['Computing wave for j == 1  Hs = ' num2str(Hs(j))]);
      clear wave
      wave = IncWave;
      Type = 0;  %Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave
      wave = set(wave,'A',Hs(j)/2);
      wave = set(wave,'Type',Type);
      wave = set(wave,'beta',-1);
      HsLast = Hs(j);
    end
   
%     if(j == 7)
%       clear wave;
%       wave = IncWave;
%       Type = 0;  %Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave
%       wave = set(wave,'A',Hs(j)/2);
%       wave = set(wave,'Type',Type);
%       wave = set(wave,'beta',-1);
%     end
    
    
    clear T Z eta0;
%    clear wave;
    
%    wave = IncWave;
%    Type = 0;  %Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave
%    wave = set(wave,'A',Hs/2);
%    wave = set(wave,'Type',Type);
%    wave = set(wave,'beta',-1);
    
    InitialCond(1) = eta(wave,0,0)-1;
    InitialCond(2) = etadot(wave,0,0);
    
    options = odeset('RelTol',1e-4,'AbsTol',[1e-4 1e-4 1e-5]);
    [T,Z] = ode45(@(t,z) ForceSum(t,z,m,mu,g,Cd,rho,Area,k,b,wave),[0 tf],InitialCond);
    
    
    for i = 1:length(T)
        eta0(i) = eta(wave,0,T(i));
        %etadot0(i) = etadot(wave,0,T(i));
    end
    
    e = eta0'-Z(:,1);

    std_e(j) = std(e)
    
    err = std(e)-std_e_trg;
    %scale(j+1) = Ks*(std(e)-std_e_trg)+scale_offset;
    
    scale(j+1) = scale(j)+Ki*err*tf
    
    
    if(scale(j+1) < .2)
        scale(j+1) = .2;
    end
    if(scale(j+1) > 1.5)
        scale(j+1) = 1.5;
    end
    
    figure
    plot(T,eta0,T,Z(:,1))
    
    figure
    plot(T,e);
    
    figure
    plot(std_e);
    
    figure
    plot(scale);
    
 end


 idx = find(~isnan(std_e));
 time = idx*5;
 figure('Position',[251 165 1324 933],'PaperOrientation', 'landscape','PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 11 8.5]); 
 plot(time,Hs(idx),time,scale(idx),time,std_e(idx),[1 time(end)],[std_e_trg std_e_trg],'--')
 legend('Sig. Wave Height (m)','Damping Scale Factor','Elon. Std Dev (m)','Std. Dev. Target (0.18m)');
 xlabel('minutes')
 foo = axis;
 foo(4) = 2.5;
 axis(foo);
 title('Integral gain only applied to scale factor.  Ki = 0.0133 [1/(m-sec)]');
  print('-dpdf','Integral Gain Results')