function animate( option ) 
    % Program displays a GUI and animates knee linkcage
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    % Objective: Display a knee linkage animation and handle the logic for it
    %
    % Key Variables: 
    % option  : String showing which switch case should run, defaults to 'initialize'
    % handles : Structure containing GUI handles and allows for some
    %           functions and cases to use the same variables
    % fig1    : Main figure for GUI

    
    if nargin<1
        option='initialize';
    end
    
    switch option
        case 'initialize'
            clc
            close
    
            % Load initial parameters
            [L1, L2, L3, L4, angle1, angle2, angle3, angle4] = getParameters();
    
            % Setup Figure
            fig1 = figure(1);
            set(fig1,...
                'Position',[100,100,500,500],...
                'NumberTitle','off',...
                'Name','Linkcage GUI');
            set(fig1,'MenuBar','none');
            % Hide the ticks and x and y axis lines
            set(gca, 'XTick', [], 'YTick', []);
            set(gca, 'XColor', 'none', 'YColor', 'none');
            axis on;
            
            % Create the handles structure and store the arrays
            handles = guihandles(fig1); 
            handles.L = [L1, L2, L3, L4];
            handles.theta = [angle1, angle2, angle3, angle4];

            % Create a frame for storing step buttons in
            handles.stepFrame = uipanel('Parent', fig1, 'Title', 'Step Controls', 'Units', 'pixels', 'Position', [10 10 170 44]);
            % Step Back button
            uicontrol('Parent', handles.stepFrame, ...
                'Style', 'pushbutton', 'String', '< Step Back', ...
                'Position', [10 4 70 20], ...
                'Callback', 'animate(''stepBackward'')');
            % Step Forward button
            uicontrol('Parent', handles.stepFrame, ...
                'Style', 'pushbutton', 'String', 'Step Fwd >', ...
                'Position', [90 4 70 20], ...
                'Callback', 'animate(''stepForward'')');

            % Create a frame for Speed buttons
            handles.speedFrame = uipanel('Parent', fig1, 'Title', 'Speed Controls', 'Units', 'pixels', 'Position', [10 53 170 44]);
            % Slow Speed button
            uicontrol('Parent', handles.speedFrame, ...
                'Style', 'pushbutton', 'String', 'Slow', ...
                'Position', [4 4 50 20], ...
                'Callback', 'animate(''slowSpeed'')');
            % Medium Speed button
            uicontrol('Parent', handles.speedFrame, ...
                'Style', 'pushbutton', 'String', 'Medium', ...
                'Position', [58 4 50 20], ...
                'Callback', 'animate(''mediumSpeed'')');
            % Fast Speed button
            uicontrol('Parent', handles.speedFrame, ...
                'Style', 'pushbutton', 'String', 'Fast', ...
                'Position', [112 4 50 20], ...
                'Callback', 'animate(''fastSpeed'')');

            % Create a frame for isolation controls
            handles.isolationFrame = uipanel('Parent', fig1, 'Title', 'Hide links','Units','pixels','Position', [10 96 170 44]);
            handles.isoLink1 = uicontrol('Parent', handles.isolationFrame, ...
                'Style', 'togglebutton', 'String', 'Link 1', 'Value', 0, ...
                'Position', [4 4 36 20],...
                'Callback', 'animate(''toggleLink1'')');
            handles.isoLink2 = uicontrol('Parent', handles.isolationFrame, ...
                'Style', 'togglebutton', 'String', 'Link 2', 'Value', 0, ...
                'Position', [45 4 36 20],...
                'Callback', 'animate(''toggleLink2'')');
            handles.isoLink3 = uicontrol('Parent', handles.isolationFrame, ...
                'Style', 'togglebutton', 'String', 'Link 3', 'Value', 0, ...
                'Position', [86 4 36 20],...
                'Callback', 'animate(''toggleLink3'')');
            handles.isoLink4 = uicontrol('Parent', handles.isolationFrame, ...
                'Style', 'togglebutton', 'String', 'Link 4', 'Value', 0, ...
                'Position', [128 4 36 20],...
                'Callback', 'animate(''toggleLink4'')');

            % Initialize menus (save them to handles if they need to be
            % greyed out/disabled)
            % Main menu
            File_menu = uimenu(gcf,'Label','File');
            Variables_menu = uimenu(gcf,'Label','Variables');
            Animate_menu = uimenu(gcf,'Label','Animate');
            handles.View_menu = uimenu(fig1, 'Label', 'View');
            % File menu
            uimenu(File_menu,'Label','Quit', 'Callback','close;');
            uimenu(File_menu, 'Label', 'Export Image', 'Callback','animate(''exportImage'')'); 
            uimenu(File_menu, 'Label', 'Export Video', 'Callback', 'animate(''exportVideo'')');
            uimenu(File_menu, 'Label', 'Reset', 'Callback', 'animate(''reset'')');
            % Variables menu
            uimenu(Variables_menu,'Label','Set Input Link Length', 'Callback','animate(''setInputLength'')');
            uimenu(Variables_menu,'Label','Set Output Link Length', 'Callback','animate(''setOutputLength'')');
            uimenu(Variables_menu,'Label','Coupler Link Length');
            % Animate menu
            handles.show_option = uimenu(Animate_menu,'Label','Show');
            handles.stride_option = uimenu(Animate_menu,'Label','Stride', 'Callback', 'animate(''showStride'')');
            handles.start_animating_option = uimenu(Animate_menu, 'Label', 'Start Animating', 'Callback', 'animate(''startAnimating'')');
            uimenu(Animate_menu, 'Label', 'Stop Animating', 'Callback', 'animate(''stopAnimating'')');
            % Option menu
            uimenu(handles.show_option,'Label','Standing', 'Callback','animate(''positionStanding'')');
            uimenu(handles.show_option,'Label','Midswing', 'Callback','animate(''positionMidstride'')');
            uimenu(handles.show_option,'Label','Seated', 'Callback','animate(''positionSeated'')');
            % View menu
            handles.zoom_out_option = uimenu(handles.View_menu, 'Label', 'Zoom Out','Checked','on', 'Callback','animate(''zoomOut'')');
            handles.zoom_in_option = uimenu(handles.View_menu, 'Label','Zoom In','Checked','off', 'Callback','animate(''zoomIn'')');
            uimenu(handles.View_menu,'Label','Pan', 'Callback','animate(''panView'')');
            handles.ic_display_option = uimenu(handles.View_menu, 'Label', 'Display IC','Checked','on','Callback','animate(''icToggle'')');
       
            % Save handles to the figure
            guidata(fig1, handles);
                   
            % Initialize Links
            upperBlock();
            link2();
            lowerBlock();
            link4();
            iC();
            Kc_prime();

            % Set the intial axis
            axis([-200 200 -210 330])

            % Set the initial speed to medium
            animate('mediumSpeed');

            animate('updatePlot');
    
        case 'updatePlot'
            %retrieve handles
            handles = guidata(gcf);
            L = handles.L;
            th = handles.theta;
    
            % Calculate new th(2) and th(4) based on the current th(3) and link lengths.
            % This is useless during initilization, but does work to make
            % setting the input and output links and having the angles get
            % recalculated
            [new_th2, new_th4] = calc4Bar(th(1), th(2), th(3), th(4), L(1), L(2), L(3), L(4));
    
            % Update the variables and save them back to handles
            th(2) = new_th2;
            th(4) = new_th4;
            handles.theta = th;
            guidata(gcf, handles);
    
            % 1. Pivot points
            Ax = 0; 
            Ay = 0;
    
            % B goes from A along Link 2
            Bx = Ax + L(2)*cosd(th(2));
            By = Ay + L(2)*sind(th(2));
    
            % D goes from B along Link 3
            Dx = Bx + L(3)*cosd(th(3));
            Dy = By + L(3)*sind(th(3));
    
            % C is another ground pivot, goes from D, backwards along link 4
            Cx = Dx-L(4)*cosd(th(4));
            Cy = Dy-L(4)*sind(th(4));
    
            % Create Kc and IC
            Kc = [30, 25];
            iCLoc = Kc+[-55, 298];
            iC(translate(iCLoc(1),iCLoc(2)));
    
            % Calculate L1
            handles.L(1) = norm([Cx,Cy]);
            guidata(gcf, handles);
            
            % Create title with new link lengths
            titleString = sprintf("L1 = %.2f  |  L2 = %.2f  |  L3 = %.2f  |  L4 = %.2f",...
                handles.L(1), handles.L(2), handles.L(3), handles.L(4));
            title(titleString);
    
            % Delete old IC lines
            if isfield(handles, 'lineB_IC') && isvalid(handles.lineB_IC)
                delete(handles.lineB_IC);
                delete(handles.lineD_IC);
            end
    
            % Draw lines from B to IC and C C to IC
            if handles.theta(3) == 0 && get(handles.ic_display_option, 'Checked') == 1
                hold on;
                handles.lineB_IC = plot([Bx, iCLoc(1)], [By, iCLoc(2)], 'r--', 'LineWidth', 1.5);
                handles.lineD_IC = plot([Dx, iCLoc(1)], [Dy, iCLoc(2)], 'b--', 'LineWidth', 1.5);
            end
            
            % Save the new line handles back to the GUI's memory
            guidata(gcf, handles);
    
            % 2. Build T-Matrices using your custom functions and update links
            % Link 1 (Upper Block / Ground) - Just sits at the origin
            T1 = translate(Ax, Ay) * rotate(0); 
            upperBlock(T1);
            
            % Link 2 (Input) - Anchor at (x1, y1), rotate by th(2)
            T2 = translate(Ax, Ay) * rotate(th(2)) * scale(L(2));
            link2(T2);
            
            % Link 3 (Lower Block / Coupler)
            T3 = translate(Bx, By) * rotate(th(3));
            lowerBlock(T3);
            
            % Link 4 (Output) - Anchor at ground pivot C
            T4 = translate(Cx, Cy) * rotate(th(4)) * scale(L(4));
            link4(T4);

            % Calculate Kc' when theta(3) = -90 degrees
            if round(handles.theta(3)) == -90
                % Kc prime x coordinate is the center of the upperBlock
                Kc_prime_x = (Ax + Cx) / 2;
                Kc_prime_y = (By + Dy) / 2; 

                % Transform Kc prime
                T_Kc_p = translate(Kc_prime_x, Kc_prime_y);
            else
                T_Kc_p = translate(0,0);
            end
            Kc_prime(T_Kc_p); % apply transformation (must do this every time so Kc can determine if it should be shown)
    
        case 'setInputLength'
            % Provid text box to input new length
            prompt = 'Input a length for the input link (45 to 75)';
            dlgtitle = 'Input:';
            dims = [1 35];
            handles = guidata(gcf);
            definput = {num2str(handles.L(2))}; 
            answer = inputdlg(prompt,dlgtitle,dims,definput);
    
            if isempty(answer)
                return; 
            end
    
            % Convert input to a number
            new_L2 = str2double(answer{1});

            % Check if input is a number and within range
            if isnan(new_L2) || new_L2 < 45 || new_L2 > 75
                animate('stopAnimating'); % If there is an error stop the animation
                errordlg('Link length must be a positive number between 45 and 75!', 'Invalid Input');
                return;
            end
            
            handles.L(2) = new_L2;
    
            % Recalculate the entire standing geometry based on the new L2
            [L1, L2, L3, L4, angle1, angle2, angle3, angle4] = getParameters(handles.L(2), handles.L(4));
    
            handles.L = [L1, L2, L3, L4];
            handles.theta = [angle1, angle2, angle3, angle4];
    
            guidata(gcf, handles);

            animate('updatePlot');

            % Moving leg into standing position
            animate('positionStanding');
    
        case 'setOutputLength'
            prompt = 'Input a length for the output link (60 to 100)';
            dlgtitle = 'Input:';
            dims = [1 35];
            handles = guidata(gcf);
            definput = {num2str(handles.L(4))}; 
            answer = inputdlg(prompt,dlgtitle,dims,definput);
    
            if isempty(answer)
                return; 
            end
    
            % Convert input to a number
            new_L4 = str2double(answer{1});
    
            % Check if input is a number and within range
            if isnan(new_L4) || new_L4 < 60 || new_L4 > 100
                animate('stopAnimating'); % If there is an error stop the animation
                errordlg('Link length must be a positive number between 60 and 100!', 'Invalid Input');
                return;
            end
            
            handles.L(4) = new_L4;
    
            % Recalculate EVERYTHING for the standing position with the new L4
            [L1, L2, L3, L4, angle1, angle2, angle3, angle4] = getParameters(handles.L(2), new_L4);
            
            % Overwrite the handles arrays with the newly calculated geometry
            handles.L = [L1, L2, L3, L4];
            handles.theta = [angle1, angle2, angle3, angle4];
    
            guidata(gcf, handles);

            animate('updatePlot');
            
            % Moving leg into standing position
            animate('positionStanding');
    
        case 'positionSeated'
            disp('Position Seated Started');
            handles = guidata(gcf);
    
            handles.theta(3) = -90; 
            guidata(gcf, handles);
    
            % Call updatePlot to calculate the new geometry and redraw
            animate('updatePlot');
            disp('Position Seated Finished');
    
        case 'positionStanding'
            disp('Position Standing Started');
            handles = guidata(gcf);
            
            handles.theta(3) = 0; 
            guidata(gcf, handles);
    
            % Call updatePlot to calculate the new geometry and redraw
            animate('updatePlot');
            disp('Position Standing Finished');
    
        case 'positionMidstride'
            disp('Position Midstride Started');
            handles = guidata(gcf); 
            
            handles.theta(3) = -65; 
            guidata(gcf, handles);
    
            % Call updatePlot to calculate the new geometry and redraw
            animate('updatePlot');
            disp('Position Midstride Finished');
    
        case 'showStride'
            disp('Show Stride Started');
            handles = guidata(gcf);

            % Set the isAnimating flag to true
            handles.isAnimating = true;
            guidata(gcf, handles);

            angleStep = 1; % degrees
            currentAngle = 0;

            % Disable invalid menu options
            set([handles.show_option], 'Enable', 'off');
            set([handles.stride_option], 'Enable', 'off');
            set([handles.start_animating_option], 'Enable', 'off');
    
            while currentAngle > -65
                currentAngle = currentAngle - angleStep;
                handles = guidata(gcf);
                handles.theta(3) = currentAngle;

                % Stop the loop if flag changes
                if handles.isAnimating == false
                    % After stopping animation put leg back to standing position
                    handles.theta(3) = 0;
                    guidata(gcf, handles);
                    animate('updatePlot');
                    break;
                end
    
                % Update the plot
                guidata(gcf, handles);
    
                % Call updatePlot to calculate the new geometry and redraw
                animate('updatePlot');
    
                % Watch the leg move
                pause(handles.pauseTime); 

                drawnow;
            end
            handles = guidata(gcf);

            handles.isAnimating = false;
            guidata(gcf, handles);

            % Enable menu options
            set([handles.show_option], 'Enable', 'on');
            set([handles.stride_option], 'Enable', 'on');
            set([handles.start_animating_option], 'Enable', 'on');
            disp('Show Stride Finished');

        case 'quitCase'
            disp('Quitting');
            handles = guidata(gcf);
            handles.isAnimating = false;
            guidata(gcf, handles);

        case 'exportImage'
            disp('Export image Started');
            % Open a dialog to let the user choose the file name, location, and format
            [filename, pathname] = uiputfile({ ...
                '*.png', 'PNG Image (*.png)'; ...
                '*.jpg', 'JPEG Image (*.jpg)'; ...
                '*.pdf', 'PDF Document (*.pdf)'}, ...
                'Save Image As');

            % Check if the user clicked "Cancel"
            if isequal(filename,0) || isequal(pathname,0)
                disp('Export cancelled by user.');
            else
                % Construct the full file path
                fullFilePath = fullfile(pathname, filename);

                % Export the current figure (gcf = get current figure)
                % Using exportgraphics for high-quality output (requires MATLAB R2020a or newer)
                try
                    exportgraphics(gcf, fullFilePath, 'Resolution', 300);
                    disp(['Image successfully saved to: ', fullFilePath]);
                catch
                    % Fallback for older MATLAB versions
                    saveas(gcf, fullFilePath);
                    disp(['Image successfully saved to: ', fullFilePath]);
                end
            end
            disp('Export image Started');
            
        case 'reset'
            disp('Reset Started');
            handles = guidata(gcf);
            handles.isAnimating = false;
            handles.theta(3) = 0;
            handles.L(2) = 60;
            handles.L(4) = 70;

            % Recalculate EVERYTHING for the original lengths
            [L1, L2, L3, L4, angle1, angle2, angle3, angle4] = getParameters(handles.L(2), handles.L(4));
            
            % Overwrite the handles arrays with the newly calculated geometry
            handles.L = [L1, L2, L3, L4];
            handles.theta = [angle1, angle2, angle3, angle4];

            % Make sure the Links are visible
            set(handles.isoLink1, 'Value', 0);
            set(handles.isoLink2, 'Value', 0);
            set(handles.isoLink3, 'Value', 0);
            set(handles.isoLink4, 'Value', 0);

            guidata(gcf, handles);
            animate('updatePlot')
            animate('zoomOut')
            disp('Reset Finished');

        case 'exportVideo'
            disp('Export Video Started');
            % Start in the standing position
            animate('positionStanding');

            v = VideoWriter('testVideo');
            v.FrameRate = 8;
            open(v);
            
            for k = 1:20
                drawnow;
                frame = getframe(gcf);
                writeVideo(v, frame);
                animate('stepForward');
            end

            % Reset to standing position
            animate('positionStanding');
        
            close(v);
            disp('Video export completed.');
            disp('Export Video Finished');
    
        case 'startAnimating'
            disp('Starting Animating');
            angleStep = 1;     % degrees
            previousAngle = 0; % degrees
            currentAngle = 0;  % degrees
    
            % Set animating flag to true
            handles = guidata(gcf); 
            handles.isAnimating = true;
            guidata(gcf, handles);

            % Disable invalid menu options
            set([handles.show_option], 'Enable', 'off');
            set([handles.stride_option], 'Enable', 'off');
            set([handles.start_animating_option], 'Enable', 'off');
    
            % Loop through the angle updating until break
            while true
                % Grab fresh handles every loop
                handles = guidata(gcf);
    
                % Stop the loop if flag changes
                if handles.isAnimating == false
                    break;
                end
    
                % If leg is moving towards stride
                if (currentAngle == 0 || currentAngle < previousAngle) && (currentAngle > -65)
                    previousAngle = currentAngle;
                    currentAngle = currentAngle - angleStep; 
                % If leg is moving towards standing
                else
                    previousAngle = currentAngle;
                    currentAngle = currentAngle + angleStep; 
                end
    
                handles.theta(3) = currentAngle;
    
                % Update the plot
                guidata(gcf, handles);
                animate('updatePlot');

                % Watch the leg move
                pause(handles.pauseTime); 

                % Allow stopping and changing of speed
                drawnow;
                
            end
    
            % After stopping animation put leg back to standing position
            handles.theta(3) = 0;
            guidata(gcf, handles);
            animate('updatePlot');

            % Enable menu options again
            set([handles.show_option], 'Enable', 'on');
            set([handles.stride_option], 'Enable', 'on');
            set([handles.start_animating_option], 'Enable', 'on');
            disp('Animating Finished');
        
        case 'stopAnimating'
            disp('Stopping animation')
            handles = guidata(gcf);
            handles.isAnimating = false;
            guidata(gcf, handles);
        
        case 'zoomIn'
            handles = guidata(gcf);

            % Turn on/off checkmarks
            set(handles.zoom_in_option, 'Checked', 'on');
            set(handles.zoom_out_option, 'Checked', 'off');

            % Zoom in world coordinates to fit mechanism
            axis([-40 100 -195 85]);
            disp('Zoomed in');

        case 'zoomOut'
            handles = guidata(gcf);

            % Turn on/off checkmarks
            set(handles.zoom_in_option, 'Checked', 'off');
            set(handles.zoom_out_option, 'Checked', 'on');

            % Zoom out world coordinates to fit whole view
            axis([-200 200 -210 330]);
            disp('Zoomed out');

        case 'panView'
            disp('Pan View Started');
            % Get click input from graph
            [x_click, y_click] = ginput(1);

            % Get current axis limits
            current_limits = axis;

            width = current_limits(2) - current_limits(1);
            height = current_limits(4) - current_limits(3);

            new_x1 = x_click - width/2;
            new_x2 = x_click + width/2;
            new_y1 = y_click - height/2;
            new_y2 = y_click + height/2;

            % Apply new limits
            axis([new_x1, new_x2, new_y1, new_y2]);
            disp('Pan View Finished');

        case 'icToggle'
            handles = guidata(gcf);
            pause(0.1);

            if get(handles.ic_display_option, 'Checked') == 1
                set(handles.ic_display_option, 'Checked', 'off');
            else
                set(handles.ic_display_option, 'Checked', 'on');
            end
            animate('updatePlot');
            disp('ic Toggled');

        case 'stepForward'
            handles = guidata(gcf);
            stepSize = 5; % degrees

            % Calculate new angle moving towards -90 degree position
            newAngle = handles.theta(3) - stepSize;

            % Enforce the bound so it doesn't bend past the seated position
            if newAngle < -90
                newAngle = -90;
                disp('Maximum flexion reached (Seated Position).');
            end

            % Update handles and redraw
            handles.theta(3) = newAngle;
            guidata(gcf, handles);
            animate('updatePlot');
            disp('Stepped Forward');

        case 'stepBackward'
            handles = guidata(gcf);
            stepSize = 5; % degrees

            % Calculate new angle moving towards -90 degree position
            newAngle = handles.theta(3) + stepSize;

            % Enforce the bound so it doesn't bend past the seated position
            if newAngle > 0
                newAngle = 0;
                disp('Minimum flexion reached (Standing Position).');
            end

            % Update handles and redraw
            handles.theta(3) = newAngle;
            guidata(gcf, handles);
            animate('updatePlot');
            disp('Stepped Backward');

        case 'slowSpeed'
            handles = guidata(gcf);
            handles.pauseTime = 0.2;
            guidata(gcf, handles);
            disp('Speed Set Slow');

        case 'mediumSpeed'
            handles = guidata(gcf);
            handles.pauseTime = 0.05;
            guidata(gcf, handles);
            disp('Speed Set Medium');

        case 'fastSpeed'
            handles = guidata(gcf);
            handles.pauseTime = 0.000;
            guidata(gcf, handles);
            disp('Speed Set Fast');

        case 'toggleLink1'
            handles = guidata(gcf);
            animate('updatePlot');

            % Value of 0 is off, value of 1 is on
            if get(handles.isoLink1, 'Value') == 1
                disp('Link1 Turned off')
            else
                disp('Link1 Turned on')
            end

        case 'toggleLink2'
            handles = guidata(gcf);
            animate('updatePlot');

            % Value of 0 is off, value of 1 is on
            if get(handles.isoLink2, 'Value') == 1
                disp('Link2 Turned off')
            else
                disp('Link2 Turned on')
            end

        case 'toggleLink3'
            handles = guidata(gcf);
            animate('updatePlot');

            % Value of 0 is off, value of 1 is on
            if get(handles.isoLink3, 'Value') == 1
                disp('Link3 Turned off')
            else
                disp('Link3 Turned on')
            end

        case 'toggleLink4'
            handles = guidata(gcf);
            animate('updatePlot');

            % Value of 0 is off, value of 1 is on
            if get(handles.isoLink4, 'Value') == 1
                disp('Link4 Turned off')
            else
                disp('Link4 Turned on')
            end

    end % end of switch

end % animate

%%

function [theta2,theta4] = calc4Bar(angle1, angle2, angle3, angle4, link1, link2, link3, link4)
    % Objective: Given the input angle theta3, this function
    % calculates and returns the values of the input link angle
    % and the output link angle. It uses Freudenstein equations;
    % all angles measured in degrees, ccw+, from the horizontal.
    %   
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Variables:
    % link1:    length of vector R1, from the
    %           input to the output ground pivot
    % link2,3,4 length of vectors R2,R3,R4 respectively
    % angle1:   angle of vector R1, from the
    %           input to the output ground pivots
    % angle2:   angle of the input link (deg)   (initially a guess)
    % angle3:   angle of link3 (deg)
    % angle4:   angle of the output link4 (deg) (initially a guess)
     
    %calculate angle2,angle4 given input 'angle3'
    
    %estimate solution (initial pass)
    fxn1 = link2*cosd(angle2) + link3*cosd(angle3) - link4*cosd(angle4) - link1*cosd(angle1);
    fxn2 = link2*sind(angle2) + link3*sind(angle3) - link4*sind(angle4) - link1*sind(angle1);
    fvec = [fxn1 fxn2]';
    
    %iterate and (hopefully!) converge
    while norm(fvec)>1E-6
        Jmatrix = [ -link2*sind(angle2)   link4*sind(angle4)  ;
                     link2*cosd(angle2)  -link4*cosd(angle4) ];
        deltaAngle = Jmatrix\(-fvec);
        angle2 = angle2 + deltaAngle(1);
        angle4 = angle4 + deltaAngle(2);
        fxn1 = link2*cosd(angle2) + link3*cosd(angle3) - link4*cosd(angle4) - link1*cosd(angle1);
        fxn2 = link2*sind(angle2) + link3*sind(angle3) - link4*sind(angle4) - link1*sind(angle1);
        fvec = [fxn1 fxn2]';
    end
    
    %Return angle2 and angle4 (return parameters named theta3 and theta4)
    theta2 = angle2;
    theta4 = angle4;

end % calc4Bar

%%

function [L1,L2,L3,L4,angle1,angle2,angle3,angle4] = getParameters(input_L2, input_L4)
    % Objective: Returns the fixed parameters for the 
    % clicker press 4-bar linkage, in the initial
    % position (theta3=0), as follows:
    % return val = [ link lengths 1-4, link angles 1-4  ]
    
    % Author: William Schrab (modified from: Frank Kelso)
    % Date: 4/3/2026
    % ME 5241: Computer-Aided Engineering
    %
    % Key Variables
    % input_L2, input_L4 : Allows for input of non default L2 and L4 and
    %                      will calculate angles and new L1 based on these
    % LX                 : length of link X (mm) where X: 1,2,3,4
    % angleX             : angle of link X (deg). ccw+, meas from y-axis
    
    
    % Known Link Lengths
    if nargin == 2
        L2 = input_L2;
        L4 = input_L4;
    else
        L4 = 70;       % Output Link
        L2 = 60;       % Input Link
    end
    
    L3 = 50;       % Lower Leg (Coupler) - Fixed at 50 mm
    
    % Point A (Fixed ground pivot on Upper Block)
    Ax = 0; 
    Ay = 0;
    
    % Cosmetic Knee Center (Kc) local coordinates
    Kc_x = 30;
    Kc_y = 25;
    
    % Alpha and Beta define the IC location relative to Kc.
    alpha = 55;  
    beta = 298;  
    IC_x = Kc_x - alpha;
    IC_y = Kc_y + beta;
    
    % Find Point B (Enforcing Collinearity: IC -> A -> B)
    vecIC_A = [Ax - IC_x, Ay - IC_y];
    dirIC_A = vecIC_A / norm(vecIC_A); % Direction pointing from IC through A
    
    % B goes from A along Link 2
    Bx = Ax + L2*dirIC_A(1);
    By = Ay + L2*dirIC_A(2);
    
    angle3 = 0; % Coupler starts horizontal
    
    % D goes from B along Link 3
    Dx = Bx + L3*cosd(angle3);
    Dy = By + L3*sind(angle3);
    
    % Find Point C (Enforcing Collinearity: IC -> C -> D)
    % C lies on the line between D and IC, at distance L4 away from D.
    vecD_IC = [IC_x - Dx, IC_y - Dy];
    dirD_IC = vecD_IC / norm(vecD_IC); % Direction pointing from D back to IC
    
    % C is another ground pivot, goes from D, backwards along link 4
    Cx = Dx+L4*dirD_IC(1);
    Cy = Dy+L4*dirD_IC(2);
    
    % Dynamically calculate Link 1 (Ground Link)
    % L1 connects the fixed point A to the newly calculated point C
    L1 = norm([Cx - Ax, Cy - Ay]);
    
    % Calculate Exact Standing Angles for all remaining links
    % Since we are using standard x-axis math, atan2d handles this perfectly
    angle1 = atan2d(Cy - Ay, Cx - Ax);
    angle2 = atan2d(By - Ay, Bx - Ax);
    angle4 = atan2d(Dy - Cy, Dx - Cx);

end % getParameters

%%

function matrix = rotate(theta)
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    % Objective:Function returns a matrix to rotate points by a certain number of degrees
    % about their origin
    % 
    % Key Variables: 
    % theta  (input) : The angle to rotate in degrees
    % matrix (output): The resulting 3x3 transformation matrix
    
    
    % Create identity matrix
    matrix = eye(3);
    matrix(1,1) = cosd(theta);
    matrix(1,2) = -sind(theta);
    matrix(2,1) = sind(theta);
    matrix(2,2) = cosd(theta);
end % rotate

%%

function matrix = scale(Sf)
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    % Objective: Function returns a matrix to scale points by a given scale factor
    % 
    % Key Variables: 
    % Sf     (input) : The scale factor multiplier
    % matrix (output): The resulting 3x3 transformation matrix
    
    % Create identity matrix
    matrix = eye(3);
    matrix(1,1) = Sf; % Scale the identity matrix by the scale factor
    matrix(2,2) = Sf;
end %scale

%%

function matrix = translate(dX, dY)
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    % Objective: Function returns a matrix to rotate points by a certain number of degrees
    % about their origin
    %
    % Key Variables: 
    % dX,dY  (input) : Translation distance along x and y axis respectively
    % matrix (output): The resulting 3x3 transformation matrix
    
    % Create identity matrix
    matrix = eye(3);
    matrix(1,3) = dX;
    matrix(2,3) = dY;
end %translate

%%

function upperBlock(Tmatrix)
    persistent ptsOriginal wHandle aCircle cCircle A C pointKc textKc
    % Objective: Shows link 1 (ground) and draws circles for its joints
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Key Variables: 
    % Tmatrix (input) : 3x3 transformation matrix applied to block
    % ptsOriginal     : Original matrix coordinates for block
    % wHandle (output): Graphics handle for upper block
    
    if nargin<1
        % Grab the dynamic coordinates directly from the GUI's memory
        handles = guidata(gcf);
    
        % Initialize box 
        px = [0 100 100 0 0];
        py = [0 0 100 100 0];
        bottomRow = ones(1,5);
        ptsOriginal = [px; py; bottomRow];
    
        % Set initial A and C points as if bottom left of box was 0,0
        A = [20; 25; 1];
    
        % Dynamically set C based on your calculations
        if isfield(handles, 'L') && isfield(handles, 'theta')
            % Map the global distance between A and C to the block's local space
            vecAC_x = handles.L(1)*cosd(handles.theta(1));
            vecAC_y = handles.L(1)*sind(handles.theta(1));
            C = [A(1) + vecAC_x; A(2) + vecAC_y; 1];
        else
            % Fallback default before handles are populated
            C = [65; 40; 1]; 
        end
    
        % Create a translation matrix to shift B to (0,0)
        T_to_origin = [1 0 -A(1); 
                       0 1 -A(2); 
                       0 0  1];
    
        % Apply the shift to everything so B is now at (0,0,1)
        ptsOriginal = T_to_origin * ptsOriginal;
        A = T_to_origin * A;
        C = T_to_origin * C;
    
        % Plot the box
        wHandle = line('xdata',ptsOriginal(1,1:5),'ydata',ptsOriginal(2,1:5),'Color', 'k', 'LineWidth', 3);
    
        % Plot the circles: viscircles needs an Nx2 input, so transpose (1x2)
        aCircle = viscircles(A(1:2)', 5, 'Color', 'k');
        cCircle = viscircles(C(1:2)', 5, 'Color', 'k');
    
        % Plot KC
        hold on;
        pointKc = plot(30, 25, 'c+', 'MarkerSize', 6);
        textKc = text(35, 27, 'K_c', 'FontSize', 10, 'Color', 'c');
    
    else
        handles = guidata(gcf);

        % Apply transformations based on Tmatrix
        ptsnew = Tmatrix*ptsOriginal;
        Anew = Tmatrix*A;
        set(wHandle,'xdata',ptsnew(1,:),'ydata',ptsnew(2,:));
    
        % Dynamically caculate C location because it could have changed
        % with link lengths
        vecAC_x = handles.L(1)*cosd(handles.theta(1));
        vecAC_y = handles.L(1)*sind(handles.theta(1));
        Cnew = [Anew(1) + vecAC_x; Anew(2) + vecAC_y; 1];

        % Transform Kc
        posKc = Tmatrix * [30; 25; 1];
        set(pointKc, 'XData', posKc(1), 'YData', posKc(2));
        set(textKc, 'Position', [posKc(1)+3, posKc(2)+4, 0]);
    
        % Delete circles before redrawing if not hidden
        delete(aCircle); delete(cCircle);

        % If Hide link button is clicked in then hide the link
        if get(handles.isoLink1, 'Value') == 1
            set(wHandle, 'Visible', 'off');
        else
            set(wHandle, 'Visible', 'on');
            aCircle = viscircles(Anew(1:2)', 5, 'Color', 'k');
            cCircle = viscircles(Cnew(1:2)', 5, 'Color', 'k');
        end
    end
end % upperBlock

%%

function lowerBlock(Tmatrix)
    persistent ptsOriginal wHandle bCircle dCircle B D
    % Objective: Shows link 3 (coupler) and draws circles for its joints
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Key Variables: 
    % Tmatrix (input) : 3x3 transformation matrix applied to block
    % ptsOriginal     : Original matrix coordinates for block
    % wHandle (output): Graphics handle for lower block
    
    if nargin<1
        % Initialize box 
        px = [0 100 100 0 0];
        py = [0 0 150 150 0];
        bottomRow = ones(1,5);
        ptsOriginal = [px; py; bottomRow];
    
        % Set initial B and D points as if bottom left of box was 0,0
        B = [24.63; 125; 1];
        D = [24.63+50; 125; 1];
    
        % Create a translation matrix to shift B to (0,0)
        T_to_origin = [1 0 -B(1); 
                       0 1 -B(2); 
                       0 0  1];
    
        % Apply the shift to everything so B is now at (0,0,1)
        ptsOriginal = T_to_origin * ptsOriginal;
        D = T_to_origin * D;
        B = T_to_origin * B;
    
        wHandle = line('xdata',ptsOriginal(1,1:5),'ydata',ptsOriginal(2,1:5),'Color', 'g', 'LineWidth', 3);
    
        % viscircles needs an Nx2 input, so transpose (1x2)
        bCircle = viscircles(B(1:2)', 5, 'Color', 'g');
        dCircle = viscircles(D(1:2)', 5, 'Color', 'g');
    
    else
        handles = guidata(gcf);

        % Apply transformations based on Tmatrix
        ptsnew = Tmatrix*ptsOriginal;
        Bnew = Tmatrix*B;
        Dnew = Tmatrix*D;
        set(wHandle,'xdata',ptsnew(1,:),'ydata',ptsnew(2,:))
    
        % Delete the circles before redrawing if visible
        delete(bCircle); delete(dCircle);

        % If Hide link button is clicked in then hide the link
        if get(handles.isoLink3, 'Value') == 1
            set(wHandle, 'Visible', 'off');
        else
            set(wHandle, 'Visible', 'on');
            bCircle = viscircles(Bnew(1:2)', 5, 'Color', 'g');
            dCircle = viscircles(Dnew(1:2)', 5, 'Color', 'g');
        end
    end
end % lowerBlock

%%

function link2(Tmatrix)
    persistent ptsOriginal wHandle jointA jointB
    % Objective: Shows link 2 (input) and draws circles for its links
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Key Variables: 
    % Tmatrix (input) : 3x3 transformation matrix applied to link
    % ptsOriginal     : Original matrix coordinates for link
    % jointA/B        : Graphics handle for joint locations
    % wHandle (output): Graphics handle for link
    
    if nargin<1
        % Define a UNIT link (Length = 1)
        % Let's give it a small thickness (e.g., 0.1)
        h = 0.05; 
        px = [0, 1, 1, 0, 0];
        py = [-h, -h, h, h, -h];
        ptsOriginal = [px; py; ones(1, 5)];
        
        % Plotting at the origin
        hold on; axis equal;
        wHandle = line(ptsOriginal(1,:), ptsOriginal(2,:), 'Color', 'r', 'LineWidth', 2);
        jointA = plot(0, 0, 'ko', 'MarkerFaceColor', 'r'); % Pivot
        jointB = plot(1, 0, 'ko', 'MarkerFaceColor', 'r'); % End effector
    else
        handles = guidata(gcf);

        % Apply the combined Scale/Rotate/Translate matrix
        ptsNew = Tmatrix * ptsOriginal;
        
        % The joints also need to be transformed
        % Joint A is at (0,0), Joint B is at (1,0) in local space
        posA = Tmatrix * [0; 0; 1];
        posB = Tmatrix * [1; 0; 1];
        
        set(wHandle, 'XData', ptsNew(1,:), 'YData', ptsNew(2,:));
        set(jointA,  'XData', posA(1),    'YData', posA(2));
        set(jointB,  'XData', posB(1),    'YData', posB(2));

        % If Hide link button is clicked in then hide the link
        if get(handles.isoLink2, 'Value') == 1
            set(wHandle, 'Visible', 'off');
            set(jointA, 'Visible', 'off');
            set(jointB, 'Visible', 'off');
        else
            set(wHandle, 'Visible', 'on');
            set(jointA, 'Visible', 'on');
            set(jointB, 'Visible', 'on');
        end
    end
end % link2

%%

function link4(Tmatrix)
    persistent ptsOriginal wHandle jointA jointB
    % Objective: Shows link 4 (output) and circles for its joints
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Key Variables: 
    % Tmatrix (input) : 3x3 transformation matrix applied to link
    % ptsOriginal     : Original matrix coordinates for link
    % jointA/B        : Graphics handle for joint locations
    % wHandle (output): Graphics handle for link
    
    if nargin<1
        % Define with length = 1 and thickness = 0.05
        h = 0.05; 
        px = [0, 1, 1, 0, 0];
        py = [-h, -h, h, h, -h];
        ptsOriginal = [px; py; ones(1, 5)];
        
        % Plotting at the origin
        hold on; axis equal;
        wHandle = line(ptsOriginal(1,:), ptsOriginal(2,:), 'Color', 'b', 'LineWidth', 2);
        jointA = plot(0, 0, 'ko', 'MarkerFaceColor', 'b'); % Pivot
        jointB = plot(1, 0, 'ko', 'MarkerFaceColor', 'b'); % End effector
    else
        handles = guidata(gcf);

        % Apply the combined Scale/Rotate/Translate matrix
        ptsNew = Tmatrix * ptsOriginal;
        
        % The joints also need to be transformed
        % Joint A is at (0,0), Joint B is at (1,0) in local space
        posA = Tmatrix * [0; 0; 1];
        posB = Tmatrix * [1; 0; 1];
        
        set(wHandle, 'XData', ptsNew(1,:), 'YData', ptsNew(2,:));
        set(jointA,  'XData', posA(1),    'YData', posA(2));
        set(jointB,  'XData', posB(1),    'YData', posB(2));

        % If Hide link button is clicked in then hide the link
        if get(handles.isoLink4, 'Value') == 1
            set(wHandle, 'Visible', 'off');
            set(jointA, 'Visible', 'off');
            set(jointB, 'Visible', 'off');
        else
            set(wHandle, 'Visible', 'on');
            set(jointA, 'Visible', 'on');
            set(jointB, 'Visible', 'on');
        end
    end
end % link4

%%

function iC(Tmatrix)
    persistent ptsOriginal  jointA  textA
    % Objective: Shows IC point and allows toggling of it through menu
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Key Variables: 
    % Tmatrix (input) : 3x3 transformation matrix applied to point
    % ptsOriginal     : Original point coordinates
    % jointA        : Graphics handle for point 
    % textA           : Graphics handle for text
    
    if nargin<1
        ptsOriginal = [0; 0; 1];
        
        % Plotting at the origin initially visible
        jointA = plot(0, 0, 'ko', 'MarkerFaceColor', 'm', 'Visible', 'on'); % Pivot
        textA = text(-5, 0, 'IC', 'FontSize', 10, 'Color', 'm');
    else
        % Grab handles to check if IC should be shown
        handles = guidata(gcf);

        % Apply the combined Scale/Rotate/Translate matrix
        ptsNew = Tmatrix * ptsOriginal;
    
        set(jointA,  'XData', ptsNew(1),    'YData', ptsNew(2), 'Visible', 'on');
        set(textA,  'Position', [ptsNew(1) - 30, ptsNew(2) - 10, 0]);

        if get(handles.ic_display_option, 'Checked') == 1
            set(jointA, 'Visible', 'on');
            set(textA, 'Visible', 'on');
        else
            set(jointA, 'Visible', 'off');
            set(textA, 'Visible', 'off');
        end
    end
end % iC

%%

function Kc_prime(Tmatrix)
    persistent ptsOriginal  pointKc_p  textKc_p 
    % Objective: Shows Kc' point (only want visible when knee is in sitting position)
    % Author: Will Schrab
    % ME 5241: Computer-Aided Engineering
    % Date: 4/3/2026
    %
    % Key Variables: 
    % Tmatrix (input) : 3x3 transformation matrix applied to point
    % ptsOriginal     : Original point coordinates
    % pointKc_p       : Graphics handle for point 
    % textKc_p        : Graphics handle for text
    
    if nargin<1
        ptsOriginal = [0; 0; 1];
        
        % Plot KC
        hold on;
        pointKc_p = plot(0, 0, 'm+', 'MarkerSize', 6);
        textKc_p = text(5, 5, 'Kc''', 'FontSize', 10, 'Color', 'm');
    else
        % Get the handles to see if leg is seated
        handles = guidata(gcf);

        % Apply the combined Scale/Rotate/Translate matrix
        ptsNew = Tmatrix * ptsOriginal;
    
        set(pointKc_p,  'XData', ptsNew(1),    'YData', ptsNew(2));
        set(textKc_p,  'Position', [ptsNew(1) - 10, ptsNew(2) + 15, 0]);

        % Check if coupler is seated (at -90)
        if round(handles.theta(3)) == -90 % degrees
            set(pointKc_p, 'Visible', 'on');
            set(textKc_p, 'Visible', 'on');
        else
            set(pointKc_p, 'Visible', 'off');
            set(textKc_p, 'Visible', 'off');
        end
    end
end % Kc_prime
