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

persistent FirstBodyCalledName;
if(isempty(FirstBodyCalledName))
  disp('Setting FirstBodyCalledName');
  FirstBodyCalledName = get(body1,'name');
end

persistent Tension;
persistent cable_out;
if(isempty(cable_out))
    cable_out = 10;
end

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

disp(['n = ' num2str(n) '   Computing tether force on ' body1.name ' due to ' body2.name]);


frac = mod(n,1);
n = floor(n);
dt = get(body1,'dt');



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

if(strcmp(get(body1,'name'),FirstBodyCalledName))
    
  disp(['Elon = ',num2str(Elongation)]);
  disp(['Elon Rate = ',num2str(ElongationRate)]);

  pos(1:3) = pos2(1:3)+xo2(1:3)';  %Position of ESP relative to global coordinate system.

  [TargetDepth TargetDepthRate] = TargetDepth_Profiling(n*dt);  %Get Target depth for this time.

 
  k = 1000;
  Tension = k*(TargetDepth-pos(3));
  
  %k = 1;       %Proportional gain (m/s per m);
  %b = .1;       %Derivative gain (m/s per m/s);
  
  
  %Vmin = [0]*4.45;  %Min Tension Request (N) 
  %Vmax = [500] *4.45;
  %depth_error = TargetDepth-pos(3);
  %depth_error_rate = 0; %TargetDepthRate-v(3);

  %V = k*depth_error+b*depth_error_rate;


  %disp(['depth_error = ',num2str(depth_error)]);
  %disp(['V = ',num2str(V)]);

  %cable_out_loc = cable_out+V*(1-frac)*dt;

  %kk = 1000;
  %Tension = -kk*(cable_out_loc-Elongation);
  %disp(['n = ',num2str(n), ' frac = ',num2str(frac)]);
  %disp(['cable_out = ',num2str(cable_out)]);
  %disp(['cable_out_loc = ',num2str(cable_out_loc)]);
  %disp(['Elongation = ',num2str(Elongation)]);
  %disp(' ');

  %if(frac == 0)
  %  disp(['setting cable_out ',num2str(cable_out_loc)]);
  %  cable_out = cable_out_loc;
  %end
  
  Tmax = TensionVSpeed(ElongationRate)
   
  if((ElongationRate <= 0) && (Tension > Tmax))  %Paying in to fast, reduce torque request
          Tension = Tmax;
  end

 if((ElongationRate > 0) && (Tension > Tmax))  %Paying out to fast, increase torque request
          Tension = Tension+(.5-ElongationRate)*2000;
  end
  
end


F = zeros(1,6);
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
