%% Numerical Analysis of the Dynamics of Single and Double Spring-Pendulum Systems by Paula Chen and Aaron Lewin

%%
% Paula Chen and Aaron Lewin
% Math 53, Final Project,
% Professor Barnett, Fall 2015

% This code has adapted portions of code provided by Professor Barnett for
% Math 53
% Depends on functions:
% Elastic_Pend_time1map.m and Double_Spring_time1map.m
% Adjustable Constants for User to Determine:
%   Time Vector t
%   All Flag
%   Matrix of Starting X and Y Positions
%   Epsilon

%% Section A: Constant and Flag Declarations

% Constant Values
clear all;
close all;

g = 3; % Constant m/s^2 on a made up planet
L = 1.1; % Natural Spring Length in Meters
k = 10; % Spring Constant in N/m
m = 1; % mass 1 kg
D = 2; % Distance between Spring fasteners in meters.
t = [0 50]; % Starting Time and Ending Time

% Flags
springFLAG = 1; % if 1, single spring, if 2 double spring
plotFLAG = 0; % plots orbits: 1 plots, 0 no plots
lyapFLAG = 0; % 1 calculates lyapunov exponents by jacobean, 0 does not
logFLAG = 0; % 1 plots sensitive dependence test, 0 does not
heatmapFLAG = 1; % 1 plots heatmap of Lyapunov Exp vs inital conditions

% Initial Conditions
startX = [0, .1, .3, .5, 1, 1.25];   % X = q1 at time 0.
startY = [0, m*g/k, 2/3, 1, 1.5 ,2];  % Y = q2 at time 0.

ep = 1e-8;     % Epsilon for sensitive dependence.

%% Section B: Single Spring Code

% Sets up the Hamilton Equations of Motion
if springFLAG == 1
        disp('Single Spring System');

    % For Clarity z(1) = q1; z(2) = q2; z(3) = p1; z(4) = p2;
    
    F = @(t, y) [y(3)/m;
        y(4)/m;
        -k*y(1)+k*L*y(1)*(y(1)^2+(y(2)-D)^2)^(-1/2);
        -k*(y(2)-D)+k*L*(y(2)-D)*(y(1)^2+(y(2)-D)^2)^(-1/2)- m*g];
end
if springFLAG == 2
    %Double SPRING
        disp('Double Spring System');

    % Clarity z(1) = x; z(2) = y; z(3) = px; z(4) = py;
    F = @(t, z) [z(3)/m;
        z(4)/m;
        -2 * k * z(1) + k * L * z(1) * (z(1)^2 + z(2)^2)^(-.5) ...
        + k * z(1) * L * (z(1)^2 + (z(2) - D)^2)^(-.5);
        -2 * k * z(2) + D * k + k * L * z(2) * (z(1)^2+z(2)^2)^(-.5)...
        + k * L * (z(2) - D) * (z(1)^2 + (z(2) - D)^2)^(-.5) - m * g];
end


% Nested Loops run through all combinations of inital conditions
for ICx = 1:length(startX)
    for ICy = 1:length(startY)
        y0 = [startX(ICx); startY(ICy); 0; 0];
        y1 = [startX(ICx)+ep; startY(ICy) + ep; 0; 0];
        
        % Solves Hamiltons Equations of Motions
        options = odeset('AbsTol',1e-10);
        sol = ode45(F, t, y0, options); ts = sol.x; ys = sol.y;
        sol1= ode45(F, t, y1, options); t1 = sol1.x; y1 = sol1.y;
        
        % Removes uniform timesteps out of ode solver
        tp = 0:0.05:max(t);
        yp = deval(sol,tp)';  % Reshapes matrix properly
        yp1 = deval(sol1,tp)'; % Reshapes matrix properly
        
        if plotFLAG == 1
            disp('Plots Orbits with Given Starting Conditions...');

            % Plots contours of Potential Energy and Orbit of Mass.
            if springFLAG == 1
                x = linspace(-3,3,500);
                y = linspace(-1,4,500);
                [X,Y] = meshgrid(x,y);
                % Potential Energy
                PE = m*g*Y+.5*k*(sqrt(X.^2+(Y-D).^2)-L).^2;
            end
            if springFLAG ==2
                X = linspace(-1.5,1.5,100);
                Y = linspace(-.5,2,100);
                [X,Y] = meshgrid(X,Y);
                PE = k/2 * ((sqrt(X.^2+Y.^2)-L).^2 + (sqrt(X.^2+(Y-D).^2)-L).^2) + m*g*Y;
            end
            figure;
            contourf(X,Y,PE, 50);
            axis equal;
            xlabel('X', 'FontSize', 16)
            ylabel('Y', 'FontSize', 16)
            str = sprintf('Initial Conditions: X = %f, Y = %f',startX(ICx),startY(ICy));
            title(str, 'FontSize', 16);
            hold on;
            for i=1:numel(ts)
                h = plot(yp(i,1), yp(i,2), 'w.','markersize',20); drawnow;
                h1 = plot(yp1(i,1), yp1(i,2), 'g.','markersize',20); drawnow;
                pause(0.01);
            end
            hold off;
        end
        
        if logFLAG == 1
            disp('Plotting Log of Distance vs. Iterations...');
            % Plots Sensitive Dependence Test
            xDist = abs(yp1(:,1)-yp(:,1));
            yDist = abs(yp1(:,2)-yp(:,2));
            
            figure; hold on;
            iters = 1:1:length(tp);
            plot(iters,log(xDist), 'r-', iters,log(yDist), 'b-')
            str = sprintf('Initial Conditions: X = %f, Y = %f',startX(ICx),startY(ICy));
            title(str, 'FontSize', 16);
            xlabel('Timesteps')
            ylabel('Log(Distance)')
            legend('Distance between X coordinates','Distance between Y coordinates')
            pause(2);
            hold off;
        end
    end
end

%% Section C: Calculates Lyapunov Exponents
% This portion of the code measures Lyapunov exponents
% by Re-orthogonalizing and repeated averaging.
% Note: This portion of the code takes a very long time to run over any
% size matrix of starting positions.

if lyapFLAG == 1
        disp('Calculating Lyapunov Exponents...Please be patient')

    myFile = fopen('Lyapunov Exp', 'w');
    for ICx = 1:length(startX)
        for ICy = 1:length(startY)
            
            y0 = [startX(ICx); startY(ICy); 0; 0];
            M = 40; % Number of Averaging loops
            N = 100; % Iterations per measurement step
            h = zeros(4,1);  % Will store averaged lyap exps
            for m=1:M
                J = eye(4);  % Identity is Jacobean at time 0
                for n=1:N
                    if springFLAG == 1
                        [x Jx] = Elastic_Pend_time1map(y0,g,L,k,m, D);
                    end
                    if springFLAG == 2
                        [x Jx] = Double_Spring_time1map(y0, g,L,k, m, D);
                    end
                    
                    J = Jx*J;                % updates Jacobean
                    [Q,R] = qr(J);           % re-orthogonalizes
                    J = Q*diag(diag(R));     % corrects lengths of Jacobian
                end
                
                rN = abs(diag(R));
                h = h + log(rN)/N;           % Sums lyap exps from each run
            end
            format short;
            Position = y0(1:2)
            h = h/M  % Prints answer: Average over all runs of lyapunov exponents
            fprintf(myFile, 'StartX: %f, StartY: %f \n', startX(ICx), startY(ICy));
            fprintf(myFile, 'h1: %f \nh2: %f \nh3: %f \nh4: %f \n', h(1), h(2), h(3),h(4));
            
        end
    end
    fclose(myFile);
end

%% Section D: Plots "Heat Map" of Lyapunov Exponents in XY Plane
% Reads from the file created by the Lyapunov Code Section Above.
% Note: We edited the formatting of the txt file significantly. The code
% for part D cannot be run on any text file. Due to time constraints and
% our abilities with MATLAB, there is alot of hard code/edits to the text
% file to ensure the code runs smoothly.

if heatmapFLAG == 1
    disp('Plotting heat map of Lyapunov Exponents in the range of ICs...')
    if springFLAG == 1
        myFile = fopen('Single.txt','r');
    end
    if springFLAG == 2
        myFile = fopen('double.txt','r');
    end
    % Reads through text file
    myMat = fscanf(myFile, '%f %f %f');
    
    for i=0:(length(myMat)/3-1)
        % Parses Vector for X values, Y values, and Lyap values
        X(i+1) = myMat(3*(i)+1);
        Y(i+1) = myMat(3*(i)+2);
        maxH(i+1) = myMat(3*i+3);
    end
    maxH = reshape(maxH, [6 6]); % Reshapes for plotting.
    
    figure;
    hold on;
    imagesc(X,Y,maxH);
    colorbar;
    xlabel('X');
    ylabel('Y');
    title('Lyapunov Exponents based on Initial Conditions (X,Y)')
    hold off;
    
    fclose(myFile);
end