% EE  392J
% PROJECT

% Ali Ercan and Ulrich Barnhoefer

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function im = blurFreeImConst(modQ,num,singleExpTime,sigma_v,sensorGain)
%
% This function gets the modified (averaged) Q values across pixel positions and 
% time and consructs the estimated photo-current matrix for each pixel. Then
% calculates the final HDR image using estimated photocurrent.
%
% Inputs: modQ      : The averaged Q values (in DV's)(generated by "qModifier.m")
%         num       : The number of averaged values per pixel. 
%         singleExpTime: Exposure time for a single exposure
%         sigma_v   : Read noise std dev. in e-
%         sensorGain: Gain of the sensor in DV/e;
% Outputs: im       : The final motion blur free low noise high DR image
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function im = blurFreeImConst(modQ,num,singleExpTime,sigma_v,sensorGain)

[m n T] = size(modQ);
modQ = modQ/sensorGain; % convert DV's to e-'s

for i = 1:m
    for j=1:n
        qPix = squeeze(modQ(i,j,:));
        nPix = squeeze(num(i,j,:));
        qPix = qPix(2:end); % The first value is zero, for reset value, after CDS
        nPix = nPix(2:end); % Get rid of the reset charge and num for that

        % Check the modQ and num entrees were constructed right.

        zeroIndQ = find(qPix == 0);
        zeroIndn = find(nPix == 0);
        
%        if ~(zeroIndQ == zeroIndn)
%            error(strcat('The zero entrees in Q and num for pixel',int2str(i),',',int2str(j),'do not match'));
%        end
        if (~(isempty(zeroIndQ))) & (~(isempty(zeroIndn)))
            temp1 = diff(zeroIndQ);
            temp2 = diff(zeroIndn);
            if (~(isempty(find(temp1>1)))) |  (~(isempty(find(temp2>1))))
           %     error(strcat('The zero entrees in Q and num for pixel',int2str(i),',',int2str(j),'are not proper'));
            end
        end
        
        
        nPix = nPix(find(nPix ~= 0)); % The non-zero entrees of nPix are only meaningful
        qPix = qPix(find(nPix ~= 0));
        
        % Calculate the coefficients for estimator and estimate of the photocurrent
        % As the estimate for the photocurrent is recursive, each step we estimate 
        % a value for the photocurrent and use its estimate as an estimate for the 
        % shot noise for the next step
 
        s = length(nPix);
        
        time = singleExpTime*(1:s);
        I_tilda = qPix'./time;
        
        b = [];
        g = [];
        h = [];
        I_hat = [];
        b(1) = 1;
        g(1) = 1;
        h(1) = 1;
        I_hat(1) = I_tilda(1);
        for k = 2:s
            sigma_uSq = I_hat(k-1)*singleExpTime;
            divisor = (1:k-1).*sqrt(nPix(1:k-1))';
            b(k) = k*nPix(k)/nPix(1)+k/(k-1)*nPix(k)/nPix(k-1)*b(k-1)+...
                k*nPix(k)*sigma_uSq/2/(sigma_v^2)*sum(b(1:k-1)./divisor);
            g(k) = sum(b(1:k));
            h(k) = b(k)/g(k);
            I_hat(k) = I_hat(k-1)+h(k)*(I_tilda(k)-I_hat(k-1));
        end
        % Form the final photocurrent estimate matrix
        I(i,j) = I_hat(s);
    end
end

% Now the final HDR image can be constructed;

im = (T-1)*singleExpTime*I*sensorGain;
im(find(im<0)) = 0;
return;
