function F = PTO(body1,bodies,n,name,p_a1,p_a2,Spring_Force_Coeffs,Damping_Force_Coeffs,Stiction_Force_Coeffs,dl)

persistent latchtime;
persistent Latched;

if isempty(Latched)
    Latched = 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);


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

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


%Latch Based on Time
Norm_t = mod((t(n)+dt*frac)/12,1);
  Latched = 0;
%if( ((Norm_t > 5/8)  & (Norm_t < 7/8)) | ((Norm_t > 1/8)  & (Norm_t < 3/8)))
%  Latched = 1;
%end
if( ((Norm_t > 4/8)  & (Norm_t < 6/8)) | ((Norm_t > 0/8)  & (Norm_t < 2/8)))
  Latched = 1;
end


%Latched Based on Stoped Piston and time offset.
% epsilon = .005;  %Criteria for stopped.
% ElongationRate
% if((abs(ElongationRate) < epsilon) & ~Latched)
%     Latched = 1;
%     latchtime = t(n);
% end
% if((t(n)-latchtime) > 20/8)
%     Latched = 0;
% end

dl.Latched(n) = Latched;

%disp(['Norm_t = ' num2str(Norm_t) '   Latched = ' num2str(Latched)]);
disp(['current time = ' num2str(t(n)+dt*frac) 'latchtime = ' num2str(latchtime) '   Latched = ' num2str(Latched)  '  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)
  if(~Latched)
      Tension = 20000*ElongationRate;
  else
      Tension = 150000*ElongationRate;
  end
  
  %Tension = 20000*ElongationRate;
  
  dl.Force(n) = Tension;
  dl.Latched(n) = Latched;
  dl.ElonRate(n) = ElongationRate;
  %Tension = ppval(Damping_Force_Coeffs,ElongationRate)

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

Stiction_Force_Coeffs
  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



