function T = TensionCommand(p,v,TetherOut,TetherRate,t)

% This function implements a depth control loop for the ESP by returning a
% tension (torque) target for the motor controller based upon the current depth of
% the ESP and the target depth.
% Arguments:    p = 6 dof position of the ESP relative to the global coordinate system.
%               v = 6 dof velocity of the ESP relative to the global coordinate system.
%               TetherOut = current amount of tether payed out.
%               TetherRate = current payout speed.  Positive equal payout.
%               t = current time in seconds.

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

%   global gTC_k gTC_b
%   
%   k = 1000;           %Proportional gain (N/m);
%   b = 100;
%   
%   gTC_k = k;
%   gTC_b  = b;
%   
%   T_Lim = [0 900]*4.45;  %Min and Max allowable tensions (N)  
%   depth_error = TargetDepth-p(3);
%   depth_error_rate = TargetDepthRate-v(3);
% 
%   T = k*depth_error+b*depth_error_rate;
%   
%   if T<T_Lim(1)
%     T = T_Lim(1);
%   end
%   if T>T_Lim(2)
%     T = T_Lim(2);
%   end

global PIDValues
persistent yold TC_P TC_I TC_D TC_index

if(isempty(yold))
    yold = 0;
end
if(isempty(TC_I))
    TC_I = 0;
end
if(isempty(TC_D))
    TC_D = 0;
end
if(isempty(TC_index))
    TC_index = 1;
end
 
T_Lim = [50 900]*4.45;  %Min and Max allowable tensions (N)  
K = 500;
h = .1;
b = 1;
Ti = 1.0;
Td = 0;
N = 8;
Tt = sqrt(Ti * Td);
if( Tt == 0 )
    Tt = 1.0;
end

    
bi = K * h / Ti;
ad = ((2 * Td) - (N * h)) / ((2 * Td) + (N * h));
bd = (2 * K * N * Td) / ((2 * Td ) + (N * h));
a0 = h / Tt;
    
    ysp = TargetDepth;
    y = p(3);
    
    TC_P = K * ((b * ysp) - y);
    TC_D = (ad * TC_D) - (bd * (y - yold));
    TCv = TC_P + TC_I + TC_D;
    
    if(TCv < T_Lim(1))
        u = T_Lim(1);
    elseif(TCv > T_Lim(2))
        u = T_Lim(2);
    else
        u = TCv;
    end
    
    T = u;
    
    PIDValues(TC_index,:) = [ TC_P TC_I TC_D TCv T ]; 
    TC_index = TC_index + 1;
       
    yold = y;
    TC_I = TC_I + (bi * (ysp - y)) + (a0 * (u - TCv));