function ScallopClimb

SL = .07;
SL = .07;
SL = 0.12;
SL = 0.12;

ncontractions = 6;
    

ScallopLengths = [0.055, 0.07, 0.10, 0.12];
    xvals = zeros(ncontractions,length(ScallopLengths));
    zvals = zeros(ncontractions,length(ScallopLengths));
    tvals = zeros(ncontractions,length(ScallopLengths));

for iScallop = 1: length(ScallopLengths)
    SL = ScallopLengths(iScallop);
    fprintf(1,'Scallop %d Length=%f\n',iScallop,SL);
    
    alpha_thrust = 8;
    alpha_thrust_rad = alpha_thrust*pi/180.0;
    alphaT_rad = 45*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 = Av*ST;
    Mb = Av*(100.0*SL - 3.5);
    M = Mb + rho*Vs;
    JetAngle = 12*pi/180.0;
    
    % time in seconds
    T = 0.3;
    nTbins = 10000;
    deltaT = T/nTbins;
    Tthrust = T/3;
    Tglide = T - Tthrust;
    Tglide = floor(Tglide/deltaT);
    Tthrust = floor(Tthrust/deltaT);
    
        
    Am = 0.054*Av;
    LambdaA = 1.0 - Am/Av;
    LambdaAj = (power((0.065244+0.0068523/SL),2))/3.0;
    BetaMax = (0.3*SL)*pi/180.0;
    DBetaDt = BetaMax/(Tthrust*deltaT);
    
    
    LambdaAj = 1.0/(-4.1094*power(SL*100,2) + 81.592*(SL*100.0) - 243.46);
    LambdaAj = 0.01; %0.0003*power(SL,-1.226);
    
    %F = 100*SL; % works
    %F = 700000*Vs;
    %%F = 7000000*Vs;
    %F = 10; % at 35 deg
    F = 800;    
    
    %F = 3000*rho*power(LambdaA*SL,2)*Av*power(DBetaDt,2)/(2*LambdaAj);

    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,'Force of thrust: %f\n',F);
    fprintf(1,'Lambda Aj %f  %f\n',LambdaAj,LambdaAj*Av);
    %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;
        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;
            
            
            %    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*cos(JetAngle)*sin(alpha_thrust_rad+alphaT_rad)/M - CDp*V*V0z - Mb*g/M)*deltaT;
                V1x = V0x + ( F*cos(JetAngle)*cos(alpha_thrust_rad+alphaT_rad)/M - CDp*V*V0x)*deltaT;
            else
                V1z = V0z + ( F*cos(JetAngle)*sin(alpha_thrust_rad+alphaT_rad)/M + CLp*V*V0x - CDp*V*V0z - Mb*g/M)*deltaT;
                V1x = V0x + ( F*cos(JetAngle)*cos(alpha_thrust_rad+alphaT_rad)/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
        %%alphaT_rad = abs(asin(V0z/V));
        %if (V0x < 0 && V0z > 0)
        %    alphaT_rad = 0;
        %end
        %if (V0x > 0 && V0z < 0)
        %    alphaT_rad = 0;
        %end
        %if (alphaT_rad > 35.0*pi/180)
        %    alphaT_rad = 35.0*pi/180;
        %end
        
        %        if (alphaT < 5.0*pi/180)
        %            alphaT = 5.0*pi/180;
        %        end
        %        alphaT = 5.0*pi/180;
        % alphaT = 35.0*pi/180;
        
        
        
        alpha = alpha_thrust;
        if (icontraction == 3)
            X0 = 0;
            Z0 = 0;
        end
        if (icontraction > 2)
            xvals(icontraction-2,iScallop) = X0;
            zvals(icontraction-2,iScallop) = Z0;
            tvals(icontraction-2,iScallop) = (Tglide+Tthrust)*deltaT*(icontraction-2);
            fprintf(1,'%d %d trajectory velocity = %f %f t=%f position=(%f,%f)\n',iScallop,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
        
    end
end
 figure(1);
 hold on;
 colorvals = ['r', 'g', 'b', 'k'];
 for iScallop = 1: length(ScallopLengths)
     
     ds = 0;
     for icontraction = 2: ncontractions
         dx = xvals(icontraction,iScallop)-xvals(icontraction-1,iScallop);
         dz = zvals(icontraction,iScallop)-zvals(icontraction-1,iScallop);
         ds = ds + sqrt(dx*dx + dz*dz);
         fprintf(1,'ds = %f v=%f\n',ds,ds/((icontraction-1)*(Tglide+Tthrust)*deltaT));
     end
     plot(xvals(:,iScallop),zvals(:,iScallop),'color',colorvals(iScallop));
     fprintf('average velocity = %f\n',ds/((ncontractions-1)*(Tglide+Tthrust)*deltaT));
 end

 xvals
 
 zvals
 