%main tracker script
%Written by Hailin Jin and Paolo Favaro
%Copyright (c) Washington University, 2001
%All rights reserved
%Last updated 10/18/2001
%results are saved in result.mat
%the variables are good featx featy featq
clear all;

next = 1;

%rotating board sequence
seq_name = 'imd';
number_list = 200:5:250;
image_type = 'bmp';
spaces = 3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
interlaced = 0;     % Set to 1, if images are interlaced
bPlot = 1;          % Set to 1 to get graphical output
FigureNumber = 1;   % output figure number
bMovieout = 0;      % Can be used only if bPlot activated
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global resolution winx winy saturation ...
    wintx winty spacing boundary boundary_t ...
    Nmax thresh levelmin levelmax ThreshQ ...
    N_max_feat method;

resolution = 0.03; 			% Required resolution (for the track, in pixel)
winx = 3; winy = 3;         % Selection window sizes (size =2*winx+1 x 2*winy+1)
saturation = 7000; 			% saturation in Q for selection (win=1)
wintx = 7; winty = 7;     % Tracking window sizes
spacing = 5;				% min spacing between 2 feats (in pixel).
boundary = 2;				% rejected pixels around the screen (selection)
boundary_t = 1;				% rejected pixels around the screen (tracking)
Nmax = 500;                 % max. selected features in selection
thresh = 0.05; 				% Threshold of selection
levelmin = 1; 				% lower level in the pyramid
levelmax = 3;				% higher level in the pyramid
ThreshQ = 0.1;				% Thresh of ejection of a point through the track
N_max_feat = 200;           % Minimum space reserved for the feature storage
method = 0;                 % Set to 1 to take into consideration
                            % the saturation (used in selection and tracking)
                              
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

DimX=5;
DimY=5;

disp('New tracking sequence');

if interlaced,
    disp('Input images interlaced');
else
    disp('Input images non interlaced');
end;

if bPlot,
    disp('Graphical output on');
    if bMovieout,
        disp('Movie on');
    else
        disp('Movie off');
    end;
else
    disp('Graphical output off');
end;

if method,
    disp('Saturation on');
else
    disp('Saturation off');
end;

% duration = length(number_list);

duration = 30;                  %number of frames to be tracked

dd = zeros(1,duration);

% opt = sprintf('%%0%dd',spaces);

good = zeros(N_max_feat,duration);
featx = zeros(N_max_feat,duration);
featy = zeros(N_max_feat,duration);
featq = zeros(N_max_feat,duration);

%process the first image

% index = sprintf(opt,number_list(1));
% first = sprintf('%s%s.%s',seq_name,index,image_type);
% 
% tt = ReadImage(first,image_type,320,240);

% fmov = aviread('x.avi',1:50); %read the AVI file
% tt3 = fmov(1,1).cdata; %true color
% tt = tt3(:,:,1);

% fmov = aviread('Craw.avi',43:58); %read the AVI file
% tdata = fmov(1,1).cdata; %indexed
% tmap = fmov(1,1).colormap;
% tt3 = ind2rgb(tdata, tmap);
% tt = tt3(:,:,1);
% tt = double(tt);
% [nx,ny] = size(tt);

filename='coastguard.qcif';
frames=150;
col=176;
row=144;
UV_ratio=2;
UV_col=col/UV_ratio;
UV_row=row/UV_ratio;
Y=zeros(row,col,frames);

% open file
Y_size=col*row;
U_size=UV_col*UV_row;
V_size=UV_col*UV_row;
buffer_size=Y_size+U_size+V_size;

[fid,message]=fopen(filename,'r');


% for ith_frame=1:1:frames
for pp = 1:100
[raw_YUV,count]=fread(fid,buffer_size,'uint8');
temp_Y_frame=reshape(uint8(raw_YUV(1:Y_size)),[col row])';  
temp_U_frame=reshape(uint8(raw_YUV(Y_size+1:Y_size+U_size)),[UV_col UV_row])';  
temp_V_frame=reshape(uint8(raw_YUV(Y_size+U_size+1:Y_size+U_size+V_size)),[UV_col UV_row])';  
end
%   subplot(3,1,1);  imshow(temp_Y_frame,[0 255]); title('Y');
%   subplot(3,1,2);  imshow(temp_U_frame,[0 255]); title('U');
%   subplot(3,1,3);  imshow(temp_V_frame,[0 255]); title('V');
%   xlabel(strcat(num2str(ith_frame),'th frame. Press any key to show next'));
%   pause
% end  

% status=fclose(fid);

tt = temp_Y_frame;
tt = double(tt);
[nx,ny] = size(tt);

if interlaced,
  Ipi(:,:,1) = tt(1:2:nx,1:2:ny,1); %throw away even lines
else,
  Ipi(:,:,1) = tt;
end;

[nrow,ncol] = size(Ipi(:,:,1));
SampleSize(:,1) = [nrow;ncol];

% fprintf(1,'\n');
% disp(['Feature selection on initial image ' first '...']);

%select feature point from Ipi
%only on the grid points

xtt = SelectFeature(Ipi);

Nini = size(xtt,2); 			% init # of features
if Nini < N_max_feat,
  xtt = [xtt,zeros(2,N_max_feat-Nini)];
  goodfeat = [ones(Nini,1);zeros(N_max_feat-Nini,1)];
else 
  xtt = xtt(:,1:N_max_feat);
  goodfeat = ones(N_max_feat,1);
end;

%%%%%%%%%%%%%%%%%%%%
%save the first
Ifirst = Ipi(:,:,1);
xttfirst = xtt;
%%%%%%%%%%%%%%%%%%%%

%note that SampleSize is only computed once here
%therefore all the images must have the same size
for ii=1:levelmax,
  % compute downsampled images
    [tt,SampleSize(:,ii+1)] ...
        = DownSample(Ipi(:,:,ii),SampleSize(:,ii));       %The method??
    Ipi(1:SampleSize(1,ii+1),1:SampleSize(2,ii+1),ii+1) = tt;
end;

fprintf(1, 'On initial image: %d features\n', size(find(goodfeat),1));

dd(1,1) = (size(find(goodfeat),1));

Qtt = ComputeQuality(Ipi,xtt,goodfeat,wintx,winty);
%quality_computation; 		% computes the quality vector Qtt from xtt and Ipi used to test lost tracks!

good(:,1) = goodfeat;
featx(:,1) = xtt(1,:)';
featy(:,1) = xtt(2,:)';
featq(:,1) = Qtt;

aviobj = avifile('test.avi', 'colormap', colormap(gray(256)), 'fps', 15, 'compression', 'none');
								
if bPlot,
  % PLOT FIRST IMAGE !!!
  figure(FigureNumber);hold off; clf;
  h = imshow(Ipi(:,:,1), [0,255]);colormap(gray(256));
  axis('equal'); axis([1 ncol 1 nrow]);
  hold on;
  xf = xtt(:,find(goodfeat));
  if size(xf,1) > 0,
    plot(xf(2,:),xf(1,:), 'r+');
  end;
  hold off; drawnow;
  
  F = getframe;
  aviobj = addframe(aviobj,F);
  
end;

%goodfeat = zeros(N_max_feat,1);

%%% MAIN TRACKING LOOP - BELOW
for nbri=2:duration;

%   seq = number_list(nbri);
  
  for ii = 1:levelmax+1,
    Ii(:,:,ii) = Ipi(:,:,ii);   %Ii saves the previous image
  end;
  
  Qt = Qtt;     %Qt saves the previous quantities
  xt = xtt;     %xt saves the previous features
  
%   next = sprintf('%s%s.%s',seq_name,sprintf(opt,seq),image_type);
%   
%   fprintf(1, '\nTracking on image %s...\n',next);
%   
%   tt = ReadImage(next,image_type,320,240);

next = next+1;      % read the next image
% tt3 = fmov(1,next).cdata;
% tt = tt3(:,:,1);  
% tt = double(tt);

% tdata = fmov(1,next).cdata; %indexed
% tmap = fmov(1,next).colormap;
% tt3 = ind2rgb(tdata, tmap);
% tt = tt3(:,:,1);
% tt = double(tt);

[raw_YUV,count]=fread(fid,buffer_size,'uint8');
temp_Y_frame=reshape(uint8(raw_YUV(1:Y_size)),[col row])';  
temp_U_frame=reshape(uint8(raw_YUV(Y_size+1:Y_size+U_size)),[UV_col UV_row])';  
temp_V_frame=reshape(uint8(raw_YUV(Y_size+U_size+1:Y_size+U_size+V_size)),[UV_col UV_row])';  

tt = temp_Y_frame;
tt = double(tt);


  if interlaced,
    Ipi(:,:,1) = tt(1:2:nx,1:2:ny,1);
  else,
    Ipi(:,:,1) = tt;
  end;
  
  % Downsampling images
  for ii = 1:levelmax,
    Ipi(1:SampleSize(1,ii+1),1:SampleSize(2,ii+1),ii+1) ...
	= DownSample(Ipi(:,:,ii),SampleSize(:,ii));
  end;
  
  %track between image Ii and Ipi %%%%%%%%
  %also their downsampled versions %%%%%%%
  %results are in xtt Qtt goodfeat %%%%%%%
  
  
  [xttnew,Qtt,goodfeat] = TKTrack(Ii,Ipi,SampleSize,xtt,Qt,goodfeat);

  good(:,nbri) = goodfeat;
  featx(:,nbri) = xttnew(1,:)';
  featy(:,nbri) = xttnew(2,:)';
  featq(:,nbri) = Qtt;

  xtt = xttnew;
  
  %%%%%%%%%%%%%%%%%  
  
  fprintf(1, 'After track #%d: %d features\n',nbri-1, size(find(goodfeat),1));
  
  dd(1,nbri) = size(find(goodfeat),1);
  
  %visualization
  if bPlot,
%     figure(FigureNumber);
    figure
    h = imshow(Ipi(:,:,1),[0,255]); colormap(gray(256));
    axis('equal'); axis([1 ncol 1 nrow]);
    hold on;
    xf = xtt(:,find(goodfeat));
    if size(xf,1) > 0,
      plot(xf(2,:),xf(1,:), 'r+');
      ind_tracked = find(good(:,nbri-1) & good(:,nbri));
      if length(ind_tracked) > 0,
	plot([featy(ind_tracked,nbri-1),featy(ind_tracked,nbri)]',[featx(ind_tracked,nbri-1),featx(ind_tracked,nbri)]', 'y-');
      end;
    end;
    hold off; drawnow;
    
  F = getframe;
  aviobj = addframe(aviobj,F);
    
    if bMovieout,
      Mv(:,nbri-1) = getframe;
    end;
  end;
end;
%%% MAIN TRACKING LOOP - ABOVE

aviobj = close(aviobj);

return;
save result good featx featy featq;
