%%========================================================
%       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];
        DataBlock = 1;
        
        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) )

    
    
    
    %iBatch = 6;     % DEBUG
    
    
    
    
    if( numDigs == 4 )
        %matTriFileName = sprintf( '%s%.4d_%.4d_TriVIds.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        txt2DTriIDfileName = sprintf( '%s%.4d_%.4d_2D_TriVIds.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    elseif( numDigs == 5 )
        %matTriFileName = sprintf( '%s%.5d_%.5d_TriVIds.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        txt2DTriIDfileName = sprintf( '%s%.5d_%.5d_2D_TriVIds.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    elseif( numDigs == 6 )
        %matTriFileName = sprintf( '%s%.6d_%.6d_TriVIds.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
        txt2DTriIDfileName = sprintf( '%s%.6d_%.6d_2D_TriVIds.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    end;
    
    %Using pad-by-6 for all TriFiles!!
    matTriFileName = sprintf( '%s%.6d_%.6d_TriVIds.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    
    
    fid = fopen(txt2DTriIDfileName,'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 )     % Skip comment-lines. Probably only needed for Boujou files.
        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:
    %             4500 0 16 5
    %             4500 0 5 1
    %             4500 7 2 4
    %             4500 3 2 1
    %             4500 16 2 5
    % where columns are:    
    %       (frameNum)   (Triangle's id# of V1)  (V2) (V3)
    % so for example, the first line has a triangle from frame# 4500, 
    % made of auto_0, auto_16, and auto_5's locations in that frame
    %
    %
    %   Boujou data format was: 
    %         auto_0  1503  934.281  558.474
    %         auto_0  1504  933.87  555.825
    %         auto_1  1500  859.231  516.434
    
    TriVIDsTable = []; % Big table to store all our Trajs, one block of each Traj at a time.
    oldFrame = -1;

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

%% ---------------------------------------------------------------------
%% ---------------------------------------------------------------------
%               First 1/3 above











%               Second 1/3 below
%% ---------------------------------------------------------------------
%% ---------------------------------------------------------------------


        DataBlock = 1;
        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\05VD_v01_'; numDigs = 4; 
        outBaseName = 'G:\MotBoostData01\Meta\Seq05VD_f';
        iFirsts = [0  750  1500 2500 3500 4500 5500];
        iLasts = [800 1550 2550 3550 4550 5550 6598];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
PerFrameCellsOfXYsIn3D = cell(size(frameList,1), 1);
for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    %iFrame = find(frameList == 4590); frameNum = frameList( iFrame );
    %fprintf( 1, 'Orientation processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    fprintf( 1, '3D localizing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    %MotBoostEvalOneFrame( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock );
    %OrientationPaint( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock );
    XYsIn3D = CalcXYsIn3D( frameNum, SeqTemplate, numDigs, iFirsts, iLasts, DataBlock );
    PerFrameCellsOfXYsIn3D{iFrame} = XYsIn3D;
end;

save( matPerFrameXYsIn3D, 'PerFrameCellsOfXYsIn3D' );
fprintf( 1, 'Wrote %s\n', matPerFrameXYsIn3D );
    
    
    
    
    
%     for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
%         frameNum = frameList(iFrame);
%         iFrame = find(frameList == 4590); frameNum = frameList( iFrame );   % For DEBUG only
% %
%     for( frameNum = 5003:5524 ) % For DEMO only.
% %
%         fprintf( 1, 'Processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
%         OrientationPaint( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock );
%     end;

    




        DataBlock = 2;
        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];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
PerFrameCellsOfXYsIn3D = cell(size(frameList,1), 1);
for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    fprintf( 1, '3D localizing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = CalcXYsIn3D( frameNum, SeqTemplate, numDigs, iFirsts, iLasts, DataBlock );
    PerFrameCellsOfXYsIn3D{iFrame} = XYsIn3D;
end;
save( matPerFrameXYsIn3D, 'PerFrameCellsOfXYsIn3D' )
fprintf( 1, 'Wrote %s\n', matPerFrameXYsIn3D );




        DataBlock = 3;
        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];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
PerFrameCellsOfXYsIn3D = cell(size(frameList,1), 1);
for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    fprintf( 1, '3D localizing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = CalcXYsIn3D( frameNum, SeqTemplate, numDigs, iFirsts, iLasts, DataBlock );
    PerFrameCellsOfXYsIn3D{iFrame} = XYsIn3D;
end;
save( matPerFrameXYsIn3D, 'PerFrameCellsOfXYsIn3D' )
fprintf( 1, 'Wrote %s\n', matPerFrameXYsIn3D );




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

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
PerFrameCellsOfXYsIn3D = cell(size(frameList,1), 1);
for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    fprintf( 1, '3D localizing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = CalcXYsIn3D( frameNum, SeqTemplate, numDigs, iFirsts, iLasts, DataBlock );
    PerFrameCellsOfXYsIn3D{iFrame} = XYsIn3D;
end;
save( matPerFrameXYsIn3D, 'PerFrameCellsOfXYsIn3D' )
fprintf( 1, 'Wrote %s\n', matPerFrameXYsIn3D );



%% ---------------------------------------------------------------------
%% ---------------------------------------------------------------------
%               Second 1/3 above






%               Third 1/3 below
%% ---------------------------------------------------------------------
%% ---------------------------------------------------------------------


%
%   Experimental!
%

        DataBlock = 1;
        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\05VD_v01_'; numDigs = 4; 
        outBaseName = 'G:\MotBoostData01\Meta\Seq05VD_f';
        iFirsts = [0  750  1500 2500 3500 4500 5500];
        iLasts = [800 1550 2550 3550 4550 5550 6598];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
load( matPerFrameXYsIn3D );

for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    %iFrame = find(frameList == 4590); frameNum = frameList( iFrame );
    fprintf( 1, 'Orientation processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = PerFrameCellsOfXYsIn3D{iFrame};
    %ECCV%OrientationPaint( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
    OrientationPaint3D( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
%         XYsIn3D = CalcXYsIn3D( frameNum, SeqTemplate, numDigs, iFirsts, iLasts, DataBlock );
%         PerFrameCellsOfXYsIn3D{iFrame} = XYsIn3D;
end;


        outBaseName = 'G:\MotBoostData01\MetaDEMO\Seq05VD_f';
        DataBlock = -1; % Paints on blank white instead of image of actual image.
for( frameNum = 986:frameList(size(frameList,1)) )  % Loops through ALL frames, even between those listed in frameList.    fprintf( 1, 'Orientation processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    fprintf( 1, 'Orientation processing frame, #%d \t \t of %s \n', frameNum, SeqTemplate );
    OrientationPaint3D( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock );
end;





        DataBlock = 2;
        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\16E5_v01_'; numDigs = 5;
        outBaseName = 'G:\MotBoostData01\Meta\0016E5_';
        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];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
load( matPerFrameXYsIn3D );

for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    fprintf( 1, 'Orientation processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = PerFrameCellsOfXYsIn3D{iFrame};
    %ECCV%OrientationPaint( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
    OrientationPaint3D( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
end;



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

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
load( matPerFrameXYsIn3D );

for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    fprintf( 1, 'Orientation processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = PerFrameCellsOfXYsIn3D{iFrame};
    %ECCV%OrientationPaint( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
    OrientationPaint3D( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );

end;
        
        


        DataBlock = 4;
        SeqTemplate = 'G:\data\CambridgeTraffic\01TP_trax\0001TP_v01_'; numDigs = 6;
        outBaseName = 'G:\MotBoostData01\Meta\0001TP_';
        iFirsts = [6690];
        iLasts = [10380];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
load( matPerFrameXYsIn3D );

for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
    frameNum = frameList(iFrame);
    fprintf( 1, 'Orientation processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = PerFrameCellsOfXYsIn3D{iFrame};
    %ECCV%OrientationPaint( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
    OrientationPaint3D( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
end;
        














        DataBlock = 1;
        SeqTemplate = 'G:\Cam_2007_01_14\EgoBoost_trax\05VD_v01_'; numDigs = 4; 
        outBaseName = 'G:\MotBoostData01\Meta\Seq05VD_f';
        iFirsts = [0  750  1500 2500 3500 4500 5500];
        iLasts = [800 1550 2550 3550 4550 5550 6598];

sEvalFramesList = sprintf( '%sEvalFrames.txt', SeqTemplate );
frameList = load( sEvalFramesList );
matPerFrameXYsIn3D = sprintf( '%s3dXYs.mat', SeqTemplate );
load( matPerFrameXYsIn3D );


% Note: PerFrameCellsOfXYsIn3D is a cell array, so each frame can have a
% different number of 3D points.

% Note2: EgoBoostGetC() is a function that assumes the points in the world
% are static, and that the camera was moving. This function just returns
% the 3D camera-center, so the points are offset by the right amount.
% REPLACE THIS by an appropriate function.

% Find the largest number of reconstructed points that we'll have to
% render:
maxNum3Dpts = 0; for iFrame = 1:size(PerFrameCellsOfXYsIn3D,1), maxNum3Dpts = max( maxNum3Dpts, size(PerFrameCellsOfXYsIn3D{iFrame}, 1 ) ); end



fid = fopen('MakeDots.mel', 'w' )
for iRow = 1:maxNum3Dpts
    %fprintf( fid, 'polySphere -r 0.114 -sx 5 -sy 5 -n Dot%d;move %f %f %f;\n', iRow, 100, 0, 0 );
    fprintf( fid, 'sphere -r 0.114 -s 2 -nsp 2 -ch 1 -n Dot%d;move %f %f %f;\n', iRow, 100, 0, 0 ); %sphere -r 1 -s 2 -nsp 1 -ch 1 -n Foo;
end
fclose(fid);

% To output the 3D points into Maya: produce these Mel-script strings:
%iFrame = find(frameList == 4260); frameNum = frameList( iFrame );

fid = fopen('MoveDots.mel', 'w' );
%fid = 1;

for( iFrame = 1:size(frameList,1) )     % Process: Loop over each frame in frameList
% for( iFrame = 143:150 )     % Process: Loop over each frame in frameList    
    frameNum = frameList(iFrame);
    fprintf( 1, '.mel processing %dth frame, #%d \t \t of %s \n', iFrame, frameNum, SeqTemplate );
    XYsIn3D = PerFrameCellsOfXYsIn3D{iFrame};
    %P = EgoBoostGetP( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
    C = EgoBoostGetC( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D );
    
    sXYZ = size(XYsIn3D);
    Pts = XYsIn3D - repmat(C,sXYZ(1),1);
    
    fprintf( fid, 'currentTime %d;\n', iFrame );        % Move in Maya's timeline. Note: could use real time, ie frameNum.
    Pts = 10 * Pts;
    
    Ave = sum(Pts) / sXYZ(1);
    
    for iRow = 1:sXYZ(1)
        fprintf( fid, 'move %f %f %f Dot%d; setKeyframe |Dot%d.translate;\n', Pts(iRow,3), Pts(iRow,1), Pts(iRow,2), iRow, iRow );
    end
    for iRow = 1+sXYZ(1) : maxNum3Dpts
        fprintf( fid, 'move %f %f %f Dot%d; setKeyframe |Dot%d.translate;\n', 100, 0, 0, iRow, iRow );
    end

end % For loop over all frames in framelist

fclose(fid);