predSIM.m

function dy = predSIM(t,y,s)

% predSIM: computes the derivatives involved in solving a system of ODEs
% which describe the position and orientation of a rigid body. This
% simulates a fish swimming through water.  
%
% The global variale 's' is a structure with parameter values
% 
% The inputs (t,y,s) are fed into fin_kine, which then outputs the lift
% vector generated by the motion of the fin and and updated structure 's'

% global s

%s.local_transform = 1;

% Time within tail beat
tBeat = t-s.tCurr;

% Unpack state variables
bod_x  = y(1);
Vbod_x = y(2);
bod_y  = y(3);
Vbod_y = y(4);
theta  = y(5);
dTheta = y(6);
pitch  = y(7);
heave  = y(8);
cLift  = y(11);

% Current rate of pitch
[p_prime,p_prime2] = differentiate(s.fPitch,tBeat);

% Current rate of heave
[h_prime,h_prime2] = differentiate(s.fHeave,tBeat);

% Current rate of change in lift coefficient
if tBeat==0
    cLift_prime = 0;
else
    cLift_prime  = differentiate(s.cLift,tBeat);
    %cLift_prime = 0;
end

% Calculate force in local FOR
if s.local_trans
        
    % Calculate forces in local FOR
    [lift,drag,torque] = local_forces(s,bod_x,bod_y,theta,dTheta,pitch,heave,...
                         p_prime,h_prime,Vbod_x,Vbod_y,cLift);
                     
    
% Otherwise, use global FOR method
else
    % Lift generated by fin, given in (x,y) components inertial FOR
    [lift,torque,drag,drag_theta] = fin_kine(s,theta,dTheta,pitch,heave,...
                                        p_prime,h_prime,Vbod_x,Vbod_y,cLift);
    
    % Add up force/torque on body (global FOR)                               
    torque = (torque + drag_theta);
end

% Preallocate derivative vector for system of equations
dy = zeros(10,1);

% x-velocity of body
dy(1) = Vbod_x;

% x-acceleration of body
dy(2) = (lift(:,1) + drag(:,1))./ s.mass;
      
% y-velocity of body
dy(3) = y(4);

% y-acceleration of body
dy(4) = (lift(:,2) + drag(:,2)) ./ s.mass;

% Rate of body rotation (assume COM of body is anterior to its midpoint)
dy(5) = dTheta;

% Body rotational acceleration
%dy(6) = (torque + drag_theta) ./ (s.bodyI);
dy(6) = torque ./ s.bodyI;

% Rate of change in fin pitch 
dy(7) = p_prime;

% Rate of change in fin heave
dy(8) = h_prime;

% Acceleration of fin pitch
dy(9) = p_prime2;

% Acceleration of heaving
dy(10) = h_prime2;

% Rate of change in lift coefficient
dy(11) = cLift_prime;

end


% function p_prime = give_pitchRate(t,s)
%     
%     % Angular frequency of fin (rad/time);
%     omega = 2*pi*s.tailFreq;
%     
%     % Function to modify shape of sine function
%     % Note: turnDirec affects which direction the tail swings)
%     %f1 = s.turnDirec*(10*(t-0.8).^5);
%     f1 = s.turnDirec * exp(t.*s.tailFreq*2);
%     
%     % Derivative of f1
%     %f1_dot = s.turnDirec*(50*(t-0.8).^4);
%     f1_dot = 2 .* s.tailFreq * exp(t.*s.tailFreq*2);
% 
%     % Phase lag (between pitch and heave, pitch leads) (rad)
%     psi = s.psi;
% 
%     pitch = s.pitch0 * f1 .* sin(omega * t + psi);
%     
%     p_prime = s.pitch0 * omega * f1 .* cos(omega*t+psi) + ...
%               s.pitch0 * sin(omega*t+psi) .* f1_dot;
% end
% 
% 
% function h_prime = give_heaveRate(t,s)
%     h_prime = h0 * omega * cos(omega * t);
% end