% Bill Herrington
% April 23, 2009
% GPU TARDEC project
% hub_matlab.m

% This function will generate the hub height using matlab

function HubHeight=hub_matlab2(r,X,Y,Z, Num)

% Roll along
Last=size(X,2);
HubHeight=zeros(size(X));

for i1=1:Last
    %fprintf('(i1,max)=(%d,%d)\n', i1, Last);
    Cx=X(i1);
    Cy=Y(i1);
    
    % Find start of wheel-ground interaction
    if (i1>Num)
        tempfirst=i1-Num;
    else
        tempfirst=1;
    end

    % Find end of wheel ground interaction
    if (i1+Num<Last)
        templast=i1+Num;
    else
        templast=Last;
    end

    % Over the given range, compute implied wheel height */
    Max=Z(i1);
    for i2=tempfirst:templast
        distance=sqrt((X(i2)-Cx).^2+(Y(i2)-Cy).^2);
        
        if (distance<r)
            TempHeight=Z(i2)+r*abs(cos(asin(distance/r)));
            
            if (TempHeight>Max)
                Max=TempHeight;
            end
            
        end
        
    end

    HubHeight(i1) = Max;
    
%     % At each point calculate the circle defining the wheel
%     if i1<Last
%         theta=atan((-Cy+Ytemp(CenterRow,i1+1))/(-Cx+Xtemp(CenterRow,i1+1)));
%     else
%         theta=atan((Cy-Ytemp(CenterRow,i1-1))/(Cx-Xtemp(CenterRow,i1-1)));
%     end
%     
%     Xp=rtemp*cos(theta);
%     Yp=rtemp*sin(theta);
%     Zp=sqrt(rtemp.^2-Xp.^2-Yp.^2);
%     
%     WheelX(i1,:)=[Xp,Xp]+Cx;
%     WheelY(i1,:)=[Yp,Yp]+Cy;
%     WheelZ(i1,:)=[Zp,-Zp]+Cz;
    
end