%%========================================================
%       The 3 sections below to load the big 2D txt files into Matlab's
%       saved .mat files. Rather slow, but now instead of file-lookups, we
%       can do find's if we already have the iBatch number right.
%%========================================================

% One at a time, each of these 3 sequences:
        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\05VD_v01_'; numDigs = 4; 
        iFirsts = [0  750  1500 2500 3500 4500 5500];
        iLasts = [800 1550 2550 3550 4550 5550 6598];

        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\16E5_v01_'; numDigs = 5;
        iFirsts = [0    950 1950 2950 3950 4950 5950 6950 7950 8950  9950 10950]; 
        iLasts = [1000 2000 3000 4000 5000 6000 7000 8000 9000 10000 11000 11380];

        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\06R0_v01_'; numDigs = 5;
        iFirsts = [0    950 1950 2950 3950 4950 5950];
        iLasts = [1000 2000 3000 4000 5000 6000 7168];

        SeqTemplate = 'G:\data\CambridgeTraffic\01TP_trax\0001TP_v01_'; numDigs = 6;
        iFirsts = [6690];
        iLasts = [10380];



for( iBatch = 1:size(iFirsts(:),1) )
    if( numDigs == 4 )
        matFileName = sprintf( '%s%.4d_%.4d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        txt2DfileName = sprintf( '%s%.4d_%.4d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );

    elseif( numDigs == 5 )
        matFileName = sprintf( '%s%.5d_%.5d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        txt2DfileName = sprintf( '%s%.5d_%.5d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    elseif( numDigs == 6 )
        matFileName = sprintf( '%s%.6d_%.6d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        txt2DfileName = sprintf( '%s%.6d_%.6d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    end;
    
    fid = fopen(txt2DfileName,'rt');      % 'rt' means read text
    if (fid < 0)    
        error('could not open file');   % just abort if error
    end;

    s = fgetl(fid);                      % get a line
    [tok tokRest] = strtok( s );
    while( strcmp( tok, '#') == 1 )
        s = fgetl(fid);                      % get a line
        [tok tokRest] = strtok( s );
    end;
    % Ok, now s has the first line of a 2D feature, in the format:
    %         auto_0  1503  934.281  558.474
    %         auto_0  1504  933.87  555.825
    %         auto_1  1500  859.231  516.434
    
    TrajTable = []; % Big table to store all our Trajs, one block of each Traj at a time.
    oldTraj = -1;

    growdata
    while (ischar(s))                   % while not end of file
        [A] = sscanf( s,'auto_%d %d %f %f\n', 4 );
        %TrajTable = [TrajTable; A'];
        growdata(A');
        s = fgetl(fid);
        if( mod( A(1), 100 ) == 0 && oldTraj ~= A(1) )
            fprintf( 1, 'Traj# %d\n', A(1) );
            oldTraj = A(1);
        end;
    end;
    fclose( fid );
    TrajTable = growdata;
    save( matFileName, 'TrajTable' )
    fprintf( 1, 'Wrote iBatch %d\n', iBatch );
    
end; %Endfor

%%========================================================
%    Obsolete
% Bottom of 3 blocks for 3 sequences for 2D .txt's
%%========================================================
%
% Previous to above 3, tried reading in the txt file one Traj at a time:
%
%

% Now load up the 2D traj file, and build up a table of 2D tracks for
% features that at least appear in the current frameNum:

fid = fopen(txt2DfileName,'rt');      % 'rt' means read text
if (fid < 0)    
    error('could not open file');   % just abort if error
end;

s = fgetl(fid);                      % get a line
[tok tokRest] = strtok( s );
while( strcmp( tok, '#') == 1 )
    s = fgetl(fid);                      % get a line
    [tok tokRest] = strtok( s );
end;
% Ok, now s has the first line of a 2D feature, in the format:
%         auto_0  1503  934.281  558.474
%         auto_0  1504  933.87  555.825
%         auto_1  1500  859.231  516.434

TrajTable = []; % Big table to store all our Trajs, one block of each Traj at a time.

bDone = 0;
[A] = sscanf( s,'auto_%d %d %f %f\n', 4 );
while (ischar(s))                   % while not end of file
    curTrajNum = A(1);
    CandidateTraj = A'; % This clobbers any old Traj that had built up here.
    fprintf( 1, 'Traj# %d\n', A(1) );
    % Now loop through until we hit a line with a new Traj
    while( bDone == 0 )
        s = fgetl(fid);
        if( ~ischar(s) )
            bDone = 1;
            break;
        end;
        [A] = sscanf( s,'auto_%d %d %f %f\n', 4 );
        if( curTrajNum == A(1) )
            CandidateTraj = [CandidateTraj ; A'];
        else
            % Have hit a line with the subseq Traj, so examine the candidate
            % before proceeding
            TrajLineForSoughtFrame = find( CandidateTraj(:, 2) == frameNum );
            if( size(TrajLineForSoughtFrame,1) == 1 )   % It appears in the sought frame! Good.
                TrajTable = [TrajTable; CandidateTraj];
                fprintf( 1, 'TrajTable size is %d x %d\n', size(TrajTable, 1), size(TrajTable,2) );
            end;

            bDone = 1;  % s and A now have the first line of the next Traj
        end; % Endif 
    end; %Endwhile loop over block of Trajs of same feature.
    bDone = 0;

end;

fclose( fid );

%% ---------------------------------------------------------------------
%% ---------------------------------------------------------------------

%
%   Now pull the .ban files into our .mat's too!
%

% One at a time, each of these 3 sequences:
        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\05VD_v01_'; numDigs = 4; 
        iFirsts = [0  750  1500 2500 3500 4500 5500];
        iLasts = [800 1550 2550 3550 4550 5550 6598];

        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\16E5_v01_'; numDigs = 5;
        iFirsts = [0    950 1950 2950 3950 4950 5950 6950 7950 8950  9950 10950]; 
        iLasts = [1000 2000 3000 4000 5000 6000 7000 8000 9000 10000 11000 11380];

        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\06R0_v01_'; numDigs = 5;
        iFirsts = [0    950 1950 2950 3950 4950 5950];
        iLasts = [1000 2000 3000 4000 5000 6000 7168];
        
        SeqTemplate = 'G:\data\CambridgeTraffic\01TP_trax\0001TP_v01_'; numDigs = 6;
        iFirsts = [6690];
        iLasts = [10380];
        

for( iBatch = 1:size(iFirsts(:),1) )
    if( numDigs == 4 )
        banFileName = sprintf( '%s%.4d_%.4d.ban', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        matFileName = sprintf( '%s%.4d_%.4d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        %txt2DfileName = sprintf( '%s%.4d_%.4d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );

    elseif( numDigs == 5 )
        banFileName = sprintf( '%s%.5d_%.5d.ban', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        matFileName = sprintf( '%s%.5d_%.5d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        %txt2DfileName = sprintf( '%s%.5d_%.5d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    elseif( numDigs == 6 )
        banFileName = sprintf( '%s%.6d_%.6d.ban', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        matFileName = sprintf( '%s%.6d_%.6d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        %txt2DfileName = sprintf( '%s%.5d_%.5d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    end;
    
    load( matFileName );
    
    fid = fopen(banFileName,'rt');      % 'rt' means read text
    if (fid < 0)    
        error('could not open file');   % just abort if error
    end;

    s = fgetl(fid);                      % get a line
    [tok tokRest] = strtok( s );
    while( strcmp( tok, 'AddDecompCameraKey') ~= 1 )
        s = fgetl(fid);                      % get a line
        [tok tokRest] = strtok( s );
    end;
    % Ok, now s has the first line with "AddDecompCameraKey" and our K, R, and
    % t matrices!

    Kbig = zeros(3, 3, 1 + iLasts(iBatch) - iFirsts(iBatch) );
    Rbig = zeros(3, 3, 1 + iLasts(iBatch) - iFirsts(iBatch) );
    Tbig = zeros(3, 1, 1 + iLasts(iBatch) - iFirsts(iBatch) );

    while( strcmp( tok, 'AddDecompCameraKey') == 1 )
        %[tok tokRest] = strtok( s );
        [sNums] = sscanf( s, 'AddDecompCameraKey { Time %d K [ %f %f %f %f %f %f %f %f %f ]  R [ %f %f %f %f %f %f %f %f %f ]  t [ %f %f %f ]  }\n' );
        sTime = sNums(1); sK = sNums( 2:10 ); sR = sNums( 11:19 ); sT = sNums( 20:22);
        K = reshape( sK, 3, 3 )';
        R = reshape( sR, 3, 3 )';
        T = sT;

        Kbig( :,:, 1 + sTime - iFirsts(iBatch) ) = K;
        Rbig( :,:, 1 + sTime - iFirsts(iBatch) ) = R;
        Tbig( :,:, 1 + sTime - iFirsts(iBatch) ) = T;

        s = fgetl(fid); % get a line
        [tok tokRest] = strtok( s );
    end; % Loop over each line - 1 for each frame.
    clear s tok tokRest;
    clear sTime sNums sK sR sT K R T;

    fclose( fid );
    %% ---------------------------------------------------------------------
    
    save( matFileName, 'TrajTable', 'Kbig', 'Rbig', 'Tbig' )
end; %Endfor