function C = EgoBoostGetC( frameNum, SeqTemplate, numDigs, outBaseName, iFirsts, iLasts, DataBlock, XYsIn3D )

%%
%%  MOSTLY  from inside MotBoostEvalOneFrame    MOSTLY
%%

bPreComputedXYsIn3D = 1;
bPreComputedXYsIn3D = (nargin == 8);    % Checks if XYsIn3D was passed in, else we'll have to compute ourselves.


%set(gcf,'Renderer','OpenGL') % MAN! Without this, renderer could be anything
set(0,'DefaultFigureRenderer','opengl') % Sets it for all future figures.
% (Find out: get(gcf,'Renderer'); ) and could then have transparency etc.
% problems.
%
% OR, 
% for samson, which has poor graphics card (??)
%opengl neverselect
% Note: also using white and 0.75 width for triangle edges, since NOT
% transparent with this setting!

%        % Replicate transparency bug:
%        load mandrill
%        image(X); colormap(map)
%        Faces = [1 2 3];
%        Vertices = [100 100; 200 200; 50 100]
%        patch( 'Vertices', Vertices, 'Faces', Faces, 'EdgeColor', 'white', 'EdgeAlpha', 0.2 );


numLasts = size(iLasts(:),1);
iLast = iLasts( numLasts );
if( frameNum > iLast )
    fprintf( 1, 'Error: Wanted Eval frame %d but batches only reach %d\n', frameNum, iLast );
elseif( frameNum > iLast -25 )
    iBatch = numLasts;
else
    iBatch = find( iLasts - 25 > frameNum, 1 ); % The (...,1) says: give me at most 1 index.
end;


if( numDigs == 4 )
    % These two have been precomputed already!
    %banFileName = sprintf( '%s%.4d_%.4d.ban', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    %txt2DfileName = sprintf( '%s%.4d_%.4d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    matFileName = sprintf( '%s%.4d_%.4d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    %matTriFileName = sprintf( '%s%.4d_%.4d_TriVIds.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
elseif( numDigs == 5 )
    % These two have been precomputed already!
    %banFileName = sprintf( '%s%.5d_%.5d.ban', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    %txt2DfileName = sprintf( '%s%.5d_%.5d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    matFileName = sprintf( '%s%.5d_%.5d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    %matTriFileName = sprintf( '%s%.5d_%.5d_TriVIds.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
elseif( numDigs == 6 )
    % These two have been precomputed already!
    %banFileName = sprintf( '%s%.5d_%.5d.ban', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    %txt2DfileName = sprintf( '%s%.5d_%.5d_2D.txt', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    matFileName = sprintf( '%s%.6d_%.6d.mat', SeqTemplate, iFirsts(iBatch), iLasts(iBatch) );
    %matTriFileName = sprintf( '%s%.6d_%.6d_TriVIds.mat', 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) );


% Now load up the 2D traj file and 3D camera calibs
load( matFileName );

%load( matTriFileName );


if( DataBlock == 1 )
    % Restore! After ICCV deadline.
    %debugImgName = sprintf( 'G:\\MotBoostData01\\StillsRaw_full\\Test\\Seq05VD_f%.5d.png', frameNum );

    debugImgName = sprintf( 'G:\\MotBoostData01\\701_StillsRaw_full\\Seq05VD_f%.5d.png', frameNum );
elseif( DataBlock == 2 )
    debugImgName = sprintf( 'G:\\MotBoostData01\\701_StillsRaw_full\\0016E5_%.5d.png', frameNum );
elseif( DataBlock ==3 )
    debugImgName = sprintf( 'G:\\MotBoostData01\\701_StillsRaw_full\\0006R0_f%.5d.png', frameNum );
elseif( DataBlock ==4 )
    debugImgName = sprintf( 'G:\\MotBoostData01\\701_StillsRaw_full\\0001TP_%.6d.png', frameNum );
elseif( DataBlock == -1 )
    debugImgName = 'G:\\MotBoostData01\\StillsRaw_full\\blankWhite960x720.png';
end;
% debugImgName =
% 'G:\\MotBoostData01\\StillsRaw_full\\blankWhite320x240.png';
% debugImgName =
% 'G:\\MotBoostData01\\StillsRaw_full\\blankWhite960x720.png';


%       find( TrajTable(:, 1) == 7074 );    % Find indexes to all frames with this Traj#
%       find( TrajTable(:, 2) == 1502 );    % Find indexes to all Traj's with this Frame#



isOfTrajsExistingNow = find( TrajTable(:, 2) == frameNum );     % Look through column 2 of TrajTable, return indexes.
XYs = TrajTable( isOfTrajsExistingNow, 3:4 );




%%
%% Accum 3D camera center for the frames in this batch:
%%
numXYs = size(XYs,1);
offsetForFrameNums = iFirsts(iBatch) -1;


iInBig = frameNum - offsetForFrameNums;
%P = Kbig(:,:, iInBig ) * Rbig(:,:, iInBig ) * [eye(3) -Tbig(:,:, iInBig )];
C = [Tbig(:,:, iInBig )]'; %Cs( frameNum-offsetForFrameNums,    1:3);
return;













































Cs = zeros( iLasts(iBatch) - iFirsts(iBatch) + 1,   3);
j=1;
for( iFrameC = iFirsts(iBatch):iLasts(iBatch) )
    iInBig = iFrameC - offsetForFrameNums;
    %                      Long way, if we only have P...
    %P = Kbig(:,:, iInBig ) * Rbig(:,:, iInBig ) * [eye(3) -Tbig(:,:, iInBig )];
    %p1 = P(:,1); p2 = P(:,2); P(:,3); p3 = P(:,3); p4 = P(:,4);
    %C = [det([p2 p3 p4])  -det([p1 p3 p4])  det([p1 p2 p4])   -det([p1 p2 p3]) ];
    %Rbig(:,:, iInBig ) * [eye(3) -Tbig(:,:, iInBig )]
    %C = C(1:3) / C(4);
    %Cs(j, 1:3) = C;
    Cs(j, 1:3) = [Tbig(:,:, iInBig )]';     % ...vs short way if we have T
    j = j+1;
end;
curC = Cs( frameNum-offsetForFrameNums,    1:3);


if( ~bPreComputedXYsIn3D  )
    XYsIn3D = zeros( numXYs, 3 );
    for( iTraj = 1:numXYs )
        %fprintf(1, 'iTraj #%d\n', iTraj);
        trajNum = TrajTable(isOfTrajsExistingNow(iTraj), 1);

        rowsTrajWasTracked = find( TrajTable(:, 1) == trajNum );
        %rowsTrajWasTracked = find( (TrajTable(:, 1) <= trajNum+0.001) && (TrajTable(:, 1) >= trajNum-0.001) );

        %% Weird hacks, attempting to circumvent Matlab bug that would crash
        %% here... Still breaks!
        %%
        %TrajByFrame = sortrows( TrajTable(rowsTrajWasTracked, 2:4), 1);
        rowsTrajWasTracked2 = find( TrajTable(:, 1) == trajNum );
        while( (size(rowsTrajWasTracked,1) ~= size(rowsTrajWasTracked2,1))  ||  sum(rowsTrajWasTracked == rowsTrajWasTracked2) ~= size(rowsTrajWasTracked,1) )
            fprintf(1, ',');
            rowsTrajWasTracked = find( TrajTable(:, 1) == trajNum );
            rowsTrajWasTracked2 = find( TrajTable(:, 1) == trajNum );
        end;
        TrajByFrame = TrajTable(round(rowsTrajWasTracked), 2:4);
        if( size( TrajByFrame, 1 ) < 3 )
            continue;   % No point in processing too short a track.
        end;

        %[VarianceIn3D Res] = VarOfBackProjectTraj( TrajByFrame, offsetForFrameNums, Kbig, Rbig, Tbig );
        XYsIn3D(iTraj, 1:3) = BackProject2DTrajTo3D( TrajByFrame, offsetForFrameNums, Kbig, Rbig, Tbig );
        fprintf(1, '.');
    end;
    fprintf(1, '\n');
end;


trajsExistingNow = TrajTable(isOfTrajsExistingNow(:), 1);
PerTraj3D = [trajsExistingNow, XYsIn3D];    % Gives:    210.0000    2.8633   -0.0752   17.6500
%           auto_#    X Y Z

isOfTrisExistingNow = find( TriVIDsTable(:, 1) == frameNum );
trisExistingNow = TriVIDsTable(isOfTrisExistingNow, 2:4);   % Column1 was just frameNum
% Great - each row lists 3 vertex ID#s. But those #s refer to real IDs,
% while the XYZ for each V is listed with that ID in a "short" matrix
% PerTraj3D.

% TrisExistingNow_AsIndexes = zeros( size(trisExistingNow));  % Is now the same size
% numTriVs = size(TrisExistingNow_AsIndexes(:));
% for( iTri = 1:numTriVs )
%     TrisExistingNow_AsIndexes(iTri) = find( PerTraj3D(:, 1) == trisExistingNow(iTri) );
% end;

% Now a row of TrisExistingNow_AsIndexes == [153  3  38]  which are 3
% indexes, each to the row in XYsIn3D (and XYs in 2D, btw) for that
% Triangle.
%
% Example:
%   XYsIn3D( TrisExistingNow_AsIndexes(1, :),: )
%   and
%   XYs( TrisExistingNow_AsIndexes(1, :),: )
% each returns 3 rows, one for each point; either 3 or 2 columns wide.



%% =====================================
%A = imread( debugImgName );

% A(:) = 0;
% 
% imagesc(A); 
% set(gca,'xtick',[]);
% set(gca,'ytick',[]);
% hold on;


% fig1 = figure;
% winsize = get(fig1,'Position');
% winsize(1:2) = [0 0];
% hold on;
% set(fig1,'NextPlot','replacechildren');


% Nice, but unneccessary while triangle edges show the same info.
%plot( XYs(:,1), XYs(:,2), 'r.'  );

Ns = zeros( size(trisExistingNow));
numTris = size(TrisExistingNow_AsIndexes,1);
TriCenters = zeros( numTris, 3 );
TriCsplusNs = zeros( numTris, 3 );
for( iTri = 1:numTris )
    ABCpts = XYsIn3D( TrisExistingNow_AsIndexes(iTri, :),: );
    Apt = ABCpts(1,:);
    Bpt = ABCpts(2,:);
    Cpt = ABCpts(3,:);
    Nvec = cross( Bpt - Cpt, Apt - Bpt );
    Ns( iTri, : ) = Nvec;
    TriCenters( iTri, : ) = (Apt + Bpt + Cpt) / 3;
    %TriCsplusNs( iTri, : ) = TriCenters( iTri, : ) + 5 * Nvec;
end;

%TriCsplusNs = TriCenters + .0756 * Ns;
[NormalizedNormals,NormLengths] = normaliz(Ns);
TriCsplusNs(1:end,:) = TriCenters(1:end,:) + 0.01 * NormalizedNormals(1:end,:);
%TriCsplusNs(i,:) = 0.01 * Ns(i, : ) / norm( Ns(i, : ) );
% plot( NormLengths );
% sum(NormLengths) / size(NormLengths,2)

% ..............................................................
%     Commented out for faster production of orientations for EgoBoost
% ...............................................................
% scaleRed = max( Ns(:,1)) - min( Ns(:,1));
% shiftRed = 0 - min( Ns(:,1));
% scaleGreen = max( Ns(:,2)) - min( Ns(:,2));
% shiftGreen = 0 - min( Ns(:,2));
% scaleBlue = max( Ns(:,3)) - min( Ns(:,3));
% shiftBlue = 0 - min( Ns(:,3));
% 
% NsScaledShifted = [(Ns( :, 1 ) + shiftRed) / scaleRed, (Ns( :, 2 ) + shiftGreen) / scaleGreen, (Ns( :, 3 ) + shiftBlue) / scaleBlue ];
% ..............................................................
% ..............................................................


%
%   For 2D Orientation: Project a scaled version of the normals (if big
%   enough).
%
% iInBig = frameNum - offsetForFrameNums;
% P = Kbig(:,:, iInBig ) * Rbig(:,:, iInBig ) * [eye(3) -Tbig(:,:, iInBig )];
% Ns2DtoGrayScale = zeros( numTris, 1);
% ThetasX = zeros( numTris, 1);
% ThetasY = zeros( numTris, 1);
% for( iTri = 1:numTris )
%     % Wasteful: gets re-computed in the plotting function just below!
%     TriCenter = [TriCenters(iTri,:) 1]';
%     ProjectedTriCenter = (P * TriCenter );
%     projectedTriCenter = ProjectedTriCenter( 1:2 ) / ProjectedTriCenter(3);
% 
% %
%     if( NormLengths( iTri ) < 0.5 )
%         TriCplusN = [TriCsplusNs(iTri,:) 1]';
%         ProjectedCpN = (P * TriCplusN );
%         projectedCpN = ProjectedCpN( 1:2 ) / ProjectedCpN(3);
%         
%         n2D = projectedCpN - projectedTriCenter;
%         %fprintf(1, '%d %d\n', n2D' );
%         preScaled = atan2(n2D(2), n2D(1)); 
%                 % same as cart2pol( x, y ) or 
%                 % acos(n2D(1) / sqrt(n2D(1)*n2D(1) + n2D(2)*n2D(2)))
%                 % though acos sign is often wrong! 
%                 % atan2 uses sign(Y) and sign(X) to determine the specific
%                 % quadrant.
%         preScaled = preScaled + pi; % Slide them all up to be positive.
%         % So hopefully [0-2pi]
% 
%         thetaX = (   (1+ cos(preScaled))*254 / 2   ) +1;
%         thetaY = (   (1+ sin(preScaled))*254 / 2   ) +1;
%         % cos/sin starts off [-1,1], and ends up [1,255], leaving room for
%         % zero to flag "no orientation" pixels
%         ThetasX(iTri, 1) = thetaX;
%         ThetasY(iTri, 1) = thetaY;
%         
%         scaled = 1 +    preScaled * 254 / (2*pi);
%         Ns2DtoGrayScale(iTri, 1) = scaled;        
% %
%     end;
% end;



%
%   For 3D Orientation: Just make the (short enough) Normals scaled up to
%   span the range [0, 254] and shift up. They start as [-1,1].
%
% iInBig = frameNum - offsetForFrameNums;
% P = Kbig(:,:, iInBig ) * Rbig(:,:, iInBig ) * [eye(3) -Tbig(:,:, iInBig )];
ScaledX = zeros( numTris, 1);
ScaledY = zeros( numTris, 1);
ScaledZ = zeros( numTris, 1);

ScaledShiftedNs = ( 127* (NormalizedNormals + 1) )  + 1;
% Shift [-1,1] to [0,2], scale that to [0,254], then shift up to 255,
% leaving space for 0's that are really zero (as opposed to -1 originally).

%GoodNs = NormLengths < 0.5;
GoodNs = NormLengths < 5000000;
ScaledX(GoodNs) = ScaledShiftedNs(GoodNs, 1);
ScaledY(GoodNs) = ScaledShiftedNs(GoodNs, 2);
ScaledZ(GoodNs) = ScaledShiftedNs(GoodNs, 3);

FacesColors = [ScaledX ScaledY ScaledZ] / 255;



%     %Plot suff
%     %patch('Vertices', XYs, 'Faces', TrisExistingNow_AsIndexes, 'FaceColor', [0.9, 0.8, 0.75], 'EdgeColor', 'black','BackFaceLighting', 'unlit', 'FaceLighting', 'phong', 'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 1);
% 
%     % Start by just plotting the triangles which are based on the tracked 2D
%     % features. 
%     FacesColorsR = ThetasX / 255;
%     FacesColorsG = ThetasY / 255;
%     FacesColorsB = Ns2DtoGrayScale / 255;
%     % FacesColorsB = zeros(size(Ns2DtoGrayScale)); % Nice blue-less colors.
%     FacesColors = [FacesColorsR FacesColorsG FacesColorsB];


Faces = [1 2 3];
for( iTri = 1:numTris )
    Vertices = XYs( TrisExistingNow_AsIndexes(iTri, :),: );
%     patch( 'Vertices', Vertices, 'Faces', Faces, 'FaceColor', ...
%            NsScaledShifted(iTri,:), 'EdgeColor', 'black', 'EdgeAlpha', 0.2, ...
%            'BackFaceLighting', 'unlit', 'FaceLighting', 'phong', ...
%            'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 1, 'FaceAlpha', 0.0);
    patch( 'Vertices', Vertices, 'Faces', Faces, ...
           'FaceColor', FacesColors(iTri,:), ...    % Ns2DtoGrayScale(iTri,:), 
           'EdgeColor', 'none', ...    % 'black'
           'FaceAlpha', 1.0 );      % 'FaceColor', NsScaledShifted(iTri,:), 'EdgeColor', 'black', ...

    % Good version - makes clean transparent black wire triangles.
%     patch( 'Vertices', Vertices, 'Faces', Faces, ...
%            'FaceColor', 'none', 'EdgeColor', [0.0 0.0 0.0], 'EdgeAlpha', 0.2, ...      % 'FaceColor', NsScaledShifted(iTri,:),
%            'BackFaceLighting', 'unlit', 'FaceLighting', 'phong', ...
%            'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 0.5 ); %  'FaceAlpha', 0.0
       
       %patch('Vertices', Vertices, 'Faces', Faces, 'FaceColor', [0.9, 0.8, 0.75], 'EdgeColor', 'black','BackFaceLighting', 'unlit', 'FaceLighting', 'phong', 'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 1);
end;

% iTri = 650;
% Vertices = XYs( TrisExistingNow_AsIndexes(iTri, :),: );
% patch('Vertices', Vertices, 'Faces', Faces, 'FaceColor', [1 1 1], 'EdgeColor', 'black','BackFaceLighting', 'unlit', 'FaceLighting', 'phong', 'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 1, 'FaceAlpha', 0.5);



bRenderNormalLine = 0;
if( bRenderNormalLine )

    iInBig = frameNum - offsetForFrameNums;
    P = Kbig(:,:, iInBig ) * Rbig(:,:, iInBig ) * [eye(3) -Tbig(:,:, iInBig )];
    for( iTri = 1:numTris )
        %Vertices = XYs( TrisExistingNow_AsIndexes(iTri, :),: );
        %     Commented out for faster production of orientations for EgoBoost
        % ...............................................................
        %     Xs = [XYsIn3D( TrisExistingNow_AsIndexes(iTri, :),: )' ; 1 1 1];
        %     ProjectedXs = (P * Xs);
        %     xs = [ProjectedXs(1:2,1) / ProjectedXs(3,1), ProjectedXs(1:2,2) / ProjectedXs(3,2), ProjectedXs(1:2,3) / ProjectedXs(3,3)];
        % ...............................................................
        %plot( xs(1,:), xs(2,:), 'b.'  );

        TriCenter = [TriCenters(iTri,:) 1]';
        ProjectedTriCenter = (P * TriCenter );
        projectedTriCenter = ProjectedTriCenter( 1:2 ) / ProjectedTriCenter(3);
        plot( projectedTriCenter(1), projectedTriCenter(2), 'gx'  );

    %    if( NormLengths( iTri ) < 0.5 )
            TriCplusN = [TriCsplusNs(iTri,:) 1]';
            ProjectedCpN = (P * TriCplusN );
            projectedCpN = ProjectedCpN( 1:2 ) / ProjectedCpN(3);
            %     plot( projectedCpN(1), projectedCpN(2), 'r.'  );

            plot( [projectedTriCenter(1) projectedCpN(1)], ...
                  [projectedTriCenter(2) projectedCpN(2)], ...
                  'r-', 'LineWidth', 1.0 ); % default is 0.5; possible choices 2,3,4.

    %    end;
        %patch('Vertices', Vertices, 'Faces', Faces, 'FaceColor', NsScaledShifted(iTri,:), 'EdgeColor', 'black','BackFaceLighting', 'unlit', 'FaceLighting', 'phong', 'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 1);
        %patch('Vertices', Vertices, 'Faces', Faces, 'FaceColor', [0.9, 0.8, 0.75], 'EdgeColor', 'black','BackFaceLighting', 'unlit', 'FaceLighting', 'phong', 'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 1);
    end;

end; % Endif over plotting normal line in Red.
    
% Example of point with BAD 3D reconstruction.
% iFeat = 1;
% XYs(iFeat,:)
% XYsIn3D(iFeat,:)
% tmp = P * [XYsIn3D(iFeat,:)'; 1]
% tmp / tmp(3)



% hold off
% F = getframe;
% pngFileName = sprintf( '%s_01_%.5d_Normals.png', outBaseName, frameNum );
% print( F, '-dpng', '-r80', pngFileName );

hold off
fig=gcf;
units=get(fig,'units');
set(fig,'units','normalized','outerposition',[0 0 1 1]);
set(fig,'units',units);

%pngFileName = sprintf( '%s_01_2DNormals_%.6d.png', outBaseName, frameNum );

% Used for EgoBoost CVPR08 submission
%pngFileName = sprintf( '%s%.5d_2DNormals.png', outBaseName, frameNum );

% Used (1st attempt) for ECCV08
pngFileName = sprintf( '%s%.5d_3DNormalsAll.png', outBaseName, frameNum );


% If UIW_SetUpGLPrinting problem:
% Try changing the Renderer property of your figure from OpenGL to ZBuffer. 
% To do this, use:      set(gcf, 'Renderer', 'ZBuffer')
% and read http://www.mathworks.com/support/tech-notes/1200/1201.shtml
% 
%saveas(gcf, pngFileName, 'png');    % Gives large-format image of Figure
%print( gcf, '-dpng', '-r80', pngFileName ); % Gives 640x480 (small) figure
%print( gcf, '-dpng', '-r130', pngFileName ); % 806 x 636 usable
%print( gcf, '-dpng', '-r140', pngFileName ); % 870 x 686
%print( gcf, '-dpng', '-r142', pngFileName ); % 882 x 695
%print( gcf, '-dpng', '-r145', pngFileName ); % 900 x 709 blocky!
%print( gcf, '-dpng', '-r147', pngFileName ); % 911 x 719        <-- good for making videos
%print( gcf, '-dpng', '-r150', pngFileName ); % 931 x 733
%print( gcf, '-dpng', '-r152', pngFileName ); % 942 x 745
print( gcf, '-dpng', '-r155', pngFileName ); % 961 x 758
%print( gcf, '-dpng', '-r156', pngFileName ); % 967 x 763

%% =====================================

%% patch('Vertices', XYsIn3D, 'Faces', TrisExistingNow_AsIndexes, 'FaceColor', [0.9, 0.8, 0.75], 'EdgeColor', 'black','BackFaceLighting', 'unlit', 'FaceLighting', 'phong', 'EdgeLighting', 'none','LineStyle', '-', 'LineWidth', 2);