function ScallopSwim

SL = .07;
SL = .10;
SL = .07;
alpha_thrust = 6;
alpha_thrust_rad = alpha_thrust*pi/180.0;
alphaT_rad = 10*pi/180.0;
rho = 1020;
g = 9.8;
AR = 1.117 + 3.6*(SL - 0.04);
SW = AR*SL;
RelativeThickness = 0.2;
ST = RelativeThickness*SL;
Av = pi*SL*SW/4.0;
Vs = 0.5*Av*ST;
Mb = Av*(100.0*SL - 3.5);
M = Mb + rho*Vs;
F = 2200*SL; % works
F = 100*SL; % works
F = 10;
%F = 6000000*Vs;
%F = 7000000*Vs;
ncontractions = 14;

xvals = zeros(ncontractions,1);
zvals = zeros(ncontractions,1);
tvals = zeros(ncontractions,1);

% time in milliseconds
deltaT = 0.0001
Tglide = 9000; %250 + 125;
Tthrust = 4000; %250;
Tglide = 4500; %250 + 125;
Tthrust = 2000; %250;

deltaT = 0.001
Tglide = 250 + 125;
Tthrust = 250;

fprintf(1,'Scallop Dimensions: SL=%f SW=%f ST=%f\n',SL,SW,ST);
fprintf(1,'                    Av=%f Volume=%f\n',Av,Vs);
fprintf(1,'Scallop Mass: Mb=%f M=%f\n',Mb,M);
%fprintf(1,'alpha = %f\n',alpha);
%fprintf(1,'CL=%f CD=%f CLalpha=%f\n',CL,CD,(1.21 + 3.8*SL));

V0x = 0.4;
V0z = 0.3;

V = sqrt(V0x^2 + V0z^2);

X0 = 0;
Z0 = 0;

for icontraction = 1:ncontractions+2
    
   alpha = alpha_thrust;
    if (icontraction == 3)
        X0 = 0;
        Z0 = 0;
    end
    if (icontraction > 2)
        xvals(icontraction-2,1) = X0;        
        zvals(icontraction-2,1) = Z0;
        tvals(icontraction-2,1) = (Tglide+Tthrust)*deltaT*(icontraction-2);
    fprintf(1,'%d trajectory velocity = %f %f t=%f position=(%f,%f)\n',icontraction-2,X0/((icontraction-2)*(Tglide+Tthrust)*deltaT),V,(Tglide+Tthrust)*deltaT*(icontraction-2),X0,Z0);
    end
    
    for i = 1:Tglide
        CL = 0.0565 + (1.21 + 3.8*SL)*alpha;
        CD = 0.06 + (1.47*CL^2)/(pi*AR);
        CLp = 0.5*rho*CL*Av/M;
        CDp = 0.5*rho*CD*Av/M;
       
    %    fprintf(1,'Coast Velocity = %f (%f,%f) Position=(%f,%f)\n', V,V0x,V0z,X0,Z0);
        X1 = X0 + V0x*0.001;
        Z1 = Z0 + V0z*0.001;
        if (V0x < 0)
            V1z = V0z + ( -CDp*V*V0z - Mb*g/M)*deltaT;
            V1x = V0x + ( -CDp*V*V0x)*deltaT;
        else
            V1z = V0z + (  CLp*V*V0x - CDp*V*V0z - Mb*g/M)*deltaT;
            V1x = V0x + ( -CLp*V*V0z - CDp*V*V0x)*deltaT;
        end
        V0x = V1x;
        V0z = V1z;
        V = sqrt(V0x^2 + V0z^2);
        X0 = X1;
        Z0 = Z1;
    end
    
    alpha = alpha_thrust;
    for i = 1:Tthrust
        CL = 0.0565 + (1.21 + 3.8*SL)*alpha;
        CD = 0.06 + (1.47*CL^2)/(pi*AR);
        CLp = 0.5*rho*CL*Av/M;
        CDp = 0.5*rho*CD*Av/M;
        
        
        alphaT = abs(asin(V0z/V));
        if (V0x < 0 && V0z > 0)
            alphaT = 0;
        end
        if (V0x > 0 && V0z < 0)
            alphaT = 0;
        end
        if (alphaT > 35.0*pi/180)
            alphaT = 35.0*pi/180;
        end
%        if (alphaT < 5.0*pi/180)
%            alphaT = 5.0*pi/180;
%        end
%        alphaT = 5.0*pi/180;
        
        %    fprintf(1,'Thrust Velocity = %f (%f,%f) Position=(%f,%f)\n', V,V0x,V0z,X0,Z0);
        X1 = X0 + V0x*deltaT;
        Z1 = Z0 + V0z*deltaT;
        
        if (V0x < 0)
            V1z = V0z + ( F*sin(alpha_thrust_rad+alphaT)/M - CDp*V*V0z - Mb*g/M)*deltaT;
            V1x = V0x + ( F*cos(alpha_thrust_rad+alphaT)/M - CDp*V*V0x)*deltaT;
        else
            V1z = V0z + ( F*sin(alpha_thrust_rad+alphaT)/M + CLp*V*V0x - CDp*V*V0z - Mb*g/M)*deltaT;
            V1x = V0x + ( F*cos(alpha_thrust_rad+alphaT)/M - CLp*V*V0z - CDp*V*V0x)*deltaT;
        end
       % fprintf(1,'%f thrust direction = %f %f\n',(alpha_thrust_rad+alphaT),sin(alpha_thrust_rad+alphaT),cos(alpha_thrust_rad+alphaT));
        V0x = V1x;
        V0z = V1z;
        V = sqrt(V0x^2 + V0z^2);
        X0 = X1;
        Z0 = Z1;
    end    
    
end

ds = 0;
for icontraction = 2: ncontractions
    dx = xvals(icontraction,1)-xvals(icontraction-1,1);
    dz = zvals(icontraction,1)-zvals(icontraction-1,1);   
    ds = ds + sqrt(dx*dx + dz*dz);
    fprintf(1,'ds = %f v=%f\n',ds,ds/((icontraction-1)*(Tglide+Tthrust)*deltaT));
end
plot(zvals,xvals);
fprintf('average velocity = %f\n',ds/((ncontractions-1)*(Tglide+Tthrust)*deltaT));