function F = WinchControlledTether(body1,bodies,n,name,p_a1,p_a2)

% This function computes the tether elongation and elongation rate at the
% current timestep and passes this infomation along with the state (position and velocity)
% of the ESP to a routine called "TensionCommand" that implements the
% desired control loop.  The value TensionCommand returns is then used to
% compute the forces and moments on body1 (the ESP).


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 WinchControlledTether');
end

body2 = bodies{j};


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);

F = zeros(1,6);
pos = pos1;
pos(1:3) = pos(1:3)+xo1(1:3)';  %Position relative to global coordinate system.
Tension = TensionCommand(pos,vel1,Elongation,ElongationRate,n*body1.dt);

if(Tension > 0)  %If Tension command is less than zero, tether is slack and no force is exerted.
  F(1) = Tension*s(1);
  F(3) = Tension*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
