%
% Purpose:  Estimate drag and added mass of the heave cone from the Sept
%           2018 tank tests.
%
% Date: 18 Sept 2018
%
  co = [0 0 1;
        0 0.5 0;
        1 0 0;
        0 0.75 0.75;
        0.75 0 0.75;
        0.75 0.75 0;
        0.25 0.25 0.25];
  set(groot,'defaultAxesColorOrder',co)

  if( exist( 'position' ) ~= 1 )
    MonitorPositions = get(0,'MonitorPositions');
    position = MonitorPositions(end,:);
    npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
    npposition(3:4) = round([position(3)*.95 position(4)*.90]);
    position = npposition;
  end
  etaFontsize=25;
  etaFontweight='normal';
  
  %  
  if( exist( 'figPtr' ) ~= 1 )
    figPtr= figure;
    set( figPtr, 'Name', 'Sept 2018 Tank Data', 'DeleteFcn', 'clear figPtr' );
    set(gcf,'position',position)
    set(gca,'fontsize',etaFontsize,'fontweigh',etaFontweight)
    set(gcf,'defaultaxesfontsize',etaFontsize,'defaulttextfontsize', ...
	    etaFontsize)
  else
    try
      figure(figPtr);
    catch
      %
      % The figure has apparently been deleted, but the
      % handle remains.  Delete the handle and start over.
      %
      clear figPtr;
      figPtr = figure;
    end
  end

  units;
%
% Most of the values below are given in English units, then converted to mks.
%
  fs = 25;                        %Hz,  sampling frequency.
  offset = 36*in2m;               %m,  center of Ram stroke.
  wweight = 1872*lbf2n;           %lbf  weight in water of the plate and
                                  %chain, plus the ram in air.
  mass = 1798*lbf2kg;             %lbf  plate, incl lead and motor, in air.
                                  %Converted to mass in kg.
  mRam = 140*lbf2kg;              %lbf  weight of Ram in air, converted to mass.
  mChain = 40*lbf2kg;             %lbf  weight of chain in air, converted to mass.
  sPlate = 124/2*in2m;            %in   length of a side.

  % For a hexagon:
  areaPlate = 3*sqrt(3)/2* sPlate^2;
  dia = 2*sqrt(areaPlate/pi);     %in   diameter of equivalent circle.

  %
  % Estimate weight of chain+Ram from known plate displacement:
  plateDisp = .110;              %m^3 Plate, incl lead and motor, displacement.
  
  RamAndChain = wweight - ( mass - plateDisp*rho )*g;
  %
  % Consider it as mass:
  RamAndChain = RamAndChain/g;

  %
  % Load the data
  if( exist('door') ~= 1 ) door = 'closed'; end
  
  if( strncmpi( door, 'closed', 2 ) )
    data=csvread('9-17-2018-SineMoves-ClosedDoors-1.csv');
    % load arrays of segment indices and commanded frequencies.
    closed;
    door = 'Closed';
  elseif( strncmpi( door, 'open', 2 ) )
    data=csvread('9-17-2018-SineMoves-OpenDoors-1.csv');
    % load arrays of segment indices and commanded frequencies.
    open;
    door = 'Open';
  else
    fprintf('Unrecognized door state.  Exit.\n');
    return
  end
  
  %force = -(data(:,3)*lbf2kg - wweight);
  force = data(:,3)*lbf2n - wweight;
  pos   = data(:,2)*in2m   -offset;
  
  %
  % Filter the force data:
  [bb aa] = butter(6, .5/(fs/2));
  filtForce = filtfilt(bb,aa, force);
  
  htop = subplot(2,1,1);
  %plot(data(:,1), pos);
  hpos = plot(pos/in2m);
  grid on
  hold on
  ylabel('Inches')
  title('RAM Displacement');
  set(gca,'xminorgrid','on')

  hbottom = subplot(2,1,2);
  %plot(data(:,1), [force filtForce])
  hforce = plot([force filtForce]/lbf2n);
  grid on
  hold on
  %xlabel('Seconds')
  xlabel('Index')
  ylabel('Pounds')
  title(['Force, Door ', door])
  set(gca,'xminorgrid','on')
  
  linkaxes(get(gcf,'children'),'x')
  
  %
  % Estimate sinusoidal amplitude and phase for each segment.  Rename
  % pos(indxC{i}) to z to look like the paper.  Assume:
  %
  % z = A sin( omega t + theta ) + C
  %   = a sin( omega t ) + b cos( omega t ) + C
  %
  for( i = 1:length(indxC) )
    %
    omega  = 2*pi*fp(i);
    omegat = omega * data(indxC{i},1);
    z = pos(indxC{i});
    %
    coefs(:,i) = [sin(omegat) cos(omegat) ones(length(indxC{i}),1)]\z;
    %
    A(i,1) = norm(coefs(1:2,i));
    theta(i,1) = atan2(coefs(2,i), coefs(1,i));
    ramOffset(i,1) = coefs(3,i);
    %
    plot(htop, indxC{i}, ...
	 (   A(i)*sin( omegat + theta(i) ) + ramOffset(i)   )/in2m, 'r');
    %
    % Morison: estimate drag and added mass:
    %
    zdot      =  omega*A(i)    *  cos(omegat + theta(i));
    zdotdot   = -omega^2*A(i)  *  sin(omegat + theta(i));
    %
    % Hydrodyamic force
    fh = filtForce(indxC{i}) - ((mass + RamAndChain)*zdotdot);
    
    Coefs(:,i) = [zdot.*abs(zdot)  zdotdot]\fh ...
	.* [8/(rho*pi*dia^2); 1 ];
%	.* [8/(rho*pi*dia^2); 6/(rho*pi*dia^3) ];
    
  end
  fileName = ['Door',door,'.txt'];
  system(['rm ',fileName])
  diary(fileName)
  fprintf('Door is %s.\n',door);
  printCoefs = [ [1:length(Coefs)]' A/in2m 2*pi*fp.*A/in2m  fp 2*pi*A/dia Coefs' ramOffset/in2m];
  fprintf('Segment #   Amp (in) Speed (in/sec) Freq (Hz)     KC     Cd      m_Added (kg) Ram Offset (in)\n');
  fprintf('    %2d     %6.2f    %6.2f           %5.3f      %5.3f  %7.4f   %8.2f       %4.2f\n',printCoefs');
  diary off
  
  
  
  
  
  
  
  
