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

% This file will open a section of track data. We will assume that a three
% wheeled cart rolls down the center of this data. At the first position,
% for a given cart width, the index difference from left to right wheel
% will be calculated. Also the index difference between front wheel and
% back wheels will be calculated. For simplicity we will assume that these
% index differences don't change along the data path.

clear;

% Path Variables
ProjectPath='/home/bill/Desktop/6_338_Project/';
FolderPath='SmallFiles/data/';
Data_Name='94w_C1_0.xyz';
DataType=94;

filename=[ProjectPath FolderPath Data_Name];

% User set values
r=30*10^-2; % Wheel Radius in m
L=2;        % Cart length in meters
w=1;     % Cart Width in meters

% Load Data from file
xyz=load(filename, '-ascii');
Xt=reshape(xyz(:,1),DataType,[]);
Yt=reshape(xyz(:,2),DataType,[]);
Zt=reshape(xyz(:,3),DataType,[]);
clear('xyz');

% Grab center Row
CenterRow=47;
Xcenter=single(Xt(CenterRow,:));
Ycenter=single(Yt(CenterRow,:));
Zcenter=single(Zt(CenterRow,:));
Cx=Xcenter(1);
Cy=Ycenter(1);
Cz=Zcenter(1);

HorDist=sqrt((Xt(:,1)-Cx).^2+(Yt(:,1)-Cy).^2);

% Grab Left Row
[A B]=min(abs(HorDist(1:CenterRow)-w/2));
if((HorDist(B)-w/2)>0)
    LeftRow=B+1;
else
    LeftRow=B;
end
Xleft=single(Xt(LeftRow,:));
Yleft=single(Yt(LeftRow,:));
Zleft=single(Zt(LeftRow,:));


% Grab Right Row
[A B]=min(abs(HorDist(CenterRow:end)-w/2));
if((HorDist(B)-w/2)>0)
    RightRow=CenterRow+B-1;
else
    RightRow=CenterRow+B;
end
Xright=single(Xt(RightRow,:));
Yright=single(Yt(RightRow,:));
Zright=single(Zt(RightRow,:));

%clear('Xt','Yt','Zt');

% Search Forward for the first place we can get the whole vehicle on the
% data set
FDist=sqrt((Xcenter(:)-Cx).^2+(Ycenter(:)-Cy).^2);
[A B]=min(abs(FDist(:)-L));
if FDist(B)>L
    IndexDiff=B-1;
    start=B;
else
    IndexDiff=B;
    start=B+1;
end
clear('FDist');

% Last Point in the dataset is the last Point we put a wheel
Last=size(Xcenter,2);

% Find average number of data points to cover one wheel diameter
DiffTemp=sqrt((Xcenter(1:100)-Xcenter(2:101)).^2+(Ycenter(1:100)-Ycenter(2:101)).^2);
Av=sum(DiffTemp)/size(DiffTemp,2);
Num=int32(2*ceil(r/Av));

% Get Hub Height along each track
HubHeight_Left=hub_matlab2(r,Xleft(1:Last),Yleft(1:Last),Zleft(1:Last), Num); % second matlab version
HubHeight_Center=hub_matlab2(r,Xcenter(1:Last),Ycenter(1:Last),Zcenter(1:Last), Num); % second matlab version
HubHeight_Right=hub_matlab2(r,Xright(1:Last),Yright(1:Last),Zright(1:Last), Num); % second matlab version


% Now Display the result

% Front wheel
Fz=HubHeight_Center(start:Last);
Fx=Xcenter(start:Last);
Fy=Ycenter(start:Last);

% Back Left wheel
BLz=HubHeight_Left((start-IndexDiff):(Last-IndexDiff));
BLx=Xleft((start-IndexDiff):(Last-IndexDiff));
BLy=Yleft((start-IndexDiff):(Last-IndexDiff));

% Back Right Wheel
BRz=HubHeight_Right((start-IndexDiff):(Last-IndexDiff));
BRx=Xright((start-IndexDiff):(Last-IndexDiff));
BRy=Yright((start-IndexDiff):(Last-IndexDiff));

% Center Point
CenterZ=(Fz+BLz+BRz)/3;
CenterY=(Fy+BLy+BRy)/3;
CenterX=(Fx+BLx+BRx)/3;

% Correct for center (so the cart always stays at the origin
Fz=Fz-CenterZ;
Fy=Fy-CenterY;
Fx=Fx-CenterX;

BLz=BLz-CenterZ;
BLy=BLy-CenterY;
BLx=BLx-CenterX;

BRz=BRz-CenterZ;
BRy=BRy-CenterY;
BRx=BRx-CenterX;


%set(gca,'nextplot','replacechildren');

% Now make a movie looking at the triangle
figure(1);
for i1=1:size(CenterZ,2)
    PointsX=[BLx(i1);BRx(i1);Fx(i1);BLx(i1)];
    PointsY=[BLy(i1);BRy(i1);Fy(i1);BLy(i1)];
    PointsZ=[BLz(i1);BRz(i1);Fz(i1);BLz(i1)];
    plot3(PointsX,PointsY,PointsZ,'-rs', 'Linewidth', 2);hold on % add the ground in
    surf(Xt(1:10:end,1:10:end)-CenterX(i1),Yt(1:10:end,1:10:end)-CenterY(i1),Zt(1:10:end,1:10:end)-CenterZ(i1));
    axis([-2.5 2.5 -2.5 2.5 -2.5 2.5]); % It's important to fix the axis or the movie won't work
    M(i1) = getframe(gcf);
    hold off
end

% % Top View
% figure(2);
% for i1=1:size(CenterZ,2)
%     PointsX=[BLx(i1);BRx(i1);Fx(i1);BLx(i1)];
%     PointsY=[BLy(i1);BRy(i1);Fy(i1);BLy(i1)];
%     PointsZ=[BLz(i1);BRz(i1);Fz(i1);BLz(i1)];
%     plot(PointsX,PointsY,'-s')
%     
%     axis([-2.5 2.5 -2.5 2.5]); % It's important to fix the axis or the movie won't work
%     N(i1) = getframe(gcf);
%     
% end
% 
% 
% % Side
% figure(3);
% for i1=1:size(CenterZ,2)
%     PointsX=[BLx(i1);BRx(i1);Fx(i1);BLx(i1)];
%     PointsY=[BLy(i1);BRy(i1);Fy(i1);BLy(i1)];
%     PointsZ=[BLz(i1);BRz(i1);Fz(i1);BLz(i1)];
%     PointsD=[-sqrt(BLx(i1).^2+BLy(i1).^2);-sqrt(BRx(i1).^2+BRy(i1).^2);sqrt(Fx(i1).^2+Fy(i1).^2);-sqrt(BLx(i1).^2+BLy(i1).^2)];
%     
%     plot(PointsD,PointsZ,'-s')
%     axis([-2.5 2.5 -2.5 2.5]); % It's important to fix the axis or the movie won't work
%     P(i1) = getframe;
%     
% end