test_Reach.m

%% ------------------------------------------------------
%  Static Wheelchair Pose Visualization (3D)
% ------------------------------------------------------
clear; clc; close all;

c = distinguishable_colors(20);

addpath("stlTools/");
addpath("path-planning/");
addpath("voxels/")

%% ------------------------------------------------------
%  Load STL Room
% ------------------------------------------------------
path = fullfile("stl/","PP_room4.stl");
[vertices, faces, ~, ~] = stlRead(path);

room.vertices = vertices * 0.0254;   % inch → meter
room.faces    = faces;

%% ------------------------------------------------------
%  Initialize Wheelchair
% ------------------------------------------------------
wc = WheelChair(3, path);

% Wheelchair ↔ robot base transform (already given by you)
wc.setT_wc_robotBase( ...
    [[0, 0, 1; 
      1, 0, 0; 
      0, 1, 0], ...
     [0.35; -0.3; 0.5]; 
     0 0 0 1]);

% Place wheelchair in world
wc.T_world_wc = ...
    [[-1, 0, 0; 
      0, -1, 0; 
      0, 0, 1], ...
     [2; 2.6; 0.005]; 
     0 0 0 1];

%% ------------------------------------------------------
%  Create Figure
% ------------------------------------------------------
figure('Color','w');
hold on; grid on;

%% --- Plot Room ---
trisurf(room.faces, ...
        room.vertices(:,1), ...
        room.vertices(:,2), ...
        room.vertices(:,3), ...
        'FaceColor', [0.8 0.85 1.0], ...
        'EdgeColor', 'none', ...
        'FaceAlpha', 0.25);

camlight headlight;
lighting gouraud;

%% --- Plot Wheelchair Bounding Box ---
[Vbb, ~] = wc.boundingBox();

% lift slightly above floor for visibility
Vbb(:,3) = Vbb(:,3) + 0.02;

plot3(Vbb(:,1), Vbb(:,2), Vbb(:,3), ...
      'Color', c(5,:), ...
      'LineWidth', 2, ...
      'DisplayName', 'Wheelchair');

%% --- Plot Wheelchair Origin ---
p_wc = wc.T_world_wc(1:3,4);

plot3(p_wc(1), p_wc(2), p_wc(3), ...
      'ko', 'MarkerFaceColor','k', ...
      'MarkerSize', 8, ...
      'DisplayName','Wheelchair Origin');

%% --- Plot Wheelchair Body Frame ---
scale = 0.4;
R = wc.T_world_wc(1:3,1:3);

quiver3(p_wc(1), p_wc(2), p_wc(3), ...
        scale*R(1,1), scale*R(2,1), scale*R(3,1), ...
        'r', 'LineWidth', 2, 'DisplayName','x_{wc}');
quiver3(p_wc(1), p_wc(2), p_wc(3), ...
        scale*R(1,2), scale*R(2,2), scale*R(3,2), ...
        'g', 'LineWidth', 2, 'DisplayName','y_{wc}');
quiver3(p_wc(1), p_wc(2), p_wc(3), ...
        scale*R(1,3), scale*R(2,3), scale*R(3,3), ...
        'b', 'LineWidth', 2, 'DisplayName','z_{wc}');

%% ------------------------------------------------------
%  Figure Formatting
% ------------------------------------------------------
axis equal;
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
title('Wheelchair Current Pose in World Frame');

set(gca, 'FontName','CMU Serif', 'FontSize',12);

% legend('Location','best');

view(45,30);                 % nice 3D angle
camproj('perspective');


%%
room.baseTransform = wc.T_world_wc * wc.T_wc_robotBase;

qqlist = [];
pplist = [];
TwcList = [];
idx = 1;
idxList = [];

hw = waitbar(0, 'Sampling the configuration space. Please wait...');

nSteps = 1;
% N = 200;        % number of moves
XY = zeros(nSteps,2);% store wheelchair positions

for i = 1:nSteps
    % add wheelchair into workspace
    % wc.spawnWheelChair();
    % wc.driveWheelChair();
    [V, F] = wc.boundingBox();
    [Vnew, Fnew] = stlAddVerts(wc.Vertices, wc.Faces, V, F);
    room.vertices = Vnew;
    room.faces = Fnew;
    % Vnew = wc.Vertices;
    % Fnew = wc.Faces;
    
    % Store (x,y)
    XY(i,:) = wc.T_world_wc(1:2,4)';

    %% Part 1. Step-by-step testing of RRT
    fprintf('Testing RRT...\n')

    robot = wc.robots(:);

    room.baseTransform = wc.T_world_wc * wc.T_wc_robotBase;
    [qListNormalized,qList,pList,aList] = rrt_wc(robot, wc, room, 10);
    
    disp(qList)
    % qqlist = [qqlist, qList];
    if size(qList, 2) == 1
        % If qlist has one column, add the value directly
        qqlist = [qqlist, qList];
    else
        % If qlist has two columns, add the second column value
        qqlist = [qqlist, qList(:, 2:end)];
    end
    
    % pplist = [pplist, pList];
    if size(pList, 2) == 1
        % If qlist has one column, add the value directly
        pplist = [pplist, pList];
    else
        % If qlist has two columns, add the second column value
        pplist = [pplist, pList(:, 2:end)];
    end
    TwcList = [TwcList; wc.T_world_wc];
    idxList = [idxList, ones(1, size(qList, 2) - 1)*idx];
    idx = idx + 1;

    waitbar(i/nSteps, hw, 'Overall Status');

    % toc;
end
close(hw);

fprintf(['RRT execution complete. Total sampled points: ' num2str(size(qList,2) - 1) ' \n\n']);


%% ======================================================
%  General Robot Visualization Inside Testing Environment
% =======================================================
figure

% --- Plot environment ---
hEnv = stlPlot(Vnew, Fnew, 'Collision detection test.');
hold on
axis equal
grid on

robot   = wc.robots;
nRobots = length(robot);

%% --- Initial plot (first configuration) ---
ii = 1;
qq = qqlist(:,ii);

% base transform for this pose
a = 4 * idxList(ii) - 3;
b = 4 * idxList(ii);
Tprev = TwcList(a:b,:) * wc.T_wc_robotBase;

X = []; Y = []; Z = [];

for r = 1:nRobots
    idx = 3*(r-1) + (1:3);

    bb = robot(r).backbone( ...
        qq(idx(1)), qq(idx(2)), qq(idx(3)), ...
        Tprev, 100);

    model = robot(r).robot_body(bb, 2, 7);

    X = [X; model.X];
    Y = [Y; model.Y];
    Z = [Z; model.Z];

    Tprev = robot(r).fkin( ...
        qq(idx(1)), qq(idx(2)), qq(idx(3)), ...
        Tprev);
end

hRobot = surf(X, Y, Z, 'FaceColor','blue', 'EdgeColor','none');

%% ======================================================
%  Interactive browsing
% ======================================================
while true
    qq = qqlist(:,ii);

    % Wheelchair pose for this sample
    a = 4 * idxList(ii) - 3;
    b = 4 * idxList(ii);
    Tprev = TwcList(a:b,:) * wc.T_wc_robotBase;

    X = []; Y = []; Z = [];

    for r = 1:nRobots
        idx = 3*(r-1) + (1:3);

        bb = robot(r).backbone( ...
            qq(idx(1)), qq(idx(2)), qq(idx(3)), ...
            Tprev, 100);

        model = robot(r).robot_body(bb, 2, 7);

        X = [X; model.X];
        Y = [Y; model.Y];
        Z = [Z; model.Z];

        Tprev = robot(r).fkin( ...
            qq(idx(1)), qq(idx(2)), qq(idx(3)), ...
            Tprev);
    end

    % Update robot surface
    hRobot.XData = X;
    hRobot.YData = Y;
    hRobot.ZData = Z;

    % Plot wheelchair trajectory (static)
    plot3(XY(:,1), XY(:,2), 0.05*ones(size(XY,1),1), ...
        '-o', 'Color', c(13,:), 'LineWidth', 2, ...
        'MarkerFaceColor', c(13,:), 'MarkerSize', 2);

    title(sprintf('Pose %d of %d', ii, size(pplist,2)));

    fprintf('Press "n" to move forward or "p" to move back.\n');
    fprintf('Press any other key to stop.\n\n');

    while ~waitforbuttonpress, end
    k = get(gcf, 'CurrentCharacter');

    switch k
        case 'p'
            ii = max(ii-1, 1);
        case 'n'
            ii = min(ii+1, size(pplist,2));
        otherwise
            break
    end
end



%%
close all



% figure
% scatter3(qList(1,:), qList(2,:), qList(3,:));
% axis equal, grid on
% xlabel('Robot Arc Length s [mm]');
% ylabel('Bending Angle \theta [rad]');
% zlabel('Bending Direction \phi [rad]');
% title('Configurations generated by RRT');
% 
% figure
% scatter3(qListNormalized(1,:), qListNormalized(2,:), qListNormalized(3,:));
% axis equal, grid on
% xlabel('Robot Arc Length s [mm]');
% ylabel('Bending Angle \theta [rad]');
% zlabel('Bending Direction \phi [rad]');
% title('Configurations generated by RRT (normalized)');
% 
% Visualize ear model

fprintf('\n Generating reachable workspace...\n')
shrinkFactor = 1;
[k,v] = boundary(pplist(end-2, :)', pplist(end-1, :)', pplist(end, :)', shrinkFactor);

figure, hold on
stlPlot(wc.Vertices, wc.Faces, 'Synthetic Model');
view([17.8 30.2]);

scatter3(pplist(end-2, :), pplist(end-1, :), pplist(end, :),'red','filled');
axis equal, grid on
xlabel('X [m]'), ylabel('Y [m]'), zlabel('Z [m]');
title('Reachable points in the task space');

figure, hold on
stlPlot(wc.Vertices, wc.Faces, 'Synthetic Model');
view([17.8 30.2]);
trisurf(k, pplist(end-2, :)', pplist(end-1, :)', pplist(end, :)','FaceColor','red','FaceAlpha',0.1)
title('Reachable workspace');

fprintf('Testing complete.\n')

% reachableSpace(pplist)


%% Orientation score checking
pf = [];
scores = [];
for ii = 1:size(qqlist, 2)
    % Perform operations for each configuration in qqlist
    % Add your processing code here
    % qqq = qqlist(:,ii);
    qqq = reshape(qqlist(:, ii), 3, []);
    Tf = wc.fkin_nmodules(qqq, room.baseTransform);

    % disp("Tf")
    % disp(Tf)
    pf = [pf; Tf(1:3, 4)'];

    R_err = [0, 0 ,-1; -1, 0, 0; 0, 1, 0]' * Tf(1:3, 1:3);
    cos_theta = (trace(R_err) - 1) / 2;

    % Numerical safety
    cos_theta = max(min(cos_theta, 1), -1);

    theta = acos(cos_theta); % rotation distance in radians
    % disp("angle = ")
    % disp(rad2deg(theta))
    score = orientationScore([0, 0 ,-1; -1, 0, 0; 0, 1, 0], Tf(1:3, 1:3),0 , 0.7);
    % disp('Score =');
    % disp(score)
    scores = [scores, score];
end


%% Voxel Grapher
voxelSize = 0.25;

vizMode = "fullBox";
% Options:
%   "dots"        -> FAST (voxel centers only)
%   "visitedBox"  -> visited voxels as dashed boxes
%   "fullBox"     -> all voxels as dashed boxes (SLOW)

%% ------------------------------------------------------
%  Define Subspace (4 vertices)
% ------------------------------------------------------
V = [1 2.5 0;
     2 2.5 0;
     1 3.5 1;
     2 3.5 1];

voxels = buildVoxelGrid(V, voxelSize);
fprintf('Total voxels: %d\n', size(voxels.centers,1));

%% ------------------------------------------------------
%  Count Visited Voxels
% ------------------------------------------------------
stats = countVisitedVoxels(pf, voxels);

fprintf('Visited voxels: %d\n', stats.visitedVoxels);
fprintf('Coverage ratio: %.2f %%\n', 100*stats.visitedRatio);

visitedCenters   = voxels.centers(stats.visitedIdx,:);
unvisitedCenters = voxels.centers(setdiff(1:end, stats.visitedIdx), :);

%% ======================================================
%  VISUALIZATION 1: Grid + Trajectory (FIXED)
% ======================================================
figure('Color','w');
hold on; grid on;

% --- Always create a dummy voxel handle for legend ---
hVoxel = plot3(NaN,NaN,NaN,'--', ...
    'Color',[0.75 0.75 0.75],'LineWidth',0.6);

switch vizMode
    case "dots"
        scatter3(voxels.centers(:,1), ...
                 voxels.centers(:,2), ...
                 voxels.centers(:,3), ...
                 10, [0.75 0.75 0.75], 'filled');

    case "visitedBox"
        % For visitedBox mode, show dots (fast) in overview
        scatter3(voxels.centers(:,1), ...
                 voxels.centers(:,2), ...
                 voxels.centers(:,3), ...
                 6, [0.85 0.85 0.85], 'filled');

    case "fullBox"
        drawVoxelWireframe(voxels.centers, voxelSize, ...
            [0.75 0.75 0.75], 0.6, '--');
end

% --- Trajectory ---
hTraj = scatter3(pf(:,1), pf(:,2), pf(:,3), ...
              'r', 'LineWidth', 2);

axis equal;
view(45,25);
camproj('perspective');

xlabel('X'); ylabel('Y'); zlabel('Z');
title('Voxel Grid with Trajectory');

legend([hVoxel, hTraj], ...
       {'Voxel grid','End-effector'}, ...
       'Location','best');

set(gca,'FontSize',12, 'FontName', 'CMU Serif');

%% ======================================================
%  VISUALIZATION 2: Coverage (Hybrid & Scalable)
% ======================================================
figure('Color','w');
hold on; grid on;

switch vizMode
    case "dots"
        hUnvisited = scatter3(unvisitedCenters(:,1), ...
                               unvisitedCenters(:,2), ...
                               unvisitedCenters(:,3), ...
                               8, [0.85 0.85 0.85], 'filled');

        hVisited = scatter3(visitedCenters(:,1), ...
                             visitedCenters(:,2), ...
                             visitedCenters(:,3), ...
                             20, [0 0.45 0.9], 'filled');

    case "visitedBox"
        scatter3(unvisitedCenters(:,1), ...
                 unvisitedCenters(:,2), ...
                 unvisitedCenters(:,3), ...
                 6, [0.9 0.9 0.9], 'filled');

        drawVoxelWireframe(visitedCenters, voxelSize, ...
            [0 0.45 0.9], 1.5, '--');

        hUnvisited = plot3(NaN,NaN,NaN,'.','Color',[0.85 0.85 0.85]);
        hVisited   = plot3(NaN,NaN,NaN,'--','Color',[0 0.45 0.9],'LineWidth',1.5);

    case "fullBox"
        drawVoxelWireframe(unvisitedCenters, voxelSize, ...
            [0.85 0.85 0.85], 0.4, '--');
        drawVoxelWireframe(visitedCenters, voxelSize, ...
            [0 0.45 0.9], 1.5, '--');

        hUnvisited = plot3(NaN,NaN,NaN,'--','Color',[0.85 0.85 0.85]);
        hVisited   = plot3(NaN,NaN,NaN,'--','Color',[0 0.45 0.9],'LineWidth',1.5);
end

hTraj = scatter3(pf(:,1), pf(:,2), pf(:,3), ...
              'r', 'LineWidth', 2);

axis equal;
view(45,25);

xlabel('X'); ylabel('Y'); zlabel('Z');
title(sprintf('Voxel Coverage (%d / %d)', ...
      stats.visitedVoxels, stats.totalVoxels));

legend([hUnvisited, hVisited, hTraj], ...
       {'Unvisited voxels','Visited voxels','End-effector'}, ...
       'Location','best');

set(gca,'FontSize',12, 'FontName', 'CMU Serif');

%% ======================================================
%  VISUALIZATION 3: Visited Voxels Only
% ======================================================
figure('Color','w');
hold on; grid on;

switch vizMode
    case "dots"
        hVisited = scatter3(visitedCenters(:,1), ...
                             visitedCenters(:,2), ...
                             visitedCenters(:,3), ...
                             25, [0 0.45 0.9], 'filled');

    otherwise
        drawVoxelWireframe(visitedCenters, voxelSize, ...
            [0 0.45 0.9], 1.6, '--');
        hVisited = plot3(NaN,NaN,NaN,'--', ...
            'Color',[0 0.45 0.9],'LineWidth',1.6);
end

hTraj = scatter3(pf(:,1), pf(:,2), pf(:,3), ...
              'r', 'LineWidth', 2);

axis equal;
view(45,25);

xlabel('X'); ylabel('Y'); zlabel('Z');
title('Visited Voxels Only');

legend([hVisited, hTraj], ...
       {'Visited voxels','End-effector'}, ...
       'Location','best');

set(gca,'FontSize',12, 'FontName', 'CMU Serif');

%% Reachability

hit = findVoxelHitIndices(pf, voxels);

voxelScore = bestOrientationPerVoxel( ...
    hit, scores, size(voxels.centers,1));

reachability = computeReachability(voxelScore) * 100;

fprintf('Reachability = %.4f %%\n', reachability);