% Primary-COnsistent Soft-Decision Color demosaicking algorithm
% Currently, code is only implemented for 'bggr'
%
% [ rgb ] = PCSD( bayer_inRGB,bayer_inPlaner, bPattern, clipToRange)
% Input : bayer_inRGB is a 3D array with the R, G and B of the
% mosaic image separated into the 3 channels. It can be obtained from 
%            bayer_inRGB = plane2mosaic(img,isa,0);
%         bayer_inPlaner = 2D planar mosaic image. 
%
function [ rgb ] = PCSD( bayer_inRGB,bayer_inPlaner, bPattern, clipToRange)


if ieNotDefined('bayer_inRGB'), error('Bayer rgb data required.'); end
if ieNotDefined('bayer_inPlaner'), error('Bayer rgb data required.'); end
if ieNotDefined('bPattern'), error('Bayer pattern info required.'); end
if ieNotDefined('clipToRange'), clipToRange = 1; end 

% mosaicConverter transforms non-grbg into grbg.  This is the only format
% that works for the remaining part of the code.
switch lower(bPattern)
    case 'grbg'
        error('GRBG not yet implemented');
    case 'rggb'
        error('RGGB not yet implemented');
    case 'bggr'
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        % Mirror 2 rows and columns on each side to help implementation
        % mosaic has RGB colors in 1 plane
        % then we need to use this to interpolate the missing R, G, B
        % values. These interpolated values are put in mosaic_H, mosaic_V
        % before starting, we need to put original existing R, G, B values in mosaic_H, mosaic_V has the
        % We pad the mosaic with 2 rows on top and bottom, 2 columns on
        % left and right because these values will be needed to simplfy algorithm implementation
        % The extra rows and columns are padded by mirroring the oringal
        % mosaic
        bayer_inRGB(:,:,1) = bayer_inRGB(:,:,1)./max(max(bayer_inRGB(:,:,1)));
        bayer_inRGB(:,:,2) = bayer_inRGB(:,:,2)./max(max(bayer_inRGB(:,:,2)));
        bayer_inRGB(:,:,3) = bayer_inRGB(:,:,3)./max(max(bayer_inRGB(:,:,3)));
        bayer_inPlaner = bayer_inRGB(:,:,1) + bayer_inRGB(:,:,2) + bayer_inRGB(:,:,3);
 
        % create planar mosaic
        mosaic = [bayer_inPlaner(:,2:-1:1) bayer_inPlaner bayer_inPlaner(:,end:-1:end - 1)];
        mosaic = [mosaic(2:-1:1, :); mosaic ;mosaic(end:-1:end - 1,:)];
        
        % create 3D mosaic
        mosaicR = zeros(size(mosaic));
        mosaicG = zeros(size(mosaic));
        mosaicB = zeros(size(mosaic));
        mosaicR(3:end - 2, 3:end - 2) = bayer_inRGB(:,:,1);
        mosaicG(3:end - 2, 3:end - 2) = bayer_inRGB(:,:,2);
        mosaicB(3:end - 2, 3:end - 2) = bayer_inRGB(:,:,3);
        % mirror rows of R
        mosaicR(1:2,3:end - 2) = bayer_inRGB(2:-1:1,:,1);
        mosaicR(end - 1:end,3:end - 2) = bayer_inRGB(end:-1:end -1,:,1);
        % mirror columns of R
        mosaicR(3:end - 2,1:2) = bayer_inRGB(:,2:-1:1,1);
        mosaicR(3:end - 2, end - 1:end) = bayer_inRGB(:,end:-1:end - 1,1);
        % mirror diagonal of R
        mosaicR(2,2) = bayer_inRGB(2,2,1);
        mosaicR(end - 1, end - 1) = bayer_inRGB(end - 1,end - 1,1);
        % mirror rows of G
        mosaicG(1:2,3:end - 2) = bayer_inRGB(2:-1:1,:,2);
        mosaicG(end - 1:end,3:end - 2) = bayer_inRGB(end:-1:end - 1,:,2);
        % mirror columns of G
        mosaicG(3:end - 2,1:2) = bayer_inRGB(:,2:-1:1,2);
        mosaicG(3:end - 2, end - 1:end) = bayer_inRGB(:,end:-1:end - 1,2);
        % mirror diagonal of G
        mosaicG(2,2) = bayer_inRGB(2,2,2);
        mosaicG(end - 1, end - 1) = bayer_inRGB(end - 1,end - 1,2);
        % mirror rows of B
        mosaicB(1:2,3:end - 2) = bayer_inRGB(2:-1:1,:,3);
        mosaicB(end - 1:end,3:end - 2) = bayer_inRGB(end:-1:end - 1,:,3);
        % mirror columns of B
        mosaicB(3:end - 2,1:2) = bayer_inRGB(:,2:-1:1,3);
        mosaicB(3:end - 2, end - 1:end) = bayer_inRGB(:,end:-1:end - 1,3);
        % mirror diagonal of B
        mosaicB(2,2) = bayer_inRGB(2,2,3);
        mosaicB(end - 1, end - 1) = bayer_inRGB(end - 1,end - 1,3);
        
        % the 3D mosaic where the interpolated pixels will be placed
        mosaic_H = cat(3,mosaicR, mosaicG, mosaicB);
        mosaic_V = mosaic_H;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % First interpolate missing green pixels.
        % Interpolate green pixels for Blue
        mosaic_H(3:2:end - 3, 3:2:end - 3, 2) = 0.5.*(mosaic(3:2:end - 3,2:2:end-4) + mosaic(3:2:end - 3,4:2:end-2) + mosaic(3:2:end - 3,3:2:end-3))...
            - 0.25.*(mosaic(3:2:end - 3,1:2:end-5) + mosaic(3:2:end-3,5:2:end-1));

        mosaic_V(3:2:end - 3, 3:2:end - 3, 2) = 0.5.*(mosaic(2:2:end - 4, 3:2:end-3) + mosaic(4:2:end - 2, 3:2:end - 3) + mosaic(3:2:end - 3, 3:2:end - 3))...
            - 0.25.*(mosaic(3:2:end - 3,1:2:end - 5) + mosaic(3:2:end - 3,5:2:end - 1));
        % Interpolate green pixels for Red
        mosaic_H(4:2:end - 2, 4:2:end - 2, 2) = 0.5.*(mosaic(4:2:end - 2,3:2:end - 3) + mosaic(4:2:end - 2, 5:2:end - 1) + mosaic(4:2:end - 2, 4:2:end - 2))...
            - 0.25.*(mosaic(4:2:end - 2,2:2:end - 4) + mosaic(4:2:end - 2,6:2:end));
        
        mosaic_V(4:2:end - 2, 4:2:end - 2, 2) = 0.5.*(mosaic(3:2:end - 3,4:2:end - 2) + mosaic(5:2:end - 1,4:2:end - 2) + mosaic(4:2:end - 2,4:2:end - 2))...
            - 0.25.*(mosaic(2:2:end - 4,4:2:end - 2) + mosaic(6:2:end,4:2:end - 2));


        % then mirror 2 rows and columns  and diagonal of interpolated mosaic_H, mosaic_V
        mosaic_H(:,2:-1:1,2) = mosaic_H(:,3:4,2);
        mosaic_H(:,end:-1:end - 1,2) = mosaic_H(:,end - 3:end -2,2);
        mosaic_V(2:-1:1,:,2) = mosaic_V(3:4,:,2);
        mosaic_V(end:-1:end - 1, :,2) = mosaic_V(end - 3:end - 2,:,2);
        mosaic_H(2,2,2) = mosaic_H(4,4,2);
        mosaic_V(2,2,2) = mosaic_V(4,4,2);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        % 2. Interpolate missing R/B values at G sample positions
        % Subcase 1
        % G B G
        % R G R <- G of interest
        % G B G 
        
        %estimate R
        mosaic_H(4:2:end - 2, 3:2:end - 3,1)= mosaic(4:2:end - 2, 3:2:end - 3) + 0.5.*(mosaic(4:2:end - 2, 2:2:end - 4) + mosaic(4:2:end - 2, 4:2:end - 2)...
                                            - mosaic_H(4:2:end - 2, 2:2:end - 4,2) - mosaic_H(4:2:end - 2, 4:2:end - 2,2));
                                        
        mosaic_V(4:2:end - 2, 3:2:end - 3,1)= mosaic(4:2:end - 2, 3:2:end - 3) + 0.5.*(mosaic(4:2:end - 2, 2:2:end - 4) + mosaic(4:2:end - 2, 4:2:end - 2)...
                                            - mosaic_V(4:2:end - 2, 2:2:end - 4,2) - mosaic_V(4:2:end - 2, 4:2:end - 2,2));
        %estimate B
        mosaic_H(4:2:end - 2, 3:2:end - 3,3) = mosaic(4:2:end - 2, 3:2:end - 3) + 0.5.*(mosaic(3:2:end - 3, 3:2:end - 3) + mosaic(5:2:end - 1, 3:2:end - 3)...
                                             - mosaic_H(3:2:end - 3, 3:2:end - 3,2) - mosaic_H(5:2:end - 1, 3:2:end - 3,2));
                                         
        mosaic_V(4:2:end - 2, 3:2:end - 3,3) = mosaic(4:2:end - 2, 3:2:end - 3) + 0.5.*(mosaic(3:2:end - 3, 3:2:end - 3) + mosaic(5:2:end - 1, 3:2:end - 3)...
                                             - mosaic_V(3:2:end - 3, 3:2:end - 3,2) - mosaic_V(5:2:end - 1, 3:2:end - 3,2)); 
                                         
        % Subcase 2
        % G R G
        % B G B <- G of interest
        % G R G   
        
        % estimate B
        mosaic_H(3:2:end - 3, 4:2:end - 2, 3) = mosaic(3:2:end - 3, 4:2:end - 2) + 0.5.*(mosaic(3:2:end - 3, 3:2:end - 3) + mosaic(3:2:end - 3, 5:2:end - 1)...
                                              - mosaic_H(3:2:end - 3, 3:2:end - 3,2) - mosaic_H(3:2:end - 3, 5:2:end - 1,2));
        mosaic_V(3:2:end - 3, 4:2:end - 2, 3) = mosaic(3:2:end - 3, 4:2:end - 2) + 0.5.*(mosaic(3:2:end - 3, 3:2:end - 3) + mosaic(3:2:end - 3, 5:2:end - 1)...
                                              - mosaic_V(3:2:end - 3, 3:2:end - 3,2) - mosaic_V(3:2:end - 3, 5:2:end - 1,2)); 
        
        % estimate R
        mosaic_H(3:2:end - 3, 4:2:end - 2, 1) = mosaic(3:2:end - 3, 4:2:end - 2) + 0.5.*(mosaic(2:2:end - 4, 4:2:end - 2) + mosaic(4:2:end - 2, 4:2:end - 2)...
                                              - mosaic_H(2:2:end - 4, 4:2:end - 2,2) - mosaic_H(4:2:end - 2, 4:2:end - 2,2));
        mosaic_V(3:2:end - 3, 4:2:end - 2, 1) = mosaic(3:2:end - 3, 4:2:end - 2) + 0.5.*(mosaic(2:2:end - 4, 4:2:end - 2) + mosaic(4:2:end - 2, 4:2:end - 2)...
                                              - mosaic_V(2:2:end - 4, 4:2:end - 2,2) - mosaic_V(4:2:end - 2, 4:2:end - 2,2));   
        
        % mirror first and last 2 rows and columns
        mosaic_H(:,2:-1:1,3) = mosaic_H(:,3:4,3);
        mosaic_H(:,end:-1:end - 1,3) = mosaic_H(:,end - 3:end -2,3);
        mosaic_V(2:-1:1,:,3) = mosaic_V(3:4,:,3);
        mosaic_V(end:-1:end - 1, :,3) = mosaic_V(end - 3:end - 2,:,3);
        mosaic_H(:,2:-1:1,1) = mosaic_H(:,3:4,1);
        mosaic_H(:,end:-1:end - 1,1) = mosaic_H(:,end - 3:end -2,1);
        mosaic_V(2:-1:1,:,1) = mosaic_V(3:4,:,1);
        mosaic_V(end:-1:end - 1, :,1) = mosaic_V(end - 3:end - 2,:,1);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % 3. Interpolate missing R/B values at B/R sampling positions
        
        % interpolate R at B sampling positions
        mosaic_H(3:2:end - 3, 3:2:end - 3, 1) = mosaic_H(3:2:end - 3, 3:2:end - 3, 2) + 0.25.*(mosaic(2:2:end - 4,2:2:end - 4) + mosaic(4:2:end - 2,4:2:end - 2)...
                                              + mosaic(2:2:end - 4,4:2:end - 2) + mosaic(4:2:end - 2,4:2:end - 2) - mosaic_H(2:2:end - 4,2:2:end - 4,2)...
                                              - mosaic_H(4:2:end - 2,4:2:end - 2,2) - mosaic_H(2:2:end - 4,4:2:end - 2,2) - mosaic_H(4:2:end - 2,4:2:end - 2,2));
        
        mosaic_V(3:2:end - 3, 3:2:end - 3, 1) = mosaic_V(3:2:end - 3, 3:2:end - 3, 2) + 0.25.*(mosaic(2:2:end - 4,2:2:end - 4) + mosaic(4:2:end - 2,4:2:end - 2)...
                                              + mosaic(2:2:end - 4,4:2:end - 2) + mosaic(4:2:end - 2,4:2:end - 2) - mosaic_V(2:2:end - 4,2:2:end - 4,2)...
                                              - mosaic_V(4:2:end - 2,4:2:end - 2,2) - mosaic_V(2:2:end - 4,4:2:end - 2,2) - mosaic_V(4:2:end - 2,4:2:end - 2,2));                                  
        
        % interpolate B at R sampling positions
        mosaic_H(4:2:end - 2, 4:2:end - 2, 3) = mosaic_H(4:2:end - 2, 4:2:end - 2, 2) + 0.25.*(mosaic(3:2:end - 3,3:2:end - 3) + mosaic(5:2:end - 1,3:2:end - 3)...
                                              + mosaic(3:2:end - 3,5:2:end - 1) + mosaic(5:2:end - 1,5:2:end - 1) - mosaic_H(3:2:end - 3,3:2:end - 3,2)...
                                              - mosaic_H(5:2:end - 1,3:2:end - 3,2) - mosaic_H(3:2:end - 3,5:2:end - 1,2) - mosaic_H(5:2:end - 1,5:2:end - 1,2));
        
        mosaic_V(4:2:end - 2, 4:2:end - 2, 3) = mosaic_V(4:2:end - 2, 4:2:end - 2, 2) + 0.25.*(mosaic(3:2:end - 3,3:2:end - 3) + mosaic(5:2:end - 1,3:2:end - 3)...
                                              + mosaic(3:2:end - 3,5:2:end - 1) + mosaic(5:2:end - 1,5:2:end - 1) - mosaic_V(3:2:end - 3,3:2:end - 3,2)...
                                              - mosaic_V(5:2:end - 1,3:2:end - 3,2) - mosaic_V(3:2:end - 3,5:2:end - 1,2) - mosaic_V(5:2:end - 1,5:2:end - 1,2));  
        
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % soft decision
        gamma_H = mosaic_H(:,:,1) - mosaic_H(:,:,2);
        gamma_V = mosaic_V(:,:,1) - mosaic_V(:,:,2);
        beta_H = mosaic_H(:,:,3) - mosaic_H(:,:,2);
        beta_V = mosaic_V(:,:,3) - mosaic_V(:,:,2);
        
        primaryDiff_H = cat(3, gamma_H, beta_H);
        primaryDiff_V = cat(3, gamma_V, beta_V);
        diff1_HH = zeros(size(mosaic_H,1),size(mosaic_H,2), 2);
        diff2_HH = diff1_HH;
        diff1_VV = diff1_HH;
        diff2_VV = diff1_HH;
        diff1_HV = diff1_HH;
        diff1_VH = diff1_HH;
        diff1_HH(:,1:end - 1,:) = abs(primaryDiff_H(:,1:end - 1,:) - primaryDiff_H(:,2:end,:));
        diff2_HH(:,1:end - 2,:) = abs(primaryDiff_H(:,1:end - 2,:) - primaryDiff_H(:,3:end,:));
        diff1_VV(1:end - 1,:,:) = abs(primaryDiff_V(1:end - 1,:,:) - primaryDiff_V(2:end,:,:));
        diff2_VV(1:end - 2,:,:) = abs(primaryDiff_V(1:end - 2,:,:) - primaryDiff_V(3:end,:,:));
        diff1_VH(1:end - 1,:,:) = abs(primaryDiff_H(1:end - 1,:,:) - primaryDiff_H(2:end,:,:));
        diff1_HV(:,1:end - 1,:) = abs(primaryDiff_V(:,1:end - 1,:) - primaryDiff_V(:,2:end,:));


        h_delta_h_filter1 = [1 1 0; 1 0 0; 1 1 0];
        h_delta_h_filter2 = [1 0 0; 1 0 0; 1 0 0];
        v_delta_h_filter1 = [1 1 1; 0 0 0; 0 0 0];
        v_delta_h_filter2 = [0 0 0; 0 0 0; 1 1 1];
        h_delta_h = filter2(h_delta_h_filter1, diff1_HH(:,:,1), 'same') + filter2(h_delta_h_filter2, diff2_HH(:,:,1), 'same')...
                   + filter2(h_delta_h_filter1, diff1_HH(:,:,2), 'same') + filter2(h_delta_h_filter2, diff2_HH(:,:,2), 'same') ;
        v_delta_v = filter2(h_delta_h_filter1', diff1_VV(:,:,1), 'same') + filter2(h_delta_h_filter2', diff2_VV(:,:,1), 'same')...
                  + filter2(h_delta_h_filter1', diff1_VV(:,:,2), 'same') + filter2(h_delta_h_filter2', diff2_VV(:,:,2), 'same');
              
        h_delta_v1 = filter2(v_delta_h_filter1, diff1_VH(:,:,1), 'same') + filter2(v_delta_h_filter1, diff1_VH(:,:,2), 'same');
        h_delta_v2 = filter2(v_delta_h_filter2, diff1_VH(:,:,1), 'same') + filter2(v_delta_h_filter2, diff1_VH(:,:,2), 'same');
        h_delta_v = min(h_delta_v1, h_delta_v2);
        
        v_delta_h1 = filter2(v_delta_h_filter1', diff1_HV(:,:,1), 'same') + filter2(v_delta_h_filter1', diff1_HV(:,:,2), 'same');
        v_delta_h2 = filter2(v_delta_h_filter2', diff1_HV(:,:,1), 'same') + filter2(v_delta_h_filter2', diff1_HV(:,:,2), 'same');
        v_delta_h =  min(v_delta_h1, v_delta_h2);
        
        
        % make decision
        a = [-0.62, -0.35, 0.62, 0.35];
        threshold =0;
        z = a(1)*h_delta_h + a(2).*v_delta_h + a(3).*v_delta_v + a(4).*h_delta_v;
        rgb = zeros(size(mosaic_V,1), size(mosaic_V,2),3);
        for i = 1:3
            mV = mosaic_V(:,:,i);
            mH = mosaic_H(:,:,i);
            temp = zeros(size(mV));
            temp(find(z < threshold)) = mV(find(z < threshold));
            temp(find(z >= threshold)) = mH(find(z >= threshold));
            rgb(:,:,i) = temp;
        end
%         rgb(find(z < threshold),1) = mosaic_V( find(z < threshold),1);
%         rgb(find(z >= threshold),:) = mosaic_H( find(z >= threshold),:);
        rgb = rgb(3:end - 2, 3:end - 2,:);
    case 'gbrg'
        error('GBRG not yet implemented');
    otherwise
        error('Unsupported Bayer RGB pattern');
end

if clipToRange, rgb = ieClip(rgb); end

return;
