function F = IdealLatchingPTO(body1,bodies,n,name,p_a1,p_a2,Spring_Force_Coeffs,Damping_Coeff,LatchParms,Stiction_Force_Coeffs,dl)

persistent Brake_Coeff;
persistent LatchState;
persistent tpeak;


if isempty(Brake_Coeff)
    Brake_Coeff = 0;
end

if isempty(LatchState)
    LatchState = 0;
end

frac = mod(n,1);
n = floor(n);

j = 0;
for i = 1:length(bodies)
name2 = get(bodies{i},'name');
  if(strcmp(name2,name))
    j = i;
  end
end
if(j == 0)
   error('ERROR: Can not find name match in PTO');
end

body2 = bodies{j};

%disp(['Computing Forces on ',get(body1,'name'),' due to ',get(body2,'name')]);

sv1 = get(body1,'sv');
sv2 = get(body2,'sv');

xo1 = get(body1,'x0');
xo2 = get(body2,'x0');
pos1 = sv1(1:6);
vel1 = sv1(7:12);
pos2 = sv2(1:6);
vel2 = sv2(7:12);

[Elongation,ElongationRate,s] = TetherElongations(xo1,pos1,vel1,p_a1,xo2,pos2,vel2,p_a2);

Elongation
ElongationRate

F1 = get(body1,'Forces');
F2 = get(body2,'Forces');

if(strcmp(get(body1,'name'),'Surface Buoy'))
    Bforce = F1;
    Fforce = F2;
    BuoyPos = pos1;
    t = get(body1,'t');
else
    Bforce = F2;
    Fforce = F1;
    BuoyPos = pos2;
    t = get(body2,'t');
end
    dt = t(2)-t(1);


F_d = Bforce{4}(n,3)
disp(['Buoy Pos = ' num2str(BuoyPos(3)) '  F_d = ' num2str(F_d) '   Sign = ' num2str(sign(F_d*BuoyPos(3)))]);

% %Latching Algorithm that latches when piston stops, and unlatches when
% sign of (ExcitingForce*BuoyVerticalCoord) is negative.
% epsilon = .005;  %Criteria for stopped.
% switch(LatchState)
%     case 0
%        if(abs(ElongationRate) < epsilon)   
%            LatchState = 1
%            Brake_Coeff = LatchParms.MaxBrakeDampingCoeff;
%        end 
%     case 1
%        if(sign(F_d*BuoyPos(3)) < 0)
%           LatchState = 2
%           tpeak=t(n);
%        end
%     case 2
%         if(t(n) > tpeak+LatchParms.tdelay)
%             LatchState = 3
%             tpeak=t(n);
%             Brake_Coeff = 0;
%         end
%     case 3
%         if(t(n) > tpeak+LatchParms.tunlatched)  %Don't relatch immediately
%             LatchState = 0
%         end 
% end

%Latching Algorithm that latches when piston stops, and unlatches when
% Exciting force is positive when piston is retracted,
% and if Exciting force is negative when piston is extended. 
epsilon = .005;  %Criteria for stopped.
switch(LatchState)
    case 0
       if(abs(ElongationRate) < epsilon)   
           LatchState = 1
           Brake_Coeff = LatchParms.MaxBrakeDampingCoeff;
       end 
    case 1
       %if(((Elongation < 1) && (F_d < 0)) || ((Elongation > 1) && (F_d > 0)))  
          LatchState = 2
          tpeak=t(n); 
       %end
    case 2
        if(t(n) > tpeak+LatchParms.tdelay)
            LatchState = 3
            Brake_Coeff = 0;
            tpeak=t(n); 
        end
    case 3
        disp('t(n) = ' num2str(t(n)) '    tpeak = ' num2str(tpeak) '  tunlatched = ' num2str(LatchParms.tunlatched)
        if(t(n) > tpeak+LatchParms.tunlatched)  %Don't relatch immediately
            LatchState = 0
        end 
end


%Latching Algorithm that latches when piston stops, and unlatches when
% the absolute value of the damping force exceeds a threshold.

% epsilon = .005;  %Criteria for stopped.
% switch(LatchState)
%     case 0
%        if(abs(ElongationRate) < epsilon)   
%            LatchState = 1
%            Brake_Coeff = LatchParms.MaxBrakeDampingCoeff;
%        end 
%     case 1
%        if(abs(ElongationRate) > LatchParms.FThres/LatchParms.MaxBrakeDampingCoeff);
%           LatchState = 2
%           tpeak=t(n); 
%        end
%     case 2
%         if(t(n) > tpeak+LatchParms.tdelay)
%             LatchState = 3
%             Brake_Coeff = 0;
%         end
%     case 3
%         if(t(n) > tpeak+LatchParms.tunlatched)  %Don't relatch immediately
%             LatchState = 0
%         end 
% end

dl.Latched(n) = LatchState;

%disp(['Norm_t = ' num2str(Norm_t) '   Latch State = ' num2str(Latched)]);
disp(['current time = ' num2str(t(n)+dt*frac) '  Latched = ' num2str(LatchState)  '  Elon Rate ' num2str(ElongationRate)]);

F = zeros(1,6);

if(Elongation > 0)  %If ram is fully retracted, tether is slack and no force is exerted. 
  SpringTension = ppval(Spring_Force_Coeffs,Elongation);
  F(1) = SpringTension*s(1);
  F(3) = SpringTension*s(3);
   
%  Tension = PTO_Force(n, ElongationRate,dl,Latched)
  Tension = (Damping_Coeff+Brake_Coeff)*ElongationRate;

  
  
  dl.Force(n) = Tension;
  dl.ElonRate(n) = ElongationRate;

  
  F(1) = F(1) + Tension*s(1);
  F(3) = F(3) + Tension*s(3);


  StictionForce = ppval(Stiction_Force_Coeffs,ElongationRate);
  F(1) = F(1) + StictionForce*s(1);
  F(3) = F(3) + StictionForce*s(3);
  
end


R = eye(3);
R(1,1) = cos(pos1(5)); R(1,3) = sin(pos1(5));
R(3,1) = -R(1,3);  R(3,3) = R(1,1);
p_size = size(p_a1);
if(p_size(2) > p_size(1)) 
    p_a1 = p_a1'; 
end
M  = cross(R*p_a1,F(1:3));
F(5) = M(2);

end