function Tutorial_4_gui(robot)
% Tutorial_4_gui creates a GUI to control a 2-link planar manipulator
% using the Robotics System Toolbox included in MATLAB, with simple
% box geometry added for each link so you can visualize them clearly.
    
    %-------------------------
    % 1) Initial Configuration
    %-------------------------
    config = homeConfiguration(robot);
    
    % Initialize path history arrays with the initial configuration
    q1_initial = wrapToPi(config(1));
    q2_initial = wrapToPi(config(2));
    pathHistory_q1 = [q1_initial];
    pathHistory_q2 = [q2_initial]; 

    %-------------------------
    % 2) Create the Figure (Maximize) and Axes
    %-------------------------
    f = figure('Name','2-Link Planar Manipulator (Robotics System Toolbox)', ...
        'NumberTitle','off', ...
        'MenuBar','none', ...
        'ToolBar','none', ...
        'WindowState','maximized');  % Open in maximized state

    % Axes for robot visualization (normalized units so it resizes nicely)
    ax = axes('Parent', f, ...
              'Units','normalized', ...
              'Position',[0.05 0.1 0.6 0.8]);
    hold(ax, 'on');
    grid(ax, 'on');
    axis(ax, 'equal');

    % Show the robot initially
    show(robot, config,Visuals='on',Collisions='off', Parent= ax, PreservePlot= false);
    title(ax, 'Two-link Manipulator');
    view(ax, [0 90]);  % top-down planar view
    xlim([-1 1]); ylim([-1 1]); zlim([-0.1 0.1]);
    
    % Precompute reachable workspace
    % Create a grid of joint configurations
    n_samples = 75;
    q1_samples = linspace(-pi, pi, n_samples);
    q2_samples = linspace(-pi, pi, n_samples);
    [Q1_grid, Q2_grid] = meshgrid(q1_samples, q2_samples);
    
    % Compute end-effector positions for all configurations
    workspace_x = zeros(size(Q1_grid));
    workspace_y = zeros(size(Q1_grid));
    
    for ii = 1:n_samples
        for jj = 1:n_samples
            temp_config = [Q1_grid(ii,jj), Q2_grid(ii,jj)];
            eeTransform = getTransform(robot, temp_config, 'tip');
            workspace_x(ii,jj) = eeTransform(1,4);
            workspace_y(ii,jj) = eeTransform(2,4);
        end
    end
    
    % Create workspace scatter plot (initially hidden)
    hWorkspaceScatter = scatter(ax, workspace_x(:), workspace_y(:), 2, 'b', 'Visible', 'off');
    alpha(hWorkspaceScatter, 0.2);

    %-------------------------
    % 3) Joint Space Visualization
    %-------------------------
    axJointSpace = axes('Parent', f, ...
                        'Units', 'normalized', ...
                        'Position', [0.68 0.70 0.27 0.25]);
    hold(axJointSpace, 'on');
    grid(axJointSpace, 'on');
    title(axJointSpace, 'Joint Space (q1, q2)');
    xlabel(axJointSpace, 'q1 (rad)');
    ylabel(axJointSpace, 'q2 (rad)');
    xlim(axJointSpace, [-pi pi]);
    ylim(axJointSpace, [-pi pi]);
    
    % Plot current position marker
    hJointMarker = plot(axJointSpace, config(1), config(2), 'ro', ...
                        'MarkerSize', 10, 'MarkerFaceColor', 'r');
    
    % Create trace plot for Euclidean joint space (initialize with starting point)
    hJointTrace = plot(axJointSpace, q1_initial, q2_initial, 'r--', 'LineWidth', 1.5);
    
    % Create torus surface (initially hidden)
    [u_grid, v_grid] = meshgrid(linspace(-pi, pi, 40), linspace(-pi, pi, 40));
    R_major = 2;  % Major radius
    r_minor = 0.8;  % Minor radius
    x_torus = (R_major + r_minor * cos(v_grid)) .* cos(u_grid);
    y_torus = (R_major + r_minor * cos(v_grid)) .* sin(u_grid);
    z_torus = r_minor * sin(v_grid);
    hTorusSurface = surf(axJointSpace, x_torus, y_torus, z_torus, ...
                         'FaceAlpha', 0.4, 'Visible', 'off', 'EdgeColor', 'none');
    
    % Marker on torus (initially hidden)
    q1_wrapped = wrapToPi(config(1));
    q2_wrapped = wrapToPi(config(2));
    x_marker = (R_major + r_minor * cos(q2_wrapped)) * cos(q1_wrapped);
    y_marker = (R_major + r_minor * cos(q2_wrapped)) * sin(q1_wrapped);
    z_marker = r_minor * sin(q2_wrapped);
    hTorusMarker = plot3(axJointSpace, x_marker, y_marker, z_marker, 'ro', ...
                         'MarkerSize', 12, 'MarkerFaceColor', 'r', 'Visible', 'off');
    
    % Create trace plot for Torus joint space (initialize with starting point)
    x_initial_torus = (R_major + r_minor * cos(q2_initial)) * cos(q1_initial);
    y_initial_torus = (R_major + r_minor * cos(q2_initial)) * sin(q1_initial);
    z_initial_torus = r_minor * sin(q2_initial);
    hTorusTrace = plot3(axJointSpace, x_initial_torus, y_initial_torus, z_initial_torus, 'r--', 'LineWidth', 1.5, 'Visible', 'off');

    %-------------------------
    % 4) Create Sliders & Labels
    %-------------------------
    % We'll allow angles from -pi to pi for each joint.
    slider1 = uicontrol('Parent', f, ...
        'Style','slider', ...
        'Min',-pi,'Max',pi,'Value',0, ...
        'Units','normalized', ...
        'Position',[0.7 0.55 0.2 0.05], ...
        'Callback',@(src,evt) sliderCallback(src, evt, 1));

    slider2 = uicontrol('Parent', f, ...
        'Style','slider', ...
        'Min',-pi,'Max',pi,'Value',0, ...
        'Units','normalized', ...
        'Position',[0.7 0.40 0.2 0.05], ...
        'Callback',@(src,evt) sliderCallback(src, evt, 2));

    uicontrol('Parent', f, ...
        'Style','text', ...
        'Units','normalized', ...
        'Position',[0.7 0.60 0.2 0.03], ...
        'String','Joint 1 Angle', ...
        'HorizontalAlignment','center');

    uicontrol('Parent', f, ...
        'Style','text', ...
        'Units','normalized', ...
        'Position',[0.7 0.45 0.2 0.03], ...
        'String','Joint 2 Angle', ...
        'HorizontalAlignment','center');

        %-------------------------
    % 5) Add Checkboxes
    %-------------------------
    % By default, both are checked (on).
    checkboxVisuals = uicontrol('Parent', f, ...
        'Style','checkbox', ...
        'Units','normalized', ...
        'Position',[0.7 0.30 0.2 0.05], ...
        'Value',1, ...           % 1 means "checked"
        'String','Show Visuals', ...
        'Callback',@(src,evt) checkboxCallback(src, evt));

    checkboxFrames = uicontrol('Parent', f, ...
        'Style','checkbox', ...
        'Units','normalized', ...
        'Position',[0.7 0.25 0.2 0.05], ...
        'Value',1, ...
        'String','Show Joint Frames', ...
        'Callback',@(src,evt) checkboxCallback(src, evt));
    
    checkboxTorusView = uicontrol('Parent', f, ...
        'Style','checkbox', ...
        'Units','normalized', ...
        'Position',[0.7 0.20 0.2 0.05], ...
        'Value',0, ...
        'String','Torus View (Joint Space)', ...
        'Callback',@(src,evt) torusViewCallback(src, evt));
    
    checkboxWorkspace = uicontrol('Parent', f, ...
        'Style','checkbox', ...
        'Units','normalized', ...
        'Position',[0.7 0.35 0.2 0.05], ...
        'Value',0, ...
        'String','Show Reachable Workspace', ...
        'Callback',@(src,evt) workspaceCallback(src, evt));

    %-------------------------
    % 6) Add Animation Controls
    %-------------------------
    uicontrol('Parent', f, 'Style', 'text', ...
        'String', 'Animation Time (s):', ...
        'Units', 'normalized', ...
        'Position', [0.7 0.15 0.12 0.03], ...
        'HorizontalAlignment', 'left');

    txtTFinal = uicontrol('Parent', f, 'Style', 'edit', ...
        'String', '2', ...
        'Units', 'normalized', ...
        'Position', [0.82 0.15 0.08 0.05]);

    uicontrol('Parent', f, 'Style', 'pushbutton', ...
        'String', 'Start Animation', ...
        'Units', 'normalized', ...
        'Position', [0.7 0.05 0.2 0.05], ...
        'Callback', @(src, evt) animateDynamics());


    %-------------------------
    % 7) Nested Callback Functions
    %-------------------------
    % Re-plot the robot based on the sliders and checkboxes
    function replotRobot()
        % Read the state of each checkbox
        visualsOn = checkboxVisuals.Value;  % 1 if checked, 0 if not
        framesOn  = checkboxFrames.Value;
        
        visOption = ternary(visualsOn, 'on', 'off');
        framesOption = ternary(framesOn, 'on', 'off');

        % Show the robot with the current config
        show(robot, config, 'Parent', ax, ...
            'PreservePlot', false, ...
            'Frames', framesOption, ...
            'Visuals', visOption);
        
        % Redraw workspace scatter if visible (it gets cleared by show() with PreservePlot=false)
        if checkboxWorkspace.Value
            hold(ax, 'on');
            set(hWorkspaceScatter, 'Visible', 'on');
        end
        
        % Update joint space marker (wrap angles to [-pi, pi])
        q1_wrapped = wrapToPi(config(1));
        q2_wrapped = wrapToPi(config(2));
        
        % Check if torus view is enabled
        if checkboxTorusView.Value
            % Torus view: hide 2D marker, show 3D torus
            set(hJointMarker, 'Visible', 'off');
            set(hJointTrace, 'Visible', 'off');
            set(hTorusSurface, 'Visible', 'on');
            set(hTorusMarker, 'Visible', 'on');
            set(hTorusTrace, 'Visible', 'on');
            
            % Update 3D marker position on torus
            x_marker = (R_major + r_minor * cos(q2_wrapped)) * cos(q1_wrapped);
            y_marker = (R_major + r_minor * cos(q2_wrapped)) * sin(q1_wrapped);
            z_marker = r_minor * sin(q2_wrapped);
            set(hTorusMarker, 'XData', x_marker, 'YData', y_marker, 'ZData', z_marker);
            
            % Update torus trace
            q1_history_wrapped = wrapToPi(pathHistory_q1);
            q2_history_wrapped = wrapToPi(pathHistory_q2);
            x_trace = (R_major + r_minor * cos(q2_history_wrapped)) .* cos(q1_history_wrapped);
            y_trace = (R_major + r_minor * cos(q2_history_wrapped)) .* sin(q1_history_wrapped);
            z_trace = r_minor * sin(q2_history_wrapped);
            set(hTorusTrace, 'XData', x_trace, 'YData', y_trace, 'ZData', z_trace);
            
            % Adjust view for 3D
            view(axJointSpace, [45 20]);
            axis(axJointSpace, 'equal');
            title(axJointSpace, 'Joint Space - Torus View');
            xlabel(axJointSpace, 'X');
            ylabel(axJointSpace, 'Y');
            zlabel(axJointSpace, 'Z');
        else
            % Euclidean view: show 2D marker, hide torus
            set(hJointMarker, 'Visible', 'on');
            set(hJointMarker, 'XData', q1_wrapped, 'YData', q2_wrapped);
            set(hJointTrace, 'Visible', 'on');
            set(hTorusSurface, 'Visible', 'off');
            set(hTorusMarker, 'Visible', 'off');
            set(hTorusTrace, 'Visible', 'off');
            
            % Update Euclidean joint space trace
            q1_history_wrapped = wrapToPi(pathHistory_q1);
            q2_history_wrapped = wrapToPi(pathHistory_q2);
            set(hJointTrace, 'XData', q1_history_wrapped, 'YData', q2_history_wrapped);
            
            % Adjust view for 2D
            view(axJointSpace, [0 90]);
            axis(axJointSpace, 'normal');
            grid(axJointSpace, 'on');
            xlim(axJointSpace, [-pi pi]);
            ylim(axJointSpace, [-pi pi]);
            title(axJointSpace, 'Joint Space (q1, q2)');
            xlabel(axJointSpace, 'q1 (rad)');
            ylabel(axJointSpace, 'q2 (rad)');
            zlabel(axJointSpace, '');
        end
        
        drawnow;
    end

    % Called when sliders change
    function sliderCallback(src, ~, jointIndex)
        val = src.Value;        % new angle for this joint
        config(jointIndex) = val;
        replotRobot();
    end

    % Called when checkboxes change
    function checkboxCallback(src, ~)
        % Just replot with updated visuals/frames options
        replotRobot();
    end
    
    % Called when torus view checkbox changes
    function torusViewCallback(src, ~)
        % Replot to switch between Euclidean and Torus views
        replotRobot();
    end
    
    % Called when reachable workspace checkbox changes
    function workspaceCallback(src, ~)
        if src.Value
            set(hWorkspaceScatter, 'Visible', 'on');
        else
            set(hWorkspaceScatter, 'Visible', 'off');
        end
        drawnow;
    end
    
    function animateDynamics()
        tFinal = str2double(txtTFinal.String);
        if isnan(tFinal) || tFinal <= 0
            errordlg('Please enter a valid positive number for animation time.');
            return;
        end
        
        % Reset path history at the start of animation to only get new simulation data
        pathHistory_q1 = [config(1)];
        pathHistory_q2 = [config(2)];
        
        dt = 0.01;
        tVec = 0:dt:tFinal;

        q = [config(1), config(2)];
        qd = [0, 0];

        tau = [0, 0];  % Zero torques (free motion)
    
        for i = 1:length(tVec)
            qdd = forwardDynamics(robot,q,qd,tau);

            qd = qd + qdd * dt;
            q = q + qd * dt;

            config(1) = q(1);
            config(2) = q(2);
            
            % Update path history (only during animation)
            pathHistory_q1(end+1) = config(1);
            pathHistory_q2(end+1) = config(2);
            
            replotRobot();
            %pause(dt);
        end
    end

    function out = ternary(cond, valTrue, valFalse)
        if cond
            out = valTrue;
        else
            out = valFalse;
        end
    end

end
