WheelChair.m

classdef WheelChair < OrigamiRobot
    % This is a subclass of the OrigamiRobot
    % Inputs: -robot_count: number of robots (defualt=1)
    %         -WheelRadius: radius of wheels in mm (default=30)
    %         -BaseWidth: distance between wheels in mm (default=50)
    % Execute this class will automaticall request the user to input robot
    % dimensions per robot_count.
    properties
        robot_count    (1,1) double       % number of robots
        room_stl       (1,1) string = ""  % path to wheelchair workspace
        wc_stl = ...
            fullfile("stl/wheelchair_may29new.stl")% stl path to wheelchair
        WheelRadius    (1,1) double       % Wheel radius of wheelchair
        BaseWidth      (1,1) double       % distance between wheels
        COMoffset      (1,1) double       % offset from wheelchari COM to wheels
        ODs            (:,1) double       % outer diameters (user input)
        InitLengths    (:,1) double       % initial arc lengths (user input)
        minLengths     (:,1) double       % minimal length of robots
        maxLengths     (:,1) double       % maximal length of robots
        robots         (:,1) OrigamiRobot % stores all the OrigamiRobots
        T_wc_robotBase (4,4) double ...
            = [1, 0, 0, 0; 0, 1, 0, -330/1000; 0, 0, 1, 1200/1000; 0, 0, 0, 1] ...
                                          % transformation WC-to-robotBase (traslation in mm)
        T_world_wc     (4,4) double=eye(4)% default wheelchair spawn
        prevT          (4,4) double       % previous transformation for driveWheelchair
        pg             (1,1)              % base polyshape of workspace
        Vertices                          % Vertices of workspace
        Faces                             % Faces of workspace
        Vwc0 = []                         % Vertices of wheelchair at init
        Vwc = []
        Fwc = []                          % Faces of wheelchair
    end
    
    methods
        %% 
        function obj = WheelChair(robot_count, room_stl, wheelRadius, baseWidth, COM)
            arguments
                robot_count   (1,1) double = 1
                room_stl      (1,1) string = ""
                wheelRadius   (1,1) double = 0.5 % [m]
                baseWidth     (1,1) double = 0.7 % [m]
                COM           (1,1) double = 0.2 % [m]
            end
            % Assign the simple properties
            obj.WheelRadius  = wheelRadius;
            obj.room_stl = room_stl;
            obj.BaseWidth    = baseWidth;
            obj.robot_count = robot_count;
            obj.COMoffset = COM;
            
            % Pre-allocate storage
            obj.ODs         = zeros(robot_count,1);
            obj.InitLengths = zeros(robot_count,1);
            obj.minLengths = zeros(robot_count,1);
            obj.maxLengths = zeros(robot_count,1);
            
            % Prompt the user for each robot’s OD and initial length
            for i = 1:robot_count
                promptOD   = sprintf('Enter outer diameter (OD) for robot %d in m: ', i);
                promptLen  = sprintf('Enter initial arc length for robot %d in m: ', i);
                promptMinLen  = sprintf('Enter minimal length for robot %d in m: ', i);
                promptMaxLen  = sprintf('Enter maximal length for robot %d in m: ', i);
                
                % obj.InitLengths(i) = input(promptLen);

                % obj.ODs(i)         = input(promptOD);
                % obj.minLengths(i)  = input(promptMinLen);
                % obj.maxLengths(i)  = input(promptMaxLen);

                
                obj.ODs(i)         = 0.08;
                obj.InitLengths(i) = 0.1;
                obj.minLengths(i)  = 0.08;
                obj.maxLengths(i) = 0.2;
            end

            % Define the Origami Robots based on user inputs of Origami
            for i = 1:robot_count
                obj.robots(i) = OrigamiRobot(obj.ODs(i), obj.InitLengths(i), obj.minLengths(i), obj.maxLengths(i));
            end
            
            % Obtian workspace polygon
            obj.computeWorkspacePolygon();

            if obj.wc_stl ~= ""
                [Vwc0, obj.Fwc] = stlRead(obj.wc_stl);
                obj.Vwc0 = Vwc0 / 1000;
            end

        end

        %% 
        function setT_wc_robotBase(obj, T)
            % Change the default transformation T_wc_robotBase
            % Input: 4-by-4 Transformation Matrix
            arguments
                obj
                T (4,4) double
            end

            % Tolerance for numerical checks
            tol = 1e-10;
            
            % 1) Check bottom row:
            if any( abs(T(4,:) - [0 0 0 1]) > tol )
                error('WheelChair:setT_wc_robotBase:NotSE3', ...
                      'Bottom row must be [0 0 0 1].');
            end
            
            % 2) Extract rotation part and check orthonormality + det=1
            R = T(1:3,1:3);
            if norm(R'*R - eye(3), 'fro') > tol
                error('WheelChair:setT_wc_robotBase:NotSE3', ...
                      'Upper‐left 3×3 must be orthonormal (R''*R = I).');
            end
            if abs(det(R) - 1) > tol
                error('WheelChair:setT_wc_robotBase:NotSE3', ...
                      'Rotation part must have det(R)=+1.');
            end
            obj.T_wc_robotBase = T;
        end
        
        %% 
        function spawnWheelChair(obj)
            if isempty(obj.Vwc0) == false
                retry = true;
                while retry
                    theta = 2*pi*rand;
                    R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1];
                    p = [obj.samplePoly(), 0]';
                    T = [R, p; 0, 0, 0, 1];

                    
                    obj.T_world_wc = T;
                    [V_wc, ~] = obj.boundingBox();
                    
                    collision = intriangulation(obj.Vertices, obj.Faces, V_wc);
                    
                    collision = sum(collision);
                    if collision >= 1
                        % disp('collision')
                        continue
                    else
                        retry = false;
                    end
                    obj.T_world_wc = T;
                    % disp('save T')
                end
            else
                retry = true;
                if obj.room_stl == ""
                    theta = 2*pi*rand;
                    R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1];
                    p = [-5000+10000*rand, -5000+10000*rand, 0]';
                    obj.T_world_wc = [R, p; 0, 0, 0, 1];
                else
                    while retry
                        theta = 2*pi*rand;
                        R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1];
                        p = [obj.samplePoly(), 0]';
                        T = [R, p; 0, 0, 0, 1];

                        [v_wc, ~] = obj.boundingBox(T);
                        collision = intriangulation(obj.Vertices, obj.Faces, v_wc);

                        collision = sum(collision);
                        if collision >= 1
                            % disp("collision")
                            continue
                        else
                            retry = false;
                        end
                        obj.T_world_wc = [R, p; 0, 0, 0, 1];
                    end
                end
            end
        end

        %%
        function [v, triFaces] = boundingBox(obj, T)
            % generate the simple boundbox for wheelchair
            arguments
                obj
                T = []
            end
            T12 = obj.T_world_wc;
            R12 = T12(1:3, 1:3);
            p12 = T12(1:3, 4);
            R21 = R12';
            p21 = -R21*p12;
            T21 = [R21, p21; 0, 0, 0, 1];
            
            if obj.wc_stl == ""
                l = 800/1000;
                w = 315/1000;
                h = 920/1000;
                corners = [ ...
                    l, w, 0;
                    0, w, 0;
                    0, -w, 0;
                    l, -w, 0;
                    l, w, h;
                    0, w, h;
                    0, -w, h;
                    l, -w, h];

                if isempty(T)
                    V = (T12 * [corners, ones(8,1)]')';   % 8×4
                else
                    V = (T * [corners, ones(8,1)]')';
                end

                v = [V(:,1), V(:,2), V(:,3)];

                F = [ 1 2 3 4;   % bottom
                    5 6 7 8;   % top
                    1 2 6 5;   % front
                    2 3 7 6;   % right
                    3 4 8 7;   % back
                    4 1 5 8];  % left
                % Split each quad [a b c d] into two triangles [a b c] and [a c d]:
                T1 = F(:,[1 2 3]);   % first triangle of each quad
                T2 = F(:,[1 3 4]);   % second triangle of each quad

                % concatenate them
                triFaces = [T1; T2];   % (6*2)×3 = 12×3
                % Define all 12 edges of the box
                allEdges = [
                    1 2; 2 3; 3 4; 4 1;  % bottom
                    5 6; 6 7; 7 8; 8 5;  % top
                    1 5; 2 6; 3 7; 4 8]; % sides
                numEdges = size(allEdges,1);
                subdivCount = 4;  % number of segments per edge => 5 vertices
                % Preallocate index map: each row edge i, columns j=1..5 hold vertex indices
                edgeVertIdx = zeros(numEdges, subdivCount+1);

                for i = 1:numEdges
                    a = allEdges(i,1);
                    b = allEdges(i,2);
                    for j = 0:subdivCount
                        t = j/subdivCount;
                        pt = (1-t)*v(a,:) + t*v(b,:);
                        if j == 0
                            edgeVertIdx(i,j+1) = a;
                        elseif j == subdivCount
                            edgeVertIdx(i,j+1) = b;
                        else
                            v(end+1,:) = pt;
                            edgeVertIdx(i,j+1) = size(v,1);
                        end
                    end
                end
            else
                Vh = [obj.Vwc0, ones(size(obj.Vwc0,1),1)];
                if isempty(T)
                    Vh2 = (T12 * Vh')';
                else
                    Vh2 = (T * Vh')';
                end
                V2 = Vh2(:,1:3);
                obj.Vwc = V2;
                v = V2;
                triFaces = obj.Fwc;
            end
        end
        
        %%
        function computeWorkspacePolygon(obj)
            % compute the spawnable workspace for wheelchair
            if obj.room_stl ~= ""
                % 1) read the STL
                [V, F] = stlRead(obj.room_stl);  % V: N×3, F: M×3
                % V = V/1000; % from mm to m
                V = V * 0.0254; % from inches to m
                % 2) pick bottom faces
                zmin = min(V(:,3));
                tol  = 1e-6;
                Z1 = V(F(:,1),3); Z2 = V(F(:,2),3); Z3 = V(F(:,3),3);
                isBottom = abs(Z1-zmin)<tol & abs(Z2-zmin)<tol & abs(Z3-zmin)<tol;
                bottomF = F(isBottom,:);

                isAllZero = ~any(bottomF(:));

                if isAllZero == 0
                    % 3) triangulation + free boundary
                    TRb = triangulation(bottomF, V);
                    E   = freeBoundary(TRb);        % L×2 list of edges :contentReference[oaicite:1]{index=1}

                    % 4) group into loops
                    loops = obj.edgeLoops(E);

                    % 5) build P = [x y; NaN NaN; x y; NaN NaN; …]
                    P = NaN(0,2);
                    for k = 1:numel(loops)
                        idxs = loops{k};
                        coords = V(idxs,1:2);        % K×2
                        P = [P; coords; NaN NaN];   %#ok<AGROW>
                    end

                    % 6) create a single polyshape with holes from P
                    obj.pg = polyshape(P);         % NaN separators => holes :contentReference[oaicite:2]{index=2}

                    % 1) Get the NaN-delimited boundary of pg
                    [xb, yb] = boundary(obj.pg);           % xb,yb include NaNs between loops :contentReference[oaicite:0]{index=0}

                    % 2) Isolate only the *first* loop (outer boundary)
                    nanIdx = find(isnan(xb), 1);
                    if isempty(nanIdx)
                        outerX = xb;
                        outerY = yb;
                    else
                        outerX = xb(1:nanIdx-1);
                        outerY = yb(1:nanIdx-1);
                    end

                    % 3) Delaunay-triangulate those outer points
                    DT = delaunayTriangulation(outerX, outerY);   % :contentReference[oaicite:1]{index=1}
                    T  = DT.ConnectivityList;                     % M×3 array of triangles
                    P  = DT.Points;                               % Mpoints×2 coordinates

                    % 4) Keep only triangles whose centroids lie *inside* the outer polygon
                    cents = ( P(T(:,1),:) + P(T(:,2),:) + P(T(:,3),:) )/3;  % M×2
                    in   = inpolygon(cents(:,1), cents(:,2), outerX, outerY);
                    TF  = T(in,:);

                    % 5) Lift to 3D at z=0
                    Vg = [P, zeros(size(P,1),1)];  % N×3 vertices at z=0
                    Fg = TF;                        % faces
                else
                    obj.pg = polyshape(P);
                    % 1) Compute bounding box of the STL
                    xmin = min(V(:,1));
                    xmax = max(V(:,1));
                    ymin = min(V(:,2));
                    ymax = max(V(:,2));

                    % Optional: expand slightly beyond the object
                    pad = 0.1;  % 10 cm margin
                    xmin = xmin - pad;
                    xmax = xmax + pad;
                    ymin = ymin - pad;
                    ymax = ymax + pad;

                    % 2) Define rectangle vertices (at z=0)
                    Vg = [
                        xmin ymin 0;
                        xmax ymin 0;
                        xmax ymax 0;
                        xmin ymax 0
                        ];

                    % 3) Define faces (two triangles)
                    Fg = [
                        1 2 3;
                        1 3 4
                        ];
                end

                [obj.Vertices, obj.Faces] = stlAddVerts(V, F, Vg, Fg);
            end
        end

        function loops = edgeLoops(~,E)
            % worker function for computeWorkspacePolygon
            % Group undirected edges E (L×2) into closed loops
            % Build adjacency
            adj = containers.Map('KeyType','double','ValueType','any');
            for i=1:size(E,1)
                u=E(i,1); v=E(i,2);
                if ~isKey(adj,u), adj(u) = v; else tmp=adj(u); tmp(end+1)=v; adj(u)=tmp; end
                if ~isKey(adj,v), adj(v) = u; else tmp=adj(v); tmp(end+1)=u; adj(v)=tmp; end
            end
            visited = false(size(E,1),1);
            loops = {};
            for seed=1:size(E,1)
                if visited(seed), continue; end
                u = E(seed,1); v = E(seed,2);
                loop = u; visited(seed)=true;
                while v~=loop(1)
                    loop(end+1)=v;                              %#ok<AGROW>
                    % find any unused edge involving v
                    idx = find(((E(:,1)==v)|(E(:,2)==v)) & ~visited, 1);
                    if isempty(idx), break; end
                    visited(idx)=true;
                    % step to the other end
                    if E(idx,1)==v, v=E(idx,2); else v=E(idx,1); end
                end
                loops{end+1} = loop;                          %#ok<AGROW>
            end
        end

        %%
        function pts = samplePoly(obj, N)
            arguments
                obj
                N (1,1) double = 1
            end
            nan_list = isnan(obj.pg.Vertices);
            nan_idx = find(nan_list, 1, 'first');
            XY = [obj.pg.Vertices(nan_idx+1:end, 1), obj.pg.Vertices(nan_idx+1:end, 2)];
            tri = delaunay(XY);

            % use a 2x2 determinant, in a vectorized form
            v1 = XY(tri(:,1),:);
            v2 = XY(tri(:,2),:);
            v3 = XY(tri(:,3),:);

            % translate
            v1 = v1-v3;
            v2 = v2-v3;

            % vectorized determinant
            % divide by factorial(2) for the area
            areas = (v1(:,1).*v2(:,2) - v1(:,2).*v2(:,1))/2;

            % normalize the areas to sum to 1
            areas = areas/sum(areas);

            nsample = N;

            R = rand(nsample,1);
            tind = discretize(R,cumsum([0;areas]));

            v1 = XY(tri(tind,1),:);
            v2 = XY(tri(tind,2),:);
            v3 = XY(tri(tind,3),:);

            R1 = rand(nsample,1);
            xyrand = v1.*repmat(R1,[1 2]) + v2.*repmat(1-R1,[1 2]);
            R2 = sqrt(rand(nsample,1));
            pts = xyrand.*repmat(R2,[1 2]) + v3.*repmat(1-R2,[1 2]);
        end

        %%
        function T = drive(obj, dL, dR)
        % Drive wheelchair based on rotation of each wheel (arc length)
        % Pose is stored at COM; kinematics integrated at wheel axle
        
            r = obj.WheelRadius;
            L = obj.BaseWidth;
            d = obj.COMoffset;    
        
            % Current COM pose
            T0 = obj.T_world_wc;
            x0 = T0(1,4);
            y0 = T0(2,4);
            theta0 = atan2(T0(2,1), T0(1,1));
        
            % ------------------------------------
            % 1. COM → axle
            % ------------------------------------
            x_axle = x0 - d * cos(theta0);
            y_axle = y0 - d * sin(theta0);
        
            % ------------------------------------
            % 2. Differential-drive kinematics
            % ------------------------------------
            dphiL = dL / r;
            dphiR = dR / r;
        
            ds     = (r/2) * (dphiR + dphiL);
            dtheta = (r/L) * (dphiR - dphiL);
        
            theta_mid = theta0 + dtheta/2;
            x_axle_new = x_axle + ds * cos(theta_mid);
            y_axle_new = y_axle + ds * sin(theta_mid);
            theta_new  = theta0 + dtheta;
        
            % ------------------------------------
            % 3. Axle → COM
            % ------------------------------------
            x_new = x_axle_new + d * cos(theta_new);
            y_new = y_axle_new + d * sin(theta_new);
        
            % ------------------------------------
            % 4. Build SE(3) transform
            % ------------------------------------
            Rz = [ cos(theta_new), -sin(theta_new), 0;
                   sin(theta_new),  cos(theta_new), 0;
                   0,               0,              1 ];
        
            T = eye(4);
            T(1:3,1:3) = Rz;
            T(1:3,4)   = [x_new; y_new; 0];
        
            % Optionally store:
            % obj.T_world_wc = T;
        end


        %%
        function driveWheelChair(obj)
            count = 0;
            
            if isempty(obj.Vwc0) == false
                disp("drive Wheelchair randomly")
                retry = true;
                while retry
                    dl = rand(1)/4;
                    dr = rand(1)/4;
                    T = obj.drive(dl, dr);
                  
                    % obj.T_world_wc = T;
                    [V_wc, ~] = obj.boundingBox(T);
                    
                    collision = intriangulation(obj.Vertices, obj.Faces, V_wc);
                    alpha = 44;      % angle in degrees
                    R = [cosd(alpha), -sind(alpha), 0;
                         sind(alpha),  cosd(alpha), 0;
                              0 ,       0 , 1];
                    
                    collision = sum(collision);
                    if collision >= 1
                        disp('collision')
                        if count < 20
                            obj.T_world_wc = T * [R, [0,0,0]'; 0,0,0,1];
                            count = count + 1;
                        else
                            disp('Move back to last pose')
                            obj.T_world_wc = obj.prevT;
                            count = 0;
                        end
                        continue
                    else
                        retry = false;
                        count = 0;
                    end
                    obj.T_world_wc = T;
                    obj.prevT = T;
                    % disp('save T')
                end
            else
                retry = true;
                if obj.room_stl == ""
                    theta = 2*pi*rand;
                    R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1];
                    p = [-5000+10000*rand, -5000+10000*rand, 0]';
                    obj.T_world_wc = [R, p; 0, 0, 0, 1];
                else
                    disp("no wheelchair stl")
                    while retry
                        dl = rand(1);
                        dr = rand(1);
                        T = obj.drive(dl, dr);

                        [v_wc, ~] = obj.boundingBox(T);
                        collision = intriangulation(obj.Vertices, obj.Faces, v_wc);

                        collision = sum(collision);
                        if collision >= 1
                            % disp("collision")
                            continue
                        else
                            retry = false;
                        end
                        obj.T_world_wc = T;
                    end
                end
            end
        end



    end
end